Merge remote-tracking branch 'origin/develop' into feature/LPSolver

.gitignore

@@ -7,3 +7,5 @@
 /examples/Data/pose3example-rewritten.txt
 *.txt.user
 *.txt.user.6d59f0c
+/python-build/
+*.pydevproject

CMakeLists.txt

@@ -64,6 +64,8 @@ option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TB
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
+option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module" ON)
+option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -316,6 +318,10 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
   add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
 
+if(GTSAM_ALLOW_DEPRECATED_SINCE_V4)
+	add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4)
+endif()
+
 ###############################################################################
 # Add components
 
@@ -344,6 +350,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	add_subdirectory(matlab)
 endif()
 
+# Python wrap
+if (GTSAM_BUILD_PYTHON)
+	include(GtsamPythonWrap)
+
+	# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
+	#       not working yet, so we're using a handwritten wrapper files on python/handwritten.
+	#       Once the python wrapping from the interface file is working, you can _swap_ the 
+	#       comments on the next lines
+	
+	# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
+	add_subdirectory(python)
+
+endif()
+
 # Build gtsam_unstable
 if (GTSAM_BUILD_UNSTABLE)
     add_subdirectory(gtsam_unstable)
@@ -456,10 +476,22 @@ print_config_flag(${GTSAM_USE_QUATERNIONS}             "Quaternions as default R
 print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking   ")
 print_config_flag(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap    ")
 print_config_flag(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap   ")
+print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4}   "Deprecated in GTSAM 4 allowed  ")
 
 message(STATUS "MATLAB toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")
 print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                     ")
+
+message(STATUS "Python module flags                                       ")
+
+if(GTSAM_PYTHON_WARNINGS)
+	message(STATUS "  Build python module            : No - dependencies missing")
+else()
+	print_config_flag(${GTSAM_BUILD_PYTHON}                "Build python module            ")
+endif()
+if(GTSAM_BUILD_PYTHON)
+	message(STATUS "  Python version                 : ${GTSAM_PYTHON_VERSION}")
+endif()
 message(STATUS "===============================================================")
 
 # Print warnings at the end
@@ -472,6 +504,9 @@ endif()
 if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
 	message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
 endif()
+if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
+	message(WARNING "${GTSAM_PYTHON_WARNINGS}")
+endif()
 
 # Include CPack *after* all flags
 include(CPack)

THANKS

@@ -38,6 +38,10 @@ at Uni Zurich:
 
 * Christian Forster
 
+at LAAS-CNRS
+
+* Ellon Paiva
+
 Many thanks for your hard work!!!!
 
 Frank Dellaert

cmake/FindNumPy.cmake

@@ -0,0 +1,102 @@
+# - Find the NumPy libraries
+# This module finds if NumPy is installed, and sets the following variables
+# indicating where it is.
+#
+# TODO: Update to provide the libraries and paths for linking npymath lib.
+#
+#  NUMPY_FOUND               - was NumPy found
+#  NUMPY_VERSION             - the version of NumPy found as a string
+#  NUMPY_VERSION_MAJOR       - the major version number of NumPy
+#  NUMPY_VERSION_MINOR       - the minor version number of NumPy
+#  NUMPY_VERSION_PATCH       - the patch version number of NumPy
+#  NUMPY_VERSION_DECIMAL     - e.g. version 1.6.1 is 10601
+#  NUMPY_INCLUDE_DIRS        - path to the NumPy include files
+
+#============================================================================
+# Copyright 2012 Continuum Analytics, Inc.
+#
+# MIT License
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files
+# (the "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to permit
+# persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be included
+# in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+#============================================================================
+
+# Finding NumPy involves calling the Python interpreter
+if(NumPy_FIND_REQUIRED)
+    find_package(PythonInterp REQUIRED)
+else()
+    find_package(PythonInterp)
+endif()
+
+if(NOT PYTHONINTERP_FOUND)
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
+    "import numpy as n; print(n.__version__); print(n.get_include());"
+    RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS
+    OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT
+    ERROR_VARIABLE _NUMPY_ERROR_VALUE
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
+
+if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0)
+    if(NumPy_FIND_REQUIRED)
+        message(FATAL_ERROR
+            "NumPy import failure:\n${_NUMPY_ERROR_VALUE}")
+    endif()
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+# Convert the process output into a list
+string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT})
+string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES})
+# Just in case there is unexpected output from the Python command.
+list(GET _NUMPY_VALUES -2 NUMPY_VERSION)
+list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS)
+
+string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}")
+if("${_VER_CHECK}" STREQUAL "")
+    # The output from Python was unexpected. Raise an error always
+    # here, because we found NumPy, but it appears to be corrupted somehow.
+    message(FATAL_ERROR
+        "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n")
+    return()
+endif()
+
+# Make sure all directory separators are '/'
+string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS})
+
+# Get the major and minor version numbers
+string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION})
+list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR)
+list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR)
+list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH)
+string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH})
+math(EXPR NUMPY_VERSION_DECIMAL
+    "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}")
+
+find_package_message(NUMPY
+    "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}"
+    "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}")
+
+set(NUMPY_FOUND TRUE)
+

cmake/GtsamPythonWrap.cmake

@@ -1,5 +1,5 @@
 #Setup cache options
-option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF)
+set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
 set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
 set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
 if(NOT GTSAM_PYTHON_INSTALL_PATH)
@@ -8,13 +8,13 @@ endif()
 
 #Author: Paul Furgale Modified by Andrew Melim
 function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
-  # Boost
+  # # Boost
-  find_package(Boost COMPONENTS python filesystem system REQUIRED)
+  # find_package(Boost COMPONENTS python filesystem system REQUIRED)
-  include_directories(${Boost_INCLUDE_DIRS})
+  # include_directories(${Boost_INCLUDE_DIRS})
 
-  # Find Python
+  # # Find Python
-  FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
+  # FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
-  INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
+  # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
 
   IF(APPLE)
     # The apple framework headers don't include the numpy headers for some reason.
@@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
     ENDIF()
   ENDIF(APPLE)
 
+  if(MSVC)
+    add_library(${moduleName}_python MODULE ${ARGN})
+    set_target_properties(${moduleName}_python PROPERTIES
+        OUTPUT_NAME         ${moduleName}_python
+        CLEAN_DIRECT_OUTPUT 1
+        VERSION             1
+        SOVERSION           0
+        SUFFIX              ".pyd")
+      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
 
-  # Create a static library version
+    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-  add_library(${TARGET_NAME} SHARED ${ARGN})
+    message(${PYLIB_OUTPUT_FILE})
+    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+    set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
 
-  target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared)
+  ELSE()
-      set_target_properties(${TARGET_NAME} PROPERTIES 
+    # Create a shared library
-          OUTPUT_NAME         ${TARGET_NAME}
+    add_library(${moduleName}_python SHARED ${generated_cpp_file})
-          CLEAN_DIRECT_OUTPUT 1
-          VERSION             1
-          SOVERSION           0)
 
+    set_target_properties(${moduleName}_python PROPERTIES
+        OUTPUT_NAME         ${moduleName}_python
+        CLEAN_DIRECT_OUTPUT 1)
+      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
+    # On OSX and Linux, the python library must end in the extension .so. Build this
+    # filename here.
+    get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
+    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
+    message(${PYLIB_OUTPUT_FILE})
+    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+    set(PYLIB_SO_NAME lib${moduleName}_python.so)
+  ENDIF(MSVC)
 
-  # On OSX and Linux, the python library must end in the extension .so. Build this
+  # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
-  # filename here.
+  set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
-  get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION)
+  # Cause the library to be output in the correct directory.
-  get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+  add_custom_command(TARGET ${moduleName}_python
-  set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so)
+    POST_BUILD
+    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+    COMMENT "Copying library files to python directory" )
 
   # Cause the library to be output in the correct directory.
   add_custom_command(TARGET ${TARGET_NAME}
@@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
   get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
   list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
   set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") 
-endfunction(wrap_python)
+endfunction(wrap_python)
+
+# Macro to get list of subdirectories
+macro(SUBDIRLIST result curdir)
+  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+  set(dirlist "")
+  foreach(child ${children})
+    if(IS_DIRECTORY ${curdir}/${child})
+        list(APPEND dirlist ${child})
+    endif()
+  endforeach()
+  set(${result} ${dirlist})
+endmacro()

examples/CreateSFMExampleData.cpp

@@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
   SfM_data data;
 
   // Create two cameras
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 identity, aPb(aRb, aTb);
   data.cameras.push_back(SfM_Camera(pose1, K));
@@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 void create5PointExample1() {
 
   // Create two cameras poses
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
@@ -85,7 +85,7 @@ void create5PointExample1() {
 void create5PointExample2() {
 
   // Create two cameras poses
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 

gtsam.h

@@ -434,11 +434,11 @@ class Rot3 {
   static gtsam::Rot3 Rz(double t);
   static gtsam::Rot3 RzRyRx(double x, double y, double z);
   static gtsam::Rot3 RzRyRx(Vector xyz);
-  static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading)
+  static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
-  static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
+  static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
-  static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
+  static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
-  static gtsam::Rot3 ypr(double y, double p, double r);
+  static gtsam::Rot3 Ypr(double y, double p, double r);
-  static gtsam::Rot3 quaternion(double w, double x, double y, double z);
+  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
   static gtsam::Rot3 Rodrigues(Vector v);
 
   // Testable

gtsam/geometry/Pose3.cpp

@@ -120,9 +120,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
   Rot3 R = Rot3::Expmap(omega.vector());
   double theta2 = omega.dot(omega);
   if (theta2 > std::numeric_limits<double>::epsilon()) {
-    double omega_v = omega.dot(v);          // translation parallel to axis
+    Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
-    Point3 omega_cross_v = omega.cross(v);  // points towards axis
+    Point3 omega_cross_v = omega.cross(v);    // points towards axis
-    Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2;
+    Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
     return Pose3(R, t);
   } else {
     return Pose3(R, v);

gtsam/geometry/Rot3.cpp

@@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const {
 
 /* ************************************************************************* */
 Vector Rot3::quaternion() const {
-  Quaternion q = toQuaternion();
+  gtsam::Quaternion q = toQuaternion();
   Vector v(4);
   v(0) = q.w();
   v(1) = q.x();

gtsam/geometry/Rot3.h

@@ -59,7 +59,7 @@ namespace gtsam {
 
 #ifdef GTSAM_USE_QUATERNIONS
     /** Internal Eigen Quaternion */
-    Quaternion quaternion_;
+    gtsam::Quaternion quaternion_;
 #else
     Matrix3 rot_;
 #endif
@@ -146,13 +146,13 @@ namespace gtsam {
     }
 
     /// Positive yaw is to right (as in aircraft heading). See ypr
-    static Rot3 yaw  (double t) { return Rz(t); }
+    static Rot3 Yaw  (double t) { return Rz(t); }
 
     /// Positive pitch is up (increasing aircraft altitude).See ypr
-    static Rot3 pitch(double t) { return Ry(t); }
+    static Rot3 Pitch(double t) { return Ry(t); }
 
     //// Positive roll is to right (increasing yaw in aircraft).
-    static Rot3 roll (double t) { return Rx(t); }
+    static Rot3 Roll (double t) { return Rx(t); }
 
     /**
      * Returns rotation nRb from body to nav frame.
@@ -163,11 +163,11 @@ namespace gtsam {
      * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf.
      * Assumes vehicle coordinate frame X forward, Y right, Z down.
      */
-    static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
+    static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
 
     /** Create from Quaternion coefficients */
-    static Rot3 quaternion(double w, double x, double y, double z) {
+    static Rot3 Quaternion(double w, double x, double y, double z) {
-      Quaternion q(w, x, y, z);
+      gtsam::Quaternion q(w, x, y, z);
       return Rot3(q);
     }
 
@@ -179,7 +179,7 @@ namespace gtsam {
      */
     static Rot3 AxisAngle(const Vector3& axis, double angle) {
 #ifdef GTSAM_USE_QUATERNIONS
-      return Quaternion(Eigen::AngleAxis<double>(angle, axis));
+      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
 #else
       return SO3::AxisAngle(axis,angle);
 #endif
@@ -313,7 +313,7 @@ namespace gtsam {
     static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
       if(H) *H = Rot3::ExpmapDerivative(v);
 #ifdef GTSAM_USE_QUATERNIONS
-      return traits<Quaternion>::Expmap(v);
+      return traits<gtsam::Quaternion>::Expmap(v);
 #else
       return traits<SO3>::Expmap(v);
 #endif
@@ -419,13 +419,13 @@ namespace gtsam {
 
     /**
      * Use RQ to calculate yaw-pitch-roll angle representation
-     * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
+     * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
      */
     Vector3 ypr() const;
 
     /**
      * Use RQ to calculate roll-pitch-yaw angle representation
-     * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
+     * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
      */
     Vector3 rpy() const;
 
@@ -460,7 +460,7 @@ namespace gtsam {
     /** Compute the quaternion representation of this rotation.
      * @return The quaternion
      */
-    Quaternion toQuaternion() const;
+    gtsam::Quaternion toQuaternion() const;
 
     /**
      * Converts to a generic matrix to allow for use with matlab
@@ -479,6 +479,8 @@ namespace gtsam {
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
     /// @}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
     /// @name Deprecated
     /// @{
     static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
@@ -486,7 +488,15 @@ namespace gtsam {
     static Rot3 rodriguez(const Unit3&   axis, double angle) { return AxisAngle(axis, angle); }
     static Rot3 rodriguez(const Vector3& w)                  { return Rodrigues(w); }
     static Rot3 rodriguez(double wx, double wy, double wz)   { return Rodrigues(wx, wy, wz); }
-    /// @}
+    static Rot3 yaw  (double t) { return Yaw(t); }
+    static Rot3 pitch(double t) { return Pitch(t); }
+    static Rot3 roll (double t) { return Roll(t); }
+    static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);}
+    static Rot3 quaternion(double w, double x, double y, double z) {
+      return Rot3::Quaternion(w, x, y, z);
+    }
+  /// @}
+#endif
 
   private:
 

gtsam/geometry/Rot3M.cpp

@@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13,
 }
 
 /* ************************************************************************* */
-Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
+Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) {
 }
 
 /* ************************************************************************* */
@@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
 Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
 
 /* ************************************************************************* */
-Quaternion Rot3::toQuaternion() const {
+gtsam::Quaternion Rot3::toQuaternion() const {
-  return Quaternion(rot_);
+  return gtsam::Quaternion(rot_);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3Q.cpp

@@ -47,30 +47,30 @@ namespace gtsam {
             R31, R32, R33).finished()) {}
 
   /* ************************************************************************* */
-  Rot3::Rot3(const Quaternion& q) :
+  Rot3::Rot3(const gtsam::Quaternion& q) :
       quaternion_(q) {
   }
 
   /* ************************************************************************* */
   Rot3 Rot3::Rx(double t) {
-    return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
+    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
   }
 
   /* ************************************************************************* */
   Rot3 Rot3::Ry(double t) {
-    return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
+    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
   }
 
   /* ************************************************************************* */
   Rot3 Rot3::Rz(double t) {
-    return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
+    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
   }
 
   /* ************************************************************************* */
   Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
-      Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
+      gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
-      Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
+      gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
-      Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
+      gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
   }
 
   /* ************************************************************************* */
@@ -98,7 +98,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
-    return traits<Quaternion>::Logmap(R.quaternion_, H);
+    return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);
   }
 
   /* ************************************************************************* */
@@ -128,7 +128,7 @@ namespace gtsam {
   Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
 
   /* ************************************************************************* */
-  Quaternion Rot3::toQuaternion() const { return quaternion_; }
+  gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
  /* ************************************************************************* */
 

gtsam/geometry/SO3.cpp

@@ -47,7 +47,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
     // get components of axis \omega, where is a unit vector
     const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
 
-    const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
+    const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
     const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
                  wz_sintheta = wz * sintheta;
 
@@ -130,7 +130,6 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
 
 /* ************************************************************************* */
 Matrix3 SO3::ExpmapDerivative(const Vector3& omega)    {
-  using std::cos;
   using std::sin;
 
   const double theta2 = omega.dot(omega);

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -135,7 +135,7 @@ TEST( CalibratedCamera, Dproject_point_pose)
 // Add a test with more arbitrary rotation
 TEST( CalibratedCamera, Dproject_point_pose2)
 {
-  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
   static const CalibratedCamera camera(pose1);
   Matrix Dpose, Dpoint;
   camera.project(point1, Dpose, Dpoint);
@@ -165,7 +165,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
 // Add a test with more arbitrary rotation
 TEST( CalibratedCamera, Dproject_point_pose2_infinity)
 {
-  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
   static const CalibratedCamera camera(pose1);
   Matrix Dpose, Dpoint;
   camera.project2(pointAtInfinity, Dpose, Dpoint);

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -20,7 +20,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
 
 //*************************************************************************
 // Create two cameras and corresponding essential matrix E
-Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
+Rot3 c1Rc2 = Rot3::Yaw(M_PI_2);
 Point3 c1Tc2(0.1, 0, 0);
 EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
 
@@ -98,8 +98,8 @@ Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
 }
 TEST (EssentialMatrix, transform_to) {
   // test with a more complicated EssentialMatrix
-  Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4)
+  Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4)
-      * Rot3::roll(M_PI / 6.0);
+      * Rot3::Roll(M_PI / 6.0);
   Point3 aTb2(19.2, 3.7, 5.9);
   EssentialMatrix E(aRb2, Unit3(aTb2));
   //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0)));
@@ -159,7 +159,7 @@ TEST (EssentialMatrix, FromPose3_a) {
 //*************************************************************************
 TEST (EssentialMatrix, FromPose3_b) {
   Matrix actualH;
-  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
   Point3 c1Tc2(0.4, 0.5, 0.6);
   EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
@@ -181,7 +181,7 @@ TEST (EssentialMatrix, streaming) {
 //*************************************************************************
 TEST (EssentialMatrix, epipoles) {
   // Create an E
-  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
   Point3 c1Tc2(0.4, 0.5, 0.6);
   EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -59,7 +59,7 @@ OrientedPlane3 transform_(const OrientedPlane3& plane,  const Pose3& xr) {
 }
 
 TEST (OrientedPlane3, transform) {
-  gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
+  gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0),
       gtsam::Point3(2.0, 3.0, 4.0));
   OrientedPlane3 plane(-1, 0, 0, 5);
   OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -242,7 +242,7 @@ TEST( PinholeCamera, Dproject2)
 // Add a test with more arbitrary rotation
 TEST( PinholeCamera, Dproject3)
 {
-  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
   static const Camera camera(pose1);
   Matrix Dpose, Dpoint;
   camera.project2(point1, Dpose, Dpoint);

gtsam/geometry/tests/testPinholePose.cpp

@@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2)
 // Add a test with more arbitrary rotation
 TEST( CalibratedCamera, Dproject3)
 {
-  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
   static const Camera camera(pose1);
   Matrix Dpose, Dpoint;
   camera.project2(point1, Dpose, Dpoint);

gtsam/geometry/tests/testPoint3.cpp

@@ -133,6 +133,17 @@ TEST (Point3, distance) {
   EXPECT(assert_equal(numH2, H2, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(Point3, cross) {
+  Matrix aH1, aH2;
+  boost::function<Point3(const Point3&, const Point3&)> f =
+      boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
+  const Point3 omega(0, 1, 0), theta(4, 6, 8);
+  omega.cross(theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPose3.cpp

@@ -556,12 +556,12 @@ TEST( Pose3, between )
 /* ************************************************************************* */
 // some shared test values - pulled from equivalent test in Pose2
 Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
-Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2);
+Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2);
 Pose3
-    xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
+    xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
-    xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
+    xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
-    xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
+    xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
-    xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
+    xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
 
 /* ************************************************************************* */
 double range_proxy(const Pose3& pose, const Point3& point) {
@@ -654,9 +654,9 @@ TEST( Pose3, unicycle )
 {
   // velocity in X should be X in inertial frame, rather than global frame
   Vector x_step = delta(6,3,1.0);
-  EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
+  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
-  EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
+  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
-  EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
+  EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
 }
 
 /* ************************************************************************* */
@@ -715,7 +715,42 @@ TEST( Pose3, ExpmapDerivative1) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, LogmapDerivative1) {
+TEST(Pose3, ExpmapDerivative2) {
+  // Iserles05an (Lie-group Methods) says:
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
+  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
+  // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3)
+  // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
+  // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
+
+  // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
+  // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)
+
+  // Let's verify the above formula.
+
+  auto xi = [](double t) {
+    Vector6 v;
+    v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
+    return v;
+  };
+  auto xi_dot = [](double t) {
+    Vector6 v;
+    v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
+    return v;
+  };
+
+  // We define a function T
+  auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
+    const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
+}
+
+/* ************************************************************************* */
+TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;
   Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
   Pose3 p = Pose3::Expmap(w);

gtsam/geometry/tests/testRot3.cpp

@@ -244,44 +244,99 @@ TEST(Rot3, retract_localCoordinates2)
   EXPECT(assert_equal(t1, t2.retract(d21)));
 }
 /* ************************************************************************* */
-Vector w = Vector3(0.1, 0.27, -0.2);
+namespace exmap_derivative {
-
+static const Vector3 w(0.1, 0.27, -0.2);
-// Left trivialization Derivative of exp(w) wrpt w:
+}
+// Left trivialized Derivative of exp(w) wrpt w:
 // How does exp(w) change when w changes?
 // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
 // => y = log (exp(-w) * exp(w+dw))
 Vector3 testDexpL(const Vector3& dw) {
+  using exmap_derivative::w;
   return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
 }
 
 TEST( Rot3, ExpmapDerivative) {
-  Matrix actualDexpL = Rot3::ExpmapDerivative(w);
+  using exmap_derivative::w;
-  Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
+  const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
+  const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
       Vector3::Zero(), 1e-2);
   EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
 
-  Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
+  const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
+  }
+
+/* ************************************************************************* */
+TEST( Rot3, ExpmapDerivative2)
+{
+  const Vector3 theta(0.1, 0, 0.1);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
+
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
 }
 
 /* ************************************************************************* */
-Vector3 thetahat(0.1, 0, 0.1);
+TEST( Rot3, ExpmapDerivative3)
-TEST( Rot3, ExpmapDerivative2)
 {
-  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+  const Vector3 theta(10, 20, 30);
-      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
 
-  Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
-  CHECK(assert_equal(Jexpected, Jactual));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
+}
 
-  Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
+/* ************************************************************************* */
-  CHECK(assert_equal(Jexpected, Jactual2));
+TEST(Rot3, ExpmapDerivative4) {
+  // Iserles05an (Lie-group Methods) says:
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
+  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
+  // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
+  // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
+  // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
+
+  // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors.
+  // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
+
+  // Let's verify the above formula.
+
+  auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
+  auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
+
+  // We define a function R
+  auto R = [w](double t) { return Rot3::Expmap(w(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
+    const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
+}
+
+/* ************************************************************************* */
+TEST(Rot3, ExpmapDerivative5) {
+  auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
+  auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
+
+  // Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
+  const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2);
+  auto R = [R0, w](double t) { return R0.expmap(w(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
+    const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
 }
 
 /* ************************************************************************* */
 TEST( Rot3, jacobianExpmap )
 {
-  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
+  const Vector3 thetahat(0.1, 0, 0.1);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
       &Rot3::Expmap, _1, boost::none), thetahat);
   Matrix3 Jactual;
   const Rot3 R = Rot3::Expmap(thetahat, Jactual);
@@ -291,18 +346,20 @@ TEST( Rot3, jacobianExpmap )
 /* ************************************************************************* */
 TEST( Rot3, LogmapDerivative )
 {
-  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Vector3 thetahat(0.1, 0, 0.1);
-  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+  const Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
       &Rot3::Logmap, _1, boost::none), R);
-  Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
+  const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, jacobianLogmap )
+TEST( Rot3, JacobianLogmap )
 {
-  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Vector3 thetahat(0.1, 0, 0.1);
-  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+  const Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
       &Rot3::Logmap, _1, boost::none), R);
   Matrix3 Jactual;
   Rot3::Logmap(R, Jactual);
@@ -501,17 +558,17 @@ TEST( Rot3, yaw_pitch_roll )
   double t = 0.1;
 
   // yaw is around z axis
-  CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
+  CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t)));
 
   // pitch is around y axis
-  CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
+  CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t)));
 
   // roll is around x axis
-  CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
+  CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t)));
 
   // Check compound rotation
-  Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
+  Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3);
-  CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
+  CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3)));
 
   CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr()));
 }
@@ -531,14 +588,14 @@ TEST( Rot3, RQ)
   CHECK(assert_equal(expected,R.xyz(),1e-6));
   CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
 
-  // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
+  // Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r]
-  CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
+  CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr()));
-  CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
+  CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy()));
 
   // Try ypr for pure yaw-pitch-roll matrices
-  CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
+  CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr()));
-  CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
+  CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr()));
-  CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
+  CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr()));
 
   // Try RQ to recover calibration from 3*3 sub-block of projection matrix
   Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();
@@ -594,9 +651,9 @@ TEST(Rot3, quaternion) {
 
   // Check creating Rot3 from quaternion
   EXPECT(assert_equal(R1, Rot3(q1)));
-  EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
+  EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
   EXPECT(assert_equal(R2, Rot3(q2)));
-  EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
+  EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
 
   // Check converting Rot3 to quaterion
   EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));

gtsam/geometry/tests/testTriangulation.cpp

@@ -40,7 +40,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
     boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
 
 // Looking along X-axis, 1 meter above ground plane (x-y)
-static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
+static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
 static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
 PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
@@ -150,7 +150,7 @@ TEST( triangulation, fourPoses) {
   EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   SimpleCamera camera3(pose3, *sharedCal);
   Point2 z3 = camera3.project(landmark);
 
@@ -167,7 +167,7 @@ TEST( triangulation, fourPoses) {
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
-  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   SimpleCamera camera4(pose4, *sharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
@@ -214,7 +214,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
   SimpleCamera camera3(pose3, K3);
   Point2 z3 = camera3.project(landmark);
@@ -232,7 +232,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
-  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   Cal3_S2 K4(700, 500, 0, 640, 480);
   SimpleCamera camera4(pose4, K4);
 

gtsam/geometry/tests/testUnit3.cpp

@@ -70,7 +70,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) {
 }
 
 TEST(Unit3, rotate) {
-  Rot3 R = Rot3::yaw(0.5);
+  Rot3 R = Rot3::Yaw(0.5);
   Unit3 p(1, 0, 0);
   Unit3 expected = Unit3(R.column(1));
   Unit3 actual = R * p;
@@ -95,7 +95,7 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
 }
 
 TEST(Unit3, unrotate) {
-  Rot3 R = Rot3::yaw(-M_PI / 4.0);
+  Rot3 R = Rot3::Yaw(-M_PI / 4.0);
   Unit3 p(1, 0, 0);
   Unit3 expected = Unit3(1, 1, 0);
   Unit3 actual = R.unrotate(p);

gtsam/geometry/triangulation.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>

gtsam/linear/GaussianBayesNet.cpp

@@ -141,7 +141,20 @@ namespace gtsam {
   /* ************************************************************************* */  
   pair<Matrix,Vector> GaussianBayesNet::matrix() const
   {
-    return GaussianFactorGraph(*this).jacobian();
+    GaussianFactorGraph factorGraph(*this);
+    KeySet keys = factorGraph.keys();
+    // add frontal keys in order
+    Ordering ordering;
+    BOOST_FOREACH (const sharedConditional& cg, *this)
+      BOOST_FOREACH (Key key, cg->frontals()) {
+        ordering.push_back(key);
+        keys.erase(key);
+      }
+    // add remaining keys in case Bayes net is incomplete
+    BOOST_FOREACH (Key key, keys)
+      ordering.push_back(key);
+    // return matrix and RHS
+    return factorGraph.jacobian(ordering);
   }
 
   ///* ************************************************************************* */

gtsam/navigation/GPSFactor.cpp

@@ -58,10 +58,10 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
 
   // Estimate Rotation
   double yaw = atan2(nV.y(), nV.x());
-  Rot3 nRy = Rot3::yaw(yaw); // yaw frame
+  Rot3 nRy = Rot3::Yaw(yaw); // yaw frame
   Point3 yV = nRy.inverse() * nV; // velocity in yaw frame
   double pitch = -atan2(yV.z(), yV.x()), roll = 0;
-  Rot3 nRb = Rot3::ypr(yaw, pitch, roll);
+  Rot3 nRb = Rot3::Ypr(yaw, pitch, roll);
 
   // Construct initial pose
   Pose3 nTb(nRb, nT); // nTb

gtsam/navigation/MagFactor.h

@@ -60,7 +60,7 @@ public:
 
   static Point3 unrotate(const Rot2& R, const Point3& p,
       boost::optional<Matrix&> HR = boost::none) {
-    Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
+    Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
     if (HR) {
       // assign to temporary first to avoid error in Win-Debug mode
       Matrix H = HR->col(2);

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -423,7 +423,7 @@ TEST (AHRSFactor, predictTest) {
 
   // Predict
   Rot3 x;
-  Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0);
+  Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
   Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
   EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
 

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -293,10 +293,10 @@ TEST(CombinedImuFactor, PredictRotation) {
       gravity, omegaCoriolis);
 
   // Predict
-  Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
+  Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
   Vector3 v(0, 0, 0), v2;
   CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
-  Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
+  Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
   EXPECT(assert_equal(expectedPose, x2, tol));
 }
 

gtsam/navigation/tests/testGPSFactor.cpp

@@ -89,7 +89,7 @@ TEST(GPSData, init) {
 
   // Check values values
   EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
-  EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
+  EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
   Point3 expectedT(2.38461, -2.31289, -0.156011);
   EXPECT(assert_equal(expectedT, T.translation(), 1e-5));
 }

gtsam/navigation/tests/testImuFactor.cpp

@@ -813,7 +813,7 @@ TEST(ImuFactor, PredictRotation) {
   Vector3 v2;
   ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
-  Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
+  Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
   Vector3 expectedVelocity;
   expectedVelocity << 0, 0, 0;
   EXPECT(assert_equal(expectedPose, x2));
@@ -891,7 +891,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
   double dt = 0.001;
 
   // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
-  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
+  Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
 
   ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true);
 
@@ -907,7 +907,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
   PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity,
       omegaCoriolis);
 
-  Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
+  Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
   EXPECT(assert_equal(expectedPose, poseVelocity.pose));
 
   Vector3 expectedVelocity(0, 0, 0);
@@ -942,7 +942,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   // table exerts an equal and opposite force w.r.t gravity
   Vector3 measuredAcc(0, 0, -9.81);
 
-  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
+  Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3());
 
   Matrix3 accCov = 1e-7 * I_3x3;
   Matrix3 gyroCov = 1e-8 * I_3x3;
@@ -1025,7 +1025,7 @@ TEST(ImuFactor, serialization) {
   using namespace gtsam::serializationTestHelpers;
 
   Vector3 n_gravity(0, 0, -9.81);
-  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
+  Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3());
   Matrix3 accCov = 1e-7 * I_3x3;
   Matrix3 gyroCov = 1e-8 * I_3x3;
   Matrix3 integrationCov = 1e-9 * I_3x3;

gtsam/navigation/tests/testMagFactor.cpp

@@ -40,7 +40,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862);
 // Let's assume scale factor,
 double scale = 255.0 / 50000.0;
 // ...ground truth orientation,
-Rot3 nRb = Rot3::yaw(-0.1);
+Rot3 nRb = Rot3::Yaw(-0.1);
 Rot2 theta = nRb.yaw();
 // ...and bias
 Point3 bias(10, -10, 50);

gtsam/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -62,7 +62,7 @@ using namespace gtsam;
 // Check that ceres rotation convention is the same
 TEST(AdaptAutoDiff, Rotation) {
   Vector3 axisAngle(0.1, 0.2, 0.3);
-  Matrix3 expected = Rot3::rodriguez(axisAngle).matrix();
+  Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix();
   Matrix3 actual;
   ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data());
   EXPECT(assert_equal(expected, actual));

gtsam/sam/RangeFactor.h

@@ -31,8 +31,7 @@ struct Range;
  * Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
  * @addtogroup SAM
  */
-template <typename A1, typename A2 = A1,
+template <typename A1, typename A2 = A1, typename T = double>
-          typename T = typename Range<A1, A2>::result_type>
 class RangeFactor : public ExpressionFactor2<T, A1, A2> {
  private:
   typedef RangeFactor<A1, A2> This;

gtsam/slam/dataset.cpp

@@ -510,7 +510,7 @@ GraphAndValues load3D(const string& filename) {
       Key id;
       double x, y, z, roll, pitch, yaw;
       ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::ypr(yaw,pitch,roll);
+      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
       Point3 t = Point3(x, y, z);
       initial->insert(id, Pose3(R,t));
     }
@@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) {
       Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
+      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
       Point3 t = Point3(x, y, z);
       initial->insert(id, Pose3(R,t));
     }
@@ -537,7 +537,7 @@ GraphAndValues load3D(const string& filename) {
       Key id1, id2;
       double x, y, z, roll, pitch, yaw;
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::ypr(yaw,pitch,roll);
+      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
       Point3 t = Point3(x, y, z);
       Matrix m = eye(6);
       for (int i = 0; i < 6; i++)
@@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) {
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
+      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
       Point3 t = Point3(x, y, z);
       for (int i = 0; i < 6; i++){
         for (int j = i; j < 6; j++){

gtsam/slam/tests/smartFactorScenarios.h

@@ -34,7 +34,7 @@ Point3 landmark4(10, 0.5, 1.2);
 Point3 landmark5(10, -0.5, 1.2);
 
 // First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
-Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
 // Second camera 1 meter to the right of first camera
 Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
 // Third camera 1 meter above the first camera
@@ -123,7 +123,7 @@ Camera cam3(pose_above, sharedBundlerK);
 
 template<class CAMERA>
 CAMERA perturbCameraPose(const CAMERA& camera) {
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Pose3 cameraPose = camera.pose();
   Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);

gtsam/slam/tests/testDataset.cpp

@@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D)
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
 
   Values expectedValues;
-  Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
+  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
   Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
   expectedValues.insert(0, Pose3(R0, p0));
 
-  Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
+  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
   Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
   expectedValues.insert(1, Pose3(R1, p1));
 
-  Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
+  Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
   Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
   expectedValues.insert(2, Pose3(R2, p2));
 
-  Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323);
+  Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
   Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
   expectedValues.insert(3, Pose3(R3, p3));
 
-  Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933);
+  Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
   Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
   expectedValues.insert(4, Pose3(R4, p4));
 
@@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D)
   NonlinearFactorGraph expectedGraph;
 
   Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
+  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
   expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
 
   Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
-  Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
+  Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
   expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model));
 
   Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
-  Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
+  Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
   expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model));
 
   Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
-  Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
+  Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
   expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model));
 
   Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
-  Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
+  Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
   expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model));
 
   Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
-  Rot3 R30 = Rot3::quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
+  Rot3 R30 = Rot3::Quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
   expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model));
 
   EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
@@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
 
   Values expectedValues;
-  Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
+  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
   Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
   expectedValues.insert(0, Pose3(R0, p0));
 
-  Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
+  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
   Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
   expectedValues.insert(1, Pose3(R1, p1));
 
@@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
   noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
   NonlinearFactorGraph expectedGraph;
   Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
+  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
   expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
 
   EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
@@ -452,7 +452,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
   SfM_data readData;
   readBAL(filenameToRead, readData);
 
-  Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
+  Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
 
   Values value;
   for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -47,7 +47,7 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
 
   // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
   Symbol init_sym('x', 0);
-  Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
+  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
   Vector sigmas(6);
   sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
   PriorFactor<Pose3> pose_prior(init_sym, init_pose,
@@ -94,7 +94,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
 
   // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
   Symbol init_sym('x', 0);
-  Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
+  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
   PriorFactor<Pose3> pose_prior(init_sym, init_pose,
       noiseModel::Diagonal::Sigmas(
           (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));

gtsam/slam/tests/testPoseRotationPrior.cpp

@@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1;
 
 // Pose3 examples
 const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
-const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
+const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
 
 // Pose2 examples
 const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);

gtsam/slam/tests/testPoseTranslationPrior.cpp

@@ -27,7 +27,7 @@ const gtsam::Key poseKey = 1;
 
 // Pose3 examples
 const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
-const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
+const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
 
 // Pose2 examples
 const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -47,7 +47,7 @@ template<class CALIBRATION>
 PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
     const PinholeCamera<CALIBRATION>& camera) {
   GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Pose3 cameraPose = camera.pose();
   Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
@@ -61,7 +61,7 @@ PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, perturbCameraPose) {
   using namespace vanilla;
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
   Camera actualCamera(perturbed_level_pose, K2);

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -163,7 +163,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
 
   Values values;
   values.insert(x1, cam1.pose());
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   values.insert(x2, pose_right.compose(noise_pose));
 
@@ -196,7 +196,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
+  Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
   Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
   Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
 
@@ -205,7 +205,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   SimpleCamera cam3(cameraPose3, *K);
 
   // create arbitrary body_Pose_sensor (transforms from sensor to body)
-  Pose3 sensor_to_body =  Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
+  Pose3 sensor_to_body =  Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
 
   // These are the poses we want to estimate, from camera measurements
   Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
@@ -263,7 +263,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   double expectedError = 0.0;
   DOUBLES_EQUAL(expectedError, actualError, 1e-7)
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
   Values values;
   values.insert(x1, bodyPose1);
   values.insert(x2, bodyPose2);
@@ -317,8 +317,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   groundTruth.insert(x3, cam3.pose());
   DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -539,8 +539,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -606,8 +606,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -667,8 +667,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
   graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -792,7 +792,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -844,7 +844,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
   graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Values values;
   values.insert(x1, level_pose);
@@ -908,8 +908,8 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -995,7 +995,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   graph.push_back(
       PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -1063,8 +1063,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   graph.push_back(
       PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -1108,7 +1108,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
   smartFactor1->add(measurements_cam1, views);
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Values values;
   values.insert(x1, cam1.pose());
@@ -1148,7 +1148,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
   boost::shared_ptr<GaussianFactor> factor = smartFactorInstance->linearize(
       values);
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
+  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(level_pose));
@@ -1161,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
   // Hessian is invariant to rotations in the nondegenerate case
   EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
       Point3(10, -4, 5));
 
   Values tranValues;
@@ -1203,7 +1203,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
 
   boost::shared_ptr<GaussianFactor> factor = smartFactor->linearize(values);
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
+  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(level_pose));
@@ -1216,7 +1216,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
   // Hessian is invariant to rotations in the nondegenerate case
   EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
       Point3(10, -4, 5));
 
   Values tranValues;
@@ -1278,8 +1278,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());
@@ -1357,8 +1357,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   graph.push_back(
       PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, cam1.pose());

gtsam/slam/tests/testTriangulationFactor.cpp

@@ -37,7 +37,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
     boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
 
 // Looking along X-axis, 1 meter above ground plane (x-y)
-static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
+static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
 static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
 SimpleCamera camera1(pose1, *sharedCal);
 

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -100,7 +100,7 @@ PoseRTV PoseRTV::flyingDynamics(
   double pitch2 = r2.pitch();
   double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
   double loss_lift = lift*fabs(sin(pitch2));
-  Rot3 yaw_correction_bn = Rot3::yaw(yaw2);
+  Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
   Point3 forward(forward_accel, 0.0, 0.0);
   Vector Acc_n =
       yaw_correction_bn.rotate(forward).vector()              // applies locally forward force in the global frame

gtsam_unstable/dynamics/tests/testIMUSystem.cpp

@@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
+          pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
-          pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
+          pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
+          pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Unit::Create(6);
@@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
+          pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
-          pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
+          pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
+          pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -193,7 +193,7 @@ TEST( testPoseRTV, transformed_from_1 ) {
   Point3 T(1.0, 2.0, 3.0);
   Velocity3 V(2.0, 3.0, 4.0);
   PoseRTV start(R, T, V);
-  Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
+  Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
@@ -212,7 +212,7 @@ TEST( testPoseRTV, transformed_from_2 ) {
   Point3 T(1.0, 2.0, 3.0);
   Velocity3 V(2.0, 3.0, 4.0);
   PoseRTV start(R, T, V);
-  Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
+  Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0));
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
@@ -229,14 +229,14 @@ TEST( testPoseRTV, transformed_from_2 ) {
 TEST(testPoseRTV, RRTMbn) {
   EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
   EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
-  EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3))));
+  EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
 }
 
 /* ************************************************************************* */
 TEST(testPoseRTV, RRTMnb) {
   EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
   EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
-  EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3))));
+  EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
 }
 
 /* ************************************************************************* */

gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp

@@ -16,7 +16,7 @@ const double tol=1e-5;
 const double h = 0.01;
 
 //const double deg2rad = M_PI/180.0;
-//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
+//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
 Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
 //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished());
 Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished());

gtsam_unstable/geometry/Pose3Upright.cpp

@@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const {
 
 /* ************************************************************************* */
 Rot3 Pose3Upright::rotation() const {
-  return Rot3::yaw(theta());
+  return Rot3::Yaw(theta());
 }
 
 /* ************************************************************************* */

gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp

@@ -17,7 +17,7 @@ using namespace std;
 using namespace gtsam;
 
 static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
-Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
 SimpleCamera level_camera(level_pose, *K);
 
 /* ************************************************************************* */
@@ -142,7 +142,7 @@ TEST(InvDepthFactor, backproject2)
   // backwards facing camera
   Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished());
   double inv_depth(1./10);
-  InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
+  InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
   Point2 z = inv_camera.project(expected, inv_depth);
 
   Vector5 actual_vec;

gtsam_unstable/geometry/tests/testPose3Upright.cpp

@@ -58,9 +58,9 @@ TEST( testPose3Upright, conversions ) {
   EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol));
   EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol));
   EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol));
-  EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol));
+  EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol));
   EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol));
-  EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
+  EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
 }
 
 /* ************************************************************************* */

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -56,14 +56,14 @@ TEST(Similarity3, Getters) {
 
 //******************************************************************************
 TEST(Similarity3, Getters2) {
-  Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
+  Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
-  EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
+  EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation()));
   EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
   EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
 }
 
 TEST(Similarity3, AdjointMap) {
-  Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
   Matrix7 result;
   result <<    -1.5739,   -2.4512,   -6.3651,  -50.7671,  -11.2503,   16.8859,  -28.0000,
       6.3167,   -2.9884,   -0.4111,    0.8502,    8.6373,  -49.7260,  -35.0000,
@@ -76,7 +76,7 @@ TEST(Similarity3, AdjointMap) {
 }
 
 TEST(Similarity3, inverse) {
-  Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
   Matrix3 Re;
   Re <<    -0.2248,    0.9024,   -0.3676,
    -0.3502,   -0.4269,   -0.8337,
@@ -87,8 +87,8 @@ TEST(Similarity3, inverse) {
 }
 
 TEST(Similarity3, multiplication) {
-  Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7);
-  Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
+  Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11);
   Matrix3 re;
   re << 0.0688,    0.9863,   -0.1496,
      -0.5665,   -0.0848,   -0.8197,
@@ -117,14 +117,14 @@ TEST(Similarity3, Manifold) {
   v3 << 0, 0, 0, 1, 2, 3, 0;
   EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
 
-//  Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
+//  Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
-  Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
+  Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
 
   Vector vlocal = sim.localCoordinates(other);
 
   EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
 
-  Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
+  Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1);
   Rot3 R = Rot3::Rodrigues(0.3,0,0);
 
   Vector vlocal2 = sim.localCoordinates(other2);
@@ -167,7 +167,7 @@ TEST(Similarity3, manifold_first_order)
 }
 
 TEST(Similarity3, Optimization) {
-  Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
+  Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
   noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
   Symbol key('x',1);
   PriorFactor<Similarity3> factor(key, prior, model);
@@ -187,10 +187,10 @@ TEST(Similarity3, Optimization) {
 
 TEST(Similarity3, Optimization2) {
   Similarity3 prior = Similarity3();
-  Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
+  Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
-  Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
+  Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
-  Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
+  Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
-  Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
+  Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
   Similarity3 loop = Similarity3(1.42);
 
   //prior.print("Goal Transform");
@@ -220,10 +220,10 @@ TEST(Similarity3, Optimization2) {
 
   Values initial;
   initial.insert<Similarity3>(X(1), Similarity3());
-  initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
+  initial.insert<Similarity3>(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
-  initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
+  initial.insert<Similarity3>(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
-  initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
+  initial.insert<Similarity3>(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
-  initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
+  initial.insert<Similarity3>(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
 
   //initial.print("Initial Estimate\n");
 

gtsam_unstable/slam/Mechanization_bRn2.cpp

@@ -60,7 +60,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
   double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3));
   double yaw = 0;
   // This returns body-to-nav nRb
-  Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse();
+  Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse();
 
   return Mechanization_bRn2(bRn, x_g, x_a);
 }

gtsam_unstable/slam/tests/testInvDepthFactor3.cpp

@@ -22,7 +22,7 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
 static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
 
 // camera pose at (0,0,1) looking straight along the x-axis.
-Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
 SimpleCamera level_camera(level_pose, *K);
 
 typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;

gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp

@@ -24,8 +24,8 @@ using namespace gtsam;
 TEST( InvDepthFactorVariant1, optimize) {
 
   // Create two poses looking in the x-direction
-  Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
+  Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
-  Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
+  Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
 
   // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
   Point3 landmark(5, 0, 1);

gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp

@@ -24,8 +24,8 @@ using namespace gtsam;
 TEST( InvDepthFactorVariant2, optimize) {
 
   // Create two poses looking in the x-direction
-  Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
+  Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
-  Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
+  Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
 
   // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
   Point3 landmark(5, 0, 1);

gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp

@@ -24,8 +24,8 @@ using namespace gtsam;
 TEST( InvDepthFactorVariant3, optimize) {
 
   // Create two poses looking in the x-direction
-  Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
+  Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
-  Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
+  Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
 
   // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
   Point3 landmark(5, 0, 1);

gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp

@@ -18,7 +18,7 @@ SharedNoiseModel model1 = noiseModel::Unit::Create(1);
 const double tol = 1e-5;
 
 const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
-const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
+const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
 const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
 const Point3 point1(3.0, 4.0, 2.0);
 const gtsam::Key poseKey = 1, pointKey = 2;

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -115,7 +115,7 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
 /* *************************************************************************/
 TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+  Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
       Point3(0, 0, 1));
   StereoCamera level_camera(level_pose, K2);
 
@@ -154,7 +154,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, noisy ) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+  Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
       Point3(0, 0, 1));
   StereoCamera level_camera(level_pose, K2);
 
@@ -172,7 +172,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
 
   Values values;
   values.insert(x1, level_pose);
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   values.insert(x2, level_pose_right.compose(noise_pose));
 
@@ -206,7 +206,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
 TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K2);
 
   // create second camera 1 meter to the right of first camera
@@ -257,8 +257,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
@@ -354,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
   Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@@ -397,8 +397,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
@@ -422,7 +422,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
   Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@@ -466,8 +466,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
@@ -490,7 +490,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
   Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
@@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
@@ -587,7 +587,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K);
 //  // create second camera 1 meter to the right of first camera
 //  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
@@ -626,8 +626,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
 //  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 //
-//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+//  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -648,7 +648,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -684,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
 //  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -708,7 +708,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
 
   // create second camera
@@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   // initialize third pose with some noise, we expect it to move back to original pose3
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   values.insert(x3, pose3 * noise_pose);
 
@@ -796,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -835,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
 //  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2*noise_pose);
@@ -862,7 +862,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -908,8 +908,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
 //  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 //
-//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+//  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -935,7 +935,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  views.push_back(x2);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -955,7 +955,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
 //  smartFactor1->add(measurements_cam1,views, model, K2);
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -977,7 +977,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
 
   // create second camera 1 meter to the right of first camera
@@ -1005,7 +1005,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
       smartFactorInstance->linearize(values);
   // hessianFactor->print("Hessian factor \n");
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
+  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(pose1));
@@ -1021,7 +1021,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
       assert_equal(hessianFactor->information(),
           hessianFactorRot->information(), 1e-7));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
       Point3(10, -4, 5));
 
   Values tranValues;
@@ -1047,7 +1047,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K2);
 
   // Second and third cameras in same place, which is a degenerate configuration
@@ -1072,7 +1072,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
       values);
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
+  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(pose1));
@@ -1087,7 +1087,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
       assert_equal(hessianFactor->information(),
           hessianFactorRot->information(), 1e-6));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
       Point3(10, -4, 5));
 
   Values tranValues;

python/.gitignore

@@ -0,0 +1 @@
+build/

python/CMakeLists.txt

@@ -0,0 +1,96 @@
+# Guard to avoid breaking this code in ccmake if by accident GTSAM_PYTHON_VERSION is set to an empty string 
+if(GTSAM_PYTHON_VERSION STREQUAL "")
+	set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version" FORCE)
+endif()
+
+# The code below allows to clear the PythonLibs cache if we change GTSAM_PYTHON_VERSION
+# Inspired from the solution found here: http://blog.bethcodes.com/cmake-tips-tricks-drop-down-list
+if(NOT DEFINED GTSAM_LAST_PYTHON_VERSION)
+	set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Python version used in the last build")
+	mark_as_advanced(FORCE GTSAM_LAST_PYTHON_VERSION)
+endif()
+if(NOT (${GTSAM_PYTHON_VERSION} MATCHES ${GTSAM_LAST_PYTHON_VERSION}))
+	unset(PYTHON_INCLUDE_DIR CACHE)
+	unset(PYTHON_INCLUDE_DIR2 CACHE)
+	unset(PYTHON_LIBRARY CACHE)
+	unset(PYTHON_LIBRARY_DEBUG CACHE)
+	set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Updating python version used in the last build" FORCE)
+endif()
+
+if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+	# Search the default version.
+	find_package(PythonInterp)
+	find_package(PythonLibs)
+else()
+	find_package(PythonInterp ${GTSAM_PYTHON_VERSION})
+	find_package(PythonLibs ${GTSAM_PYTHON_VERSION})
+endif()
+
+# Find NumPy C-API -- this is part of the numpy package
+find_package(NumPy)
+
+# Compose strings used to specify the boost python version. They will be empty if we want to use the defaut
+if(NOT GTSAM_PYTHON_VERSION STREQUAL "Default")
+	string(REPLACE "." "" BOOST_PYTHON_VERSION_SUFFIX ${GTSAM_PYTHON_VERSION})           # Remove '.' from version
+	string(SUBSTRING ${BOOST_PYTHON_VERSION_SUFFIX} 0 2 BOOST_PYTHON_VERSION_SUFFIX)     # Trim version number to 2 digits
+	set(BOOST_PYTHON_VERSION_SUFFIX "-py${BOOST_PYTHON_VERSION_SUFFIX}")                 # Append '-py' prefix
+	string(TOUPPER ${BOOST_PYTHON_VERSION_SUFFIX} BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE) # Get uppercase string
+else()
+	set(BOOST_PYTHON_VERSION_SUFFIX "")
+	set(BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE "")
+endif()
+
+# Find Boost Python
+find_package(Boost COMPONENTS python${BOOST_PYTHON_VERSION_SUFFIX})
+
+if(Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND AND PYTHONLIBS_FOUND AND NUMPY_FOUND)
+	# Build library
+	include_directories(${NUMPY_INCLUDE_DIRS})
+	include_directories(${PYTHON_INCLUDE_DIRS})
+	include_directories(${Boost_INCLUDE_DIRS})
+	include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/)
+
+	# Build the python module library
+	add_subdirectory(handwritten)
+
+	# Create and invoke setup.py, see https://bloerg.net/2012/11/10/cmake-and-distutils.html
+    set(SETUP_PY_IN "${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in")
+    set(SETUP_PY    "${CMAKE_CURRENT_BINARY_DIR}/setup.py")
+
+	# Hacky way to figure out install folder - valid for Linux & Mac
+	# default pattern: prefix/lib/pythonX.Y/site-packages from https://docs.python.org/2/install/ 
+	SET(PY_INSTALL_FOLDER "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages") 
+
+    configure_file(${SETUP_PY_IN} ${SETUP_PY})
+
+	# TODO(frank): possibly support a different prefix a la matlab wrapper
+    install(CODE "execute_process(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_EXECUTABLE} setup.py -v install --prefix ${CMAKE_INSTALL_PREFIX})")
+else()
+	# Disable python module if we didn't find required libraries
+	# message will print at end of main CMakeLists.txt
+	SET(GTSAM_PYTHON_WARNINGS "Python dependencies not found - Python module will not be built. Set GTSAM_BUILD_PYTHON to 'Off' to disable this warning. Details:")
+	
+	if(NOT PYTHONLIBS_FOUND)
+		if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+			SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default PythonLibs not found")
+		else()
+			SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- PythonLibs version ${GTSAM_PYTHON_VERSION} not found")
+		endif()
+	endif()
+	
+	if(NOT NUMPY_FOUND)
+    	SET(GTSAM_PYTHON_WARNINGS  "${GTSAM_PYTHON_WARNINGS}\n -- Numpy not found")
+	endif()
+	
+	if(NOT Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND)
+		if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+			SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default Boost python not found")
+		else()
+			SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Boost Python for python ${GTSAM_PYTHON_VERSION} not found")
+		endif()
+	endif()
+	
+	# make available at top-level
+	SET(GTSAM_PYTHON_WARNINGS ${GTSAM_PYTHON_WARNINGS} PARENT_SCOPE)
+	
+endif()

python/README.md

@@ -0,0 +1,18 @@
+Python Wrapper and Packaging
+============================
+
+This directory contains the basic setup script and directory structure for the gtsam python module.
+During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run.
+
+* The handwritten module source files are compiled and linked with Boost Python, generating a shared 
+  library which can then be imported by python
+* A setup.py script is configured from setup.py.in
+* The gtsam packages 'gtsam', 'gtsam_utils', 'gtsam_examples', and 'gtsam_tests' are installed into
+  the site-packages folder within the (possibly non-default) installation prefix folder. If 
+  installing to a non-standard prefix, make sure that _prefix_/lib/pythonX.Y/site-packages is 
+  present in your $PYTHONPATH
+
+The target version of Python to create the module can be set by defining GTSAM_PYTHON_VERSION to 'X.Y' (Example: 2.7 or 3.4), or 'Default' if you want to use the default python installed in your system. Note that if you specify a target version of python, you should also have the correspondening Boost 
+Python version installed (Example: libboost_python-py27.so or libboost_python-py34.so on Linux). 
+If you're using the default version, your default Boost Python library (Example: libboost_python.so on Linux) should correspond to the default python version in your system.
+

python/gtsam/__init__.py

@@ -0,0 +1 @@
+from ._libgtsam_python import *

python/gtsam_examples/OdometryExample.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+from __future__ import print_function
+import gtsam
+import numpy as np
+
+# 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/gtsam_examples/SFMdata.py

@@ -0,0 +1,32 @@
+ 
+  # A structure-from-motion example with landmarks
+  #  - The landmarks form a 10 meter cube
+  #  - The robot rotates around the landmarks, always facing towards the cube
+
+import gtsam
+import numpy as np
+
+def createPoints():
+	# Create the set of ground-truth landmarks
+	points = [gtsam.Point3(10.0,10.0,10.0),
+			  gtsam.Point3(-10.0,10.0,10.0),
+	          gtsam.Point3(-10.0,-10.0,10.0),
+	          gtsam.Point3(10.0,-10.0,10.0),
+	          gtsam.Point3(10.0,10.0,-10.0),
+	          gtsam.Point3(-10.0,10.0,-10.0),
+	          gtsam.Point3(-10.0,-10.0,-10.0),
+	          gtsam.Point3(10.0,-10.0,-10.0)]
+	return points
+
+def createPoses():
+	# Create the set of ground-truth poses
+	radius = 30.0
+	angles = np.linspace(0,2*np.pi,8,endpoint=False)
+	up = gtsam.Point3(0,0,1)
+	target = gtsam.Point3(0,0,0)
+	poses = []
+	for theta in angles:
+		position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0)
+		camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up)
+		poses.append(camera.pose())
+	return poses

python/gtsam_examples/VisualISAM2Example.py

@@ -0,0 +1,132 @@
+from __future__ import print_function
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+import time  # for sleep()
+
+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
+
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+    plt.cla()
+
+    # Plot points
+    # Can't use data because current frame might not see all points
+    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow
+    # gtsam.plot3DPoints(result, [], marginals)
+    gtsam_utils.plot3DPoints(fignum, result, 'rx')
+
+    # Plot cameras
+    i = 0
+    while result.exists(X(i)):
+        pose_i = result.atPose3(X(i))
+        gtsam_utils.plotPose3(fignum, pose_i, 10)
+        i += 1
+
+    # draw
+    ax.set_xlim3d(-40, 40)
+    ax.set_ylim3d(-40, 40)
+    ax.set_zlim3d(-40, 40)
+    plt.pause(1)
+
+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)
+
+    # Define the camera observation noise model
+    measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+
+    # Create the set of ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of ground-truth poses
+    poses = SFMdata.createPoses()
+
+    # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
+    # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
+    # structure is available that allows the user to set various properties, such as the relinearization threshold
+    # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result
+    # will approach the batch result.
+    parameters = gtsam.ISAM2Params()
+    parameters.relinearize_threshold = 0.01
+    parameters.relinearize_skip = 1
+    isam = gtsam.ISAM2(parameters)
+
+    # Create a Factor Graph and Values to hold the new data
+    graph = gtsam.NonlinearFactorGraph()
+    initialEstimate = gtsam.Values()
+
+    #  Loop over the different poses, adding the observations to iSAM incrementally
+    for i, pose in enumerate(poses):
+
+        # Add factors for each landmark observation
+        for j, point in enumerate(points):
+            camera = gtsam.PinholeCameraCal3_S2(pose, K)
+            measurement = camera.project(point)
+            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(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
+        # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
+        # adding it to iSAM.
+        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(X(0), poses[0], poseNoise))
+
+            # Add a prior on landmark l0
+            pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+            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(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15))
+        else:
+            # Update iSAM with the new factors
+            isam.update(graph, initialEstimate)
+            # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
+            # 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()
+            print("****************************************************")
+            print("Frame", i, ":")
+            for j in range(i + 1):
+                print(X(j), ":", currentEstimate.atPose3(X(j)))
+
+            for j in range(len(points)):
+                print(L(j), ":", currentEstimate.atPoint3(L(j)))
+
+            visual_ISAM2_plot(poses, points, currentEstimate)
+
+            # Clear the factor graph and values for the next iteration
+            graph.resize(0)
+            initialEstimate.clear()
+            
+    plt.ioff()
+    plt.show()
+
+if __name__ == '__main__':
+    visual_ISAM2_example()

python/gtsam_examples/__init__.py

@@ -0,0 +1,2 @@
+from . import SFMdata
+from . import VisualISAM2Example

python/gtsam_tests/__init__.py

@@ -0,0 +1 @@
+

python/gtsam_tests/testPoint2.py

@@ -0,0 +1,13 @@
+import unittest
+from gtsam import *
+
+#https://docs.python.org/2/library/unittest.html
+class TestPoint2(unittest.TestCase):
+    def setUp(self):
+        self.point = Point2()
+
+    def test_constructor(self):
+        pass
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_utils/__init__.py

@@ -0,0 +1 @@
+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.point3_at(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/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/CMakeLists.txt

@@ -0,0 +1,30 @@
+# get subdirectories list
+subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR})
+
+# get the sources needed to compile gtsam python module
+set(gtsam_python_srcs "")
+foreach(subdir ${SUBDIRS})
+	file(GLOB ${subdir}_src "${subdir}/*.cpp")
+	list(APPEND gtsam_python_srcs ${${subdir}_src})
+endforeach()
+
+# Create the library
+add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs})
+set_target_properties(gtsam_python PROPERTIES
+	OUTPUT_NAME         gtsam_python
+	SKIP_BUILD_RPATH    TRUE
+	CLEAN_DIRECT_OUTPUT 1
+)
+target_link_libraries(gtsam_python
+                      ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY}
+                      ${PYTHON_LIBRARY} gtsam)
+
+# Cause the library to be output in the correct directory.
+# TODO: Change below to work on different systems (currently works only with Linux)
+add_custom_command(
+	OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so
+	DEPENDS gtsam_python
+	COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:gtsam_python> ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so
+	COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so"
+)
+add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so)

python/handwritten/base/FastVector.cpp

@@ -0,0 +1,37 @@
+ /* ----------------------------------------------------------------------------
+
+ * 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 FastVector instances to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/base/FastVector.h"
+#include "gtsam/base/types.h" // for Key definition
+
+using namespace boost::python;
+using namespace gtsam;
+
+void exportFastVectors(){
+
+  typedef FastVector<Key> KeyVector;
+
+  class_<KeyVector>("KeyVector")
+  	.def(vector_indexing_suite<KeyVector>())
+  ;
+
+}

python/handwritten/exportgtsam.cpp

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <numpy_eigen/NumpyEigenConverter.hpp>
+
+// Base
+void exportFastVectors();
+
+// Geometry
+void exportPoint2();
+void exportPoint3();
+void exportRot2();
+void exportRot3();
+void exportPose2();
+void exportPose3();
+void exportPinholeBaseK();
+void exportPinholeCamera();
+void exportCal3_S2();
+
+// Inference
+void exportSymbol();
+
+// Linear
+void exportNoiseModels();
+
+// Nonlinear
+void exportValues();
+void exportNonlinearFactor();
+void exportNonlinearFactorGraph();
+void exportLevenbergMarquardtOptimizer();
+void exportISAM2();
+
+// Slam
+void exportPriorFactors();
+void exportBetweenFactors();
+void exportGenericProjectionFactor();
+
+// Utils (or Python wrapper specific functions)
+void registerNumpyEigenConversions();
+
+//-----------------------------------//
+
+BOOST_PYTHON_MODULE(_libgtsam_python){
+
+  // NOTE: We need to call import_array1() instead of import_array() to support both python 2 
+  //       and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function 
+  //       returning void, and import_array() is a macro that when expanded for python 3, adds 
+  //       a 'return __null' statement to that function. For more info check files: 
+  //       boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file).
+  // Should be the first thing to be done
+  import_array1();
+  
+  registerNumpyEigenConversions();
+
+  exportFastVectors();
+
+  exportPoint2();
+  exportPoint3();
+  exportRot2();
+  exportRot3();
+  exportPose2();
+  exportPose3();
+  exportPinholeBaseK();
+  exportPinholeCamera();
+  exportCal3_S2();
+
+  exportSymbol();
+
+  exportNoiseModels();
+
+  exportValues();
+  exportNonlinearFactor();
+  exportNonlinearFactorGraph();
+  exportLevenbergMarquardtOptimizer();
+  exportISAM2();
+
+  exportPriorFactors();
+  exportBetweenFactors();
+  exportGenericProjectionFactor();
+}

python/handwritten/geometry/Cal3_S2.cpp

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Cal3_S2 class to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/Cal3_S2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3)
+
+// Function pointers to desambiguate Cal3_S2::calibrate calls
+Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate;
+Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate;
+
+void exportCal3_S2(){
+
+class_<Cal3_S2>("Cal3_S2", init<>())
+  .def(init<double,double,double,double,double>())
+  .def(init<const Vector &>())
+  .def(init<double,int,int>())
+  .def(init<std::string>())
+  .def("print", &Cal3_S2::print, print_overloads(args("s")))
+  .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol")))
+  .def("fx",&Cal3_S2::fx)
+  .def("fy",&Cal3_S2::fy)
+  .def("skew",&Cal3_S2::skew)
+  .def("px",&Cal3_S2::px)
+  .def("py",&Cal3_S2::py)
+  .def("principal_point",&Cal3_S2::principalPoint)
+  .def("vector",&Cal3_S2::vector)
+  .def("k",&Cal3_S2::K)
+  .def("matrix",&Cal3_S2::matrix)
+  .def("matrix_inverse",&Cal3_S2::matrix_inverse)
+  .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads())
+  .def("calibrate",calibrate1, calibrate_overloads())
+  .def("calibrate",calibrate2)
+  .def("between",&Cal3_S2::between, between_overloads())
+;
+
+}

python/handwritten/geometry/PinholeBaseK.cpp

@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 PinholeCamera classes to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/PinholeCamera.h"
+#include "gtsam/geometry/Cal3_S2.h"
+
+
+using namespace boost::python;
+using namespace gtsam;
+
+typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
+
+// Wrapper on PinholeBaseK<Cal3_S2> because it has pure virtual method calibration()
+struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCal3_S2>
+{
+  const Cal3_S2 & calibration () const {
+    return this->get_override("calibration")();
+  }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3)
+
+// Function pointers to desambiguate project() calls
+Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project;
+Point2  (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
+Point2  (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
+
+// function pointers to desambiguate range() calls
+double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range;
+double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose,   OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose)  const = &PinholeBaseKCal3_S2::range;
+double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range;
+
+void exportPinholeBaseK(){
+
+  class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
+    .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>())
+    .def("project", project1)
+    .def("project", project2, project_overloads())
+    .def("project", project3, project_overloads())
+    .def("backproject", &PinholeBaseKCal3_S2::backproject)
+    .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
+    .def("range", range1, range_overloads())
+    .def("range", range2, range_overloads())
+    .def("range", range3, range_overloads())
+  ;
+
+}

python/handwritten/geometry/PinholeCamera.cpp

@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 PinholeCamera classes to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/PinholeCamera.h"
+#include "gtsam/geometry/Cal3_S2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
+typedef PinholeCamera<Cal3_S2> PinholeCameraCal3_S2;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, PinholeCameraCal3_S2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PinholeCameraCal3_S2::equals, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(Lookat_overloads, PinholeCameraCal3_S2::Lookat, 3, 4)
+
+void exportPinholeCamera(){
+
+class_<PinholeCameraCal3_S2, bases<PinholeBaseKCal3_S2> >("PinholeCameraCal3_S2", init<>())
+  .def(init<const Pose3 &>())
+  .def(init<const Pose3 &, const Cal3_S2 &>())
+  .def(init<const Vector &>())
+  .def(init<const Vector &, const Vector &>())
+  .def("print", &PinholeCameraCal3_S2::print, print_overloads(args("s")))
+  .def("equals", &PinholeCameraCal3_S2::equals, equals_overloads(args("q","tol")))
+  .def("pose", &PinholeCameraCal3_S2::pose, return_value_policy<copy_const_reference>())
+  // We don't need to define calibration() here because it's already defined as virtual in the base class PinholeBaseKCal3_S2
+  // .def("calibration", &PinholeCameraCal3_S2::calibration, return_value_policy<copy_const_reference>())
+  .def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads())
+  .staticmethod("Lookat")
+;
+
+}

python/handwritten/geometry/Point2.cpp

@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/Point2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3)
+
+void exportPoint2(){
+
+  class_<Point2>("Point2", init<>())
+    .def(init<double, double>())
+    .def(init<const Vector2 &>())
+    .def("identity", &Point2::identity)
+    .def("dist", &Point2::dist)
+    .def("distance", &Point2::distance)
+    .def("equals", &Point2::equals, equals_overloads(args("q","tol")))
+    .def("norm", &Point2::norm)
+    .def("print", &Point2::print, print_overloads(args("s")))
+    .def("unit", &Point2::unit)
+    .def("vector", &Point2::vector)
+    .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,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.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<const Vector3 &>())
+  .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

@@ -0,0 +1,93 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/Pose2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3)
+
+// Manually wrap 
+
+void exportPose2(){
+
+  // double (Pose2::*range1)(const Pose2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
+  //   = &Pose2::range;
+  // double (Pose2::*range2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
+  //   = &Pose2::range;
+
+  // Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
+  //   = &Pose2::bearing;
+  // Rot2 (Pose2::*bearing2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
+  //   = &Pose2::bearing;
+
+  class_<Pose2>("Pose2", init<>())
+    .def(init<Pose2>())
+    .def(init<double, double, double>())
+    .def(init<double, Point2>())
+    .def("print", &Pose2::print, print_overloads(args("s")))
+
+    .def("equals", &Pose2::equals, equals_overloads(args("pose","tol")))
+    // .def("inverse", &Pose2::inverse)
+    // .def("compose", &Pose2::compose, compose_overloads(args("p2", "H1", "H2")))
+    // .def("between", &Pose2::between, between_overloads(args("p2", "H1", "H2")))
+    // .def("dim", &Pose2::dim)
+    // .def("retract", &Pose2::retract)
+
+    .def("transform_to", &Pose2::transform_to, 
+      transform_to_overloads(args("point", "H1", "H2")))
+    .def("transform_from", &Pose2::transform_from, 
+      transform_to_overloads(args("point", "H1", "H2")))
+
+    .def("x", &Pose2::x)
+    .def("y", &Pose2::y)
+    .def("theta", &Pose2::theta)
+    // See documentation on call policy for more information
+    // https://wiki.python.org/moin/boost.python/CallPolicy
+    .def("t", &Pose2::t, return_value_policy<copy_const_reference>())
+    .def("r", &Pose2::r, return_value_policy<copy_const_reference>())
+    .def("translation", &Pose2::translation, return_value_policy<copy_const_reference>())
+    .def("rotation", &Pose2::rotation, return_value_policy<copy_const_reference>())
+
+    // .def("bearing", bearing1, bearing_overloads())
+    // .def("bearing", bearing2, bearing_overloads())
+
+    // Function overload example
+    // .def("range", range1, range_overloads())
+    // .def("range", range2, range_overloads())
+
+
+    .def("Expmap", &Pose2::Expmap)
+    .staticmethod("Expmap")
+
+    .def(self * self) // __mult__
+  ;
+
+}

python/handwritten/geometry/Pose3.cpp

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.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(compose_overloads, Pose3::compose, 2, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3)
+
+void exportPose3(){
+
+  // function pointers to desambiguate transform_to() calls
+  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;
+  // function pointers to desambiguate compose() calls
+  Pose3 (Pose3::*compose1)(const Pose3 &g) const  = &Pose3::compose;
+  Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const  = &Pose3::compose;
+  // function pointers to desambiguate between() calls
+  Pose3 (Pose3::*between1)(const Pose3 &g) const  = &Pose3::between;
+  Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const  = &Pose3::between;
+  // function pointers to desambiguate range() calls
+  double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const  = &Pose3::range;
+  double (Pose3::*range2)(const Pose3 &,  OptionalJacobian<1,6>, OptionalJacobian<1,6>) const  = &Pose3::range;
+
+  class_<Pose3>("Pose3")
+    .def(init<>())
+    .def(init<const Pose3 &>())
+    .def(init<const Rot3 &,const Point3 &>())
+    .def(init<const Rot3 &,const Vector3 &>())
+    .def(init<const Pose2 &>())
+    .def(init<const 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__
+    .def("compose", compose1)
+    .def("compose", compose2, compose_overloads())
+    .def("between", between1)
+    .def("between", between2, between_overloads())
+    .def("range", range1, range_overloads())
+    .def("range", range2, range_overloads())
+    .def("bearing", &Pose3::bearing, bearing_overloads())
+  ;
+}

python/handwritten/geometry/Rot2.cpp

@@ -0,0 +1,65 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/Rot2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3)
+
+void exportRot2(){
+
+  class_<Rot2>("Rot2", init<>())
+    .def(init<double>())
+    .def("Expmap", &Rot2::Expmap)
+    .staticmethod("Expmap")
+    .def("Logmap", &Rot2::Logmap)
+    .staticmethod("Logmap")
+    .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,111 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/geometry/Rot3.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+static Rot3 Quaternion_0(const Vector4& q)
+{
+    return Rot3::Quaternion(q[0],q[1],q[2],q[3]);
+}
+
+static Rot3 Quaternion_1(double w, double x, double y, double z)
+{
+    return Rot3::Quaternion(w,x,y,z);
+}
+
+// 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;
+Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion;
+
+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<Point3,Point3,Point3>())
+    .def(init<double,double,double,double,double,double,double,double,double>())
+    .def(init<const Matrix3 &>())
+    .def(init<const Matrix &>())
+    .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion")
+    .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) )
+    .staticmethod("Quaternion")
+    .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, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" )
+    .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" )
+    .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")))
+#ifndef GTSAM_USE_QUATERNIONS
+    .def("localCayley", &Rot3::localCayley)
+    .def("retractCayley", &Rot3::retractCayley)
+#endif
+    .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("rpy", &Rot3::rpy)
+    .def("slerp", &Rot3::slerp)
+    .def("transpose", &Rot3::transpose)
+    .def("xyz", &Rot3::xyz)
+    .def("quaternion", quaternion_0)
+    .def(self * self)
+    .def(self * other<Point3>())
+    .def(self * other<Unit3>())
+    .def(self_ns::str(self)) // __str__
+    .def(repr(self))         // __repr__
+  ;
+
+}

python/handwritten/inference/Symbol.cpp

@@ -0,0 +1,83 @@
+ /* ----------------------------------------------------------------------------
+
+ * 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 Symbol class to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+#include <boost/make_shared.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include <sstream> // for stringstream
+
+#include "gtsam/inference/Symbol.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Symbol::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Symbol::equals, 1, 2)
+
+// Helper function to allow building a symbol from a python string and a index.
+static boost::shared_ptr<Symbol> makeSymbol(const std::string &str, size_t j)
+{
+  if(str.size() > 1)
+  	throw std::runtime_error("string argument must have one character only");
+
+  return boost::make_shared<Symbol>(str.at(0),j);
+}
+
+// Helper function to print the symbol as "char-and-index" in python
+std::string selfToString(const Symbol & self)
+{
+	return (std::string)self;
+}
+
+// Helper function to convert a Symbol to int using int() cast in python
+size_t selfToKey(const Symbol & self)
+{
+	return self.key();
+}
+
+// Helper function to recover symbol's unsigned char as string
+std::string chrFromSelf(const Symbol & self)
+{
+	std::stringstream ss;
+	ss << self.chr();
+	return ss.str();
+}
+
+void exportSymbol(){
+
+class_<Symbol, boost::shared_ptr<Symbol> >("Symbol")
+  .def(init<>())
+  .def(init<const Symbol &>())
+  .def("__init__", make_constructor(makeSymbol))
+  .def(init<Key>())
+  .def("print", &Symbol::print, print_overloads(args("s")))
+  .def("equals", &Symbol::equals, equals_overloads(args("q","tol")))
+  .def("key", &Symbol::key)
+  .def("index", &Symbol::index)
+  .def(self < self)
+  .def(self == self)
+  .def(self == other<Key>())
+  .def(self != self)
+  .def(self != other<Key>())
+  .def("__repr__", &selfToString)
+  .def("__int__", &selfToKey)
+  .def("chr", &chrFromSelf)
+;
+
+}

python/handwritten/linear/NoiseModel.cpp

@@ -0,0 +1,137 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#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(){
+
+  // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html
+  std::string noiseModel_name = extract<std::string>(scope().attr("__name__") + ".noiseModel");
+  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,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/nonlinear/ISAM2.h"
+#include "gtsam/geometry/Pose3.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")
+  .add_property("relinearize_skip", &ISAM2Params::getRelinearizeSkip, &ISAM2Params::setRelinearizeSkip)
+  .add_property("enable_relinearization", &ISAM2Params::isEnableRelinearization, &ISAM2Params::setEnableRelinearization)
+  .add_property("evaluate_non_linear_error", &ISAM2Params::isEvaluateNonlinearError, &ISAM2Params::setEvaluateNonlinearError)
+  .add_property("factorization", &ISAM2Params::getFactorization, &ISAM2Params::setFactorization)
+  .add_property("cache_linearized_factors", &ISAM2Params::isCacheLinearizedFactors, &ISAM2Params::setCacheLinearizedFactors)
+  .add_property("enable_detailed_results", &ISAM2Params::isEnableDetailedResults, &ISAM2Params::setEnableDetailedResults)
+  .add_property("enable_partial_linearization_check", &ISAM2Params::isEnablePartialRelinearizationCheck, &ISAM2Params::setEnablePartialRelinearizationCheck)
+  // TODO(Ellon): Check if it works with FastMap; Implement properly if it doesn't.
+  .add_property("relinearization_threshold", &ISAM2Params::getRelinearizeThreshold, &ISAM2Params::setRelinearizeThreshold)
+  // TODO(Ellon): Wrap the following setters/getters:
+  //     void 	setOptimizationParams (OptimizationParams optimizationParams)
+  //     OptimizationParams 	getOptimizationParams () const
+  //     void 	setKeyFormatter (KeyFormatter keyFormatter)
+  //     KeyFormatter 	getKeyFormatter () const
+  //     GaussianFactorGraph::Eliminate 	getEliminationFunction () const 
+;
+
+// 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")
+  .def(init<const ISAM2Params &>())
+  // TODO(Ellon): wrap all optional values of update
+  .def("update",&ISAM2::update, update_overloads())
+  .def("calculate_estimate", calculateEstimate_0)
+  .def("calculate_pose3_estimate", &ISAM2::calculateEstimate<Pose3>, (arg("self"), arg("key")) )
+  .def("value_exists", &ISAM2::valueExists)
+;
+
+}

python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -0,0 +1,27 @@
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+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/NonlinearFactor.cpp

@@ -0,0 +1,49 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.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

@@ -0,0 +1,56 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/nonlinear/NonlinearFactorGraph.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
+
+using namespace boost::python;
+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;
+
+  void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back;
+  void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add;
+
+  class_<NonlinearFactorGraph>("NonlinearFactorGraph", init<>())
+    .def("size",&NonlinearFactorGraph::size)
+    .def("push_back", push_back1)
+    .def("add", add1)
+    .def("resize", &NonlinearFactorGraph::resize)
+    .def("empty", &NonlinearFactorGraph::empty)
+    .def("print", &NonlinearFactorGraph::print, print_overloads(args("s")))
+  ;
+
+  def("getNonlinearFactor", getNonlinearFactor);
+
+}

python/handwritten/nonlinear/Values.cpp

@@ -0,0 +1,80 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#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"
+#include "gtsam/navigation/ImuBias.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1);
+
+void exportValues(){
+
+  typedef imuBias::ConstantBias Bias;
+
+  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;
+  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>())
+  .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, print_overloads(args("s")))
+  .def("size", &Values::size)
+  .def("swap", &Values::swap)
+  .def("insert", insert_point2)
+  .def("insert", insert_rot2)
+  .def("insert", insert_pose2)
+  .def("insert", insert_point3)
+  .def("insert", insert_rot3)
+  .def("insert", insert_pose3)
+  .def("insert", insert_bias)
+  .def("insert", insert_vector3)
+  .def("atPoint2", &Values::at<Point2>, return_value_policy<copy_const_reference>())
+  .def("atRot2", &Values::at<Rot2>, return_value_policy<copy_const_reference>())
+  .def("atPose2", &Values::at<Pose2>, return_value_policy<copy_const_reference>())
+  .def("atPoint3", &Values::at<Point3>, return_value_policy<copy_const_reference>())
+  .def("atRot3", &Values::at<Rot3>, 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/BearingFactor.cpp

@@ -0,0 +1,16 @@
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include <gtsam/sam/BearingFactor.h>
+
+using namespace boost::python;
+using namespace gtsam;
+
+using namespace std;
+
+template <class VALUE>
+void exportBearingFactor(const std::string& name) {
+  class_<VALUE>(name, init<>());
+}

python/handwritten/slam/BetweenFactor.cpp

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#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 T>
+// void exportBetweenFactor(const std::string& name){
+//   class_<T>(name, init<>())
+//   .def(init<Key, Key, T, SharedNoiseModel>())
+//   ;
+// }
+
+#define BETWEENFACTOR(T) \
+  class_< BetweenFactor<T>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<T> > >("BetweenFactor"#T) \
+  .def(init<Key,Key,T,noiseModel::Base::shared_ptr>()) \
+  .def("measured", &BetweenFactor<T>::measured, return_internal_reference<>()) \
+;
+
+void exportBetweenFactors()
+{
+  BETWEENFACTOR(Point2)
+  BETWEENFACTOR(Rot2)
+  BETWEENFACTOR(Pose2)
+  BETWEENFACTOR(Point3)
+  BETWEENFACTOR(Rot3)
+  BETWEENFACTOR(Pose3)
+}

python/handwritten/slam/GenericProjectionFactor.cpp

@@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 GenericProjectionFactor for several values to python
+ * @author Ellon Paiva Mendes (LAAS-CNRS)
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/slam/ProjectionFactor.h"
+#include "gtsam/geometry/Pose3.h"
+#include "gtsam/geometry/Point3.h"
+#include "gtsam/geometry/Cal3_S2.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+using namespace std;
+
+typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4)
+
+void exportGenericProjectionFactor()
+{
+
+  class_<GenericProjectionFactorCal3_S2, bases<NonlinearFactor> >("GenericProjectionFactorCal3_S2", init<>())
+    .def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, optional<Pose3> >())
+    .def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, bool, bool, optional<Pose3> >())
+    .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s")))
+    .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol")))
+    .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads())
+    .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy<copy_const_reference>())
+    // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &'
+    // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy<copy_const_reference>())
+    .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality)
+    .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality)
+  ;
+
+}

python/handwritten/slam/PriorFactor.cpp

@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#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 >
+// void exportPriorFactor(const std::string& name){
+//   class_< FACTOR >(name.c_str(), init<>())
+//   .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)
+  PRIORFACTOR(Vector3)
+}

python/handwritten/utils/NumpyEigen.cpp

@@ -0,0 +1,54 @@
+ /* ----------------------------------------------------------------------------
+
+ * 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>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/Vector.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+void registerNumpyEigenConversions()
+{
+  // NOTE: import array should be called only in the cpp defining the module
+  // 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();
+
+}

python/include/numpy_eigen/NumpyEigenConverter.hpp

@@ -0,0 +1,334 @@
+/**
+ * @file   NumpyEigenConverter.hpp
+ * @author Paul Furgale <paul.furgale@utoronto.ca>
+ * @date   Fri Feb  4 11:17:25 2011
+ * 
+ * @brief  Classes to support conversion from numpy arrays in Python
+ *         to Eigen3 matrices in c++
+ * 
+ * 
+ */
+
+#ifndef NUMPY_EIGEN_CONVERTER_HPP
+#define NUMPY_EIGEN_CONVERTER_HPP
+
+#include <numpy_eigen/boost_python_headers.hpp>
+//#include <iostream>
+
+#include "numpy/numpyconfig.h"
+#ifdef NPY_1_7_API_VERSION
+#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
+#define NPE_PY_ARRAY_OBJECT PyArrayObject
+#else
+//TODO Remove this as soon as support for Numpy version before 1.7 is dropped
+#define NPE_PY_ARRAY_OBJECT PyObject
+#endif
+
+#define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS
+#include <numpy/arrayobject.h> 
+
+#include "type_traits.hpp"
+#include <boost/lexical_cast.hpp>
+#include "copy_routines.hpp"
+
+
+
+/**
+ * @class NumpyEigenConverter
+ * @tparam the Eigen3 matrix type this class is specialized for
+ * 
+ * adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/
+ * General help available http://docs.scipy.org/doc/numpy/reference/c-api.array.html
+ *
+ * To use: 
+ * 
+ * #include <NumpyEigenConverter.hpp>
+ * 
+ * 
+ * BOOST_PYTHON_MODULE(libmy_module_python)
+ * {
+ *   // The converters will cause a segfault unless import_array() is called before the first one
+ *   import_array();
+ *   NumpyEigenConverter<Eigen::Matrix< double, 1, 1 > >::register_converter();
+ *   NumpyEigenConverter<Eigen::Matrix< double, 2, 1 > >::register_converter();
+ * }
+ * 
+ */
+template<typename EIGEN_MATRIX_T>
+struct NumpyEigenConverter
+{
+
+  typedef EIGEN_MATRIX_T matrix_t;
+  typedef typename matrix_t::Scalar scalar_t;
+
+  enum {
+    RowsAtCompileTime = matrix_t::RowsAtCompileTime,
+    ColsAtCompileTime = matrix_t::ColsAtCompileTime,
+    MaxRowsAtCompileTime = matrix_t::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = matrix_t::MaxColsAtCompileTime,
+    NpyType = TypeToNumPy<scalar_t>::NpyType,
+    //Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
+    //CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    Options = matrix_t::Options
+    //InnerStrideAtCompileTime = 1,
+    //OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime
+  };
+
+  static std::string castSizeOption(int option)
+  {
+    if(option == Eigen::Dynamic)
+      return "Dynamic";
+    else
+      return boost::lexical_cast<std::string>(option);
+  }
+
+  static std::string toString()
+  {
+    return std::string() + "Eigen::Matrix<" + TypeToNumPy<scalar_t>::typeString() + ", " +
+      castSizeOption(RowsAtCompileTime) + ", " +
+      castSizeOption(ColsAtCompileTime) + ", " +
+      boost::lexical_cast<std::string>((int)Options) + ", " +
+      castSizeOption(MaxRowsAtCompileTime) + ", " +
+      castSizeOption(MaxColsAtCompileTime) + ">";
+  }
+
+  // The "Convert from C to Python" API
+  static PyObject * convert(const matrix_t & M)
+  {
+    PyObject * P = NULL;
+    if(RowsAtCompileTime == 1 || ColsAtCompileTime == 1)
+      {
+	// Create a 1D array
+	npy_intp dimensions[1];
+	dimensions[0] = M.size();
+	P = PyArray_SimpleNew(1, dimensions, TypeToNumPy<scalar_t>::NpyType);
+	numpyTypeDemuxer< CopyEigenToNumpyVector<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(P));
+      }
+    else
+      {
+	// create a 2D array.
+	npy_intp dimensions[2];
+	dimensions[0] = M.rows();
+	dimensions[1] = M.cols();
+	P = PyArray_SimpleNew(2, dimensions, TypeToNumPy<scalar_t>::NpyType);
+	numpyTypeDemuxer< CopyEigenToNumpyMatrix<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(P));
+      }
+    
+    // incrementing the reference seems to cause a memory leak.
+    // boost::python::incref(P);
+    // This agrees with the sample code found here:
+    // http://mail.python.org/pipermail/cplusplus-sig/2008-October/013825.html
+    return P;
+  }
+
+  static bool isDimensionValid(int requestedSize, int sizeAtCompileTime, int maxSizeAtCompileTime)
+  {
+    bool valid = true;
+    if(sizeAtCompileTime == Eigen::Dynamic)
+      {
+	// Check for dynamic fixed size
+	// http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html#TutorialMatrixOptTemplParams
+	if(!(maxSizeAtCompileTime == Eigen::Dynamic || requestedSize <= maxSizeAtCompileTime))
+	  {
+	    valid = false;
+	  }
+      }
+    else if(sizeAtCompileTime != requestedSize)
+      {
+	valid = false;
+      }
+    return valid;
+  }
+      
+  static void checkMatrixSizes(NPE_PY_ARRAY_OBJECT * obj_ptr)
+  {
+    int rows = PyArray_DIM(obj_ptr, 0);
+    int cols = PyArray_DIM(obj_ptr, 1);
+
+    bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime);
+    bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime);
+    if(!rowsValid || !colsValid)
+    {
+	THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() 
+			 << ". Mismatched sizes.");
+      }
+  }
+
+  static void checkRowVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int cols)
+  {
+    if(!isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime))
+      {
+	THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() 
+			 << ". Mismatched sizes.");
+      }
+  }
+
+  static void checkColumnVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int rows)
+  {
+    // Check if the type can accomidate one column.
+    if(ColsAtCompileTime == Eigen::Dynamic || ColsAtCompileTime == 1)
+      {
+	if(!isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime))
+	  {
+	    THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() 
+			     << ". Mismatched sizes.");
+	  }
+      }
+    else
+      {
+	THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() 
+			 << ". Mismatched sizes.");
+      }
+
+  }
+
+  static void checkVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr)
+  {
+	int size = PyArray_DIM(obj_ptr, 0);
+
+    // If the number of rows is fixed at 1, assume that is the sense of the vector.
+    // Otherwise, assume it is a column.
+    if(RowsAtCompileTime == 1)
+      {
+	checkRowVectorSizes(obj_ptr, size);
+      }
+    else
+      {
+	checkColumnVectorSizes(obj_ptr, size);
+      }
+  }
+
+    
+  static void* convertible(PyObject *obj_ptr)
+  {
+    // Check for a null pointer.
+    if(!obj_ptr)
+      {
+        //THROW_TYPE_ERROR("PyObject pointer was null");
+        return 0;
+      }
+
+    // Make sure this is a numpy array.
+    if (!PyArray_Check(obj_ptr))
+      {
+        //THROW_TYPE_ERROR("Conversion is only defined for numpy array and matrix types");
+        return 0;
+      }
+
+    NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(obj_ptr);
+
+    // Check the type of the array.
+    int npyType = getNpyType(array_ptr);
+    
+    if(!TypeToNumPy<scalar_t>::canConvert(npyType))
+      {
+        //THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() 
+        //                 << ". Mismatched types.");
+        return 0;
+      }
+
+    
+
+    // Check the array dimensions.
+    int nd = PyArray_NDIM(array_ptr);
+    
+    if(nd != 1 && nd != 2)
+      {
+	THROW_TYPE_ERROR("Conversion is only valid for arrays with 1 or 2 dimensions. Argument has " << nd << " dimensions");
+      }
+
+    if(nd == 1)
+      {
+	checkVectorSizes(array_ptr);
+      }
+    else 
+      {
+	// Two-dimensional matrix type.
+	checkMatrixSizes(array_ptr);
+      }
+
+
+    return obj_ptr;
+  }
+  
+
+  static void construct(PyObject *obj_ptr, boost::python::converter::rvalue_from_python_stage1_data *data)
+  {
+    boost::python::converter::rvalue_from_python_storage<matrix_t> * matData = reinterpret_cast<boost::python::converter::rvalue_from_python_storage<matrix_t> * >(data);
+    void* storage = matData->storage.bytes;
+    
+    // Make sure storage is 16byte aligned. With help from code from Memory.h
+    void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
+    
+    matrix_t * Mp = new (aligned) matrix_t();
+    // Stash the memory chunk pointer for later use by boost.python
+    // This signals boost::python that the new value must be deleted eventually
+    data->convertible = storage;
+
+    
+    // std::cout << "Creating aligned pointer " << aligned << " from storage " << storage << std::endl;
+    // std::cout << "matrix size: " << sizeof(matrix_t) << std::endl;
+    // std::cout << "referent size: " << boost::python::detail::referent_size< matrix_t & >::value << std::endl;
+    // std::cout << "sizeof(storage): " << sizeof(matData->storage) << std::endl;
+    // std::cout << "sizeof(bytes): " << sizeof(matData->storage.bytes) << std::endl;
+    
+    
+
+    matrix_t & M = *Mp;
+
+    if (!PyArray_Check(obj_ptr))
+    {
+      THROW_TYPE_ERROR("construct is only defined for numpy array and matrix types");
+    }
+
+    NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(obj_ptr);
+
+    int nd = PyArray_NDIM(array_ptr);
+    if(nd == 1)
+      {
+	int size = PyArray_DIM(array_ptr, 0);
+	// This is a vector type
+	if(RowsAtCompileTime == 1)
+	  {
+	    // Row Vector
+	    M.resize(1,size);
+	  }
+	else
+	  {
+	    // Column Vector
+	    M.resize(size,1);
+	  }
+	numpyTypeDemuxer< CopyNumpyToEigenVector<matrix_t> >(&M, array_ptr);
+      }
+    else
+      {
+	int rows = PyArray_DIM(array_ptr, 0);
+	int cols = PyArray_DIM(array_ptr, 1);
+	
+	M.resize(rows,cols);
+	numpyTypeDemuxer< CopyNumpyToEigenMatrix<matrix_t> >(&M, array_ptr);
+      }
+
+    
+
+
+  }
+
+
+  // The registration function.
+  static void register_converter()
+  {
+    boost::python::to_python_converter<matrix_t,NumpyEigenConverter>();
+    boost::python::converter::registry::push_back(
+						  &NumpyEigenConverter::convertible,
+						  &NumpyEigenConverter::construct,
+						  boost::python::type_id<matrix_t>());
+
+  }
+  
+};
+
+
+
+
+#endif /* NUMPY_EIGEN_CONVERTER_HPP */

python/include/numpy_eigen/README.md

@@ -0,0 +1,6 @@
+numpy_eigen
+===========
+
+A boost python converter that handles the copy of matrices from the Eigen linear algebra library in C++ to numpy in Python.
+
+This is a minimal version based on the original from Paul Furgale (https://github.com/ethz-asl/Schweizer-Messer/tree/master/numpy_eigen)

python/include/numpy_eigen/boost_python_headers.hpp

@@ -0,0 +1,250 @@
+/**
+ * @file   boost_python_headers.hpp
+ * @author Paul Furgale <paul.furgale@gmail.com>
+ * @date   Mon Dec 12 10:36:03 2011
+ * 
+ * @brief  A header that specializes boost-python to work with fixed-sized Eigen types.
+ * 
+ * The original version of this library did not include these specializations and this caused
+ * assert failures when running on Ubuntu 10.04 32-bit. More information about fixed-size 
+ * vectorizable types in Eigen is available here: 
+ * http://eigen.tuxfamily.org/dox-devel/TopicFixedSizeVectorizable.html
+ *
+ * This code has been tested on Ubunutu 10.04 64 and 32 bit, OSX Snow Leopard and OSX Lion.
+ *
+ * This code is derived from boost/python/converter/arg_from_python.hpp
+ * Copyright David Abrahams 2002.
+ * Distributed under the Boost Software License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt)
+ * 
+ */
+#ifndef NUMPY_EIGEN_CONVERTERS_HPP
+#define NUMPY_EIGEN_CONVERTERS_HPP
+
+#include <Eigen/Core>
+#include <boost/python.hpp>
+#include <boost/python/detail/referent_storage.hpp>
+#include <boost/python/converter/arg_from_python.hpp>
+#include <boost/python/converter/rvalue_from_python_data.hpp>
+#include <boost/python/tuple.hpp>
+
+
+namespace boost { namespace python { namespace detail {
+      template<typename T>
+      struct referent_size;
+
+      // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
+       template<typename T, int A, int B, int C, int D, int E>
+       struct referent_size< Eigen::Matrix<T,A,B,C,D,E>& >
+       {
+       	// Add 16 bytes so we can get alignment
+       	BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
+       };
+
+      // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
+       template<typename T, int A, int B, int C, int D, int E>
+       struct referent_size< Eigen::Matrix<T,A,B,C,D,E> const & >
+       {
+       	// Add 16 bytes so we can get alignment
+       	BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
+       };
+
+      // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
+       template<typename T, int A, int B, int C, int D, int E>
+       struct referent_size< Eigen::Matrix<T,A,B,C,D,E> >
+       {
+       	// Add 16 bytes so we can get alignment
+       	BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
+       };
+
+
+    }}}
+
+namespace boost { namespace python { namespace converter { 
+
+
+      template<typename S, int A, int B, int C, int D, int E>
+      struct rvalue_from_python_data< Eigen::Matrix<S,A,B,C,D,E> const &> : rvalue_from_python_storage< Eigen::Matrix<S,A,B,C,D,E> const & >
+      {
+	typedef typename Eigen::Matrix<S,A,B,C,D,E> T;
+# if (!defined(__MWERKS__) || __MWERKS__ >= 0x3000)		 \
+  && (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 245)	 \
+  && (!defined(__DECCXX_VER) || __DECCXX_VER > 60590014)		\
+  && !defined(BOOST_PYTHON_SYNOPSIS) /* Synopsis' OpenCXX has trouble parsing this */
+	// This must always be a POD struct with m_data its first member.
+	BOOST_STATIC_ASSERT(BOOST_PYTHON_OFFSETOF(rvalue_from_python_storage<T>,stage1) == 0);
+# endif
+
+	// The usual constructor 
+	rvalue_from_python_data(rvalue_from_python_stage1_data const & _stage1)
+	{
+	  this->stage1 = _stage1;
+	}
+
+
+	// This constructor just sets m_convertible -- used by
+	// implicitly_convertible<> to perform the final step of the
+	// conversion, where the construct() function is already known.
+	rvalue_from_python_data(void* convertible)
+	{
+	  this->stage1.convertible = convertible;
+	}
+	
+	// Destroys any object constructed in the storage.
+	~rvalue_from_python_data()
+	{
+	  // Realign the pointer and destroy
+	  if (this->stage1.convertible == this->storage.bytes)
+	    {
+	      void * storage = reinterpret_cast<void *>(this->storage.bytes);
+	      T * aligned = reinterpret_cast<T *>(reinterpret_cast<void *>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16));
+	      
+	      //std::cout << "Destroying " << (void*)aligned << std::endl;
+	      aligned->T::~T();
+	    }
+	}
+      private:
+	typedef typename add_reference<typename add_cv<T>::type>::type ref_type;
+      };
+
+
+      
+
+
+      // Used when T is a plain value (non-pointer, non-reference) type or
+      // a (non-volatile) const reference to a plain value type.
+      template<typename S, int A, int B, int C, int D, int E>
+      struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> >
+      {
+	typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
+	typedef typename boost::add_reference<
+	  T
+	  // We can't add_const here, or it would be impossible to pass
+	  // auto_ptr<U> args from Python to C++
+	  >::type result_type;
+	
+	arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::converters))
+					       , m_source(obj)
+	{
+	  
+	}
+	bool convertible() const
+	{
+	  return m_data.stage1.convertible != 0;
+	}
+
+	
+# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196
+	typename arg_rvalue_from_python<T>::
+# endif 
+	result_type operator()()
+	{
+	  if (m_data.stage1.construct != 0)
+	    m_data.stage1.construct(m_source, &m_data.stage1);
+	  
+	  // Here is the magic...
+	  // Realign the pointer
+	  void * storage = reinterpret_cast<void *>(m_data.storage.bytes);
+	  void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
+	  
+	  return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
+	}
+
+      private:
+	rvalue_from_python_data<result_type> m_data;
+	PyObject* m_source;
+      };
+
+
+      // Used when T is a plain value (non-pointer, non-reference) type or
+      // a (non-volatile) const reference to a plain value type.
+      template<typename S, int A, int B, int C, int D, int E>
+      struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const & >
+      {
+	typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
+	typedef typename boost::add_reference<
+	  T
+	  // We can't add_const here, or it would be impossible to pass
+	  // auto_ptr<U> args from Python to C++
+	  >::type result_type;
+	
+	arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::converters))
+					       , m_source(obj)
+	{
+	  
+	}
+	bool convertible() const
+	{
+	  return m_data.stage1.convertible != 0;
+	}
+
+	
+# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196
+	typename arg_rvalue_from_python<T>::
+# endif 
+	result_type operator()()
+	{
+	  if (m_data.stage1.construct != 0)
+	    m_data.stage1.construct(m_source, &m_data.stage1);
+	  
+	  // Here is the magic...
+	  // Realign the pointer
+	  void * storage = reinterpret_cast<void *>(m_data.storage.bytes);
+	  void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
+	  
+	  return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
+	}
+
+      private:
+	rvalue_from_python_data<result_type> m_data;
+	PyObject* m_source;
+      };
+
+      // Used when T is a plain value (non-pointer, non-reference) type or
+      // a (non-volatile) const reference to a plain value type.
+      template<typename S, int A, int B, int C, int D, int E>
+      struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const >
+      {
+	typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
+	typedef typename boost::add_reference<
+	  T
+	  // We can't add_const here, or it would be impossible to pass
+	  // auto_ptr<U> args from Python to C++
+	  >::type result_type;
+	
+	arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::converters))
+					       , m_source(obj)
+	{
+	  
+	}
+	bool convertible() const
+	{
+	  return m_data.stage1.convertible != 0;
+	}
+
+	
+# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196
+	typename arg_rvalue_from_python<T>::
+# endif 
+	result_type operator()()
+	{
+	  if (m_data.stage1.construct != 0)
+	    m_data.stage1.construct(m_source, &m_data.stage1);
+	  
+	  // Here is the magic...
+	  // Realign the pointer
+	  void * storage = reinterpret_cast<void *>(m_data.storage.bytes);
+	  void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
+	  
+	  return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
+	}
+
+      private:
+	rvalue_from_python_data<result_type> m_data;
+	PyObject* m_source;
+      };
+
+
+    }}}
+
+
+#endif /* NUMPY_EIGEN_CONVERTERS_HPP */

python/include/numpy_eigen/copy_routines.hpp

@@ -0,0 +1,149 @@
+#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP
+#define NUMPY_EIGEN_COPY_ROUTINES_HPP
+
+
+template<typename EIGEN_T>
+struct CopyNumpyToEigenMatrix
+{
+  typedef EIGEN_T matrix_t;
+  typedef typename matrix_t::Scalar scalar_t;
+  
+  template<typename T>
+  void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_)
+  {
+    // Assumes M is already initialized.
+    for(int r = 0; r < M_->rows(); r++)
+      {
+    for(int c = 0; c < M_->cols(); c++)
+      {
+        T * p = static_cast<T*>(PyArray_GETPTR2(P_, r, c));
+        (*M_)(r,c) = static_cast<scalar_t>(*p);
+      }
+      }
+  }
+
+};
+
+template<typename EIGEN_T>
+struct CopyEigenToNumpyMatrix
+{
+  typedef EIGEN_T matrix_t;
+  typedef typename matrix_t::Scalar scalar_t;
+  
+  template<typename T>
+  void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_)
+  {
+    // Assumes M is already initialized.
+    for(int r = 0; r < M_->rows(); r++)
+      {
+    for(int c = 0; c < M_->cols(); c++)
+      {
+        T * p = static_cast<T*>(PyArray_GETPTR2(P_, r, c));
+        *p = static_cast<T>((*M_)(r,c));
+      }
+      }
+  }
+
+};
+
+template<typename EIGEN_T>
+struct CopyEigenToNumpyVector
+{
+  typedef EIGEN_T matrix_t;
+  typedef typename matrix_t::Scalar scalar_t;
+  
+  template<typename T>
+  void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_)
+  {
+    // Assumes M is already initialized.
+    for(int i = 0; i < M_->size(); i++)
+      {
+    T * p = static_cast<T*>(PyArray_GETPTR1(P_, i));
+    *p = static_cast<T>((*M_)(i));
+      }
+  }
+
+};
+
+
+template<typename EIGEN_T>
+struct CopyNumpyToEigenVector
+{
+  typedef EIGEN_T matrix_t;
+  typedef typename matrix_t::Scalar scalar_t;
+  
+  template<typename T>
+  void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_)
+  {
+    // Assumes M is already initialized.
+    for(int i = 0; i < M_->size(); i++)
+      {
+    T * p = static_cast<T*>(PyArray_GETPTR1(P_, i));
+    (*M_)(i) = static_cast<scalar_t>(*p);
+      }
+  }
+
+};
+
+
+
+
+// Crazy syntax in this function was found here:
+// http://stackoverflow.com/questions/1840253/c-template-member-function-of-template-class-called-from-template-function/1840318#1840318
+template< typename FUNCTOR_T>
+inline void numpyTypeDemuxer(typename FUNCTOR_T::matrix_t * M, NPE_PY_ARRAY_OBJECT * P)
+{
+  FUNCTOR_T f;
+
+  int npyType = getNpyType(P);
+  switch(npyType)
+    {
+    case NPY_BOOL:
+      f.template exec<bool>(M,P);
+      break;
+    case NPY_BYTE:
+      f.template exec<char>(M,P);
+      break;
+    case NPY_UBYTE:
+      f.template exec<unsigned char>(M,P);
+      break;
+    case NPY_SHORT:
+      f.template exec<short>(M,P);
+      break;
+    case NPY_USHORT:
+      f.template exec<unsigned short>(M,P);
+      break;
+    case NPY_INT:
+      f.template exec<int>(M,P);
+      break;
+    case NPY_UINT:
+      f.template exec<unsigned int>(M,P);
+      break;
+    case NPY_LONG:
+      f.template exec<long>(M,P);
+      break;
+    case NPY_ULONG:
+      f.template exec<unsigned long>(M,P);
+      break;
+    case NPY_LONGLONG:
+      f.template exec<long long>(M,P);
+      break;
+    case NPY_ULONGLONG:
+      f.template exec<unsigned long long>(M,P);
+      break;
+    case NPY_FLOAT:
+      f.template exec<float>(M,P);
+      break;
+    case NPY_DOUBLE:
+      f.template exec<double>(M,P);
+      break;
+    case NPY_LONGDOUBLE:
+      f.template exec<long double>(M,P);
+      break;
+     default:
+       THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType));
+   }
+}
+
+
+#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */