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)