def plot_estimate(self, fignum=0): """ VisualISAMPlot plots current state of ISAM2 object Author: Ellon Paiva Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() # Plot points # Can't use data because current frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # gtsam.plot_3d_points(result, [], marginals) gtsam_plot.plot_3d_points(fignum, self.current_estimate, 'rx') # Plot cameras i = 0 while self.current_estimate.exists(iSAM2Wrapper.get_key('x', i)): pose_i = self.current_estimate.atPose3(iSAM2Wrapper.get_key( 'x', i)) gtsam_plot.plot_pose3(fignum, pose_i, 10) i += 1 # draw #axes.set_xlim3d(-40, 40) #axes.set_ylim3d(-40, 40) #axes.set_zlim3d(-40, 40) axes.view_init(90, 0) plt.pause(.01)
def visual_ISAM2_plot(result): # Declare an id for the figure fignum = 0 fig = None fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() gtsam_plot.plot_trajectory(fignum, result) # Plot cameras i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) gtsam_plot.plot_pose3(fignum, pose_i, 1) i += 1 # draw # axes.set_xlim3d(-40, 40) # axes.set_ylim3d(-40, 40) # axes.set_zlim3d(-40, 40) # plt.show() axes.view_init(elev=180, azim=90) plt.savefig("Figures/traj.png") pickle.dump(fig, open("Figures/fig.pickle", "wb")) plt.show()
def plot(self, values: gtsam.Values, title: str = "Estimated Trajectory", fignum: int = POSES_FIG + 1, show: bool = False): """ Plot poses in values. Args: values: The values object with the poses to plot. title: The title of the plot. fignum: The matplotlib figure number. POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure. show: Flag indicating whether to display the figure. """ i = 0 while values.exists(X(i)): pose_i = values.atPose3(X(i)) plot_pose3(fignum, pose_i, 1) i += 1 plt.title(title) gtsam.utils.plot.set_axes_equal(fignum) print("Bias Values", values.atConstantBias(BIAS_KEY)) plt.ioff() if show: plt.show()
def visual_ISAM2_plot(result): """ VisualISAMPlot plots current state of ISAM2 object Author: Ellon Paiva Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert """ # Declare an id for the figure fignum = 0 fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() # Plot points # Can't use data because current frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # gtsam.plot_3d_points(result, [], marginals) gtsam_plot.plot_3d_points(fignum, result, 'rx') # Plot cameras i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) gtsam_plot.plot_pose3(fignum, pose_i, 10) i += 1 # draw axes.set_xlim3d(-40, 40) axes.set_ylim3d(-40, 40) axes.set_zlim3d(-40, 40) plt.pause(1)
def run(self, T=12): gtNavStates = [] predictedNavStates = [] # simulate the loop for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) if i % 100 == 0: self.plotImu(t, measuredOmega, measuredAcc) gtNavState = self.plotGroundTruthPose(t, time_interval=0.001) pim = self.runner.integrate(t, self.actualBias, True) predictedNavState = self.runner.predict(pim, self.actualBias) plot_pose3(POSES_FIG, predictedNavState.pose(), 1) gtNavStates.append(gtNavState) predictedNavStates.append(predictedNavState) ATE = [] for gt, pred in zip(gtNavStates, predictedNavStates): predPose = pred.pose() delta = gt.inverse().compose(predPose) ATE.append(np.linalg.norm(delta.Logmap(delta))**2) print("ATE={}".format(np.sqrt(np.mean(ATE)))) plt.ioff() plt.show()
def plot_sfm_result(self,result): # Declare an id for the figure fignum = 0 fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() # Plot points # Can't use data because current frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # gtsam.plot_3d_points(result, [], marginals) gtsam_plot.plot_3d_points(fignum, result, 'rx') # gtsam_plot.plot_pose3(fignum, result, 'rx') # Plot cameras i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) gtsam_plot.plot_pose3(fignum, pose_i, 2) i += 1 # draw axes.set_xlim3d(-50, 50) axes.set_ylim3d(-50, 50) axes.set_zlim3d(-50, 50) plt.legend() plt.show()
def plot_sfm_result(result, pose_indices, point_indices, x_axe=20, y_axe=20, z_axe=20): """ Plot mapping result. """ # Declare an id for the figure figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot points # gtsam_plot.plot_3d_points(figure_number, result, 'rx') for idx in point_indices: point_i = result.atPoint3(P(idx)) gtsam_plot.plot_point3(figure_number, point_i, 'rx') # Plot cameras for idx in pose_indices: pose_i = result.atPose3(X(idx)) gtsam_plot.plot_pose3(figure_number, pose_i, 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 plot_with_result(result, x_axe=30, y_axe=30, z_axe=30, axis_length=2, figure_number=0): """plot the result of sfm""" # Declare an id for the figure fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot points gtsam_plot.plot_3d_points(figure_number, result, 'rx') # Plot cameras i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) axis_length = axis_length gtsam_plot.plot_pose3(figure_number, pose_i, axis_length) i += 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 main(): """Main runner.""" parser = argparse.ArgumentParser( description="A 3D Pose SLAM example that reads input from g2o, and " "initializes Pose3") 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("-p", "--plot", action="store_true", help="Flag to plot results") args = parser.parse_args() g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ else args.input is3D = True graph, initial = gtsam.readG2o(g2oFile, is3D) # Add Prior on the first key priorModel = gtsam.noiseModel.Diagonal.Variances( vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity( "Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: 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.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show()
def plotGroundTruthPose(i): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = gtsam.Pose3(true_poses[i]) plot_pose3(POSES_FIG, actualPose, 0.3) ax = plt.gca() ax.set_xlim3d(-5, 5) ax.set_ylim3d(-5, 5) ax.set_zlim3d(-5, 5) plt.pause(0.01)
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) t = actualPose.translation() self.maxDim = max([max(np.abs(t)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) ax.set_zlim3d(-self.maxDim, self.maxDim) plt.pause(time_interval)
def plotGroundTruthPose(self, t): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, 0.3) t = actualPose.translation() self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) ax.set_zlim3d(-self.maxDim, self.maxDim) plt.pause(0.01)
def run(self, T: int = 12): """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) if i % 25 == 0: self.plotImu(t, measuredOmega, measuredAcc) self.plotGroundTruthPose(t) pim = self.runner.integrate(t, self.actualBias, True) predictedNavState = self.runner.predict(pim, self.actualBias) plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1) plt.ioff() plt.show()
def plot_cameras_gtsam(transform_matrix_filename: str): set_cams = np.load( '/home/sushmitawarrier/clevr-dataset-gen/output/images/CLEVR_new000000/set_cams.npz' ) with open(transform_matrix_filename) as f: transforms = json.load(f) figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot cameras idx = 0 for i in range(len(set_cams['set_rot'])): print(set_cams['set_rot'][i]) print("i", i) rot = set_cams['set_rot'][i] t = set_cams['set_loc'][i] rot_3 = gtsam.Rot3.RzRyRx(rot) pose_i = gtsam.Pose3(rot_3, t) if idx % 2 == 0: gtsam_plot.plot_pose3(figure_number, pose_i, 1) break idx += 1 for i in transforms['frames']: T = i['transform_matrix'] pose_i = gtsam.Pose3(T) #print("T", T) #print("pose_i",pose_i) #print(set_cams['set_loc'][0]) #print(set_cams['set_rot'][0]) # Reference pose # trying to look along x-axis, 0.2m above ground plane # x forward, y left, z up (x-y is the ground plane) upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2)) axis_length = 1 # plotting alternate poses if idx % 2 == 0: # gtsam_plot.plot_pose3(figure_number, pose_i, axis_length) gtsam_plot.plot_pose3(figure_number, pose_j, axis_length) idx += 1 x_axe, y_axe, z_axe = 10, 10, 10 # Draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(0, z_axe) plt.legend() plt.show()
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 plot_it(result): resultPoses = gtsam.allPose3s(result) xyz = [ resultPoses.atPose3(i).translation() for i in range(resultPoses.size()) ] x_ = [pose.x() for pose in xyz] y_ = [pose.y() for pose in xyz] z_ = [pose.z() for pose in xyz] fig = plt.figure() ax = fig.gca(projection='3d') for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) ax.text(x_[i], y_[i], z_[i], str(i)) plt.plot(x_, y_, z_, 'k-') plt.show()
def test_pose_estimate_generator(self): """test pose estimate generator""" # theta = 45 # delta_x = 1 # delta_y = 2 # delta_z = 3 # rows = 2 # cols = 2 # angles = 8 theta = 45 delta_x = 1 delta_y = 2 delta_z = 3 rows = 2 cols = 2 angles = 4 degree = math.radians(45) R = np.array([[math.cos(degree), -math.sin(degree), 0], [math.sin(degree), math.cos(degree), 0], [0, 0, 1]]) prior1_delta = [5, 5, 5, 30] prior2_delta = [-5, -5, -3, -90] actual_pose_estimates = pose_estimate_generator( theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows, cols, angles, R) # Declare an id for the figure figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot cameras pose_origin = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(0, 0, 0)) axis_length = 1 gtsam_plot.plot_pose3(figure_number, pose_origin, axis_length) for pose in actual_pose_estimates: gtsam_plot.plot_pose3(figure_number, pose, axis_length) # Draw x_axe = y_axe = z_axe = 20 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 plot_with_results(result1, result2, x_axe=30, y_axe=30, z_axe=30, figure_number1=0, figure_number2=1): """plot the two results of sfm at the same time""" # Declare an id for the figure fig = plt.figure(figure_number1) axes = fig.gca(projection='3d') plt.cla() # Plot points gtsam_plot.plot_3d_points(figure_number1, result1, 'rx') # Plot cameras i = 0 while result1.exists(X(i)): pose_i = result1.atPose3(X(i)) gtsam_plot.plot_pose3(figure_number1, pose_i, 2) i += 1 # draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(-z_axe, z_axe) # Declare an id for the figure fig = plt.figure(figure_number2) axes = fig.gca(projection='3d') plt.cla() # Plot points gtsam_plot.plot_3d_points(figure_number2, result2, 'rx') # Plot cameras i = 0 while result2.exists(X(i)): pose_i = result2.atPose3(X(i)) gtsam_plot.plot_pose3(figure_number2, pose_i, 2) i += 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()
class TestPoses(unittest.TestCase): """ class to check if poses are correct """ # World coordinate system: right handed. X forward, Y left, Z up # Create a reference object, facing along X-axis and elevation of 0 deg. az = 90 ele = 0 tilt = 0 dist = 3 # get location and rotation vector from az,ele and dist values loc, rot = config_cam(az, ele, dist) # compute transformation matrix using pose_spherical function computed_T = pose_spherical(az, ele, dist) pose_i = gtsam.Pose3(computed_T) # Construct pose using rotation and location vectors pose_j = gtsam.Pose3(gtsam.Rot3.RzRyRx(rot), loc) # Compares rotation vector from constructed pose with rotation vector initially computed rot_p = pose_j.rotation() rot_from_ele = rot_p.xyz() # Returns true assert_array_almost_equal(rot_from_ele, rot) figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2)) axis_length = 1 gtsam_plot.plot_pose3(figure_number, pose_i, axis_length) gtsam_plot.plot_pose3(figure_number, pose_j, axis_length) x_axe, y_axe, z_axe = 10, 10, 10 # Draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(0, z_axe) plt.legend() #plt.show() filename = "/home/sushmitawarrier/results/test_pose_clevr3.png" plt.savefig(filename)
def plot_localization(self, fignum, result): fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() # Plot points # Can't use data because self.img_pose[i+1]rent frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # gtsam.plot_3d_points(result, [], marginals) for point in result[1]: gtsam_plot.plot_point3(fignum, point, 'rx') # Plot cameras for pose in result[0]: gtsam_plot.plot_pose3(fignum, pose, 1) # draw axes.set_xlim3d(-20, 20) axes.set_ylim3d(-20, 20) axes.set_zlim3d(-20, 20) plt.pause(60)
def plotGroundTruthPose(self, t: float, scale: float = 0.3, time_interval: float = 0.01): """ Plot ground truth pose, as well as prediction from integrated IMU measurements. Args: t: Time at which the pose was obtained. scale: The scaling factor for the pose axes. time_interval: The time to wait before showing the plot. """ actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) translation = actualPose.translation() self.maxDim = max([max(np.abs(translation)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) ax.set_zlim3d(-self.maxDim, self.maxDim) plt.pause(time_interval)
def test_pose_estimate_generator(self): """test pose estimate generator""" # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name theta = 45 delta_x = [0, 3.7592, 3.7592 + 5] delta_y = [0, 1.75895, 1.75895 * 2, 1.75895 * 3] delta_z = 0.9652 rows = 4 cols = 3 angles = 1 degree = math.radians(theta) R = np.array([[math.cos(degree), -math.sin(degree), 0], [math.sin(degree), math.cos(degree), 0], [0, 0, 1]]) prior1_delta = [0, 0, delta_z, 0] prior2_delta = [3.7592, 0, delta_z, 0] actual_pose_estimates = pose_estimate_generator_rectangle( theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows, cols, angles) # Declare an id for the figure figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot cameras pose_origin = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(0, 0, 0)) axis_length = 1 gtsam_plot.plot_pose3(figure_number, pose_origin, axis_length) for pose in actual_pose_estimates: gtsam_plot.plot_pose3(figure_number, pose, axis_length) # Draw x_axe = y_axe = z_axe = 20 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 ISAM2_plot(values, fignum=0): """Plot poses.""" fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() i = 0 min_bounds = 0, 0, 0 max_bounds = 0, 0, 0 while values.exists(X(i)): pose_i = values.atPose3(X(i)) gtsam_plot.plot_pose3(fignum, pose_i, 10) position = pose_i.translation().vector() min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)] max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)] # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0 i += 1 # draw axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1) axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1) axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1) plt.pause(1)
def plot_sfm_result(result, pose_indices, point_indices): """ Plot mapping result. """ # Declare an id for the figure _fignum = 0 fig = plt.figure(_fignum) axes = fig.gca(projection='3d') plt.cla() # Plot points # gtsam_plot.plot_3d_points(_fignum, result, 'rx') for idx in point_indices: point_i = result.atPoint3(P(idx)) gtsam_plot.plot_point3(_fignum, point_i, 'rx') # Plot cameras for idx in pose_indices: pose_i = result.atPose3(X(idx)) gtsam_plot.plot_pose3(_fignum, pose_i, 1) # Draw axes.set_xlim3d(-20, 20) axes.set_ylim3d(-20, 20) axes.set_zlim3d(-20, 20) plt.legend() plt.show()
def plot_trajectory(landmarks, trajectory, x_axe=30, y_axe=30, z_axe=30, axis_length=2, figure_number=0): """Plot the map and the generated trajectory. Parameters: landmarks - a list of [x,y,z] 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 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='red', alpha=1, marker="^") # draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(-z_axe, z_axe) plt.pause(1)
params.dt) ############################### Optimize the factor graph #################################### # Use the Levenberg-Marquardt algorithm optimization_params = gtsam.LevenbergMarquardtParams() optimization_params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(factor_graph, initial_values, optimization_params) optimization_result = optimizer.optimize() ############################### Plot the solution #################################### for i in preintegration_steps: # Ground truth pose plot_pose3(1, gtsam.Pose3(true_poses[i]), 0.3) # Estimated pose plot_pose3(1, optimization_result.atPose3(symbol('x', i)), 0.1) ax = plt.gca() ax.set_xlim3d(-5, 5) ax.set_ylim3d(-5, 5) ax.set_zlim3d(-5, 5) ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.figure(1).suptitle("Large poses: ground truth, small poses: estimate") plt.ioff() plt.show()
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: 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.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show()
def run(self): graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) T = 12 num_poses = T + 1 # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) initial.insert(X(i), state_i.pose()) initial.insert(V(i), state_i.velocity()) # simulate the loop i = 0 # state index actual_state_i = self.scenario.navState(0) for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) # Plot IMU many times if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) # Plot every second if k % int(1 / self.dt) == 0: self.plotGroundTruthPose(t) # create IMU factor every second if (k + 1) % int(1 / self.dt) == 0: factor = gtsam.ImuFactor(X(i), V(i), X( i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) if True: print(factor) print(pim.predict(actual_state_i, self.actualBias)) pim.resetIntegration() actual_state_i = self.scenario.navState(t + self.dt) i += 1 # add priors on beginning and end self.addPrior(0, graph) self.addPrior(num_poses - 1, graph) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() # Calculate and print marginal covariances marginals = gtsam.Marginals(graph, result) print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) for i in range(num_poses): print("Covariance on pose {}:\n{}\n".format( i, marginals.marginalCovariance(X(i)))) print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) # Plot resulting poses i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 print(result.atimuBias_ConstantBias(BIAS_KEY)) plt.ioff() plt.show()
# Use the Levenberg-Marquardt algorithm optimization_params = gtsam.LevenbergMarquardtParams() optimization_params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(factor_graph, initial_values, optimization_params) optimization_result = optimizer.optimize() ############################### Plot the solution #################################### figure = plt.figure(1) axes = plt.gca(projection='3d') axes.set_xlim3d(-2, 2) axes.set_ylim3d(-2, 2) axes.set_zlim3d(-2, 2) axes.set_xlabel('x') axes.set_ylabel('y') axes.set_zlabel('z') axes.margins(0) figure.suptitle("Large/small poses: initial/optimized estimate") for i in preintegration_steps: # Initial estimate from IMU preintegration plot_pose3(1, initial_values.atPose3(symbol('x', i)), 0.2) # Optimized estimate plot_pose3(1, optimization_result.atPose3(symbol('x', i)), 0.08) plt.pause(0.01) plt.ioff() plt.show()
for i,image in enumerate(fe.image_list): # for i in range(1,2): # image = fe.image_list[1] assert counter != 3, "Localization Failed" superpoint_features = estimator.superpoint_generator(image) # print(superpoint_features.key_points) pre_pose = estimator.trajectory[len(estimator.atrium_map.trajectory)-1] projected_features, map_indices = estimator.landmarks_projection( pre_pose) # print(projected_features.key_points) # print(map_indices) features, visible_map = estimator.data_association( superpoint_features, projected_features, map_indices) # if features.get_length() < 5: # counter += 1 # print("Not enough correspondences.") # continue # elif counter > 0: # counter = 0 # print(features.key_points) cur_pose = estimator.trajectory_estimator(features, visible_map) # gtsam_plot.plot_pose3(fignum, sim3.pose(cur_pose), 5) gtsam_plot.plot_pose3(fignum, cur_pose, 5) print("cur_pose",i,":",cur_pose) # # print(estimator.atrium_map.trajectory) plt.ioff() plt.show()