Merge branch 'develop' into feature/cython_wrapper

gtsam/base/ProductLieGroup.h

@@ -138,17 +138,27 @@ public:
     return ProductLieGroup(g,h);
   }
   static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
-    if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
-    G g = traits<G>::Expmap(v.template head<dimension1>());
-    H h = traits<H>::Expmap(v.template tail<dimension2>());
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
+    H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
+    if (Hv) {
+      Hv->setZero();
+      Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
     return ProductLieGroup(g,h);
   }
   static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
-    if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
-    typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
-    typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
+    typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
     TangentVector v;
     v << v1, v2;
+    if (Hp) {
+      Hp->setZero();
+      Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
     return v;
   }
   ProductLieGroup expmap(const TangentVector& v) const {

gtsam/base/VectorSpace.h

@@ -138,14 +138,14 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
   }
 
   static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+      ChartJacobian H2 = boost::none) {
     if (H1) *H1 = Eye(v1);
     if (H2) *H2 = Eye(v2);
     return v1 + v2;
   }
 
   static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+      ChartJacobian H2 = boost::none) {
     if (H1) *H1 = - Eye(v1);
     if (H2) *H2 =   Eye(v2);
     return v2 - v1;

matlab/README-gtsam-toolbox.txt

@@ -7,8 +7,14 @@ http://borg.cc.gatech.edu/projects/gtsam
 ================================================================================
 
 This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++
-library.
+library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake.
 
+The interface to the toolbox is generated automatically by the wrap
+tool which directly parses C++ header files. The tool generates matlab
+proxy objects together with all the native functions for wrapping and
+unwrapping scalar and non scalar types and objects. The interface
+generated by the wrap tool also redirects the standard output stream
+(cout) to matlab's console.
 
 ----------------------------------------
 Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04)

python/gtsam/__init__.py

@@ -1 +1 @@
-from gtsampy import *
+from _gtsampy import *

python/handwritten/CMakeLists.txt

@@ -12,7 +12,7 @@ endforeach()
 add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs})
 string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper)
 set_target_properties(gtsam_python PROPERTIES
-	OUTPUT_NAME                   gtsampy
+	OUTPUT_NAME                   _gtsampy
     PREFIX                        ""
     ${build_type_toupper}_POSTFIX ""
 	SKIP_BUILD_RPATH              TRUE
@@ -31,11 +31,11 @@ target_link_libraries(gtsam_python
 
 # Cause the library to be output in the correct directory.
 # TODO: Change below to work on different systems (currently works only with Linux)
-set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so)
+set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_gtsampy.so)
 add_custom_command(
 	OUTPUT ${output_path}
 	DEPENDS gtsam_python
 	COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:gtsam_python> ${output_path}
-	COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so"
+	COMMENT "Copying extension module to python/gtsam/_gtsampy.so"
 )
 add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path})

python/handwritten/exportgtsam.cpp

@@ -62,7 +62,7 @@ void registerNumpyEigenConversions();
 
 //-----------------------------------//
 
-BOOST_PYTHON_MODULE(gtsampy){
+BOOST_PYTHON_MODULE(_gtsampy){
 
   // 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

python/setup.py.in

@@ -11,5 +11,5 @@ setup(name='gtsam',
       package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' },
       packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'],
       #package_data={'gtsam' : ['_libgtsam_python.so']},  # location of .so file is relative to package_dir
-      data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])],  # location of .so file relative to setup.py
+      data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_gtsampy.so'])],  # location of .so file relative to setup.py
      )

tests/testLie.cpp

@@ -102,6 +102,33 @@ TEST( testProduct, inverse ) {
   EXPECT(assert_equal(numericH1, actH1, tol));
 }
 
+/* ************************************************************************* */
+Product expmap_proxy(const Vector5& vec) {
+  return Product::Expmap(vec);
+}
+TEST( testProduct, Expmap ) {
+  Vector5 vec;
+  vec << 1, 2, 0.1, 0.2, 0.3;
+
+  Matrix actH;
+  Product::Expmap(vec, actH);
+  Matrix numericH = numericalDerivative11(expmap_proxy, vec);
+  EXPECT(assert_equal(numericH, actH, tol));
+}
+
+/* ************************************************************************* */
+Vector5 logmap_proxy(const Product& p) {
+  return Product::Logmap(p);
+}
+TEST( testProduct, Logmap ) {
+  Product state(Point2(1, 2), Pose2(3, 4, 5));
+
+  Matrix actH;
+  Product::Logmap(state, actH);
+  Matrix numericH = numericalDerivative11(logmap_proxy, state);
+  EXPECT(assert_equal(numericH, actH, tol));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;

wrap/Argument.cpp

@@ -69,16 +69,22 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
 
   string cppType = type.qualifiedName("::");
   string matlabUniqueType = type.qualifiedName();
+  bool isNotScalar = !Argument::isScalar();
+
+  // We cannot handle scalar non const references
+  if (!isNotScalar && is_ref && !is_const) {
+    throw std::runtime_error("Cannot unwrap a scalar non-const reference");
+  }
 
   if (is_ptr && type.category != Qualified::EIGEN)
     // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
     file.oss << "boost::shared_ptr<" << cppType << "> " << name
         << " = unwrap_shared_ptr< ";
-  else if (is_ref && type.category != Qualified::EIGEN)
+  else if (is_ref && isNotScalar && type.category != Qualified::EIGEN)
     // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
     file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
   else
-    // Not a pointer or a reference: emit an "unwrap" call
+    // Not a pointer, or a reference to a scalar type. Therefore, emit an "unwrap" call
     // unwrap is specified in matlab.h as a series of template specializations
     // that know how to unpack the expected MATLAB object
     // example: double tol = unwrap< double >(in[2]);
@@ -86,7 +92,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
     file.oss << cppType << " " << name << " = unwrap< ";
 
   file.oss << cppType << " >(" << matlabName;
-  if( (is_ptr || is_ref) && type.category != Qualified::EIGEN)
+  if( (is_ptr || is_ref) && isNotScalar && type.category != Qualified::EIGEN)
     file.oss << ", \"ptr_" << matlabUniqueType << "\"";
   file.oss << ");" << endl;
 }