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
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])
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"
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)