Merge pull request #347 from borglab/feature/wrap-symbol-shorthand

Wrapper Updates for Global Functions

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))
+BIAS_KEY = 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)
 
 
 np.set_printoptions(precision=3, suppress=True)

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)
-
+from gtsam import symbol_shorthand_B as B
-
+from gtsam import symbol_shorthand_V as V
-def X(key):
+from gtsam import symbol_shorthand_X as X
-    """Create symbol for pose key."""
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-    return gtsam.symbol(ord('x'), key)
-
-
-def V(key):
-    """Create symbol for velocity key."""
-    return gtsam.symbol(ord('v'), key)
 
 
 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)

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)
+X1 = X(1)
-X2 = gtsam.symbol(ord('x'), 2)
+X2 = X(2)
-X3 = gtsam.symbol(ord('x'), 3)
+X3 = X(3)
-L1 = gtsam.symbol(ord('l'), 4)
+L1 = L(4)
-L2 = gtsam.symbol(ord('l'), 5)
+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))

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()

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()

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
 

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
-
+from gtsam import symbol_shorthand_L as L
-import gtsam
+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,
+                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         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

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)
 
     """

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
-
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-
-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))
 
 
 def visual_ISAM2_plot(result):

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)
-
+from gtsam import symbol_shorthand_L as L
-
+from gtsam import symbol_shorthand_X as X
-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():
@@ -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)

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)
+    x0 = X(0)
-    x1 = gtsam.symbol(ord('x'), 1)
+    x1 = X(1)
-    x2 = gtsam.symbol(ord('x'), 2)
+    x2 = X(2)
     
     BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
     PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))

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__':

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
 

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<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
   T at(size_t j);
 
   /// version for double

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<overloads[0].namespaces_.size(); i++){
+      if (i >= 1) {
+        result += (overloads[0].namespaces_[i] + "_");
+      }
+    }
+    result += pyRename(name_);
+    return result;
+  }
 
   // emit cython wrapper
   void emit_cython_pxd(FileWriter& pxdFile) const;