OdometryExample and necessary wrapping

python/gtsam_examples/OdometryExample.py

@@ -1,7 +1,36 @@
 #!/usr/bin/env python
-from gtsam import *
-import numpy_eigen as npe
+from __future__ import print_function
+import gtsam
+import numpy as np
 
-noisemodel = DiagonalNoiseModel.Sigmas([0.1, 0.1, 0.1])
-graph = NonlinearFactorGraph()
-initialEstimate = Values()
+# Create an empty nonlinear factor graph
+graph = gtsam.NonlinearFactorGraph()
+
+# Add a prior on the first pose, setting it to the origin
+# A prior factor consists of a mean and a noise model (covariance matrix)
+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]))
+graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
+
+# Add odometry factors
+odometry = gtsam.Pose2(2.0, 0.0, 0.0)
+# For simplicity, we will use the same noise model for each odometry factor
+odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+# Create odometry (Between) factors between consecutive poses
+graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
+graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
+graph.print("\nFactor Graph:\n")
+
+# Create the data structure to hold the initialEstimate estimate to the solution
+# For illustrative purposes, these have been deliberately set to incorrect values
+initial = gtsam.Values()
+initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
+initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
+initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
+initial.print("\nInitial Estimate:\n")
+
+# optimize using Levenberg-Marquardt optimization
+params = gtsam.LevenbergMarquardtParams()
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+result = optimizer.optimize()
+result.print("\nFinal Result:\n")

python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -9,8 +9,19 @@ using namespace boost::python;
 using namespace gtsam;
 
 void exportLevenbergMarquardtOptimizer(){
+  class_<LevenbergMarquardtParams>("LevenbergMarquardtParams", init<>())
+  .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping)
+  .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor)
+  .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial)
+  .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound)
+  .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound)
+  .def("setLogFile", &LevenbergMarquardtParams::setLogFile)
+  .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor)
+  .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM)
+  ;
+
   class_<LevenbergMarquardtOptimizer>("LevenbergMarquardtOptimizer",
     init<const NonlinearFactorGraph&, const Values&, const LevenbergMarquardtParams&>())
   .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy<copy_const_reference>())
   ;
-}
+}

python/handwritten/nonlinear/NonlinearFactorGraph.cpp

@@ -26,6 +26,7 @@
 using namespace boost::python;
 using namespace gtsam;
 
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1);
 
 void exportNonlinearFactorGraph(){
 
@@ -40,6 +41,7 @@ void exportNonlinearFactorGraph(){
     .def("add", add1)
     .def("resize", &NonlinearFactorGraph::resize)
     .def("empty", &NonlinearFactorGraph::empty)
+    .def("print", &NonlinearFactorGraph::print, print_overloads(args("s")))
   ;
 
-}
+}

python/handwritten/nonlinear/Values.cpp

@@ -20,6 +20,9 @@
 #include <numpy_eigen/NumpyEigenConverter.hpp>
 
 #include "gtsam/nonlinear/Values.h"
+#include "gtsam/geometry/Point2.h"
+#include "gtsam/geometry/Rot2.h"
+#include "gtsam/geometry/Pose2.h"
 #include "gtsam/geometry/Point3.h"
 #include "gtsam/geometry/Rot3.h"
 #include "gtsam/geometry/Pose3.h"
@@ -69,6 +72,8 @@ using namespace gtsam;
 //   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 
@@ -77,10 +82,14 @@ void exportValues(){
   // 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;
+  void  (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert;
   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;
 
+
   class_<Values>("Values", init<>())
   .def(init<Values>())
   .def("clear", &Values::clear)
@@ -89,12 +98,15 @@ void exportValues(){
   .def("equals", &Values::equals)
   .def("erase", &Values::erase)
   .def("insert_fixed", &Values::insertFixed)
-  .def("print", &Values::print)
+  .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)
@@ -109,4 +121,4 @@ void exportValues(){
   .def("exists", exists1)
   .def("keys", &Values::keys)
   ;
-}
+}