From 9216934ca8295a00200121a687d4378abbcb8350 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 09:32:31 -0400 Subject: [PATCH 01/22] Replace with new python tests --- python/gtsam_py/python/__init__.py | 29 +-- .../python/examples/DogLegOptimizerExample.py | 6 +- .../python/examples/GPSFactorExample.py | 4 +- .../python/examples/ImuFactorExample.py | 50 ++--- .../python/examples/ImuFactorExample2.py | 77 +++---- .../python/examples/OdometryExample.py | 4 +- .../examples/PlanarManipulatorExample.py | 6 +- .../python/examples/PlanarSLAMExample.py | 21 +- .../python/examples/Pose2SLAMExample.py | 4 +- .../python/examples/Pose2SLAMExample_g2o.py | 9 +- .../python/examples/Pose3SLAMExample_g2o.py | 13 +- ...Pose3SLAMExample_initializePose3Chordal.py | 2 +- .../python/examples/PreintegrationExample.py | 4 +- python/gtsam_py/python/examples/README.md | 9 +- python/gtsam_py/python/examples/SFMExample.py | 33 +-- python/gtsam_py/python/examples/SFMdata.py | 3 +- .../python/examples/SimpleRotation.py | 7 +- .../python/examples/VisualISAM2Example.py | 27 ++- .../python/examples/VisualISAMExample.py | 33 +-- python/gtsam_py/python/tests/__init__.py | 0 .../python/tests/testScenarioRunner.py | 2 +- .../python/tests/test_GaussianFactorGraph.py | 13 +- .../python/tests/test_JacobianFactor.py | 6 +- .../python/tests/test_KalmanFilter.py | 4 +- .../python/tests/test_KarcherMeanFactor.py | 6 +- .../python/tests/test_LocalizationExample.py | 4 +- .../python/tests/test_NonlinearOptimizer.py | 15 +- .../python/tests/test_OdometryExample.py | 4 +- .../python/tests/test_PlanarSLAMExample.py | 6 +- .../python/tests/test_Pose2SLAMExample.py | 6 +- .../python/tests/test_Pose3SLAMExample.py | 2 +- .../gtsam_py/python/tests/test_PriorFactor.py | 26 +-- .../gtsam_py/python/tests/test_SFMExample.py | 24 +-- python/gtsam_py/python/tests/test_Scenario.py | 5 +- .../python/tests/test_SimpleCamera.py | 8 +- .../python/tests/test_StereoVOExample.py | 12 +- .../python/tests/test_Triangulation.py | 80 +++++++ python/gtsam_py/python/tests/test_Values.py | 9 +- .../python/tests/test_VisualISAMExample.py | 4 +- python/gtsam_py/python/tests/test_dataset.py | 6 +- .../python/tests/test_initialize_pose3.py | 7 +- .../python/tests/test_logging_optimizer.py | 2 +- python/gtsam_py/python/utils/circlePose3.py | 6 +- python/gtsam_py/python/utils/plot.py | 200 +++--------------- python/gtsam_py/python/utils/test_case.py | 6 +- .../python/utils/visual_data_generator.py | 53 +++-- python/gtsam_py/python/utils/visual_isam.py | 22 +- .../examples/FixedLagSmootherExample.py | 24 +-- .../python/examples/TimeOfArrivalExample.py | 4 +- .../tests/test_FixedLagSmootherExample.py | 21 +- 50 files changed, 408 insertions(+), 520 deletions(-) delete mode 100644 python/gtsam_py/python/tests/__init__.py create mode 100644 python/gtsam_py/python/tests/test_Triangulation.py diff --git a/python/gtsam_py/python/__init__.py b/python/gtsam_py/python/__init__.py index d40ee4502..9e3e19187 100644 --- a/python/gtsam_py/python/__init__.py +++ b/python/gtsam_py/python/__init__.py @@ -1,26 +1,9 @@ from .gtsam import * -try: - import gtsam_unstable - - - def _deprecated_wrapper(item, name): - def wrapper(*args, **kwargs): - from warnings import warn - message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + - 'Please import it from gtsam_unstable.') - warn(message) - return item(*args, **kwargs) - return wrapper - - - for name in dir(gtsam_unstable): - if not name.startswith('__'): - item = getattr(gtsam_unstable, name) - if callable(item): - item = _deprecated_wrapper(item, name) - globals()[name] = item - -except ImportError: - pass +def Point2(x=0, y=0): + import numpy as np + return np.array([x, y], dtype=float) +def Point3(x=0, y=0, z=0): + import numpy as np + return np.array([x, y, z], dtype=float) diff --git a/python/gtsam_py/python/examples/DogLegOptimizerExample.py b/python/gtsam_py/python/examples/DogLegOptimizerExample.py index 776ceedc4..26f4fef84 100644 --- a/python/gtsam_py/python/examples/DogLegOptimizerExample.py +++ b/python/gtsam_py/python/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_py/python/examples/GPSFactorExample.py b/python/gtsam_py/python/examples/GPSFactorExample.py index 493a07725..0bc0d1bf3 100644 --- a/python/gtsam_py/python/examples/GPSFactorExample.py +++ b/python/gtsam_py/python/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_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index 0e01766e7..adaa3c08a 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -5,27 +5,32 @@ All Rights Reserved See LICENSE for the license information -A script validating and demonstrating the ImuFactor inference. - -Author: Frank Dellaert, Varun Agrawal +A script validating the ImuFactor inference. """ from __future__ import print_function 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.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D +import gtsam +from gtsam.utils.plot import plot_pose3 from PreintegrationExample import POSES_FIG, PreintegrationExample -BIAS_KEY = B(0) +BIAS_KEY = int(gtsam.symbol('b', 0)) + + +def X(key): + """Create symbol for pose key.""" + return gtsam.symbol('x', key) + + +def V(key): + """Create symbol for velocity key.""" + return gtsam.symbol('v', key) np.set_printoptions(precision=3, suppress=True) @@ -35,8 +40,8 @@ class ImuFactorExample(PreintegrationExample): def __init__(self): 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)) @@ -47,7 +52,7 @@ class ImuFactorExample(PreintegrationExample): 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) @@ -71,14 +76,8 @@ class ImuFactorExample(PreintegrationExample): 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) + initial.insert(X(i), state_i.pose()) + initial.insert(V(i), state_i.velocity()) # simulate the loop i = 0 # state index @@ -89,12 +88,6 @@ class ImuFactorExample(PreintegrationExample): 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) @@ -140,10 +133,7 @@ class ImuFactorExample(PreintegrationExample): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 - - gtsam.utils.plot.set_axes_equal(POSES_FIG) - - print(result.atimuBias_ConstantBias(BIAS_KEY)) + print(result.atimuBias.ConstantBias(BIAS_KEY)) plt.ioff() plt.show() diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py index 0d98f08fe..39e22eb17 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample2.py +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -8,20 +8,23 @@ 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, - ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, - 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 mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 +import gtsam +import gtsam.utils.plot as gtsam_plot + + +def X(key): + """Create symbol for pose key.""" + return gtsam.symbol('x', key) + + +def V(key): + """Create symbol for velocity key.""" + return gtsam.symbol('v', key) + def vector3(x, y, z): """Create 3d double numpy array.""" @@ -40,7 +43,7 @@ def ISAM2_plot(values, fignum=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() + position = pose_i.translation() 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 @@ -65,20 +68,21 @@ 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)) +BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1) +DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) def IMU_example(): """Run iSAM 2 example with IMU factor.""" + ZERO_BIAS = gtsam.imuBias.ConstantBias() # 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()) + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + position = gtsam.Point3(radius, 0, 0) + camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses @@ -87,37 +91,37 @@ def IMU_example(): angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = ConstantTwistScenario( + scenario = gtsam.ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph - newgraph = NonlinearFactorGraph() + newgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver - isam = ISAM2() + isam = gtsam.ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth - initialEstimate = Values() + initialEstimate = gtsam.Values() # 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)) + newgraph.push_back(gtsam.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) + biasKey = gtsam.symbol('b', 0) + biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + biasprior = gtsam.PriorFactorConstantBias(biasKey, ZERO_BIAS, + biasnoise) newgraph.push_back(biasprior) - initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + initialEstimate.insert(biasKey, ZERO_BIAS) + 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) + velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) @@ -138,10 +142,10 @@ def IMU_example(): # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - factor = BetweenFactorConstantBias( - biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + factor = gtsam.BetweenFactorConstantBias( + biasKey - 1, biasKey, ZERO_BIAS, BIAS_COVARIANCE) newgraph.add(factor) - initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + initialEstimate.insert(biasKey, ZERO_BIAS) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() @@ -151,7 +155,8 @@ def IMU_example(): accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor - imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + imufac = gtsam.ImuFactor( + X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong @@ -164,7 +169,7 @@ def IMU_example(): ISAM2_plot(result) # reset - newgraph = NonlinearFactorGraph() + newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear() diff --git a/python/gtsam_py/python/examples/OdometryExample.py b/python/gtsam_py/python/examples/OdometryExample.py index e778e3f85..8b519ce9a 100644 --- a/python/gtsam_py/python/examples/OdometryExample.py +++ b/python/gtsam_py/python/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_py/python/examples/PlanarManipulatorExample.py b/python/gtsam_py/python/examples/PlanarManipulatorExample.py index e42ae09d7..9af4f7fcc 100644 --- a/python/gtsam_py/python/examples/PlanarManipulatorExample.py +++ b/python/gtsam_py/python/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_py/python/examples/PlanarSLAMExample.py b/python/gtsam_py/python/examples/PlanarSLAMExample.py index c84f0f834..0baf2fa9a 100644 --- a/python/gtsam_py/python/examples/PlanarSLAMExample.py +++ b/python/gtsam_py/python/examples/PlanarSLAMExample.py @@ -13,25 +13,24 @@ 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 # 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() # 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) +X1 = gtsam.symbol('x', 1) +X2 = gtsam.symbol('x', 2) +X3 = gtsam.symbol('x', 3) +L1 = gtsam.symbol('l', 4) +L2 = gtsam.symbol('l', 5) # 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)) diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample.py b/python/gtsam_py/python/examples/Pose2SLAMExample.py index 680f2209f..686da4db3 100644 --- a/python/gtsam_py/python/examples/Pose2SLAMExample.py +++ b/python/gtsam_py/python/examples/Pose2SLAMExample.py @@ -29,8 +29,8 @@ def vector3(x, y, z): # 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_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py index 09114370d..d27adfc2c 100644 --- a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py @@ -53,15 +53,16 @@ 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)) +graphWithPrior = graph +priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) +graphWithPrior.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) +optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) # ... and optimize result = optimizer.optimize() @@ -82,7 +83,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.utilities_extractPose2(result) + resultPoses = gtsam.extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py index 3c1a54f7b..20b02500e 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py @@ -39,21 +39,22 @@ 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 ") +graphWithPrior = graph firstKey = initial.keys().at(0) -graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) +graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) result = optimizer.optimize() print("Optimization complete") -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) +print("initial error = ", graphWithPrior.error(initial)) +print("final error = ", graphWithPrior.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) @@ -65,7 +66,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.utilities_allPose3s(result) + resultPoses = gtsam.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py index 02c696905..140669c95 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -24,7 +24,7 @@ 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) graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index 76520b355..e543e5d82 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -68,7 +68,7 @@ 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) @@ -111,7 +111,7 @@ class PreintegrationExample(object): 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(t), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) diff --git a/python/gtsam_py/python/examples/README.md b/python/gtsam_py/python/examples/README.md index 99bce00e2..ec4437d4c 100644 --- a/python/gtsam_py/python/examples/README.md +++ b/python/gtsam_py/python/examples/README.md @@ -1,7 +1,8 @@ -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. +# THIS FILE IS OUTDATED! + +~~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, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol('b', 0))`~~ # Porting Progress diff --git a/python/gtsam_py/python/examples/SFMExample.py b/python/gtsam_py/python/examples/SFMExample.py index e02def2f9..0c03a105d 100644 --- a/python/gtsam_py/python/examples/SFMExample.py +++ b/python/gtsam_py/python/examples/SFMExample.py @@ -10,20 +10,24 @@ A structure-from-motion problem on a simulated dataset """ 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 + +import gtsam from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, - Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, + NonlinearFactorGraph, Point3, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, SimpleCamera, Values) from gtsam.utils import plot +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ + return gtsam.symbol(name, index) + + def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -56,7 +60,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,24 +73,24 @@ 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])) - factor = PriorFactorPose3(X(0), poses[0], pose_noise) + pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) graph.push_back(factor) # Simulated measurements from each camera pose, adding them to the factor graph for i, pose in enumerate(poses): - camera = PinholeCameraCal3_S2(pose, K) + camera = SimpleCamera(pose, K) for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, X(i), L(j), K) + measurement, measurement_noise, symbol('x', i), symbol('l', j), K) graph.push_back(factor) # 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) - factor = PriorFactorPoint3(L(0), points[0], point_noise) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') @@ -95,10 +99,10 @@ def main(): initial_estimate = Values() for i, pose in enumerate(poses): transformed_pose = pose.retract(0.1*np.random.randn(6,1)) - initial_estimate.insert(X(i), transformed_pose) + initial_estimate.insert(symbol('x', i), transformed_pose) for j, point in enumerate(points): transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) - initial_estimate.insert(L(j), transformed_point) + initial_estimate.insert(symbol('l', j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results @@ -117,5 +121,6 @@ def main(): plot.set_axes_equal(1) plt.show() + if __name__ == '__main__': main() diff --git a/python/gtsam_py/python/examples/SFMdata.py b/python/gtsam_py/python/examples/SFMdata.py index c586f7e52..6ac9c5726 100644 --- a/python/gtsam_py/python/examples/SFMdata.py +++ b/python/gtsam_py/python/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_py/python/examples/SimpleRotation.py b/python/gtsam_py/python/examples/SimpleRotation.py index 4e82d3778..2dd7b1194 100644 --- a/python/gtsam_py/python/examples/SimpleRotation.py +++ b/python/gtsam_py/python/examples/SimpleRotation.py @@ -10,9 +10,8 @@ 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 def main(): @@ -33,8 +32,8 @@ def main(): """ prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) prior.print_('goal angle') - model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) - key = X(1) + model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) + key = gtsam.symbol('x', 1) factor = gtsam.PriorFactorRot2(key, prior, model) """ diff --git a/python/gtsam_py/python/examples/VisualISAM2Example.py b/python/gtsam_py/python/examples/VisualISAM2Example.py index 49e6ca95c..d33ec98bf 100644 --- a/python/gtsam_py/python/examples/VisualISAM2Example.py +++ b/python/gtsam_py/python/examples/VisualISAM2Example.py @@ -13,15 +13,24 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) from __future__ import print_function -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.examples import SFMdata from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 +import gtsam +import gtsam.utils.plot as gtsam_plot +from gtsam.examples import SFMdata + + +def X(i): + """Create key for pose i.""" + return int(gtsam.symbol('x', i)) + + +def L(j): + """Create key for landmark j.""" + return int(gtsam.symbol('l', j)) + def visual_ISAM2_plot(result): """ @@ -64,7 +73,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 +119,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 +132,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_py/python/examples/VisualISAMExample.py b/python/gtsam_py/python/examples/VisualISAMExample.py index 5cc37867b..2e90ce21b 100644 --- a/python/gtsam_py/python/examples/VisualISAMExample.py +++ b/python/gtsam_py/python/examples/VisualISAMExample.py @@ -15,12 +15,15 @@ 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, +from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, + NonlinearFactorGraph, NonlinearISAM, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - PinholeCameraCal3_S2, Values) -from gtsam import symbol_shorthand_L as L -from gtsam import symbol_shorthand_X as X + SimpleCamera, Values, Point3) + + +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ + return gtsam.symbol(name, index) def main(): @@ -34,7 +37,7 @@ 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() @@ -51,11 +54,11 @@ def main(): # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): - camera = PinholeCameraCal3_S2(pose, K) + camera = SimpleCamera(pose, K) # 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, symbol('x', i), symbol('l', j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -63,7 +66,7 @@ def main(): initial_xi = pose.compose(noise) # Add an initial guess for the current pose - initial_estimate.insert(X(i), initial_xi) + initial_estimate.insert(symbol('x', i), initial_xi) # If this is the first iteration, add a prior on the first pose to set the coordinate frame # and a prior on the first landmark to set the scale @@ -71,21 +74,21 @@ 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])) - factor = PriorFactorPose3(X(0), poses[0], pose_noise) + pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('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) - factor = PriorFactorPoint3(L(0), points[0], point_noise) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) graph.push_back(factor) # Add initial guesses to all observed landmarks 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(symbol('l', j), initial_lj) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/python/gtsam_py/python/tests/__init__.py b/python/gtsam_py/python/tests/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam_py/python/tests/testScenarioRunner.py b/python/gtsam_py/python/tests/testScenarioRunner.py index 97a97b0ec..2af16a794 100644 --- a/python/gtsam_py/python/tests/testScenarioRunner.py +++ b/python/gtsam_py/python/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_py/python/tests/test_GaussianFactorGraph.py b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py index 983825d8b..759de0f3b 100644 --- a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py +++ b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py @@ -15,21 +15,20 @@ from __future__ import print_function import unittest import gtsam -import numpy as np -from gtsam import symbol_shorthand_X as X from gtsam.utils.test_case import GtsamTestCase +import numpy as np def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - x0 = X(0) - x1 = X(1) - x2 = X(2) + x0 = gtsam.symbol('x', 0) + x1 = gtsam.symbol('x', 1) + x2 = gtsam.symbol('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_py/python/tests/test_JacobianFactor.py b/python/gtsam_py/python/tests/test_JacobianFactor.py index 04433492b..6e049ed47 100644 --- a/python/gtsam_py/python/tests/test_JacobianFactor.py +++ b/python/gtsam_py/python/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_py/python/tests/test_KalmanFilter.py b/python/gtsam_py/python/tests/test_KalmanFilter.py index 94c41df72..48a91b96c 100644 --- a/python/gtsam_py/python/tests/test_KalmanFilter.py +++ b/python/gtsam_py/python/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_py/python/tests/test_KarcherMeanFactor.py b/python/gtsam_py/python/tests/test_KarcherMeanFactor.py index 6976decc1..a315a506c 100644 --- a/python/gtsam_py/python/tests/test_KarcherMeanFactor.py +++ b/python/gtsam_py/python/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_py/python/tests/test_LocalizationExample.py b/python/gtsam_py/python/tests/test_LocalizationExample.py index 6ce65f087..8ae3583f0 100644 --- a/python/gtsam_py/python/tests/test_LocalizationExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_NonlinearOptimizer.py b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py index 985dc30a2..3041e325c 100644 --- a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py +++ b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py @@ -17,8 +17,7 @@ import unittest import gtsam from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, PCGSolverParameters, - DummyPreconditionerParameters, NonlinearFactorGraph, Ordering, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase @@ -30,7 +29,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 @@ -62,16 +61,6 @@ class TestScenario(GtsamTestCase): fg, initial_values, lmParams).optimize() self.assertAlmostEqual(0, fg.error(actual2)) - # Levenberg-Marquardt - lmParams = LevenbergMarquardtParams.CeresDefaults() - lmParams.setLinearSolverType("ITERATIVE") - cgParams = PCGSolverParameters() - cgParams.setPreconditionerParams(DummyPreconditionerParameters()) - lmParams.setIterativeParams(cgParams) - actual2 = LevenbergMarquardtOptimizer( - fg, initial_values, lmParams).optimize() - self.assertAlmostEqual(0, fg.error(actual2)) - # Dogleg dlParams = DoglegParams() dlParams.setOrdering(ordering) diff --git a/python/gtsam_py/python/tests/test_OdometryExample.py b/python/gtsam_py/python/tests/test_OdometryExample.py index c8ea95588..72e532f20 100644 --- a/python/gtsam_py/python/tests/test_OdometryExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_PlanarSLAMExample.py b/python/gtsam_py/python/tests/test_PlanarSLAMExample.py index ae813d35c..8cb3ad2ac 100644 --- a/python/gtsam_py/python/tests/test_PlanarSLAMExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_Pose2SLAMExample.py b/python/gtsam_py/python/tests/test_Pose2SLAMExample.py index a79b6b18c..e47b9fbff 100644 --- a/python/gtsam_py/python/tests/test_Pose2SLAMExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_Pose3SLAMExample.py b/python/gtsam_py/python/tests/test_Pose3SLAMExample.py index 1e9eaac67..fce171b55 100644 --- a/python/gtsam_py/python/tests/test_Pose3SLAMExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_PriorFactor.py b/python/gtsam_py/python/tests/test_PriorFactor.py index 66207b800..da072d0bd 100644 --- a/python/gtsam_py/python/tests/test_PriorFactor.py +++ b/python/gtsam_py/python/tests/test_PriorFactor.py @@ -23,39 +23,17 @@ 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) - def test_AddPrior(self): - """ - Test adding prior factors directly to factor graph via the .addPrior method. - """ - # define factor graph - graph = gtsam.NonlinearFactorGraph() - - # define and add Pose3 prior - key = 5 - priorPose3 = gtsam.Pose3() - 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) - graph.addPriorVector(key, priorVector, model) - self.assertEqual(graph.size(), 2) - - if __name__ == "__main__": unittest.main() diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam_py/python/tests/test_SFMExample.py index e8fa46186..8b5473ff0 100644 --- a/python/gtsam_py/python/tests/test_SFMExample.py +++ b/python/gtsam_py/python/tests/test_SFMExample.py @@ -34,29 +34,29 @@ class TestSFMExample(GtsamTestCase): graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements - measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + measurementNoise = gtsam.noiseModel.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)) + symbol('x', i), symbol('p', j), data.K)) - posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) - graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0), + posePriorNoise = gtsam.noiseModel.Diagonal.Sigmas(poseNoiseSigmas) + graph.add(gtsam.PriorFactorPose3(symbol('x', 0), truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma) - graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0), + pointPriorNoise = gtsam.noiseModel.Isotropic.Sigma(3, pointNoiseSigma) + graph.add(gtsam.PriorFactorPoint3(symbol('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(symbol('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(symbol('p', j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) @@ -66,16 +66,16 @@ class TestSFMExample(GtsamTestCase): # Marginalization marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(symbol(ord('p'), 0)) - marginals.marginalCovariance(symbol(ord('x'), 0)) + marginals.marginalCovariance(symbol('p', 0)) + marginals.marginalCovariance(symbol('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(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('p'), j)) + point_j = result.atPoint3(symbol('p', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": diff --git a/python/gtsam_py/python/tests/test_Scenario.py b/python/gtsam_py/python/tests/test_Scenario.py index 09601fba5..fc5965829 100644 --- a/python/gtsam_py/python/tests/test_Scenario.py +++ b/python/gtsam_py/python/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_py/python/tests/test_SimpleCamera.py b/python/gtsam_py/python/tests/test_SimpleCamera.py index a3654a5f1..efdfec561 100644 --- a/python/gtsam_py/python/tests/test_SimpleCamera.py +++ b/python/gtsam_py/python/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_py/python/tests/test_StereoVOExample.py b/python/gtsam_py/python/tests/test_StereoVOExample.py index 3f5f57522..cefc08aab 100644 --- a/python/gtsam_py/python/tests/test_StereoVOExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_Triangulation.py b/python/gtsam_py/python/tests/test_Triangulation.py new file mode 100644 index 000000000..b43ad9b57 --- /dev/null +++ b/python/gtsam_py/python/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_py/python/tests/test_Values.py b/python/gtsam_py/python/tests/test_Values.py index 20634a21c..dddd11c40 100644 --- a/python/gtsam_py/python/tests/test_Values.py +++ b/python/gtsam_py/python/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_py/python/tests/test_VisualISAMExample.py b/python/gtsam_py/python/tests/test_VisualISAMExample.py index 99d7e6160..6eb05eeee 100644 --- a/python/gtsam_py/python/tests/test_VisualISAMExample.py +++ b/python/gtsam_py/python/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_py/python/tests/test_dataset.py b/python/gtsam_py/python/tests/test_dataset.py index 60fb9450d..87fc2ad54 100644 --- a/python/gtsam_py/python/tests/test_dataset.py +++ b/python/gtsam_py/python/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_py/python/tests/test_initialize_pose3.py b/python/gtsam_py/python/tests/test_initialize_pose3.py index 3aa7e3470..6d7f66653 100644 --- a/python/gtsam_py/python/tests/test_initialize_pose3.py +++ b/python/gtsam_py/python/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_py/python/tests/test_logging_optimizer.py b/python/gtsam_py/python/tests/test_logging_optimizer.py index 2560a72a2..47eb32e7b 100644 --- a/python/gtsam_py/python/tests/test_logging_optimizer.py +++ b/python/gtsam_py/python/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_py/python/utils/circlePose3.py b/python/gtsam_py/python/utils/circlePose3.py index 7012548f4..37e7c5568 100644 --- a/python/gtsam_py/python/utils/circlePose3.py +++ b/python/gtsam_py/python/utils/circlePose3.py @@ -3,7 +3,7 @@ import numpy as np from math import pi, cos, sin -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 +18,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 diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index 1e976a69e..7b8da0f06 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/utils/plot.py @@ -13,9 +13,8 @@ def set_axes_equal(fignum): Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. - - Args: - fignum (int): An integer representing the figure number for Matplotlib. + Input + ax: a matplotlib axis, e.g., as output from plt.gca(). """ fig = plt.figure(fignum) ax = fig.gca(projection='3d') @@ -35,21 +34,7 @@ def set_axes_equal(fignum): def ellipsoid(xc, yc, zc, rx, ry, rz, n): - """ - Numpy equivalent of Matlab's ellipsoid function. - - Args: - xc (double): Center of ellipsoid in X-axis. - yc (double): Center of ellipsoid in Y-axis. - zc (double): Center of ellipsoid in Z-axis. - rx (double): Radius of ellipsoid in X-axis. - ry (double): Radius of ellipsoid in Y-axis. - rz (double): Radius of ellipsoid in Z-axis. - n (int): The granularity of the ellipsoid plotted. - - Returns: - tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. - """ + """Numpy equivalent of Matlab's ellipsoid function""" u = np.linspace(0, 2*np.pi, n+1) v = np.linspace(0, np.pi, n+1) x = -rx * np.outer(np.cos(u), np.sin(v)).T @@ -62,18 +47,9 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n): def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): """ Plots a Gaussian as an uncertainty ellipse - Based on Maybeck Vol 1, page 366 k=2.296 corresponds to 1 std, 68.26% of all probability k=11.82 corresponds to 3 std, 99.74% of all probability - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - origin (gtsam.Point3): The origin in the world frame. - P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. - scale (float): Scaling factor of the radii of the covariance ellipse. - n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. - alpha (float): Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 U, S, _ = np.linalg.svd(P) @@ -97,19 +73,10 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): - """ - Plot a 2D pose on given axis `axes` with given `axis_length`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - """ + """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" # 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 = pose.translation() # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -134,89 +101,34 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): np.rad2deg(angle), fill=False) axes.add_patch(e1) - -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 2D pose on given figure with given `axis_length`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - """ +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): + """Plot a 2D pose on given figure with given 'axis_length'.""" # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length=axis_length, - covariance=covariance) - - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) - - return fig + plot_pose2_on_axes(axes, pose, axis_length, covariance) def plot_point3_on_axes(axes, point, linespec, P=None): - """ - Plot a 3D point on given axis `axes` with given `linespec`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - 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) + """Plot a 3D point on given axis 'axes' with given 'linespec' and optional covariance 'P'.""" + axes.plot([point[0]], [point[1]], [point[2]], linespec) if P is not None: plot_covariance_ellipse_3d(axes, point.vector(), P) -def plot_point3(fignum, point, linespec, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 3D point on given figure with given `linespec`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - - Returns: - fig: The matplotlib figure. - - """ +def plot_point3(fignum, point, linespec, P=None): + """Plot a 3D point on given figure with given 'linespec' and optional covariance 'P'.""" fig = plt.figure(fignum) axes = fig.gca(projection='3d') plot_point3_on_axes(axes, point, linespec, P) - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) - return fig - - -def plot_3d_points(fignum, values, linespec="g*", marginals=None, - title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_3d_points(fignum, values, linespec="g*", marginals=None): """ - Plots the Point3s in `values`, with optional covariances. + Plots the Point3s in 'values', with optional covariances. Finds all the Point3 objects in the given Values object and plots them. If a Marginals object is given, this function will also plot marginal covariance ellipses for each point. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dictionary consisting of points to be plotted. - linespec (string): String representing formatting options for Matplotlib. - marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - title (string): The title of the plot. - axis_labels (iterable[string]): List of axis labels to set. """ keys = values.keys() @@ -227,34 +139,25 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, key = keys.at(i) point = values.atPoint3(key) if marginals is not None: - covariance = marginals.marginalCovariance(key) + P = marginals.marginalCovariance(key); else: - covariance = None + P = None - fig = plot_point3(fignum, point, linespec, covariance, - axis_labels=axis_labels) + plot_point3(fignum, point, linespec, P) except RuntimeError: continue # I guess it's not a Point3 - fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) - -def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): +def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1): """ - Plot a 3D pose on given axis `axes` with given `axis_length`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + Plot a 3D pose on given axis 'axes' with given 'axis_length'. + Optional 'scale' the pose and plot the covariance 'P'. """ # 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 @@ -269,7 +172,6 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') - # plot the covariance if P is not None: # covariance matrix in pose coordinate frame pPp = P[3:6, 3:6] @@ -278,49 +180,16 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 3D pose on given figure with given `axis_length`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose3): 3D pose to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - - Returns: - fig: The matplotlib figure. - """ +def plot_pose3(fignum, pose, P, axis_length=0.1): + """Plot a 3D pose on given figure with given 'axis_length'.""" # get figure object fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, P=P, - axis_length=axis_length) - - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) - - return fig + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) -def plot_trajectory(fignum, values, scale=1, marginals=None, - title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - 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. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. - Used to plot uncertainty bounds. - title (string): The title of the plot. - axis_labels (iterable[string]): List of axis labels to set. - """ - pose3Values = gtsam.utilities_allPose3s(values) +def plot_trajectory(fignum, values, scale=1, marginals=None): + pose3Values = gtsam.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) lastIndex = None @@ -340,12 +209,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, pass if marginals: - covariance = marginals.marginalCovariance(lastKey) + P = marginals.marginalCovariance(lastKey) else: - covariance = None + P = None - fig = plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale, axis_labels=axis_labels) + plot_pose3(fignum, lastPose, P, scale) lastIndex = i @@ -355,15 +223,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, try: lastPose = pose3Values.atPose3(lastKey) if marginals: - covariance = marginals.marginalCovariance(lastKey) + P = marginals.marginalCovariance(lastKey) else: - covariance = None + P = None - fig = plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale, axis_labels=axis_labels) + plot_pose3(fignum, lastPose, P, scale) except: pass - - fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) diff --git a/python/gtsam_py/python/utils/test_case.py b/python/gtsam_py/python/utils/test_case.py index 7df1e6ee9..3effd7f65 100644 --- a/python/gtsam_py/python/utils/test_case.py +++ b/python/gtsam_py/python/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_py/python/utils/visual_data_generator.py b/python/gtsam_py/python/utils/visual_data_generator.py index f04588e70..bc60dcd70 100644 --- a/python/gtsam_py/python/utils/visual_data_generator.py +++ b/python/gtsam_py/python/utils/visual_data_generator.py @@ -1,9 +1,8 @@ from __future__ import print_function import numpy as np - +from math import pi, cos, sin import gtsam -from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3 class Options: @@ -11,7 +10,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -28,10 +27,10 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.cameras = [Pose3()] * nrCameras - self.points = [Point3()] * nrPoints + self.cameras = [gtsam.Pose3()] * nrCameras + self.points = [gtsam.Point3(0, 0, 0)] * nrPoints def print_(self, s=""): print(s) @@ -53,28 +52,28 @@ class Data: class NoiseModels: pass - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=gtsam.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 + self.odometry = [gtsam.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): """ Generate ground-truth and measurement data. """ - K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) nrPoints = 3 if options.triangle else 8 truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) @@ -84,26 +83,26 @@ 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] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ - Point3(10, 10, 10), Point3(-10, 10, 10), - Point3(-10, -10, 10), Point3(10, -10, 10), - Point3(10, 10, -10), Point3(-10, 10, -10), - Point3(-10, -10, -10), Point3(10, -10, -10) + gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), + gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), + gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), + gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) ] # Create camera cameras on a circle around the triangle height = 10 r = 40 for i in range(options.nrCameras): - theta = i * 2 * np.pi / options.nrCameras - t = Point3(r * np.cos(theta), r * np.sin(theta), height) - truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, - Point3(), - Point3(0, 0, 1), - truth.K) + theta = i * 2 * pi / options.nrCameras + t = gtsam.Point3(r * cos(theta), r * sin(theta), height) + truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, + gtsam.Point3(0, 0, 0), + gtsam.Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame diff --git a/python/gtsam_py/python/utils/visual_isam.py b/python/gtsam_py/python/utils/visual_isam.py index b0ebe68c3..a8fed4b23 100644 --- a/python/gtsam_py/python/utils/visual_isam.py +++ b/python/gtsam_py/python/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_py/python/examples/FixedLagSmootherExample.py b/python/gtsam_unstable_py/python/examples/FixedLagSmootherExample.py index 786701e0f..7d2cea8ae 100644 --- a/python/gtsam_unstable_py/python/examples/FixedLagSmootherExample.py +++ b/python/gtsam_unstable_py/python/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_py/python/examples/TimeOfArrivalExample.py b/python/gtsam_unstable_py/python/examples/TimeOfArrivalExample.py index 6ba06f0f2..59f008a05 100644 --- a/python/gtsam_unstable_py/python/examples/TimeOfArrivalExample.py +++ b/python/gtsam_unstable_py/python/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_py/python/tests/test_FixedLagSmootherExample.py b/python/gtsam_unstable_py/python/tests/test_FixedLagSmootherExample.py index 8d3af311f..c1ccd1ea1 100644 --- a/python/gtsam_unstable_py/python/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam_unstable_py/python/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( From 2bda74950a6fc2f97f4323029d7340a67de15958 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 10:56:09 -0400 Subject: [PATCH 02/22] Sync ImuFactorExample --- .../python/examples/ImuFactorExample.py | 46 ++-- python/gtsam_py/python/utils/plot.py | 200 +++++++++++++++--- 2 files changed, 197 insertions(+), 49 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index adaa3c08a..e041afd87 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -5,32 +5,29 @@ All Rights Reserved See LICENSE for the license information -A script validating the ImuFactor inference. +A script validating and demonstrating the ImuFactor inference. + +Author: Frank Dellaert, Varun Agrawal """ from __future__ import print_function import math +import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand +B = symbol_shorthand.B +V = symbol_shorthand.V +X = symbol_shorthand.X + +from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D -import gtsam -from gtsam.utils.plot import plot_pose3 from PreintegrationExample import POSES_FIG, PreintegrationExample -BIAS_KEY = int(gtsam.symbol('b', 0)) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol('x', key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol('v', key) +BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) @@ -76,8 +73,14 @@ class ImuFactorExample(PreintegrationExample): initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) - initial.insert(X(i), state_i.pose()) - initial.insert(V(i), state_i.velocity()) + + 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 @@ -88,6 +91,12 @@ class ImuFactorExample(PreintegrationExample): 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) @@ -133,7 +142,10 @@ class ImuFactorExample(PreintegrationExample): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 - print(result.atimuBias.ConstantBias(BIAS_KEY)) + + gtsam.utils.plot.set_axes_equal(POSES_FIG) + + print(result.atConstantBias(BIAS_KEY)) plt.ioff() plt.show() diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index 7b8da0f06..276cefd54 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/utils/plot.py @@ -13,8 +13,9 @@ def set_axes_equal(fignum): Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. - Input - ax: a matplotlib axis, e.g., as output from plt.gca(). + + Args: + fignum (int): An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) ax = fig.gca(projection='3d') @@ -34,7 +35,21 @@ def set_axes_equal(fignum): def ellipsoid(xc, yc, zc, rx, ry, rz, n): - """Numpy equivalent of Matlab's ellipsoid function""" + """ + Numpy equivalent of Matlab's ellipsoid function. + + Args: + xc (double): Center of ellipsoid in X-axis. + yc (double): Center of ellipsoid in Y-axis. + zc (double): Center of ellipsoid in Z-axis. + rx (double): Radius of ellipsoid in X-axis. + ry (double): Radius of ellipsoid in Y-axis. + rz (double): Radius of ellipsoid in Z-axis. + n (int): The granularity of the ellipsoid plotted. + + Returns: + tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. + """ u = np.linspace(0, 2*np.pi, n+1) v = np.linspace(0, np.pi, n+1) x = -rx * np.outer(np.cos(u), np.sin(v)).T @@ -47,9 +62,18 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n): def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): """ Plots a Gaussian as an uncertainty ellipse + Based on Maybeck Vol 1, page 366 k=2.296 corresponds to 1 std, 68.26% of all probability k=11.82 corresponds to 3 std, 99.74% of all probability + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin (gtsam.Point3): The origin in the world frame. + P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. + scale (float): Scaling factor of the radii of the covariance ellipse. + n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha (float): Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 U, S, _ = np.linalg.svd(P) @@ -73,10 +97,19 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): - """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" + """ + Plot a 2D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global - origin = pose.translation() + t = pose.translation() + origin = np.array([t.x(), t.y()]) # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -101,34 +134,89 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): np.rad2deg(angle), fill=False) axes.add_patch(e1) -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): - """Plot a 2D pose on given figure with given 'axis_length'.""" + +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 2D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + """ # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length, covariance) + plot_pose2_on_axes(axes, pose, axis_length=axis_length, + covariance=covariance) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig def plot_point3_on_axes(axes, point, linespec, P=None): - """Plot a 3D point on given axis 'axes' with given 'linespec' and optional covariance 'P'.""" - axes.plot([point[0]], [point[1]], [point[2]], linespec) + """ + Plot a 3D point on given axis `axes` with given `linespec`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + 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) 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): - """Plot a 3D point on given figure with given 'linespec' and optional covariance 'P'.""" +def plot_point3(fignum, point, linespec, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 3D point on given figure with given `linespec`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. + + """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') plot_point3_on_axes(axes, point, linespec, P) + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) -def plot_3d_points(fignum, values, linespec="g*", marginals=None): + return fig + + +def plot_3d_points(fignum, values, linespec="g*", marginals=None, + title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): """ - Plots the Point3s in 'values', with optional covariances. + Plots the Point3s in `values`, with optional covariances. Finds all the Point3 objects in the given Values object and plots them. If a Marginals object is given, this function will also plot marginal covariance ellipses for each point. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dictionary consisting of points to be plotted. + linespec (string): String representing formatting options for Matplotlib. + marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. """ keys = values.keys() @@ -139,21 +227,30 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None): key = keys.at(i) point = values.atPoint3(key) if marginals is not None: - P = marginals.marginalCovariance(key); + covariance = marginals.marginalCovariance(key) else: - P = None + covariance = None - plot_point3(fignum, point, linespec, P) + fig = plot_point3(fignum, point, linespec, covariance, + axis_labels=axis_labels) except RuntimeError: continue # I guess it's not a Point3 + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) -def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1): + +def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): """ - Plot a 3D pose on given axis 'axes' with given 'axis_length'. - Optional 'scale' the pose and plot the covariance 'P'. + Plot a 3D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global @@ -172,6 +269,7 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1): line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') + # plot the covariance if P is not None: # covariance matrix in pose coordinate frame pPp = P[3:6, 3:6] @@ -180,16 +278,49 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1): plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, P, axis_length=0.1): - """Plot a 3D pose on given figure with given 'axis_length'.""" +def plot_pose3(fignum, pose, axis_length=0.1, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 3D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose3): 3D pose to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. + """ # get figure object fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) + plot_pose3_on_axes(axes, pose, P=P, + axis_length=axis_length) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig -def plot_trajectory(fignum, values, scale=1, marginals=None): - pose3Values = gtsam.allPose3s(values) +def plot_trajectory(fignum, values, scale=1, marginals=None, + title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + 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. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. + """ + pose3Values = gtsam.utilities_allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) lastIndex = None @@ -209,11 +340,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): pass if marginals: - P = marginals.marginalCovariance(lastKey) + covariance = marginals.marginalCovariance(lastKey) else: - P = None + covariance = None - plot_pose3(fignum, lastPose, P, scale) + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) lastIndex = i @@ -223,11 +355,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): try: lastPose = pose3Values.atPose3(lastKey) if marginals: - P = marginals.marginalCovariance(lastKey) + covariance = marginals.marginalCovariance(lastKey) else: - P = None + covariance = None - plot_pose3(fignum, lastPose, P, scale) + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) except: pass + + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) From 48b0c845dc92d46389c8bd57097cd2ce80f644fe Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 11:01:59 -0400 Subject: [PATCH 03/22] Sync ImuFactorExample2 --- .../python/examples/ImuFactorExample2.py | 70 +++++++++---------- 1 file changed, 33 insertions(+), 37 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py index 39e22eb17..4f4b02f63 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample2.py +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -8,22 +8,20 @@ from __future__ import print_function import math -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - import gtsam import gtsam.utils.plot as gtsam_plot - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol('x', key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol('v', key) +import matplotlib.pyplot as plt +import numpy as np +from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, + ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, + PinholeCameraCal3_S2, Point3, Pose3, + PriorFactorConstantBias, PriorFactorPose3, + PriorFactorVector, Rot3, Values) +from gtsam import symbol_shorthand +B = symbol_shorthand.B +V = symbol_shorthand.V +X = symbol_shorthand.X +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def vector3(x, y, z): @@ -69,20 +67,19 @@ PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1) -DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) +DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), + Point3(0.05, -0.10, 0.20)) def IMU_example(): """Run iSAM 2 example with IMU factor.""" - ZERO_BIAS = gtsam.imuBias.ConstantBias() # Start with a camera on x-axis looking at origin radius = 30 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - position = gtsam.Point3(radius, 0, 0) - camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + up = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses @@ -91,37 +88,37 @@ def IMU_example(): angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = gtsam.ConstantTwistScenario( + scenario = ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver - isam = gtsam.ISAM2() + isam = ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth - initialEstimate = gtsam.Values() + initialEstimate = Values() # 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( np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) + newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors - biasKey = gtsam.symbol('b', 0) + biasKey = B(0) biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - biasprior = gtsam.PriorFactorConstantBias(biasKey, ZERO_BIAS, - biasnoise) + biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(), + biasnoise) newgraph.push_back(biasprior) - initialEstimate.insert(biasKey, ZERO_BIAS) + 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 = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) + velprior = PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) @@ -142,10 +139,10 @@ def IMU_example(): # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - factor = gtsam.BetweenFactorConstantBias( - biasKey - 1, biasKey, ZERO_BIAS, BIAS_COVARIANCE) + factor = BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE) newgraph.add(factor) - initialEstimate.insert(biasKey, ZERO_BIAS) + initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() @@ -155,8 +152,7 @@ def IMU_example(): accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor - imufac = gtsam.ImuFactor( - X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong @@ -169,7 +165,7 @@ def IMU_example(): ISAM2_plot(result) # reset - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() initialEstimate.clear() From 128db80fec87a261a27bc3b55377b2c834ca9457 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 11:17:06 -0400 Subject: [PATCH 04/22] Fix Pose2 plot --- python/gtsam_py/python/utils/plot.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index 276cefd54..eef08ac88 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/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 = np.array(t) # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -155,7 +155,6 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) return fig @@ -170,7 +169,7 @@ 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, P) From 7b4266ed6be3b601b04dc7edba1df9957a270321 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 14:35:28 -0400 Subject: [PATCH 05/22] Update to match cython --- .../python/examples/ImuFactorExample.py | 5 +-- .../python/examples/ImuFactorExample2.py | 5 +-- .../python/examples/PlanarSLAMExample.py | 11 ++++--- .../python/examples/Pose2SLAMExample_g2o.py | 2 +- .../python/examples/Pose3SLAMExample_g2o.py | 4 +-- .../python/examples/PreintegrationExample.py | 2 +- python/gtsam_py/python/examples/README.md | 14 +++----- python/gtsam_py/python/examples/SFMExample.py | 32 ++++++++----------- python/gtsam_py/python/utils/plot.py | 14 ++++---- 9 files changed, 37 insertions(+), 52 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index e041afd87..1f8dfca07 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -17,10 +17,7 @@ import math import gtsam import matplotlib.pyplot as plt import numpy as np -from gtsam import symbol_shorthand -B = symbol_shorthand.B -V = symbol_shorthand.V -X = symbol_shorthand.X +from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py index 4f4b02f63..d5be69f47 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample2.py +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -17,10 +17,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) -from gtsam import symbol_shorthand -B = symbol_shorthand.B -V = symbol_shorthand.V -X = symbol_shorthand.X +from gtsam.gtsam.symbol_shorthand import B, V, X from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 diff --git a/python/gtsam_py/python/examples/PlanarSLAMExample.py b/python/gtsam_py/python/examples/PlanarSLAMExample.py index 0baf2fa9a..ba0529479 100644 --- a/python/gtsam_py/python/examples/PlanarSLAMExample.py +++ b/python/gtsam_py/python/examples/PlanarSLAMExample.py @@ -16,6 +16,7 @@ from __future__ import print_function import numpy as np import gtsam +from gtsam.gtsam.symbol_shorthand import X, L # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) graph = gtsam.NonlinearFactorGraph() # Create the keys corresponding to unknown variables in the factor graph -X1 = gtsam.symbol('x', 1) -X2 = gtsam.symbol('x', 2) -X3 = gtsam.symbol('x', 3) -L1 = gtsam.symbol('l', 4) -L2 = gtsam.symbol('l', 5) +X1 = X(1) +X2 = X(2) +X3 = X(3) +L1 = L(4) +L2 = L(5) # 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)) diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py index d27adfc2c..1ea7fc300 100644 --- a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py @@ -83,7 +83,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.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_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py index 20b02500e..c06a9d12f 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py @@ -44,7 +44,7 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, print("Adding prior to g2o file ") graphWithPrior = graph -firstKey = initial.keys().at(0) +firstKey = initial.keys()[0] graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() @@ -66,7 +66,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.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_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index e543e5d82..ee7f9cd8f 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -111,7 +111,7 @@ class PreintegrationExample(object): actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, 0.3) t = actualPose.translation() - self.maxDim = max([max(t), 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) diff --git a/python/gtsam_py/python/examples/README.md b/python/gtsam_py/python/examples/README.md index ec4437d4c..46b87f079 100644 --- a/python/gtsam_py/python/examples/README.md +++ b/python/gtsam_py/python/examples/README.md @@ -1,18 +1,12 @@ -# THIS FILE IS OUTDATED! - -~~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, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol('b', 0))`~~ - # 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 exposed through Python | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python | | ImuFactorExample2 | X | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | @@ -26,7 +20,7 @@ | Pose2SLAMExample_g2o | X | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMExample_lago | lago not exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | | Pose3SLAMExample_changeKeys | | diff --git a/python/gtsam_py/python/examples/SFMExample.py b/python/gtsam_py/python/examples/SFMExample.py index 0c03a105d..f0c4c82ba 100644 --- a/python/gtsam_py/python/examples/SFMExample.py +++ b/python/gtsam_py/python/examples/SFMExample.py @@ -10,24 +10,21 @@ A structure-from-motion problem on a simulated dataset """ from __future__ import print_function +import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand +L = symbol_shorthand.L +X = symbol_shorthand.X -import gtsam from gtsam.examples import SFMdata -from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, +from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, Point3, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) + NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, + Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) from gtsam.utils import plot -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(name, index) - - def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -74,23 +71,23 @@ 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])) - factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Simulated measurements from each camera pose, adding them to the factor graph for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, symbol('x', i), symbol('l', j), K) + measurement, measurement_noise, X(i), L(j), K) graph.push_back(factor) # 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) - factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') @@ -99,10 +96,10 @@ def main(): initial_estimate = Values() for i, pose in enumerate(poses): transformed_pose = pose.retract(0.1*np.random.randn(6,1)) - initial_estimate.insert(symbol('x', i), transformed_pose) + initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) - initial_estimate.insert(symbol('l', j), transformed_point) + transformed_point = point + 0.1*np.random.randn(3) + initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results @@ -121,6 +118,5 @@ def main(): plot.set_axes_equal(1) plt.show() - if __name__ == '__main__': main() diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index eef08ac88..ce602f506 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/utils/plot.py @@ -221,9 +221,9 @@ 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 i in range(len(keys)): try: - key = keys.at(i) + key = keys[i] point = values.atPoint3(key) if marginals is not None: covariance = marginals.marginalCovariance(key) @@ -319,19 +319,19 @@ 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 - for i in range(keys.size()): - key = keys.at(i) + for i in range(len(keys)): + key = keys[i] try: pose = pose3Values.atPose3(key) except: print("Warning: no Pose3 at key: {0}".format(key)) if lastIndex is not None: - lastKey = keys.at(lastIndex) + lastKey = keys[lastIndex] try: lastPose = pose3Values.atPose3(lastKey) except: @@ -350,7 +350,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, # Draw final pose if lastIndex is not None: - lastKey = keys.at(lastIndex) + lastKey = keys[lastIndex] try: lastPose = pose3Values.atPose3(lastKey) if marginals: From bc95b41efcb7f70eba92c13c3a5bad33ebeb6e75 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 15:16:26 -0400 Subject: [PATCH 06/22] Update more to match cython --- .../python/examples/Pose2SLAMExample_g2o.py | 5 ++--- .../python/examples/Pose3SLAMExample_g2o.py | 9 ++++----- .../gtsam_py/python/examples/SimpleRotation.py | 4 ++-- .../python/examples/VisualISAM2Example.py | 18 ++++-------------- 4 files changed, 12 insertions(+), 24 deletions(-) diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py index 1ea7fc300..b2ba9c5bc 100644 --- a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py @@ -53,16 +53,15 @@ 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 -graphWithPrior = graph priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) +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(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py index c06a9d12f..82b3bda98 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py @@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") -graphWithPrior = graph firstKey = initial.keys()[0] -graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) +graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") -print("initial error = ", graphWithPrior.error(initial)) -print("final error = ", graphWithPrior.error(result)) +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) diff --git a/python/gtsam_py/python/examples/SimpleRotation.py b/python/gtsam_py/python/examples/SimpleRotation.py index 2dd7b1194..de8ab07c3 100644 --- a/python/gtsam_py/python/examples/SimpleRotation.py +++ b/python/gtsam_py/python/examples/SimpleRotation.py @@ -12,7 +12,7 @@ a single variable with a single factor. import numpy as np import gtsam - +from gtsam.gtsam.symbol_shorthand import X def main(): """ @@ -33,7 +33,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)) - key = gtsam.symbol('x', 1) + key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) """ diff --git a/python/gtsam_py/python/examples/VisualISAM2Example.py b/python/gtsam_py/python/examples/VisualISAM2Example.py index d33ec98bf..e4c0c8b8d 100644 --- a/python/gtsam_py/python/examples/VisualISAM2Example.py +++ b/python/gtsam_py/python/examples/VisualISAM2Example.py @@ -13,23 +13,13 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) from __future__ import print_function -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - import gtsam import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np +from gtsam.gtsam.symbol_shorthand import L, X from gtsam.examples import SFMdata - - -def X(i): - """Create key for pose i.""" - return int(gtsam.symbol('x', i)) - - -def L(j): - """Create key for landmark j.""" - return int(gtsam.symbol('l', j)) +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def visual_ISAM2_plot(result): From 7114cf93d336ddf08d8e368e34882ed65de9d759 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 20:35:23 -0500 Subject: [PATCH 07/22] update ImuFactorExample.py --- .../python/examples/ImuFactorExample.py | 138 +++++++++++------- 1 file changed, 84 insertions(+), 54 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index 1f8dfca07..03b16098d 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -12,15 +12,17 @@ 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.gtsam.symbol_shorthand import B, V, X - -from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D +import numpy as np + +import gtsam +from gtsam.gtsam.symbol_shorthand import B, V, X +from gtsam.utils.plot import plot_pose3 + from PreintegrationExample import POSES_FIG, PreintegrationExample @@ -32,24 +34,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) # 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) 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) @@ -58,65 +63,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() @@ -124,29 +137,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.atConstantBias(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) From e92c5e2ed42640df1991c675a83cc66ef988c2a9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:00:08 -0500 Subject: [PATCH 08/22] fix warnings from subplots and improve code --- .../python/examples/PreintegrationExample.py | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index ee7f9cd8f..e35722571 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -73,40 +73,43 @@ class PreintegrationExample(object): 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) @@ -117,11 +120,10 @@ class PreintegrationExample(object): 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) From 858f5d42d3757372eaf5835f4d513ade436918fa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:00:24 -0500 Subject: [PATCH 09/22] add incremental plotting function --- python/gtsam_py/python/utils/plot.py | 38 ++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index ce602f506..90f5fe5e7 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/utils/plot.py @@ -366,3 +366,41 @@ 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 i in range(start, len(keys)): + key = keys[i] + if values.exists(key): + pose_i = values.atPose3(keys[i]) + 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) \ No newline at end of file From 0b550b355fa040a3916605eb859efdf8dbbb7e7c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:00:44 -0500 Subject: [PATCH 10/22] update ImuFactorExample2.py --- .../python/examples/ImuFactorExample2.py | 117 +++++++++--------- 1 file changed, 57 insertions(+), 60 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py index d5be69f47..969d4fe6b 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample2.py +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -1,6 +1,6 @@ """ iSAM2 example with ImuFactor. -Author: Robert Truax (C++), Frank Dellaert (Python) +Author: Frank Dellaert, Varun Agrawal """ # pylint: disable=invalid-name, E1101 @@ -8,17 +8,18 @@ from __future__ import print_function import math -import gtsam -import gtsam.utils.plot as gtsam_plot import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import numpy as np + +import gtsam from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) from gtsam.gtsam.symbol_shorthand import B, V, X -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 +from gtsam.utils import plot def vector3(x, y, z): @@ -26,46 +27,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() - 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(): @@ -73,23 +73,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,21 +96,21 @@ def IMU_example(): # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw 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) - newgraph.push_back(biasprior) + 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) @@ -138,7 +132,7 @@ def IMU_example(): biasKey += 1 factor = BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE) - newgraph.add(factor) + graph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame @@ -150,21 +144,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() From c8806dcb24f0aef8ed4c4712db1af323980fc535 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:01:38 -0500 Subject: [PATCH 11/22] rename ImuFactorExample2 to more descriptive name --- .../examples/{ImuFactorExample2.py => ImuFactorISAM2Example.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename python/gtsam_py/python/examples/{ImuFactorExample2.py => ImuFactorISAM2Example.py} (100%) diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorISAM2Example.py similarity index 100% rename from python/gtsam_py/python/examples/ImuFactorExample2.py rename to python/gtsam_py/python/examples/ImuFactorISAM2Example.py From 95d3582c2eddc8b477fd511b91c7d222d548b010 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:25:27 -0500 Subject: [PATCH 12/22] replaced SimpleCamera with PinholeCamera and updated tests --- cython/gtsam/examples/SFMExample.py | 2 +- .../python/tests/test_GaussianFactorGraph.py | 9 ++++---- .../python/tests/test_NonlinearOptimizer.py | 15 +++++++++++-- .../gtsam_py/python/tests/test_PriorFactor.py | 22 +++++++++++++++++++ .../gtsam_py/python/tests/test_SFMExample.py | 7 +++--- .../python/utils/visual_data_generator.py | 9 ++++---- 6 files changed, 50 insertions(+), 14 deletions(-) diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index e02def2f9..68053f862 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -20,7 +20,7 @@ from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) + PinholeCameraCal3_S2, Values) from gtsam.utils import plot diff --git a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py index 759de0f3b..d520b0fa4 100644 --- a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py +++ b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py @@ -15,17 +15,18 @@ from __future__ import print_function import unittest import gtsam +import numpy as np +from gtsam.gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase -import numpy as np def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - x0 = gtsam.symbol('x', 0) - x1 = gtsam.symbol('x', 1) - x2 = gtsam.symbol('x', 2) + x0 = X(0) + x1 = X(1) + x2 = X(2) BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) diff --git a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py index 3041e325c..e9234a43b 100644 --- a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py +++ b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py @@ -15,10 +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, NonlinearFactorGraph, Ordering, - Point2, PriorFactorPoint2, Values) + PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase): fg, initial_values, lmParams).optimize() self.assertAlmostEqual(0, fg.error(actual2)) + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setLinearSolverType("ITERATIVE") + cgParams = PCGSolverParameters() + cgParams.setPreconditionerParams(DummyPreconditionerParameters()) + lmParams.setIterativeParams(cgParams) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + # Dogleg dlParams = DoglegParams() dlParams.setOrdering(ordering) diff --git a/python/gtsam_py/python/tests/test_PriorFactor.py b/python/gtsam_py/python/tests/test_PriorFactor.py index da072d0bd..0582cf5d7 100644 --- a/python/gtsam_py/python/tests/test_PriorFactor.py +++ b/python/gtsam_py/python/tests/test_PriorFactor.py @@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase): values.insert(key, priorVector) self.assertEqual(factor.error(values), 0) + def test_AddPrior(self): + """ + Test adding prior factors directly to factor graph via the .addPrior method. + """ + # define factor graph + graph = gtsam.NonlinearFactorGraph() + + # define and add Pose3 prior + key = 5 + priorPose3 = gtsam.Pose3() + 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) + graph.addPriorVector(key, priorVector, model) + self.assertEqual(graph.size(), 2) + + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam_py/python/tests/test_SFMExample.py index 8b5473ff0..1425902cf 100644 --- a/python/gtsam_py/python/tests/test_SFMExample.py +++ b/python/gtsam_py/python/tests/test_SFMExample.py @@ -15,6 +15,7 @@ import numpy as np import gtsam import gtsam.utils.visual_data_generator as generator from gtsam import symbol +from gtsam.gtsam.noiseModel improt Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase @@ -34,7 +35,7 @@ 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] @@ -42,10 +43,10 @@ class TestSFMExample(GtsamTestCase): data.Z[i][k], measurementNoise, symbol('x', i), symbol('p', j), data.K)) - posePriorNoise = gtsam.noiseModel.Diagonal.Sigmas(poseNoiseSigmas) + posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) graph.add(gtsam.PriorFactorPose3(symbol('x', 0), truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = gtsam.noiseModel.Isotropic.Sigma(3, pointNoiseSigma) + pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) graph.add(gtsam.PriorFactorPoint3(symbol('p', 0), truth.points[0], pointPriorNoise)) diff --git a/python/gtsam_py/python/utils/visual_data_generator.py b/python/gtsam_py/python/utils/visual_data_generator.py index bc60dcd70..24404ca03 100644 --- a/python/gtsam_py/python/utils/visual_data_generator.py +++ b/python/gtsam_py/python/utils/visual_data_generator.py @@ -3,6 +3,7 @@ from __future__ import print_function import numpy as np from math import pi, cos, sin import gtsam +from gtsam import PinholeCameraCal3_S2 class Options: @@ -99,10 +100,10 @@ def generate_data(options): for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras t = gtsam.Point3(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, - gtsam.Point3(0, 0, 0), - gtsam.Point3(0, 0, 1), - truth.K) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + gtsam.Point3(0, 0, 0), + gtsam.Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame From 95b77f76347a653536cec52507241f766c54edee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 27 Jul 2020 21:25:44 -0500 Subject: [PATCH 13/22] sort imports in python examples --- .../python/examples/ImuFactorExample.py | 8 +++----- .../python/examples/ImuFactorISAM2Example.py | 7 +++---- .../python/examples/PreintegrationExample.py | 5 ++--- .../python/examples/VisualISAMExample.py | 20 +++++++++++-------- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index 03b16098d..f9ff44738 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -15,14 +15,12 @@ from __future__ import print_function import argparse import math -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -import numpy as np - import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 - +from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample diff --git a/python/gtsam_py/python/examples/ImuFactorISAM2Example.py b/python/gtsam_py/python/examples/ImuFactorISAM2Example.py index 969d4fe6b..6556eb554 100644 --- a/python/gtsam_py/python/examples/ImuFactorISAM2Example.py +++ b/python/gtsam_py/python/examples/ImuFactorISAM2Example.py @@ -8,11 +8,9 @@ from __future__ import print_function import math -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 -import numpy as np - import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, @@ -20,6 +18,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PriorFactorVector, Rot3, Values) from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.utils import plot +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def vector3(x, y, z): diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index e35722571..e308bc933 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/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 diff --git a/python/gtsam_py/python/examples/VisualISAMExample.py b/python/gtsam_py/python/examples/VisualISAMExample.py index 2e90ce21b..7d562c9b4 100644 --- a/python/gtsam_py/python/examples/VisualISAMExample.py +++ b/python/gtsam_py/python/examples/VisualISAMExample.py @@ -16,9 +16,9 @@ import numpy as np import gtsam from gtsam.examples import SFMdata from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, - NonlinearFactorGraph, NonlinearISAM, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values, Point3) + NonlinearFactorGraph, NonlinearISAM, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + PinholeCameraCal3_S2, Values, Point3) def symbol(name: str, index: int) -> int: @@ -37,7 +37,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() @@ -54,15 +55,17 @@ def main(): # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) + factor = GenericProjectionFactorCal3_S2( + measurement, camera_noise, symbol('x', i), symbol('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 @@ -74,7 +77,8 @@ 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(symbol('x', 0), poses[0], pose_noise) graph.push_back(factor) From b6ab778e96ff299d0853c4a4e89d834c3cca48b3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 28 Jul 2020 11:05:24 -0400 Subject: [PATCH 14/22] Fix import typo --- python/gtsam_py/python/tests/test_SFMExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam_py/python/tests/test_SFMExample.py index 1425902cf..295bf0b5e 100644 --- a/python/gtsam_py/python/tests/test_SFMExample.py +++ b/python/gtsam_py/python/tests/test_SFMExample.py @@ -15,7 +15,7 @@ import numpy as np import gtsam import gtsam.utils.visual_data_generator as generator from gtsam import symbol -from gtsam.gtsam.noiseModel improt Isotropic, Diagonal +from gtsam.gtsam.noiseModel import Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase From 08da0ab5a607086d605cc1470bff0344c5549542 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 28 Jul 2020 11:16:58 -0400 Subject: [PATCH 15/22] Change to new KeyVector accessor --- .../python/examples/Pose3SLAMExample_initializePose3Chordal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py index 140669c95..2b2c5f991 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -26,7 +26,7 @@ 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().at(0) +firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) # Initializing Pose3 - chordal relaxation" From e9deca590ae3809b824e226a23af6bf1a7332058 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 Jul 2020 13:54:49 -0400 Subject: [PATCH 16/22] Sync with varun's pr --- python/gtsam_py/python/__init__.py | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/python/gtsam_py/python/__init__.py b/python/gtsam_py/python/__init__.py index 9e3e19187..ee00905cf 100644 --- a/python/gtsam_py/python/__init__.py +++ b/python/gtsam_py/python/__init__.py @@ -1,9 +1,23 @@ from .gtsam import * -def Point2(x=0, y=0): - import numpy as np - return np.array([x, y], dtype=float) -def Point3(x=0, y=0, z=0): +def _init(): import numpy as np - return np.array([x, y, z], dtype=float) + + global Point2 # export function + + def Point2(x=0, y=0): + return np.array([x, y], dtype=float) + + global Point3 # export function + + def Point3(x=0, y=0, z=0): + return np.array([x, y, z], dtype=float) + + # for interactive debugging + if __name__ == "__main__": + # we want all definitions accessible + globals().update(locals()) + + +_init() From 3d4a8e16a26907df941af55ad9da0c3173be6bf2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 11:49:31 -0400 Subject: [PATCH 17/22] Moved python files --- python/{gtsam_py/python => gtsam}/__init__.py | 0 .../{gtsam_py/python => gtsam}/examples/DogLegOptimizerExample.py | 0 python/{gtsam_py/python => gtsam}/examples/GPSFactorExample.py | 0 python/{gtsam_py/python => gtsam}/examples/ImuFactorExample.py | 0 .../{gtsam_py/python => gtsam}/examples/ImuFactorISAM2Example.py | 0 python/{gtsam_py/python => gtsam}/examples/OdometryExample.py | 0 .../python => gtsam}/examples/PlanarManipulatorExample.py | 0 python/{gtsam_py/python => gtsam}/examples/PlanarSLAMExample.py | 0 python/{gtsam_py/python => gtsam}/examples/Pose2SLAMExample.py | 0 .../{gtsam_py/python => gtsam}/examples/Pose2SLAMExample_g2o.py | 0 .../{gtsam_py/python => gtsam}/examples/Pose3SLAMExample_g2o.py | 0 .../examples/Pose3SLAMExample_initializePose3Chordal.py | 0 .../{gtsam_py/python => gtsam}/examples/PreintegrationExample.py | 0 python/{gtsam_py/python => gtsam}/examples/README.md | 0 python/{gtsam_py/python => gtsam}/examples/SFMExample.py | 0 python/{gtsam_py/python => gtsam}/examples/SFMdata.py | 0 python/{gtsam_py/python => gtsam}/examples/SimpleRotation.py | 0 python/{gtsam_py/python => gtsam}/examples/VisualISAM2Example.py | 0 python/{gtsam_py/python => gtsam}/examples/VisualISAMExample.py | 0 python/{gtsam_py/python => gtsam}/examples/__init__.py | 0 python/{gtsam_py/python => gtsam}/tests/testScenarioRunner.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Cal3Unified.py | 0 python/{gtsam_py/python => gtsam}/tests/test_FrobeniusFactor.py | 0 .../{gtsam_py/python => gtsam}/tests/test_GaussianFactorGraph.py | 0 python/{gtsam_py/python => gtsam}/tests/test_JacobianFactor.py | 0 python/{gtsam_py/python => gtsam}/tests/test_KalmanFilter.py | 0 python/{gtsam_py/python => gtsam}/tests/test_KarcherMeanFactor.py | 0 .../{gtsam_py/python => gtsam}/tests/test_LocalizationExample.py | 0 .../{gtsam_py/python => gtsam}/tests/test_NonlinearOptimizer.py | 0 python/{gtsam_py/python => gtsam}/tests/test_OdometryExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_PlanarSLAMExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Pose2.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Pose2SLAMExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Pose3.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Pose3SLAMExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_PriorFactor.py | 0 python/{gtsam_py/python => gtsam}/tests/test_SFMExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_SO4.py | 0 python/{gtsam_py/python => gtsam}/tests/test_SOn.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Scenario.py | 0 python/{gtsam_py/python => gtsam}/tests/test_SimpleCamera.py | 0 python/{gtsam_py/python => gtsam}/tests/test_StereoVOExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Triangulation.py | 0 python/{gtsam_py/python => gtsam}/tests/test_Values.py | 0 python/{gtsam_py/python => gtsam}/tests/test_VisualISAMExample.py | 0 python/{gtsam_py/python => gtsam}/tests/test_dataset.py | 0 python/{gtsam_py/python => gtsam}/tests/test_dsf_map.py | 0 python/{gtsam_py/python => gtsam}/tests/test_initialize_pose3.py | 0 python/{gtsam_py/python => gtsam}/tests/test_logging_optimizer.py | 0 python/{gtsam_py/python => gtsam}/utils/__init__.py | 0 python/{gtsam_py/python => gtsam}/utils/circlePose3.py | 0 python/{gtsam_py/python => gtsam}/utils/logging_optimizer.py | 0 python/{gtsam_py/python => gtsam}/utils/plot.py | 0 python/{gtsam_py/python => gtsam}/utils/test_case.py | 0 python/{gtsam_py/python => gtsam}/utils/visual_data_generator.py | 0 python/{gtsam_py/python => gtsam}/utils/visual_isam.py | 0 python/{gtsam_unstable_py/python => gtsam_unstable}/__init__.py | 0 .../python => gtsam_unstable}/examples/FixedLagSmootherExample.py | 0 .../python => gtsam_unstable}/examples/TimeOfArrivalExample.py | 0 .../python => gtsam_unstable}/examples/__init__.py | 0 .../python => gtsam_unstable}/tests/__init__.py | 0 .../tests/test_FixedLagSmootherExample.py | 0 62 files changed, 0 insertions(+), 0 deletions(-) rename python/{gtsam_py/python => gtsam}/__init__.py (100%) rename python/{gtsam_py/python => gtsam}/examples/DogLegOptimizerExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/GPSFactorExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/ImuFactorExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/ImuFactorISAM2Example.py (100%) rename python/{gtsam_py/python => gtsam}/examples/OdometryExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/PlanarManipulatorExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/PlanarSLAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/Pose2SLAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/Pose2SLAMExample_g2o.py (100%) rename python/{gtsam_py/python => gtsam}/examples/Pose3SLAMExample_g2o.py (100%) rename python/{gtsam_py/python => gtsam}/examples/Pose3SLAMExample_initializePose3Chordal.py (100%) rename python/{gtsam_py/python => gtsam}/examples/PreintegrationExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/README.md (100%) rename python/{gtsam_py/python => gtsam}/examples/SFMExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/SFMdata.py (100%) rename python/{gtsam_py/python => gtsam}/examples/SimpleRotation.py (100%) rename python/{gtsam_py/python => gtsam}/examples/VisualISAM2Example.py (100%) rename python/{gtsam_py/python => gtsam}/examples/VisualISAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/examples/__init__.py (100%) rename python/{gtsam_py/python => gtsam}/tests/testScenarioRunner.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Cal3Unified.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_FrobeniusFactor.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_GaussianFactorGraph.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_JacobianFactor.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_KalmanFilter.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_KarcherMeanFactor.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_LocalizationExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_NonlinearOptimizer.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_OdometryExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_PlanarSLAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Pose2.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Pose2SLAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Pose3.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Pose3SLAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_PriorFactor.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_SFMExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_SO4.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_SOn.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Scenario.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_SimpleCamera.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_StereoVOExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Triangulation.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_Values.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_VisualISAMExample.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_dataset.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_dsf_map.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_initialize_pose3.py (100%) rename python/{gtsam_py/python => gtsam}/tests/test_logging_optimizer.py (100%) rename python/{gtsam_py/python => gtsam}/utils/__init__.py (100%) rename python/{gtsam_py/python => gtsam}/utils/circlePose3.py (100%) rename python/{gtsam_py/python => gtsam}/utils/logging_optimizer.py (100%) rename python/{gtsam_py/python => gtsam}/utils/plot.py (100%) rename python/{gtsam_py/python => gtsam}/utils/test_case.py (100%) rename python/{gtsam_py/python => gtsam}/utils/visual_data_generator.py (100%) rename python/{gtsam_py/python => gtsam}/utils/visual_isam.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/__init__.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/examples/FixedLagSmootherExample.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/examples/TimeOfArrivalExample.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/examples/__init__.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/tests/__init__.py (100%) rename python/{gtsam_unstable_py/python => gtsam_unstable}/tests/test_FixedLagSmootherExample.py (100%) diff --git a/python/gtsam_py/python/__init__.py b/python/gtsam/__init__.py similarity index 100% rename from python/gtsam_py/python/__init__.py rename to python/gtsam/__init__.py diff --git a/python/gtsam_py/python/examples/DogLegOptimizerExample.py b/python/gtsam/examples/DogLegOptimizerExample.py similarity index 100% rename from python/gtsam_py/python/examples/DogLegOptimizerExample.py rename to python/gtsam/examples/DogLegOptimizerExample.py diff --git a/python/gtsam_py/python/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py similarity index 100% rename from python/gtsam_py/python/examples/GPSFactorExample.py rename to python/gtsam/examples/GPSFactorExample.py diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py similarity index 100% rename from python/gtsam_py/python/examples/ImuFactorExample.py rename to python/gtsam/examples/ImuFactorExample.py diff --git a/python/gtsam_py/python/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py similarity index 100% rename from python/gtsam_py/python/examples/ImuFactorISAM2Example.py rename to python/gtsam/examples/ImuFactorISAM2Example.py diff --git a/python/gtsam_py/python/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py similarity index 100% rename from python/gtsam_py/python/examples/OdometryExample.py rename to python/gtsam/examples/OdometryExample.py diff --git a/python/gtsam_py/python/examples/PlanarManipulatorExample.py b/python/gtsam/examples/PlanarManipulatorExample.py similarity index 100% rename from python/gtsam_py/python/examples/PlanarManipulatorExample.py rename to python/gtsam/examples/PlanarManipulatorExample.py diff --git a/python/gtsam_py/python/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py similarity index 100% rename from python/gtsam_py/python/examples/PlanarSLAMExample.py rename to python/gtsam/examples/PlanarSLAMExample.py diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py similarity index 100% rename from python/gtsam_py/python/examples/Pose2SLAMExample.py rename to python/gtsam/examples/Pose2SLAMExample.py diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py similarity index 100% rename from python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py rename to python/gtsam/examples/Pose2SLAMExample_g2o.py diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py similarity index 100% rename from python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py rename to python/gtsam/examples/Pose3SLAMExample_g2o.py diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py similarity index 100% rename from python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py rename to python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py similarity index 100% rename from python/gtsam_py/python/examples/PreintegrationExample.py rename to python/gtsam/examples/PreintegrationExample.py diff --git a/python/gtsam_py/python/examples/README.md b/python/gtsam/examples/README.md similarity index 100% rename from python/gtsam_py/python/examples/README.md rename to python/gtsam/examples/README.md diff --git a/python/gtsam_py/python/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py similarity index 100% rename from python/gtsam_py/python/examples/SFMExample.py rename to python/gtsam/examples/SFMExample.py diff --git a/python/gtsam_py/python/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py similarity index 100% rename from python/gtsam_py/python/examples/SFMdata.py rename to python/gtsam/examples/SFMdata.py diff --git a/python/gtsam_py/python/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py similarity index 100% rename from python/gtsam_py/python/examples/SimpleRotation.py rename to python/gtsam/examples/SimpleRotation.py diff --git a/python/gtsam_py/python/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py similarity index 100% rename from python/gtsam_py/python/examples/VisualISAM2Example.py rename to python/gtsam/examples/VisualISAM2Example.py diff --git a/python/gtsam_py/python/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py similarity index 100% rename from python/gtsam_py/python/examples/VisualISAMExample.py rename to python/gtsam/examples/VisualISAMExample.py diff --git a/python/gtsam_py/python/examples/__init__.py b/python/gtsam/examples/__init__.py similarity index 100% rename from python/gtsam_py/python/examples/__init__.py rename to python/gtsam/examples/__init__.py diff --git a/python/gtsam_py/python/tests/testScenarioRunner.py b/python/gtsam/tests/testScenarioRunner.py similarity index 100% rename from python/gtsam_py/python/tests/testScenarioRunner.py rename to python/gtsam/tests/testScenarioRunner.py diff --git a/python/gtsam_py/python/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py similarity index 100% rename from python/gtsam_py/python/tests/test_Cal3Unified.py rename to python/gtsam/tests/test_Cal3Unified.py diff --git a/python/gtsam_py/python/tests/test_FrobeniusFactor.py b/python/gtsam/tests/test_FrobeniusFactor.py similarity index 100% rename from python/gtsam_py/python/tests/test_FrobeniusFactor.py rename to python/gtsam/tests/test_FrobeniusFactor.py diff --git a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py similarity index 100% rename from python/gtsam_py/python/tests/test_GaussianFactorGraph.py rename to python/gtsam/tests/test_GaussianFactorGraph.py diff --git a/python/gtsam_py/python/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py similarity index 100% rename from python/gtsam_py/python/tests/test_JacobianFactor.py rename to python/gtsam/tests/test_JacobianFactor.py diff --git a/python/gtsam_py/python/tests/test_KalmanFilter.py b/python/gtsam/tests/test_KalmanFilter.py similarity index 100% rename from python/gtsam_py/python/tests/test_KalmanFilter.py rename to python/gtsam/tests/test_KalmanFilter.py diff --git a/python/gtsam_py/python/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py similarity index 100% rename from python/gtsam_py/python/tests/test_KarcherMeanFactor.py rename to python/gtsam/tests/test_KarcherMeanFactor.py diff --git a/python/gtsam_py/python/tests/test_LocalizationExample.py b/python/gtsam/tests/test_LocalizationExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_LocalizationExample.py rename to python/gtsam/tests/test_LocalizationExample.py diff --git a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py similarity index 100% rename from python/gtsam_py/python/tests/test_NonlinearOptimizer.py rename to python/gtsam/tests/test_NonlinearOptimizer.py diff --git a/python/gtsam_py/python/tests/test_OdometryExample.py b/python/gtsam/tests/test_OdometryExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_OdometryExample.py rename to python/gtsam/tests/test_OdometryExample.py diff --git a/python/gtsam_py/python/tests/test_PlanarSLAMExample.py b/python/gtsam/tests/test_PlanarSLAMExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_PlanarSLAMExample.py rename to python/gtsam/tests/test_PlanarSLAMExample.py diff --git a/python/gtsam_py/python/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py similarity index 100% rename from python/gtsam_py/python/tests/test_Pose2.py rename to python/gtsam/tests/test_Pose2.py diff --git a/python/gtsam_py/python/tests/test_Pose2SLAMExample.py b/python/gtsam/tests/test_Pose2SLAMExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_Pose2SLAMExample.py rename to python/gtsam/tests/test_Pose2SLAMExample.py diff --git a/python/gtsam_py/python/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py similarity index 100% rename from python/gtsam_py/python/tests/test_Pose3.py rename to python/gtsam/tests/test_Pose3.py diff --git a/python/gtsam_py/python/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_Pose3SLAMExample.py rename to python/gtsam/tests/test_Pose3SLAMExample.py diff --git a/python/gtsam_py/python/tests/test_PriorFactor.py b/python/gtsam/tests/test_PriorFactor.py similarity index 100% rename from python/gtsam_py/python/tests/test_PriorFactor.py rename to python/gtsam/tests/test_PriorFactor.py diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_SFMExample.py rename to python/gtsam/tests/test_SFMExample.py diff --git a/python/gtsam_py/python/tests/test_SO4.py b/python/gtsam/tests/test_SO4.py similarity index 100% rename from python/gtsam_py/python/tests/test_SO4.py rename to python/gtsam/tests/test_SO4.py diff --git a/python/gtsam_py/python/tests/test_SOn.py b/python/gtsam/tests/test_SOn.py similarity index 100% rename from python/gtsam_py/python/tests/test_SOn.py rename to python/gtsam/tests/test_SOn.py diff --git a/python/gtsam_py/python/tests/test_Scenario.py b/python/gtsam/tests/test_Scenario.py similarity index 100% rename from python/gtsam_py/python/tests/test_Scenario.py rename to python/gtsam/tests/test_Scenario.py diff --git a/python/gtsam_py/python/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py similarity index 100% rename from python/gtsam_py/python/tests/test_SimpleCamera.py rename to python/gtsam/tests/test_SimpleCamera.py diff --git a/python/gtsam_py/python/tests/test_StereoVOExample.py b/python/gtsam/tests/test_StereoVOExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_StereoVOExample.py rename to python/gtsam/tests/test_StereoVOExample.py diff --git a/python/gtsam_py/python/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py similarity index 100% rename from python/gtsam_py/python/tests/test_Triangulation.py rename to python/gtsam/tests/test_Triangulation.py diff --git a/python/gtsam_py/python/tests/test_Values.py b/python/gtsam/tests/test_Values.py similarity index 100% rename from python/gtsam_py/python/tests/test_Values.py rename to python/gtsam/tests/test_Values.py diff --git a/python/gtsam_py/python/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py similarity index 100% rename from python/gtsam_py/python/tests/test_VisualISAMExample.py rename to python/gtsam/tests/test_VisualISAMExample.py diff --git a/python/gtsam_py/python/tests/test_dataset.py b/python/gtsam/tests/test_dataset.py similarity index 100% rename from python/gtsam_py/python/tests/test_dataset.py rename to python/gtsam/tests/test_dataset.py diff --git a/python/gtsam_py/python/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py similarity index 100% rename from python/gtsam_py/python/tests/test_dsf_map.py rename to python/gtsam/tests/test_dsf_map.py diff --git a/python/gtsam_py/python/tests/test_initialize_pose3.py b/python/gtsam/tests/test_initialize_pose3.py similarity index 100% rename from python/gtsam_py/python/tests/test_initialize_pose3.py rename to python/gtsam/tests/test_initialize_pose3.py diff --git a/python/gtsam_py/python/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py similarity index 100% rename from python/gtsam_py/python/tests/test_logging_optimizer.py rename to python/gtsam/tests/test_logging_optimizer.py diff --git a/python/gtsam_py/python/utils/__init__.py b/python/gtsam/utils/__init__.py similarity index 100% rename from python/gtsam_py/python/utils/__init__.py rename to python/gtsam/utils/__init__.py diff --git a/python/gtsam_py/python/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py similarity index 100% rename from python/gtsam_py/python/utils/circlePose3.py rename to python/gtsam/utils/circlePose3.py diff --git a/python/gtsam_py/python/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py similarity index 100% rename from python/gtsam_py/python/utils/logging_optimizer.py rename to python/gtsam/utils/logging_optimizer.py diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam/utils/plot.py similarity index 100% rename from python/gtsam_py/python/utils/plot.py rename to python/gtsam/utils/plot.py diff --git a/python/gtsam_py/python/utils/test_case.py b/python/gtsam/utils/test_case.py similarity index 100% rename from python/gtsam_py/python/utils/test_case.py rename to python/gtsam/utils/test_case.py diff --git a/python/gtsam_py/python/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py similarity index 100% rename from python/gtsam_py/python/utils/visual_data_generator.py rename to python/gtsam/utils/visual_data_generator.py diff --git a/python/gtsam_py/python/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py similarity index 100% rename from python/gtsam_py/python/utils/visual_isam.py rename to python/gtsam/utils/visual_isam.py diff --git a/python/gtsam_unstable_py/python/__init__.py b/python/gtsam_unstable/__init__.py similarity index 100% rename from python/gtsam_unstable_py/python/__init__.py rename to python/gtsam_unstable/__init__.py diff --git a/python/gtsam_unstable_py/python/examples/FixedLagSmootherExample.py b/python/gtsam_unstable/examples/FixedLagSmootherExample.py similarity index 100% rename from python/gtsam_unstable_py/python/examples/FixedLagSmootherExample.py rename to python/gtsam_unstable/examples/FixedLagSmootherExample.py diff --git a/python/gtsam_unstable_py/python/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py similarity index 100% rename from python/gtsam_unstable_py/python/examples/TimeOfArrivalExample.py rename to python/gtsam_unstable/examples/TimeOfArrivalExample.py diff --git a/python/gtsam_unstable_py/python/examples/__init__.py b/python/gtsam_unstable/examples/__init__.py similarity index 100% rename from python/gtsam_unstable_py/python/examples/__init__.py rename to python/gtsam_unstable/examples/__init__.py diff --git a/python/gtsam_unstable_py/python/tests/__init__.py b/python/gtsam_unstable/tests/__init__.py similarity index 100% rename from python/gtsam_unstable_py/python/tests/__init__.py rename to python/gtsam_unstable/tests/__init__.py diff --git a/python/gtsam_unstable_py/python/tests/test_FixedLagSmootherExample.py b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py similarity index 100% rename from python/gtsam_unstable_py/python/tests/test_FixedLagSmootherExample.py rename to python/gtsam_unstable/tests/test_FixedLagSmootherExample.py From fda79057e4e764e3d5028202c1af0571bc2ddaae Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 12:16:25 -0400 Subject: [PATCH 18/22] import from is working --- python/gtsam/__init__.py | 1 - python/gtsam/examples/ImuFactorExample.py | 2 +- python/gtsam/examples/ImuFactorISAM2Example.py | 2 +- python/gtsam/examples/PlanarSLAMExample.py | 2 +- python/gtsam/examples/Pose2SLAMExample.py | 1 - python/gtsam/examples/SimpleRotation.py | 2 +- python/gtsam/examples/VisualISAM2Example.py | 2 +- python/gtsam/imuBias.py | 1 + python/gtsam/noiseModel.py | 1 + python/gtsam/symbol_shorthand.py | 1 + python/gtsam/tests/test_GaussianFactorGraph.py | 2 +- python/gtsam/tests/test_SFMExample.py | 2 +- 12 files changed, 10 insertions(+), 9 deletions(-) create mode 100644 python/gtsam/imuBias.py create mode 100644 python/gtsam/noiseModel.py create mode 100644 python/gtsam/symbol_shorthand.py diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index ee00905cf..555808d5a 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,6 +1,5 @@ from .gtsam import * - def _init(): import numpy as np diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index f9ff44738..eec7c5ebd 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -18,7 +18,7 @@ import math import gtsam import matplotlib.pyplot as plt import numpy as np -from gtsam.gtsam.symbol_shorthand import B, V, X +from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D diff --git a/python/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py index 6556eb554..e0999b25b 100644 --- a/python/gtsam/examples/ImuFactorISAM2Example.py +++ b/python/gtsam/examples/ImuFactorISAM2Example.py @@ -16,7 +16,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) -from gtsam.gtsam.symbol_shorthand import B, V, X +from gtsam.symbol_shorthand import B, V, X from gtsam.utils import plot from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py index ba0529479..5ffdf048d 100644 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ b/python/gtsam/examples/PlanarSLAMExample.py @@ -16,7 +16,7 @@ from __future__ import print_function import numpy as np import gtsam -from gtsam.gtsam.symbol_shorthand import X, L +from gtsam.symbol_shorthand import X, L # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py index 686da4db3..2569f0953 100644 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ b/python/gtsam/examples/Pose2SLAMExample.py @@ -27,7 +27,6 @@ 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)) diff --git a/python/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py index de8ab07c3..0fef261f8 100644 --- a/python/gtsam/examples/SimpleRotation.py +++ b/python/gtsam/examples/SimpleRotation.py @@ -12,7 +12,7 @@ a single variable with a single factor. import numpy as np import gtsam -from gtsam.gtsam.symbol_shorthand import X +from gtsam.symbol_shorthand import X def main(): """ diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index e4c0c8b8d..bacf510ec 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -17,7 +17,7 @@ import gtsam import gtsam.utils.plot as gtsam_plot import matplotlib.pyplot as plt import numpy as np -from gtsam.gtsam.symbol_shorthand import L, X +from gtsam.symbol_shorthand import L, X from gtsam.examples import SFMdata from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 diff --git a/python/gtsam/imuBias.py b/python/gtsam/imuBias.py new file mode 100644 index 000000000..1cb367b9f --- /dev/null +++ b/python/gtsam/imuBias.py @@ -0,0 +1 @@ +from .gtsam.imuBias import * diff --git a/python/gtsam/noiseModel.py b/python/gtsam/noiseModel.py new file mode 100644 index 000000000..9b1929a8e --- /dev/null +++ b/python/gtsam/noiseModel.py @@ -0,0 +1 @@ +from .gtsam.noiseModel import * \ No newline at end of file diff --git a/python/gtsam/symbol_shorthand.py b/python/gtsam/symbol_shorthand.py new file mode 100644 index 000000000..956ed693a --- /dev/null +++ b/python/gtsam/symbol_shorthand.py @@ -0,0 +1 @@ +from .gtsam.symbol_shorthand import * \ No newline at end of file diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index d520b0fa4..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.gtsam.symbol_shorthand import X +from gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase diff --git a/python/gtsam/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py index 295bf0b5e..5e787f5ae 100644 --- a/python/gtsam/tests/test_SFMExample.py +++ b/python/gtsam/tests/test_SFMExample.py @@ -15,7 +15,7 @@ import numpy as np import gtsam import gtsam.utils.visual_data_generator as generator from gtsam import symbol -from gtsam.gtsam.noiseModel import Isotropic, Diagonal +from gtsam.noiseModel import Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase From 1cabd2000fcd580625395d02f926e0a216f95f39 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 12:39:04 -0400 Subject: [PATCH 19/22] Sync with new_wrapper develop branch --- python/gtsam/__init__.py | 5 ++++ .../gtsam/examples/ImuFactorISAM2Example.py | 2 +- python/gtsam/examples/README.md | 12 +++++----- python/gtsam/examples/VisualISAMExample.py | 17 +++++--------- python/gtsam/tests/test_SFMExample.py | 20 ++++++++-------- python/gtsam/utils/plot.py | 23 ++++++++----------- 6 files changed, 37 insertions(+), 42 deletions(-) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 555808d5a..e6fd8c9c8 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,16 +1,21 @@ from .gtsam import * + def _init(): + """This function is to add shims for the long-gone Point2 and Point3 types""" + import numpy as np global Point2 # export function def Point2(x=0, y=0): + """Shim for the deleted Point2 type.""" return np.array([x, y], dtype=float) global Point3 # export function def Point3(x=0, y=0, z=0): + """Shim for the deleted Point3 type.""" return np.array([x, y, z], dtype=float) # for interactive debugging diff --git a/python/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py index e0999b25b..98f6218ea 100644 --- a/python/gtsam/examples/ImuFactorISAM2Example.py +++ b/python/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ iSAM2 example with ImuFactor. -Author: Frank Dellaert, Varun Agrawal +Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal """ # pylint: disable=invalid-name, E1101 diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 46b87f079..e998e4dcd 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -5,8 +5,8 @@ | CameraResectioning | | | CreateSFMExampleData | | | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | -| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through Python | -| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | | ImuFactorExample2 | X | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | @@ -20,7 +20,7 @@ | Pose2SLAMExample_g2o | X | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not exposed through Python | +| Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | | Pose3SLAMExample_changeKeys | | @@ -42,11 +42,11 @@ | 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/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 7d562c9b4..f99d3f3e6 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -19,12 +19,7 @@ from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, PinholeCameraCal3_S2, Values, Point3) - - -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(name, index) - +from gtsam.symbol_shorthand import X, L def main(): """ @@ -60,7 +55,7 @@ def main(): for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( - measurement, camera_noise, symbol('x', i), symbol('l', j), K) + measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -69,7 +64,7 @@ def main(): initial_xi = pose.compose(noise) # Add an initial guess for the current pose - initial_estimate.insert(symbol('x', i), initial_xi) + initial_estimate.insert(X(i), initial_xi) # If this is the first iteration, add a prior on the first pose to set the coordinate frame # and a prior on the first landmark to set the scale @@ -79,12 +74,12 @@ def main(): # 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])) - factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + 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) - factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) # Add initial guesses to all observed landmarks @@ -92,7 +87,7 @@ def main(): for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth initial_lj = points[j] + noise - initial_estimate.insert(symbol('l', j), initial_lj) + 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/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py index 5e787f5ae..47a3cbe3e 100644 --- a/python/gtsam/tests/test_SFMExample.py +++ b/python/gtsam/tests/test_SFMExample.py @@ -17,7 +17,7 @@ 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): @@ -41,23 +41,23 @@ class TestSFMExample(GtsamTestCase): j = data.J[i][k] graph.add(gtsam.GenericProjectionFactorCal3_S2( data.Z[i][k], measurementNoise, - symbol('x', i), symbol('p', j), data.K)) + X(i), P(j), data.K)) posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) - graph.add(gtsam.PriorFactorPose3(symbol('x', 0), + graph.add(gtsam.PriorFactorPose3(X(0), truth.cameras[0].pose(), posePriorNoise)) pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) - graph.add(gtsam.PriorFactorPoint3(symbol('p', 0), + 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('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('p', j), point_j) + initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) @@ -67,16 +67,16 @@ class TestSFMExample(GtsamTestCase): # Marginalization marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(symbol('p', 0)) - marginals.marginalCovariance(symbol('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('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('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/utils/plot.py b/python/gtsam/utils/plot.py index 90f5fe5e7..8b34ad2bf 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) + origin = t # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -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(len(keys)): + for key in keys: try: - key = keys[i] point = values.atPoint3(key) if marginals is not None: covariance = marginals.marginalCovariance(key) @@ -321,17 +320,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, """ pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) - lastIndex = None + lastKey = None - for i in range(len(keys)): - key = keys[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[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[lastIndex] + if lastKey is not None: try: lastPose = pose3Values.atPose3(lastKey) if marginals: @@ -390,10 +386,9 @@ def plot_incremental_trajectory(fignum, values, start=0, pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) - for i in range(start, len(keys)): - key = keys[i] + for key in keys[start:]: if values.exists(key): - pose_i = values.atPose3(keys[i]) + pose_i = values.atPose3(key) plot_pose3(fignum, pose_i, scale) # Update the plot space to encompass all plotted points From b02cc3f7e3aba926392d29e4b734e05ef2d4c6ee Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 12:42:09 -0400 Subject: [PATCH 20/22] remove function import --- python/gtsam/utils/circlePose3.py | 5 +++-- python/gtsam/utils/visual_data_generator.py | 7 ++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index 37e7c5568..e1def9427 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -1,6 +1,7 @@ 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'): @@ -25,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/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 24404ca03..c6e5b8c63 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,7 +1,8 @@ from __future__ import print_function import numpy as np -from math import pi, cos, sin +import math +from math import pi import gtsam from gtsam import PinholeCameraCal3_S2 @@ -85,7 +86,7 @@ def generate_data(options): r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) + truth.points[j] = gtsam.Point3(r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), @@ -99,7 +100,7 @@ def generate_data(options): r = 40 for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras - t = gtsam.Point3(r * cos(theta), r * sin(theta), height) + t = gtsam.Point3(r * math.cos(theta), r * math.sin(theta), height) truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, gtsam.Point3(0, 0, 0), gtsam.Point3(0, 0, 1), From c0486d39a0d7ef05e421f787a77008248b2d64ff Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 12:47:08 -0400 Subject: [PATCH 21/22] Import classes used more than once --- python/gtsam/utils/visual_data_generator.py | 32 ++++++++++----------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index c6e5b8c63..32ccbc8fa 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -4,7 +4,7 @@ import numpy as np import math from math import pi import gtsam -from gtsam import PinholeCameraCal3_S2 +from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 class Options: @@ -12,7 +12,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): + def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -29,10 +29,10 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + 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) @@ -54,11 +54,11 @@ class Data: class NoiseModels: pass - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] - self.odometry = [gtsam.Pose3()] * nrCameras + self.odometry = [Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() @@ -75,7 +75,7 @@ class Data: def generate_data(options): """ Generate ground-truth and measurement data. """ - K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) nrPoints = 3 if options.triangle else 8 truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) @@ -86,13 +86,13 @@ def generate_data(options): r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = gtsam.Point3(r * math.cos(theta), r * math.sin(theta), 0) + truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ - gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), - gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), - gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), - gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) + Point3(10, 10, 10), Point3(-10, 10, 10), + Point3(-10, -10, 10), Point3(10, -10, 10), + Point3(10, 10, -10), Point3(-10, 10, -10), + Point3(-10, -10, -10), Point3(10, -10, -10) ] # Create camera cameras on a circle around the triangle @@ -100,10 +100,10 @@ def generate_data(options): r = 40 for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras - t = gtsam.Point3(r * math.cos(theta), r * math.sin(theta), height) + t = Point3(r * math.cos(theta), r * math.sin(theta), height) truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, - gtsam.Point3(0, 0, 0), - gtsam.Point3(0, 0, 1), + Point3(0, 0, 0), + Point3(0, 0, 1), truth.K) # Create measurements for j in range(nrPoints): From fa97bc23727c63a42810629b6235e5c1cf250ae8 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 31 Jul 2020 12:48:50 -0400 Subject: [PATCH 22/22] Newline --- python/gtsam/utils/plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 8b34ad2bf..0267da8c3 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -398,4 +398,4 @@ def plot_incremental_trajectory(fignum, values, start=0, set_axes_equal(fignum) # Pause for a fixed amount of seconds - plt.pause(time_interval) \ No newline at end of file + plt.pause(time_interval)