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_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 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 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 createPointsLines() -> Tuple[List[Point3],List[LineSegment3D], List[np.array]]: step = 5 points = [ Point3(-1, -1, 0), Point3(-1, 1, 0), Point3(1, -1, 0), Point3(1, 1, 0), Point3(-1.5, 1, -1), Point3(1.5, 1, -1), Point3(-1.5, -1, -1), Point3(1.5, -1, -1), Point3(-1.5, 0, -0.5), Point3(1.5, 0, -0.5) ] lines = [ LineSegment3D(points[0], points[1]), LineSegment3D(points[2], points[3]), LineSegment3D(points[4], points[5]), LineSegment3D(points[6], points[7]), LineSegment3D(points[8], points[9]) ] semantics = [0.8, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] lines_array = [] for i in range(step): l = np.append(points[i*2], points[i*2 + 1]) lines_array.append(l) return [points, lines, semantics]
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 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 test_back_projection(self): """test back projection""" cal = Cal3DS2(fx=1, fy=1, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0) point = Point2(320, 240) pose = Pose3(Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]).T), Point3(0, 2, 1.2)) actual_point = back_projection(cal, point, pose) actual_point.equals(Point3(10, 2, 1.2), tol=1e-9)
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 define_microphones(): """Create microphones.""" height = 0.5 microphones = [] microphones.append(Point3(0, 0, height)) microphones.append(Point3(403 * CM, 0, height)) microphones.append(Point3(403 * CM, 403 * CM, height)) microphones.append(Point3(0, 403 * CM, 2 * height)) return microphones
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 create_points(): # Create the set of ground-truth landmarks points = [ Point3(10.0, 0.0, 0.0), Point3(10.0, 5.0, 0.0), Point3(10.0, 2.5, 2.5), Point3(10.0, 0.0, 5.0), Point3(10.0, 5.0, 5.0) ] return points
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 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_level2(self): # Create a level camera, looking in Y-direction pose2 = Pose2(0.4, 0.3, math.pi / 2.0) camera = SimpleCamera.Level(K, pose2, 0.1) # expected x = Point3(1, 0, 0) y = Point3(0, 0, -1) z = Point3(0, 1, 0) wRc = Rot3(x, y, z) expected = Pose3(wRc, Point3(0.4, 0.3, 0.1)) self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
def test_transformFrom(self): """Test transformFrom method.""" pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi / 2), Point3(2, 4, 0)) actual = pose.transformFrom(Point3(2, 1, 10)) expected = Point3(3, 2, 10) self.gtsamAssertEquals(actual, expected, 1e-6) # multi-point version points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T actual_array = pose.transformFrom(points) self.assertEqual(actual_array.shape, (3, 2)) expected_array = np.stack([expected, expected]).T np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
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 createPoses(K: Cal3_S2) -> List[Pose3]: """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" radius = 4.0 height = -2.0 angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) up = Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ??? target = Point3(0, 0, 0) poses = [] for theta in angles: position = Point3(radius * np.cos(theta), radius * np.sin(theta), height) camera = PinholeCameraCal3_S2.Lookat(position, target, up, K) poses.append(camera.pose()) return poses
def plot_trajectory_verification(landmarks, poses, trajectory, x_axe=30, y_axe=30, z_axe=30, axis_length=2, figure_number=0): """Plot the map, mapping pose results, and the generated trajectory. Parameters: landmarks - a list of [x,y,z] poses - a list of [x,y,z,1*9 rotation matrix] trajectory - a list of Pose3 object """ # Declare an id for the figure fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot landmark points for landmark in landmarks: gtsam_plot.plot_point3(figure_number, Point3(landmark[0], landmark[1], landmark[2]), 'rx') # Plot mapping pose results for pose in poses: rotation = Rot3(np.array(pose[3:]).reshape(3, 3)) actual_pose_i = Pose3(rotation, Point3(np.array(pose[0:3]))) gtsam_plot.plot_pose3(figure_number, actual_pose_i, axis_length) gRp = actual_pose_i.rotation().matrix() # rotation from pose to global t = actual_pose_i.translation() axes.scatter([t.x()], [t.y()], [t.z()], s=20, color='red', alpha=1, marker="^") # Plot cameras for pose in trajectory: gtsam_plot.plot_pose3(figure_number, pose, axis_length) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() axes.scatter([t.x()], [t.y()], [t.z()], s=20, color='blue', alpha=1) # draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(-z_axe, z_axe) plt.legend() plt.show()
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 createPointsLines() -> Tuple[List[Point3], List[Line3D]]: points = [ Point3(-1, -1, 0), Point3(-1, 1, 0), Point3(1, -1, 0), Point3(1, 1, 0), Point3(-1.5, 1, -1), Point3(1.5, 1, -1), Point3(-1.5, -1, -1), Point3(1.5, -1, -1), Point3(-1.5, 0, -0.5), Point3(1.5, 0, -0.5) ] lines = [Line3D(points[0], points[1]), Line3D(points[0], points[1]), Line3D(points[2], points[3]), Line3D(points[2], points[3]), Line3D(points[4], points[5]), Line3D(points[4], points[5]), Line3D(points[6], points[7]), Line3D(points[6], points[7]), Line3D(points[8], points[9]), Line3D(points[8], points[9]),] return [points, lines]
def landmark_projection(self, pose): """ Project landmark points in the map to the given camera pose. And filter landmark points outside the view of the current camera pose. Parameters: pose - gtsam.Point3, the pose of a camera self.map - A Landmarks Object Returns: observed_landmarks - A Landmarks Object """ # Check if the atrium map is empty assert self.map.get_length(), "the map is empty" observed_landmarks = Landmarks([], []) for i, landmark_point in enumerate(self.map.landmarks): camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration) # feature is gtsam.Point2 object landmark_point = Point3(landmark_point[0], landmark_point[1], landmark_point[2]) feature_point = camera.project(landmark_point) # Check if the projected feature is within the field of view. if (feature_point.x() >= 0 and feature_point.x() < self.width and feature_point.y() >= 0 and feature_point.y() < self.height): observed_landmarks.append( self.map.landmarks[i], self.map.descriptors[i], [feature_point.x(), feature_point.y()]) return observed_landmarks
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 test_compute_translation_to_direction_angle_is_nonzero(self): rz = np.deg2rad(90) wRi2 = Rot3.RzRyRx(0, 0, rz) # x-axis of i2 points along y in world frame wTi2_estimated = Pose3(wRi2, Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3( -1, 0, 0)) # At (0, 1, 0) in i2 frame, rotation of i1 is irrelevant here. i2Ui1_measured = Unit3(Point3(1, 0, 0)) # Estimated relative translation of i1 in i2 frame is (0, 1, 0), and the measurement in i2 frame is (1, 0, 0). # Expected angle between the two is 90 degrees. self.assertTrue( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 90.0, )
def verify_with_exact_intrinsics( self, keypoints_i1: Keypoints, keypoints_i2: Keypoints, match_indices: np.ndarray, camera_intrinsics_i1: Cal3Bundler, camera_intrinsics_i2: Cal3Bundler, ) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray]: """Estimates the essential matrix and verifies the feature matches. Note: this function is preferred when camera intrinsics are approximate (i.e from image size/exif). The feature coordinates are used to compute the fundamental matrix, which is then converted to the essential matrix. Args: keypoints_i1: detected features in image #i1. keypoints_i2: detected features in image #i2. match_indices: matches as indices of features from both images, of shape (N3, 2), where N3 <= min(N1, N2). camera_intrinsics_i1: intrinsics for image #i1. camera_intrinsics_i2: intrinsics for image #i2. Returns: Estimated rotation i2Ri1, or None if it cannot be estimated. Estimated unit translation i2Ui1, or None if it cannot be estimated. Indices of verified correspondences, of shape (N, 2) with N <= N3. These are subset of match_indices. """ v_inlier_idxs = np.array([], dtype=np.uint32) # check if we don't have the minimum number of points if match_indices.shape[0] < self.min_pts: return None, None, v_inlier_idxs # set a random seed using descriptor data for repeatability np.random.seed( int(1000 * (match_indices[0, 0] + match_indices[0, 1]) % (UINT32_MAX))) # get the number of entries in the input num_matches = match_indices.shape[0] # get the number of verified_pts we will output num_verifier_pts = np.random.randint(low=0, high=num_matches) # randomly sample the indices for matches which will be verified v_inlier_idxs = np.random.choice(num_matches, num_verifier_pts, replace=False).astype(np.uint32) # use a random 3x3 matrix if the number of verified points are less that if num_verifier_pts >= self.min_pts: # generate random rotation and translation for essential matrix rotation_angles = np.random.uniform(low=0.0, high=2 * np.pi, size=(3, )) i2Ri1 = Rot3.RzRyRx(rotation_angles[0], rotation_angles[1], rotation_angles[2]) i2Ti1 = Point3(np.random.uniform(low=-1.0, high=1.0, size=(3, ))) return i2Ri1, Unit3(i2Ti1), match_indices[v_inlier_idxs] else: return None, None, match_indices[v_inlier_idxs]
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 run(args: Namespace) -> None: """Run LAGO on input data stored in g2o file.""" g2oFile = gtsam.findExampleDataFile( "noisyToyGraph.txt") if args.input is None else args.input graph = gtsam.NonlinearFactorGraph() graph, initial = gtsam.readG2o(g2oFile) # Add prior on the pose having index (key) = 0 priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8)) graph.add(PriorFactorPose2(0, Pose2(), priorModel)) print(graph) print("Computing LAGO estimate") estimateLago: Values = gtsam.lago.initialize(graph) print("done!") if args.output is None: estimateLago.print("estimateLago") else: outputFile = args.output print("Writing results to file: ", outputFile) graphNoKernel = gtsam.NonlinearFactorGraph() graphNoKernel, initial2 = gtsam.readG2o(g2oFile) gtsam.writeG2o(graphNoKernel, estimateLago, outputFile) print("Done! ")
def test_landmark_projection(self): """Test landmark projection.""" wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name pose = Pose3(wRc, Point3(0, 0, 1.2)) depth = 10 # Calculate the camera field of view area vision_area = calculate_vision_area(self.calibration, pose, depth, self.image_size) points = [ vision_area[0], vision_area[1], vision_area[2], vision_area[3], [1, 10, 10], [10, 10, 6000] ] map_data = Landmarks( points, [[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0]]) self.trajectory_estimator.load_map(map_data) expected_observed_landmarks = Landmarks( points[:5], [[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1]]) actual_observed_landmarks = self.trajectory_estimator.landmark_projection( pose) if (expected_observed_landmarks == actual_observed_landmarks) is False: print("Expected_observed_landmarks: ", expected_observed_landmarks.landmarks, expected_observed_landmarks.descriptors) print("Actual_observed_landmarks: ", actual_observed_landmarks.landmarks, actual_observed_landmarks.descriptors) self.assertEqual( expected_observed_landmarks == actual_observed_landmarks, True)
def test_data(self): """Test functions in SfmData""" # Create new track with 3 measurements i1, i2, i3 = 3, 5, 6 uv_i1 = Point2(21.23, 45.64) # translating along X-axis uv_i2 = Point2(45.7, 45.64) uv_i3 = Point2(68.35, 45.64) # add measurements and arbitrary point to the track measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] pt = Point3(1.0, 6.0, 2.0) track2 = SfmTrack(pt) track2.addMeasurement(i1, uv_i1) track2.addMeasurement(i2, uv_i2) track2.addMeasurement(i3, uv_i3) self.data.addTrack(self.tracks) self.data.addTrack(track2) # Number of tracks in SfmData is 2 self.assertEqual(self.data.numberTracks(), 2) # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1)
def run(): """Execution.""" # Input images(undistorted) calibration fov, w, h = 60, 1280, 720 calibration = Cal3_S2(fov, w, h) # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name # Create pose estimates 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/datasets/sim_match_data/' num_images = 3 min_landmark_seen = 3 back_end = MappingBackEnd(data_directory, num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_landmark_seen) # Bundle Adjustment tic_ba = time.time() sfm_result, poses, points = back_end.bundle_adjustment() toc_ba = time.time() print('BA spents ', toc_ba - tic_ba, 's') # Plot Result plot_sfm_result(sfm_result, poses, points)