Merged in feature/more_python (pull request #209)

Feature/more_python

python/gtsam_examples/VisualISAM2Example.py

@@ -9,13 +9,17 @@ import gtsam
 from gtsam_examples import SFMdata
 import gtsam_utils
 
+# shorthand symbols:
+X = lambda i: int(gtsam.Symbol('x', i))
+L = lambda j: int(gtsam.Symbol('l', j))
+
 def visual_ISAM2_plot(poses, points, result):
     # VisualISAMPlot plots current state of ISAM2 object
     # Author: Ellon Paiva
     # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
 
     # Declare an id for the figure
-    fignum = 0;
+    fignum = 0
 
     fig = plt.figure(fignum)
     ax = fig.gca(projection='3d')
@@ -23,28 +27,26 @@ def visual_ISAM2_plot(poses, points, result):
 
     # Plot points
     # Can't use data because current frame might not see all points
-    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow
+    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow
-    # gtsam.plot3DPoints(result, [], marginals);
+    # gtsam.plot3DPoints(result, [], marginals)
-    gtsam_utils.plot3DPoints(fignum, result, 'rx');
+    gtsam_utils.plot3DPoints(fignum, result, 'rx')
 
     # Plot cameras
-    M = 0;
+    i = 0
-    while result.exists(int(gtsam.Symbol('x',M))):
+    while result.exists(X(i)):
-        ii = int(gtsam.Symbol('x',M));
+        pose_i = result.atPose3(X(i))
-        pose_i = result.pose3_at(ii);
+        gtsam_utils.plotPose3(fignum, pose_i, 10)
-        gtsam_utils.plotPose3(fignum, pose_i, 10);
+        i += 1
-        
-        M = M + 1;
 
     # draw
     ax.set_xlim3d(-40, 40)
     ax.set_ylim3d(-40, 40)
     ax.set_zlim3d(-40, 40)
-    plt.ion()
+    plt.pause(1)
-    plt.show()
-    plt.draw()
 
 def visual_ISAM2_example():
+    plt.ion()
+
     # Define the camera calibration parameters
     K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
@@ -78,11 +80,11 @@ def visual_ISAM2_example():
         for j, point in enumerate(points):
             camera = gtsam.PinholeCameraCal3_S2(pose, K)
             measurement = camera.project(point)
-            graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K))
+            graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K))
 
         # Add an initial guess for the current pose
         # Intentionally initialize the variables off from the ground truth
-        initialEstimate.insert(int(gtsam.Symbol('x', i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
+        initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
 
         # 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
@@ -91,16 +93,16 @@ def visual_ISAM2_example():
         if(i == 0):
             # Add a prior on pose x0
             poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-            graph.push_back(gtsam.PriorFactorPose3(int(gtsam.Symbol('x', 0)), poses[0], poseNoise))
+            graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise))
 
             # Add a prior on landmark l0
             pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
-            graph.push_back(gtsam.PriorFactorPoint3(int(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph
+            graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise))  # add directly to graph
             
             # Add initial guesses to all observed landmarks
             # Intentionally initialize the variables off from the ground truth
             for j, point in enumerate(points):
-                initialEstimate.insert(int(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15));
+                initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15))
         else:
             # Update iSAM with the new factors
             isam.update(graph, initialEstimate)
@@ -108,23 +110,23 @@ def visual_ISAM2_example():
             # If accuracy is desired at the expense of time, update(*) can be called additional times
             # to perform multiple optimizer iterations every step.
             isam.update()
-            currentEstimate = isam.calculate_estimate();
+            currentEstimate = isam.calculate_estimate()
             print("****************************************************")
             print("Frame", i, ":")
             for j in range(i + 1):
-                print( gtsam.Symbol('x',j) )
+                print(X(j), ":", currentEstimate.atPose3(X(j)))
-                print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) )
 
             for j in range(len(points)):
-                print( gtsam.Symbol('l',j) )
+                print(L(j), ":", currentEstimate.atPoint3(L(j)))
-                print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) )
 
-            visual_ISAM2_plot(poses, points, currentEstimate);
+            visual_ISAM2_plot(poses, points, currentEstimate)
-            time.sleep(1)
 
             # Clear the factor graph and values for the next iteration
-            graph.resize(0);
+            graph.resize(0)
-            initialEstimate.clear();
+            initialEstimate.clear()
+            
+    plt.ioff()
+    plt.show()
 
 if __name__ == '__main__':
     visual_ISAM2_example()

python/gtsam_utils/__init__.py

@@ -1 +1 @@
-from ._plot import *
+from .plot import *

python/gtsam_utils/plot.py

@@ -0,0 +1,59 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D as _Axes3D
+
+def plotPoint3(fignum, point, linespec):
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+    ax.plot([point.x()], [point.y()], [point.z()], linespec)
+
+
+def plot3DPoints(fignum, values, linespec, marginals=None):
+    # PLOT3DPOINTS Plots the Point3's in a 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.
+
+    keys = values.keys()
+
+    # Plot points and covariance matrices
+    for key in keys:
+        try:
+            p = values.atPoint3(key);
+            # if haveMarginals
+            #     P = marginals.marginalCovariance(key);
+            #     gtsam.plotPoint3(p, linespec, P);
+            # else
+            plotPoint3(fignum, p, linespec);
+        except RuntimeError:
+            continue
+            # I guess it's not a Point3
+
+def plotPose3(fignum, pose, axisLength=0.1):
+    # get figure object
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    C = pose.translation().vector()
+
+    # draw the camera axes
+    xAxis = C + gRp[:, 0] * axisLength
+    L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0)
+    ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-')
+    
+    yAxis = C + gRp[:, 1] * axisLength
+    L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0)
+    ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-')
+    
+    zAxis = C + gRp[:, 2] * axisLength
+    L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0)
+    ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-')
+
+    # # plot the covariance
+    # if (nargin>2) && (~isempty(P))
+    #     pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame    
+    #     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
+    #     gtsam.covarianceEllipse3D(C,gPp);  
+    # end

python/handwritten/nonlinear/NonlinearFactorGraph.cpp

@@ -28,6 +28,13 @@ using namespace gtsam;
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1);
 
+boost::shared_ptr<NonlinearFactor> getNonlinearFactor(
+    const NonlinearFactorGraph& graph, size_t idx) {
+  auto p = boost::dynamic_pointer_cast<NonlinearFactor>(graph.at(idx));
+  if (!p) throw std::runtime_error("No NonlinearFactor at requested index");
+  return p;
+};
+
 void exportNonlinearFactorGraph(){
 
   typedef NonlinearFactorGraph::sharedFactor sharedFactor;
@@ -44,4 +51,6 @@ void exportNonlinearFactorGraph(){
     .def("print", &NonlinearFactorGraph::print, print_overloads(args("s")))
   ;
 
+  def("getNonlinearFactor", getNonlinearFactor);
+
 }

python/handwritten/nonlinear/Values.cpp

@@ -26,61 +26,17 @@
 #include "gtsam/geometry/Point3.h"
 #include "gtsam/geometry/Rot3.h"
 #include "gtsam/geometry/Pose3.h"
+#include "gtsam/navigation/ImuBias.h"
 
 using namespace boost::python;
 using namespace gtsam;
 
-/** The function ValuesAt is a workaround to be able to call the correct templated version
-  * of Values::at. Without it, python would only try to match the last 'at' metho defined
-  * below. With this wrapper function we can call 'at' in python passing an extra type,
-  * which will define the type to be returned. Example:
-  *
-  *     >>> import gtsam
-  *     >>> v = gtsam.nonlinear.Values()
-  *     >>> v.insert(1,gtsam.geometry.Point3())
-  *     >>> v.insert(2,gtsam.geometry.Rot3())
-  *     >>> v.insert(3,gtsam.geometry.Pose3())
-  *     >>> v.at(1,gtsam.geometry.Point3())
-  *     >>> v.at(2,gtsam.geometry.Rot3())
-  *     >>> v.at(3,gtsam.geometry.Pose3())
-  *
-  * A more 'pythonic' way I think would be to not use this function and define different 
-  * 'at' methods below using the name of the type in the function name, like:
-  *
-  *     .def("point3_at", &Values::at<Point3>, return_internal_reference<>())
-  *     .def("rot3_at", &Values::at<Rot3>, return_internal_reference<>())
-  *     .def("pose3_at", &Values::at<Pose3>, return_internal_reference<>())
-  *
-  * and then they could be accessed from python as 
-  * 
-  *     >>> import gtsam
-  *     >>> v = gtsam.nonlinear.Values()
-  *     >>> v.insert(1,gtsam.geometry.Point3())
-  *     >>> v.insert(2,gtsam.geometry.Rot3())
-  *     >>> v.insert(3,gtsam.geometry.Pose3())
-  *     >>> v.point3_at(1)
-  *     >>> v.rot3_at(2)
-  *     >>> v.pose3_at(3)
-  *
-  * In fact, I just saw the pythonic way sounds more clear, so I'm sticking with this and
-  * leaving the comments here for future reference. I'm using the PEP0008 for method naming.
-  * See: https://www.python.org/dev/peps/pep-0008/#function-and-method-arguments
-  */
-// template<typename T>
-// const T  & ValuesAt( const Values & v, Key j, T /*type*/)
-// {
-//   return v.at<T>(j);
-// }
-
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1);
 
 void exportValues(){
 
-  // NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below 
+  typedef imuBias::ConstantBias Bias;
-  //       will compile, but are useless in the python wrapper. We need to use specific 
+
-  //       'at' and 'insert' methods for each type.
-  // const Value& (Values::*at1)(Key) const = &Values::at;
-  // void (Values::*insert1)(Key, const Value&) = &Values::insert;
   bool (Values::*exists1)(Key) const = &Values::exists;
   void  (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert;
   void  (Values::*insert_rot2)  (Key, const gtsam::Rot2&) = &Values::insert;
@@ -88,7 +44,8 @@ void exportValues(){
   void  (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert;
   void  (Values::*insert_rot3)  (Key, const gtsam::Rot3&) = &Values::insert;
   void  (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert;
-
+  void  (Values::*insert_bias) (Key, const Bias&) = &Values::insert;
+  void  (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert;
 
   class_<Values>("Values", init<>())
   .def(init<Values>())
@@ -101,23 +58,22 @@ void exportValues(){
   .def("print", &Values::print, print_overloads(args("s")))
   .def("size", &Values::size)
   .def("swap", &Values::swap)
-  // NOTE: Following commented lines add useless methods on Values
-  // .def("insert", insert1)
-  // .def("at", at1, return_value_policy<copy_const_reference>())
   .def("insert", insert_point2)
   .def("insert", insert_rot2)
   .def("insert", insert_pose2)
   .def("insert", insert_point3)
   .def("insert", insert_rot3)
   .def("insert", insert_pose3)
-  // NOTE: The following commented lines are another way of specializing the return type.
+  .def("insert", insert_bias)
-  //       See long comment above.
+  .def("insert", insert_vector3)
-  // .def("at", &ValuesAt<Point3>, return_internal_reference<>())
+  .def("atPoint2", &Values::at<Point2>, return_value_policy<copy_const_reference>())
-  // .def("at", &ValuesAt<Rot3>, return_internal_reference<>())
+  .def("atRot2", &Values::at<Rot2>, return_value_policy<copy_const_reference>())
-  // .def("at", &ValuesAt<Pose3>, return_internal_reference<>())
+  .def("atPose2", &Values::at<Pose2>, return_value_policy<copy_const_reference>())
-  .def("point3_at", &Values::at<Point3>, return_value_policy<copy_const_reference>())
+  .def("atPoint3", &Values::at<Point3>, return_value_policy<copy_const_reference>())
-  .def("rot3_at", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
+  .def("atRot3", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
-  .def("pose3_at", &Values::at<Pose3>, return_value_policy<copy_const_reference>())  
+  .def("atPose3", &Values::at<Pose3>, return_value_policy<copy_const_reference>())
+  .def("atConstantBias", &Values::at<Bias>, return_value_policy<copy_const_reference>())
+  .def("atVector3", &Values::at<Vector3>, return_value_policy<copy_const_reference>())
   .def("exists", exists1)
   .def("keys", &Values::keys)
   ;

python/handwritten/slam/BetweenFactor.cpp

@@ -33,17 +33,17 @@ using namespace gtsam;
 
 using namespace std;
 
-// template<class VALUE>
+// template<class T>
 // void exportBetweenFactor(const std::string& name){
-//   class_<VALUE>(name, init<>())
+//   class_<T>(name, init<>())
-//   .def(init<Key, Key, VALUE, SharedNoiseModel>())
+//   .def(init<Key, Key, T, SharedNoiseModel>())
 //   ;
 // }
 
-#define BETWEENFACTOR(VALUE) \
+#define BETWEENFACTOR(T) \
-  class_< BetweenFactor<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<VALUE> > >("BetweenFactor"#VALUE) \
+  class_< BetweenFactor<T>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<T> > >("BetweenFactor"#T) \
-  .def(init<Key,Key,VALUE,noiseModel::Base::shared_ptr>()) \
+  .def(init<Key,Key,T,noiseModel::Base::shared_ptr>()) \
-  .def("measured", &BetweenFactor<VALUE>::measured, return_internal_reference<>()) \
+  .def("measured", &BetweenFactor<T>::measured, return_internal_reference<>()) \
 ;
 
 void exportBetweenFactors()

python/handwritten/slam/PriorFactor.cpp

@@ -54,4 +54,5 @@ void exportPriorFactors()
   PRIORFACTOR(Point3)
   PRIORFACTOR(Rot3)
   PRIORFACTOR(Pose3)
+  PRIORFACTOR(Vector3)
 }