diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index d7be83bab..0e01766e7 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -14,25 +14,18 @@ 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 = int(gtsam.symbol(ord('b'), 0)) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) +BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 80f573d12..0d98f08fe 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -8,27 +8,19 @@ 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 +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) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) +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 def vector3(x, y, z): @@ -115,7 +107,7 @@ def IMU_example(): newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors - biasKey = gtsam.symbol(ord('b'), 0) + biasKey = B(0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index fb038d043..c84f0f834 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) from __future__ import print_function -import numpy as np - import gtsam +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X # 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(ord('x'), 1) -X2 = gtsam.symbol(ord('x'), 2) -X3 = gtsam.symbol(ord('x'), 3) -L1 = gtsam.symbol(ord('l'), 4) -L2 = gtsam.symbol(ord('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/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 83d543310..09114370d 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -82,7 +82,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/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 25686c762..3c1a54f7b 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -65,7 +65,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/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 35ec4f66d..99bce00e2 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,7 +1,7 @@ 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(ord('b'), 0))` +Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing. # Porting Progress diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index 7b664f2d0..e02def2f9 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -10,24 +10,20 @@ 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 - -import gtsam +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, Point3, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, + NonlinearFactorGraph, PinholeCameraCal3_S2, 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(ord(name), index) - - def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -74,7 +70,7 @@ def main(): # Add a prior on pose x1. This indirectly specifies where the origin is. # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - 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 @@ -83,14 +79,14 @@ def main(): 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 +95,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) + initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py index 6e9b1320b..4e82d3778 100644 --- a/cython/gtsam/examples/SimpleRotation.py +++ b/cython/gtsam/examples/SimpleRotation.py @@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on a single variable with a single factor. """ -import numpy as np import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X def main(): @@ -33,7 +34,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(ord('x'), 1) + key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) """ diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index c0f4fbd94..49e6ca95c 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -13,23 +13,14 @@ 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 import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata - - -def X(i): - """Create key for pose i.""" - return int(gtsam.symbol(ord('x'), i)) - - -def L(j): - """Create key for landmark j.""" - return int(gtsam.symbol(ord('l'), j)) +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def visual_ISAM2_plot(result): diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index b7b83e2a1..5cc37867b 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -19,11 +19,8 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, PinholeCameraCal3_S2, Values) - - -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(ord(name), index) +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X def main(): @@ -58,7 +55,7 @@ def main(): # 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, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -66,7 +63,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 @@ -75,12 +72,12 @@ def main(): 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(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 @@ -88,7 +85,7 @@ def main(): for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth initial_lj = points[j].vector() + noise - initial_estimate.insert(symbol('l', j), Point3(initial_lj)) + initial_estimate.insert(L(j), Point3(initial_lj)) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/cython/gtsam/tests/test_GaussianFactorGraph.py b/cython/gtsam/tests/test_GaussianFactorGraph.py index 2e7875af0..983825d8b 100644 --- a/cython/gtsam/tests/test_GaussianFactorGraph.py +++ b/cython/gtsam/tests/test_GaussianFactorGraph.py @@ -15,17 +15,18 @@ 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 = gtsam.symbol(ord('x'), 0) - x1 = gtsam.symbol(ord('x'), 1) - x2 = gtsam.symbol(ord('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/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py index b2ede57bf..73ddcb050 100644 --- a/cython/gtsam/tests/test_dsf_map.py +++ b/cython/gtsam/tests/test_dsf_map.py @@ -33,7 +33,7 @@ class TestDSFMap(GtsamTestCase): # testing the merge feature of dsf dsf.merge(pair1, pair2) - self.assertEquals(key(dsf.find(pair1)), key(dsf.find(pair2))) + self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) if __name__ == '__main__': diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index bd6b5ab60..b67444fc1 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -279,7 +279,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): marginals (gtsam.Marginals): Marginalized probability values of the estimation. Used to plot uncertainty bounds. """ - pose3Values = gtsam.allPose3s(values) + pose3Values = gtsam.utilities_allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) lastIndex = None diff --git a/gtsam.h b/gtsam.h index 092355875..c42c4582a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1977,6 +1977,35 @@ size_t symbol(char chr, size_t index); char symbolChr(size_t key); size_t symbolIndex(size_t key); +namespace symbol_shorthand { + size_t A(size_t j); + size_t B(size_t j); + size_t C(size_t j); + size_t D(size_t j); + size_t E(size_t j); + size_t F(size_t j); + size_t G(size_t j); + size_t H(size_t j); + size_t I(size_t j); + size_t J(size_t j); + size_t K(size_t j); + size_t L(size_t j); + size_t M(size_t j); + size_t N(size_t j); + size_t O(size_t j); + size_t P(size_t j); + size_t Q(size_t j); + size_t R(size_t j); + size_t S(size_t j); + size_t T(size_t j); + size_t U(size_t j); + size_t V(size_t j); + size_t W(size_t j); + size_t X(size_t j); + size_t Y(size_t j); + size_t Z(size_t j); +}///\namespace symbol + // Default keyformatter void PrintKeyList (const gtsam::KeyList& keys); void PrintKeyList (const gtsam::KeyList& keys, string s); @@ -2141,6 +2170,7 @@ class Values { void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, Vector vector); void insert(size_t j, Matrix matrix); @@ -2158,10 +2188,11 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 9742733f7..099cefa70 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -54,7 +54,16 @@ struct GlobalFunction: public FullyOverloadedFunction { // function name in Cython pxd std::string pxdName() const { return "pxd_" + pyRename(name_); } // function name in Python pyx - std::string pyxName() const { return pyRename(name_); } + std::string pyxName() const { + std::string result = ""; + for(size_t i=0; i= 1) { + result += (overloads[0].namespaces_[i] + "_"); + } + } + result += pyRename(name_); + return result; + } // emit cython wrapper void emit_cython_pxd(FileWriter& pxdFile) const;