diff --git a/python/gtsam/examples/DogLegOptimizerExample.py b/python/gtsam/examples/DogLegOptimizerExample.py index 776ceedc4..26f4fef84 100644 --- a/python/gtsam/examples/DogLegOptimizerExample.py +++ b/python/gtsam/examples/DogLegOptimizerExample.py @@ -35,17 +35,17 @@ def run(args): graph = gtsam.NonlinearFactorGraph() # Priors - prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) + 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])) + 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) + model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01) graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) params = gtsam.DoglegParams() diff --git a/python/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py index 493a07725..0bc0d1bf3 100644 --- a/python/gtsam/examples/GPSFactorExample.py +++ b/python/gtsam/examples/GPSFactorExample.py @@ -26,8 +26,8 @@ lon0 = -84.30626 h0 = 274 # Create noise models -GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) -PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25) +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() diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 0e01766e7..eec7c5ebd 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -12,14 +12,13 @@ Author: Frank Dellaert, Varun Agrawal from __future__ import print_function +import argparse import math import gtsam import matplotlib.pyplot as plt import numpy as np -from gtsam import symbol_shorthand_B as B -from gtsam import symbol_shorthand_V as V -from gtsam import symbol_shorthand_X as X +from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D @@ -33,24 +32,27 @@ np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): - def __init__(self): + def __init__(self, twist_scenario="sick_twist"): self.velocity = np.array([2, 0, 0]) - self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) - self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # Choose one of these twists to change scenario: - zero_twist = (np.zeros(3), np.zeros(3)) - forward_twist = (np.zeros(3), self.velocity) - loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) - sick_twist = ( - np.array([math.radians(30), -math.radians(30), 0]), self.velocity) + twist_scenarios = dict( + zero_twist=(np.zeros(3), np.zeros(3)), + forward_twist=(np.zeros(3), self.velocity), + loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), + sick_twist=(np.array([math.radians(30), -math.radians(30), 0]), + self.velocity) + ) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) - bias = gtsam.imuBias_ConstantBias(accBias, gyroBias) + bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) dt = 1e-2 - super(ImuFactorExample, self).__init__(sick_twist, bias, dt) + super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], + bias, dt) def addPrior(self, i, graph): state = self.scenario.navState(i) @@ -59,65 +61,73 @@ class ImuFactorExample(PreintegrationExample): graph.push_back(gtsam.PriorFactorVector( V(i), state.velocity(), self.velNoise)) - def run(self): + def run(self, T=12, compute_covariances=False, verbose=True): 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 + num_poses = T # 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)) - - poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) - pose = state_i.pose().compose(poseNoise) - - velocity = state_i.velocity() + np.random.randn(3)*0.1 - - initial.insert(X(i), pose) - initial.insert(V(i), velocity) # simulate the loop i = 0 # state index - actual_state_i = self.scenario.navState(0) + initial_state_i = self.scenario.navState(0) + initial.insert(X(i), initial_state_i.pose()) + initial.insert(V(i), initial_state_i.velocity()) + + # add prior on beginning + self.addPrior(0, graph) + 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) - poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) - - actual_state_i = gtsam.NavState( - actual_state_i.pose().compose(poseNoise), - actual_state_i.velocity() + np.random.randn(3)*0.1) - # 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) + if (k+1) % int(1 / self.dt) == 0: + # Plot every second + self.plotGroundTruthPose(t, scale=1) + plt.title("Ground Truth Trajectory") - # 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) + # create IMU factor every second + factor = gtsam.ImuFactor(X(i), V(i), + X(i + 1), V(i + 1), + BIAS_KEY, pim) graph.push_back(factor) - if True: + + if verbose: print(factor) print(pim.predict(actual_state_i, self.actualBias)) + pim.resetIntegration() + + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) + translationNoise = gtsam.Point3(*np.random.randn(3)*1) + poseNoise = gtsam.Pose3(rotationNoise, translationNoise) + actual_state_i = self.scenario.navState(t + self.dt) + print("Actual state at {0}:\n{1}".format( + t+self.dt, actual_state_i)) + + noisy_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + + initial.insert(X(i+1), noisy_state_i.pose()) + initial.insert(V(i+1), noisy_state_i.velocity()) i += 1 - # add priors on beginning and end - self.addPrior(0, graph) - self.addPrior(num_poses - 1, graph) + # add priors on end + # self.addPrior(num_poses - 1, graph) + + initial.print_("Initial values:") # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() @@ -125,29 +135,46 @@ class ImuFactorExample(PreintegrationExample): 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)))) + result.print_("Optimized values:") + + if compute_covariances: + # 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) + plot_pose3(POSES_FIG+1, pose_i, 1) i += 1 + plt.title("Estimated Trajectory") - gtsam.utils.plot.set_axes_equal(POSES_FIG) + gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - print(result.atimuBias_ConstantBias(BIAS_KEY)) + print("Bias Values", result.atConstantBias(BIAS_KEY)) plt.ioff() plt.show() if __name__ == '__main__': - ImuFactorExample().run() + parser = argparse.ArgumentParser("ImuFactorExample.py") + parser.add_argument("--twist_scenario", + default="sick_twist", + choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) + parser.add_argument("--time", "-T", default=12, + type=int, help="Total time in seconds") + parser.add_argument("--compute_covariances", + default=False, action='store_true') + parser.add_argument("--verbose", default=False, action='store_true') + args = parser.parse_args() + + ImuFactorExample(args.twist_scenario).run( + args.time, args.compute_covariances, args.verbose) diff --git a/python/gtsam/examples/ImuFactorExample2.py b/python/gtsam/examples/ImuFactorISAM2Example.py similarity index 61% rename from python/gtsam/examples/ImuFactorExample2.py rename to python/gtsam/examples/ImuFactorISAM2Example.py index 0d98f08fe..98f6218ea 100644 --- a/python/gtsam/examples/ImuFactorExample2.py +++ b/python/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ iSAM2 example with ImuFactor. -Author: Robert Truax (C++), Frank Dellaert (Python) +Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal """ # pylint: disable=invalid-name, E1101 @@ -9,7 +9,6 @@ from __future__ import print_function import math import gtsam -import gtsam.utils.plot as gtsam_plot import matplotlib.pyplot as plt import numpy as np from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, @@ -17,9 +16,8 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) -from gtsam import symbol_shorthand_B as B -from gtsam import symbol_shorthand_V as V -from gtsam import symbol_shorthand_X as X +from gtsam.symbol_shorthand import B, V, X +from gtsam.utils import plot from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 @@ -28,46 +26,45 @@ def vector3(x, y, z): return np.array([x, y, z], dtype=np.float) -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) - - -# IMU preintegration parameters -# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 n_gravity = vector3(0, 0, -g) -PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) -I = np.eye(3) -PARAMS.setAccelerometerCovariance(I * 0.1) -PARAMS.setGyroscopeCovariance(I * 0.1) -PARAMS.setIntegrationCovariance(I * 0.1) -PARAMS.setUse2ndOrderCoriolis(False) -PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) -BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) -DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), - Point3(0.05, -0.10, 0.20)) + +def preintegration_parameters(): + # IMU preintegration parameters + # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) + I = np.eye(3) + PARAMS.setAccelerometerCovariance(I * 0.1) + PARAMS.setGyroscopeCovariance(I * 0.1) + PARAMS.setIntegrationCovariance(I * 0.1) + PARAMS.setUse2ndOrderCoriolis(False) + PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) + + BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1) + DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), + Point3(0.05, -0.10, 0.20)) + + return PARAMS, BIAS_COVARIANCE, DELTA + + +def get_camera(radius): + up = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) + return camera + + +def get_scenario(radius, pose_0, angular_velocity, delta_t): + """Create the set of ground-truth landmarks and poses""" + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) + + return scenario def IMU_example(): @@ -75,23 +72,17 @@ def IMU_example(): # Start with a camera on x-axis looking at origin radius = 30 - up = Point3(0, 0, 1) - target = Point3(0, 0, 0) - position = Point3(radius, 0, 0) - camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) + camera = get_camera(radius) pose_0 = camera.pose() - # Create the set of ground-truth landmarks and poses - angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step + angular_velocity = math.radians(180) # rad/sec + scenario = get_scenario(radius, pose_0, angular_velocity, delta_t) - angular_velocity_vector = vector3(0, -angular_velocity, 0) - linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = ConstantTwistScenario( - angular_velocity_vector, linear_velocity_vector, pose_0) + PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters() # Create a factor graph - newgraph = NonlinearFactorGraph() + graph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = ISAM2() @@ -102,23 +93,23 @@ def IMU_example(): # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noise = gtsam.noiseModel_Diagonal.Sigmas( + noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) - newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) + graph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = B(0) - biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) - biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(), biasnoise) - newgraph.push_back(biasprior) - initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + graph.push_back(biasprior) + initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) + velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) velprior = PriorFactorVector(V(0), n_velocity, velnoise) - newgraph.push_back(velprior) + graph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) @@ -139,9 +130,9 @@ def IMU_example(): if i % 5 == 0: biasKey += 1 factor = BetweenFactorConstantBias( - biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) - newgraph.add(factor) - initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE) + graph.add(factor) + initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() @@ -152,21 +143,24 @@ def IMU_example(): # Add Imu Factor imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) - newgraph.add(imufac) + graph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution - isam.update(newgraph, initialEstimate) + isam.update(graph, initialEstimate) result = isam.calculateEstimate() - ISAM2_plot(result) + plot.plot_incremental_trajectory(0, result, + start=i, scale=3, time_interval=0.01) # reset - newgraph = NonlinearFactorGraph() + graph = NonlinearFactorGraph() initialEstimate.clear() + plt.show() + if __name__ == '__main__': IMU_example() diff --git a/python/gtsam/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py index e778e3f85..8b519ce9a 100644 --- a/python/gtsam/examples/OdometryExample.py +++ b/python/gtsam/examples/OdometryExample.py @@ -21,8 +21,8 @@ import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot # 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])) +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() diff --git a/python/gtsam/examples/PlanarManipulatorExample.py b/python/gtsam/examples/PlanarManipulatorExample.py index e42ae09d7..9af4f7fcc 100644 --- a/python/gtsam/examples/PlanarManipulatorExample.py +++ b/python/gtsam/examples/PlanarManipulatorExample.py @@ -167,13 +167,11 @@ class ThreeLinkArm(object): axes = fig.gca() sXl1 = Pose2(0, 0, math.radians(90)) - t = sXl1.translation() - p1 = np.array([t.x(), t.y()]) + p1 = sXl1.translation() gtsam_plot.plot_pose2_on_axes(axes, sXl1) def plot_line(p, g, color): - t = g.translation() - q = np.array([t.x(), t.y()]) + q = g.translation() line = np.append(p[np.newaxis], q[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], color) return q diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py index c84f0f834..5ffdf048d 100644 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ b/python/gtsam/examples/PlanarSLAMExample.py @@ -13,15 +13,15 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) from __future__ import print_function -import gtsam import numpy as np -from gtsam import symbol_shorthand_L as L -from gtsam import symbol_shorthand_X as X + +import gtsam +from gtsam.symbol_shorthand import X, L # 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])) +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() diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py index 680f2209f..2569f0953 100644 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ b/python/gtsam/examples/Pose2SLAMExample.py @@ -27,10 +27,9 @@ def vector3(x, y, z): """Create 3d double numpy array.""" return np.array([x, y, z], dtype=np.float) - # 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)) +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() diff --git a/python/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py index 09114370d..b2ba9c5bc 100644 --- a/python/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose2SLAMExample_g2o.py @@ -53,7 +53,7 @@ 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)) +priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) params = gtsam.GaussNewtonParams() @@ -82,7 +82,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.utilities_extractPose2(result) + resultPoses = gtsam.utilities.extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show() diff --git a/python/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py index 3c1a54f7b..82b3bda98 100644 --- a/python/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose3SLAMExample_g2o.py @@ -39,11 +39,11 @@ 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, +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().at(0) +firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() @@ -65,7 +65,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.utilities_allPose3s(result) + resultPoses = gtsam.utilities.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show() diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index 02c696905..2b2c5f991 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -24,9 +24,9 @@ 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( +priorModel = gtsam.noiseModel.Diagonal.Variances( np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) -firstKey = initial.keys().at(0) +firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) # Initializing Pose3 - chordal relaxation" diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 76520b355..e308bc933 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements import math +import gtsam import matplotlib.pyplot as plt import numpy as np -from mpl_toolkits.mplot3d import Axes3D - -import gtsam from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 @@ -68,60 +67,62 @@ class PreintegrationExample(object): else: accBias = np.array([0, 0.1, 0]) gyroBias = np.array([0, 0, 0]) - self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias) + self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias) self.runner = gtsam.ScenarioRunner( self.scenario, self.params, self.dt, self.actualBias) + fig, self.axes = plt.subplots(4, 3) + fig.set_tight_layout(True) + def plotImu(self, t, measuredOmega, measuredAcc): plt.figure(IMU_FIG) # plot angular velocity omega_b = self.scenario.omega_b(t) for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 1) - plt.scatter(t, omega_b[i], color='k', marker='.') - plt.scatter(t, measuredOmega[i], color=color, marker='.') - plt.xlabel('angular velocity ' + label) + ax = self.axes[0][i] + ax.scatter(t, omega_b[i], color='k', marker='.') + ax.scatter(t, measuredOmega[i], color=color, marker='.') + ax.set_xlabel('angular velocity ' + label) # plot acceleration in nav acceleration_n = self.scenario.acceleration_n(t) for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 4) - plt.scatter(t, acceleration_n[i], color=color, marker='.') - plt.xlabel('acceleration in nav ' + label) + ax = self.axes[1][i] + ax.scatter(t, acceleration_n[i], color=color, marker='.') + ax.set_xlabel('acceleration in nav ' + label) # plot acceleration in body acceleration_b = self.scenario.acceleration_b(t) for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 7) - plt.scatter(t, acceleration_b[i], color=color, marker='.') - plt.xlabel('acceleration in body ' + label) + ax = self.axes[2][i] + ax.scatter(t, acceleration_b[i], color=color, marker='.') + ax.set_xlabel('acceleration in body ' + label) # plot actual specific force, as well as corrupted actual = self.runner.actualSpecificForce(t) for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 10) - plt.scatter(t, actual[i], color='k', marker='.') - plt.scatter(t, measuredAcc[i], color=color, marker='.') - plt.xlabel('specific force ' + label) + ax = self.axes[3][i] + ax.scatter(t, actual[i], color='k', marker='.') + ax.scatter(t, measuredAcc[i], color=color, marker='.') + ax.set_xlabel('specific force ' + label) - def plotGroundTruthPose(self, t): + 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, 0.3) t = actualPose.translation() - self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) + 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(0.01) + plt.pause(time_interval) - def run(self): + def run(self, T=12): # simulate the loop - T = 12 for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 99bce00e2..e998e4dcd 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -1,17 +1,12 @@ -These examples are almost identical to the old handwritten python wrapper -examples. However, there are just some slight name changes, for example -`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc... -Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing. - # Porting Progress | C++ Example Name | Ported | |-------------------------------------------------------|--------| | CameraResectioning | | | CreateSFMExampleData | | -| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython | -| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython | -| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython | +| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | | ImuFactorExample2 | X | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | @@ -25,7 +20,7 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan | Pose2SLAMExample_g2o | X | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | | Pose3SLAMExample_changeKeys | | @@ -47,11 +42,11 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan | StereoVOExample | | | StereoVOExample_large | | | TimeTBB | | -| UGM_chain | discrete functionality not exposed | -| UGM_small | discrete functionality not exposed | +| UGM_chain | discrete functionality not yet exposed | +| UGM_small | discrete functionality not yet exposed | | VisualISAM2Example | X | | VisualISAMExample | X | Extra Examples (with no C++ equivalent) - PlanarManipulatorExample -- SFMData +- SFMData \ No newline at end of file diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index 68053f862..f0c4c82ba 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -13,14 +13,15 @@ from __future__ import print_function import gtsam import matplotlib.pyplot as plt import numpy as np -from gtsam import symbol_shorthand_L as L -from gtsam import symbol_shorthand_X as X +from gtsam import symbol_shorthand +L = symbol_shorthand.L +X = symbol_shorthand.X + from gtsam.examples import SFMdata -from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, +from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, - Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - PinholeCameraCal3_S2, Values) + Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) from gtsam.utils import plot @@ -56,7 +57,7 @@ def main(): K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model - measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks points = SFMdata.createPoints() @@ -69,7 +70,7 @@ def main(): # Add a prior on pose x1. This indirectly specifies where the origin is. # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z - pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) @@ -85,7 +86,7 @@ def main(): # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. - point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') @@ -97,7 +98,7 @@ def main(): transformed_pose = pose.retract(0.1*np.random.randn(6,1)) initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) + transformed_point = point + 0.1*np.random.randn(3) initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index c586f7e52..6ac9c5726 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/python/gtsam/examples/SFMdata.py @@ -33,7 +33,8 @@ def createPoses(K): poses = [] for theta in angles: position = gtsam.Point3(radius*np.cos(theta), - radius*np.sin(theta), height) + radius*np.sin(theta), + height) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) poses.append(camera.pose()) return poses diff --git a/python/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py index 4e82d3778..0fef261f8 100644 --- a/python/gtsam/examples/SimpleRotation.py +++ b/python/gtsam/examples/SimpleRotation.py @@ -10,10 +10,9 @@ This example will perform a relatively trivial optimization on a single variable with a single factor. """ -import gtsam import numpy as np -from gtsam import symbol_shorthand_X as X - +import gtsam +from gtsam.symbol_shorthand import X def main(): """ @@ -33,7 +32,7 @@ def main(): """ prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) prior.print_('goal angle') - model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) + model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 49e6ca95c..bacf510ec 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -17,8 +17,7 @@ import gtsam import gtsam.utils.plot as gtsam_plot import matplotlib.pyplot as plt import numpy as np -from gtsam import symbol_shorthand_L as L -from gtsam import symbol_shorthand_X as X +from gtsam.symbol_shorthand import L, X from gtsam.examples import SFMdata from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 @@ -64,7 +63,7 @@ def visual_ISAM2_example(): K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model - measurement_noise = gtsam.noiseModel_Isotropic.Sigma( + measurement_noise = gtsam.noiseModel.Isotropic.Sigma( 2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks @@ -110,12 +109,12 @@ def visual_ISAM2_example(): # at least twice before adding it to iSAM. if i == 0: # Add a prior on pose x0 - pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array( + pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array( [0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise)) # Add a prior on landmark l0 - point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) graph.push_back(gtsam.PriorFactorPoint3( L(0), points[0], point_noise)) # add directly to graph @@ -123,7 +122,7 @@ def visual_ISAM2_example(): # Intentionally initialize the variables off from the ground truth for j, point in enumerate(points): initial_estimate.insert(L(j), gtsam.Point3( - point.x()-0.25, point.y()+0.20, point.z()+0.15)) + point[0]-0.25, point[1]+0.20, point[2]+0.15)) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 5cc37867b..f99d3f3e6 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -15,13 +15,11 @@ from __future__ import print_function import numpy as np import gtsam from gtsam.examples import SFMdata -from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, - NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - PinholeCameraCal3_S2, Values) -from gtsam import symbol_shorthand_L as L -from gtsam import symbol_shorthand_X as X - +from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, + NonlinearFactorGraph, NonlinearISAM, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + PinholeCameraCal3_S2, Values, Point3) +from gtsam.symbol_shorthand import X, L def main(): """ @@ -34,7 +32,8 @@ def main(): K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model - camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + camera_noise = gtsam.noiseModel.Isotropic.Sigma( + 2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks points = SFMdata.createPoints() @@ -55,11 +54,13 @@ def main(): # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K) + factor = GenericProjectionFactorCal3_S2( + measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth - noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) + noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), + t=Point3(0.05, -0.10, 0.20)) initial_xi = pose.compose(noise) # Add an initial guess for the current pose @@ -71,12 +72,13 @@ def main(): # adding it to iSAM. if i == 0: # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z - pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + pose_noise = gtsam.noiseModel.Diagonal.Sigmas( + np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Add a prior on landmark l0 - point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) @@ -84,8 +86,8 @@ def main(): noise = np.array([-0.25, 0.20, 0.15]) for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth - initial_lj = points[j].vector() + noise - initial_estimate.insert(L(j), Point3(initial_lj)) + initial_lj = points[j] + noise + initial_estimate.insert(L(j), initial_lj) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/python/gtsam/tests/__init__.py b/python/gtsam/tests/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam/tests/testScenarioRunner.py b/python/gtsam/tests/testScenarioRunner.py index 97a97b0ec..2af16a794 100644 --- a/python/gtsam/tests/testScenarioRunner.py +++ b/python/gtsam/tests/testScenarioRunner.py @@ -32,7 +32,7 @@ class TestScenarioRunner(GtsamTestCase): dt = 0.1 params = gtsam.PreintegrationParams.MakeSharedU(self.g) - bias = gtsam.imuBias_ConstantBias() + bias = gtsam.imuBias.ConstantBias() runner = gtsam.ScenarioRunner( scenario, params, dt, bias) diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index 983825d8b..a29b0f263 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -16,7 +16,7 @@ import unittest import gtsam import numpy as np -from gtsam import symbol_shorthand_X as X +from gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase @@ -28,8 +28,8 @@ def create_graph(): x1 = X(1) x2 = X(2) - BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) - PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) + BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) diff --git a/python/gtsam/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py index 04433492b..6e049ed47 100644 --- a/python/gtsam/tests/test_JacobianFactor.py +++ b/python/gtsam/tests/test_JacobianFactor.py @@ -48,7 +48,7 @@ class TestJacobianFactor(GtsamTestCase): # the RHS b2 = np.array([-1., 1.5, 2., -1.]) sigmas = np.array([1., 1., 1., 1.]) - model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas) + model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas) combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) # eliminate the first variable (x2) in the combined factor, destructive @@ -66,7 +66,7 @@ class TestJacobianFactor(GtsamTestCase): [+0.00, -8.94427]]) d = np.array([2.23607, -1.56525]) expectedCG = gtsam.GaussianConditional( - x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) + x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2)) # check if the result matches self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) @@ -82,7 +82,7 @@ class TestJacobianFactor(GtsamTestCase): # the RHS b1 = np.array([0.0, 0.894427]) - model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.])) + model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.])) expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor diff --git a/python/gtsam/tests/test_KalmanFilter.py b/python/gtsam/tests/test_KalmanFilter.py index 94c41df72..48a91b96c 100644 --- a/python/gtsam/tests/test_KalmanFilter.py +++ b/python/gtsam/tests/test_KalmanFilter.py @@ -22,13 +22,13 @@ class TestKalmanFilter(GtsamTestCase): F = np.eye(2) B = np.eye(2) u = np.array([1.0, 0.0]) - modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + modelQ = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) Q = 0.01 * np.eye(2) H = np.eye(2) z1 = np.array([1.0, 0.0]) z2 = np.array([2.0, 0.0]) z3 = np.array([3.0, 0.0]) - modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) R = 0.01 * np.eye(2) # Create the set of expected output TestValues diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index 6976decc1..a315a506c 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -18,7 +18,7 @@ import numpy as np from gtsam.utils.test_case import GtsamTestCase KEY = 0 -MODEL = gtsam.noiseModel_Unit.Create(3) +MODEL = gtsam.noiseModel.Unit.Create(3) def find_Karcher_mean_Rot3(rotations): @@ -59,8 +59,8 @@ class TestKarcherMean(GtsamTestCase): R12 = R.compose(R.compose(R)) graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) keys = gtsam.KeyVector() - keys.push_back(1) - keys.push_back(2) + keys.append(1) + keys.append(2) graph.add(gtsam.KarcherMeanFactorRot3(keys)) initial = gtsam.Values() diff --git a/python/gtsam/tests/test_LocalizationExample.py b/python/gtsam/tests/test_LocalizationExample.py index 6ce65f087..8ae3583f0 100644 --- a/python/gtsam/tests/test_LocalizationExample.py +++ b/python/gtsam/tests/test_LocalizationExample.py @@ -26,7 +26,7 @@ class TestLocalizationExample(GtsamTestCase): # 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( + 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)) @@ -37,7 +37,7 @@ class TestLocalizationExample(GtsamTestCase): 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.])) + 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)) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 985dc30a2..e9234a43b 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -15,11 +15,11 @@ from __future__ import print_function import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, +from gtsam import (DoglegOptimizer, DoglegParams, + DummyPreconditionerParameters, GaussNewtonOptimizer, GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, PCGSolverParameters, - DummyPreconditionerParameters, NonlinearFactorGraph, Ordering, - Point2, PriorFactorPoint2, Values) + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -30,7 +30,7 @@ class TestScenario(GtsamTestCase): def test_optimize(self): """Do trivial test with three optimizer variants.""" fg = NonlinearFactorGraph() - model = gtsam.noiseModel_Unit.Create(2) + model = gtsam.noiseModel.Unit.Create(2) fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) # test error at minimum diff --git a/python/gtsam/tests/test_OdometryExample.py b/python/gtsam/tests/test_OdometryExample.py index c8ea95588..72e532f20 100644 --- a/python/gtsam/tests/test_OdometryExample.py +++ b/python/gtsam/tests/test_OdometryExample.py @@ -25,7 +25,7 @@ class TestOdometryExample(GtsamTestCase): # 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( + 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)) @@ -33,7 +33,7 @@ class TestOdometryExample(GtsamTestCase): # 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( + 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)) diff --git a/python/gtsam/tests/test_PlanarSLAMExample.py b/python/gtsam/tests/test_PlanarSLAMExample.py index ae813d35c..8cb3ad2ac 100644 --- a/python/gtsam/tests/test_PlanarSLAMExample.py +++ b/python/gtsam/tests/test_PlanarSLAMExample.py @@ -32,13 +32,13 @@ class TestPlanarSLAM(GtsamTestCase): # 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])) + 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])) + 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( @@ -49,7 +49,7 @@ class TestPlanarSLAM(GtsamTestCase): 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])) + 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 diff --git a/python/gtsam/tests/test_Pose2SLAMExample.py b/python/gtsam/tests/test_Pose2SLAMExample.py index a79b6b18c..e47b9fbff 100644 --- a/python/gtsam/tests/test_Pose2SLAMExample.py +++ b/python/gtsam/tests/test_Pose2SLAMExample.py @@ -32,13 +32,13 @@ class TestPose2SLAMExample(GtsamTestCase): # 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])) + 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])) + 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( @@ -49,7 +49,7 @@ class TestPose2SLAMExample(GtsamTestCase): 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])) + 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 diff --git a/python/gtsam/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py index 1e9eaac67..fce171b55 100644 --- a/python/gtsam/tests/test_Pose3SLAMExample.py +++ b/python/gtsam/tests/test_Pose3SLAMExample.py @@ -30,7 +30,7 @@ class TestPose3SLAMExample(GtsamTestCase): fg = gtsam.NonlinearFactorGraph() fg.add(gtsam.NonlinearEqualityPose3(0, p0)) delta = p0.between(p1) - covariance = gtsam.noiseModel_Diagonal.Sigmas( + covariance = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) diff --git a/python/gtsam/tests/test_PriorFactor.py b/python/gtsam/tests/test_PriorFactor.py index 66207b800..0582cf5d7 100644 --- a/python/gtsam/tests/test_PriorFactor.py +++ b/python/gtsam/tests/test_PriorFactor.py @@ -23,14 +23,14 @@ class TestPriorFactor(GtsamTestCase): key = 5 priorPose3 = gtsam.Pose3() - model = gtsam.noiseModel_Unit.Create(6) + model = gtsam.noiseModel.Unit.Create(6) factor = gtsam.PriorFactorPose3(key, priorPose3, model) values.insert(key, priorPose3) self.assertEqual(factor.error(values), 0) key = 3 priorVector = np.array([0., 0., 0.]) - model = gtsam.noiseModel_Unit.Create(3) + model = gtsam.noiseModel.Unit.Create(3) factor = gtsam.PriorFactorVector(key, priorVector, model) values.insert(key, priorVector) self.assertEqual(factor.error(values), 0) @@ -45,14 +45,14 @@ class TestPriorFactor(GtsamTestCase): # define and add Pose3 prior key = 5 priorPose3 = gtsam.Pose3() - model = gtsam.noiseModel_Unit.Create(6) + model = gtsam.noiseModel.Unit.Create(6) graph.addPriorPose3(key, priorPose3, model) self.assertEqual(graph.size(), 1) # define and add Vector prior key = 3 priorVector = np.array([0., 0., 0.]) - model = gtsam.noiseModel_Unit.Create(3) + model = gtsam.noiseModel.Unit.Create(3) graph.addPriorVector(key, priorVector, model) self.assertEqual(graph.size(), 2) diff --git a/python/gtsam/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py index e8fa46186..47a3cbe3e 100644 --- a/python/gtsam/tests/test_SFMExample.py +++ b/python/gtsam/tests/test_SFMExample.py @@ -15,8 +15,9 @@ import numpy as np import gtsam import gtsam.utils.visual_data_generator as generator from gtsam import symbol +from gtsam.noiseModel import Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase - +from gtsam.symbol_shorthand import X, P class TestSFMExample(GtsamTestCase): @@ -34,29 +35,29 @@ class TestSFMExample(GtsamTestCase): graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements - measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + 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, - symbol(ord('x'), i), symbol(ord('p'), j), data.K)) + X(i), P(j), data.K)) - posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) - graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0), + posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) + graph.add(gtsam.PriorFactorPose3(X(0), truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma) - graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0), + 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(symbol(ord('x'), i), pose_i) + initialEstimate.insert(X(i), pose_i) for j in range(len(truth.points)): point_j = truth.points[j] - initialEstimate.insert(symbol(ord('p'), j), point_j) + initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) @@ -66,16 +67,16 @@ class TestSFMExample(GtsamTestCase): # Marginalization marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(symbol(ord('p'), 0)) - marginals.marginalCovariance(symbol(ord('x'), 0)) + 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(symbol(ord('x'), i)) + 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(symbol(ord('p'), j)) + point_j = result.atPoint3(P(j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_Scenario.py b/python/gtsam/tests/test_Scenario.py index 09601fba5..fc5965829 100644 --- a/python/gtsam/tests/test_Scenario.py +++ b/python/gtsam/tests/test_Scenario.py @@ -43,8 +43,11 @@ class TestScenario(GtsamTestCase): # R = v/w, so test if loop crests at 2*R R = v / w T30 = scenario.pose(T) + xyz = T30.rotation().xyz() + if xyz[0] < 0: + xyz = -xyz np.testing.assert_almost_equal( - np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) + np.array([math.pi, 0, math.pi]), xyz) self.gtsamAssertEquals(gtsam.Point3( 0, 0, 2.0 * R), T30.translation(), 1e-9) diff --git a/python/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py index a3654a5f1..efdfec561 100644 --- a/python/gtsam/tests/test_SimpleCamera.py +++ b/python/gtsam/tests/test_SimpleCamera.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -PinholeCameraCal3_S2 unit tests. +SimpleCamera unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import math @@ -14,7 +14,7 @@ import unittest import numpy as np import gtsam -from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) @@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) - camera = PinholeCameraCal3_S2(pose1, K) + camera = SimpleCamera(pose1, K) self.gtsamAssertEquals(camera.calibration(), K, 1e-9) self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction pose2 = Pose2(0.4,0.3,math.pi/2.0) - camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1) + camera = SimpleCamera.Level(K, pose2, 0.1) # expected x = Point3(1,0,0) diff --git a/python/gtsam/tests/test_StereoVOExample.py b/python/gtsam/tests/test_StereoVOExample.py index 3f5f57522..cefc08aab 100644 --- a/python/gtsam/tests/test_StereoVOExample.py +++ b/python/gtsam/tests/test_StereoVOExample.py @@ -28,11 +28,11 @@ class TestStereoVOExample(GtsamTestCase): # - No noise on measurements ## Create keys for variables - x1 = symbol(ord('x'),1) - x2 = symbol(ord('x'),2) - l1 = symbol(ord('l'),1) - l2 = symbol(ord('l'),2) - l3 = symbol(ord('l'),3) + x1 = symbol('x',1) + x2 = symbol('x',2) + l1 = symbol('l',1) + l2 = symbol('l',2) + l3 = symbol('l',3) ## Create graph container and add factors to it graph = gtsam.NonlinearFactorGraph() @@ -44,7 +44,7 @@ class TestStereoVOExample(GtsamTestCase): ## Create realistic calibration and measurement noise model # format: fx fy skew cx cy baseline K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) - stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) + stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) ## Add measurements # pose 1 diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py new file mode 100644 index 000000000..b43ad9b57 --- /dev/null +++ b/python/gtsam/tests/test_Triangulation.py @@ -0,0 +1,80 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Test Triangulation +Author: Frank Dellaert & Fan Jiang (Python) +""" +import unittest + +import numpy as np + +import gtsam as g +from gtsam.utils.test_case import GtsamTestCase +from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ + PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3 + +class TestVisualISAMExample(GtsamTestCase): + def test_TriangulationExample(self): + # Some common constants + sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) + + # Looking along X-axis, 1 meter above ground plane (x-y) + upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) + pose1 = Pose3(upright, Point3(0, 0, 1)) + camera1 = PinholeCameraCal3_S2(pose1, sharedCal) + + # create second camera 1 meter to the right of first camera + pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + camera2 = PinholeCameraCal3_S2(pose2, sharedCal) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(landmark) + z2 = camera2.project(landmark) + + # twoPoses + poses = Pose3Vector() + measurements = Point2Vector() + + poses.append(pose1) + poses.append(pose2) + measurements.append(z1) + measurements.append(z2) + + optimize = True + rank_tol = 1e-9 + + triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + + # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements = Point2Vector() + measurements.append(z1 - np.array([0.1, 0.5])) + measurements.append(z2 - np.array([-0.2, 0.3])) + + triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2) + # + # # two Poses with Bundler Calibration + # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) + # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) + # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) + # + # z1 = camera1.project(landmark) + # z2 = camera2.project(landmark) + # + # measurements = Point2Vector() + # measurements.append(z1) + # measurements.append(z2) + # + # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) + # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_Values.py b/python/gtsam/tests/test_Values.py index 20634a21c..dddd11c40 100644 --- a/python/gtsam/tests/test_Values.py +++ b/python/gtsam/tests/test_Values.py @@ -15,8 +15,7 @@ import numpy as np import gtsam from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, - Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, - imuBias_ConstantBias) + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias) from gtsam.utils.test_case import GtsamTestCase @@ -37,7 +36,7 @@ class TestValues(GtsamTestCase): values.insert(7, Cal3DS2()) values.insert(8, Cal3Bundler()) values.insert(9, E) - values.insert(10, imuBias_ConstantBias()) + values.insert(10, imuBias.ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision @@ -70,8 +69,8 @@ class TestValues(GtsamTestCase): self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) - self.gtsamAssertEquals(values.atimuBias_ConstantBias( - 10), imuBias_ConstantBias(), tol) + self.gtsamAssertEquals(values.atConstantBias( + 10), imuBias.ConstantBias(), tol) # special cases for Vector and Matrix: actualVector = values.atVector(11) diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index 99d7e6160..6eb05eeee 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -46,11 +46,11 @@ class TestVisualISAMExample(GtsamTestCase): isam, result = visual_isam.step(data, isam, result, truth, currentPose) for i in range(len(truth.cameras)): - pose_i = result.atPose3(symbol(ord('x'), i)) + pose_i = result.atPose3(symbol('x', i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): - point_j = result.atPoint3(symbol(ord('l'), j)) + point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_dataset.py b/python/gtsam/tests/test_dataset.py index 60fb9450d..87fc2ad54 100644 --- a/python/gtsam/tests/test_dataset.py +++ b/python/gtsam/tests/test_dataset.py @@ -15,7 +15,7 @@ from __future__ import print_function import unittest import gtsam -from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam import BetweenFactorPose3 from gtsam.utils.test_case import GtsamTestCase @@ -37,8 +37,8 @@ class TestDataset(GtsamTestCase): def test_parse3Dfactors(self): """Test parsing into data structure.""" factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) - self.assertEqual(factors.size(), 6) - self.assertIsInstance(factors.at(0), BetweenFactorPose3) + self.assertEqual(len(factors), 6) + self.assertIsInstance(factors[0], BetweenFactorPose3) if __name__ == '__main__': diff --git a/python/gtsam/tests/test_initialize_pose3.py b/python/gtsam/tests/test_initialize_pose3.py index 3aa7e3470..6d7f66653 100644 --- a/python/gtsam/tests/test_initialize_pose3.py +++ b/python/gtsam/tests/test_initialize_pose3.py @@ -24,7 +24,7 @@ class TestValues(GtsamTestCase): def setUp(self): - model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) # We consider a small graph: # symbolic FG @@ -64,9 +64,8 @@ class TestValues(GtsamTestCase): def test_orientations(self): pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) - initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) - + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) @@ -77,7 +76,7 @@ class TestValues(GtsamTestCase): g2oFile = gtsam.findExampleDataFile("pose3example-grid") is3D = True inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) - priorModel = gtsam.noiseModel_Unit.Create(6) + priorModel = gtsam.noiseModel.Unit.Create(6) inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) initial = gtsam.InitializePose3.initialize(inputGraph) diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py index 2560a72a2..47eb32e7b 100644 --- a/python/gtsam/tests/test_logging_optimizer.py +++ b/python/gtsam/tests/test_logging_optimizer.py @@ -21,7 +21,7 @@ from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.logging_optimizer import gtsam_optimize KEY = 0 -MODEL = gtsam.noiseModel_Unit.Create(3) +MODEL = gtsam.noiseModel.Unit.Create(3) class TestOptimizeComet(GtsamTestCase): diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index 7012548f4..e1def9427 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -1,9 +1,10 @@ import gtsam +import math import numpy as np -from math import pi, cos, sin +from math import pi -def circlePose3(numPoses=8, radius=1.0, symbolChar=0): +def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential @@ -18,10 +19,6 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0): Vehicle at p0 is looking towards y axis (X-axis points towards world y) """ - # Force symbolChar to be a single character - if type(symbolChar) is str: - symbolChar = ord(symbolChar[0]) - values = gtsam.Values() theta = 0.0 dtheta = 2 * pi / numPoses @@ -29,7 +26,7 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0): np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) - gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0) + gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0) oRi = gtsam.Rot3.Yaw( -theta) # negative yaw goes counterclockwise, with Z down ! gTi = gtsam.Pose3(gRo.compose(oRi), gti) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 77031eec4..0267da8c3 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() - origin = np.array([t.x(), t.y()]) + origin = t # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -169,9 +169,9 @@ def plot_point3_on_axes(axes, point, linespec, P=None): linespec (string): String representing formatting options for Matplotlib. P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ - axes.plot([point.x()], [point.y()], [point.z()], linespec) + axes.plot([point[0]], [point[1]], [point[2]], linespec) if P is not None: - plot_covariance_ellipse_3d(axes, point.vector(), P) + plot_covariance_ellipse_3d(axes, point, P) def plot_point3(fignum, point, linespec, P=None, @@ -221,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, keys = values.keys() # Plot points and covariance matrices - for i in range(keys.size()): + for key in keys: try: - key = keys.at(i) point = values.atPoint3(key) if marginals is not None: covariance = marginals.marginalCovariance(key) @@ -253,7 +252,7 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global - origin = pose.translation().vector() + origin = pose.translation() # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -319,19 +318,17 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, title (string): The title of the plot. axis_labels (iterable[string]): List of axis labels to set. """ - pose3Values = gtsam.utilities_allPose3s(values) + pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) - lastIndex = None + lastKey = None - for i in range(keys.size()): - key = keys.at(i) + for key in keys: try: pose = pose3Values.atPose3(key) except: print("Warning: no Pose3 at key: {0}".format(key)) - if lastIndex is not None: - lastKey = keys.at(lastIndex) + if lastKey is not None: try: lastPose = pose3Values.atPose3(lastKey) except: @@ -346,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, fig = plot_pose3(fignum, lastPose, P=covariance, axis_length=scale, axis_labels=axis_labels) - lastIndex = i + lastKey = key # Draw final pose - if lastIndex is not None: - lastKey = keys.at(lastIndex) + if lastKey is not None: try: lastPose = pose3Values.atPose3(lastKey) if marginals: @@ -366,3 +362,40 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, fig.suptitle(title) fig.canvas.set_window_title(title.lower()) + + +def plot_incremental_trajectory(fignum, values, start=0, + scale=1, marginals=None, + time_interval=0.0): + """ + Incrementally plot a complete 3D trajectory using poses in `values`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dict containing the poses. + start (int): Starting index to start plotting from. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + time_interval (float): Time in seconds to pause between each rendering. + Used to create animation effect. + """ + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + + pose3Values = gtsam.utilities.allPose3s(values) + keys = gtsam.KeyVector(pose3Values.keys()) + + for key in keys[start:]: + if values.exists(key): + pose_i = values.atPose3(key) + plot_pose3(fignum, pose_i, scale) + + # Update the plot space to encompass all plotted points + axes.autoscale() + + # Set the 3 axes equal + set_axes_equal(fignum) + + # Pause for a fixed amount of seconds + plt.pause(time_interval) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 7df1e6ee9..3effd7f65 100644 --- a/python/gtsam/utils/test_case.py +++ b/python/gtsam/utils/test_case.py @@ -21,7 +21,11 @@ class GtsamTestCase(unittest.TestCase): Keyword Arguments: tol {float} -- tolerance passed to 'equals', default 1e-9 """ - equal = actual.equals(expected, tol) + import numpy + if isinstance(expected, numpy.ndarray): + equal = numpy.allclose(actual, expected, atol=tol) + else: + equal = actual.equals(expected, tol) if not equal: raise self.failureException( "Values are not equal:\n{}!={}".format(actual, expected)) diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 5ce72fe68..32ccbc8fa 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,9 +1,10 @@ from __future__ import print_function import numpy as np - +import math +from math import pi import gtsam -from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3 +from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 class Options: @@ -30,8 +31,8 @@ class GroundTruth: def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.cameras = [gtsam.Pose3()] * nrCameras - self.points = [gtsam.Point3(0, 0, 0)] * nrPoints + self.cameras = [Pose3()] * nrCameras + self.points = [Point3(0, 0, 0)] * nrPoints def print_(self, s=""): print(s) @@ -55,20 +56,20 @@ class Data: def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] + self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] self.odometry = [Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() - self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas( + self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) - # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + # noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) - self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) - self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) - self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) + self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) def generate_data(options): @@ -84,8 +85,8 @@ def generate_data(options): if options.triangle: # Create a triangle target, just 3 points on a plane r = 10 for j in range(len(truth.points)): - theta = j * 2 * np.pi / nrPoints - truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0) + theta = j * 2 * pi / nrPoints + truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ Point3(10, 10, 10), Point3(-10, 10, 10), @@ -98,12 +99,12 @@ def generate_data(options): height = 10 r = 40 for i in range(options.nrCameras): - theta = i * 2 * np.pi / options.nrCameras - t = gtsam.Point3(r * np.cos(theta), r * np.sin(theta), height) - truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, - gtsam.Point3(0, 0, 0), - gtsam.Point3(0, 0, 1), - truth.K) + theta = i * 2 * pi / options.nrCameras + t = Point3(r * math.cos(theta), r * math.sin(theta), height) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + Point3(0, 0, 0), + Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py index b0ebe68c3..a8fed4b23 100644 --- a/python/gtsam/utils/visual_isam.py +++ b/python/gtsam/utils/visual_isam.py @@ -25,7 +25,7 @@ def initialize(data, truth, options): newFactors = gtsam.NonlinearFactorGraph() initialEstimates = gtsam.Values() for i in range(2): - ii = symbol(ord('x'), i) + ii = symbol('x', i) if i == 0: if options.hardConstraint: # add hard constraint newFactors.add( @@ -41,10 +41,10 @@ def initialize(data, truth, options): # Add visual measurement factors from two first poses and initialize # observed landmarks for i in range(2): - ii = symbol(ord('x'), i) + ii = symbol('x', i) for k in range(len(data.Z[i])): j = data.J[i][k] - jj = symbol(ord('l'), j) + jj = symbol('l', j) newFactors.add( gtsam.GenericProjectionFactorCal3_S2(data.Z[i][ k], data.noiseModels.measurement, ii, jj, data.K)) @@ -59,8 +59,8 @@ def initialize(data, truth, options): # Add odometry between frames 0 and 1 newFactors.add( gtsam.BetweenFactorPose3( - symbol(ord('x'), 0), - symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry)) + symbol('x', 0), + symbol('x', 1), data.odometry[1], data.noiseModels.odometry)) # Update ISAM if options.batchInitialization: # Do a full optimize for first two poses @@ -98,28 +98,28 @@ def step(data, isam, result, truth, currPoseIndex): odometry = data.odometry[prevPoseIndex] newFactors.add( gtsam.BetweenFactorPose3( - symbol(ord('x'), prevPoseIndex), - symbol(ord('x'), currPoseIndex), odometry, + symbol('x', prevPoseIndex), + symbol('x', currPoseIndex), odometry, data.noiseModels.odometry)) # Add visual measurement factors and initializations as necessary for k in range(len(data.Z[currPoseIndex])): zij = data.Z[currPoseIndex][k] j = data.J[currPoseIndex][k] - jj = symbol(ord('l'), j) + jj = symbol('l', j) newFactors.add( gtsam.GenericProjectionFactorCal3_S2( zij, data.noiseModels.measurement, - symbol(ord('x'), currPoseIndex), jj, data.K)) + symbol('x', currPoseIndex), jj, data.K)) # TODO: initialize with something other than truth if not result.exists(jj) and not initialEstimates.exists(jj): lmInit = truth.points[j] initialEstimates.insert(jj, lmInit) # Initial estimates for the new pose. - prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex)) + prevPose = result.atPose3(symbol('x', prevPoseIndex)) initialEstimates.insert( - symbol(ord('x'), currPoseIndex), prevPose.compose(odometry)) + symbol('x', currPoseIndex), prevPose.compose(odometry)) # Update ISAM # figure(1)tic diff --git a/python/gtsam_unstable/examples/FixedLagSmootherExample.py b/python/gtsam_unstable/examples/FixedLagSmootherExample.py index 786701e0f..7d2cea8ae 100644 --- a/python/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/python/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -16,16 +16,6 @@ import numpy as np import gtsam import gtsam_unstable - -def _timestamp_key_value(key, value): - """ - - """ - return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( - key, value - ) - - def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry @@ -45,21 +35,21 @@ def BatchFixedLagSmootherExample(): # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) - prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) X1 = 0 new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + new_timestamps.insert((X1, 0.0)) delta_time = 0.25 time = 0.25 while time <= 3.0: - previous_key = 1000 * (time - delta_time) - current_key = 1000 * time + previous_key = int(1000 * (time - delta_time)) + current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert(_timestamp_key_value(current_key, time)) + new_timestamps.insert((current_key, time)) # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] @@ -69,14 +59,14 @@ def BatchFixedLagSmootherExample(): # Add odometry factors from two different sources with different error # stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_1, odometry_noise_1 )) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.05, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_2, odometry_noise_2 diff --git a/python/gtsam_unstable/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py index 6ba06f0f2..59f008a05 100644 --- a/python/gtsam_unstable/examples/TimeOfArrivalExample.py +++ b/python/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -12,7 +12,7 @@ Author: Frank Dellaert # pylint: disable=invalid-name, no-name-in-module from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic) + NonlinearFactorGraph, Point3, Values, noiseModel) from gtsam_unstable import Event, TimeOfArrival, TOAFactor # units @@ -64,7 +64,7 @@ def create_graph(microphones, simulatedTOA): graph = NonlinearFactorGraph() # Create a noise model for the TOA error - model = noiseModel_Isotropic.Sigma(1, 0.5 * MS) + model = noiseModel.Isotropic.Sigma(1, 0.5 * MS) K = len(microphones) key = 0 diff --git a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 8d3af311f..c1ccd1ea1 100644 --- a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -16,13 +16,6 @@ import gtsam import gtsam_unstable from gtsam.utils.test_case import GtsamTestCase - -def _timestamp_key_value(key, value): - return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( - key, value - ) - - class TestFixedLagSmootherExample(GtsamTestCase): ''' Tests the fixed lag smoother wrapper @@ -47,14 +40,14 @@ class TestFixedLagSmootherExample(GtsamTestCase): # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) - prior_noise = gtsam.noiseModel_Diagonal.Sigmas( + prior_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.3, 0.3, 0.1])) X1 = 0 new_factors.push_back( gtsam.PriorFactorPose2( X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + new_timestamps.insert((X1, 0.0)) delta_time = 0.25 time = 0.25 @@ -80,11 +73,11 @@ class TestFixedLagSmootherExample(GtsamTestCase): # and its two odometers measure the change. The smoothed # result is then compared to the ground truth while time <= 3.0: - previous_key = 1000 * (time - delta_time) - current_key = 1000 * time + previous_key = int(1000 * (time - delta_time)) + current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert(_timestamp_key_value(current_key, time)) + new_timestamps.insert((current_key, time)) # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * @@ -95,7 +88,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): # Add odometry factors from two different sources with different # error stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.05])) new_factors.push_back( gtsam.BetweenFactorPose2( @@ -105,7 +98,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): odometry_noise_1)) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.05, 0.05])) new_factors.push_back( gtsam.BetweenFactorPose2(