diff --git a/.cproject b/.cproject index 59ff12b72..617aa3795 100644 --- a/.cproject +++ b/.cproject @@ -2409,6 +2409,46 @@ true true + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + make -j5 @@ -2657,6 +2697,14 @@ false true + + make + + testSymbolicBayesNetB.run + true + false + true + make -j5 diff --git a/README.md b/README.md index 460f51bf3..623b1ff32 100644 --- a/README.md +++ b/README.md @@ -1,47 +1,49 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== - -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -Additional Information ----------------------- - -See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. - -Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM. +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`] here. + +See the [`INSTALL`] file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM. \ No newline at end of file diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 9a0b297ab..2f11df94e 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) if(MSVC) - set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) @@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) diff --git a/gtsam.h b/gtsam.h index f194995cc..67b3f2888 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1734,6 +1734,8 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); @@ -1746,9 +1748,10 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& t); void update(size_t j, const gtsam::EssentialMatrix& t); void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); - template + template T at(size_t j); }; @@ -2146,7 +2149,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 49d5a2fbb..301548dcf 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -45,6 +45,8 @@ endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) add_subdirectory(metis) +add_subdirectory(ceres) + ############ NOTE: When updating GeographicLib be sure to disable building their examples ############ and unit tests by commenting out their lines: # add_subdirectory (examples) diff --git a/gtsam/3rdparty/ceres/CMakeLists.txt b/gtsam/3rdparty/ceres/CMakeLists.txt new file mode 100644 index 000000000..98b2cffce --- /dev/null +++ b/gtsam/3rdparty/ceres/CMakeLists.txt @@ -0,0 +1,2 @@ +file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h") +install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres) \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam/3rdparty/ceres/autodiff.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_autodiff.h rename to gtsam/3rdparty/ceres/autodiff.h index 2a0ac8987..f124425cc 100644 --- a/gtsam_unstable/nonlinear/ceres_autodiff.h +++ b/gtsam/3rdparty/ceres/autodiff.h @@ -142,10 +142,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #define DCHECK assert #define DCHECK_GT(a,b) assert((a)>(b)) diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam/3rdparty/ceres/eigen.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_eigen.h rename to gtsam/3rdparty/ceres/eigen.h diff --git a/gtsam_unstable/nonlinear/ceres_example.h b/gtsam/3rdparty/ceres/example.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_example.h rename to gtsam/3rdparty/ceres/example.h index 45ec3428e..6b051fce0 100644 --- a/gtsam_unstable/nonlinear/ceres_example.h +++ b/gtsam/3rdparty/ceres/example.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Templated pinhole camera model for used with Ceres. The camera is // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_fixed_array.h rename to gtsam/3rdparty/ceres/fixed_array.h index 69a426379..db1591636 100644 --- a/gtsam_unstable/nonlinear/ceres_fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam/3rdparty/ceres/fpclassify.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_fpclassify.h rename to gtsam/3rdparty/ceres/fpclassify.h diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam/3rdparty/ceres/jet.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_jet.h rename to gtsam/3rdparty/ceres/jet.h index ed4834caf..12d4e8bc9 100644 --- a/gtsam_unstable/nonlinear/ceres_jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -163,7 +163,7 @@ #include #include -#include +#include namespace ceres { diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam/3rdparty/ceres/macros.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_macros.h rename to gtsam/3rdparty/ceres/macros.h diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam/3rdparty/ceres/manual_constructor.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_manual_constructor.h rename to gtsam/3rdparty/ceres/manual_constructor.h diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam/3rdparty/ceres/rotation.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_rotation.h rename to gtsam/3rdparty/ceres/rotation.h index b02c10211..6ed3b88cb 100644 --- a/gtsam_unstable/nonlinear/ceres_rotation.h +++ b/gtsam/3rdparty/ceres/rotation.h @@ -47,6 +47,7 @@ #include #include +#include #include #define DCHECK assert diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam/3rdparty/ceres/variadic_evaluate.h similarity index 97% rename from gtsam_unstable/nonlinear/ceres_variadic_evaluate.h rename to gtsam/3rdparty/ceres/variadic_evaluate.h index 7d22fe22e..86aec4829 100644 --- a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h +++ b/gtsam/3rdparty/ceres/variadic_evaluate.h @@ -34,9 +34,9 @@ #include -#include -#include -#include +#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index a3a5b029b..1190822ed 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -73,7 +73,6 @@ template struct dimension: public Dynamic { }; - /** * zero::value is intended to be the origin of a canonical coordinate system * with canonical(t) == DefaultChart::local(zero::value, t) @@ -111,14 +110,16 @@ struct is_group > : publi }; template -struct is_manifold > : public boost::true_type{ +struct is_manifold > : public boost::true_type { }; template -struct dimension > : public boost::integral_constant { - //TODO after switch to c++11 : the above should should be extracted to a constexpr function - // for readability and to reduce code duplication +struct dimension > : public boost::integral_constant< + int, + M == Eigen::Dynamic ? Eigen::Dynamic : + (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { + //TODO after switch to c++11 : the above should should be extracted to a constexpr function + // for readability and to reduce code duplication }; template @@ -131,10 +132,10 @@ struct zero > { }; template -struct identity > : public zero > { +struct identity > : public zero< + Eigen::Matrix > { }; - template struct is_chart: public boost::false_type { }; @@ -248,12 +249,16 @@ struct DefaultChart > { * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. */ - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; typedef type T; - typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "DefaultChart has not been implemented yet for dynamically sized matrices"); + typedef Eigen::Matrix::value, 1> vector; + + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "Internal error: DefaultChart for Dynamic should be handled by template below"); + static vector local(const T& origin, const T& other) { - return reshape(other) - reshape(origin); + return reshape(other) + - reshape(origin); } static T retract(const T& origin, const vector& d) { return origin + reshape(d); @@ -266,20 +271,36 @@ struct DefaultChart > { // Dynamically sized Vector template<> struct DefaultChart { - typedef Vector T; - typedef T type; - typedef T vector; - static vector local(const T& origin, const T& other) { + typedef Vector type; + typedef Vector vector; + static vector local(const Vector& origin, const Vector& other) { return other - origin; } - static T retract(const T& origin, const vector& d) { + static Vector retract(const Vector& origin, const vector& d) { return origin + d; } - static int getDimension(const T& origin) { + static int getDimension(const Vector& origin) { return origin.size(); } }; +// Dynamically sized Matrix +template<> +struct DefaultChart { + typedef Matrix type; + typedef Vector vector; + static vector local(const Matrix& origin, const Matrix& other) { + Matrix d = other - origin; + return Eigen::Map(d.data(),getDimension(d)); + } + static Matrix retract(const Matrix& origin, const vector& d) { + return origin + Eigen::Map(d.data(),origin.rows(),origin.cols()); + } + static int getDimension(const Matrix& m) { + return m.size(); + } +}; + /** * Old Concept check class for Manifold types * Requires a mapping between a linear tangent space and the underlying diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 50bed2e3b..7299d7c8a 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -26,8 +26,8 @@ namespace gtsam { /* ************************************************************************* */ template void MetisIndex::augment(const FactorGraph& factors) { - std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first - std::map >::iterator iAdjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ @@ -36,7 +36,7 @@ void MetisIndex::augment(const FactorGraph& factors) { // starting at index xadj[i] and ending at(but not including) // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). - idx_t keyCounter = 0; + int32_t keyCounter = 0; // First: Record a copy of each key inside the factorgraph and create a // key to integer mapping. This is referenced during the adjaceny step @@ -58,7 +58,7 @@ void MetisIndex::augment(const FactorGraph& factors) { BOOST_FOREACH(const Key& k1, *factors[i]) BOOST_FOREACH(const Key& k2, *factors[i]) if (k1 != k2) { - // Store both in Key and idx_t format + // Store both in Key and int32_t format int i = intKeyBMap_.left.at(k1); int j = intKeyBMap_.left.at(k2); iAdjMap[i].insert(iAdjMap[i].end(), j); @@ -71,14 +71,14 @@ void MetisIndex::augment(const FactorGraph& factors) { xadj_.push_back(0); // Always set the first index to zero for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { - std::vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); - xadj_.push_back((idx_t) adj_.size()); + xadj_.push_back((int32_t) adj_.size()); } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 51624e29e..22b03ee5d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -22,7 +22,6 @@ #include #include #include -#include // Boost bimap generates many ugly warnings in CLANG #ifdef __clang__ @@ -47,13 +46,13 @@ namespace gtsam { class GTSAM_EXPORT MetisIndex { public: typedef boost::shared_ptr shared_ptr; - typedef boost::bimap bm_type; + typedef boost::bimap bm_type; private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; - boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship size_t nKeys_; public: @@ -84,16 +83,16 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { + std::vector xadj() const { return xadj_; } - std::vector adj() const { + std::vector adj() const { return adj_; } size_t nValues() const { return nKeys_; } - Key intToKey(idx_t value) const { + Key intToKey(int32_t value) const { assert(value >= 0); return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 3c397c9e9..d6e1b6e98 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -106,9 +106,9 @@ namespace gtsam { typedef boost::tuple Base; KeyInfoEntry(){} KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - const size_t index() const { return this->get<0>(); } - const size_t dim() const { return this->get<1>(); } - const size_t colstart() const { return this->get<2>(); } + size_t index() const { return this->get<0>(); } + size_t dim() const { return this->get<1>(); } + size_t colstart() const { return this->get<2>(); } }; /************************************************************************************/ diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp new file mode 100644 index 000000000..a2ce631ea --- /dev/null +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -0,0 +1,497 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file CombinedImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class CombinedPreintegratedMeasurements +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : + biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + deltaRij_(Rot3()), deltaTij_(0.0), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) +{ + measurementCovariance_.setZero(); + measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; + measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; + measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; + measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; + measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; + measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ + cout << s << endl; + biasHat_.print(" biasHat"); + cout << " deltaTij " << deltaTij_ << endl; + cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; + deltaRij_.print(" deltaRij "); + cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, + double deltaT, boost::optional body_P_sensor) { + + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). + + // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + }else{ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() + * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + } + + delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; + delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; + delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we + // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij_ * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + + // Single Jacobians to propagate covariance + Matrix3 H_pos_pos = I_3x3; + Matrix3 H_pos_vel = I_3x3 * deltaT; + Matrix3 H_pos_angles = Z_3x3; + + Matrix3 H_vel_pos = Z_3x3; + Matrix3 H_vel_vel = I_3x3; + Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; + + Matrix3 H_angles_pos = Z_3x3; + Matrix3 H_angles_vel = Z_3x3; + Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(15,15); + F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + + // first order uncertainty propagation + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() + + Matrix G_measCov_Gt = Matrix::Zero(15,15); + // BLOCK DIAGONAL TERMS + G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); + + G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * + (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (H_vel_biasacc.transpose()); + + G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * + (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (H_angles_biasomega.transpose()); + + G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); + + G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); + + // NEW OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3,3>(3,6) = block23; + G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + + PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } + deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; +} + +//------------------------------------------------------------------------------ +// CombinedImuFactor methods +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); +} + +//------------------------------------------------------------------------------ +Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5, boost::optional H6) const { + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(15,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3, + //dBiasAcc/dPi + Z_3x3, Z_3x3, + //dBiasOmega/dPi + Z_3x3, Z_3x3; + } + + if(H2) { + H2->resize(15,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3, + //dBiasAcc/dVi + Z_3x3, + //dBiasOmega/dVi + Z_3x3; + } + + if(H3) { + H3->resize(15,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3, + //dBiasAcc/dPosej + Z_3x3, Z_3x3, + //dBiasOmega/dPosej + Z_3x3, Z_3x3; + } + + if(H4) { + H4->resize(15,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3, + //dBiasAcc/dVj + Z_3x3, + //dBiasOmega/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(15,6); + (*H5) << + // dfP/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias_i + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), + //dBiasAcc/dBias_i + -I_3x3, Z_3x3, + //dBiasOmega/dBias_i + Z_3x3, -I_3x3; + } + + if(H6) { + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Z_3x3, Z_3x3, + // dfV/dBias_j + Z_3x3, Z_3x3, + // dfR/dBias_j + Z_3x3, Z_3x3, + //dBiasAcc/dBias_j + I_3x3, Z_3x3, + //dBiasOmega/dBias_j + Z_3x3, I_3x3; + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); + + const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); + + Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 + + return r; +} + +//------------------------------------------------------------------------------ +PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ + + const double& deltaTij = preintegratedMeasurements.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ + + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + + return PoseVelocityBias(pose_j, vel_j, bias_i); +} + +} /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 069723eca..5e2bfeadb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,714 +11,291 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ -#include -#include #include -#include +#include #include -/* External or standard includes */ -#include - - namespace gtsam { - /** - * Struct to hold all state variables of CombinedImuFactor - * returned by predict function - */ - struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : +/** + * Struct to hold all state variables of CombinedImuFactor returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : pose(_pose), velocity(_velocity), bias(_bias) { - } - }; + } +}; - /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. +/** + * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias + * at previous time step), and current state (pose, velocity, bias at current time step). According to the + * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are + * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). + * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices + * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty + * and the preintegrated measurements uncertainty. + */ +class CombinedImuFactor: public NoiseModelFactor6 { +public: + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). + * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received + * from the IMU) so as to avoid costly integration at time of factor construction. */ + class CombinedPreintegratedMeasurements { - class CombinedImuFactor: public NoiseModelFactor6 { + friend class CombinedImuFactor; + + protected: + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector + ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation + ///< between the preintegrated measurements and the biases + + bool use2ndOrderIntegration_; ///< Controls the order of integration public: - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution) + * @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution) + * @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class CombinedPreintegratedMeasurements { - friend class CombinedImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + /// equals + bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration -// public: - ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - /** Default constructor, initialize with no IMU measurements */ - public: - CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) - ) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(use2ndOrderIntegration) - { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + /// Re-initialize CombinedPreintegratedMeasurements + void resetIntegration(); - } + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none); - CombinedPreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(false) ///< Controls the order of integration - { - } + /// methods to access class variables + Matrix measurementCovariance() const {return measurementCovariance_;} + Matrix deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const{return deltaTij_;} + Vector deltaPij() const {return deltaPij_;} + Vector deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} + Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} + Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} + Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} + Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} + Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} + Matrix PreintMeasCov() const { return PreintMeasCov_;} - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - /** equals */ - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(15,15); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; - - - // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3); - - G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block(3,3,3,3) + measurementCovariance_.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block(6,6,3,3) + measurementCovariance_.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); - - G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3); - - G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3); - - // NEW OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose(); - G_measCov_Gt.block(3,6,3,3) = block23; - G_measCov_Gt.block(6,3,3,3) = block23.transpose(); - - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: - - typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; - - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - - /** Default constructor - only use for serialization */ - CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} - - /** Constructor */ - CombinedImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias_i, ///< previous bias key - Key bias_j, ///< current bias key - const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ - } - - virtual ~CombinedImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," - << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - /* - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - */ - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), - //dBiasAcc/dPosej - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPosej - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(), - //dBiasAcc/dVj - Matrix3::Zero(), - //dBiasOmega/dVj - Matrix3::Zero(); - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_i - Matrix3::Zero(), -Matrix3::Identity(); - } - - if(H6) { - - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - - return r; - } - - - /** predicted states from IMU */ - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); - } - - - private: - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } - }; // \class CombinedImuFactor + }; - typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; +private: + + typedef CombinedImuFactor This; + typedef NoiseModelFactor6 Base; + + CombinedPreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + CombinedImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias_i Previous bias key + * @param bias_j Current bias key + * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + + virtual ~CombinedImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + + /** Access the preintegrated measurements. */ + + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none, + boost::optional H6 = boost::none) const; + + /// predicted states from IMU + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // class CombinedImuFactor + +typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp new file mode 100644 index 000000000..9a9cc9d62 --- /dev/null +++ b/gtsam/navigation/ImuFactor.cpp @@ -0,0 +1,438 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file ImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class PreintegratedMeasurements +//------------------------------------------------------------------------------ +ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration) : + biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + deltaRij_(Rot3()), deltaTij_(0.0), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) +{ + measurementCovariance_.setZero(); + measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; + measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; + measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; + PreintMeasCov_.setZero(9,9); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::print(const string& s) const { + cout << s << endl; + biasHat_.print(" biasHat"); + cout << " deltaTij " << deltaTij_ << endl; + cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; + deltaRij_.print(" deltaRij "); + cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::resetIntegration(){ + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor) { + + // NOTE: order is important here because each update uses old values (i.e., we have to update + // jacobians and covariances before updating preintegrated measurements). + + // First we compensate the measurements for the bias + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + }else{ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() + * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + } + delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; + delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; + delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance + // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij_ * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + + Matrix H_pos_pos = I_3x3; + Matrix H_pos_vel = I_3x3 * deltaT; + Matrix H_pos_angles = Z_3x3; + + Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = I_3x3; + Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(9,9); + F << H_pos_pos, H_pos_vel, H_pos_angles, + H_vel_pos, H_vel_vel, H_vel_angles, + H_angles_pos, H_angles_vel, H_angles_angles; + + // first order uncertainty propagation: + // the deltaT allows to pass from continuous time noise to discrete time noise + // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) + // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT + PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + + // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT + // This in only kept for documentation. + // + // Matrix G(9,9); + // G << I_3x3 * deltaT, Z_3x3, Z_3x3, + // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, + // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; + // + // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + + // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } + deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; +} + +//------------------------------------------------------------------------------ +// ImuFactor methods +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} + +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, + const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); +} + +//------------------------------------------------------------------------------ +bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); +} + +//------------------------------------------------------------------------------ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5) const +{ + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; + } + + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3; + } + + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3; + } + + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; +} + +//------------------------------------------------------------------------------ +PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) +{ + + const double& deltaTij = preintegratedMeasurements.deltaTij_; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ + + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocity(pose_j, vel_j); +} + +} /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d1b8a9d80..2af634926 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,23 +11,39 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ #include -#include #include -#include #include -/* External or standard includes */ -#include - - namespace gtsam { + +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + /** * Struct to hold return variables by the Predict Function */ @@ -35,265 +51,121 @@ struct PoseVelocity { Pose3 pose; Vector3 velocity; PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { + pose(_pose), velocity(_velocity) { } }; +/** + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), + * current state (pose and velocity at current time step), and the bias estimate. According to the + * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are + * "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not force "temporal consistency" of the biases (which are usually + * slowly varying quantities), see also CombinedImuFactor for more details. + */ +class ImuFactor: public NoiseModelFactor5 { +public: + /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * PreintegratedMeasurements accumulates (integrates) the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). + * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received + * from the IMU) so as to avoid costly integration at time of factor construction. */ + class PreintegratedMeasurements { - class ImuFactor: public NoiseModelFactor5 { - public: + friend class ImuFactor; - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ + protected: + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class PreintegratedMeasurements { - friend class ImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). + + bool use2ndOrderIntegration_; ///< Controls the order of integration - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration public: - /** Default constructor, initialize with no IMU measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) - { - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov_ = Matrix::Zero(9,9); - } - PreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) - { - measurementCovariance_ = Matrix::Zero(9,9); - PreintMeasCov_ = Matrix::Zero(9,9); - } + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; - /** equals */ - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} + /// equals + bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(9,9); - } + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none); - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { + /// methods to access class variables + Matrix measurementCovariance() const {return measurementCovariance_;} + Matrix deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const{return deltaTij_;} + Vector deltaPij() const {return deltaPij_;} + Vector deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} + Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} + Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} + Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} + Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} + Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} + Matrix preintMeasCov() const { return PreintMeasCov_;} - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation: - // the deltaT allows to pass from continuous time noise to discrete time noise - // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: /** Serialization function */ @@ -336,65 +208,41 @@ struct PoseVelocity { #endif /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){} + ImuFactor(); - /** Constructor */ - ImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias, ///< previous bias key - const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ - } + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); virtual ~ImuFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual gtsam::NonlinearFactor::shared_ptr clone() const; /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; /** Access the preintegrated measurements. */ + const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } @@ -404,205 +252,19 @@ struct PoseVelocity { /** implement functions needed to derive from Factor */ - /** vector of errors */ + /// vector of errors Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const - { + boost::optional H5 = boost::none) const; - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(); - } - - if(H5) { - - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - - /** predicted states from IMU */ + /// predicted states from IMU static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); - } - + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: @@ -617,7 +279,7 @@ struct PoseVelocity { ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } - }; // \class ImuFactor + }; // class ImuFactor typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fda82eca9..aab38f258 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -10,19 +10,22 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @file testCombinedImuFactor.cpp + * @brief Unit test for Lupton-style combined IMU factor + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts */ -#include -#include #include #include #include #include +#include +#include #include #include + #include #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index eac021f4e..cf81c32ae 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -39,15 +39,13 @@ using symbol_shorthand::B; namespace { Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ + const imuBias::ConstantBias& bias){ return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ + const imuBias::ConstantBias& bias){ return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } @@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); @@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaPij(); } @@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) -{ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) -{ +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); } @@ -212,7 +202,6 @@ TEST( ImuFactor, Error ) Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // positions and velocities Matrix H1etop6 = H1e.topRows(6); Matrix H1atop6 = H1a.topRows(6); @@ -230,7 +219,7 @@ TEST( ImuFactor, Error ) EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H4e, H4a)); -// EXPECT(assert_equal(H5e, H5a)); + // EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ @@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases ) // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Vector3 v2(Vector3(0.5, 0.0, 0.0)); - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); @@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases ) ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); + // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); @@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases ) // Expected error Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; -// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; double deltaT = 0.5; - // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); @@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap ) // Compare Jacobians EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations - } /* ************************************************************************* */ @@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap ) const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; -// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; -// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; - // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); @@ -361,30 +344,30 @@ TEST( ImuFactor, PartialDerivativeLogmap ) TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias - // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; - double deltaT = 1.0; + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 1.0; - // change w.r.t. linearization point - double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - // Compare Jacobians - EXPECT(assert_equal(expectedRot, actualRot)); + // Compare Jacobians + EXPECT(assert_equal(expectedRot, actualRot)); } /* ************************************************************************* */ @@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) // tictoc_print_(); //} - /* ************************************************************************* */ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { @@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); -// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), -// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega); - - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) EXPECT(assert_equal(H5e, H5a)); } +/* ************************************************************************* */ TEST(ImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ } +/* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h similarity index 97% rename from gtsam_unstable/nonlinear/AdaptAutoDiff.h rename to gtsam/nonlinear/AdaptAutoDiff.h index 5a4b5a09a..5118426b4 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index ebc1d8f15..d6dc5088d 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -109,7 +109,7 @@ struct UseBlockIf { }; } -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { @@ -188,28 +188,28 @@ public: } } /** - * *** This is the main entry point for reverseAD, called from Expression *** + * *** This is the main entry point for reverse AD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ typedef Eigen::Matrix JacobianTT; - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD1(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors static const JacobianTT I = JacobianTT::Identity(); handleLeafCase(I, jacobians, content.key); } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD(jacobians); + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) template - void reverseAD(const Eigen::MatrixBase & dTdA, + void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA, jacobians); + content.ptr->reverseAD2(dTdA, jacobians); } /// Define type so we can apply it as a meta-function @@ -471,10 +471,10 @@ struct FunctionalBase: ExpressionNode { struct Record { void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { } }; /// Construct an execution trace for reverse AD @@ -506,9 +506,9 @@ struct JacobianTrace { typename Jacobian::type dTdA; }; -/** - * Recursive Definition of Functional ExpressionNode - */ +// Recursive Definition of Functional ExpressionNode +// The reason we inherit from Argument is because we can then +// case to this unique signature to retrieve the expression at any level template struct GenerateFunctionalNode: Argument, Base { @@ -529,7 +529,9 @@ struct GenerateFunctionalNode: Argument, Base { This::expression->dims(map); } - /// Recursive Record Class for Functional Expressions + // Recursive Record Class for Functional Expressions + // The reason we inherit from JacobianTrace is because we can then + // case to this unique signature to retrieve the value/trace at any level struct Record: JacobianTrace, Base::Record { typedef T return_type; @@ -544,17 +546,26 @@ struct GenerateFunctionalNode: Argument, Base { } /// Start the reverse AD process - void startReverseAD(JacobianMap& jacobians) const { - Base::Record::startReverseAD(jacobians); - This::trace.reverseAD(This::dTdA, jacobians); + void startReverseAD4(JacobianMap& jacobians) const { + Base::Record::startReverseAD4(jacobians); + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + This::trace.reverseAD1(This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process + // Cols is always known at compile time template - void reverseAD(const Eigen::Matrix & dFdT, + void reverseAD4(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { - Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); + Base::Record::reverseAD4(dFdT, jacobians); + This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } }; @@ -615,8 +626,8 @@ struct FunctionalNode { struct Record: public internal::CallRecordImplementor::value>, public Base::Record { using Base::Record::print; - using Base::Record::startReverseAD; - using Base::Record::reverseAD; + using Base::Record::startReverseAD4; + using Base::Record::reverseAD4; virtual ~Record() { } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 71bd16b1c..498e865b9 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -209,7 +209,7 @@ private: ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD(jacobians); + trace.startReverseAD1(jacobians); return value; } diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp similarity index 98% rename from gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp rename to gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index a8151ec11..7d31616c5 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -17,8 +17,8 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include +#include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09fe0f253..941728d8c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -12,6 +12,8 @@ /** * @file testValues.cpp * @author Richard Roberts + * @author Frank Dellaert + * @author Mike Bosse */ #include @@ -168,9 +170,9 @@ TEST(Values, basic_functions) Values values; const Values& values_c = values; values.insert(2, Vector3()); - values.insert(4, Vector3()); - values.insert(6, Vector3()); - values.insert(8, Vector3()); + values.insert(4, Vector(3)); + values.insert(6, Matrix23()); + values.insert(8, Matrix(2,3)); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 1b630374c..9c5df7ea0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -74,25 +74,26 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + traits::print()(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none, boost::optional H2 = + boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) + DefaultChart chart; // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.localCoordinates(hx); + return chart.local(measured_, hx); } /** return the measured */ @@ -129,7 +130,9 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) + {} private: diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..8fbbd4d88 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,23 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::print()(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(p.dim()); + DefaultChart chart; + if (H) (*H) = eye(chart.getDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + return chart.local(prior_,p); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d0dbe7908..b22763099 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) { EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* */ +/* +// Constructor scalar +TEST(BetweenFactor, ConstructorScalar) { + SharedNoiseModel model; + double measured_value = 0.0; + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor vector3 +TEST(BetweenFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Vector3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor dynamic sized vector +TEST(BetweenFactor, ConstructorDynamicSizeVector) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured_value, model); +} +*/ + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b3e54bedb..b26d633f5 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,8 +5,8 @@ * @date Nov 4, 2014 */ +#include #include -#include #include using namespace std; @@ -14,10 +14,23 @@ using namespace gtsam; /* ************************************************************************* */ -// Constructor -TEST(PriorFactor, Constructor) { +// Constructor scalar +TEST(PriorFactor, ConstructorScalar) { SharedNoiseModel model; - PriorFactor factor(1, LieScalar(1.0), model); + PriorFactor factor(1, 1.0, model); +} + +// Constructor vector3 +TEST(PriorFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + PriorFactor factor(1, Vector3(1,2,3), model); +} + +// Constructor dynamic sized vector +TEST(PriorFactor, ConstructorDynamicSizeVector) { + Vector v(5); v << 1, 2, 3, 4, 5; + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + PriorFactor factor(1, v, model); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index f1ac0b044..159a841e5 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -32,12 +32,6 @@ class JacobianMap; // forward declaration //----------------------------------------------------------------------------- -/** - * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific - * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. - */ -const int MaxVirtualStaticRows = 4; - namespace internal { /** @@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType { template<> struct ConvertToVirtualFunctionSupportedMatrixType { template - static const Eigen::Matrix convert( + static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } @@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -/** - * Recursive definition of an interface having several purely - * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) - * with Rows in 1..MaxSupportedStaticRows - */ -template -struct ReverseADInterface: ReverseADInterface { - using ReverseADInterface::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -template -struct ReverseADInterface<0, Cols> { - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -/** - * ReverseADImplementor is a utility class used by CallRecordImplementor to - * implementing the recursive ReverseADInterface interface. - */ -template -struct ReverseADImplementor: ReverseADImplementor { -private: - using ReverseADImplementor::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - static_cast(this)->reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - -template -struct ReverseADImplementor : virtual internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { -private: - using internal::ReverseADInterface::_reverseAD; - const Derived & derived() const { - return static_cast(*this); - } - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - } // namespace internal /** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an execution trace - * (defined below) for each of its arguments. - * - * It is implemented in the function-style ExpressionNode's nested Record class below. + * The CallRecord is an abstract base class for the any class that stores + * the Jacobians of applying a function with respect to each of its arguments, + * as well as an execution trace for each of its arguments. */ template -struct CallRecord: virtual private internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { +struct CallRecord { + // Print entire record, recursively inline void print(const std::string& indent) const { _print(indent); } - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); + // Main entry point for the reverse AD process of a functional expression. + // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 + // This function then calls ExecutionTrace::reverseAD for every argument + // which will in turn call the reverseAD method below. + // This non-virtual function _startReverseAD3, implemented in derived + inline void startReverseAD2(JacobianMap& jacobians) const { + _startReverseAD3(jacobians); } + // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 + // Here we convert to dynamic if the template - inline void reverseAD(const Eigen::MatrixBase & dFdT, + inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { - _reverseAD( - internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( - dFdT), jacobians); + _reverseAD3( + internal::ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), + jacobians); } - inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - _reverseAD(dFdT, jacobians); + // This overload supports matrices with both rows and columns dynamically sized. + // The template version above would be slower by introducing an extra conversion + // to statically sized columns. + inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD3(dFdT, jacobians); } virtual ~CallRecord() { } private: + virtual void _print(const std::string& indent) const = 0; - virtual void _startReverseAD(JacobianMap& jacobians) const = 0; - using internal::ReverseADInterface::_reverseAD; + virtual void _startReverseAD3(JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; }; +/** + * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * interface. It is used to keep the testCallRecord unit test in sync. + */ +const int CallRecordMaxVirtualStaticRows = 5; + namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. */ template -struct CallRecordImplementor: public CallRecord, - private ReverseADImplementor { +struct CallRecordImplementor: public CallRecord { private: + const Derived & derived() const { return static_cast(*this); } + virtual void _print(const std::string& indent) const { derived().print(indent); } - virtual void _startReverseAD(JacobianMap& jacobians) const { - derived().startReverseAD(jacobians); + + virtual void _startReverseAD3(JacobianMap& jacobians) const { + derived().startReverseAD4(jacobians); + } + + virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); } - template friend struct ReverseADImplementor; }; } // namespace internal diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 95c4d71a8..de6e4983a 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -35,7 +35,6 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices - size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -55,15 +54,6 @@ public: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now boost::tie(keys_, dims_) = expression_.keysAndDims(); - - // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1); - -#ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dims_) - std::cout << d << " "; - std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; -#endif } /** diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index eda90e203..c3bf83f71 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -16,22 +16,9 @@ * @brief unit tests for Basis Decompositions w Expressions */ -#include -#include -#include -#include -#include -#include +#include -#include - -#include -using boost::assign::list_of; - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); +namespace gtsam { /// Fourier template @@ -67,6 +54,80 @@ public: } }; +} + +#include +#include +#include +#include + +namespace gtsam { + +/// For now, this is our sequence representation +typedef std::map Sequence; + +/** + * Class that does Fourier Decomposition via least squares + */ +class FourierDecomposition { +public: + + typedef Vector3 Coefficients; ///< Fourier coefficients + +private: + Coefficients c_; + +public: + + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model) { + NonlinearFactorGraph graph; + Expression c(0); + typedef std::pair Sample; + BOOST_FOREACH(Sample sample, sequence) { + Expression expression(Fourier<3>(sample.first), c); + ExpressionFactor factor(model, sample.second, expression); + graph.add(factor); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, + const SharedNoiseModel& model) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model); + Values values; + values.insert(0, Coefficients::Zero()); // does not matter + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /// Constructor + FourierDecomposition(const Sequence& sequence, + const SharedNoiseModel& model) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); + VectorValues solution = gfg->optimize(); + c_ = solution.at(0); + } + + /// Return Fourier coefficients + Coefficients coefficients() { + return c_; + } +}; + +} + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + //****************************************************************************** TEST(BasisDecompositions, Fourier) { Fourier<3> fx(0); @@ -79,12 +140,14 @@ TEST(BasisDecompositions, Fourier) { } //****************************************************************************** -TEST(BasisDecompositions, FourierExpression) { +TEST(BasisDecompositions, ManualFourier) { // Create linear factor graph GaussianFactorGraph g; Key key(1); - Vector3_ c(key); + Expression c(key); + Values values; + values.insert(key, Vector3::Zero()); // does not matter for (size_t i = 0; i < 16; i++) { double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); @@ -93,13 +156,44 @@ TEST(BasisDecompositions, FourierExpression) { A << 1, cos(x), sin(x); Vector b(1); b << y; - JacobianFactor f1(key, A, b, model); + JacobianFactor f1(key, A, b); + g.add(f1); // With ExpressionFactor Expression expression(Fourier<3>(x), c); - ExpressionFactor f2(model, y, expression); - - g.add(f1); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } } // Solve @@ -110,6 +204,24 @@ TEST(BasisDecompositions, FourierExpression) { EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); } +//****************************************************************************** +TEST(BasisDecompositions, FourierDecomposition) { + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); + sequence[x] = y; + } + + // Do Fourier Decomposition + FourierDecomposition actual(sequence, model); + + // Check + Vector3 expected(1.5661, 1.2717, 1.2717); + EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index a4561b349..1cc674901 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -33,7 +33,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > MaxVirtualStaticRows){ + if(i > CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -43,7 +43,6 @@ struct CallConfig { int compTimeCols; int runTimeRows; int runTimeCols; - CallConfig() {} CallConfig(int rows, int cols): compTimeRows(dynamicIfAboveMax(rows)), compTimeCols(cols), @@ -72,25 +71,26 @@ struct CallConfig { }; struct Record: public internal::CallRecordImplementor { + Record() : cc(0, 0) {} virtual ~Record() { } void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); cc.runTimeCols = dFdT.cols(); } - template - friend struct internal::ReverseADImplementor; + template + friend struct internal::CallRecordImplementor; }; JacobianMap & NJM= *static_cast(NULL); @@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) { Record record; { const int Rows = 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 3; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 4; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 5; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 6; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index a412b47eb..1107d1e30 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) { // Concise version ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ @@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) { EXPECT( assert_equal(expected, *jf, 1e-9)); } +/* ************************************************************************* */ +// Unary(Leaf)) and Unary(Unary(Leaf))) +// wide version (not handled in fixed-size pipeline) +typedef Eigen::Matrix Matrix93; +Vector9 wide(const Point3& p, boost::optional H) { + Vector9 v; + v << p.vector(), p.vector(), p.vector(); + if (H) *H << eye(3), eye(3), eye(3); + return v; +} +typedef Eigen::Matrix Matrix9; +Vector9 id9(const Vector9& v, boost::optional H) { + if (H) *H = Matrix9::Identity(); + return v; +} +TEST(ExpressionFactor, Wide) { + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + Point3_ point(2); + Vector9 measured; + Expression expression(wide,point); + SharedNoiseModel model = noiseModel::Unit::Create(9); + + ExpressionFactor f1(model, measured, expression); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9); + + Expression expression2(id9,expression); + ExpressionFactor f2(model, measured, expression2); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9); +} + /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp index 32bce9ba5..be11453b5 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp @@ -17,7 +17,7 @@ */ #include "timeLinearize.h" -#include +#include #include #include #include diff --git a/matlab/gtsam_tests/.gitignore b/matlab/gtsam_tests/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/gtsam_tests/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m new file mode 100644 index 000000000..fe2cd30fe --- /dev/null +++ b/matlab/gtsam_tests/testValues.m @@ -0,0 +1,40 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; +E = EssentialMatrix(Rot3,Unit3); +tol = 1e-9; + +values.insert(0, Point2); +values.insert(1, Point3); +values.insert(2, Rot2); +values.insert(3, Pose2); +values.insert(4, Rot3); +values.insert(5, Pose3); +values.insert(6, Cal3_S2); +values.insert(7, Cal3DS2); +values.insert(8, Cal3Bundler); +values.insert(9, E); +values.insert(10, imuBias.ConstantBias); + +% special cases for Vector and Matrix: +values.insert(11, [1;2;3]); +values.insert(12, [1 2;3 4]); + +EXPECT('at',values.atPoint2(0).equals(Point2,tol)); +EXPECT('at',values.atPoint3(1).equals(Point3,tol)); +EXPECT('at',values.atRot2(2).equals(Rot2,tol)); +EXPECT('at',values.atPose2(3).equals(Pose2,tol)); +EXPECT('at',values.atRot3(4).equals(Rot3,tol)); +EXPECT('at',values.atPose3(5).equals(Pose3,tol)); +EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol)); +EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol)); +EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol)); +EXPECT('at',values.atEssentialMatrix(9).equals(E,tol)); +EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol)); + +% special cases for Vector and Matrix: +actualVector = values.atVector(11); +EQUALITY('at',[1;2;3],actualVector,tol); +actualMatrix = values.atMatrix(12); +EQUALITY('at',[1 2;3 4],actualMatrix,tol); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e3705948d..e08019610 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +display 'Starting: testValues' +testValues + display 'Starting: testJacobianFactor' testJacobianFactor diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 4989afb0d..1f57917d8 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -31,12 +32,13 @@ using namespace wrap; /* ************************************************************************* */ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { Argument instArg = *this; - instArg.type = ts(type); + instArg.type = ts.tryToSubstitite(type); return instArg; } /* ************************************************************************* */ -ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { +ArgumentList ArgumentList::expandTemplate( + const TemplateSubstitution& ts) const { ArgumentList instArgList; BOOST_FOREACH(const Argument& arg, *this) { Argument instArg = arg.expandTemplate(ts); @@ -48,25 +50,25 @@ ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, type.namespaces) + BOOST_FOREACH(const string& ns, type.namespaces()) result += ns + delim; - if (type.name == "string" || type.name == "unsigned char" - || type.name == "char") + if (type.name() == "string" || type.name() == "unsigned char" + || type.name() == "char") return result + "char"; - if (type.name == "Vector" || type.name == "Matrix") + if (type.name() == "Vector" || type.name() == "Matrix") return result + "double"; - if (type.name == "int" || type.name == "size_t") + if (type.name() == "int" || type.name() == "size_t") return result + "numeric"; - if (type.name == "bool") + if (type.name() == "bool") return result + "logical"; - return result + type.name; + return result + type.name(); } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type.name == "bool" || type.name == "char" - || type.name == "unsigned char" || type.name == "int" - || type.name == "size_t" || type.name == "double"); + return (type.name() == "bool" || type.name() == "char" + || type.name() == "unsigned char" || type.name() == "int" + || type.name() == "size_t" || type.name() == "double"); } /* ************************************************************************* */ @@ -97,6 +99,13 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << ");" << endl; } +/* ************************************************************************* */ +void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { + proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')"; + if (type.name() == "Vector") + proxyFile.oss << " && size(" << s << ",2)==1"; +} + /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -104,7 +113,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type.name; + str += arg.type.name(); first = false; } return str; @@ -116,14 +125,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type.name) + BOOST_FOREACH(char ch, arg.type.name()) if (isupper(ch)) { sig += ch; //If there is a capital letter, we don't want to read it below cap = true; } if (!cap) - sig += arg.type.name[0]; + sig += arg.type.name()[0]; //Reset to default cap = false; } @@ -170,25 +179,14 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type.name << " " << arg.name; + file.oss << arg.type.name() << " " << arg.name; first = false; } file.oss << ")"; } + /* ************************************************************************* */ -void ArgumentList::emit_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { - returnVal.emit_matlab(proxyFile); - proxyFile.oss << wrapperName << "(" << id; - if (!staticMethod) - proxyFile.oss << ", this"; - proxyFile.oss << ", varargin{:});\n"; -} -/* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { +void ArgumentList::proxy_check(FileWriter& proxyFile) const { // Check nr of arguments proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) @@ -198,15 +196,12 @@ void ArgumentList::emit_conditional_call(FileWriter& proxyFile, for (size_t i = 0; i < size(); i++) { if (!first) proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << (*this)[i].matlabClass(".") << "')"; + string s = "varargin{" + boost::lexical_cast(i + 1) + "}"; + (*this)[i].proxy_check(proxyFile, s); first = false; } proxyFile.oss << "\n"; - - // output call to C++ wrapper - proxyFile.oss << " "; - emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } + /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 3d8d7288f..fd7e82061 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -28,13 +28,23 @@ namespace wrap { /// Argument class struct Argument { Qualified type; - bool is_const, is_ref, is_ptr; std::string name; + bool is_const, is_ref, is_ptr; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + Argument(const Qualified& t, const std::string& n) : + type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { + } + + bool operator==(const Argument& other) const { + return type == other.type && name == other.name + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + Argument expandTemplate(const TemplateSubstitution& ts) const; /// return MATLAB class for use in isa(x,class) @@ -46,6 +56,12 @@ struct Argument { /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + /** + * emit checking argument to MATLAB proxy + * @param proxyFile output stream + */ + void proxy_check(FileWriter& proxyFile, const std::string& s) const; + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") << (arg.is_ref ? "&" : ""); @@ -88,26 +104,12 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to proxy + * emit checking arguments to MATLAB proxy * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call */ - void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; - - /** - * emit conditional MATLAB call to proxy (checking arguments first) - * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call - */ - void emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const std::string& wrapperName, int id, - bool staticMethod = false) const; + void proxy_check(FileWriter& proxyFile) const; + /// Output stream operator friend std::ostream& operator<<(std::ostream& os, const ArgumentList& argList) { os << "("; @@ -122,5 +124,88 @@ struct ArgumentList: public std::vector { }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentGrammar: public classic::grammar { + + wrap::Argument& result_; ///< successful parse will be placed in here + TypeGrammar argument_type_g; ///< Type parser for Argument::type + + /// Construct type grammar and specify where result is placed + ArgumentGrammar(wrap::Argument& result) : + result_(result), argument_type_g(result.type) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule argument_p; + + definition(ArgumentGrammar const& self) { + using namespace classic; + + // NOTE: allows for pointers to all types + // Slightly more permissive than before on basis/eigen type qualification + // Also, currently parses Point2*&, can't make it work otherwise :-( + argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] // + >> self.argument_type_g // + >> !ch_p('*')[assign_a(self.result_.is_ptr, T)] + >> !ch_p('&')[assign_a(self.result_.is_ref, T)] + >> BasicRules::name_p[assign_a(self.result_.name)]; + } + + Rule const& start() const { + return argument_p; + } + + }; +}; +// ArgumentGrammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentListGrammar: public classic::grammar { + + wrap::ArgumentList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + ArgumentListGrammar(wrap::ArgumentList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + const Argument arg0; ///< used to reset arg + Argument arg; ///< temporary argument for use during parsing + ArgumentGrammar argument_g; ///< single Argument parser + + classic::rule argument_p, argumentList_p; + + definition(ArgumentListGrammar const& self) : + argument_g(arg) { + using namespace classic; + + argument_p = argument_g // + [classic::push_back_a(self.result_, arg)] // + [assign_a(arg, arg0)]; + + argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')'; + } + + classic::rule const& start() const { + return argumentList_p; + } + + }; +}; +// ArgumentListGrammar + +/* ************************************************************************* */ + +}// \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 0e480f0fd..6e415065d 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -22,23 +22,68 @@ #include #include +#include +#include #include #include #include +#include // std::ostream_iterator //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC using namespace std; using namespace wrap; +/* ************************************************************************* */ +void Class::assignParent(const Qualified& parent) { + parentClass.reset(parent); +} + +/* ************************************************************************* */ +boost::optional Class::qualifiedParent() const { + boost::optional result = boost::none; + if (parentClass) + result = parentClass->qualifiedName("::"); + return result; +} + +/* ************************************************************************* */ +static void handleException(const out_of_range& oor, + const Class::Methods& methods) { + cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n"; + using boost::adaptors::map_keys; + ostream_iterator out_it(cerr, "\n"); + boost::copy(methods | map_keys, out_it); +} + +/* ************************************************************************* */ +Method& Class::mutableMethod(Str key) { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + +/* ************************************************************************* */ +const Method& Class::method(Str key) const { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + /* ************************************************************************* */ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); + createNamespaceStructure(namespaces(), toolboxPath); // open destination classFile string classFile = matlabName(toolboxPath); @@ -48,21 +93,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const string matlabQualName = qualifiedName("."); const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string matlabBaseName = qualifiedParent.qualifiedName("."); - const string cppBaseName = qualifiedParent.qualifiedName("::"); // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = + parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name() << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; // Constructor - proxyFile.oss << " function obj = " << name << "(varargin)\n"; + proxyFile.oss << " function obj = " << name() << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and // assign a pointer returned from a C++ function. In turn this MATLAB // constructor calls a special C++ function that just adds the object to @@ -75,11 +119,12 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, wrapperFile.oss << "\n"; // Regular constructors + boost::optional cppBaseName = qualifiedParent(); for (size_t i = 0; i < constructor.nrOverloads(); i++) { ArgumentList args = constructor.argumentList(i); const int id = (int) functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), - id, args); + constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id, + args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; @@ -89,9 +134,9 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; proxyFile.oss << " end\n"; - if (!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" - << ptr_constructor_key << "), base_ptr);\n"; + if (parentClass) + proxyFile.oss << " obj = obj@" << parentClass->qualifiedName(".") + << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; @@ -111,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { + BOOST_FOREACH(const Methods::value_type& name_m, methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -151,7 +196,6 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -188,7 +232,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } else { proxyFile.oss << " my_ptr = varargin{2};\n"; } - if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + if (!parentClass) // If this class has a base class, we'll get a base class pointer back proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; @@ -211,9 +255,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!qualifiedParent.empty()) { + boost::optional cppBaseName = qualifiedParent(); + if (cppBaseName) { wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + wrapperFile.oss << " typedef boost::shared_ptr<" << *cppBaseName << "> SharedBase;\n"; wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; @@ -244,10 +289,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, /* ************************************************************************* */ Class Class::expandTemplate(const TemplateSubstitution& ts) const { Class inst = *this; - inst.methods = expandMethodTemplate(methods, ts); + inst.methods_ = expandMethodTemplate(methods_, ts); inst.static_methods = expandMethodTemplate(static_methods, ts); inst.constructor = constructor.expandTemplate(ts); - inst.deconstructor.name = inst.name; + inst.deconstructor.name = inst.name(); return inst; } @@ -257,10 +302,10 @@ vector Class::expandTemplate(Str templateArg, vector result; BOOST_FOREACH(const Qualified& instName, instantiations) { Qualified expandedClass = (Qualified) (*this); - expandedClass.name += instName.name; + expandedClass.expand(instName.name()); const TemplateSubstitution ts(templateArg, instName, expandedClass); Class inst = expandTemplate(ts); - inst.name = expandedClass.name; + inst.name_ = expandedClass.name(); inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + ">"; @@ -272,50 +317,50 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const vector& templateArgValues) { + const Template& tmplate) { // Check if templated - if (!templateArgName.empty() && templateArgValues.size() > 0) { + if (tmplate.valid()) { // Create method to expand // For all values of the template argument, create a new method - BOOST_FOREACH(const Qualified& instName, templateArgValues) { - const TemplateSubstitution ts(templateArgName, instName, this->name); + BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) { + const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); // do the same for return type ReturnValue expandedRetVal = returnValue.expandTemplate(ts); // Now stick in new overload stack with expandedMethodName key // but note we use the same, unexpanded methodName in overload - string expandedMethodName = methodName + instName.name; - methods[expandedMethodName].addOverload(methodName, expandedArgs, + string expandedMethodName = methodName + instName.name(); + methods_[expandedMethodName].addOverload(methodName, expandedArgs, expandedRetVal, is_const, instName, verbose); } } else // just add overload - methods[methodName].addOverload(methodName, argumentList, returnValue, - is_const, Qualified(), verbose); + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); } /* ************************************************************************* */ void Class::erase_serialization() { - Methods::iterator it = methods.find("serializable"); - if (it != methods.end()) { + Methods::iterator it = methods_.find("serializable"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; #else // cout << "Ignoring serializable() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } - it = methods.find("serialize"); - if (it != methods.end()) { + it = methods_.find("serialize"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; hasSerialization = true; #else // cout << "Ignoring serialize() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } } @@ -327,31 +372,31 @@ void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { // verify all of the function arguments //TODO:verifyArguments(validTypes, constructor.args_list); verifyArguments(validTypes, static_methods); - verifyArguments(validTypes, methods); + verifyArguments(validTypes, methods_); // verify function return types verifyReturnTypes(validTypes, static_methods); - verifyReturnTypes(validTypes, methods); + verifyReturnTypes(validTypes, methods_); // verify parents - if (!qualifiedParent.empty() - && find(validTypes.begin(), validTypes.end(), - qualifiedParent.qualifiedName("::")) == validTypes.end()) - throw DependencyMissing(qualifiedParent.qualifiedName("::"), - qualifiedName("::")); + boost::optional parent = qualifiedParent(); + if (parent + && find(validTypes.begin(), validTypes.end(), *parent) + == validTypes.end()) + throw DependencyMissing(*parent, qualifiedName("::")); } /* ************************************************************************* */ void Class::appendInheritedMethods(const Class& cls, const vector& classes) { - if (!cls.qualifiedParent.empty()) { + if (cls.parentClass) { // Find parent BOOST_FOREACH(const Class& parent, classes) { // We found a parent class for our parent, TODO improve ! - if (parent.name == cls.qualifiedParent.name) { - methods.insert(parent.methods.begin(), parent.methods.end()); + if (parent.name() == cls.parentClass->name()) { + methods_.insert(parent.methods_.begin(), parent.methods_.end()); appendInheritedMethods(parent, classes); } } @@ -361,11 +406,11 @@ void Class::appendInheritedMethods(const Class& cls, /* ************************************************************************* */ string Class::getTypedef() const { string result; - BOOST_FOREACH(Str namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces()) { result += ("namespace " + namesp + " { "); } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i < namespaces.size(); ++i) { + result += ("typedef " + typedefName + " " + name() + ";"); + for (size_t i = 0; i < namespaces().size(); ++i) { result += " }"; } return result; @@ -373,15 +418,15 @@ string Class::getTypedef() const { /* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name() << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; constructor.comment_fragment(proxyFile); - if (!methods.empty()) + if (!methods_.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) + BOOST_FOREACH(const Methods::value_type& name_m, methods_) name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) @@ -393,7 +438,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; proxyFile.oss << "%string_deserialize(string serialized) : returns " - << this->name << "\n"; + << name() << "\n"; } proxyFile.oss << "%\n"; @@ -586,12 +631,12 @@ string Class::getSerializationExport() const { /* ************************************************************************* */ void Class::python_wrapper(FileWriter& wrapperFile) const { - wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n"; - constructor.python_wrapper(wrapperFile, name); + wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n"; + constructor.python_wrapper(wrapperFile, name()); BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); - BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); + m.python_wrapper(wrapperFile, name()); + BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values) + m.python_wrapper(wrapperFile, name()); wrapperFile.oss << ";\n\n"; } diff --git a/wrap/Class.h b/wrap/Class.h index 34323b797..f4c687eca 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,14 +19,29 @@ #pragma once +#include "spirit.h" +#include "Template.h" #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + #include #include +#include #include #include @@ -36,13 +51,20 @@ namespace wrap { /// Class has name, constructors, methods class Class: public Qualified { +public: + typedef const std::string& Str; typedef std::map Methods; - Methods methods; ///< Class methods + typedef std::map StaticMethods; + +private: + + boost::optional parentClass; ///< The *single* parent + Methods methods_; ///< Class methods + Method& mutableMethod(Str key); public: - typedef const std::string& Str; - typedef std::map StaticMethods; + StaticMethods static_methods; ///< Static methods // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments @@ -50,26 +72,28 @@ public: bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports bool hasSerialization; ///< Whether we should create the serialization functions - Qualified qualifiedParent; ///< The *single* parent - StaticMethods static_methods; ///< Static methods Constructor constructor; ///< Class constructors Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object bool verbose_; ///< verbose flag /// Constructor creates an empty class Class(bool verbose = true) : - isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor( - verbose), verbose_(verbose) { + parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization( + false), deconstructor(verbose), verbose_(verbose) { } + void assignParent(const Qualified& parent); + + boost::optional qualifiedParent() const; + size_t nrMethods() const { - return methods.size(); - } - Method& method(Str name) { - return methods.at(name); + return methods_.size(); } + + const Method& method(Str key) const; + bool exists(Str name) const { - return methods.find(name) != methods.end(); + return methods_.find(name) != methods_.end(); } // And finally MATLAB code is emitted, methods below called by Module::matlab_code @@ -85,7 +109,7 @@ public: /// Add potentially overloaded, potentially templated method void addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const std::vector& templateArgValues); + const Template& tmplate); /// Post-process classes for serialization markers void erase_serialization(); // non-const ! @@ -115,11 +139,11 @@ public: void python_wrapper(FileWriter& wrapperFile) const; friend std::ostream& operator<<(std::ostream& os, const Class& cls) { - os << "class " << cls.name << "{\n"; + os << "class " << cls.name() << "{\n"; os << cls.constructor << ";\n"; BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values) os << m << ";\n"; - BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values) + BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values) os << m << ";\n"; os << "};" << std::endl; return os; @@ -134,5 +158,122 @@ private: void comment_fragment(FileWriter& proxyFile) const; }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ClassGrammar: public classic::grammar { + + Class& cls_; ///< successful parse will be placed in here + Template& template_; ///< result needs to be visible outside + + /// Construct type grammar and specify where result is placed + ClassGrammar(Class& cls, Template& t) : + cls_(cls), template_(t) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + using BasicRules::name_p; + using BasicRules::className_p; + using BasicRules::comments_p; + + // NOTE: allows for pointers to all types + ArgumentList args; + ArgumentListGrammar argumentList_g; + + Constructor constructor0, constructor; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Template methodTemplate; + TemplateGrammar methodTemplate_g, classTemplate_g; + + std::string methodName; + bool isConst, T, F; + + // Parent class + Qualified possibleParent; + TypeGrammar classParent_g; + + classic::rule constructor_p, methodName_p, method_p, + staticMethodName_p, static_method_p, templateList_p, classParent_p, + functions_p, class_p; + + definition(ClassGrammar const& self) : + argumentList_g(args), returnValue_g(retVal), // + methodTemplate_g(methodTemplate), classTemplate_g(self.template_), // + T(true), F(false), classParent_g(possibleParent) { + + using namespace classic; + bool verbose = false; // TODO + + // ConstructorGrammar + constructor_p = (className_p >> argumentList_g >> ';' >> !comments_p) // + [bl::bind(&Constructor::push_back, bl::var(constructor), + bl::var(args))] // + [clear_a(args)]; + + // MethodGrammar + methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // gtsam::Values retract(const gtsam::VectorValues& delta) const; + method_p = !methodTemplate_g + >> (returnValue_g >> methodName_p[assign_a(methodName)] + >> argumentList_g >> !str_p("const")[assign_a(isConst, T)] >> ';' + >> *comments_p) // + [bl::bind(&Class::addMethod, bl::var(self.cls_), verbose, + bl::var(isConst), bl::var(methodName), bl::var(args), + bl::var(retVal), bl::var(methodTemplate))] // + [assign_a(retVal, retVal0)][clear_a(args)] // + [clear_a(methodTemplate)][assign_a(isConst, F)]; + + // StaticMethodGrammar + staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + static_method_p = (str_p("static") >> returnValue_g + >> staticMethodName_p[assign_a(methodName)] >> argumentList_g >> ';' + >> *comments_p) // + [bl::bind(&StaticMethod::addOverload, + bl::var(self.cls_.static_methods)[bl::var(methodName)], + bl::var(methodName), bl::var(args), bl::var(retVal), boost::none, + verbose)] // + [assign_a(retVal, retVal0)][clear_a(args)]; + + // template + templateList_p = (str_p("template") >> '<' + >> name_p[push_back_a(self.cls_.templateArgs)] + >> *(',' >> name_p[push_back_a(self.cls_.templateArgs)]) >> '>'); + + // parse a full class + classParent_p = (':' >> classParent_g >> '{') // + [bl::bind(&Class::assignParent, bl::var(self.cls_), + bl::var(possibleParent))][clear_a(possibleParent)]; + + functions_p = constructor_p | method_p | static_method_p; + + // parse a full class + class_p = (!(classTemplate_g[push_back_a(self.cls_.templateArgs, + self.template_.argName())] | templateList_p) + >> !(str_p("virtual")[assign_a(self.cls_.isVirtual, T)]) + >> str_p("class") >> className_p[assign_a(self.cls_.name_)] + >> (classParent_p | '{') >> // + *(functions_p | comments_p) >> str_p("};")) // + [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), + bl::var(self.cls_.name_), boost::none, verbose)][assign_a( + self.cls_.constructor, constructor)] // + [assign_a(self.cls_.deconstructor.name, self.cls_.name_)] // + [assign_a(constructor, constructor0)]; + } + + classic::rule const& start() const { + return class_p; + } + + }; +}; +// ClassGrammar + +}// \namespace wrap diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 782c0ca80..35cc0fa53 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -69,7 +69,7 @@ void Constructor::proxy_fragment(FileWriter& file, /* ************************************************************************* */ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const { const string wrapFunctionName = matlabUniqueName + "_constructor_" @@ -100,9 +100,9 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, << endl; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!cppBaseClassName.empty()) { + if (cppBaseClassName) { file.oss << "\n"; - file.oss << " typedef boost::shared_ptr<" << cppBaseClassName + file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName << "> SharedBase;\n"; file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index a026bf423..1e061d17c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -19,7 +19,7 @@ #pragma once #include "OverloadedFunction.h" - +#include #include #include @@ -68,7 +68,7 @@ struct Constructor: public OverloadedFunction { /// cpp wrapper std::string wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const; /// constructor function diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 5506dd778..5ec022ca4 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -27,6 +27,7 @@ namespace wrap { std::string name; bool isVirtual; ForwardDeclaration() : isVirtual(false) {} + ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {} }; -} \ No newline at end of file +} diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index ac22ec3a8..30e27764b 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -109,8 +109,8 @@ class FullyOverloadedFunction: public Function, public SignatureOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false) { + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false) { bool first = initializeOrCheck(name, instName, verbose); SignatureOverloads::push_back(args, retVal); return first; diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 6faa70fb9..7e4ded040 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -29,12 +29,11 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -bool Function::initializeOrCheck(const std::string& name, - const Qualified& instName, bool verbose) { +bool Function::initializeOrCheck(const string& name, + boost::optional instName, bool verbose) { if (name.empty()) - throw std::runtime_error( - "Function::initializeOrCheck called with empty name"); + throw runtime_error("Function::initializeOrCheck called with empty name"); // Check if this overload is give to the correct method if (name_.empty()) { @@ -43,15 +42,38 @@ bool Function::initializeOrCheck(const std::string& name, verbose_ = verbose; return true; } else { - if (name_ != name || templateArgValue_ != instName || verbose_ != verbose) - throw std::runtime_error( - "Function::initializeOrCheck called with different arguments: with name " - + name + " instead of expected " + name_ - + ", or with template argument " + instName.qualifiedName(":") - + " instead of expected " + templateArgValue_.qualifiedName(":")); + if (name_ != name || verbose_ != verbose + || ((bool) templateArgValue_ != (bool) instName) + || ((bool) templateArgValue_ && (bool) instName + && !(*templateArgValue_ == *instName))) + throw runtime_error( + "Function::initializeOrCheck called with different arguments"); return false; } } /* ************************************************************************* */ +void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const string& wrapperName, int id) const { + returnVal.emit_matlab(proxyFile); + proxyFile.oss << wrapperName << "(" << id; + if (!isStatic()) + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; +} + +/* ************************************************************************* */ +void Function::emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const string& wrapperName, int id) const { + + // Check all arguments + args.proxy_check(proxyFile); + + // output call to C++ wrapper + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id); +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h index 49a26bd8d..ad57a28c8 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -19,6 +19,7 @@ #pragma once #include "Argument.h" +#include namespace wrap { @@ -28,7 +29,7 @@ class Function { protected: std::string name_; ///< name of method - Qualified templateArgValue_; ///< value of template argument if applicable + boost::optional templateArgValue_; ///< value of template argument if applicable bool verbose_; public: @@ -37,19 +38,35 @@ public: * @brief first time, fill in instance variables, otherwise check if same * @return true if first time, false thereafter */ - bool initializeOrCheck(const std::string& name, const Qualified& instName = - Qualified(), bool verbose = false); + bool initializeOrCheck(const std::string& name, + boost::optional instName = boost::none, bool verbose = + false); std::string name() const { return name_; } - std::string matlabName() const { - if (templateArgValue_.empty()) - return name_; - else - return name_ + templateArgValue_.name; + /// Only Methods are non-static + virtual bool isStatic() const { + return true; } + + std::string matlabName() const { + if (templateArgValue_) + return name_ + templateArgValue_->name(); + else + return name_; + } + + /// Emit function call to MATLAB (no argument checking) + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const std::string& wrapperName, int id) const; + + /// Emit checking arguments and function call to MATLAB + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const std::string& wrapperName, int id) const; + }; } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index e843481a1..013ef6d28 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -18,9 +18,9 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const Qualified& instName, bool verbose) { - string name(overload.name); - FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose); + boost::optional instName, bool verbose) { + FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName, + verbose); overloads.push_back(overload); } @@ -37,7 +37,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath, for (size_t i = 0; i < overloads.size(); ++i) { Qualified overload = overloads.at(i); // use concatenated namespaces as key - string str_ns = qualifiedName("", overload.namespaces); + string str_ns = qualifiedName("", overload.namespaces()); const ReturnValue& ret = returnValue(i); const ArgumentList& args = argumentList(i); grouped_functions[str_ns].addOverload(overload, args, ret); @@ -59,7 +59,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // create the folder for the namespace const Qualified& overload1 = overloads.front(); - createNamespaceStructure(overload1.namespaces, toolboxPath); + createNamespaceStructure(overload1.namespaces(), toolboxPath); // open destination mfunctionFileName string mfunctionFileName = overload1.matlabName(toolboxPath); @@ -80,7 +80,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // Output proxy matlab code mfunctionFile.oss << " " << (i == 0 ? "" : "else"); - args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" + emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id); // Output C++ wrapper code @@ -104,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1.name != "void") + if (returnVal.type1.name() != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6a49fe5ce..b2a582654 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -11,6 +11,18 @@ #include "FullyOverloadedFunction.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + namespace wrap { struct GlobalFunction: public FullyOverloadedFunction { @@ -19,8 +31,8 @@ struct GlobalFunction: public FullyOverloadedFunction { // adds an overloaded version of this function, void addOverload(const Qualified& overload, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false); + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false); void verifyArguments(const std::vector& validArgs) const { SignatureOverloads::verifyArguments(validArgs, name_); @@ -47,5 +59,67 @@ private: }; -} // \namespace wrap +typedef std::map GlobalFunctions; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct GlobalFunctionGrammar: public classic::grammar { + + GlobalFunctions& global_functions_; ///< successful parse will be placed in here + std::vector& namespaces_; + + /// Construct type grammar and specify where result is placed + GlobalFunctionGrammar(GlobalFunctions& global_functions, + std::vector& namespaces) : + global_functions_(global_functions), namespaces_(namespaces) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + +// using BasicRules::name_p; +// using BasicRules::className_p; + using BasicRules::comments_p; + + ArgumentList args; + ArgumentListGrammar argumentList_g; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Qualified globalFunction; + + classic::rule globalFunctionName_p, global_function_p; + + definition(GlobalFunctionGrammar const& self) : + argumentList_g(args), returnValue_g(retVal) { + + using namespace classic; + bool verbose = false; // TODO + + globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // parse a global function + global_function_p = (returnValue_g + >> globalFunctionName_p[assign_a(globalFunction.name_)] + >> argumentList_g >> ';' >> *comments_p) // + [assign_a(globalFunction.namespaces_, self.namespaces_)][bl::bind( + &GlobalFunction::addOverload, + bl::var(self.global_functions_)[bl::var(globalFunction.name_)], + bl::var(globalFunction), bl::var(args), bl::var(retVal), + boost::none, verbose)] // + [assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)]; + + } + + classic::rule const& start() const { + return global_function_p; + } + + }; +}; +// GlobalFunctionGrammar + +}// \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 49d90378d..2ad6e8259 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -30,9 +30,9 @@ using namespace wrap; /* ************************************************************************* */ bool Method::addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName, - bool verbose) { - bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose); + const ReturnValue& retVal, bool is_const, + boost::optional instName, bool verbose) { + bool first = MethodBase::addOverload(name, args, retVal, instName, verbose); if (first) is_const_ = is_const; else if (is_const && !is_const_) @@ -52,14 +52,12 @@ void Method::proxy_header(FileWriter& proxyFile) const { /* ************************************************************************* */ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," - << args.size() << ");\n"; + wrapperFile.oss << " checkArguments(\"" << matlabName() + << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); @@ -72,8 +70,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(obj->return_field(t)); string expanded = "obj->" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } diff --git a/wrap/Method.h b/wrap/Method.h index db9e6bb9f..33ff7072e 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,12 +18,12 @@ #pragma once -#include "StaticMethod.h" +#include "MethodBase.h" namespace wrap { /// Method class -class Method: public StaticMethod { +class Method: public MethodBase { bool is_const_; @@ -32,8 +32,9 @@ public: typedef const std::string& Str; bool addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName = - Qualified(), bool verbose = false); + const ReturnValue& retVal, bool is_const, + boost::optional instName = boost::none, bool verbose = + false); virtual bool isStatic() const { return false; @@ -55,9 +56,7 @@ private: void proxy_header(FileWriter& proxyFile) const; virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp new file mode 100644 index 000000000..9b5f7135f --- /dev/null +++ b/wrap/MethodBase.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file MethodBase.ccp + * @author Frank Dellaert + * @author Andrew Melim + * @author Richard Roberts + **/ + +#include "Method.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { + + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); + + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); + + // Emit URL to Doxygen page + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { + // Output proxy matlab code + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; + const int id = (int) functionNames.size(); + emit_call(proxyFile, returnValue(0), wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t i = 0; i < nrOverloads(); ++i) { + + // Output proxy matlab code + proxyFile.oss << " " << (i == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + emit_conditional_call(proxyFile, returnValue(i), argumentList(i), + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, i, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name_ << "');" << endl; + proxyFile.oss << " end\n"; + } + + proxyFile.oss << " end\n"; +} + +/* ************************************************************************* */ +string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { + + // generate code + + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + + boost::lexical_cast(id); + + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); + + // call + wrapperFile.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + wrapperFile.oss << "{\n"; + + returnVal.wrapTypeUnwrap(wrapperFile); + + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; + + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args); + + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name() != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); + else + wrapperFile.oss << " " + expanded + ";\n"; + + // finish + wrapperFile.oss << "}\n"; + + return wrapFunctionName; +} + +/* ************************************************************************* */ +void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" + << name_ << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h new file mode 100644 index 000000000..903b89569 --- /dev/null +++ b/wrap/MethodBase.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file MethodBase.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + * @author Richard Roberts + **/ + +#pragma once + +#include "FullyOverloadedFunction.h" + +namespace wrap { + +/// MethodBase class +struct MethodBase: public FullyOverloadedFunction { + + typedef const std::string& Str; + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile) const { + SignatureOverloads::comment_fragment(proxyFile, matlabName()); + } + + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); + } + + // MATLAB code generation + // classPath is class directory, e.g., ../matlab/@Point2 + void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; + + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + +protected: + + virtual void proxy_header(FileWriter& proxyFile) const = 0; + + std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const = 0; +}; + +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index ac0e0a579..55fd13715 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -22,22 +22,6 @@ #include "TypeAttributesTable.h" #include "utilities.h" -//#define BOOST_SPIRIT_DEBUG -#include "spirit_actors.h" - -#include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif -#include #include #include #include @@ -51,8 +35,6 @@ using namespace BOOST_SPIRIT_CLASSIC_NS; namespace bl = boost::lambda; namespace fs = boost::filesystem; -typedef rule Rule; - /* ************************************************************************* */ // We parse an interface file into a Module object. // The grammar is defined using the boost/spirit combinatorial parser. @@ -102,7 +84,6 @@ Module::Module(const string& interfacePath, void Module::parseMarkup(const std::string& data) { // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -113,211 +94,40 @@ void Module::parseMarkup(const std::string& data) { // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html // ---------------------------------------------------------------------------- - Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); - - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char"); - - Rule keywords_p = - (str_p("const") | "static" | "namespace" | "void" | basisType_p); - - Rule eigenType_p = - (str_p("Vector") | "Matrix"); + // Define Rule and instantiate basic rules + typedef rule Rule; + BasicRules basic; - //Rule for STL Containers (class names are lowercase) - Rule stlType_p = (str_p("vector") | "list"); - - Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p; - - Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; - - Argument arg0, arg; - Rule namespace_arg_p = namespace_name_p - [push_back_a(arg.type.namespaces)] >> str_p("::"); - - Rule argEigenType_p = - eigenType_p[assign_a(arg.type.name)]; - - Rule eigenRef_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - eigenType_p [assign_a(arg.type.name)] >> - ch_p('&') [assign_a(arg.is_ref,true)]; - - Rule classArg_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - *namespace_arg_p >> - className_p[assign_a(arg.type.name)] >> - !ch_p('&')[assign_a(arg.is_ref,true)]; - - Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; + vector namespaces; // current namespace tag - // TODO, do we really need cls here? Non-local + // parse a full class Class cls0(verbose),cls(verbose); - Rule classParent_p = - *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >> - className_p[assign_a(cls.qualifiedParent.name)]; - - // parse "gtsam::Pose2" and add to templateArgValues - Qualified templateArgValue; - vector templateArgValues; - Rule templateArgValue_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(templateArgValues, templateArgValue)] - [clear_a(templateArgValue)]; - - // template - string templateArgName; - Rule templateArgValues_p = - (str_p("template") >> - '<' >> name_p[assign_a(templateArgName)] >> '=' >> - '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >> - '>'); - + Template classTemplate; + ClassGrammar class_g(cls,classTemplate); + Rule class_p = class_g // + [assign_a(cls.namespaces_, namespaces)] + [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), + bl::var(classTemplate.argValues()))] + [clear_a(classTemplate)] // + [assign_a(cls,cls0)]; + // parse "gtsam::Pose2" and add to singleInstantiation.typeList - TemplateInstantiationTypedef singleInstantiation; - Rule templateSingleInstantiationArg_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(singleInstantiation.typeList, templateArgValue)] - [clear_a(templateArgValue)]; + TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; + TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); // typedef gtsam::RangeFactor RangeFactorPosePoint2; - TemplateInstantiationTypedef singleInstantiation0; + TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = - (str_p("typedef") >> - *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >> - className_p[assign_a(singleInstantiation.class_.name)] >> - '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> - '>' >> - className_p[assign_a(singleInstantiation.name)] >> + (str_p("typedef") >> instantiationClass_g >> + typelist_g >> + basic.className_p[assign_a(singleInstantiation.name_)] >> ';') - [assign_a(singleInstantiation.namespaces, namespaces)] + [assign_a(singleInstantiation.namespaces_, namespaces)] [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; - // template - Rule templateList_p = - (str_p("template") >> - '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> - '>'); - - // NOTE: allows for pointers to all types - ArgumentList args; - Rule argument_p = - ((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p) - >> !ch_p('*')[assign_a(arg.is_ptr,true)] - >> name_p[assign_a(arg.name)]) - [push_back_a(args, arg)] - [assign_a(arg,arg0)]; - - Rule argumentList_p = !argument_p >> * (',' >> argument_p); - - // parse class constructor - Constructor constructor0(verbose), constructor(verbose); - Rule constructor_p = - (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) - [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))] - [clear_a(args)]; - - vector namespaces_return; /// namespace for current return type - Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); - - // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish - static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN; - static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS; - static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS; - static const ReturnType::return_category RETURN_VOID = ReturnType::VOID; - - ReturnType retType0, retType; - Rule returnType_p = - (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >> - !ch_p('*')[assign_a(retType.isPtr,true)]) | - (eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]); - - ReturnValue retVal0, retVal; - Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)]; - Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)]; - - Rule pair_p = - (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(retVal.isPair,true)]; - - Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - - Rule returnValue_p = void_p | pair_p | returnType1_p; - - Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - // gtsam::Values retract(const gtsam::VectorValues& delta) const; - string methodName; - bool isConst, isConst0 = false; - Rule method_p = - !templateArgValues_p >> - (returnValue_p >> methodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) - [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst), - bl::var(methodName), bl::var(args), bl::var(retVal), - bl::var(templateArgName), bl::var(templateArgValues))] - [assign_a(retVal,retVal0)] - [clear_a(args)] - [clear_a(templateArgValues)] - [assign_a(isConst,isConst0)]; - - Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - Rule static_method_p = - (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [bl::bind(&StaticMethod::addOverload, - bl::var(cls.static_methods)[bl::var(methodName)], - bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(args)]; - - Rule functions_p = constructor_p | method_p | static_method_p; - - // parse a full class - vector templateInstantiations; - Rule class_p = - eps_p[assign_a(cls,cls0)] - >> (!(templateArgValues_p - [push_back_a(cls.templateArgs, templateArgName)] - [assign_a(templateInstantiations,templateArgValues)] - [clear_a(templateArgValues)] - | templateList_p) - >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) - >> str_p("class") - >> className_p[assign_a(cls.name)] - >> ((':' >> classParent_p >> '{') | '{') - >> *(functions_p | comments_p) - >> str_p("};")) - [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), - bl::var(cls.name), Qualified(), verbose)] - [assign_a(cls.constructor, constructor)] - [assign_a(cls.namespaces, namespaces)] - [assign_a(cls.deconstructor.name,cls.name)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(templateInstantiations))] - [clear_a(templateInstantiations)] - [assign_a(constructor, constructor0)] - [assign_a(cls,cls0)]; - - // parse a global function - Qualified globalFunction; - Rule global_function_p = - (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [assign_a(globalFunction.namespaces,namespaces)] - [bl::bind(&GlobalFunction::addOverload, - bl::var(global_functions)[bl::var(globalFunction.name)], - bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(globalFunction)] - [clear_a(args)]; + // Create grammar for global functions + GlobalFunctionGrammar global_function_g(global_functions,namespaces); Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); @@ -328,9 +138,9 @@ void Module::parseMarkup(const std::string& data) { Rule namespace_def_p = (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] + >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -343,47 +153,23 @@ void Module::parseMarkup(const std::string& data) { Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") - >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)] + >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] [assign_a(fwDec, fwDec0)]; - Rule module_content_p = comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_p | namespace_def_p; + Rule module_content_p = basic.comments_p | include_p | class_p + | templateSingleInstantiation_p | forward_declaration_p + | global_function_g | namespace_def_p; Rule module_p = *module_content_p >> !end_p; - //---------------------------------------------------------------------------- - // for debugging, define BOOST_SPIRIT_DEBUG -# ifdef BOOST_SPIRIT_DEBUG - BOOST_SPIRIT_DEBUG_NODE(className_p); - BOOST_SPIRIT_DEBUG_NODE(classPtr_p); - BOOST_SPIRIT_DEBUG_NODE(classRef_p); - BOOST_SPIRIT_DEBUG_NODE(basisType_p); - BOOST_SPIRIT_DEBUG_NODE(name_p); - BOOST_SPIRIT_DEBUG_NODE(argument_p); - BOOST_SPIRIT_DEBUG_NODE(argumentList_p); - BOOST_SPIRIT_DEBUG_NODE(constructor_p); - BOOST_SPIRIT_DEBUG_NODE(returnType1_p); - BOOST_SPIRIT_DEBUG_NODE(returnType2_p); - BOOST_SPIRIT_DEBUG_NODE(pair_p); - BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnValue_p); - BOOST_SPIRIT_DEBUG_NODE(methodName_p); - BOOST_SPIRIT_DEBUG_NODE(method_p); - BOOST_SPIRIT_DEBUG_NODE(class_p); - BOOST_SPIRIT_DEBUG_NODE(namespace_def_p); - BOOST_SPIRIT_DEBUG_NODE(module_p); -# endif - //---------------------------------------------------------------------------- - // and parse contents parse_info info = parse(data.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); - cout << "Stopped near:\n" - "class '" << cls.name << "'\n" - "method '" << methodName << "'\n" - "argument '" << arg.name << "'" << endl; + cout << "Stopped in:\n" + "class '" << cls.name_ << "'" << endl; throw ParseFailed((int)info.length); } @@ -416,6 +202,11 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); + // add Eigen types as template arguments are also checked ? + vector eigen; + eigen.push_back(ForwardDeclaration("Vector")); + eigen.push_back(ForwardDeclaration("Matrix")); + typeAttributes.addForwardDeclarations(eigen); typeAttributes.checkValidity(expandedClasses); } @@ -546,7 +337,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { vector validTypes; BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { - validTypes.push_back(fwDec.name); + validTypes.push_back(fwDec.name); } validTypes.push_back("void"); validTypes.push_back("string"); diff --git a/wrap/Module.h b/wrap/Module.h index a4659dc3a..e0c1b3f31 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - #include "Class.h" #include "GlobalFunction.h" #include "TemplateInstantiationTypedef.h" #include "ForwardDeclaration.h" +#include +#include +#include + namespace wrap { /** @@ -34,9 +34,6 @@ namespace wrap { */ struct Module { - typedef std::map GlobalFunctions; - typedef std::map Methods; - // Filled during parsing: std::string name; ///< module name bool verbose; ///< verbose flag diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 47c418748..7718bc139 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -87,7 +87,8 @@ class OverloadedFunction: public Function, public ArgumentOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const Qualified& instName = Qualified(), bool verbose = false) { + boost::optional instName = boost::none, bool verbose = + false) { bool first = initializeOrCheck(name, instName, verbose); ArgumentOverloads::push_back(args); return first; diff --git a/wrap/Qualified.h b/wrap/Qualified.h index b23e9020d..bcc4c0829 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,43 +27,122 @@ namespace wrap { /** * Class to encapuslate a qualified name, i.e., with (nested) namespaces */ -struct Qualified { +class Qualified { - std::vector namespaces; ///< Stack of namespaces - std::string name; ///< type name +//protected: +public: - Qualified(const std::string& name_ = "") : - name(name_) { + std::vector namespaces_; ///< Stack of namespaces + std::string name_; ///< type name + + friend struct TypeGrammar; + friend class TemplateSubstitution; + +public: + + /// the different categories + typedef enum { + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 + } Category; + Category category; + + Qualified() : + category(VOID) { + } + + Qualified(const std::string& n, Category c = CLASS) : + name_(n), category(c) { + } + + Qualified(const std::string& ns1, const std::string& ns2, + const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + namespaces_.push_back(ns2); + } + + Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + } + + Qualified(std::vector ns, const std::string& name) : + namespaces_(ns), name_(name), category(CLASS) { + } + + std::string name() const { + return name_; + } + + std::vector namespaces() const { + return namespaces_; + } + + // Qualified is 'abused' as template argument name as well + // this function checks whether *this matches with templateArg + bool match(const std::string& templateArg) const { + return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS); + } + + void rename(const Qualified& q) { + namespaces_ = q.namespaces_; + name_ = q.name_; + category = q.category; + } + + void expand(const std::string& expansion) { + name_ += expansion; + } + + bool operator==(const Qualified& other) const { + return namespaces_ == other.namespaces_ && name_ == other.name_ + && category == other.category; } bool empty() const { - return namespaces.empty() && name.empty(); + return namespaces_.empty() && name_.empty(); } - void clear() { - namespaces.clear(); - name.clear(); + virtual void clear() { + namespaces_.clear(); + name_.clear(); + category = VOID; } - bool operator!=(const Qualified& other) const { - return other.name != name || other.namespaces != namespaces; +public: + + static Qualified MakeClass(std::vector namespaces, + const std::string& name) { + return Qualified(namespaces, name); + } + + static Qualified MakeEigen(const std::string& name) { + return Qualified(name, EIGEN); + } + + static Qualified MakeBasis(const std::string& name) { + return Qualified(name, BASIS); + } + + static Qualified MakeVoid() { + return Qualified("void", VOID); } /// Return a qualified string using given delimiter std::string qualifiedName(const std::string& delimiter = "") const { std::string result; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += (namespaces[i] + delimiter); - result += name; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += (namespaces_[i] + delimiter); + result += name_; return result; } /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m" std::string matlabName(const std::string& toolboxPath) const { std::string result = toolboxPath; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += ("/+" + namespaces[i]); - result += "/" + name + ".m"; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += ("/+" + namespaces_[i]); + result += "/" + name_ + ".m"; return result; } @@ -73,5 +153,109 @@ struct Qualified { }; +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TypeGrammar: classic::grammar { + + wrap::Qualified& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeGrammar(wrap::Qualified& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p; + + definition(TypeGrammar const& self) { + + using namespace wrap; + using namespace classic; + typedef BasicRules Basic; + + // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish + static const Qualified::Category EIGEN = Qualified::EIGEN; + static const Qualified::Category BASIS = Qualified::BASIS; + static const Qualified::Category CLASS = Qualified::CLASS; + static const Qualified::Category VOID = Qualified::VOID; + + void_p = str_p("void") // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, VOID)]; + + basisType_p = Basic::basisType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, BASIS)]; + + eigenType_p = Basic::eigenType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, EIGEN)]; + + namespace_del_p = Basic::namespace_p // + [push_back_a(self.result_.namespaces_)] >> str_p("::"); + + class_p = *namespace_del_p >> Basic::className_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, CLASS)]; + + type_p = void_p | basisType_p | class_p | eigenType_p; + } + + Rule const& start() const { + return type_p; + } + + }; +}; +// type_grammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +template +struct TypeListGrammar: public classic::grammar > { + + typedef std::vector TypeList; + TypeList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeListGrammar(TypeList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + wrap::Qualified type; ///< temporary for use during parsing + TypeGrammar type_g; ///< Individual Type grammars + + classic::rule type_p, typeList_p; + + definition(TypeListGrammar const& self) : + type_g(type) { + using namespace classic; + + type_p = type_g[push_back_a(self.result_, type)][clear_a(type)]; + + typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE; + } + + classic::rule const& start() const { + return typeList_p; + } + + }; +}; +// TypeListGrammar + +/* ************************************************************************* */ +// Needed for other parsers in Argument.h and ReturnType.h +static const bool T = true; + } // \namespace wrap diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index a317a5420..41fd51680 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -14,7 +14,7 @@ using namespace wrap; /* ************************************************************************* */ string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); } /* ************************************************************************* */ @@ -24,37 +24,46 @@ void ReturnType::wrap_result(const string& out, const string& result, string cppType = qualifiedName("::"), matlabType = qualifiedName("."); if (category == CLASS) { + + // Handle Classes string objCopy, ptrType; - ptrType = "Shared" + name; - const bool isVirtual = typeAttributes.at(cppType).isVirtual; - if (isVirtual) { - if (isPtr) - objCopy = result; - else + const bool isVirtual = typeAttributes.attributes(cppType).isVirtual; + if (isPtr) + objCopy = result; // a shared pointer can always be passed as is + else { + // but if we want an actual new object, things get more complex + if (isVirtual) + // A virtual class needs to be cloned, so the whole hierarchy is returned objCopy = result + ".clone()"; - } else { - if (isPtr) - objCopy = result; else - objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + // ...but a non-virtual class can just be copied + objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; } + // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n"; + } else if (isPtr) { - wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "(" - << result << ");" << endl; + + // Handle shared pointer case for BASIS/EIGEN/VOID + wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name() + << "(" << result << ");" << endl; wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType - << "\");\n"; + << "\");\n }\n"; + } else if (matlabType != "void") + + // Handle normal case case for BASIS/EIGEN wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result << ");\n"; + } /* ************************************************************************* */ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { if (category == CLASS) wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name << ";" << endl; + << "> Shared" << name() << ";" << endl; } /* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 1829fbc81..1c67a1d9a 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -9,6 +9,7 @@ #include "FileWriter.h" #include "TypeAttributesTable.h" #include "utilities.h" +#include #pragma once @@ -17,28 +18,25 @@ namespace wrap { /** * Encapsulates return value of a method or function */ -struct ReturnType: Qualified { - - /// the different supported return value categories - typedef enum { - CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 - } return_category; +struct ReturnType: public Qualified { bool isPtr; - return_category category; + friend struct ReturnValueGrammar; + + /// Makes a void type ReturnType() : - isPtr(false), category(CLASS) { + isPtr(false) { } - ReturnType(const std::string& name) : - isPtr(false), category(CLASS) { - Qualified::name = name; + /// Constructor, no namespaces + ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : + Qualified(name, c), isPtr(ptr) { } - void rename(const Qualified& q) { - name = q.name; - namespaces = q.namespaces; + virtual void clear() { + Qualified::clear(); + isPtr = false; } /// Check if this type is in a set of valid types @@ -64,4 +62,36 @@ private: }; +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnTypeGrammar: public classic::grammar { + + wrap::ReturnType& result_; ///< successful parse will be placed in here + + TypeGrammar type_g; + + /// Construct ReturnType grammar and specify where result is placed + ReturnTypeGrammar(wrap::ReturnType& result) : + result_(result), type_g(result_) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule type_p; + + definition(ReturnTypeGrammar const& self) { + using namespace classic; + type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)]; + } + + classic::rule const& start() const { + return type_p; + } + + }; +}; +// ReturnTypeGrammar + } // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 54e585cad..596acb2c3 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -17,9 +17,9 @@ using namespace wrap; /* ************************************************************************* */ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { ReturnValue instRetVal = *this; - instRetVal.type1 = ts(type1); + instRetVal.type1 = ts.tryToSubstitite(type1); if (isPair) - instRetVal.type2 = ts(type2); + instRetVal.type2 = ts.tryToSubstitite(type2); return instRetVal; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index abfcec2b0..629684a34 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -25,6 +25,8 @@ struct ReturnValue { bool isPair; ReturnType type1, type2; + friend struct ReturnValueGrammar; + /// Constructor ReturnValue() : isPair(false) { @@ -35,6 +37,22 @@ struct ReturnValue { isPair(false), type1(type) { } + /// Constructor + ReturnValue(const ReturnType& t1, const ReturnType& t2) : + isPair(true), type1(t1), type2(t2) { + } + + virtual void clear() { + type1.clear(); + type2.clear(); + isPair = false; + } + + bool operator==(const ReturnValue& other) const { + return isPair == other.isPair && type1 == other.type1 + && type2 == other.type2; + } + /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; @@ -59,4 +77,39 @@ struct ReturnValue { }; -} // \namespace wrap +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnValueGrammar: public classic::grammar { + + wrap::ReturnValue& result_; ///< successful parse will be placed in here + ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers + + /// Construct type grammar and specify where result is placed + ReturnValueGrammar(wrap::ReturnValue& result) : + result_(result), returnType1_g(result.type1), returnType2_g(result.type2) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule pair_p, returnValue_p; + + definition(ReturnValueGrammar const& self) { + using namespace classic; + + pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ',' + >> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)]; + + returnValue_p = pair_p | self.returnType1_g; + } + + classic::rule const& start() const { + return returnValue_p; + } + + }; +}; +// ReturnValueGrammar + +}// \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 5f91a15b4..e4e7c89ae 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "Method.h" +#include "StaticMethod.h" #include "utilities.h" #include @@ -36,117 +36,9 @@ void StaticMethod::proxy_header(FileWriter& proxyFile) const { proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; } -/* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, - Str matlabUniqueName, Str wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // emit header, e.g., function varargout = templatedMethod(this, varargin) - proxy_header(proxyFile); - - // Emit comments for documentation - string up_name = boost::to_upper_copy(matlabName()); - proxyFile.oss << " % " << up_name << " usage: "; - usage_fragment(proxyFile, matlabName()); - - // Emit URL to Doxygen page - proxyFile.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; - - // Handle special case of single overload with all numeric arguments - if (nrOverloads() == 1 && argumentList(0).allScalar()) { - // Output proxy matlab code - // TODO: document why is it OK to not check arguments in this case - proxyFile.oss << " "; - const int id = (int) functionNames.size(); - argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, - isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes, templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } else { - // Check arguments for all overloads - for (size_t i = 0; i < nrOverloads(); ++i) { - - // Output proxy matlab code - proxyFile.oss << " " << (i == 0 ? "" : "else"); - const int id = (int) functionNames.size(); - argumentList(i).emit_conditional_call(proxyFile, returnValue(i), - wrapperName, id, isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, i, id, typeAttributes, - templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name_ << "');" << endl; - proxyFile.oss << " end\n"; - } - - proxyFile.oss << " end\n"; -} - -/* ************************************************************************* */ -string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argumentList(overload); - const ReturnValue& returnVal = returnValue(overload); - - // call - wrapperFile.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - wrapperFile.oss << "{\n"; - - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - - // get call - // for static methods: cppClassName::staticMethod - // for instance methods: obj->instanceMethod - string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, - args, returnVal, typeAttributes, instName); - - expanded += ("(" + args.names() + ")"); - if (returnVal.type1.name != "void") - returnVal.wrap_result(expanded, wrapperFile, typeAttributes); - else - wrapperFile.oss << " " + expanded + ";\n"; - - // finish - wrapperFile.oss << "}\n"; - - return wrapFunctionName; -} - /* ************************************************************************* */ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // NOTE: for static functions, there is no object passed wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ @@ -158,17 +50,10 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(staticMethod(t)); string expanded = cppClassName + "::" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } /* ************************************************************************* */ -void StaticMethod::python_wrapper(FileWriter& wrapperFile, - Str className) const { - wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ - << ");\n"; -} - -/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index de8e4a94e..a01eeff62 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,42 +19,15 @@ #pragma once -#include "FullyOverloadedFunction.h" +#include "MethodBase.h" namespace wrap { /// StaticMethod class -struct StaticMethod: public FullyOverloadedFunction { +struct StaticMethod: public MethodBase { typedef const std::string& Str; - virtual bool isStatic() const { - return true; - } - - // emit a list of comments, one for each overload - void comment_fragment(FileWriter& proxyFile) const { - SignatureOverloads::comment_fragment(proxyFile, matlabName()); - } - - void verifyArguments(const std::vector& validArgs) const { - SignatureOverloads::verifyArguments(validArgs, name_); - } - - void verifyReturnTypes(const std::vector& validtypes) const { - SignatureOverloads::verifyReturnTypes(validtypes, name_); - } - - // MATLAB code generation - // classPath is class directory, e.g., ../matlab/@Point2 - void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - Str cppClassName, Str matlabQualName, Str matlabUniqueName, - Str wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; - - // emit python wrapper - void python_wrapper(FileWriter& wrapperFile, Str className) const; - friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) { for (size_t i = 0; i < m.nrOverloads(); i++) os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; @@ -65,15 +38,8 @@ protected: virtual void proxy_header(FileWriter& proxyFile) const; - std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, const Qualified& instName = - Qualified()) const; ///< cpp wrapper - virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/Template.h b/wrap/Template.h new file mode 100644 index 000000000..5a64412ed --- /dev/null +++ b/wrap/Template.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Template.h + * @brief Template name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include + +namespace wrap { + +/// The template specification that goes before a method or a class +class Template { + std::string argName_; + std::vector argValues_; + friend struct TemplateGrammar; +public: + /// The only way to get values into a Template is via our friendly Grammar + Template() { + } + void clear() { + argName_.clear(); + argValues_.clear(); + } + const std::string& argName() const { + return argName_; + } + const std::vector& argValues() const { + return argValues_; + } + size_t nrValues() const { + return argValues_.size(); + } + const Qualified& operator[](size_t i) const { + return argValues_[i]; + } + bool valid() const { + return !argName_.empty() && argValues_.size() > 0; + } + +}; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TemplateGrammar: public classic::grammar { + + Template& result_; ///< successful parse will be placed in here + TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser + + /// Construct type grammar and specify where result is placed + TemplateGrammar(Template& result) : + result_(result), argValues_g(result.argValues_) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + classic::rule templateArgValues_p; + + definition(TemplateGrammar const& self) { + using classic::str_p; + using classic::assign_a; + templateArgValues_p = (str_p("template") >> '<' + >> (BasicRules::name_p)[assign_a(self.result_.argName_)] + >> '=' >> self.argValues_g >> '>'); + } + + classic::rule const& start() const { + return templateArgValues_p; + } + + }; +}; +// TemplateGrammar + +/// Cool initializer for tests +static inline boost::optional