diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 463b5f5d9..87ead88f0 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/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::Expmap(v.template head()); - H h = traits::Expmap(v.template tail()); + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); + H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + if (Hv) { + Hv->setZero(); + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = 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::TangentVector v1 = traits::Logmap(p.first); - typename traits::TangentVector v2 = traits::Logmap(p.second); + Jacobian1 D_g_first; Jacobian2 D_h_second; + typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; + if (Hp) { + Hp->setZero(); + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; + } return v; } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 5456ae7f5..43644b5c4 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } 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; diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index 6f74f9806..a1691be32 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/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) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 2e95ea33a..2d7ac72f7 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from gtsampy import * +from _gtsampy import * diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index fd022dd17..689354b4e 100644 --- a/python/handwritten/CMakeLists.txt +++ b/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 $ ${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}) \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 94dc10e56..8fa7e0fdd 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/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 diff --git a/python/setup.py.in b/python/setup.py.in index d3b5fcde4..8b2de7352 100644 --- a/python/setup.py.in +++ b/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 ) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index a134a899c..2d8a0b975 100644 --- a/tests/testLie.cpp +++ b/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; diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 8d35c1e12..149d6db18 100644 --- a/wrap/Argument.cpp +++ b/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; }