def test_serialization(self): """Test if serialization is working normally""" expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) actual = Pose3() serialized = expected.serialize() actual.deserialize(serialized) self.gtsamAssertEquals(expected, actual, 1e-10)
def setUp(self): # Create poses for the source map s_pose1 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])), Point3(0, 0, 0)) s_pose2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, 1]])), Point3(4, 0, 0)) s_poses = [s_pose1, s_pose2] # Create points for the source map s_point1 = Point3(1, 1, 0) s_point2 = Point3(1, 3, 0) s_point3 = Point3(3, 3, 0) s_point4 = Point3(3, 1, 0) s_point5 = Point3(2, 2, 1) s_points = [s_point1, s_point2, s_point3, s_point4, s_point5] # Create the source map self.s_map = (s_poses, s_points) # Create poses for the destination map d_pose1 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])), Point3(4, 6, 10)) d_pose2 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]])), Point3(-4, 6, 10)) d_poses = [d_pose1, d_pose2] # Create points for the destination map d_point1 = Point3(2, 8, 10) d_point2 = Point3(2, 12, 10) d_point3 = Point3(-2, 12, 10) d_point4 = Point3(-2, 8, 10) d_point5 = Point3(0, 10, 8) d_points = [d_point1, d_point2, d_point3, d_point4, d_point5] d_map = (d_poses, d_points) # Create the destination map self.d_map = (d_poses, d_points)
def test_nonconsecutive_indices(self): """Run rotation averaging on a graph with indices that are not ordered as [0,...,N-1]. Note: Test would fail if Shonan keys were not temporarily re-ordered inside the implementation. See https://github.com/borglab/gtsam/issues/784 """ num_images = 4 # assume pose 0 is orphaned in the visibility graph # Let wTi0's (R,t) be parameterized as identity Rot3(), and t = [1,1,0] wTi1 = Pose3(Rot3(), np.array([3, 1, 0])) wTi2 = Pose3(Rot3(), np.array([3, 3, 0])) wTi3 = Pose3(Rot3(), np.array([1, 3, 0])) # generate i2Ri1 rotations # (i1,i2) -> i2Ri1 i2Ri1_input = { (1, 2): wTi2.between(wTi1).rotation(), (2, 3): wTi3.between(wTi2).rotation(), (1, 3): wTi3.between(wTi1).rotation(), } i2Ri1_priors = {edge: None for edge in i2Ri1_input.keys()} wRi_computed = self.obj.run(num_images, i2Ri1_input, i2Ri1_priors) wRi_expected = [ None, wTi1.rotation(), wTi2.rotation(), wTi3.rotation() ] self.assertTrue( geometry_comparisons.compare_rotations( wRi_computed, wRi_expected, angular_error_threshold_degrees=0.1))
def test_calculate_triangulation_angle_in_degrees(self) -> None: """Test the computation of triangulation angle using a simple example. Lengths of line segments are defined as follows: 5*sqrt(2) X ---- C1 | / 5*sqrt(2) | / 10 = 5*sqrt(2)*sqrt(2) C2 Cameras and point situated as follows in the x-z plane: (0,0,0) o---- +z | | +x X (5,0,5) (10,0,0) o---- +z | | +x """ camera_center_1 = np.array([0, 0, 0]) camera_center_2 = np.array([10, 0, 0]) point_3d = np.array([5, 0, 5]) expected = 90 wT0 = Pose3(Rot3(), camera_center_1) wT1 = Pose3(Rot3(), camera_center_2) computed = mvs_utils.calculate_triangulation_angle_in_degrees( camera_1=PinholeCameraCal3Bundler(wT0), camera_2=PinholeCameraCal3Bundler(wT1), point_3d=point_3d, ) self.assertAlmostEqual(computed, expected)
def simulate_two_planes_scene(M: int, N: int) -> Tuple[Keypoints, Keypoints, EssentialMatrix]: """Generate a scene where 3D points are on two planes, and projects the points to the 2 cameras. There are M points on plane 1, and N points on plane 2. The two planes in this test are: 1. -10x -y -20z +150 = 0 2. 15x -2y -35z +200 = 0 Args: M: number of points on 1st plane. N: number of points on 2nd plane. Returns: keypoints for image i1, of length (M+N). keypoints for image i2, of length (M+N). Essential matrix i2Ei1. """ # range of 3D points range_x_coordinate = (-5, 7) range_y_coordinate = (-10, 10) # define the plane equation plane1_coeffs = (-10, -1, -20, 150) plane2_coeffs = (15, -2, -35, 200) # sample the points from planes plane1_points = sample_points_on_plane(plane1_coeffs, range_x_coordinate, range_y_coordinate, M) plane2_points = sample_points_on_plane(plane2_coeffs, range_x_coordinate, range_y_coordinate, N) points_3d = np.vstack((plane1_points, plane2_points)) # define the camera poses and compute the essential matrix wti1 = np.array([0.1, 0, -20]) wti2 = np.array([1, -2, -20.4]) wRi1 = Rot3.RzRyRx(np.pi / 20, 0, 0.0) wRi2 = Rot3.RzRyRx(0.0, np.pi / 6, 0.0) wTi1 = Pose3(wRi1, wti1) wTi2 = Pose3(wRi2, wti2) i2Ti1 = wTi2.between(wTi1) i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation())) # project 3D points to 2D image measurements intrinsics = Cal3Bundler() camera_i1 = PinholeCameraCal3Bundler(wTi1, intrinsics) camera_i2 = PinholeCameraCal3Bundler(wTi2, intrinsics) uv_im1 = [] uv_im2 = [] for point in points_3d: uv_im1.append(camera_i1.project(point)) uv_im2.append(camera_i2.project(point)) uv_im1 = np.vstack(uv_im1) uv_im2 = np.vstack(uv_im2) # return the points as keypoints and the essential matrix return Keypoints(coordinates=uv_im1), Keypoints(coordinates=uv_im2), i2Ei1
def main(): q = [0, 0, 0, 0, 0, 0, 0] # q = [1, 1, 1, 1, 1, 1, 1] lengths = [10, 20, 10, 20, 10, 10, 20] si = get_si() T = get_T(q, lengths) # FK init fk = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0)) # expMap g = get_expMap(q, si) gtool = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(100, 0, 0)) G = compose(*(g + [gtool])) fk_1 = get_fk(lengths, q) fk_1.between(G) # Jacobian J = get_Jacobian(T, si) Jinv = np.linalg.pinv(J) #check jacobian forward q_dot = np.array([0.5, 0, 1, 0, 0, -1, 0]).T J.dot(q_dot) # check with movement v = np.array([[0, 0, 0, 0, 0, 1]]).T d = np.matmul(Jinv, v) q += np.squeeze(d) fk = get_fk(lengths, q) print fk
def test_transform_from(self): """Test transform form""" T = Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3(1, 2, 3)) pose = Pose3(Rot3(), Point3(1, 2, 3)) actual = transform_from(T, pose) expected = Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3(2, 5, 1)) self.gtsamAssertEquals(actual, expected)
def test_sim3_pose(self): """Test generating similarity transform with Pose3 pairs.""" # Create expected sim3 expected_R = Rot3.Ry(math.radians(180)) expected_s = 2 expected_t = Point3(4, 6, 10) print(expected_R) # Create source poses s_pose1 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])), Point3(0, 0, 0)) s_pose2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, 1]])), Point3(4, 0, 0)) # Create destination poses d_pose1 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])), Point3(4, 6, 10)) d_pose2 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]])), Point3(-4, 6, 10)) # Align pose_pairs = [[s_pose1, d_pose1], [s_pose2, d_pose2]] sim3 = Similarity3() sim3.sim3_pose(pose_pairs) # Check actual sim3 equals to expected sim3 assert (expected_R.equals(sim3._R, 1e-6)) self.assertAlmostEqual(sim3._s, expected_s, delta=1e-6) self.assert_gtsam_equals(sim3._t, expected_t)
def create_initial_estimate(self, pose_indices): """Create initial estimate from ???.""" wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation depth = 20 initial_estimate = gtsam.Values() # Create dictionary for initial estimation for img_idx, features in enumerate(self._image_features): for kp_idx, keypoint in enumerate(features.keypoints): if self.image_points[img_idx].kp_3d_exist(kp_idx): landmark_id = self.image_points[img_idx].kp_3d(kp_idx) # Filter invalid landmarks if self._landmark_estimates[ landmark_id].seen >= self._min_landmark_seen: key_point = Point2(keypoint[0], keypoint[1]) key = P(landmark_id) if not initial_estimate.exists(key): # do back-projection pose = Pose3(wRc, Point3(0, self.delta_z * img_idx, 2)) landmark_3d_point = self.back_projection( key_point, pose, depth) initial_estimate.insert(P(landmark_id), landmark_3d_point) # Initial estimate for poses for idx in pose_indices: pose_i = Pose3(wRc, Point3(0, self.delta_z * idx, 2)) initial_estimate.insert(X(idx), pose_i) return initial_estimate
def test_between(self): """Test between method.""" T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2)) T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) self.gtsamAssertEquals(actual, expected, 1e-6)
def epipolar_inlier_correspondences( keypoints_i1: Keypoints, keypoints_i2: Keypoints, intrinsics_i1: Cal3Bundler, intrinsics_i2: Cal3Bundler, i2Ti1: Pose3, dist_threshold: float, ) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]: """Compute inlier correspondences using epipolar geometry and the ground truth relative pose. Args: keypoints_i1: keypoints in image i1. keypoints_i2: corr. keypoints in image i2. intrinsics_i1: intrinsics for i1. intrinsics_i2: intrinsics for i2. i2Ti1: relative pose dist_threshold: max acceptable distance for a correct correspondence. Returns: is_inlier: (N, ) mask of inlier correspondences. distance_squared: squared sampson distance between corresponding keypoints. """ i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation())) i2Fi1 = verification_utils.essential_to_fundamental_matrix( i2Ei1, intrinsics_i1, intrinsics_i2) distance_squared = verification_utils.compute_epipolar_distances_sq_sampson( keypoints_i1.coordinates, keypoints_i2.coordinates, i2Fi1) is_inlier = distance_squared < dist_threshold**2 if distance_squared is not None else None return is_inlier, distance_squared
def test_values(self): values = Values() E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 values.insert(0, Point2(0, 0)) values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) values.insert(5, Pose3()) values.insert(6, Cal3_S2()) values.insert(7, Cal3DS2()) values.insert(8, Cal3Bundler()) values.insert(9, E) values.insert(10, imuBias_ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision # floating point numbers in column-major (Fortran style) storage order, # whereas by default, numpy.array is in row-major order and the type is # in whatever the number input type is, e.g. np.array([1,2,3]) # will have 'int' type. # # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. # for vectors, the order is not important, but dtype still is vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) self.assertTrue(values.atimuBias_ConstantBias( 10).equals(imuBias_ConstantBias(), tol)) # special cases for Vector and Matrix: actualVector = values.atVector(11) self.assertTrue(np.allclose(vec, actualVector, tol)) actualMatrix = values.atMatrix(12) self.assertTrue(np.allclose(mat, actualMatrix, tol)) actualMatrix2 = values.atMatrix(13) self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def test_compute_translation_to_direction_angle_is_zero(self): i2Ui1_measured = Unit3(Point3(1, 0, 0)) wTi2_estimated = Pose3(Rot3(), Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3(2, 0, 0)) self.assertEqual( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 0.0, )
def run(self): reached = 0 r = rospy.Rate(10) while not rospy.is_shutdown(): goal = self.goals[self.goalindex] self.sTg = Pose3(Rot3.Rz(goal[3]), Point3(goal[0], goal[1], goal[2])) R = quaternion_matrix([ self.sTcurr_ros.pose.orientation.x, self.sTcurr_ros.pose.orientation.y, self.sTcurr_ros.pose.orientation.z, self.sTcurr_ros.pose.orientation.w ])[0:3, 0:3] self.sTcurr = Pose3( Rot3(R), Point3(self.sTcurr_ros.pose.position.x, self.sTcurr_ros.pose.position.y, self.sTcurr_ros.pose.position.z)) currTg = self.sTcurr.between(self.sTg) u_pitch = self.kp_x * currTg.x() + self.kd_x * ( currTg.x() - self.lastT.x()) + min( max(-0.2, self.ki_x * self.ix), 0.2) u_roll = self.kp_y * currTg.y() + self.kd_y * (currTg.y() - self.lastT.y()) currTg_yaw = currTg.rotation().rpy()[2] u_yaw = self.kp_yaw * currTg_yaw + self.kd_yaw * (currTg_yaw - self.lastyaw) u_z = self.kp_x * currTg.z() + self.kd_z * (currTg.z() - self.lastT.z()) self.lastT = currTg self.lastyaw = currTg_yaw self.ix += currTg.x() self.iy += currTg.y() self.iz += currTg.z() self.iyaw += currTg_yaw self.move_cmd.linear.x = min(max(-1, u_pitch), 1) self.move_cmd.linear.y = min(max(-1, u_roll), 1) self.move_cmd.linear.z = min(max(-1, u_z), 1) self.move_cmd.angular.z = min(max(-1, u_yaw), 1) self.velPub.publish(self.move_cmd) reached = self.check(currTg) if (reached == 1): reached = 0 self.goalindex += 1 r.sleep()
def test_get_points_within_radius_of_cameras_negative_radius(): """Catch degenerate input.""" wTi0 = Pose3(Rot3(), np.zeros(3)) wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0])) wTi_list = [wTi0, wTi1] points_3d = np.array([[-15, 0, 0], [0, 15, 0], [-5, 0, 0], [15, 0, 0], [25, 0, 0]]) radius = -5 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) assert nearby_points_3d is None, "Non-positive radius is not allowed"
def test_get_points_within_radius_of_cameras_no_points(): """Catch degenerate input.""" wTi0 = Pose3(Rot3(), np.zeros(3)) wTi1 = Pose3(Rot3(), np.array([10.0, 0, 0])) wTi_list = [wTi0, wTi1] points_3d = np.zeros((0, 3)) radius = 10.0 nearby_points_3d = geometry_comparisons.get_points_within_radius_of_cameras( wTi_list, points_3d, radius) assert nearby_points_3d is None, "At least one 3d point must be provided"
def setUp(self): """Set up the data association object and test data.""" super().setUp() self.obj = DummyDataAssociation(0.5, 2) # set up ground truth data for comparison self.dummy_corr_idxs_dict = { (0, 1): np.array([[0, 2]]), (1, 2): np.array([[2, 3], [4, 5], [7, 9]]), (0, 2): np.array([[1, 8]]), } self.keypoints_list = [ Keypoints(coordinates=np.array([[12, 16], [13, 18], [0, 10]])), Keypoints(coordinates=np.array([ [8, 2], [16, 14], [22, 23], [1, 6], [50, 50], [16, 12], [82, 121], [39, 60], ])), Keypoints(coordinates=np.array([ [1, 1], [8, 13], [40, 6], [82, 21], [1, 6], [12, 18], [15, 14], [25, 28], [7, 10], [14, 17], ])), ] # Generate two poses for use in triangulation tests # Looking along X-axis, 1 meter above ground plane (x-y) upright = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) pose1 = Pose3(upright, Point3(0, 0, 1)) # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) self.poses = Pose3Vector() self.poses.append(pose1) self.poses.append(pose2) # landmark ~5 meters infront of camera self.expected_landmark = Point3(5, 0.5, 1.2)
def test_two_view_correspondences(self): """Tests the bundle adjustment for relative pose on a simulated scene.""" i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation() i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation() i2Ti1 = Pose3(i1Ri2, i1ti2) camera_i1 = PinholeCameraCal3Bundler(Pose3(), Cal3Bundler()) camera_i2 = PinholeCameraCal3Bundler(i2Ti1, Cal3Bundler()) tracks_3d, valid_indices = TwoViewEstimator.triangulate_two_view_correspondences( camera_i1, camera_i2, self.keypoints_i1, self.keypoints_i2, self.corr_idxs) self.assertEqual(len(tracks_3d), 5) self.assertEqual(len(valid_indices), 5)
def setUp(self): """Set up two camera poses""" # Looking along X-axis, 1 meter above ground plane (x-y) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) # twoPoses self.poses = Pose3Vector() self.poses.append(pose1) self.poses.append(pose2) # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2)
def test_compare_global_poses_scaled_squares(self): """Make sure a big and small square can be aligned. The u's represent a big square (10x10), and v's represents a small square (4x4). """ R0 = Rotation.from_euler("z", 0, degrees=True).as_matrix() R90 = Rotation.from_euler("z", 90, degrees=True).as_matrix() R180 = Rotation.from_euler("z", 180, degrees=True).as_matrix() R270 = Rotation.from_euler("z", 270, degrees=True).as_matrix() wTu0 = Pose3(Rot3(R0), np.array([2, 3, 0])) wTu1 = Pose3(Rot3(R90), np.array([12, 3, 0])) wTu2 = Pose3(Rot3(R180), np.array([12, 13, 0])) wTu3 = Pose3(Rot3(R270), np.array([2, 13, 0])) wTi_list = [wTu0, wTu1, wTu2, wTu3] wTv0 = Pose3(Rot3(R0), np.array([4, 3, 0])) wTv1 = Pose3(Rot3(R90), np.array([8, 3, 0])) wTv2 = Pose3(Rot3(R180), np.array([8, 7, 0])) wTv3 = Pose3(Rot3(R270), np.array([4, 7, 0])) wTi_list_ = [wTv0, wTv1, wTv2, wTv3] pose_graphs_equal = geometry_comparisons.compare_global_poses( wTi_list, wTi_list_) self.assertTrue(pose_graphs_equal)
def test_computation_graph(self): """Test the dask computation graph execution using a valid collection of relative unit-translations.""" """Test a simple case with 8 camera poses. The camera poses are arranged on the circle and point towards the center of the circle. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. This test is copied from GTSAM's TranslationAveragingExample. """ fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 expected_wTi_list = SFMdata.createPoses(Cal3_S2(fx, fy, s, u0, v0)) wRi_list = [x.rotation() for x in expected_wTi_list] # create relative translation directions between a pose index and the # next two poses i2Ui1_dict = {} for i1 in range(len(expected_wTi_list) - 1): for i2 in range(i1 + 1, min(len(expected_wTi_list), i1 + 3)): # create relative translations using global R and T. i2Ui1_dict[(i1, i2)] = Unit3(expected_wTi_list[i2].between( expected_wTi_list[i1]).translation()) # use the `run` API to get expected results expected_wti_list = self.obj.run(len(wRi_list), i2Ui1_dict, wRi_list) expected_wTi_list = [ Pose3(wRi, wti) if wti is not None else None for (wRi, wti) in zip(wRi_list, expected_wti_list) ] # form computation graph and execute i2Ui1_graph = dask.delayed(i2Ui1_dict) wRi_graph = dask.delayed(wRi_list) computation_graph = self.obj.create_computation_graph( len(wRi_list), i2Ui1_graph, wRi_graph) with dask.config.set(scheduler="single-threaded"): wti_list = dask.compute(computation_graph)[0] wTi_list = [ Pose3(wRi, wti) if wti is not None else None for (wRi, wti) in zip(wRi_list, wti_list) ] # compare the entries self.assertTrue( geometry_comparisons.compare_global_poses(wTi_list, expected_wTi_list))
def test_load_poses_from_file(self): """Test load psoes from file.""" file_name = self.data_directory+"result/poses.dat" poses = load_poses_from_file(file_name) # """Test pose output""" delta_z = 1 num_frames = 3 wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) for i in range(num_frames): # Create ground truth poses expected_pose_i = Pose3(wRc, Point3(0, delta_z*i, 2)) # Get actual poses rotation = Rot3(np.array(poses[i][3:]).reshape(3, 3)) actual_pose_i = Pose3(rotation, Point3(np.array(poses[i][0:3]))) self.gtsamAssertEquals(actual_pose_i, expected_pose_i, 1e-6)
def get_ortho_axis_alignment_transform(gtsfm_data: GtsfmData) -> Pose3: """Wrapper function for all the functions in ellipsoid.py. Obtains the Pose3 transformation required to align the GtsfmData to the x,y,z axes. Args: gtsfm_data: scene data to write to transform. Returns: The final transformation required to align point cloud and frustums. """ # Iterate through each track to gather a list of 3D points forming the point cloud. num_pts = gtsfm_data.number_tracks() point_cloud = [gtsfm_data.get_track(j).point3() for j in range(num_pts)] point_cloud = np.array(point_cloud) # point_cloud has shape Nx3 # Filter outlier points, Center point cloud, and obtain alignment rotation. points_filtered, inlier_mask = remove_outlier_points(point_cloud) points_centered = center_point_cloud(points_filtered) wuprightRw = get_alignment_rotation_matrix_from_svd(points_centered) # Calculate translation vector based off rotated point cloud (excluding outliers). point_cloud_rotated = point_cloud @ wuprightRw.T rotated_mean = np.mean(point_cloud_rotated[inlier_mask], axis=0) # Obtain the Pose3 object needed to align camera frustums. walignedTw = Pose3(Rot3(wuprightRw), -1 * rotated_mean) return walignedTw
def test_back_projection(self): """Test back projection""" actual = self.back_end.back_projection( Point2(640, 360), Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3()), 20) expected = Point3(0, 20, 0) self.gtsamAssertEquals(actual, expected)
def main(): """Main runner.""" args = parse_args() kitti_calibration, imu_measurements, gps_measurements = loadKittiData() if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): raise ValueError( "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" ) # Configure different variables first_gps_pose = 1 gps_skip = 10 # Configure noise models noise_model_gps = noiseModel.Diagonal.Precisions( np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) sigma_init_x = noiseModel.Diagonal.Precisions( np.asarray([0, 0, 0, 1, 1, 1])) sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) sigma_init_b = noiseModel.Diagonal.Sigmas( np.asarray([0.1] * 3 + [5.00e-05] * 3)) isam = optimize(gps_measurements, imu_measurements, sigma_init_x, sigma_init_v, sigma_init_b, noise_model_gps, kitti_calibration, first_gps_pose, gps_skip) save_results(isam, args.output_filename, first_gps_pose, gps_measurements)
def setUp(self): """Create mapping Back-end and read csv file.""" # Input images(undistorted) calibration fov, w, h = 60, 1280, 720 calibration = Cal3_S2(fov, w, h) # Create pose priors wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation pose_estimates = [Pose3(wRc, Point3(0, i, 2)) for i in range(3)] # Create measurement noise for bundle adjustment sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # Create pose prior noise rotation_sigma = np.radians(60) translation_sigma = 1 pose_noise_sigmas = np.array([ rotation_sigma, rotation_sigma, rotation_sigma, translation_sigma, translation_sigma, translation_sigma ]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) # Create MappingBackEnd instance data_directory = 'mapping/sim_match_data/' min_landmark_seen = 3 self.num_images = 3 self.back_end = MappingBackEnd( data_directory, self.num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_landmark_seen) # pylint: disable=line-too-long
def view_scene(args: argparse.Namespace) -> None: """Read GTSFM output from .txt files and render the scene to the GUI. We also zero-center the point cloud, and transform camera poses to a new world frame, where the point cloud is zero-centered. Args: args: rendering options. """ points_fpath = f"{args.output_dir}/points3D.txt" images_fpath = f"{args.output_dir}/images.txt" cameras_fpath = f"{args.output_dir}/cameras.txt" # Read in data. wTi_list, img_fnames, calibrations, point_cloud, rgb = io_utils.read_scene( images_fpath, cameras_fpath, points_fpath) if args.show_mvs_result: point_cloud, rgb = io_utils.read_point_cloud_from_ply(args.ply_fpath) if len(calibrations) == 1: calibrations = calibrations * len(img_fnames) mean_pt = compute_point_cloud_center_robust(point_cloud) # Zero-center the point cloud (about estimated center). zcwTw = Pose3(Rot3(np.eye(3)), -mean_pt) # expression below is equivalent to applying zcwTw.transformFrom() to each world point point_cloud -= mean_pt is_nearby = np.linalg.norm(point_cloud, axis=1) < args.max_range point_cloud = point_cloud[is_nearby] rgb = rgb[is_nearby] for i in range(len(wTi_list)): wTi_list[i] = zcwTw.compose(wTi_list[i]) draw_scene_open3d(point_cloud, rgb, wTi_list, calibrations, args)
def decompose_camera_projection_matrix(M: np.ndarray) -> Pose3: """Recovera camera pose from a 3x4 projection matrix. Reference: https://johnwlambert.github.io/cam-calibration/#decomposition Args: M: array of shape (3,4) representing camera projection matrix. Let M = [m1 | m2 | m3 | m4] = [ Q | m4] Returns: K: 3x3 upper triangular camera intrinsics matrix. The matrix diagonal is guaranteed to contain all positive entries. wTc: camera pose in world frame. """ m4 = M[:, 3] Q = M[:3, :3] wtc = np.linalg.inv(-Q) @ m4 K, cRw = scipy.linalg.rq(Q) # multiply columns of K by -1 if K(i,i) entry on diagonal is negative. # T is a matrix that scales by -1 or 1 T = np.diag(np.sign(np.diag(K))) K = K @ T # scale rows of cRw by T, to keep a valid decomposition. T is its own inverse. wRc = Rot3(T @ cRw).inverse() wTc = Pose3(wRc, wtc) return K, wTc
def test_round_trip_images_txt(self) -> None: """Starts with a pose. Writes the pose to images.txt (in a temporary directory). Then reads images.txt to recover that same pose. Checks if the original wTc and recovered wTc match up.""" # fmt: off # Rotation 45 degrees about the z-axis. original_wRc = np.array([[np.cos(np.pi / 4), -np.sin(np.pi / 4), 0], [np.sin(np.pi / 4), np.cos(np.pi / 4), 0], [0, 0, 1]]) original_wtc = np.array([3, -2, 1]) # fmt: on # Setup dummy GtsfmData Object with one image original_wTc = Pose3(Rot3(original_wRc), original_wtc) default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0) camera = PinholeCameraCal3Bundler(original_wTc, default_intrinsics) gtsfm_data = GtsfmData(number_images=1) gtsfm_data.add_camera(0, camera) image = Image(value_array=None, file_name="dummy_image.jpg") images = [image] # Perform write and read operations inside a temporary directory with tempfile.TemporaryDirectory() as tempdir: images_fpath = os.path.join(tempdir, "images.txt") io_utils.write_images(gtsfm_data, images, tempdir) wTi_list, _ = io_utils.read_images_txt(images_fpath) recovered_wTc = wTi_list[0] npt.assert_almost_equal(original_wTc.matrix(), recovered_wTc.matrix(), decimal=3)
def test_bundle_adjust(self): """Tests the bundle adjustment for relative pose on a simulated scene.""" two_view_estimator = TwoViewEstimator(matcher=None, verifier=None, inlier_support_processor=None, bundle_adjust_2view=True, eval_threshold_px=4) # Extract example poses. i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation() i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation() i2Ti1 = Pose3(i1Ri2, i1ti2).inverse() i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation())) # Perform bundle adjustment. i2Ri1_optimized, i2Ui1_optimized, corr_idxs = two_view_estimator.bundle_adjust( keypoints_i1=self.keypoints_i1, keypoints_i2=self.keypoints_i2, verified_corr_idxs=self.corr_idxs, camera_intrinsics_i1=Cal3Bundler(), camera_intrinsics_i2=Cal3Bundler(), i2Ri1_initial=i2Ei1.rotation(), i2Ui1_initial=i2Ei1.direction(), i2Ti1_prior=None, ) # Assert rotation_angular_error = comp_utils.compute_relative_rotation_angle( i2Ri1_optimized, i2Ei1.rotation()) translation_angular_error = comp_utils.compute_relative_unit_translation_angle( i2Ui1_optimized, i2Ei1.direction()) self.assertLessEqual(rotation_angular_error, 1) self.assertLessEqual(translation_angular_error, 1) np.testing.assert_allclose(corr_idxs, self.corr_idxs)