def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" # Print the current estimates computed using iSAM2. print("*"*50 + f"\nInference after State {key+1}:\n") print(current_estimate) # Compute the marginals for all states in the graph. marginals = gtsam.Marginals(graph, current_estimate) # Plot the newly updated iSAM2 inference. fig = plt.figure(0) axes = fig.gca() plt.cla() i = 1 while current_estimate.exists(i): gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i)) i += 1 plt.axis('equal') axes.set_xlim(-1, 5) axes.set_ylim(-1, 3) plt.pause(1)
def run_example(): """ Use trajectory interpolation and then trajectory tracking a la Murray to move a 3-link arm on a straight line. """ # Create arm arm = ThreeLinkArm() # Get initial pose using forward kinematics q = np.radians(vector3(30, -30, 45)) sTt_initial = arm.fk(q) # Create interpolated trajectory in task space to desired goal pose sTt_goal = Pose2(2.4, 4.3, math.radians(0)) poses = trajectory(sTt_initial, sTt_goal, 50) # Setup figure and plot initial pose fignum = 0 fig = plt.figure(fignum) axes = fig.gca() axes.set_xlim(-5, 5) axes.set_ylim(0, 10) gtsam_plot.plot_pose2(fignum, arm.fk(q)) # For all poses in interpolated trajectory, calculate dq to move to next pose. # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). for pose in poses: sTt = arm.fk(q) error = delta(sTt, pose) J = arm.jacobian(q) q += np.dot(np.linalg.inv(J), error) arm.plot(fignum, q) plt.pause(0.01) plt.pause(10)
def build_graph(): print("build_graph !!!") # Create noise models ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) # Create an empty nonlinear factor graph graph = gtsam.NonlinearFactorGraph() # Add a prior on the first pose, setting it to the origin # A prior factor consists of a mean and a noise model (covariance matrix) priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) # Add odometry factors odometry = gtsam.Pose2(2.0, 0.0, 0.0) odometry1 = gtsam.Pose2(edge_list[0].position.x, edge_list[0].position.y, edge_list[0].position.z) odometry2 = gtsam.Pose2(edge_list[1].position.x, edge_list[1].position.y, edge_list[1].position.z) # For simplicity, we will use the same noise model for each odometry factor # Create odometry (Between) factors between consecutive poses graph.add(gtsam.BetweenFactorPose2(1, 2, odometry1, ODOMETRY_NOISE)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry2, ODOMETRY_NOISE)) print("\nFactor Graph:\n{}".format(graph)) # Create the data structure to hold the initialEstimate estimate to the solution # For illustrative purposes, these have been deliberately set to incorrect values initial = gtsam.Values() initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) print("\nInitial Estimate:\n{}".format(initial)) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() print("\nFinal Result:\n{}".format(result)) # 5. Calculate and print marginal covariances for all variables marginals = gtsam.Marginals(graph, result) for i in range(1, 4): print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) fig = plt.figure(0) for i in range(1, 4): gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) plt.axis('equal') plt.show()
def main(): """Main runner.""" # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( gtsam.Point3(0.2, 0.2, 0.1)) # 1. Create a factor graph container and add factors to it graph = gtsam.NonlinearFactorGraph() # 2a. Add a prior on the first pose, setting it to the origin # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) # 2b. Add odometry factors # Create odometry (Between) factors between consecutive poses graph.add( gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) graph.add( gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) graph.add( gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) graph.add( gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) # 2c. Add the loop closure constraint # This factor encodes the fact that we have returned to the same pose. In real # systems, these constraints may be identified in many ways, such as appearance-based # techniques with camera images. We will use another Between Factor to enforce this constraint: graph.add( gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) print("\nFactor Graph:\n{}".format(graph)) # print # 3. Create the data structure to hold the initial_estimate estimate to the # solution. For illustrative purposes, these have been deliberately set to incorrect values initial_estimate = gtsam.Values() initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) print("\nInitial Estimate:\n{}".format(initial_estimate)) # print # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer # The optimizer accepts an optional set of configuration parameters, # controlling things like convergence criteria, the type of linear # system solver to use, and the amount of information displayed during # optimization. We will set a few parameters as a demonstration. parameters = gtsam.GaussNewtonParams() # Stop iterating once the change in error between steps is less than this value parameters.setRelativeErrorTol(1e-5) # Do not perform more than N iteration steps parameters.setMaxIterations(100) # Create the optimizer ... optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) # ... and optimize result = optimizer.optimize() print("Final Result:\n{}".format(result)) # 5. Calculate and print marginal covariances for all variables marginals = gtsam.Marginals(graph, result) for i in range(1, 6): print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) for i in range(1, 6): gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) plt.axis('equal') plt.show()
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) # Create (deliberately inaccurate) initial estimate initial = gtsam.Values() initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) # Optimize using Levenberg-Marquardt optimization result = gtsam.DoglegOptimizer(graph, initial).optimize() # Print results np.set_printoptions(precision=4, suppress=True) print(result) marginals = gtsam.Marginals(graph, result) print('x1 covariance: '), print(marginals.marginalCovariance(1)) print('x2 covariance: '), print(marginals.marginalCovariance(2)) print('x3 covariance: '), print(marginals.marginalCovariance(3)) # Plot results plt.figure(0) plt.xlabel('x') plt.ylabel('y') plt.title('Odometry Estimates') plt.grid(True) plt.axis('equal') for i in range(1,4): gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) plt.show()
params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") params.setMaxIterations(maxIterations) # parameters.setRelativeErrorTol(1e-5) # Create the optimizer ... optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: print("\nFactor Graph:\n{}".format(graph)) print("\nInitial Estimate:\n{}".format(initial)) print("Final Result:\n{}".format(result)) else: outputFile = args.output print("Writing results to file: ", outputFile) graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) gtsam.writeG2o(graphNoKernel, result, outputFile) print("Done!") if args.plot: resultPoses = gtsam.utilities.extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show()
# traveled amongst each frame, and traveling that distance along the calculated # thetas at each step. for t in thetas: # Some sign manipulation to make the visualization. poses.append(gtsam.Pose2(current_x/100, current_y/100, t)) current_x = current_x + dist_to_travel * math.sin(-t) current_y = current_y + dist_to_travel * math.cos(-t) # Add ground truth pose to the image poses.append(gtsam.Pose2(final_x/100, final_y/100, theta)) fig = plt.figure(0) for i in range(len(poses)): gtsam_plot.plot_pose2(0, poses[i]) plt.axis('equal') plt.show() # Run this to unmount your google drive folders. drive.flush_and_unmount() """### Unit Tests These unit tests will verify the basic functionality of all of the functions you will implement. Note, these are not exhaustive and will just test some basic cases. """ import unittest class TestLaneFinder(unittest.TestCase):
def main(): """Main runner.""" parser = argparse.ArgumentParser( description="A 2D Pose SLAM example that reads input from g2o, " "converts it to a factor graph and does the optimization. " "Output is written on a file, in g2o format") parser.add_argument('-i', '--input', help='input file g2o format') parser.add_argument( '-o', '--output', help="the path to the output file with optimized graph") parser.add_argument('-m', '--maxiter', type=int, help="maximum number of iterations for optimizer") parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], default="none", help="Type of kernel used") parser.add_argument("-p", "--plot", action="store_true", help="Flag to plot results") args = parser.parse_args() g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ else args.input maxIterations = 100 if args.maxiter is None else args.maxiter is3D = False graph, initial = gtsam.readG2o(g2oFile, is3D) assert args.kernel == "none", "Supplied kernel type is not yet implemented" # Add prior on the pose having index (key) = 0 priorModel = gtsam.noiseModel.Diagonal.Variances( gtsam.Point3(1e-6, 1e-6, 1e-8)) graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") params.setMaxIterations(maxIterations) # parameters.setRelativeErrorTol(1e-5) # Create the optimizer ... optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: print("\nFactor Graph:\n{}".format(graph)) print("\nInitial Estimate:\n{}".format(initial)) print("Final Result:\n{}".format(result)) else: outputFile = args.output print("Writing results to file: ", outputFile) graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) gtsam.writeG2o(graphNoKernel, result, outputFile) print("Done!") if args.plot: resultPoses = gtsam.utilities.extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show()
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