def test_solve_pnp():
points = np.random.uniform(-10, 10, (7, 3))
omegas = np.random.uniform(-np.pi, np.pi, (6, 3))
translations = generate_translations(exp_so3(omegas), points)
for omega_true, t_true in zip(omegas, translations):
P = transform(exp_so3(omega_true), t_true, points)
keypoints_true = pi(P)
pose = solve_pnp(points, keypoints_true)
P = transform(pose.R, pose.t, points)
keypoints_pred = pi(P)
# omega_true and omega_pred can be different but
# they have to be linearly dependent and satisfy
# norm(omega_true) - norm(omega_pred) = 2 * n * pi
assert_array_almost_equal(t_true, pose.t)
assert_array_almost_equal(keypoints_true, keypoints_pred)
P = transform(exp_so3(omegas[0]), translations[0], points)
keypoints0 = pi(P)
# 6 correspondences
# this should be able to run
solve_pnp(points[0:6], keypoints0[0:6])
with pytest.raises(NotEnoughInliersException):
# not enough correspondences
solve_pnp(points[0:5], keypoints0[0:5])
def run(pose0w, pose1w):
p0 = transform(pose0w.R, pose0w.t, point)
p1 = transform(pose1w.R, pose1w.t, point)
x0 = pi(p0)
x1 = pi(p1)
pose10 = pose1w * pose0w.inv()
depth = calc_depth0_(pose10.T, x0, x1)
assert_array_almost_equal(depth, p0[2])
def test_depths_from_triangulation():
rotation0 = Rotation.from_quat([0, 0, 0, 1])
rotation1 = Rotation.from_quat([0, 0, 1, 0])
t0 = np.array([-1, 3, 4], dtype=np.float64)
t1 = np.array([4, 1, 6], dtype=np.float64)
point = np.array([0, 0, 5], dtype=np.float64)
p0 = transform(rotation0.as_matrix(), t0, point)
p1 = transform(rotation1.as_matrix(), t1, point)
x0 = pi(p0)
x1 = pi(p1)
pose0 = Pose(rotation0, t0)
pose1 = Pose(rotation1, t1)
depths = DepthsFromTriangulation(pose0, pose1)(x0, x1)
assert_array_almost_equal(depths, [p0[2], p1[2]])
def __call__(self, I0, D0, I1, pose10, weights=None):
def warn():
warnings.warn("Camera pose change is too large.", RuntimeWarning)
error = PhotometricError(self.camera_model0, self.camera_model1,
I0, D0, I1)
us0 = image_coordinates(I0.shape)
xs0 = self.camera_model0.normalize(us0)
P0 = inv_pi(xs0, D0.flatten())
GX1, GY1 = calc_image_gradient(I1)
residuals = (I0 - I1).flatten()
prev_error = error(pose10)
for k in range(self.max_iter):
P1 = transform(pose10.R, pose10.t, P0)
xi = calc_pose_update(self.camera_model1, residuals,
GX1, GY1, P1, weights)
if xi is None:
warn()
return pose10
dpose = Pose.from_se3(xi)
candidate = dpose * pose10
curr_error = error(candidate)
if curr_error > prev_error:
break
prev_error = curr_error
pose10 = candidate
return pose10
def test_calc_depth0():
rotation0 = Rotation.from_rotvec([0, np.pi / 2, 0])
t0 = np.array([-3, 0, 1])
rotation1 = Rotation.from_rotvec([0, -np.pi / 2, 0])
t1 = np.array([0, 0, 2])
pose_w0 = Pose(rotation0, t0)
pose_w1 = Pose(rotation1, t1)
point = np.array([-1, 0, 1], dtype=np.float64)
pose_0w = pose_w0.inv()
pose_1w = pose_w1.inv()
x0 = pi(transform(pose_0w.R, pose_0w.t, point))
x1 = pi(transform(pose_1w.R, pose_1w.t, point))
assert_almost_equal(calc_depth0(pose_w0, pose_w1, x0, x1), 2)
def case2():
# 5 points are behind cameras
t10 = np.array([0, 0, 0], dtype=np.float64)
P0 = X0
P1 = transform(R10, t10, X0)
keypoints0 = pi(P0)
keypoints1 = pi(P1)
message = "Most of points are behind cameras. Maybe wrong matches?"
with pytest.warns(RuntimeWarning, match=message):
estimate_pose_change(keypoints0, keypoints1)
def case1():
t10 = np.array([0, 0, 5], dtype=np.float64)
P0 = X0
P1 = transform(R10, t10, X0)
keypoints0 = pi(P0)
keypoints1 = pi(P1)
pose10 = estimate_pose_change(keypoints0, keypoints1)
assert_array_almost_equal(pose10.R, R10)
# test if t pred and t true are parallel
# because we cannot know the scale
assert_array_almost_equal(np.cross(pose10.t, t10), np.zeros(3))
def test_mul():
# case1
pose1 = Pose(Rotation.from_rotvec(np.zeros(3)), np.ones(3))
pose2 = Pose(Rotation.from_rotvec(np.zeros(3)), np.ones(3))
pose3 = pose1 * pose2
assert_array_equal(pose3.rotation.as_rotvec(), np.zeros(3))
assert_array_equal(pose3.t, 2 * np.ones(3))
# case2
axis = np.array([0.0, 1.0, 2.0])
rotvec10 = 0.4 * axis
rotvec21 = 0.1 * axis
t10 = np.array([-0.1, 2.0, 0.1])
t21 = np.array([0.2, 0.4, -0.1])
pose10 = Pose(Rotation.from_rotvec(rotvec10), t10)
pose21 = Pose(Rotation.from_rotvec(rotvec21), t21)
pose20 = pose21 * pose10
assert_array_almost_equal(pose20.rotation.as_rotvec(), 0.5 * axis)
assert_array_almost_equal(pose20.t, np.dot(pose21.R, pose10.t) + pose21.t)
# case3
point = np.random.random(3)
rotvec21 = np.random.random(3)
t21 = np.random.uniform(-10, 10, 3)
rotvec10 = np.random.random(3)
t10 = np.random.uniform(-10, 10, 3)
pose10 = Pose(Rotation.from_rotvec(rotvec10), t10)
pose21 = Pose(Rotation.from_rotvec(rotvec21), t21)
pose20 = pose21 * pose10
assert_array_almost_equal(
transform(pose21.R, pose21.t, transform(pose10.R, pose10.t, point)),
transform(pose20.R, pose20.t, point))
def camera_poly3d(pose, scale):
# this code is the modified version of
# [an answer](https://stackoverflow.com/a/44920709)
# by [serenity](https://stackoverflow.com/users/2666859/serenity)
v = transform(pose.R, pose.t, vertices_ * scale)
P = np.array([[v[0], v[1], v[4]], [v[0], v[3], v[4]], [v[2], v[1], v[4]],
[v[2], v[3], v[4]]])
return Poly3DCollection(P,
facecolors='cyan',
linewidths=1,
edgecolors='red',
alpha=.25)
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))
)
calc_depth0, calc_depth0_)
# TODO add the case such that x[3] = 0
points_true = np.array(
[[4, -1, 3], [1, -3, -2], [-2, 3, -2], [-3, -2, -5], [-3, -1, 2],
[-4, -2, 3], [4, 1, 1], [-2, 3, 1], [4, 1, 2], [-4, 4, -1]],
dtype=np.float64)
R0 = Rotation.from_euler('xyz', np.random.random(3)).as_matrix()
R1 = Rotation.from_euler('xyz', np.random.random(3)).as_matrix()
R2 = Rotation.from_euler('xyz', np.random.random(3)).as_matrix()
[t0, t1, t2] = generate_translations(np.array([R0, R1, R2]), points_true)
keypoints0 = pi(transform(R0, t0, points_true))
keypoints1 = pi(transform(R1, t1, points_true))
keypoints2 = pi(transform(R2, t2, points_true))
def test_linear_triangulation():
rotations = np.array([R0, R1, R2])
translations = np.array([t0, t1, t2])
keypoints = np.stack((keypoints0, keypoints1, keypoints2))
points, depths = linear_triangulation(rotations, translations, keypoints)
assert_array_almost_equal(points, points_true)
assert (depths.shape == (3, points_true.shape[0]))
for i, x in enumerate(points_true):
def assert_projection_equal(projection, pose, points, keypoints_pred):
assert_array_almost_equal(
projection.compute(transform(pose.R, pose.t, points)), keypoints_pred)
def optical_axis(pose, scale):
V = transform(pose.R, pose.t, optical_axis_ * scale)
src, dst = V[0], V[1]
return [[src[0], dst[0]], [src[1], dst[1]], [src[2], dst[2]]]
