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