Move my developments to the handwritten structure of files that existed before

There's a problem with numpy_eigen causing a segmentation fault.

python/gtsam/__init__.py

@@ -1 +1 @@
-from libgtsam_python import *
+from libgtsam_python import *

python/gtsam/geometry/.gitignore

@@ -1 +0,0 @@
-/libgeometry_python.so

python/gtsam/geometry/__init__.py

@@ -1 +0,0 @@
-from libgeometry_python import *

python/gtsam/noiseModel/.gitignore

@@ -1 +0,0 @@
-/libnoiseModel_python.so

python/gtsam/noiseModel/__init__.py

@@ -1 +0,0 @@
-from libnoiseModel_python import *

python/gtsam/nonlinear/.gitignore

@@ -1 +0,0 @@
-/libnonlinear_python.so

python/gtsam/nonlinear/__init__.py

@@ -1 +0,0 @@
-from libnonlinear_python import *

python/gtsam/registernumpyeigen/.gitignore

@@ -1 +0,0 @@
-/libregisternumpyeigen_python.so

python/gtsam/registernumpyeigen/__init__.py

@@ -1 +0,0 @@
-from libregisternumpyeigen_python import *

python/gtsam/slam/.gitignore

@@ -1 +0,0 @@
-/libslam_python.so

python/gtsam/slam/__init__.py

@@ -1 +0,0 @@
-from libslam_python import *

python/handwritten/exportgtsam.cpp

@@ -1,63 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief exports the python module 
+ * @author Andrew Melim 
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
 #include <boost/cstdint.hpp>
 
-#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Matrix.h>
-#include <gtsam/nonlinear/Values.h>
+#include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
 
 using namespace boost::python;
 using namespace gtsam;
 using namespace std;
 
-// Geometery
+// Geometry
 void exportPoint2();
+void exportPoint3();
 void exportRot2();
+void exportRot3();
 void exportPose2();
+void exportPose3();
 
 // Linear
 void exportNoiseModels();
 
 // Nonlinear
 void exportValues();
+void exportNonlinearFactor();
 void exportNonlinearFactorGraph();
 void exportLevenbergMarquardtOptimizer();
+void exportISAM2();
 
 // Slam
-template< class FACTOR, class VALUE >
+void exportPriorFactors();
-void exportPriorFactor(const std::string& name){
+void exportBetweenFactors();
-  class_< FACTOR >(name.c_str(), init<>())
-  .def(init< Key, VALUE&, SharedNoiseModel >())
-  ;
-}
 
-template<class FACTOR, class VALUE>
+// Utils (or Python wrapper specific functions)
-void exportBetweenFactor(const std::string& name){
+void registerNumpyEigenConversions();
-  class_<FACTOR>(name.c_str(), init<>())
-  .def(init<Key, Key, VALUE&, SharedNoiseModel>())
-  ;
-}
-
-typedef gtsam::PriorFactor<gtsam::Point2> Point2PriorFactor;
-typedef gtsam::PriorFactor<gtsam::Pose2> Pose2PriorFactor;
-typedef gtsam::BetweenFactor<gtsam::Pose2> Pose2BetweenFactor;
 
 //-----------------------------------//
 
 BOOST_PYTHON_MODULE(libgtsam_python){
-  using namespace boost::python;
+
+  // Should be the first thing to be done
+  registerNumpyEigenConversions();
+
   exportPoint2();
+  exportPoint3();
   exportRot2();
+  exportRot3();
   exportPose2();
+  exportPose3();
 
   exportNoiseModels();
 
   exportValues();
+  exportNonlinearFactor();
   exportNonlinearFactorGraph();
   exportLevenbergMarquardtOptimizer();
+  exportISAM2();
 
-  exportPriorFactor< Point2PriorFactor, Point2 >("Point2PriorFactor");
+  exportPriorFactors();
-  exportPriorFactor< Pose2PriorFactor, Pose2 >("Pose2PriorFactor");
+  exportBetweenFactors();
-  exportBetweenFactor< Pose2BetweenFactor, Pose2 >("Pose2BetweenFactor");
 }

python/handwritten/geometry/Point2.cpp

@@ -1,3 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Point2 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
 #include "gtsam/geometry/Point2.h"
 
@@ -12,15 +29,26 @@ void exportPoint2(){
 
   class_<Point2>("Point2", init<>())
     .def(init<double, double>())
-    .def("print", &Point2::print, print_overloads(args("s")))
+    .def(init<Vector>())
+    .def("identity", &Point2::identity)
+    .def("dist", &Point2::dist)
+    .def("distance", &Point2::distance)
     .def("equals", &Point2::equals, equals_overloads(args("q","tol")))
-    .def("inverse", &Point2::inverse)
+    .def("norm", &Point2::norm)
-    // .def("compose", &Point2::compose, compose_overloads(args("q", "H1", "H2")))
+    .def("print", &Point2::print, print_overloads(args("s")))
-    .def("between", &Point2::between)
+    .def("unit", &Point2::unit)
-    // .def("dim", &Point2::dim)
+    .def("vector", &Point2::vector)
-    .def("retract", &Point2::retract)
     .def("x", &Point2::x)
     .def("y", &Point2::y)
+    .def(self * other<double>()) // __mult__
+    .def(other<double>() * self) // __mult__
+    .def(self + self)
+    .def(-self)
+    .def(self - self)
+    .def(self / other<double>())
+    .def(self_ns::str(self))
+    .def(repr(self))
+    .def(self == self)
   ;
 
 }

python/handwritten/geometry/Point3.cpp

@@ -0,0 +1,60 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Point3 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include "gtsam/geometry/Point3.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2)
+
+void exportPoint3(){
+
+class_<Point3>("Point3")
+  .def(init<>())
+  .def(init<double,double,double>())
+  .def(init<Vector>())
+  .def("identity", &Point3::identity)
+  .staticmethod("identity")
+  .def("add", &Point3::add)
+  .def("cross", &Point3::cross)
+  .def("dist", &Point3::dist)
+  .def("distance", &Point3::distance)
+  .def("dot", &Point3::dot)
+  .def("equals", &Point3::equals, equals_overloads(args("q","tol")))
+  .def("norm", &Point3::norm)
+  .def("normalize", &Point3::normalize)
+  .def("print", &Point3::print, print_overloads(args("s")))
+  .def("sub", &Point3::sub)
+  .def("vector", &Point3::vector)
+  .def("x", &Point3::x)
+  .def("y", &Point3::y)
+  .def("z", &Point3::z)
+  .def(self * other<double>())
+  .def(other<double>() * self)
+  .def(self + self)
+  .def(-self)
+  .def(self - self)
+  .def(self / other<double>())
+  .def(self_ns::str(self))
+  .def(repr(self))
+  .def(self == self)
+;
+
+}

python/handwritten/geometry/Pose2.cpp

@@ -1,3 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Pose2 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
 #include "gtsam/geometry/Pose2.h"
 

python/handwritten/geometry/Pose3.cpp

@@ -0,0 +1,74 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Pose3 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include "gtsam/geometry/Pose3.h"
+
+#include "gtsam/geometry/Pose2.h"
+#include "gtsam/geometry/Point3.h"
+#include "gtsam/geometry/Rot3.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1)
+// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3)
+// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3)
+// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 1, 3)
+// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 1, 3)
+
+void exportPose3(){
+
+  Point3 (Pose3::*transform_to1)(const Point3&,  OptionalJacobian< 3, 6 >,  OptionalJacobian< 3, 3 > ) const
+    = &Pose3::transform_to;
+  Pose3 (Pose3::*transform_to2)(const Pose3&) const
+    = &Pose3::transform_to;
+  
+  class_<Pose3>("Pose3")
+    .def(init<>())
+    .def(init<Pose3>())
+    .def(init<Rot3,Point3>())
+    .def(init<Rot3,Vector3>())
+    .def(init<Pose2>())
+    .def(init<Matrix>())
+    .def("print", &Pose3::print, print_overloads(args("s")))
+    .def("equals", &Pose3::equals, equals_overloads(args("pose","tol")))
+    .def("identity", &Pose3::identity)
+    .staticmethod("identity")
+    .def("bearing", &Pose3::bearing)
+    .def("matrix", &Pose3::matrix)
+    .def("transform_from", &Pose3::transform_from, 
+      transform_from_overloads(args("point", "H1", "H2")))
+    .def("transform_to", transform_to1, 
+      transform_to_overloads(args("point", "H1", "H2")))
+    .def("transform_to", transform_to2)
+    .def("x", &Pose3::x)
+    .def("y", &Pose3::y)
+    .def("z", &Pose3::z)
+    .def("translation", &Pose3::translation,
+      translation_overloads()[return_value_policy<copy_const_reference>()])
+    .def("rotation", &Pose3::rotation, return_value_policy<copy_const_reference>())
+    .def(self * self)            // __mult__
+    .def(self * other<Point3>()) // __mult__
+    .def(self_ns::str(self))     // __str__
+    .def(repr(self))             // __repr__
+  ;
+}

python/handwritten/geometry/Rot2.cpp

@@ -1,3 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Rot2 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
 #include "gtsam/geometry/Rot2.h"
 
@@ -13,35 +30,31 @@ void exportRot2(){
 
   class_<Rot2>("Rot2", init<>())
     .def(init<double>())
-
-    .def("fromAngle", &Rot2::fromAngle)
-    .staticmethod("fromAngle")
-
-    .def("fromDegrees", &Rot2::fromDegrees)
-    .staticmethod("fromDegrees")
-
-    .def("fromCosSin", &Rot2::fromCosSin)
-    .staticmethod("fromCosSin")
-
-    .def("atan2", &Rot2::atan2)
-    .staticmethod("atan2")
-
-    .def("print", &Rot2::print, print_overloads(args("s")))
-    .def("equals", &Rot2::equals, equals_overloads(args("q","tol")))
-    // .def("inverse", &Rot2::inverse)
-    // .def("compose", &Rot2::compose, compose_overloads(args("q", "H1", "H2")))
-    // .def("between", &Rot2::between)
-    // .def("dim", &Rot2::dim)
-    // .def("retract", &Rot2::retract)
-
     .def("Expmap", &Rot2::Expmap)
     .staticmethod("Expmap")
-
+    .def("Logmap", &Rot2::Logmap)
-    .def("theta", &Rot2::theta)
+    .staticmethod("Logmap")
-    .def("degrees", &Rot2::degrees)
+    .def("atan2", &Rot2::atan2)
+    .staticmethod("atan2")
+    .def("fromAngle", &Rot2::fromAngle)
+    .staticmethod("fromAngle")
+    .def("fromCosSin", &Rot2::fromCosSin)
+    .staticmethod("fromCosSin")
+    .def("fromDegrees", &Rot2::fromDegrees)
+    .staticmethod("fromDegrees")
+    .def("identity", &Rot2::identity)
+    .staticmethod("identity")
+    .def("relativeBearing", &Rot2::relativeBearing)
+    .staticmethod("relativeBearing")
     .def("c", &Rot2::c)
+    .def("degrees", &Rot2::degrees)
+    .def("equals", &Rot2::equals, equals_overloads(args("q","tol")))
+    .def("matrix", &Rot2::matrix)
+    .def("print", &Rot2::print, print_overloads(args("s")))
+    .def("rotate", &Rot2::rotate)
     .def("s", &Rot2::s)
-
+    .def("theta", &Rot2::theta)
+    .def("unrotate", &Rot2::unrotate)
     .def(self * self) // __mult__
   ;
 

python/handwritten/geometry/Rot3.cpp

@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Rot3 class to python
+ * @author Andrew Melim
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include "gtsam/geometry/Rot3.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+// Prototypes used to perform overloading
+// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html
+gtsam::Rot3  (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle;
+gtsam::Rot3  (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle;
+gtsam::Rot3  (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues;
+gtsam::Rot3  (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues;
+gtsam::Rot3  (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx;
+gtsam::Rot3  (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2)
+
+void exportRot3(){
+
+  class_<Rot3>("Rot3")
+    .def(init<>())
+    .def(init<Point3,Point3,Point3>())
+    .def(init<double,double,double,double,double,double,double,double,double>())
+    .def(init<Matrix3>())
+    .def(init<Matrix>())
+    .def("AxisAngle", AxisAngle_0)
+    .def("AxisAngle", AxisAngle_1)
+    .staticmethod("AxisAngle")
+    .def("Expmap", &Rot3::Expmap)
+    .staticmethod("Expmap")
+    .def("ExpmapDerivative", &Rot3::ExpmapDerivative)
+    .staticmethod("ExpmapDerivative")
+    .def("Logmap", &Rot3::Logmap)
+    .staticmethod("Logmap")
+    .def("LogmapDerivative", &Rot3::LogmapDerivative)
+    .staticmethod("LogmapDerivative")
+    .def("Rodrigues", Rodrigues_0)
+    .def("Rodrigues", Rodrigues_1)
+    .staticmethod("Rodrigues")
+    .def("Rx", &Rot3::Rx)
+    .staticmethod("Rx")
+    .def("Ry", &Rot3::Ry)
+    .staticmethod("Ry")
+    .def("Rz", &Rot3::Rz)
+    .staticmethod("Rz")
+    .def("RzRyRx", RzRyRx_0)
+    .def("RzRyRx", RzRyRx_1)
+    .staticmethod("RzRyRx")
+    .def("identity", &Rot3::identity)
+    .staticmethod("identity")
+    .def("AdjointMap", &Rot3::AdjointMap)
+    .def("column", &Rot3::column)
+    .def("conjugate", &Rot3::conjugate)
+    .def("equals", &Rot3::equals, equals_overloads(args("q","tol")))
+    .def("localCayley", &Rot3::localCayley)
+    .def("matrix", &Rot3::matrix)
+    .def("print", &Rot3::print, print_overloads(args("s")))
+    .def("r1", &Rot3::r1)
+    .def("r2", &Rot3::r2)
+    .def("r3", &Rot3::r3)
+    .def("retractCayley", &Rot3::retractCayley)
+    .def("rpy", &Rot3::rpy)
+    .def("slerp", &Rot3::slerp)
+    .def("transpose", &Rot3::transpose)
+    .def("xyz", &Rot3::xyz)
+    .def(self * self)
+    .def(self * other<Point3>())
+    .def(self * other<Unit3>())
+    .def(self_ns::str(self)) // __str__
+    .def(repr(self))         // __repr__
+  ;
+
+}

python/handwritten/linear/NoiseModel.cpp

@@ -1,13 +1,134 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps the noise model classes into the noiseModel module
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+ /** TODOs Summary:
+  *
+  * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models.
+  *              I think it's only worthy if we want to access virtual the virtual functions from python.
+  * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap
+  */
+
 #include <boost/python.hpp>
-#include <gtsam/linear/NoiseModel.h>
+
+#include "gtsam/linear/NoiseModel.h"
 
 using namespace boost::python;
 using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+// Wrap around pure virtual class Base.
+// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the 
+// overloading through inheritance in Python.
+// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions
+struct BaseCallback : Base, wrapper<Base>
+{
+  void print (const std::string & name="") const {
+    this->get_override("print")();
+  }
+  bool equals (const Base & expected, double tol=1e-9) const {
+    return this->get_override("equals")();
+  }
+  Vector whiten (const Vector & v) const {
+    return this->get_override("whiten")();
+  }
+  Matrix Whiten (const Matrix & v) const {
+    return this->get_override("Whiten")();
+  }
+  Vector unwhiten (const Vector & v) const {
+    return this->get_override("unwhiten")();
+  }
+  double distance (const Vector & v) const {
+    return this->get_override("distance")();
+  }
+  void WhitenSystem (std::vector< Matrix > &A, Vector &b) const {
+    this->get_override("WhitenSystem")();
+  }
+  void WhitenSystem (Matrix &A, Vector &b) const {
+    this->get_override("WhitenSystem")();
+  }
+  void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const {
+    this->get_override("WhitenSystem")();
+  }
+  void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const {
+    this->get_override("WhitenSystem")();
+  }
+
+  // TODO(Ellon): Wrap non-pure virtual methods should go here.
+  //              See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations
+
+};
+
+// Overloads for named constructors. Named constructors are static, so we declare them 
+// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
+// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments
+BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3)
+
 
 void exportNoiseModels(){
-  // Diagonal Noise Model, no constructor
+
-  class_<noiseModel::Diagonal>("DiagonalNoiseModel", no_init)
+  // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html
-  .def("Sigmas", &noiseModel::Diagonal::Sigmas)
+  std::string noiseModel_name = extract<std::string>(scope().attr("__name__") + ".noiseModel");
-  .staticmethod("Sigmas")
+  object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str()))));
+  scope().attr("noiseModel") = noiseModel_module;
+  scope noiseModel_scope = noiseModel_module;
+  
+  // Then export our classes in the noiseModel scope
+  class_<BaseCallback,boost::noncopyable>("Base")
+    .def("print", pure_virtual(&Base::print))
   ;
+  
+  // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining)
+  class_<Gaussian, boost::shared_ptr<Gaussian>, bases<Base> >("Gaussian", no_init)
+    .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads())
+    .staticmethod("SqrtInformation")
+    .def("Information",&Gaussian::Information, Gaussian_Information_overloads())
+    .staticmethod("Information")
+    .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads())
+    .staticmethod("Covariance")
+  ;
+  
+  class_<Diagonal, boost::shared_ptr<Diagonal>, bases<Gaussian> >("Diagonal", no_init)
+    .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads())
+    .staticmethod("Sigmas")
+    .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads())
+    .staticmethod("Variances")
+    .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads())
+    .staticmethod("Precisions")
+  ;
+  
+  class_<Isotropic, boost::shared_ptr<Isotropic>, bases<Diagonal> >("Isotropic", no_init)
+    .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads())
+    .staticmethod("Sigma")
+    .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads())
+    .staticmethod("Variance")
+    .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads())
+    .staticmethod("Precision")
+  ;
+  
+  class_<Unit, boost::shared_ptr<Unit>, bases<Isotropic> >("Unit", no_init)
+    .def("Create",&Unit::Create)
+    .staticmethod("Create")
+  ;
+
 }

python/handwritten/nonlinear/ISAM2.cpp

@@ -0,0 +1,44 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief exports ISAM2 class to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include "gtsam/nonlinear/ISAM2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7)
+
+void exportISAM2(){
+
+// TODO(Ellon): Export all properties of ISAM2Params
+class_<ISAM2Params>("ISAM2Params")
+;
+
+// TODO(Ellon): Export useful methods/properties of ISAM2Result
+class_<ISAM2Result>("ISAM2Result")
+;
+
+// Function pointers for overloads in ISAM2
+Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate;
+
+class_<ISAM2>("ISAM2")
+  // TODO(Ellon): wrap all optional values of update
+  .def("update",&ISAM2::update, update_overloads())
+  .def("calculate_estimate", calculateEstimate_0)
+;
+
+}

python/handwritten/nonlinear/NonlinearFactor.cpp

@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief exports virtual class NonlinearFactor to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include "gtsam/nonlinear/NonlinearFactor.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+// Wrap around pure virtual class NonlinearFactor.
+// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the 
+// overloading through inheritance in Python.
+// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions
+struct NonlinearFactorCallback : NonlinearFactor, wrapper<NonlinearFactor>
+{
+  double error (const Values & values) const {
+    return this->get_override("error")(values);
+  }
+  size_t dim () const {
+    return this->get_override("dim")();
+  }
+  boost::shared_ptr<GaussianFactor> linearize(const Values & values) const {
+    return this->get_override("linearize")(values);
+  }
+};
+
+void exportNonlinearFactor(){
+
+  class_<NonlinearFactorCallback,boost::noncopyable>("NonlinearFactor")
+  ;
+
+}

python/handwritten/nonlinear/NonlinearFactorGraph.cpp

@@ -1,6 +1,23 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief exports NonlinearFactorGraph class to python
+ * @author Andrew Melim 
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
 #include "gtsam/nonlinear/NonlinearFactorGraph.h"
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 using namespace boost::python;
 using namespace gtsam;
@@ -8,11 +25,15 @@ using namespace gtsam;
 
 void exportNonlinearFactorGraph(){
 
-  typedef boost::shared_ptr<NonlinearFactor> shared_factor;
+  typedef NonlinearFactorGraph::sharedFactor sharedFactor;
 
-  void (NonlinearFactorGraph::*push_back1)(const shared_factor& factor) = &NonlinearFactorGraph::push_back;
+  void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back;
+  void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add;
 
   class_<NonlinearFactorGraph>("NonlinearFactorGraph", init<>())
-  .def("push_back", push_back1)
+    .def("size",&NonlinearFactorGraph::size)  
+    .def("push_back", push_back1)
+    .def("add", add1)
   ;
+
 }

python/handwritten/nonlinear/Values.cpp

@@ -1,19 +1,107 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps Values class to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
-#include <gtsam/nonlinear/Values.h>
+#include "gtsam/nonlinear/Values.h"
+#include "gtsam/geometry/Point3.h"
+#include "gtsam/geometry/Rot3.h"
+#include "gtsam/geometry/Pose3.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);
+// }
+
 void exportValues(){
 
-  const Value& (Values::*at1)(Key) const = &Values::at;
+  // NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below 
+  //       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::*insert1)(Key, const Value&) = &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("at", at1, return_value_policy<copy_const_reference>())
+  .def("clear", &Values::clear)
+  .def("dim", &Values::dim)
+  .def("empty", &Values::empty)
+  .def("equals", &Values::equals)
+  .def("erase", &Values::erase)
+  .def("insert_fixed", &Values::insertFixed)
+  .def("print", &Values::print)
+  .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_point3)
+  .def("insert", insert_rot3)
+  .def("insert", insert_pose3)
+  // NOTE: The following commented lines are another way of specializing the return type.
+  //       See long comment above.
+  // .def("at", &ValuesAt<Point3>, return_internal_reference<>())
+  // .def("at", &ValuesAt<Rot3>, return_internal_reference<>())
+  // .def("at", &ValuesAt<Pose3>, return_internal_reference<>())
+  .def("point3_at", &Values::at<Point3>, return_value_policy<copy_const_reference>())
+  .def("rot3_at", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
+  .def("pose3_at", &Values::at<Pose3>, return_value_policy<copy_const_reference>())  
   .def("exists", exists1)
-  .def("insert", insert1, return_value_policy<copy_const_reference>())
   ;
 }

python/handwritten/slam/BetweenFactor.cpp

@@ -1,14 +1,53 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps BetweenFactor for several values to python
+ * @author Andrew Melim 
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
-#include <gtsam/slam/BetweenFactor.h>
+#include "gtsam/slam/BetweenFactor.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"
 
 using namespace boost::python;
 using namespace gtsam;
 
 using namespace std;
 
-template<class VALUE>
+// template<class VALUE>
-void exportBetweenFactor(const std::string& name){
+// void exportBetweenFactor(const std::string& name){
-  class_<VALUE>(name, init<>())
+//   class_<VALUE>(name, init<>())
-  .def(init<Key, Key, VALUE, SharedNoiseModel>())
+//   .def(init<Key, Key, VALUE, SharedNoiseModel>())
-  ;
+//   ;
-}
+// }
+
+#define BETWEENFACTOR(VALUE) \
+  class_< BetweenFactor<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<VALUE> > >("BetweenFactor"#VALUE) \
+  .def(init<Key,Key,VALUE,noiseModel::Base::shared_ptr>()) \
+  .def("measured", &BetweenFactor<VALUE>::measured, return_internal_reference<>()) \
+;
+
+void exportBetweenFactors()
+{
+  BETWEENFACTOR(Point2)
+  BETWEENFACTOR(Rot2)
+  BETWEENFACTOR(Pose2)
+  BETWEENFACTOR(Point3)
+  BETWEENFACTOR(Rot3)
+  BETWEENFACTOR(Pose3)
+}

python/handwritten/slam/PriorFactor.cpp

@@ -1,14 +1,53 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps PriorFactor for several values to python
+ * @author Andrew Melim 
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
 #include <boost/python.hpp>
-#include <gtsam/slam/PriorFactor.h>
+#include "gtsam/slam/PriorFactor.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"
 
 using namespace boost::python;
 using namespace gtsam;
 
 using namespace std;
 
-template< class FACTOR, class VALUE >
+// template< class FACTOR, class VALUE >
-void exportPriorFactor(const std::string& name){
+// void exportPriorFactor(const std::string& name){
-  class_< FACTOR >(name.c_str(), init<>())
+//   class_< FACTOR >(name.c_str(), init<>())
-  .def(init< Key, VALUE&, SharedNoiseModel >())
+//   .def(init< Key, VALUE&, SharedNoiseModel >())
-  ;
+//   ;
+// }
+
+#define PRIORFACTOR(VALUE) \
+  class_< PriorFactor<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< PriorFactor<VALUE> > >("PriorFactor"#VALUE) \
+  .def(init<Key,VALUE,noiseModel::Base::shared_ptr>()) \
+  .def("prior", &PriorFactor<VALUE>::prior, return_internal_reference<>()) \
+;
+
+void exportPriorFactors()
+{
+  PRIORFACTOR(Point2)
+  PRIORFACTOR(Rot2)
+  PRIORFACTOR(Pose2)
+  PRIORFACTOR(Point3)
+  PRIORFACTOR(Rot3)
+  PRIORFACTOR(Pose3)
 }

python/handwritten/utils/NumpyEigen.cpp

@@ -0,0 +1,52 @@
+ /* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief register conversion matrix between numpy and Eigen
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+ #include <boost/python.hpp>
+
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/Vector.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+void registerNumpyEigenConversions()
+{
+  import_array();
+  NumpyEigenConverter<Vector>::register_converter();
+  NumpyEigenConverter<Vector1>::register_converter();
+  NumpyEigenConverter<Vector2>::register_converter();
+  NumpyEigenConverter<Vector3>::register_converter();
+  NumpyEigenConverter<Vector4>::register_converter();
+  NumpyEigenConverter<Vector5>::register_converter();
+  NumpyEigenConverter<Vector6>::register_converter();
+  NumpyEigenConverter<Vector7>::register_converter();
+  NumpyEigenConverter<Vector8>::register_converter();
+  NumpyEigenConverter<Vector9>::register_converter();
+  NumpyEigenConverter<Vector10>::register_converter();
+  
+  NumpyEigenConverter<Matrix>::register_converter();
+  NumpyEigenConverter<Matrix2>::register_converter();
+  NumpyEigenConverter<Matrix3>::register_converter();
+  NumpyEigenConverter<Matrix4>::register_converter();
+  NumpyEigenConverter<Matrix5>::register_converter();
+  NumpyEigenConverter<Matrix6>::register_converter();
+  NumpyEigenConverter<Matrix7>::register_converter();
+  NumpyEigenConverter<Matrix8>::register_converter();
+  NumpyEigenConverter<Matrix9>::register_converter();
+
+}