Merge pull request #903 from borglab/feature/python-examples
						commit
						c56579c61d
					
				|  | @ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task. | |||
| Author: Fan Jiang, Frank Dellaert | ||||
| """ | ||||
| 
 | ||||
| from functools import partial | ||||
| from typing import List, Optional | ||||
| 
 | ||||
| import gtsam | ||||
| import numpy as np | ||||
| 
 | ||||
| from typing import List, Optional | ||||
| from functools import partial | ||||
| I = np.eye(1) | ||||
| 
 | ||||
| 
 | ||||
| def simulate_car(): | ||||
|     # Simulate a car for one second | ||||
| def simulate_car() -> List[float]: | ||||
|     """Simulate a car for one second""" | ||||
|     x0 = 0 | ||||
|     dt = 0.25  # 4 Hz, typical GPS | ||||
|     v = 144 * 1000 / 3600  # 144 km/hour = 90mph, pretty fast | ||||
|  | @ -26,46 +28,9 @@ def simulate_car(): | |||
|     return x | ||||
| 
 | ||||
| 
 | ||||
| x = simulate_car() | ||||
| print(f"Simulated car trajectory: {x}") | ||||
| 
 | ||||
| # %% | ||||
| add_noise = True  # set this to False to run with "perfect" measurements | ||||
| 
 | ||||
| # GPS measurements | ||||
| sigma_gps = 3.0  # assume GPS is +/- 3m | ||||
| g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) | ||||
|      for k in range(5)] | ||||
| 
 | ||||
| # Odometry measurements | ||||
| sigma_odo = 0.1  # assume Odometry is 10cm accurate at 4Hz | ||||
| o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0) | ||||
|      for k in range(4)] | ||||
| 
 | ||||
| # Landmark measurements: | ||||
| sigma_lm = 1  # assume landmark measurement is accurate up to 1m | ||||
| 
 | ||||
| # Assume first landmark is at x=5, we measure it at time k=0 | ||||
| lm_0 = 5.0 | ||||
| z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) | ||||
| 
 | ||||
| # Assume other landmark is at x=28, we measure it at time k=3 | ||||
| lm_3 = 28.0 | ||||
| z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) | ||||
| 
 | ||||
| unknown = [gtsam.symbol('x', k) for k in range(5)] | ||||
| 
 | ||||
| print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) | ||||
| 
 | ||||
| # We now can use nonlinear factor graphs | ||||
| factor_graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # Add factors for GPS measurements | ||||
| I = np.eye(1) | ||||
| gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) | ||||
| 
 | ||||
| 
 | ||||
| def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): | ||||
| def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, | ||||
|               values: gtsam.Values, | ||||
|               jacobians: Optional[List[np.ndarray]]) -> float: | ||||
|     """GPS Factor error function | ||||
|     :param measurement: GPS measurement, to be filled with `partial` | ||||
|     :param this: gtsam.CustomFactor handle | ||||
|  | @ -82,36 +47,9 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobia | |||
|     return error | ||||
| 
 | ||||
| 
 | ||||
| # Add the GPS factors | ||||
| for k in range(5): | ||||
|     gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) | ||||
|     factor_graph.add(gf) | ||||
| 
 | ||||
| # New Values container | ||||
| v = gtsam.Values() | ||||
| 
 | ||||
| # Add initial estimates to the Values container | ||||
| for i in range(5): | ||||
|     v.insert(unknown[i], np.array([0.0])) | ||||
| 
 | ||||
| # Initialize optimizer | ||||
| params = gtsam.GaussNewtonParams() | ||||
| optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
| 
 | ||||
| # Optimize the factor graph | ||||
| result = optimizer.optimize() | ||||
| 
 | ||||
| # calculate the error from ground truth | ||||
| error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) | ||||
| 
 | ||||
| print("Result with only GPS") | ||||
| print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
| 
 | ||||
| # Adding odometry will improve things a lot | ||||
| odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) | ||||
| 
 | ||||
| 
 | ||||
| def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): | ||||
| def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, | ||||
|                values: gtsam.Values, | ||||
|                jacobians: Optional[List[np.ndarray]]) -> float: | ||||
|     """Odometry Factor error function | ||||
|     :param measurement: Odometry measurement, to be filled with `partial` | ||||
|     :param this: gtsam.CustomFactor handle | ||||
|  | @ -130,25 +68,9 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobi | |||
|     return error | ||||
| 
 | ||||
| 
 | ||||
| for k in range(4): | ||||
|     odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) | ||||
|     factor_graph.add(odof) | ||||
| 
 | ||||
| params = gtsam.GaussNewtonParams() | ||||
| optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
| 
 | ||||
| result = optimizer.optimize() | ||||
| 
 | ||||
| error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) | ||||
| 
 | ||||
| print("Result with GPS+Odometry") | ||||
| print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
| 
 | ||||
| # This is great, but GPS noise is still apparent, so now we add the two landmarks | ||||
| lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) | ||||
| 
 | ||||
| 
 | ||||
| def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): | ||||
| def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, | ||||
|              values: gtsam.Values, | ||||
|              jacobians: Optional[List[np.ndarray]]) -> float: | ||||
|     """Landmark Factor error function | ||||
|     :param measurement: Landmark measurement, to be filled with `partial` | ||||
|     :param this: gtsam.CustomFactor handle | ||||
|  | @ -165,15 +87,120 @@ def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobian | |||
|     return error | ||||
| 
 | ||||
| 
 | ||||
| factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) | ||||
| factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) | ||||
| def main(): | ||||
|     """Main runner.""" | ||||
| 
 | ||||
| params = gtsam.GaussNewtonParams() | ||||
| optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
|     x = simulate_car() | ||||
|     print(f"Simulated car trajectory: {x}") | ||||
| 
 | ||||
| result = optimizer.optimize() | ||||
|     add_noise = True  # set this to False to run with "perfect" measurements | ||||
| 
 | ||||
| error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) | ||||
|     # GPS measurements | ||||
|     sigma_gps = 3.0  # assume GPS is +/- 3m | ||||
|     g = [ | ||||
|         x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) | ||||
|         for k in range(5) | ||||
|     ] | ||||
| 
 | ||||
| print("Result with GPS+Odometry+Landmark") | ||||
| print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
|     # Odometry measurements | ||||
|     sigma_odo = 0.1  # assume Odometry is 10cm accurate at 4Hz | ||||
|     o = [ | ||||
|         x[k + 1] - x[k] + | ||||
|         (np.random.normal(scale=sigma_odo) if add_noise else 0) | ||||
|         for k in range(4) | ||||
|     ] | ||||
| 
 | ||||
|     # Landmark measurements: | ||||
|     sigma_lm = 1  # assume landmark measurement is accurate up to 1m | ||||
| 
 | ||||
|     # Assume first landmark is at x=5, we measure it at time k=0 | ||||
|     lm_0 = 5.0 | ||||
|     z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) | ||||
| 
 | ||||
|     # Assume other landmark is at x=28, we measure it at time k=3 | ||||
|     lm_3 = 28.0 | ||||
|     z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) | ||||
| 
 | ||||
|     unknown = [gtsam.symbol('x', k) for k in range(5)] | ||||
| 
 | ||||
|     print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) | ||||
| 
 | ||||
|     # We now can use nonlinear factor graphs | ||||
|     factor_graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
|     # Add factors for GPS measurements | ||||
|     gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) | ||||
| 
 | ||||
|     # Add the GPS factors | ||||
|     for k in range(5): | ||||
|         gf = gtsam.CustomFactor(gps_model, [unknown[k]], | ||||
|                                 partial(error_gps, np.array([g[k]]))) | ||||
|         factor_graph.add(gf) | ||||
| 
 | ||||
|     # New Values container | ||||
|     v = gtsam.Values() | ||||
| 
 | ||||
|     # Add initial estimates to the Values container | ||||
|     for i in range(5): | ||||
|         v.insert(unknown[i], np.array([0.0])) | ||||
| 
 | ||||
|     # Initialize optimizer | ||||
|     params = gtsam.GaussNewtonParams() | ||||
|     optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
| 
 | ||||
|     # Optimize the factor graph | ||||
|     result = optimizer.optimize() | ||||
| 
 | ||||
|     # calculate the error from ground truth | ||||
|     error = np.array([(result.atVector(unknown[k]) - x[k])[0] | ||||
|                       for k in range(5)]) | ||||
| 
 | ||||
|     print("Result with only GPS") | ||||
|     print(result, np.round(error, 2), | ||||
|           f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
| 
 | ||||
|     # Adding odometry will improve things a lot | ||||
|     odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) | ||||
| 
 | ||||
|     for k in range(4): | ||||
|         odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], | ||||
|                                   partial(error_odom, np.array([o[k]]))) | ||||
|         factor_graph.add(odof) | ||||
| 
 | ||||
|     params = gtsam.GaussNewtonParams() | ||||
|     optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
| 
 | ||||
|     result = optimizer.optimize() | ||||
| 
 | ||||
|     error = np.array([(result.atVector(unknown[k]) - x[k])[0] | ||||
|                       for k in range(5)]) | ||||
| 
 | ||||
|     print("Result with GPS+Odometry") | ||||
|     print(result, np.round(error, 2), | ||||
|           f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
| 
 | ||||
|     # This is great, but GPS noise is still apparent, so now we add the two landmarks | ||||
|     lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) | ||||
| 
 | ||||
|     factor_graph.add( | ||||
|         gtsam.CustomFactor(lm_model, [unknown[0]], | ||||
|                            partial(error_lm, np.array([lm_0 + z_0])))) | ||||
|     factor_graph.add( | ||||
|         gtsam.CustomFactor(lm_model, [unknown[3]], | ||||
|                            partial(error_lm, np.array([lm_3 + z_3])))) | ||||
| 
 | ||||
|     params = gtsam.GaussNewtonParams() | ||||
|     optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) | ||||
| 
 | ||||
|     result = optimizer.optimize() | ||||
| 
 | ||||
|     error = np.array([(result.atVector(unknown[k]) - x[k])[0] | ||||
|                       for k in range(5)]) | ||||
| 
 | ||||
|     print("Result with GPS+Odometry+Landmark") | ||||
|     print(result, np.round(error, 2), | ||||
|           f"\nJ(X)={0.5 * np.sum(np.square(error))}") | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -13,13 +13,8 @@ Author: Mandy Xie | |||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| 
 | ||||
| import matplotlib.pyplot as plt | ||||
| import gtsam.utils.plot as gtsam_plot | ||||
| 
 | ||||
| # ENU Origin is where the plane was in hold next to runway | ||||
| lat0 = 33.86998 | ||||
| lon0 = -84.30626 | ||||
|  | @ -29,28 +24,34 @@ h0 = 274 | |||
| GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) | ||||
| PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25) | ||||
| 
 | ||||
| # 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)) | ||||
| def main(): | ||||
|     """Main runner.""" | ||||
|     # Create an empty nonlinear factor graph | ||||
|     graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # Add GPS factors | ||||
| gps = gtsam.Point3(lat0, lon0, h0) | ||||
| graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) | ||||
| print("\nFactor Graph:\n{}".format(graph)) | ||||
|     # 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)) | ||||
| 
 | ||||
| # 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)) | ||||
|     # Add GPS factors | ||||
|     gps = gtsam.Point3(lat0, lon0, h0) | ||||
|     graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) | ||||
|     print("\nFactor Graph:\n{}".format(graph)) | ||||
| 
 | ||||
| # optimize using Levenberg-Marquardt optimization | ||||
| params = gtsam.LevenbergMarquardtParams() | ||||
| optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) | ||||
| result = optimizer.optimize() | ||||
| print("\nFinal Result:\n{}".format(result)) | ||||
|     # 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)) | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -13,57 +13,60 @@ Author: Frank Dellaert | |||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| 
 | ||||
| import matplotlib.pyplot as plt | ||||
| import gtsam.utils.plot as gtsam_plot | ||||
| import matplotlib.pyplot as plt | ||||
| import numpy as np | ||||
| 
 | ||||
| # 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)) | ||||
| def main(): | ||||
|     """Main runner""" | ||||
|     # Create an empty nonlinear factor graph | ||||
|     graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # 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 | ||||
| # Create odometry (Between) factors between consecutive poses | ||||
| graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) | ||||
| graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) | ||||
| print("\nFactor Graph:\n{}".format(graph)) | ||||
|     # 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)) | ||||
| 
 | ||||
| # 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)) | ||||
|     # 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 | ||||
|     # Create odometry (Between) factors between consecutive poses | ||||
|     graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) | ||||
|     graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) | ||||
|     print("\nFactor Graph:\n{}".format(graph)) | ||||
| 
 | ||||
| # optimize using Levenberg-Marquardt optimization | ||||
| params = gtsam.LevenbergMarquardtParams() | ||||
| optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) | ||||
| result = optimizer.optimize() | ||||
| print("\nFinal Result:\n{}".format(result)) | ||||
|     # 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)) | ||||
| 
 | ||||
| # 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() | ||||
|     # 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))) | ||||
| 
 | ||||
|     for i in range(1, 4): | ||||
|         gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, | ||||
|                               marginals.marginalCovariance(i)) | ||||
|     plt.axis('equal') | ||||
|     plt.show() | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -13,69 +13,85 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) | |||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| from gtsam.symbol_shorthand import X, L | ||||
| import numpy as np | ||||
| from gtsam.symbol_shorthand import L, X | ||||
| 
 | ||||
| # Create noise models | ||||
| PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) | ||||
| ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) | ||||
| MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) | ||||
| 
 | ||||
| # Create an empty nonlinear factor graph | ||||
| graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # Create the keys corresponding to unknown variables in the factor graph | ||||
| X1 = X(1) | ||||
| X2 = X(2) | ||||
| X3 = X(3) | ||||
| L1 = L(4) | ||||
| L2 = L(5) | ||||
| def main(): | ||||
|     """Main runner""" | ||||
| 
 | ||||
| # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model | ||||
| graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) | ||||
|     # Create an empty nonlinear factor graph | ||||
|     graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # Add odometry factors between X1,X2 and X2,X3, respectively | ||||
| graph.add(gtsam.BetweenFactorPose2( | ||||
|     X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) | ||||
| graph.add(gtsam.BetweenFactorPose2( | ||||
|     X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) | ||||
|     # Create the keys corresponding to unknown variables in the factor graph | ||||
|     X1 = X(1) | ||||
|     X2 = X(2) | ||||
|     X3 = X(3) | ||||
|     L1 = L(4) | ||||
|     L2 = L(5) | ||||
| 
 | ||||
| # Add Range-Bearing measurements to two different landmarks L1 and L2 | ||||
| graph.add(gtsam.BearingRangeFactor2D( | ||||
|     X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) | ||||
| graph.add(gtsam.BearingRangeFactor2D( | ||||
|     X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) | ||||
| graph.add(gtsam.BearingRangeFactor2D( | ||||
|     X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) | ||||
|     # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model | ||||
|     graph.add( | ||||
|         gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) | ||||
| 
 | ||||
| # Print graph | ||||
| print("Factor Graph:\n{}".format(graph)) | ||||
|     # Add odometry factors between X1,X2 and X2,X3, respectively | ||||
|     graph.add( | ||||
|         gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), | ||||
|                                  ODOMETRY_NOISE)) | ||||
|     graph.add( | ||||
|         gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), | ||||
|                                  ODOMETRY_NOISE)) | ||||
| 
 | ||||
| # Create (deliberately inaccurate) initial estimate | ||||
| initial_estimate = gtsam.Values() | ||||
| initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) | ||||
| initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) | ||||
| initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) | ||||
| initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) | ||||
| initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) | ||||
|     # Add Range-Bearing measurements to two different landmarks L1 and L2 | ||||
|     graph.add( | ||||
|         gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), | ||||
|                                    np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) | ||||
|     graph.add( | ||||
|         gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, | ||||
|                                    MEASUREMENT_NOISE)) | ||||
|     graph.add( | ||||
|         gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, | ||||
|                                    MEASUREMENT_NOISE)) | ||||
| 
 | ||||
| # Print | ||||
| print("Initial Estimate:\n{}".format(initial_estimate)) | ||||
|     # Print graph | ||||
|     print("Factor Graph:\n{}".format(graph)) | ||||
| 
 | ||||
| # Optimize using Levenberg-Marquardt optimization. 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. | ||||
| # Here we will use the default set of parameters.  See the | ||||
| # documentation for the full set of parameters. | ||||
| params = gtsam.LevenbergMarquardtParams() | ||||
| optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) | ||||
| result = optimizer.optimize() | ||||
| print("\nFinal Result:\n{}".format(result)) | ||||
|     # Create (deliberately inaccurate) initial estimate | ||||
|     initial_estimate = gtsam.Values() | ||||
|     initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) | ||||
|     initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) | ||||
|     initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) | ||||
|     initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) | ||||
|     initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) | ||||
| 
 | ||||
| # Calculate and print marginal covariances for all variables | ||||
| marginals = gtsam.Marginals(graph, result) | ||||
| for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: | ||||
|     print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) | ||||
|     # Print | ||||
|     print("Initial Estimate:\n{}".format(initial_estimate)) | ||||
| 
 | ||||
|     # Optimize using Levenberg-Marquardt optimization. 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. | ||||
|     # Here we will use the default set of parameters.  See the | ||||
|     # documentation for the full set of parameters. | ||||
|     params = gtsam.LevenbergMarquardtParams() | ||||
|     optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, | ||||
|                                                   params) | ||||
|     result = optimizer.optimize() | ||||
|     print("\nFinal Result:\n{}".format(result)) | ||||
| 
 | ||||
|     # Calculate and print marginal covariances for all variables | ||||
|     marginals = gtsam.Marginals(graph, result) | ||||
|     for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), | ||||
|                      (L2, "L2")]: | ||||
|         print("{} covariance:\n{}\n".format(s, | ||||
|                                             marginals.marginalCovariance(key))) | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -15,82 +15,88 @@ from __future__ import print_function | |||
| 
 | ||||
| import math | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| 
 | ||||
| import matplotlib.pyplot as plt | ||||
| import gtsam.utils.plot as gtsam_plot | ||||
| import matplotlib.pyplot as plt | ||||
| 
 | ||||
| 
 | ||||
| def vector3(x, y, z): | ||||
|     """Create 3d double numpy array.""" | ||||
|     return np.array([x, y, z], dtype=float) | ||||
| 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)) | ||||
| 
 | ||||
| # Create noise models | ||||
| PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) | ||||
| ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) | ||||
|     # 1. Create a factor graph container and add factors to it | ||||
|     graph = gtsam.NonlinearFactorGraph() | ||||
| 
 | ||||
| # 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)) | ||||
| 
 | ||||
| # 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)) | ||||
| 
 | ||||
| # 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 | ||||
| 
 | ||||
| # 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 | ||||
| 
 | ||||
| # 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() | ||||
| 
 | ||||
| # 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)) | ||||
| 
 | ||||
| # 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))) | ||||
| 
 | ||||
| # 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)) | ||||
| 
 | ||||
| fig = plt.figure(0) | ||||
| for i in range(1, 6): | ||||
|     gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) | ||||
|     plt.axis('equal') | ||||
|     plt.show() | ||||
| 
 | ||||
| plt.axis('equal') | ||||
| plt.show() | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -12,77 +12,86 @@ and does the optimization. Output is written on a file, in g2o format | |||
| # pylint: disable=invalid-name, E1101 | ||||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import argparse | ||||
| import math | ||||
| import numpy as np | ||||
| import matplotlib.pyplot as plt | ||||
| 
 | ||||
| import gtsam | ||||
| import matplotlib.pyplot as plt | ||||
| from gtsam.utils import plot | ||||
| 
 | ||||
| 
 | ||||
| def vector3(x, y, z): | ||||
|     """Create 3d double numpy array.""" | ||||
|     return np.array([x, y, z], dtype=float) | ||||
| 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() | ||||
| 
 | ||||
| 
 | ||||
| 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(vector3(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() | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -8,13 +8,14 @@ | |||
| # pylint: disable=invalid-name, E1101 | ||||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import argparse | ||||
| import numpy as np | ||||
| import matplotlib.pyplot as plt | ||||
| from mpl_toolkits.mplot3d import Axes3D | ||||
| 
 | ||||
| import gtsam | ||||
| import matplotlib.pyplot as plt | ||||
| import numpy as np | ||||
| from gtsam.utils import plot | ||||
| from mpl_toolkits.mplot3d import Axes3D | ||||
| 
 | ||||
| 
 | ||||
| def vector6(x, y, z, a, b, c): | ||||
|  | @ -22,50 +23,62 @@ def vector6(x, y, z, a, b, c): | |||
|     return np.array([x, y, z, a, b, c], dtype=float) | ||||
| 
 | ||||
| 
 | ||||
| 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() | ||||
| def main(): | ||||
|     """Main runner.""" | ||||
| 
 | ||||
| g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ | ||||
|     else args.input | ||||
|     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() | ||||
| 
 | ||||
| is3D = True | ||||
| graph, initial = gtsam.readG2o(g2oFile, is3D) | ||||
|     g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ | ||||
|         else args.input | ||||
| 
 | ||||
| # Add Prior on the first key | ||||
| priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, | ||||
|                                                          1e-4, 1e-4, 1e-4)) | ||||
|     is3D = True | ||||
|     graph, initial = gtsam.readG2o(g2oFile, is3D) | ||||
| 
 | ||||
| print("Adding prior to g2o file ") | ||||
| firstKey = initial.keys()[0] | ||||
| graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) | ||||
|     # Add Prior on the first key | ||||
|     priorModel = gtsam.noiseModel.Diagonal.Variances( | ||||
|         vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) | ||||
| 
 | ||||
| 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("Adding prior to g2o file ") | ||||
|     firstKey = initial.keys()[0] | ||||
|     graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) | ||||
| 
 | ||||
| print("initial error = ", graph.error(initial)) | ||||
| print("final error = ", graph.error(result)) | ||||
|     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") | ||||
| 
 | ||||
| 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!") | ||||
|     print("initial error = ", graph.error(initial)) | ||||
|     print("final error = ", graph.error(result)) | ||||
| 
 | ||||
| if args.plot: | ||||
|     resultPoses = gtsam.utilities.allPose3s(result) | ||||
|     for i in range(resultPoses.size()): | ||||
|         plot.plot_pose3(1, resultPoses.atPose3(i)) | ||||
|     plt.show() | ||||
|     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() | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
|  | @ -13,23 +13,29 @@ Author: Luca Carlone, Frank Dellaert (python port) | |||
| 
 | ||||
| from __future__ import print_function | ||||
| 
 | ||||
| import gtsam | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| 
 | ||||
| # Read graph from file | ||||
| g2oFile = gtsam.findExampleDataFile("pose3example.txt") | ||||
| def main(): | ||||
|     """Main runner.""" | ||||
|     # Read graph from file | ||||
|     g2oFile = gtsam.findExampleDataFile("pose3example.txt") | ||||
| 
 | ||||
| is3D = True | ||||
| graph, initial = gtsam.readG2o(g2oFile, is3D) | ||||
|     is3D = True | ||||
|     graph, initial = gtsam.readG2o(g2oFile, is3D) | ||||
| 
 | ||||
| # Add prior on the first key. TODO: assumes first key ios z | ||||
| priorModel = gtsam.noiseModel.Diagonal.Variances( | ||||
|     np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) | ||||
| firstKey = initial.keys()[0] | ||||
| graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) | ||||
|     # Add prior on the first key. TODO: assumes first key ios z | ||||
|     priorModel = gtsam.noiseModel.Diagonal.Variances( | ||||
|         np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) | ||||
|     firstKey = initial.keys()[0] | ||||
|     graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) | ||||
| 
 | ||||
| # Initializing Pose3 - chordal relaxation" | ||||
| initialization = gtsam.InitializePose3.initialize(graph) | ||||
|     # Initializing Pose3 - chordal relaxation | ||||
|     initialization = gtsam.InitializePose3.initialize(graph) | ||||
| 
 | ||||
| print(initialization) | ||||
|     print(initialization) | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     main() | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue