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_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_fundamental_to_essential():
R = random_rotation_matrix(3)
t = np.random.uniform(-10, 10, 3)
K0 = CameraParameters(focal_length=[0.8, 1.2], offset=[0.8, -0.2]).matrix
K1 = CameraParameters(focal_length=[0.7, 0.9], offset=[-1.0, 0.1]).matrix
E_true = to_essential(R, t)
F = inv(K1).T.dot(E_true).dot(inv(K0))
E_pred = fundamental_to_essential(F, K0, K1)
assert_array_almost_equal(E_true, E_pred)
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 test_camera():
camera_parameters = CameraParameters(focal_length=[1.0, 1.2],
offset=[0.8, 0.2])
expected = np.array([[1.0, 0.0, 0.8], [0.0, 1.2, 0.2], [0.0, 0.0, 1.0]])
assert_array_equal(camera_parameters.matrix, expected)
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 test_perspective_projection():
camera_parameters = CameraParameters(focal_length=[0.8, 1.2],
offset=[1.0, 2.0])
projection = PerspectiveProjection(camera_parameters)
points = np.array([[1.0, 2.0, 2.0], [4.0, 3.0, 5.0]])
expected = np.array([
[1.4, 3.2],
[1.64, 2.72],
])
assert_array_almost_equal(projection.compute(points), expected)
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 test_calc_projection_jacobian():
camera_parameters = CameraParameters(focal_length=[3, 2], offset=[0, 0])
P = np.array([
[1, 6, 2],
[8, 5, 3],
])
J = calc_projection_jacobian(camera_parameters, P)
GT = np.array([
# x' = 1, y' = 6, z' = 2, fx = 3, fy = 2
[[3 / 2, 0, -3 * 1 / 4, -3 * 1 * 6 / 4, 3 * (1 + 1 / 4), -3 * 6 / 2],
[0, 2 / 2, -2 * 6 / 4, -2 * (1 + 36 / 4), 2 * 1 * 6 / 4, 2 * 1 / 2]],
# x' = 8, y' = 5, z' = 3, fx = 3, fy = 2
[[3 / 3, 0, -3 * 8 / 9, -3 * 8 * 5 / 9, 3 * (1 + 64 / 9), -3 * 5 / 3],
[0, 2 / 3, -2 * 5 / 9, -2 * (1 + 25 / 9), 2 * 8 * 5 / 9, 2 * 8 / 3]]
])
assert_array_equal(J, GT)
def test_estimate_fundamental():
camera_parameters = CameraParameters(
focal_length=[0.8, 1.2],
offset=[0.8, 0.2]
)
projection = PerspectiveProjection(camera_parameters)
R = random_rotation_matrix(3)
t = np.random.uniform(-10, 10, 3)
points_true = np.random.uniform(-10, 10, (10, 3))
keypoints0 = projection.compute(points_true)
keypoints1 = projection.compute(transform(R, t, points_true))
K = camera_parameters.matrix
K_inv = inv(K)
F = estimate_fundamental(keypoints0, keypoints1)
E = fundamental_to_essential(F, K)
for i in range(points_true.shape[0]):
x0 = np.append(keypoints0[i], 1)
x1 = np.append(keypoints1[i], 1)
assert_almost_equal(x1.dot(F).dot(x0), 0)
y0 = np.dot(K_inv, x0)
y1 = np.dot(K_inv, x1)
assert_almost_equal(y1.dot(E).dot(y0), 0)
# properties of the essential matrix
assert_almost_equal(np.linalg.det(E), 0)
assert_array_almost_equal(
2 * np.dot(E, np.dot(E.T, E)) - np.trace(np.dot(E, E.T)) * E,
np.zeros((3, 3))
)
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)
def camera_parameters_at(self, level):
"""Change camera parameters as the image is resized"""
focal_length = self.camera_parameters.focal_length
offset = self.camera_parameters.offset
ratio = level_to_ratio(level)
return CameraParameters(focal_length * ratio, offset * ratio)
from tadataka.camera import CameraParameters
from tadataka.rigid_motion import LeastSquaresRigidMotion
from tadataka.rigid_transform import Transform
from tadataka.plot.visualizers import set_aspect_equal
def set_line_3d(line, data):
line.set_data(data[:, 0:2].T)
line.set_3d_properties(data[:, 2])
def set_image(image_axis, image):
image_axis.set_array(image)
camera_parameters = CameraParameters(focal_length=[525.0, 525.0],
offset=[319.5, 239.5])
def set_ax_range(ax, data):
min_ = np.min(data, axis=0)
max_ = np.max(data, axis=0)
ax.set_xlim([min_[0], max_[0]])
ax.set_ylim([min_[1], max_[1]])
ax.set_zlim([min_[2], max_[2]])
set_aspect_equal(ax)
class Drawer(object):
def __init__(self, fig, vo, dataset):
self.ax1 = fig.add_subplot(1, 2, 1, projection='3d')
self.ax2 = fig.add_subplot(2, 2, 2)