def optimize(self, graph_to_optimize=None): # Optimization params = gtsam.LevenbergMarquardtParams() if graph_to_optimize is None: optimizer = gtsam.LevenbergMarquardtOptimizer( self.graph, self.initial, params) else: optimizer = gtsam.LevenbergMarquardtOptimizer( graph_to_optimize, self.prop_initial, params) self.result = optimizer.optimize() self.initial = self.result self.resultsFlag = True
def test_Pose2SLAMExample(self): # Assumptions # - All values are axis aligned # - Robot poses are facing along the X axis (horizontal, to the right in images) # - We have full odometry for measurements # - The robot is on a grid, moving 2 meters each step # Create graph container and add factors to it graph = gtsam.NonlinearFactorGraph() # Add prior # gaussian for prior priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) # add directly to graph graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) # Add odometry # general noisemodel for odometry odometryNoise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.2, 0.2, 0.1])) graph.add( gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) graph.add( gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) graph.add( gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) graph.add( gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) # Add pose constraint model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) graph.add( gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) # Initialize to noisy points initialEstimate = gtsam.Values() initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() # Plot Covariance Ellipses marginals = gtsam.Marginals(graph, result) P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
def main(): """Main runner.""" # Create an empty nonlinear factor graph graph = gtsam.NonlinearFactorGraph() # Add a prior on the first point, setting it to the origin # A prior factor consists of a mean and a noise model (covariance matrix) priorMean = gtsam.Pose3() # prior at origin graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) # Add GPS factors gps = gtsam.Point3(lat0, lon0, h0) graph.add(gtsam.GPSFactor(1, gps, GPS_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.Pose3()) 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))
def optimize_old(self, graph_to_optimize=None ): #TODO: Old one; Turn back on if ISAM2 refuses to work. # Optimization params = gtsam.LevenbergMarquardtParams() if graph_to_optimize is None: optimizer = gtsam.LevenbergMarquardtOptimizer( self.graph, self.initial, params) else: optimizer = gtsam.LevenbergMarquardtOptimizer( graph_to_optimize, self.prop_initial, params) self.result = optimizer.optimize() self.initial = self.result self.resultsFlag = True
def optimize(self, graph: gtsam.NonlinearFactorGraph, initial: gtsam.Values): """Optimize using Levenberg-Marquardt optimization.""" params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() return result
def estimate_global(self): np.random.seed(2) n0 = 0.000001 n03 = np.array([n0, n0, n0]) nNoiseFactor3 = np.array( [0.2, 0.2, 0.2]) # TODO: something about floats and major row? check cython # Create an empty nonlinear factor graph graph = gtsam.NonlinearFactorGraph() # Add a prior on the first pose, setting it to the origin priorMean = self.odom_global[0] priorNoise = gtsam.noiseModel_Diagonal.Sigmas(n03) graph.add(gtsam.PriorFactorPose2(self.X(1), priorMean, priorNoise)) # Add odometry factors odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3) for i, pose in enumerate(self.odom_relative): graph.add( gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose, odometryNoise)) # Add visual factors visualNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3) for i, pose in enumerate(self.visual_relative): graph.add( gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose, visualNoise)) # set initial guess to odometry estimates initialEstimate = gtsam.Values() for i, pose in enumerate(self.odom_global): initialEstimate.insert(self.X(i + 1), pose) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") # gtsam_quadrics.setMaxIterations(params, iter) # gtsam_quadrics.setRelativeErrorTol(params, 1e-8) # gtsam_quadrics.setAbsoluteErrorTol(params, 1e-8) # graph.print_('graph') # initialEstimate.print_('initialEstimate ') optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate, params) # parameters = gtsam.GaussNewtonParams() # parameters.relativeErrorTol = 1e-8 # parameters.maxIterations = 300 # optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters) result = optimizer.optimize() # result.print_('result ') # self.draw_trajectories([self.odom_global, self.visual_global], ['b', 'r'], 2) return self.unwrap_results(result)
def test_SFMExample(self): options = generator.Options() options.triangle = False options.nrCameras = 10 [data, truth] = generator.generate_data(options) measurementNoiseSigma = 1.0 pointNoiseSigma = 0.1 poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma) for i in range(len(data.Z)): for k in range(len(data.Z[i])): j = data.J[i][k] graph.add(gtsam.GenericProjectionFactorCal3_S2( data.Z[i][k], measurementNoise, X(i), P(j), data.K)) posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) graph.add(gtsam.PriorFactorPose3(X(0), truth.cameras[0].pose(), posePriorNoise)) pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) graph.add(gtsam.PriorFactorPoint3(P(0), truth.points[0], pointPriorNoise)) # Initial estimate initialEstimate = gtsam.Values() for i in range(len(truth.cameras)): pose_i = truth.cameras[i].pose() initialEstimate.insert(X(i), pose_i) for j in range(len(truth.points)): point_j = truth.points[j] initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) for i in range(5): optimizer.iterate() result = optimizer.values() # Marginalization marginals = gtsam.Marginals(graph, result) marginals.marginalCovariance(P(0)) marginals.marginalCovariance(X(0)) # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(X(i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(P(j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
def bundle_adjustment(self): """ Parameters: calibration - gtsam.Cal3_S2, camera calibration landmark_map - list, A map of landmarks and their correspondence """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initial_estimate = self.create_initial_estimate() # Create Projection Factors # """ # Measurement noise for bundle adjustment: # sigma = 1.0 # measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # """ for landmark_idx, observation_list in enumerate(self._landmark_map): for obersvation in observation_list: pose_idx = obersvation[0] key_point = obersvation[1] # To test indeterminate system # if(landmark_idx == 2 or landmark_idx == 480): # continue # if(pose_idx == 6): # print("yes2") # continue graph.add( gtsam.GenericProjectionFactorCal3_S2( key_point, self._measurement_noise, X(pose_idx), P(landmark_idx), self._calibration)) # Create priors for first two poses # """ # Pose prior noise: # rotation_sigma = np.radians(60) # translation_sigma = 1 # pose_noise_sigmas = np.array([rotation_sigma, rotation_sigma, rotation_sigma, # translation_sigma, translation_sigma, translation_sigma]) # """ for idx in (0, 1): pose_i = initial_estimate.atPose3(X(idx)) graph.add( gtsam.PriorFactorPose3(X(idx), pose_i, self._pose_prior_noise)) # Optimization # Using QR rather than Cholesky decomposition # params = gtsam.LevenbergMarquardtParams() # params.setLinearSolverType("MULTIFRONTAL_QR") # optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) sfm_result = optimizer.optimize() # Check if factor covariances are under constrain marginals = gtsam.Marginals( # pylint: disable=unused-variable graph, sfm_result) return sfm_result
def optimize(self, verbosity=0): params = gtsam.LevenbergMarquardtParams() params.setVerbosity(["SILENT", "TERMINATION"][verbosity]) params.setAbsoluteErrorTol(1e-10) params.setRelativeErrorTol(1e-10) optimizer = gtsam.LevenbergMarquardtOptimizer(self.gtsam_graph, self.values, params) result_values = optimizer.optimize() return {name: gtsam2sophus(result_values.atPose3(var)) for name, var in self.vars.items()}
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 __optimize_factor_graph(self, graph: NonlinearFactorGraph, initial_values: Values) -> Values: """Optimize the factor graph.""" params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("ERROR") if self._max_iterations: params.setMaxIterations(self._max_iterations) lm = gtsam.LevenbergMarquardtOptimizer(graph, initial_values, params) result_values = lm.optimize() return result_values
def trajectory_estimator(self, features, visible_map): """ Estimate current pose with matched features through GTSAM and update the trajectory Parameters: features: A Feature Object. Stores all matched Superpoint features. visible_map: A Map Object. Stores all matched Landmark points. pose: gtsam.pose3 Object. The pose at the last state from the atrium map trajectory. Returns: current_pose: gtsam.pose3 Object. The current estimate pose. Use input matched features as the projection factors of the graph. Use input visible_map (match landmark points of the map) as the landmark priors of the graph. Use the last time step pose from the trajectory as the initial estimate value of the current pose """ assert len(self.atrium_map.trajectory) != 0, "Trajectory is empty." pose = self.atrium_map.trajectory[len(self.atrium_map.trajectory)-1] # Initialize factor graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add projection factors with matched feature points for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) for i, feature in enumerate(features.key_point_list): graph.add(gtsam.GenericProjectionFactorCal3_S2( feature, measurementNoise, X(0), P(i), self.calibration)) # Create initial estimate for the pose # Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose. initialEstimate.insert(X(0), pose) # Create priors for visual # Because the map is known, we use the landmarks from the visible map with nearly zero error as priors. pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01) for i, landmark in enumerate(visible_map.landmark_list): graph.add(gtsam.PriorFactorPoint3(P(i), landmark, pointPriorNoise)) initialEstimate.insert(P(i), landmark) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimize() # Add current pose to the trajectory current_pose = result.atPose3(symbol(ord('x'), 0)) self.atrium_map.append_trajectory(current_pose) return current_pose
def test_lm_simple_printing(self): """Make sure we are properly terminating LM""" def hook(_, error): print(error) params = gtsam.LevenbergMarquardtParams() actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial) self.check(actual) actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial, params) self.check(actual) actual = gtsam_optimize( gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params), params, hook)
def test_Pose3SLAMExample(self) -> None: # Create a hexagon of poses hexagon = circlePose3(numPoses=6, radius=1.0) p0 = hexagon.atPose3(0) p1 = hexagon.atPose3(1) # create a Pose graph with one equality constraint and one measurement fg = gtsam.NonlinearFactorGraph() fg.add(gtsam.NonlinearEqualityPose3(0, p0)) delta = p0.between(p1) covariance = gtsam.noiseModel.Diagonal.Sigmas( np.array([ 0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.) ])) fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance)) # Create initial config initial = gtsam.Values() s = 0.10 initial.insert(0, p0) initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1))) initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1))) initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1))) initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) # optimize optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial) result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) self.gtsamAssertEquals(pose_1, p1, 1e-4)
def bundle_adjustment(self): """ Bundle Adjustment. Input: self._image_features self._image_points Output: result - gtsam optimzation result """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() pose_indices, point_indices = self.create_index_sets() initial_estimate = self.create_initial_estimate(pose_indices) # Create Projection Factor sigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) for img_idx, features in enumerate(self._image_features): for kp_idx, keypoint in enumerate(features.keypoints): if self.image_points[img_idx].kp_3d_exist(kp_idx): landmark_id = self.image_points[img_idx].kp_3d(kp_idx) if self._landmark_estimates[ landmark_id].seen >= self._min_landmark_seen: key_point = Point2(keypoint[0], keypoint[1]) graph.add( gtsam.GenericProjectionFactorCal3_S2( key_point, measurementNoise, X(img_idx), P(landmark_id), self._calibration)) # Create priors for first two poses s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 1, 1, 1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) for idx in (0, 1): pose_i = initial_estimate.atPose3(X(idx)) graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) sfm_result = optimizer.optimize() return sfm_result, pose_indices, point_indices
def pose_estimate(self, observations, estimated_pose): """ Estimate current pose with matched features through GTSAM and update the trajectory Parameters: observations - [(Point2(), Point3())] pose - gtsam.pose3 Object. The pose at the last state from the trajectory. Returns: current_pose - gtsam.pose3 Object. The current estimate pose. """ if len(observations) == 0: print("No observation") # Initialize factor graph graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() measurement_noise_sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma( 2, measurement_noise_sigma) # Because the map is known, we use the landmarks from the visible map with nearly zero error as priors. point_prior_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01) for i, observation in enumerate(observations): print(i) # Add projection factors with matched feature points graph.add( gtsam.GenericProjectionFactorCal3_S2(observation[0], measurement_noise, X(0), P(i), self.calibration)) # Create priors for all observations graph.add( gtsam.PriorFactorPoint3(P(i), observation[1], point_prior_noise)) initial_estimate.insert(P(i), observation[1]) # Create initial estimate for the pose # Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose. initial_estimate.insert(X(0), estimated_pose) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) result = optimizer.optimize() # Add current pose to the trajectory current_pose = result.atPose3(X(0)) print(current_pose) return current_pose
def test_optimization(self): """Tests if a factor graph with a CustomFactor can be properly optimized""" gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) expected = Pose2(2, 2, np.pi / 2) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated :param v: Values object :param H: list of references to the Jacobian arrays :return: the non-linear error """ key0 = this.keys()[0] key1 = this.keys()[1] gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) if H is not None: result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) fg = gtsam.NonlinearFactorGraph() fg.add(cf) fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model)) v = Values() v.insert(0, Pose2(0, 0, 0)) v.insert(1, Pose2(0, 0, 0)) params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params) result = optimizer.optimize() self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5) self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
def test_convertNonlinear(self): """Test converting a linear factor graph to a nonlinear one""" graph, X = create_graph() EXPECTEDM = [1, 2, 3] # create nonlinear factor graph for marginalization nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph) optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values()) nlresult = optimizer.optimizeSafely() # marginalize marginals = gtsam.Marginals(nfg, nlresult) m = [marginals.marginalCovariance(x) for x in X] # check linear marginalizations self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from # NonlinearFactorGraph) graph = gtsam.NonlinearFactorGraph() # Add two odometry factors # create a measurement for both factors (the same in this case) odometry = gtsam.Pose2(2.0, 0.0, 0.0) odometryNoise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) # Add three "GPS" measurements # We use Pose2 Priors here with high variance on theta groundTruth = gtsam.Values() groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0)) groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0)) groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0)) model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) for i in range(3): graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model)) # Initialize to noisy points initialEstimate = gtsam.Values() initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() # Plot Covariance Ellipses marginals = gtsam.Marginals(graph, result) P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) P[i] = marginals.marginalCovariance(i)
def optimize(graph, initial_estimate, calibration): # create optimizer parameters params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") # SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA : VALUES, ERROR params.setMaxIterations(100) params.setlambdaInitial(1e-5) params.setlambdaUpperBound(1e10) params.setlambdaLowerBound(1e-8) params.setRelativeErrorTol(1e-5) params.setAbsoluteErrorTol(1e-5) # create optimizer optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) # run optimizer print('starting optimization') estimate = optimizer.optimize() print('optimization finished') return estimate
def compute_control(self, start_state, nominal_states): # build graph graph = gtsam.NonlinearFactorGraph() x0_key = self.state_key(0) graph.add(gtd.PriorFactorVector5(x0_key, start_state, self.prior_noise)) for i in range(self.N): x_curr_key = self.state_key(i) x_next_key = self.state_key(i + 1) graph.add( gtd.CPDynamicsFactor(x_curr_key, x_next_key, self.transition_noise, self.dt)) graph.add(gtd.CPStateCostFactor(x_next_key, self.cost_noise)) # build init values init_values = gtsam.Values() for i in range(self.N + 1): x_key = self.state_key(i) init_values.insert(x_key, np.array(nominal_states[i])) # optimize params = gtsam.LevenbergMarquardtParams() params.setMaxIterations(20) # params.setVerbosityLM("SUMMARY") params.setLinearSolverType("MULTIFRONTAL_QR") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, init_values, params) results = optimizer.optimize() # print("error:", graph.error(results)) # get results states = [] for i in range(self.N + 1): x_key = self.state_key(i) states.append(results.atVector(x_key)) dx_c = states[1][-1] - states[0][-1] u_star = states[0][-1] + 1.1 * dx_c / (20 * self.dt) return u_star, states
def setUp(self): """Set up a small Karcher mean optimization example.""" # Grabbed from KarcherMeanFactor unit tests. R = Rot3.Expmap(np.array([0.1, 0, 0])) rotations = {R, R.inverse()} # mean is the identity self.expected = Rot3() graph = gtsam.NonlinearFactorGraph() for R in rotations: graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) initial = gtsam.Values() initial.insert(KEY, R) self.params = gtsam.GaussNewtonParams() self.optimizer = gtsam.GaussNewtonOptimizer(graph, initial, self.params) self.lmparams = gtsam.LevenbergMarquardtParams() self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( graph, initial, self.lmparams) # setup output capture self.capturedOutput = StringIO() sys.stdout = self.capturedOutput
def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from # NonlinearFactorGraph) graph = gtsam.NonlinearFactorGraph() # Add a Gaussian prior on pose x_1 priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array( [0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta # add directly to graph graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) # Add two odometry factors # create a measurement for both factors (the same in this case) odometry = gtsam.Pose2(2.0, 0.0, 0.0) odometryNoise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) # Initialize to noisy points initialEstimate = gtsam.Values() initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() marginals = gtsam.Marginals(graph, result) marginals.marginalCovariance(1) # Check first pose equality pose_1 = result.atPose2(1) self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
def run(self, initial_data: GtsfmData) -> GtsfmData: """Run the bundle adjustment by forming factor graph and optimizing using Levenberg–Marquardt optimization. Args: initial_data: initialized cameras, tracks w/ their 3d landmark from triangulation. Results: optimized camera poses, 3D point w/ tracks, and error metrics. """ logger.info( f"Input: {initial_data.number_tracks()} tracks on {len(initial_data.get_valid_camera_indices())} cameras\n" ) # noise model for measurements -- one pixel in u and v measurement_noise = gtsam.noiseModel.Isotropic.Sigma( IMG_MEASUREMENT_DIM, 1.0) # Create a factor graph graph = gtsam.NonlinearFactorGraph() # Add measurements to the factor graph for j in range(initial_data.number_tracks()): track = initial_data.get_track(j) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P graph.add( GeneralSFMFactorCal3Bundler(uv, measurement_noise, C(i), P(j))) # get all the valid camera indices, which need to be added to the graph. valid_camera_indices = initial_data.get_valid_camera_indices() # Add a prior on first pose. This indirectly specifies where the origin is. graph.push_back( gtsam.PriorFactorPinholeCameraCal3Bundler( C(valid_camera_indices[0]), initial_data.get_camera(valid_camera_indices[0]), gtsam.noiseModel.Isotropic.Sigma(PINHOLE_CAM_CAL3BUNDLER_DOF, 0.1), )) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( P(0), initial_data.get_track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1))) # Create initial estimate initial = gtsam.Values() # add each PinholeCameraCal3Bundler for i in valid_camera_indices: camera = initial_data.get_camera(i) initial.insert(C(i), camera) # add each SfmTrack for j in range(initial_data.number_tracks()): track = initial_data.get_track(j) initial.insert(P(j), track.point3()) # Optimize the graph and print results try: params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("ERROR") lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result_values = lm.optimize() except Exception: logger.exception("LM Optimization failed") # as we did not perform the bundle adjustment, we skip computing the total reprojection error return GtsfmData(initial_data.number_images()) final_error = graph.error(result_values) # Error drops from ~2764.22 to ~0.046 logger.info(f"initial error: {graph.error(initial):.2f}") logger.info(f"final error: {final_error:.2f}") # construct the results optimized_data = values_to_gtsfm_data(result_values, initial_data) metrics_dict = {} metrics_dict["before_filtering"] = optimized_data.aggregate_metrics() logger.info("[Result] Number of tracks before filtering: %d", metrics_dict["before_filtering"]["number_tracks"]) # filter the largest errors filtered_result = optimized_data.filter_landmarks( self.output_reproj_error_thresh) metrics_dict["after_filtering"] = filtered_result.aggregate_metrics() io_utils.save_json_file( os.path.join(METRICS_PATH, "bundle_adjustment_metrics.json"), metrics_dict) logger.info("[Result] Number of tracks after filtering: %d", metrics_dict["after_filtering"]["number_tracks"]) logger.info( "[Result] Mean track length %.3f", metrics_dict["after_filtering"]["3d_track_lengths"]["mean"]) logger.info( "[Result] Median track length %.3f", metrics_dict["after_filtering"]["3d_track_lengths"]["median"]) filtered_result.log_scene_reprojection_error_stats() return filtered_result
predicted_nav_state = current_preintegrated_IMU.predict( params.initial_state, params.IMU_bias) initial_values.insert(symbol('x', i), predicted_nav_state.pose()) initial_values.insert(symbol('v', i), predicted_nav_state.velocity()) current_preintegrated_IMU.integrateMeasurement(imu_measurements[i, :3], imu_measurements[i, 3:], 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')
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 priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) # Add odometry factors odometry = gtsam.Pose2(2.0, 0.0, 0.0) # For simplicity, we will use the same noise model for each odometry factor odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) # Create odometry (Between) factors between consecutive poses graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) graph.print("\nFactor Graph:\n") # 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)) initial.print("\nInitial Estimate:\n") # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() result.print("\nFinal Result:\n")
def run(self): graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) H9 = gtsam.OptionalJacobian9() 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) H2 = gtsam.OptionalJacobian96() print(pim.predict(actual_state_i, self.actualBias, H9, H2)) 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)) plotPose3(POSES_FIG, pose_i, 0.1) i += 1 print(result.atConstantBias(BIAS_KEY)) plt.ioff() plt.show()
def run(args): """Test Dogleg vs LM, inspired by issue #452.""" # print parameters print("num samples = {}, deltaInitial = {}".format(args.num_samples, args.delta)) # Ground truth solution T11 = gtsam.Pose2(0, 0, 0) T12 = gtsam.Pose2(1, 0, 0) T21 = gtsam.Pose2(0, 1, 0) T22 = gtsam.Pose2(1, 1, 0) # Factor graph graph = gtsam.NonlinearFactorGraph() # Priors prior = gtsam.noiseModel.Isotropic.Sigma(3, 1) graph.add(gtsam.PriorFactorPose2(11, T11, prior)) graph.add(gtsam.PriorFactorPose2(21, T21, prior)) # Odometry model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) # Range model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01) graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) params = gtsam.DoglegParams() params.setDeltaInitial(args.delta) # default is 10 # Add progressively more noise to ground truth sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] n = len(sigmas) p_dl, s_dl, p_lm, s_lm = [0] * n, [0] * n, [0] * n, [0] * n for i, sigma in enumerate(sigmas): dl_fails, lm_fails = 0, 0 # Attempt num_samples optimizations for both DL and LM for _attempt in range(args.num_samples): initial = gtsam.Values() initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) # Run dogleg optimizer dl = gtsam.DoglegOptimizer(graph, initial, params) result = dl.optimize() dl_fails += graph.error(result) > 1e-9 # Run lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) result = lm.optimize() lm_fails += graph.error(result) > 1e-9 # Calculate Bayes estimate of success probability # using a beta prior of alpha=0.5, beta=0.5 alpha, beta = 0.5, 0.5 v = args.num_samples + alpha + beta p_dl[i] = (args.num_samples - dl_fails + alpha) / v p_lm[i] = (args.num_samples - lm_fails + alpha) / v def stddev(p): """Calculate standard deviation.""" return math.sqrt(p * (1 - p) / (1 + v)) s_dl[i] = stddev(p_dl[i]) s_lm[i] = stddev(p_lm[i]) fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" print( fmt.format(sigma, 100 * p_dl[i], 100 * s_dl[i], 100 * p_lm[i], 100 * s_lm[i])) if args.plot: fig, ax = plt.subplots() dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") plt.title("Dogleg emprical success vs. LM") plt.legend(handles=[dl_plot, lm_plot]) ax.set_xlim(0, sigmas[-1] + 1) ax.set_ylim(0, 1) plt.show()
graph.add(odometry_factor) # reproject true quadrics into each true pose for j, quadric in enumerate(quadrics): for i, pose in enumerate(poses): conic = quadricslam.QuadricCamera.project(quadric, pose, calibration) bounds = conic.bounds() bbf = quadricslam.BoundingBoxFactor(bounds, calibration, X(i), Q(j), bbox_noise) bbf.addToGraph(graph) # define lm parameters parameters = gtsam.LevenbergMarquardtParams() parameters.setVerbosityLM( "SUMMARY" ) # SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA : VALUES, ERROR parameters.setMaxIterations(100) parameters.setlambdaInitial(1e-5) parameters.setlambdaUpperBound(1e10) parameters.setlambdaLowerBound(1e-8) parameters.setRelativeErrorTol(1e-5) parameters.setAbsoluteErrorTol(1e-5) # create optimizer optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, parameters) # run optimizer result = optimizer.optimize()
def main(): """ Step 1: Create a factor to express a unary constraint The "prior" in this case is the measurement from a sensor, with a model of the noise on the measurement. The "Key" created here is a label used to associate parts of the state (stored in "RotValues") with particular factors. They require an index to allow for lookup, and should be unique. In general, creating a factor requires: - A key or set of keys labeling the variables that are acted upon - A measurement value - A measurement model with the correct dimensionality for the factor """ prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) prior.print_('goal angle') model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) key = gtsam.symbol(ord('x'), 1) factor = gtsam.PriorFactorRot2(key, prior, model) """ Step 2: Create a graph container and add the factor to it Before optimizing, all factors need to be added to a Graph container, which provides the necessary top-level functionality for defining a system of constraints. In this case, there is only one factor, but in a practical scenario, many more factors would be added. """ graph = gtsam.NonlinearFactorGraph() graph.push_back(factor) graph.print_('full graph') """ Step 3: Create an initial estimate An initial estimate of the solution for the system is necessary to start optimization. This system state is the "Values" instance, which is similar in structure to a dictionary, in that it maps keys (the label created in step 1) to specific values. The initial estimate provided to optimization will be used as a linearization point for optimization, so it is important that all of the variables in the graph have a corresponding value in this structure. """ initial = gtsam.Values() initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) initial.print_('initial estimate') """ Step 4: Optimize After formulating the problem with a graph of constraints and an initial estimate, executing optimization is as simple as calling a general optimization function with the graph and initial estimate. This will yield a new RotValues structure with the final state of the optimization. """ result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() result.print_('final result')