Exemple #1
0
def check_undistortion(cam_opts):
    cam = _build_test_camera(**cam_opts)

    step = 5
    border = 65

    distorteds = []
    for row in range(border, cam.height - border, step):
        for col in range(border, cam.width - border, step):
            distorted = [col, row]
            distorteds.append(distorted)
    npdistorted = np.array(distorteds, dtype=np.float)

    src = numpy2opencv_pointmat(npdistorted)
    dst = cv.CloneMat(src)
    cv.UndistortPoints(src,
                       dst,
                       numpy2opencv_image(cam.get_K()),
                       numpy2opencv_image(cam.get_D()),
                       R=numpy2opencv_image(cam.get_rect()),
                       P=numpy2opencv_image(cam.get_P()))
    undistorted_cv = opencv_pointmat2numpy(dst)

    undistorted_np = cam.undistort(npdistorted)
    assert undistorted_cv.shape == undistorted_np.shape
    if cam.is_opencv_compatible():
        assert np.allclose(undistorted_cv, undistorted_np)
    else:
        from nose.plugins.skip import SkipTest
        raise SkipTest(
            "Test %s is skipped: %s" %
            (check_undistortion.__name__,
             'camera model is not OpenCV compatible, skipping test'))
def check_undistortion(cam_opts):
    cam = _build_test_camera(**cam_opts)

    step = 5
    border = 65

    distorteds = []
    for row in range(border, cam.height-border, step):
        for col in range(border, cam.width-border, step):
            distorted = [col, row]
            distorteds.append(distorted)
    npdistorted = np.array(distorteds,dtype=np.float)

    src = numpy2opencv_pointmat(npdistorted)
    dst = cv.CloneMat(src)
    cv.UndistortPoints(src, dst,
                       numpy2opencv_image(cam.get_K()),
                       numpy2opencv_image(cam.get_D()),
                       R = numpy2opencv_image(cam.get_rect()),
                       P = numpy2opencv_image(cam.get_P()))
    undistorted_cv = opencv_pointmat2numpy(dst)

    undistorted_np = cam.undistort( npdistorted )
    assert undistorted_cv.shape == undistorted_np.shape
    if cam.is_opencv_compatible():
        assert np.allclose(undistorted_cv, undistorted_np)
    else:
        from nose.plugins.skip import SkipTest
        raise SkipTest("Test %s is skipped: %s" %(
            check_undistortion.__name__,
            'camera model is not OpenCV compatible, skipping test'))
def check_flip(cam_opts):
    cam_orig = _build_test_camera(**cam_opts)
    try:
        cam_flip = cam_orig.get_flipped_camera()
    except NotImplementedError as err:
        raise SkipTest(str(err))

    # They have different orientation (but same position) in space,
    assert not np.allclose( cam_orig.get_rotation(), cam_flip.get_rotation())
    assert np.allclose( cam_orig.get_camcenter(), cam_flip.get_camcenter())

    eye, lookat, up = cam_orig.get_view()
    eye2, lookat2, up2 = cam_flip.get_view()

    assert not np.allclose( lookat, lookat2 )

    # but they project 3D points to same pixel locations
    verts = np.array([[ 0.042306,  0.015338,  0.036328],
                      [ 1.03323,   2.030344,  3.041542],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.036396,  0.026464,  0.052408]])

    expected = cam_orig.project_3d_to_pixel(verts)
    actual   = cam_flip.project_3d_to_pixel(verts)
    assert np.allclose( expected, actual )
def check_camera_distortion_roundtrip(cam_opts):
    """check that uv == distort( undistort( uv ))"""
    cam = _build_test_camera(**cam_opts)
    uv_raw = _generate_uv_raw(cam.width, cam.height)
    uv_rect = cam.undistort( uv_raw )
    uv_unrect = cam.distort( uv_rect )
    assert uv_raw.shape == uv_unrect.shape
    assert np.allclose(uv_raw, uv_unrect, atol=1.0) # within one pixel
def check_built_from_M(cam_opts):
    """check that M is preserved in load_camera_from_M() factory"""
    cam_orig = _build_test_camera(**cam_opts)
    if cam_orig.is_distorted_and_skewed():
        raise SkipTest('do not expect that skewed camera passes this test')
    M_orig = cam_orig.M
    cam = CameraModel.load_camera_from_M( M_orig )
    assert np.allclose( cam.M, M_orig)
def check_camera_projection_roundtrip(cam_opts,distorted=False):
    """check that uv == project_to_2d( project_to_3d( uv ))"""
    cam = _build_test_camera(**cam_opts)
    uv_raw = _generate_uv_raw(cam.width, cam.height)

    pts3d = cam.project_pixel_to_3d_ray( uv_raw, distorted=distorted )
    uv_unrect = cam.project_3d_to_pixel( pts3d, distorted=distorted )
    assert uv_raw.shape == uv_unrect.shape
    assert np.allclose(uv_raw, uv_unrect, atol=1.0) # within one pixel
Exemple #7
0
def check_roundtrip_ros_tf(cam_opts):
    cam1 = _build_test_camera(**cam_opts)
    translation, rotation = cam1.get_ROS_tf()
    i = cam1.get_intrinsics_as_bunch()
    cam2 = CameraModel.load_camera_from_ROS_tf( translation=translation,
                                                rotation=rotation,
                                                intrinsics = i,
                                                name = cam1.name)
    assert cam1==cam2
def check_projection(cam_opts,distorted=True):
    cam = _build_test_camera(**cam_opts)
    R = cam.get_rect()
    if not np.allclose(R, np.eye(3)):
        # opencv's ProjectPoints2 does not take into account
        # rectifciation matrix, thus we skip this test.
        from nose.plugins.skip import SkipTest
        raise SkipTest("Test %s is skipped: %s" %(
            check_projection.__name__,
            'cannot check if rectification matrix is not unity'))

    pts3D = _build_points_3d()
    n_pts = len(pts3D)

    src = numpy2opencv_image(pts3D)
    dst = numpy2opencv_pointmat(np.empty( (n_pts, 2) ))

    t = np.array(cam.get_translation(),copy=True)
    t.shape = 3,1

    R = cam.get_rotation()
    rvec = numpy2opencv_image(np.empty( (1,3) ))
    cv.Rodrigues2(numpy2opencv_image(R), rvec)

    if distorted:
        K = cam.get_K()
        cv_distortion = numpy2opencv_image(cam.get_D())
    else:
        K = cam.get_P()[:3,:3]
        cv_distortion = numpy2opencv_image(np.zeros((5,1)))

    cv.ProjectPoints2(src,
                      rvec,
                      numpy2opencv_image(t),
                      numpy2opencv_image(K),
                      cv_distortion,
                      dst)
    result_cv = opencv_pointmat2numpy(dst)

    result_np = cam.project_3d_to_pixel( pts3D, distorted=distorted )
    assert result_cv.shape == result_np.shape
    if cam.is_opencv_compatible():
        try:
            assert np.allclose(result_cv, result_np)
        except:
            debug()
            debug('result_cv')
            debug(result_cv)
            debug('result_np')
            debug(result_np)
            debug()
            raise
    else:
        from nose.plugins.skip import SkipTest
        raise SkipTest("Test %s is skipped: %s" %(
            check_projection.__name__,
            'camera model is not OpenCV compatible, skipping test'))
def check_camcenter_like(cam_opts):
    cam = _build_test_camera(**cam_opts)
    cc_expected = cam.get_camcenter()
    for n in range(4):
        nparr = np.zeros( (n,3), dtype=np.float )
        cc = cam.camcenter_like( nparr )
        for i in range(n):
            this_cc = cc[i]
            assert np.allclose(cc_expected,this_cc)
def check_projection_to_undistorted1(cam_opts):
    """check that points along optical axis are imaged onto principal point"""
    cam = _build_test_camera(**cam_opts)
    for z in np.linspace(0.1, 10, 20):
        pt = np.array([[0,0,z]])
        result = cam.project_3d_to_pixel( pt, distorted=False )
        u,v = result[0]

        assert np.allclose(u, cam.P[0,2])
        assert np.allclose(v, cam.P[1,2])
Exemple #11
0
def check_projection(cam_opts, distorted=True):
    cam = _build_test_camera(**cam_opts)
    R = cam.get_rect()
    if not np.allclose(R, np.eye(3)):
        # opencv's ProjectPoints2 does not take into account
        # rectifciation matrix, thus we skip this test.
        from nose.plugins.skip import SkipTest
        raise SkipTest("Test %s is skipped: %s" %
                       (check_projection.__name__,
                        'cannot check if rectification matrix is not unity'))

    pts3D = _build_points_3d()
    n_pts = len(pts3D)

    src = numpy2opencv_image(pts3D)
    dst = numpy2opencv_pointmat(np.empty((n_pts, 2)))

    t = np.array(cam.get_translation(), copy=True)
    t.shape = 3, 1

    R = cam.get_rotation()
    rvec = numpy2opencv_image(np.empty((1, 3)))
    cv.Rodrigues2(numpy2opencv_image(R), rvec)

    if distorted:
        K = cam.get_K()
        cv_distortion = numpy2opencv_image(cam.get_D())
    else:
        K = cam.get_P()[:3, :3]
        cv_distortion = numpy2opencv_image(np.zeros((5, 1)))

    cv.ProjectPoints2(src, rvec, numpy2opencv_image(t), numpy2opencv_image(K),
                      cv_distortion, dst)
    result_cv = opencv_pointmat2numpy(dst)

    result_np = cam.project_3d_to_pixel(pts3D, distorted=distorted)
    assert result_cv.shape == result_np.shape
    if cam.is_opencv_compatible():
        try:
            assert np.allclose(result_cv, result_np)
        except:
            debug()
            debug('result_cv')
            debug(result_cv)
            debug('result_np')
            debug(result_np)
            debug()
            raise
    else:
        from nose.plugins.skip import SkipTest
        raise SkipTest(
            "Test %s is skipped: %s" %
            (check_projection.__name__,
             'camera model is not OpenCV compatible, skipping test'))
def check_extrinsic_msg(cam_opts):
    """check that ROS message contains actual camera extrinsic parameters"""
    cam_opts = cam_opts.copy()
    cam_opts['get_input_data']=True
    r = _build_test_camera(**cam_opts)
    cam = r['cam']
    tfm = cam.get_extrinsics_as_bunch()
    if 'translation' in r:
        assert np.allclose(point_msg_to_tuple(tfm.translation), point_msg_to_tuple(r['translation']))
    if 'rotation' in r:
        assert np.allclose(parse_rotation_msg(tfm.rotation,force_matrix=True),
                           parse_rotation_msg(r['rotation'],force_matrix=True))
def check_stages(cam_opts, distorted=False):
    """check the sum of all stages equals all stages summed"""
    cam = _build_test_camera(**cam_opts)

    uv_raw = _generate_uv_raw(cam.width, cam.height)
    pts3d = cam.project_pixel_to_3d_ray( uv_raw, distorted=distorted )

    # case 1: direct projection to pixels
    direct = cam.project_3d_to_pixel( pts3d, distorted=distorted )

    # case 2: project to camera frame, then to pixels
    cam_frame = cam.project_3d_to_camera_frame(pts3d)
    indirect = cam.project_camera_frame_to_pixel(cam_frame, distorted=distorted)

    assert np.allclose(direct, indirect)
def check_distortion_yamlfile_roundtrip(cam_opts):
    """check that roundtrip of camera model to/from a yaml file works"""
    cam = _build_test_camera(**cam_opts)
    fname = tempfile.mktemp(suffix='.yaml')
    cam.save_intrinsics_to_yamlfile(fname)
    try:
        cam2 = CameraModel.load_camera_from_file( fname, extrinsics_required=False )
    finally:
        os.unlink(fname)

    distorted = np.array( [[100.0,100],
                           [100,200],
                           [100,300],
                           [100,400]] )
    orig_undistorted = cam.undistort( distorted )
    reloaded_undistorted = cam2.undistort( distorted )
    assert np.allclose( orig_undistorted, reloaded_undistorted )
def check_camera_mirror_projection_roundtrip(cam_opts,distorted=False,axis='lr'):
    """check that a mirrored camera gives reflected pixel coords"""
    cam_orig = _build_test_camera(**cam_opts)
    cam_mirror = cam_orig.get_mirror_camera(axis=axis)
    uv_raw = _generate_uv_raw(cam_orig.width, cam_orig.height)

    # Get a collection of 3D points for which we know the pixel location of
    pts3d = cam_orig.project_pixel_to_3d_ray( uv_raw, distorted=distorted )
    # Now project that through our mirror-image camera.
    uv_mirror = cam_mirror.project_3d_to_pixel( pts3d, distorted=distorted )
    # Which points should be xnew = (width-x)
    expected = np.array(uv_raw)
    if axis=='lr':
        expected[:,0] = cam_orig.width - uv_raw[:,0]
    else:
        expected[:,1] = cam_orig.height - uv_raw[:,1]
    assert expected.shape == uv_mirror.shape
    assert np.allclose(expected, uv_mirror, atol=1.0) # within one pixel
def check_align(cam_opts):

    cam_orig = _build_test_camera(**cam_opts)
    M_orig = cam_orig.M
    cam_orig = CameraModel.load_camera_from_M( M_orig )
    R1 = np.eye(3)
    R2 = np.zeros((3,3))
    R2[0,1] = 1
    R2[1,0] = 1
    R2[2,2] = -1
    t1 = np.array( (0.0, 0.0, 0.0) )
    t2 = np.array( (0.0, 0.0, 0.1) )
    t3 = np.array( (0.1, 0.0, 0.0) )
    for s in [1.0, 2.0]:
        for R in [R1, R2]:
            for t in [t1, t2, t3]:
                cam_actual = cam_orig.get_aligned_camera( s, R, t )
                M_expected = mcsc_align.align_M( s,R,t, M_orig )
                cam_expected = CameraModel.load_camera_from_M( M_expected )
                assert cam_actual==cam_expected
def check_view(cam_opts):
    """check that we can reset camera extrinsic parameters"""

    # This is not a very good test. (Should maybe check more eye
    # positions, more lookat directions, and more up vectors.)

    cam_orig = _build_test_camera(**cam_opts)
    eye = (10,20,30)
    lookat = (11,20,30) # must be unit length for below to work
    up = (0,-1,0)
    cam_new = cam_orig.get_view_camera(eye, lookat, up)
    eye2, lookat2, up2 = cam_new.get_view()
    assert np.allclose( eye, eye2)
    assert np.allclose( lookat, lookat2 )
    assert np.allclose( up, up2 )

    # check a case that was previously failing
    n=6
    x = np.linspace(0, 2*n, n)
    theta = np.linspace(0, 2*np.pi, n)
    dim = 5.0
    for i in range(n):
        center = np.array( (x[i], 0.0, dim) )
        lookat = center + np.array( (0,1,0))
        up = -np.sin(theta[i]), 0, np.cos(theta[i])

        cam_new2 = cam_orig.get_view_camera(eye=center, lookat=lookat)

    # check a pathological case
    center= [ 0.,  0.,  5.]
    lookat= [ 0.,  1.,  5.]
    up = [0,-1,0]
    try:
        cam_new3 = cam_orig.get_view_camera(eye=center, lookat=lookat, up=up)
    except AssertionError as err:
        # we should get this exception
        pass
    else:
        assert 1==0, "did not fail test"
Exemple #18
0
def check_bagfile_roundtrip(cam_opts):
    """check that roundtrip of camera model to/from a bagfile works"""
    cam = _build_test_camera(**cam_opts)
    fname = tempfile.mktemp(suffix='.bag')
    try:
        with open(fname,mode='wb') as fd:
            cam.save_to_bagfile(fd, roslib)

        with open(fname,mode='r') as fd:
            bag = rosbag.Bag(fd, 'r')
            cam2 = CameraModel.load_camera_from_opened_bagfile( bag )
    finally:
        os.unlink(fname)

    verts = np.array([[ 0.042306,  0.015338,  0.036328],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.036396,  0.026464,  0.052408]])

    expected =  cam.project_3d_to_pixel(verts)
    actual   = cam2.project_3d_to_pixel(verts)
    assert np.allclose( expected, actual )
def check_dict_roundtrip(cam_opts):
    cam = _build_test_camera(**cam_opts)
    d = cam.to_dict()
    cam2 = CameraModel.from_dict(d)
    assert cam==cam2
def test_camcenter():
    """check that our idea of camera center is theoretically expected value"""
    all_options = get_default_options()
    for opts in all_options:
        cam = _build_test_camera(**opts)
        assert np.allclose( cam.get_camcenter(), cam.t_inv.T )
def check_pickle_roundtrip(cam_opts):
    cam = _build_test_camera(**cam_opts)
    buf = pickle.dumps(cam)
    cam2 = pickle.loads(buf)
    assert cam==cam2
def check_built_from_M(cam_opts):
    """check that M is preserved in load_camera_from_M() factory"""
    cam_orig = _build_test_camera(**cam_opts)
    M_orig = cam_orig.get_M()
    cam = CameraModel.load_camera_from_M( M_orig )
    assert np.allclose( cam.get_M(), M_orig)