def test_warp2d_(): rotation = Rotation.from_rotvec([0, np.pi/2, 0]) t = np.array([0, 0, 4]) pose10 = Pose(rotation, t) xs0 = np.array([ [0, 0], [2, -1] ], dtype=np.float64) depths0 = np.array([2, 4], dtype=np.float64) # [2, 0, 0] + [0, 0, 4] = [2, 0, 4] # [4, -4, -8] + [0, 0, 4] = [4, -4, -4] xs1, depths1 = warp2d_(pose10.T, xs0, depths0) assert_array_almost_equal(xs1, [[0.5, 0.0], [-1.0, 1.0]]) assert_array_almost_equal(depths1, [4.0, -4.0]) camera_model0 = CameraModel( CameraParameters(focal_length=[2, 2], offset=[0, 0]), distortion_model=None ) camera_model1 = CameraModel( CameraParameters(focal_length=[3, 3], offset=[0, 0]), distortion_model=None ) warp2d = LocalWarp2D(camera_model0, camera_model1, pose10) us0 = 2.0 * xs0 us1, depths1 = warp2d(us0, depths0) assert_array_almost_equal(us1, 3.0 * xs1)
def test_warp2d(): rotation = Rotation.from_rotvec([0, np.pi/2, 0]) t_w0 = np.array([0, 0, 3]) pose_w0 = Pose(rotation, t_w0) t_w1 = np.array([0, 0, 4]) pose_w1 = Pose(rotation, t_w1) warp3d = Warp3D(pose_w0, pose_w1) xs0 = np.array([ [0, 0], [0, -1] ], dtype=np.float64) depths0 = np.array([2, 4], dtype=np.float64) xs1, depths1 = warp_depth(warp3d, xs0, depths0) assert_array_almost_equal(xs1, [[0.5, 0], [0.25, -1]]) assert_array_almost_equal(depths1, [2, 4]) camera_model0 = CameraModel( CameraParameters(focal_length=[2, 2], offset=[0, 0]), distortion_model=None ) camera_model1 = CameraModel( CameraParameters(focal_length=[3, 3], offset=[0, 0]), distortion_model=None ) us0 = 2.0 * xs0 warp2d = Warp2D(camera_model0, camera_model1, pose_w0, pose_w1) us1, depths1 = warp2d(us0, depths0) assert_array_almost_equal(us1, 3.0 * xs1)
def test_normalize(): camera_model = CameraModel(CameraParameters(focal_length=[1.0, 0.5], offset=[1.0, 0.5]), distortion_model=None) width, height = 3, 2 image_shape = (height, width) table = NoramlizationMapTable(camera_model, image_shape) us_map_0 = np.array([[0, 1, 2], [0, 1, 2]]) us_map_1 = np.array([[0, 0, 0], [1, 1, 1]]) xs_map_0 = np.array([ [-1, 0, 1], [-1, 0, 1], ]) xs_map_1 = np.array([ [-1, -1, -1], [1, 1, 1], ]) us_query = np.array([[1.5, 0.5], [0.3, 1.0]]) xs_pred = table.normalize(us_query) us_pred = camera_model.unnormalize(xs_pred) assert_array_almost_equal(us_pred, us_query)
def dvo(camera_params0, camera_params1, image0, image1, depth_map0, variance_map0): estimator = PoseChangeEstimator( CameraModel(camera_params0, distortion_model=None), CameraModel(camera_params1, distortion_model=None), n_coarse_to_fine=7 ) weights = safe_invert(variance_map0) pose10 = estimator(image0, depth_map0, image1, weights) return pose10.T
def get_camera_model_rgb(freiburg): if freiburg == 1: return CameraModel( CameraParameters(focal_length=[517.3, 516.5], offset=[318.6, 255.3]), RadTan([0.2624, -0.9531, -0.0054, 0.0026, 1.1633])) if freiburg == 2: return CameraModel( CameraParameters(focal_length=[520.9, 521.0], offset=[325.1, 249.7]), RadTan([0.2312, -0.7849, -0.0033, -0.0001, 0.9172])) if freiburg == 3: return CameraModel( CameraParameters(focal_length=[535.4, 539.2], offset=[320.1, 247.6]), RadTan([0, 0, 0, 0, 0])) raise ValueError(no_such_sequence_message(freiburg))
def get_camera_model_depth(freiburg): if freiburg == 1: return CameraModel( CameraParameters(focal_length=[591.1, 590.1], offset=[331.0, 234.0]), RadTan([-0.0410, 0.3286, 0.0087, 0.0051, -0.5643])) if freiburg == 2: return CameraModel( CameraParameters(focal_length=[580.8, 581.8], offset=[308.8, 253.0]), RadTan([-0.2297, 1.4766, 0.0005, -0.0075, -3.4194])) if freiburg == 3: return CameraModel( CameraParameters(focal_length=[567.6, 570.2], offset=[324.7, 250.1]), RadTan([0, 0, 0, 0, 0])) raise ValueError(no_such_sequence_message(freiburg))
def __init__(self, dataset_root, condition="daylight"): self.camera_model = CameraModel(CameraParameters( focal_length=[615, 615], offset=[320, 240]), distortion_model=None) groundtruth_dir = Path(dataset_root, "groundtruth") illumination_dir = Path(dataset_root, "illumination") pose_path = Path(groundtruth_dir, "camera_track.txt") self.baseline_length = 10.0 self.rotations, self.positions = load_poses(pose_path) depth_dir = Path(groundtruth_dir, "depth_maps") depth_cache_dir = Path(groundtruth_dir, "depth_cache") if not depth_cache_dir.exists(): generate_depth_cache(depth_dir, depth_cache_dir) self.depth_L_paths = sorted( Path(depth_cache_dir, "left").glob("*.npy")) self.depth_R_paths = sorted( Path(depth_cache_dir, "right").glob("*.npy")) image_dir = Path(illumination_dir, condition) image_cache_dir = Path(illumination_dir, condition + "_cache") if not image_cache_dir.exists(): generate_image_cache(image_dir, image_cache_dir) self.image_L_paths = sorted( Path(image_cache_dir, "left").glob("*.npy")) self.image_R_paths = sorted( Path(image_cache_dir, "right").glob("*.npy")) assert ((len(self.depth_L_paths) == len(self.depth_R_paths) == len( self.image_L_paths) == len(self.image_R_paths) == len( self.rotations) == len(self.positions))) for i in range(len(self.positions)): DL = self.depth_L_paths[i].name DR = self.depth_R_paths[i].name IL = self.image_L_paths[i].name IR = self.image_R_paths[i].name assert (DL[-8:] == DR[-8:] == IL[-8:] == IR[-8:])
def to_perspective(camera_model): return CameraModel(camera_model.camera_parameters, distortion_model=None)
def resize_camera_model(cm, scale): from tadataka.camera import CameraParameters return CameraModel( CameraParameters(cm.camera_parameters.focal_length * scale, cm.camera_parameters.offset * scale), cm.distortion_model)