def setUpClass(cls): """ Equidistant fisheye projection An equidistant fisheye projection with focal length f is defined as the relation r/f = tan(theta), with r being the radius in the image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) p1 = [-1.0, 0.0, -1.0] p2 = [1.0, 0.0, -1.0] q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) pose1 = gtsam.Pose3(q1, p1) pose2 = gtsam.Pose3(q2, p2) camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) cls.origin = np.array([0.0, 0.0, 0.0]) cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) cls.measurements = gtsam.Point2Vector( [k.project(cls.origin) for k in cls.cameras])
def setUpClass(cls): """ Stereographic fisheye projection An equidistant fisheye projection with focal length f is defined as the relation r/f = 2*tan(theta/2), with r being the radius in the image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 r = np.linalg.norm([x, y, z]) u, v = 2*x/(z+r), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 k1, k2, p1, p2 = 0, 0, 0, 0 xi = 1 cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) p1 = [-1.0, 0.0, -1.0] p2 = [ 1.0, 0.0, -1.0] q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) pose1 = gtsam.Pose3(q1, p1) pose2 = gtsam.Pose3(q2, p2) camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) cls.origin = np.array([0.0, 0.0, 0.0]) cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential keys starting from 0. An optional character may be provided, which will be stored in the msb of each key (i.e. gtsam.Symbol). We use aerospace/navlab convention, X forward, Y right, Z down First pose will be at (R,0,0) ^y ^ X | | z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) Vehicle at p0 is looking towards y axis (X-axis points towards world y) """ values = gtsam.Values() theta = 0.0 dtheta = 2 * pi / numPoses gRo = gtsam.Rot3( np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0) oRi = gtsam.Rot3.Yaw( -theta) # negative yaw goes counterclockwise, with Z down ! gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta return values
def test_find(self): # Check that optimizing for Karcher mean (which minimizes Between distance) # gets correct result. rotations = {R, R.inverse()} expected = gtsam.Rot3() actual = find_Karcher_mean_Rot3(rotations) self.gtsamAssertEquals(expected, actual)
def sim3_transform_map(result, nrCameras, nrPoints): # Similarity transform s_poses = [] s_points = [] _sim3 = sim3.Similarity3() for i in range(nrCameras): pose_i = result.atPose3(symbol(ord('x'), i)) s_poses.append(pose_i) for j in range(nrPoints): point_j = result.atPoint3(symbol(ord('p'), j)) s_points.append(point_j) theta = np.radians(30) wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0, math.cos(theta)], [-math.cos(theta), 0, -math.sin(theta)], [0, 1, 0]])) d_pose1 = gtsam.Pose3( wRc, gtsam.Point3(-math.sin(theta)*1.58, -math.cos(theta)*1.58, 1.2)) d_pose2 = gtsam.Pose3( wRc, gtsam.Point3(-math.sin(theta)*1.58*3, -math.cos(theta)*1.58*3, 1.2)) pose_pairs = [[s_poses[2], d_pose2], [s_poses[0], d_pose1]] _sim3.align_pose(pose_pairs) print('R', _sim3._R) print('t', _sim3._t) print('s', _sim3._s) s_map = (s_poses, s_points) actual_poses, actual_points = _sim3.map_transform(s_map) d_map = _sim3.map_transform(s_map) return _sim3, d_map
def setUp(self): self.testclouda = np.array([[1], [1], [1]]) self.testcloudb = np.array([[2, 10], [1, 1], [1, 1]]) self.testcloudc = np.array([[2], [1], [1]]) self.testbTa = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)) self.testcloudd = np.array([[0, 20, 10], [0, 10, 20], [0, 0, 0]]) self.testcloude = np.array([[10, 30, 20], [10, 20, 30], [0, 0, 0]])
def create_past_poses(self, delta): """Create the set of ground-truth poses of the last time step.""" for i, y in enumerate([0, 2.5, 5, 7.5, 10]): wRc = gtsam.Rot3( np.array([[0 + delta, 1 + delta, 0 + delta], [0 + delta, 0 - delta, -1 + delta], [1 - delta, 0 + delta, 0 - delta]]).T) self.past_poses.append( gtsam.Pose3(wRc, gtsam.Point3(0, y - delta, 1.5)))
def ExampleValues(): T = [] T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) data = gtsam.Values() for i in range(len(T)): data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i])) return data
def find_Karcher_mean_Rot3(rotations): """Find the Karcher mean of given values.""" # Cost function C(R) = \sum PriorFactor(R_i)::error(R) # No closed form solution. graph = gtsam.NonlinearFactorGraph() for R in rotations: graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) initial = gtsam.Values() initial.insert(KEY, gtsam.Rot3()) result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() return result.atRot3(KEY)
def setUp(cls): test_clouds = read_ply(*scans_fnames)[:6] PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) test_graph = gtsam.NonlinearFactorGraph() test_initial_estimates = gtsam.Values() initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) test_initial_estimates.insert(0, initial_pose) test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds) cls.graph = test_graph cls.initial_estimates = test_initial_estimates
def test_insertProjectionFactors(self): """Test insertProjectionFactors.""" graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) self.assertEqual(graph.size(), 2) graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) self.assertEqual(graph.size(), 2)
def setUp(self): """Set up two constraints to test.""" aTb = gtsam.Pose3(gtsam.Rot3.Yaw(np.pi / 2), gtsam.Point3(1, 2, 3)) cov = gtsam.noiseModel.Isotropic.Variance(6, 1e-3).covariance() counts = np.zeros((5, 5)) counts[0][0] = 200 self.a_constraint_b = Constraint(1, 2, aTb, cov, counts) aTc = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.5, 0.5, 0.5)) variances = np.array([1e-4, 1e-5, 1e-6, 1e-1, 1e-2, 1e-3]) aCOVc = gtsam.noiseModel.Diagonal.Variances(variances).covariance() counts = np.zeros((5, 5)) counts[1][2] = 100 self.a_constraint_c = Constraint(1, 3, aTc, aCOVc, counts)
def horn_align(trajectory1, trajectory2): """ Aligns trajectory1 with trajectory2 using HORN """ xyz1 = np.matrix([ p.translation().vector() for p in trajectory1.data() ]).transpose() xyz2 = np.matrix([ p.translation().vector() for p in trajectory2.data() ]).transpose() R, T, trans_error = Evaluation._horn_align(xyz1, xyz2) transform = gtsam.Pose3(gtsam.Rot3(R), gtsam.Point3( np.array(T)[:, 0])) # T = [[x],[y],[z]] [:,0] = tranpose()[0] warped_trajectory = trajectory1.applyTransform(transform) # print('trans_error: ', np.sqrt(np.dot(trans_error,trans_error) / len(trans_error))) return warped_trajectory
def bounds_3D(self, box3D, pose, calibration, color=(255, 0, 255)): # convert 3D box to set of 8 points b = box3D.vector() points_3D = np.array([[x, y, z] for x in b[0:2] for y in b[2:4] for z in b[4:6]]) # ensure points infront of camera for point in points_3D: if pose.between(gtsam.Pose3( gtsam.Rot3(), gtsam.Point3(point))).translation().vector()[-1] < 0: return # project 3D points to 2D P = quadricslam.QuadricCamera.transformToImage(pose, calibration) points_3DTH = np.concatenate( (points_3D.T, np.ones((1, points_3D.shape[0])))) points_2D = P.dot(points_3DTH) points_2D = points_2D[0:2, :] / points_2D[2] points_2D = points_2D.transpose() # only draw if all points project correctly if np.any(np.isinf(points_2D)) or np.any(np.isnan(points_2D)): return # draw points for point in points_2D: cv2.circle(self._image, tuple(point.astype('int')), 1, color=(0, 0, 255), thickness=-1, lineType=cv2.LINE_AA) # draw lines for index in [(0, 1), (1, 3), (3, 2), (2, 0), (4, 5), (5, 7), (7, 6), (6, 4), (2, 6), (1, 5), (0, 4), (3, 7)]: p1 = tuple(points_2D[index[0]].astype('int')) p2 = tuple(points_2D[index[1]].astype('int')) cv2.line(self._image, p1, p2, color=color, thickness=1, lineType=cv2.LINE_AA)
def add_sfm_factors(graph, init_est, dataset_path, scale): f = open(dataset_path + '/reconstruction.json', 'r') data = json.load(f) images = data[0]['shots'] for img in images.items(): #print('Adding sfm factor for ' + img[0]) img_num = int(img[0].split('.')[0]) pos = np.array(img[1]['translation']) / scale rot_aa = np.array(img[1]['rotation']) #axis-angle rot_gtsam = gtsam.Rot3((Rotation.from_rotvec(rot_aa)).as_dcm()) pos = (Rotation.from_rotvec(rot_aa)).inv().as_dcm() @ pos pos[2] = -pos[2] trans_gtsam = gtsam.Point3(pos) pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam) factor = gtsam.PriorFactorPose3(C(img_num), pose_gtsam, sfm_noise) graph.push_back(factor) init_est.insert(C(img_num), pose_gtsam)
def get_rotation_from_axis(axis, angle): """ Compute the matrix that rotates around `axis` by `angle` radians. See https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle. axis: np.ndarray, shape == (3,) angle: float return: gtsam.Rot3 """ axis = axis / np.linalg.norm(axis) return gtsam.Rot3( np.cos(angle) * np.eye(3) + \ np.sin(angle) * gtsam.Unit3(gtsam.Point3(axis)).skew() + \ (1 - np.cos(angle)) * np.outer(axis, axis) )
def sequence1(): """ a manually generated dataset sequence """ points = [] points.append(gtsam.Point3(10, 0, 0)) points.append(gtsam.Point3(0, -10, 0)) points.append(gtsam.Point3(-10, 0, 0)) points.append(gtsam.Point3(0, 10, 0)) points.append(gtsam.Point3(10, 0, 0)) quadrics = [] quadrics.append( quadricslam.ConstrainedDualQuadric(gtsam.Pose3(), np.array([0.2, 0.3, 0.4]))) quadrics.append( quadricslam.ConstrainedDualQuadric( gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.2, 0.2, 0.2)), np.array([0.2, 0.3, 0.4]))) sequence = SimulatedSequence(points, quadrics) return sequence
def add_sfm_factors_gt(graph, init_est, dataset_path): f = open(dataset_path + '/ep_data.pkl', 'rb') data = pickle.load(f) for ind in range(len(data['cam_pose'])): pose_mat = np.eye(4) quat = data['cam_pose'][ind][3:] quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0] pose_mat[:3, :3] = ( Rotation.from_quat(quat) * Rotation.from_euler('xyz', [0, np.pi, 0])).as_dcm() pose_mat[:3, 3] = data['cam_pose'][ind][:3] pos = pose_mat[:3, 3] rot_gtsam = gtsam.Rot3(pose_mat[:3, :3]) trans_gtsam = gtsam.Point3(pos) pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam) factor = gtsam.PriorFactorPose3(C(ind), pose_gtsam, sfm_noise) graph.push_back(factor) init_est.insert(C(ind), pose_gtsam)
def add_pose_factors(graph, init_est, dataset_path): f = open(dataset_path + '/obj_det_poses.pkl', 'rb') data = pickle.load(f) cnt = 0 for pose in data.items(): #print('Adding pose det factor for ' + pose[0]) num = int(pose[0].split('.')[0]) pos = gtsam.Point3(pose[1][:3]) quat = pose[1][3:] #wxyz to xyzw quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0] rot = gtsam.Rot3((Rotation.from_quat(quat)).as_dcm()) pose = gtsam.Pose3(rot, pos) factor = gtsam.BetweenFactorPose3(C(num), O(0), pose, pose_noise) print('new factor') graph.push_back(factor) cnt += 1 init_est.insert(O(0), gtsam.Pose3()) #identity
def test_PriorWeights(self): lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults() params = ShonanAveragingParameters3(lmParams) self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9) self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9) self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9) alpha, beta, gamma = 100.0, 200.0, 300.0 params.setAnchorWeight(alpha) params.setKarcherWeight(beta) params.setGaugesWeight(gamma) self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9) self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9) self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9) params.setKarcherWeight(0) shonan = fromExampleName("Klaus3.g2o", params) initial = gtsam.Values() for i in range(3): initial.insert(i, gtsam.Rot3()) self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
def sim3_transform_map(self, result, img_pose_id_list, landmark_id_list): # Similarity transform s_poses = [] s_points = [] _sim3 = sim3.Similarity3() theta = np.radians(30) wRc = gtsam.Rot3( np.array([[-math.sin(theta), 0, math.cos(theta)], [-math.cos(theta), 0, -math.sin(theta)], [0, 1, 0]])) pose_pairs = [] for i, idx in enumerate(img_pose_id_list): pose_i = result.atPose3(symbol(ord('x'), idx)) s_poses.append(pose_i) if i < 2: d_pose = gtsam.Pose3( wRc, gtsam.Point3(-math.sin(theta) * 1.58 * idx, -math.cos(theta) * 1.58 * idx, 1.2)) pose_pairs.append([s_poses[i], d_pose]) i += 1 for idx in landmark_id_list: point_j = result.atPoint3(symbol(ord('p'), idx)) s_points.append(point_j) _sim3.align_pose(pose_pairs) print('R', _sim3._R) print('t', _sim3._t) print('s', _sim3._s) s_map = (s_poses, s_points) actual_poses, actual_points = _sim3.map_transform(s_map) d_map = _sim3.map_transform(s_map) return _sim3, d_map
Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour. """ # load in all clouds in our dataset clouds = read_ply(*scans_fnames) # Setting up our factor graph graph = gtsam.NonlinearFactorGraph() initial_estimates = gtsam.Values() # We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas( np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) initial_pose = gtsam.Pose3( gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) initial_estimates.insert(0, initial_pose) # We'll use your function to populate the factor graph graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds) # Now optimize for the states parameters = gtsam.GaussNewtonParams() parameters.setRelativeErrorTol(1e-5) parameters.setMaxIterations(100) optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters) result = optimizer.optimize() """Let's plot these poses to see how our vechicle moves.
""" This file is not a real python unittest. It contains small experiments to test the wrapper with gtsam_test, a short version of gtsam.h. Its name convention is different from other tests so it won't be discovered. """ import gtsam import numpy as np r = gtsam.Rot3() print(r) print(r.pitch()) r2 = gtsam.Rot3() r3 = r.compose(r2) print("r3 pitch:", r3.pitch()) v = np.array([1, 1, 1]) print("v = ", v) r4 = r3.retract(v) print("r4 pitch:", r4.pitch()) r4.print_(b'r4: ') r3.print_(b"r3: ") v = r3.localCoordinates(r4) print("localCoordinates:", v) Rmat = np.array([[0.990074, -0.0942928, 0.104218], [0.104218, 0.990074, -0.0942928], [-0.0942928, 0.104218, 0.990074]]) r5 = gtsam.Rot3(Rmat) r5.print_(b"r5: ")
def test_StereoVOExample(self): ## Assumptions # - For simplicity this example is in the camera's coordinate frame # - X: right, Y: down, Z: forward # - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) # - x1 is fixed with a constraint, x2 is initialized with noisy values # - No noise on measurements ## Create keys for variables x1 = symbol('x', 1) x2 = symbol('x', 2) l1 = symbol('l', 1) l2 = symbol('l', 2) l3 = symbol('l', 3) ## Create graph container and add factors to it graph = gtsam.NonlinearFactorGraph() ## add a constraint on the starting pose first_pose = gtsam.Pose3() graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose)) ## Create realistic calibration and measurement noise model # format: fx fy skew cx cy baseline K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) stereo_model = gtsam.noiseModel.Diagonal.Sigmas( np.array([1.0, 1.0, 1.0])) ## Add measurements # pose 1 graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)) #pose 2 graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(70, 20, 490), stereo_model, x2, l2, K)) graph.add( gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) ## Create initial estimate for camera poses and landmarks initialEstimate = gtsam.Values() initialEstimate.insert(x1, first_pose) # noisy estimate for pose 2 initialEstimate.insert( x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, -.1, 1.1))) expected_l1 = gtsam.Point3(1, 1, 5) initialEstimate.insert(l1, expected_l1) initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5)) initialEstimate.insert(l3, gtsam.Point3(0, -.5, 5)) ## optimize optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimize() ## check equality for the first pose and point pose_x1 = result.atPose3(x1) self.gtsamAssertEquals(pose_x1, first_pose, 1e-4) point_l1 = result.atPoint3(l1) self.gtsamAssertEquals(point_l1, expected_l1, 1e-4)
def bundle_adjustment(self): MIN_LANDMARK_SEEN = 3 # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add factors for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) landmark_id_list = {} img_pose_id_list = {} for i, img_pose in enumerate(self.img_pose): for k in range(len(img_pose.kp)): if img_pose.kp_3d_exist(k): landmark_id = img_pose.kp_3d(k) if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN): landmark_id_list[landmark_id] = True img_pose_id_list[i] = True graph.add( gtsam.GenericProjectionFactorCal3_S2( Point2(img_pose.kp[k][0], img_pose.kp[k][1]), measurementNoise, X(i), P(landmark_id), self.cal)) # Initialize estimate for poses # Create priors s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 10, 10, 10]) # poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) # theta = np.radians(30) # wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0 , math.cos(theta)], # [-math.cos(theta), 0 , -math.sin(theta)], # [0, 1 , 0]])) wRc = gtsam.Rot3(np.identity(3)) for i, idx in enumerate(img_pose_id_list): initialEstimate.insert(X(idx), self.img_pose[idx].T) if i < 2: # graph.add(gtsam.PriorFactorPose3(X(idx), gtsam.Pose3( # wRc, gtsam.Point3(-math.sin(theta)*1.58*idx, -math.cos(theta)*1.58*idx, 1.2)), posePriorNoise)) graph.add( gtsam.PriorFactorPose3( X(idx), gtsam.Pose3(wRc, gtsam.Point3(1.38 * idx, 0, -0.29 * idx)), posePriorNoise)) # i+=1 # Initialize estimate for landmarks for idx in landmark_id_list: p = self.landmark[idx].point initialEstimate.insert(P(idx), Point3(p[0], p[1], p[2])) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) # sfm_result = optimizer.optimize() for i in range(1000): optimizer.iterate() sfm_result = optimizer.values() # print(sfm_result) # Marginalization marginals = gtsam.Marginals(graph, sfm_result) for idx in img_pose_id_list: marginals.marginalCovariance(X(idx)) for idx in landmark_id_list: marginals.marginalCovariance(P(idx)) nrCamera = len(img_pose_id_list) nrPoint = len(landmark_id_list) return sfm_result, img_pose_id_list, landmark_id_list
gtsam.Point3(0, 0, 1)).pose()) poses.append( gtsam.SimpleCamera.Lookat(gtsam.Point3(0, 10, 0), gtsam.Point3(), gtsam.Point3(0, 0, 1)).pose()) poses.append( gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(), gtsam.Point3(0, 0, 1)).pose()) # define quadrics quadrics = [] quadrics.append( quadricslam.ConstrainedDualQuadric(gtsam.Pose3(), np.array([1., 2., 3.]))) quadrics.append( quadricslam.ConstrainedDualQuadric( gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, 0.1, 0.1)), np.array([1., 2., 3.]))) # add prior on first pose prior_factor = gtsam.PriorFactorPose3(X(0), poses[0], prior_noise) graph.add(prior_factor) # add trajectory estimate for i, pose in enumerate(poses): # add a perturbation to initial pose estimates to simulate noise perturbed_pose = poses[i].compose( gtsam.Pose3(gtsam.Rot3.Rodrigues(0.1, 0.1, 0.1), gtsam.Point3(0.1, 0.2, 0.3))) initial_estimate.insert(X(i), perturbed_pose)
"""The real power of GTSAM will show here. In five lines, we'll setup a Gauss Newton nonlinear optimizer and optimize for the vehicle's poses in world coordinates. Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour. """ # load in all clouds in our dataset clouds = read_ply(*scans_fnames) # Setting up our factor graph graph = gtsam.NonlinearFactorGraph() initial_estimates = gtsam.Values() # We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) initial_estimates.insert(0, initial_pose) # We'll use your function to populate the factor graph graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds) # Now optimize for the states parameters = gtsam.GaussNewtonParams() parameters.setRelativeErrorTol(1e-5) parameters.setMaxIterations(100) optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters) result = optimizer.optimize() """Let's plot these poses to see how our vechicle moves.
def optimize(gps_measurements: List[GpsMeasurement], imu_measurements: List[ImuMeasurement], sigma_init_x: gtsam.noiseModel.Diagonal, sigma_init_v: gtsam.noiseModel.Diagonal, sigma_init_b: gtsam.noiseModel.Diagonal, noise_model_gps: gtsam.noiseModel.Diagonal, kitti_calibration: KittiCalibration, first_gps_pose: int, gps_skip: int) -> gtsam.ISAM2: """Run ISAM2 optimization on the measurements.""" # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) current_pose_global = Pose3(gtsam.Rot3(), gps_measurements[first_gps_pose].position) # the vehicle is stationary at the beginning at position 0,0,0 current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object isam_params = gtsam.ISAM2Params() isam_params.setFactorization("CHOLESKY") isam_params.relinearizeSkip = 10 isam = gtsam.ISAM2(isam_params) # Create the factor graph and values object that will store new factors and # values to add to the incremental graph new_factors = gtsam.NonlinearFactorGraph() # values storing the initial estimates of new nodes in the factor graph new_values = gtsam.Values() # Main loop: # (1) we read the measurements # (2) we create the corresponding factors in the graph # (3) we solve the graph to obtain and optimal estimate of robot trajectory print("-- Starting main loop: inference is performed at each time step, " "but we plot trajectory every 10 steps") j = 0 included_imu_measurement_count = 0 for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) current_vel_key = V(i) current_bias_key = B(i) t = gps_measurements[i].time if i == first_gps_pose: # Create initial estimate and prior on initial pose, velocity, and biases new_values.insert(current_pose_key, current_pose_global) new_values.insert(current_vel_key, current_velocity_global) new_values.insert(current_bias_key, current_bias) new_factors.addPriorPose3(current_pose_key, current_pose_global, sigma_init_x) new_factors.addPriorVector(current_vel_key, current_velocity_global, sigma_init_v) new_factors.addPriorConstantBias(current_bias_key, current_bias, sigma_init_b) else: t_previous = gps_measurements[i - 1].time # Summarize IMU data between the previous GPS measurement and now current_summarized_measurement = gtsam.PreintegratedImuMeasurements( imu_params, current_bias) while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: current_summarized_measurement.integrateMeasurement( imu_measurements[j].accelerometer, imu_measurements[j].gyroscope, imu_measurements[j].dt) included_imu_measurement_count += 1 j += 1 # Create IMU factor previous_pose_key = X(i - 1) previous_vel_key = V(i - 1) previous_bias_key = B(i - 1) new_factors.push_back( gtsam.ImuFactor(previous_pose_key, previous_vel_key, current_pose_key, current_vel_key, previous_bias_key, current_summarized_measurement)) # Bias evolution as given in the IMU metadata sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas( np.asarray([ np.sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma ] * 3 + [ np.sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma ] * 3)) new_factors.push_back( gtsam.BetweenFactorConstantBias(previous_bias_key, current_bias_key, gtsam.imuBias.ConstantBias(), sigma_between_b)) # Create GPS factor gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position) if (i % gps_skip) == 0: new_factors.addPriorPose3(current_pose_key, gps_pose, noise_model_gps) new_values.insert(current_pose_key, gps_pose) print(f"############ POSE INCLUDED AT TIME {t} ############") print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) # Add initial values for velocity and bias based on the previous # estimates new_values.insert(current_vel_key, current_velocity_global) new_values.insert(current_bias_key, current_bias) # Update solver # ======================================================================= # We accumulate 2*GPSskip GPS measurements before updating the solver at # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): print(f"############ NEW FACTORS AT TIME {t:.6f} ############") new_factors.print() isam.update(new_factors, new_values) # Reset the newFactors and newValues list new_factors.resize(0) new_values.clear() # Extract the result/current estimates result = isam.calculateEstimate() current_pose_global = result.atPose3(current_pose_key) current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) print(f"############ POSE AT TIME {t} ############") current_pose_global.print() print("\n") return isam
import cv2 import gtsam import numpy as np # load images to test with img_l = cv2.imread("imagesL/tag_L.jpg") img_c = cv2.imread("imagesL/tag_C.jpg") img_r = cv2.imread("imagesL/tag_R.jpg") # set up camera and import intrinsic parameters paramsl = np.load('params/left_params.npz') paramsr = np.load('params/right_params.npz') left = [ "L", img_l, paramsl['mtx_l'], paramsl['dist_l'], gtsam.Pose3(gtsam.Rot3(paramsl['R_L']), paramsl['T_L']) ] center = ["C", img_c, paramsl['mtx_c'], paramsl['dist_c'], gtsam.Pose3()] right = [ "R", img_r, paramsr['mtx_r'], paramsr['dist_r'], gtsam.Pose3(gtsam.Rot3(paramsr['R_R']), paramsr['T_R']).inverse() ] all_cameras = [left, center, right] # setup aruco finder dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_APRILTAG_36h11) aruco_params = cv2.aruco.DetectorParameters_create() size = 2.6 #inches aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX aruco_params.cornerRefinementWinSize = 5 aruco_params.cornerRefinementMaxIterations = 30 aruco_params.cornerRefinementMinAccuracy = 1e-5
def icp(clouda, cloudb, initial_transform=gtsam.Pose3(), max_iterations=25): """Runs ICP on two clouds by calling all five steps implemented above. Iterates until close enough or max iterations. Returns a series of intermediate clouds for visualization purposes. Args: clouda (ndarray): point cloud A cloudb (ndarray): point cloud B initial_transform (gtsam.Pose3): the initial estimate of transform between clouda and cloudb (step 1 of icp) max_iterations (int): maximum iters of ICP to run before breaking Ret: bTa (gtsam.Pose3): the final transform icp_series (list): visualized icp for debugging """ icp_series = [] bTa = initial_transform i = 0 temp = True while i < max_iterations and temp: newClTr = transform_cloud(bTa,clouda) newCloudb = assign_closest_pairs(newClTr, cloudb) transform = estimate_transform(newClTr,newCloudb) if transform.equals(gtsam.Pose3.identity(),tol=1e-2: temp = False else: bTa = gtsam.Pose3(np.matmul(bTa.matrix(),transform.matrix())) i += 1 icp_series.append([newClTr, cloudb]) return bTa, icp_series """The animation shows how clouda has moved after each iteration of ICP. You should see stationary landmarks, like walls and parked cars, converge onto each other.""" aTb, icp_series = icp(clouda, cloudb) visualize_clouds_animation(icp_series, speed=400, show_grid_lines=True) """ICP is a computationally intense algorithm and we plan to run it between each cloud pair in our 180 clouds dataset. Use the python profiler to identify the computationally expensive subroutines in your algorithm and use numpy to reduce your runtime. The TAs get ~6.5 seconds.""" import cProfile cProfile.run('icp(clouda, cloudb)') """These unit tests will verify the basic functionality of the functions you've implemented in this section. Keep in mind that these are not exhaustive.""" import unittest class TestICP(unittest.TestCase): def setUp(self): self.testclouda = np.array([[1], [1], [1]]) self.testcloudb = np.array([[2, 10], [1, 1], [1, 1]]) self.testcloudc = np.array([[2], [1], [1]]) self.testbTa = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)) self.testcloudd = np.array([[0, 20, 10], [0, 10, 20], [0, 0, 0]]) self.testcloude = np.array([[10, 30, 20], [10, 20, 30], [0, 0, 0]]) def test_assign_closest_pairs1(self): expected = (3, 1) actual = assign_closest_pairs(self.testclouda, self.testcloudb).shape self.assertEqual(expected, actual) def test_assign_closest_pairs2(self): expected = 2 actual = assign_closest_pairs(self.testclouda, self.testcloudb)[0][0] self.assertEqual(expected, actual) def test_estimate_transform1(self): expected = 1 actual = estimate_transform(self.testclouda, self.testcloudc).x() self.assertEqual(expected, actual) def test_estimate_transform2(self): expected = 10 actual = estimate_transform(self.testcloudd, self.testcloude).x() self.assertAlmostEqual(expected, actual, places=7) actua2 = estimate_transform(self.testcloudd, self.testcloude).y() self.assertAlmostEqual(expected, actua2, places=7) def test_transform_cloud1(self): expected = 2 actual = transform_cloud(self.testbTa, self.testclouda)[0][0] self.assertEqual(expected, actual) def test_icp1(self): ret = icp(self.testclouda, self.testcloudb) expected1 = type(gtsam.Pose3()) actual1 = type(ret[0]) self.assertEqual(expected1, actual1) expected2 = type([]) actual2 = type(ret[1]) self.assertEqual(expected2, actual2) expected3 = type([]) actual3 = type(ret[1][0]) self.assertEqual(expected3, actual3) def test_icp2(self): expected = 1 actual = icp(self.testclouda, self.testcloudb)[0].x() self.assertEqual(expected, actual) def test_icp3(self): expected = 1 actual = icp(self.testclouda, self.testcloudc)[0].x() self.assertEqual(expected, actual) if __name__ == "__main__": unittest.main(argv=['first-arg-is-ignored'], exit=False) """# Factor Graph In this section, we'll build a factor graph to estimate the pose of our vechicle using the transforms our ICP algorithm gives us between frames. These ICP transforms are the factors that tie the pose variables together. We will be using GTSAM to construct the factor graph as well as perform a optimization for the pose of the car as it travels down the street. Let's start with a simple example first. Recall from PoseSLAM describe in the LIDAR slides how we could add a factor (aka constraint) between two state variables. When we revisited a state, we could add a loop closure. Since the car in our dataset never revisits a previous pose, there is not loop closure. Here is that example from the slides copied here. Note how the graph is initialized and how factors are added. """ # # Factor graph example # Helper function to create a pose def vector3(x, y, z): """Create 3d double numpy array.""" return np.array([x, y, z], dtype=np.float) # Create noise model priorNoise = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) model = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) # Instantiate the factor graph example_graph = gtsam.NonlinearFactorGraph() # Adding a prior on the first pose example_graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) # Create odometry (Between) factors between consecutive poses example_graph.add(gtsam.BetweenFactorPose2( 1, 2, gtsam.Pose2(2, 0, 0), model)) example_graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) example_graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) example_graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) # Add the loop closure constraint example_graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) # Create the initial estimate example_initial_estimate = gtsam.Values() example_initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) example_initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) example_initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) example_initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) example_initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer ex_parameters = gtsam.GaussNewtonParams() ex_parameters.setRelativeErrorTol(1e-5) ex_parameters.setMaxIterations(100) ex_optimizer = gtsam.GaussNewtonOptimizer(example_graph, example_initial_estimate, ex_parameters) ex_result = ex_optimizer.optimize() print("Final Result:\n{}".format(ex_result)) # Plot your graph marginals = gtsam.Marginals(example_graph, ex_result) fig = plt.figure(0) for i in range(1, 6): gtsam_plot.plot_pose2(0, ex_result.atPose2(i), 0.5, marginals.marginalCovariance(i)) plt.axis('equal') plt.show() """**TODO** [25 points] You will be using your ICP implementation here to find the transform between two subsequent clouds. These transforms become the factors between pose variables in the graph. So, you will need to go through all the point clouds and run icp pair-wise to find the relative movement of the car. With these transformation, create a factor representing the transform between the pose variables. We talked about how loop closure helps us consolidate conflicting data into a better global estimate. Unfortunately, our car does not perform a loop closure. So, our graph would just be a long series of poses connected by icp-returned transforms. However, our lidar scans are noisy, which means that our icp-returned transforms are not perfect either. This ultimately results in incorrect vehicle poses and overall map. One way that we can augment our graph is through "skipping". We simply run ICP between every other cloud, and add these skip connections into the graph. You can basically perform ICP between two non-consecutive point clouds and add that transform as a factor in the factor graph. """ def populate_factor_graph(graph, initial_estimates, initial_pose, clouds): """Populates a gtsam.NonlinearFactorGraph with factors between state variables. Populates initial_estimates for state variables as well. Args: graph (gtsam.NonlinearFactorGraph): the factor graph populated with ICP constraints initial_estimates (gtsam.Values): the populated estimates for vehicle poses initial_pose (gtsam.Pose3): the starting pose for the estimates in world coordinates clouds (np.ndarray): the numpy array with all our point clouds """ ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) factor_pose = initial_pose # Add ICP Factors between each pair of clouds prev_T = gtsam.Pose3() for i in range(len(clouds) - 1): # TODO: Run ICP between clouds (hint: use inital_tranform argument) bta, icp_series = icp(clouds[i], clouds[i+1], initial_transform=prev_T) #T = initial_transform(initial_pose, clouds) # TODO: Set T to its inverse: use `gtsam.Pose3.inverse()` T = bta.inverse() # TODO: Add a `gtsam.BetweenFactorPose3()` to the graph graph.add(gtsam.BetweenFactorPose3(i, i+1, T, ICP_NOISE)) factor_pose = factor_pose.compose(T) initial_estimates.insert(i+1, factor_pose) print(".", end="") # Add skip connections between every other frame prev_T = gtsam.Pose3() for i in range(0, len(clouds) - 2, 2): # TODO: Run ICP between clouds (hint: use inital_tranform argument) bta, icp_series = icp(clouds[i], clouds[i+2],initial_transform=prev_T) # TODO: Set T to its inverse: use `gtsam.Pose3.inverse()` T = bta.inverse() # TODO: Add a `gtsam.BetweenFactorPose3()` to the graph graph.add(gtsam.BetweenFactorPose3(i, i+2, T, ICP_NOISE)) print(".", end="") return graph, initial_estimates """The real power of GTSAM will show here. In five lines, we'll setup a Gauss Newton nonlinear optimizer and optimize for the vehicle's poses in world coordinates. Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour. """ # load in all clouds in our dataset clouds = read_ply(*scans_fnames) # Setting up our factor graph graph = gtsam.NonlinearFactorGraph() initial_estimates = gtsam.Values() # We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) initial_estimates.insert(0, initial_pose) # We'll use your function to populate the factor graph graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds) # Now optimize for the states parameters = gtsam.GaussNewtonParams() parameters.setRelativeErrorTol(1e-5) parameters.setMaxIterations(100) optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters) result = optimizer.optimize() """Let's plot these poses to see how our vechicle moves. Screenshot this for your reflection. """ poses_cloud = np.array([[], [], []]) for i in range(len(clouds)): poses_cloud = np.hstack([poses_cloud, np.array([[result.atPose3(i).x()], [result.atPose3(i).y()], [result.atPose3(i).z()]])]) init_car_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) visualize_clouds([poses_cloud, transform_cloud(init_car_pose, clouds[0])], show_grid_lines=True) """These unit tests will verify the basic functionality of the function you've implemented in this section. Keep in mind that these are not exhaustive.""" import unittest class TestFactorGraph(unittest.TestCase): def setUp(cls): test_clouds = read_ply(*scans_fnames)[:6] PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) test_graph = gtsam.NonlinearFactorGraph() test_initial_estimates = gtsam.Values() initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) test_initial_estimates.insert(0, initial_pose) test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds) cls.graph = test_graph cls.initial_estimates = test_initial_estimates def test_graph_params(self): self.assertTrue(type(self.graph) == gtsam.NonlinearFactorGraph) def test_initial_estimates_params(self): self.assertTrue(type(self.initial_estimates) == gtsam.Values) def suite(): functions = ['test_graph_params', 'test_initial_estimates_params'] suite = unittest.TestSuite() for func in functions: suite.addTest(TestFactorGraph(func)) return suite if __name__ == "__main__": runner = unittest.TextTestRunner() runner.run(suite()) """# Mapping In this section, we'll tackle the mapping component of SLAM (Simulataneous Localization and Mapping). The previous section used a factor graph to localize our vehicle's poses in world coordinates. We'll now use those poses to form a map of the street from the point clouds. Given the poses and the clouds, this task is easy. We'll use your `transform_cloud` method from the ICP section to transform every other cloud in our dataset to be centered at the corresponding pose where the cloud was captured. Visualizing all of these clouds yields the complete map. We don't use every cloud in our dataset to reduce the amount of noise in our map while retaining plenty of detail. Screenshot this for your reflection. """ cloud_map = [] for i in range(0, len(clouds), 2): cloud_map.append(transform_cloud(result.atPose3(i), clouds[i-1])) visualize_clouds(cloud_map, show_grid_lines=True, single_color="#C6C6C6") """# Reflection