diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1f87b5119..addde8c64 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -19,22 +19,20 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - # ubuntu-18.04-gcc-5, #TODO Enable once the Python wrapper is optimized for memory + ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, - # ubuntu-18.04-gcc-5-tbb, + ubuntu-18.04-gcc-5-tbb, ] - #TODO update wrapper to prevent OOM - # build_type: [Debug, Release] - build_type: [Release] + build_type: [Debug, Release] python_version: [3] include: - # - name: ubuntu-18.04-gcc-5 - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" + - name: ubuntu-18.04-gcc-5 + os: ubuntu-18.04 + compiler: gcc + version: "5" - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04 @@ -46,7 +44,7 @@ jobs: compiler: clang version: "9" - #NOTE temporarily added this as it is a required check. + # NOTE temporarily added this as it is a required check. - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 compiler: clang @@ -59,11 +57,11 @@ jobs: compiler: xcode version: "11.3.1" - # - name: ubuntu-18.04-gcc-5-tbb - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" - # flag: tbb + - name: ubuntu-18.04-gcc-5-tbb + os: ubuntu-18.04 + compiler: gcc + version: "5" + flag: tbb steps: - name: Checkout diff --git a/INSTALL.md b/INSTALL.md index 520bddf3c..64857774a 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -20,9 +20,10 @@ $ make install Optional dependent libraries: - If TBB is installed and detectable by CMake GTSAM will use it automatically. Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, - disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB - may be installed from the Ubuntu repositories, and for other platforms it - may be downloaded from https://www.threadingbuildingblocks.org/ + disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing + the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`. On Ubuntu, TBB may be + installed from the Ubuntu repositories, and for other platforms it may be + downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually achieved with MKL disabled. We therefore advise you to benchmark your problem diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a3a14c6c3..013947bbd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction { // The function phi should calculate f(a)*b, with derivatives in a and b. // Naturally, the derivative in b is f(a). - typedef boost::function, OptionalJacobian)> Operator; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index e8cdbd8e0..6062c7ae1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,8 +34,9 @@ #pragma once #include -#include +#include #include +#include #include #define GTSAM_PRINT(x)((x).print(#x)) @@ -119,10 +120,10 @@ namespace gtsam { * Binary predicate on shared pointers */ template - struct equals_star : public std::function&, const boost::shared_ptr&)> { + struct equals_star : public std::function&, const std::shared_ptr&)> { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} - bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + bool operator()(const std::shared_ptr& expected, const std::shared_ptr& actual) { if (!actual && !expected) return true; return actual && expected && traits::Equals(*actual,*expected, tol_); } diff --git a/gtsam/base/base.i b/gtsam/base/base.i new file mode 100644 index 000000000..c24b04088 --- /dev/null +++ b/gtsam/base/base.i @@ -0,0 +1,115 @@ +//************************************************************************* +// base +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ##### + +#include +bool isDebugVersion(); + +#include +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; + +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; + +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; + +#include +bool linear_independent(Matrix A, Matrix B, double tol); + +#include +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s = "") const; + + // Manifold + size_t dim() const; +}; + +#include +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 820fc7333..c811eb58a 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -24,7 +24,7 @@ * * These should not be used outside of tests, as they are just remappings * of the original functions. We use these to avoid needing to do - * too much boost::bind magic or writing a bunch of separate proxy functions. + * too much std::bind magic or writing a bunch of separate proxy functions. * * Don't expect all classes to work for all of these functions. */ diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index fc5531cdc..05b60033f 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -18,15 +18,7 @@ // \callgraph #pragma once -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif +#include #include #include @@ -45,13 +37,13 @@ namespace gtsam { * for a function with one relevant param and an optional derivative: * Foo bar(const Obj& a, boost::optional H1) * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) + * std::bind(bar, std::placeholders::_1, boost::none) * This syntax will fix the optional argument to boost::none, while using the first argument provided * * For member functions, such as below, with an instantiated copy instanceOfSomeClass * Foo SomeClass::bar(const Obj& a) * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1) * * For additional details, see the documentation: * http://www.boost.org/doc/libs/release/libs/bind/bind.html @@ -76,7 +68,7 @@ struct FixedSizeMatrix { template ::dimension> typename Eigen::Matrix numericalGradient( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); BOOST_STATIC_ASSERT_MSG( @@ -116,7 +108,7 @@ typename Eigen::Matrix numericalGradient( template ::dimension> // TODO Should compute fixed-size matrix typename internal::FixedSizeMatrix::type numericalDerivative11( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), @@ -157,7 +149,8 @@ typename internal::FixedSizeMatrix::type numericalDerivative11( template typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); + return numericalDerivative11(std::bind(h, std::placeholders::_1), x, + delta); } /** @@ -170,20 +163,23 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, +typename internal::FixedSizeMatrix::type numericalDerivative21(const std::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); + return numericalDerivative11( + std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ template typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); + return numericalDerivative21( + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, + delta); } /** @@ -196,20 +192,23 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, +typename internal::FixedSizeMatrix::type numericalDerivative22(std::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta); } /** use a raw C++ function pointer */ template typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); + return numericalDerivative22( + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, + delta); } /** @@ -225,20 +224,24 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); + return numericalDerivative11( + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)), + x1, delta); } template typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, - x2, x3, delta); + return numericalDerivative31( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + x1, x2, x3, delta); } /** @@ -254,20 +257,24 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)), + x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, - x2, x3, delta); + return numericalDerivative32( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + x1, x2, x3, delta); } /** @@ -283,20 +290,24 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1), + x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, - x2, x3, delta); + return numericalDerivative33( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + x1, x2, x3, delta); } /** @@ -312,19 +323,25 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); + return numericalDerivative11( + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4)), + x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - return numericalDerivative41(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); + return numericalDerivative41( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + x1, x2, x3, x4); } /** @@ -340,19 +357,25 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4)), + x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - return numericalDerivative42(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); + return numericalDerivative42( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + x1, x2, x3, x4); } /** @@ -368,19 +391,25 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4)), + x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - return numericalDerivative43(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); + return numericalDerivative43( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + x1, x2, x3, x4); } /** @@ -396,19 +425,25 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1), + x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - return numericalDerivative44(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); + return numericalDerivative44( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + x1, x2, x3, x4); } /** @@ -425,19 +460,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); + return numericalDerivative11( + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5)), + x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - return numericalDerivative51(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); + return numericalDerivative51( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -454,19 +496,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5)), + x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - return numericalDerivative52(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); + return numericalDerivative52( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -483,19 +532,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5)), + x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - return numericalDerivative53(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); + return numericalDerivative53( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -512,19 +568,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5)), + x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - return numericalDerivative54(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); + return numericalDerivative54( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -541,19 +604,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1), + x5, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - return numericalDerivative55(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); + return numericalDerivative55( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -571,19 +641,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); + return numericalDerivative11( + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), + x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative61(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative61( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -601,19 +678,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), + x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative62(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative62( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -631,19 +715,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5), std::cref(x6)), + x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative63(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative63( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -661,19 +752,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5), std::cref(x6)), + x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative64(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative64( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -691,19 +789,26 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1, std::cref(x6)), + x5, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative65(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative65( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -721,20 +826,27 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( - boost::function h, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); + return numericalDerivative11( + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::placeholders::_1), + x6, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - return numericalDerivative66(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); + return numericalDerivative66( + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -746,22 +858,22 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (* * @return n*n Hessian matrix computed via central differencing */ template -inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, +inline typename internal::FixedSizeMatrix::type numericalHessian(std::function f, const X& x, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); typedef Eigen::Matrix::dimension, 1> VectorD; - typedef boost::function F; - typedef boost::function G; + typedef std::function F; + typedef std::function G; G ng = static_cast(numericalGradient ); - return numericalDerivative11(boost::bind(ng, f, _1, delta), x, - delta); + return numericalDerivative11( + std::bind(ng, f, std::placeholders::_1, delta), x, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f)(const X&), const X& x, double delta = 1e-5) { - return numericalHessian(boost::function(f), x, delta); + return numericalHessian(std::function(f), x, delta); } /** Helper class that computes the derivative of f w.r.t. x1, centered about @@ -769,80 +881,86 @@ inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f */ template class G_x1 { - const boost::function& f_; + const std::function& f_; X1 x1_; double delta_; public: typedef typename internal::FixedSizeMatrix::type Vector; - G_x1(const boost::function& f, const X1& x1, + G_x1(const std::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { - return numericalGradient(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_); + return numericalGradient( + std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_); } }; template inline typename internal::FixedSizeMatrix::type numericalHessian212( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; G_x1 g_x1(f, x1, delta); return numericalDerivative11( - boost::function( - boost::bind(boost::ref(g_x1), _1)), x2, delta); + std::function( + std::bind(std::ref(g_x1), std::placeholders::_1)), + x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian212(boost::function(f), + return numericalHessian212(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, boost::cref(x2))); + std::function f2( + std::bind(f, std::placeholders::_1, std::cref(x2))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian211(boost::function(f), + return numericalHessian211(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), _1)); + std::function f2( + std::bind(f, std::cref(x1), std::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian222(boost::function(f), + return numericalHessian222(std::function(f), x1, x2, delta); } @@ -852,15 +970,17 @@ inline typename internal::FixedSizeMatrix::type numericalHessian222(doubl /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian311( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3))); + std::function f2(std::bind( + f, std::placeholders::_1, std::cref(x2), std::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } @@ -868,22 +988,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian311( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian322( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3))); + std::function f2(std::bind( + f, std::cref(x1), std::placeholders::_1, std::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } @@ -891,22 +1013,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian322( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian333( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X3&, + Vector (*numGrad)(std::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1)); + std::function f2(std::bind( + f, std::cref(x1), std::cref(x2), std::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x3, delta); } @@ -914,35 +1038,41 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian333( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian312( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, _1, _2, boost::cref(x3))), + std::function( + std::bind(f, std::placeholders::_1, std::placeholders::_2, + std::cref(x3))), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian313( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, _1, boost::cref(x2), _2)), + std::function( + std::bind(f, std::placeholders::_1, std::cref(x2), + std::placeholders::_2)), x1, x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian323( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, boost::cref(x1), _1, _2)), + std::function( + std::bind(f, std::cref(x1), std::placeholders::_1, + std::placeholders::_2)), x2, x3, delta); } @@ -951,7 +1081,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian312( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -959,7 +1089,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian313( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -967,7 +1097,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian323( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold); + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task_group +#include // tbb::task, tbb::task_list #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask + class PreOrderTask : public tbb::task { public: const boost::shared_ptr& treeNode; @@ -42,30 +42,28 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; bool makeNewTasks; - // Keep track of order phase across multiple calls to the same functor - mutable bool isPostOrderPhase; + bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - tbb::task_group& tg, bool makeNewTasks = true) + bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), - tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - void operator()() const + tbb::task* execute() override { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); + return nullptr; } else { @@ -73,10 +71,14 @@ namespace gtsam { { if(!treeNode->children.empty()) { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - // If we have child tasks, start subtasks and wait for them to complete - tbb::task_group ctg; + tbb::task* firstChild = 0; + tbb::task_list childTasks; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -84,30 +86,37 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, ctg, overThreshold)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; } - ctg.wait(); - // Allocate post-order task as a continuation - isPostOrderPhase = true; - tg.run(*this); + // If we have child tasks, start subtasks and wait for them to complete + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); + return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { for(const boost::shared_ptr& child: node->children) { @@ -122,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask + class RootTask : public tbb::task { public: const ROOTS& roots; @@ -130,31 +139,38 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold, tbb::task_group& tg) : + int problemSizeThreshold) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), tg(tg) {} + problemSizeThreshold(problemSizeThreshold) {} - void operator()() const + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children + tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return nullptr + return nullptr; } }; template - void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - tbb::task_group tg; - tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); - } + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } } diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h new file mode 100644 index 000000000..8eb5617a8 --- /dev/null +++ b/gtsam/base/utilities.h @@ -0,0 +1,29 @@ +#pragma once + +namespace gtsam { +/** + * For Python __str__(). + * Redirect std cout to a string stream so we can return a string representation + * of an object when it prints to cout. + * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string + */ +struct RedirectCout { + /// constructor -- redirect stdout buffer to a stringstream buffer + RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} + + /// return the string + std::string str() const { + return ssBuffer_.str(); + } + + /// destructor -- redirect stdout buffer to its original buffer + ~RedirectCout() { + std::cout.rdbuf(coutBuffer_); + } + +private: + std::stringstream ssBuffer_; + std::streambuf* coutBuffer_; +}; + +} diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 80b226e3a..439889ebf 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -450,7 +450,7 @@ namespace gtsam { template template DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op) { + const std::map& map, std::function op) { root_ = convert(other.root_, map, op); } @@ -568,7 +568,7 @@ namespace gtsam { template typename DecisionTree::NodePtr DecisionTree::convert( const typename DecisionTree::NodePtr& f, const std::map& map, - boost::function op) { + std::function op) { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index ecf03ad5d..0ee0b8be0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,10 +20,12 @@ #pragma once #include + #include +#include #include -#include #include +#include namespace gtsam { @@ -38,8 +40,8 @@ namespace gtsam { public: /** Handy typedefs for unary and binary function types */ - typedef boost::function Unary; - typedef boost::function Binary; + typedef std::function Unary; + typedef std::function Binary; /** A label annotated with cardinality */ typedef std::pair LabelC; @@ -107,7 +109,7 @@ namespace gtsam { /** Convert to a different type */ template NodePtr convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); + L>& map, std::function op); /** Default constructor */ DecisionTree(); @@ -143,7 +145,7 @@ namespace gtsam { /** Convert from a different type */ template DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op); + const std::map& map, std::function op); /// @} /// @name Testable diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 0261ef833..be720dbca 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) { } /** I can't get this to work ! - class Mul: boost::function { + class Mul: std::function { inline double operator()(const double& a, const double& b) { return a * b; } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 71cb4abe3..96f503abc 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -196,7 +196,7 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - boost::function op = convert; + std::function op = convert; BDT f2(f1, ordering, op); // f1.print("f1"); // f2.print("f2"); diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index b9e60acee..52d475d5d 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, /* ************************************************************************* */ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // initial gues just inverts the pinhole model + // Apply inverse camera matrix to map the pixel coordinate (u, v) + // of the equidistant fisheye image to angular coordinate space (xd, yd) + // with radius theta given in radians. const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; - Point2 pi(xd, yd); + const double theta = sqrt(xd * xd + yd * yd); + + // Provide initial guess for the Gauss-Newton search. + // The angular coordinates given by (xd, yd) are mapped back to + // the focal plane of the perspective undistorted projection pi. + // See Cal3Unified.calibrate() using the same pattern for the + // undistortion of omnidirectional fisheye projection. + const double scale = (theta > 0) ? tan(theta) / theta : 1.0; + Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, // throw exception if max iterations are reached diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 0b81ff0f9..35578ffe0 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -17,7 +17,9 @@ #include #include -#include // for cout :-( + +#include +#include // for cout :-( namespace gtsam { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 318baab3d..d76e1b41a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -142,7 +142,7 @@ public: static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..5ff6b9816 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,7 +21,9 @@ #include #include #include +#include #include +#include #include #include @@ -33,6 +35,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i new file mode 100644 index 000000000..b1c34ba42 --- /dev/null +++ b/gtsam/geometry/geometry.i @@ -0,0 +1,1019 @@ +//************************************************************************* +// geometry +//************************************************************************* + +namespace gtsam { + +#include +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Point2& point, double tol) const; + + // Group + static gtsam::Point2 identity(); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +// std::vector +class Point2Vector { + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + // Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + // Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + // Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + +#include +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Operator Overloads + gtsam::StereoPoint2 operator-() const; + // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet + // supported + gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const; + + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); + + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +class Point3Pairs { + Point3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + +#include +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s = "theta") const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Operator Overloads + gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing( + const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO3& other, double tol) const; + + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; + + // Operator Overloads + gtsam::SO3 operator*(const gtsam::SO3& R) const; + + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO4& other, double tol) const; + + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; + + // Operator Overloads + gtsam::SO4 operator*(const gtsam::SO4& Q) const; + + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SOn { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SOn& other, double tol) const; + + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; + + // Operator Overloads + gtsam::SOn operator*(const gtsam::SOn& Q) const; + + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Quaternion { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; +}; + +#include +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, + const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); + + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw( + double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch( + double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll( + double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Operator Overloads + gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; + + // Manifold + // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both + // Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Vector logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& other); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Operator Overloads + gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; + + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; + + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& other); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); + Pose3(Matrix mat); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; + + // Operator Overloads + gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& pose); + gtsam::Pose3 expmap(Vector v); + Vector logmap(const gtsam::Pose3& pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, + double vz); + + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +class Pose3Pairs { + Pose3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Pose3Pair at(size_t n) const; + void push_back(const gtsam::Pose3Pair& pose_pair); +}; + +class Pose3Vector { + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& pose); +}; + +#include +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Unit3& pose, double tol) const; + + // Other functionality + Matrix basis() const; + Matrix skew() const; + gtsam::Point3 point3() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::Unit3& expected, double tol) const; +}; + +#include +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; + +#include +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); + + // Testable + void print(string s = "Cal3_S2") const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); + + // Testable + void print(string s = "") const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, + double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, + double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double k3, double k4, double tol = 1e-5); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; + +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, + double tol); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, + double height); + + // Testable + void print(string s = "CalibratedCamera") const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& point) const; + double range(const gtsam::Pose3& pose) const; + double range(const gtsam::CalibratedCamera& camera) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, + double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); + + // Testable + void print(string s = "PinholeCamera") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Similarity3 { + // Standard Constructors + Similarity3(); + Similarity3(double s); + Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); + Similarity3(const Matrix& R, const Vector& t, double s); + Similarity3(const Matrix& T); + + gtsam::Point3 transformFrom(const gtsam::Point3& p) const; + gtsam::Pose3 transformFrom(const gtsam::Pose3& T); + + static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs); + static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); + + // Standard Interface + const Matrix matrix() const; + const gtsam::Rot3& rotation(); + const gtsam::Point3& translation(); + double scale() const; +}; + +// Forward declaration of PinholeCameraCalX is defined here. +#include +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; + +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; + +#include +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; + + // Manifold + gtsam::StereoCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include + +// Templates appear not yet supported for free functions - issue raised at +// borglab/wrap#14 to add support +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); + +#include +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s = "") const; +}; + +typedef gtsam::BearingRange + BearingRange2D; + +} // namespace gtsam diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 1982b8f50..0fb0754fe 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -24,6 +24,7 @@ #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -52,8 +53,8 @@ TEST(CalibratedCamera, Create) { EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH))); // Check derivative - boost::function f = // - boost::bind(CalibratedCamera::Create, _1, boost::none); + std::function f = // + std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f, kDefaultPose); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 60cee59ee..761ef3a8c 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc..ff8c61f35 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -5,13 +5,15 @@ * @date December 17, 2013 */ -#include -#include -#include -#include #include +#include +#include +#include +#include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -39,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) { 1e-8)); Matrix expectedH1 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none, - boost::none), + std::bind(EssentialMatrix::FromRotationAndDirection, + std::placeholders::_1, trueDirection, boost::none, boost::none), trueRotation); EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); Matrix expectedH2 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none, - boost::none), - trueDirection); + std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, + std::placeholders::_1, boost::none, boost::none), + trueDirection); EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); } @@ -173,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) { Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -186,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) { Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 2461cea1a..533041a2c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -23,6 +23,7 @@ #include using namespace boost::assign; +using namespace std::placeholders; using namespace gtsam; using namespace std; using boost::none; @@ -136,8 +137,9 @@ TEST(OrientedPlane3, errorVector) { Vector2(actual[0], actual[1]))); EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); - boost::function f = - boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); @@ -148,8 +150,8 @@ TEST(OrientedPlane3, errorVector) { TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; - boost::function f = - boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); + std::function f = std::bind( + &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); { Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ad6f4e921..0679a4609 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -27,6 +27,7 @@ #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -64,8 +65,9 @@ TEST(PinholeCamera, Create) { EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); // Check derivative - boost::function f = // - boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + std::function f = // + std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f,pose,K); EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); Matrix numericalH2 = numericalDerivative22(f,pose,K); @@ -79,8 +81,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 1cf2f4a3f..acfcd9f39 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index f32fe60ed..9ca8847f8 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a655011a0..315391ac8 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -14,11 +14,14 @@ * @brief Unit tests for Point3 class */ -#include +#include #include #include -#include +#include +#include + +using namespace std::placeholders; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) @@ -98,7 +101,7 @@ TEST( Point3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = + std::function f = [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); }; { gtsam::dot(p, q, H1, H2); @@ -120,8 +123,9 @@ TEST( Point3, dot) { /* ************************************************************************* */ TEST(Point3, cross) { Matrix aH1, aH2; - boost::function f = - boost::bind(>sam::cross, _1, _2, boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); @@ -139,8 +143,9 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(>sam::cross, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); @@ -160,7 +165,7 @@ TEST (Point3, normalize) { Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(gtsam::normalize, _1, boost::none), point); + std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 594d15c91..11b8791d4 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -22,6 +22,7 @@ #include // for operator += using namespace boost::assign; +using namespace std::placeholders; #include #include @@ -213,7 +214,7 @@ TEST(Pose3, translation) { EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::translation, _1, boost::none), T); + std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -224,7 +225,7 @@ TEST(Pose3, rotation) { EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::rotation, _1, boost::none), T); + std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -1050,7 +1051,9 @@ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); EXPECT(assert_equal(T, actual)); - boost::function create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none); + std::function create = + std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index b2c781b35..910d482b0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,13 +15,12 @@ * @author Frank Dellaert **/ -#include - +#include #include #include +#include -#include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -209,7 +208,7 @@ TEST(SO3, ExpmapDerivative) { TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -220,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -275,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) { TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), thetahat); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); Matrix3 Jactual; SO3::Expmap(thetahat, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -286,7 +285,7 @@ TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } @@ -296,7 +295,7 @@ TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); Matrix3 Jactual; SO3::Logmap(R, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -306,7 +305,7 @@ TEST(SO3, JacobianLogmap) { TEST(SO3, ApplyDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); }; @@ -329,7 +328,7 @@ TEST(SO3, ApplyDexp) { TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); }; @@ -355,7 +354,7 @@ TEST(SO3, vec) { Matrix actualH; const Vector9 actual = R2.vec(actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { return Q.vec(); }; + std::function f = [](const SO3& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); CHECK(assert_equal(numericalH, actualH)); } @@ -369,7 +368,7 @@ TEST(Matrix, compose) { Matrix actualH; const Matrix3 actual = so3::compose(M, R, actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [R](const Matrix3& M) { + std::function f = [R](const Matrix3& M) { return so3::compose(M, R); }; Matrix numericalH = numericalDerivative11(f, M, 1e-2); diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index f771eea5f..5486755f7 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -166,7 +166,7 @@ TEST(SO4, vec) { Matrix actualH; const Vector16 actual = Q2.vec(actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q) { + std::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); @@ -179,7 +179,7 @@ TEST(SO4, topLeft) { Matrix actualH; const Matrix3 actual = topLeft(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return topLeft(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); @@ -192,7 +192,7 @@ TEST(SO4, stiefel) { Matrix actualH; const Matrix43 actual = stiefel(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return stiefel(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 4d0ed98b3..d9d4da34c 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); } /// Test Jacobian of Retract at origin TEST(SOn, RetractJacobian) { Matrix actualH = RetractJacobian(3); - boost::function h = [](const Vector &v) { + std::function h = [](const Vector &v) { return SOn::ChartAtOrigin::Retract(v).matrix(); }; Vector3 v; @@ -205,7 +205,7 @@ TEST(SOn, vec) { SOn Q = SOn::ChartAtOrigin::Retract(v); Matrix actualH; const Vector actual = Q.vec(actualH); - boost::function h = [](const SOn &Q) { return Q.vec(); }; + std::function h = [](const SOn &Q) { return Q.vec(); }; const Matrix H = numericalDerivative11(h, Q, 1e-5); CHECK(assert_equal(H, actualH)); } diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 8f466e21b..428422072 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -16,23 +16,22 @@ * @author Zhaoyang Lv */ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#include - -#include -#include +#include +using namespace std::placeholders; using namespace gtsam; using namespace std; using symbol_shorthand::X; @@ -242,8 +241,9 @@ TEST(Similarity3, GroupAction) { EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa))); // Test derivative - boost::function f = boost::bind( - &Similarity3::transformFrom, _1, _2, boost::none, boost::none); + // Use lambda to resolve overloaded method + std::function + f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); }; Point3 q(1, 2, 3); for (const auto& T : { T1, T2, T3, T4, T5, T6 }) { diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 9929df21a..b548b9315 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -31,13 +31,13 @@ #include -#include #include #include #include using namespace boost::assign; +using namespace std::placeholders; using namespace gtsam; using namespace std; using gtsam::symbol_shorthand::U; @@ -126,8 +126,9 @@ TEST(Unit3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Unit3::dot, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { p.dot(q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); @@ -157,13 +158,13 @@ TEST(Unit3, error) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), q); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), r); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -184,25 +185,33 @@ TEST(Unit3, error2) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -220,13 +229,13 @@ TEST(Unit3, distance) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), q); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), r); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -318,7 +327,7 @@ TEST(Unit3, basis) { Matrix62 actualH; Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); // without H, first time EXPECT(assert_equal(expected, p.basis(), 1e-6)); @@ -347,7 +356,7 @@ TEST(Unit3, basis_derivatives) { p.basis(actualH); Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -375,8 +384,8 @@ TEST(Unit3, retract) { TEST (Unit3, jacobian_retract) { Matrix22 H; Unit3 p; - boost::function f = - boost::bind(&Unit3::retract, p, _1, boost::none); + std::function f = + std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); { Vector2 v (-0.2, 0.1); p.retract(v, H); @@ -439,7 +448,7 @@ TEST (Unit3, FromPoint3) { Unit3 expected(point); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( - boost::bind(Unit3::FromPoint3, _1, boost::none), point); + std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..6f6ade6f7 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,14 +18,16 @@ #pragma once -#include #include +#include +#include +#include #include #include #include -#include -#include #include +#include +#include namespace gtsam { @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 155275702..a55581e50 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2,10 +2,12 @@ * GTSAM Wrap Module Definition * - * These are the current classes available through the matlab and python wrappers, + * These are the current classes available through the matlab and python + wrappers, * add more functions/classes as they are available. * - * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md + * Please refer to the wrapping docs: + https://github.com/borglab/wrap/blob/master/README.md */ namespace gtsam { @@ -61,8 +63,8 @@ class KeySet { // structure specific methods void insert(size_t key); void merge(const gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists void serialize() const; @@ -123,8 +125,8 @@ class FactorIndexSet { // structure specific methods void insert(size_t factorIndex); - bool erase(size_t factorIndex); // returns true if value was removed - bool count(size_t factorIndex) const; // returns true if value exists + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists }; // Actually a vector @@ -144,3495 +146,53 @@ class FactorIndices { void push_back(size_t factorIndex) const; }; -//************************************************************************* -// base -//************************************************************************* - -/** gtsam namespace functions */ - -#include -bool isDebugVersion(); - -#include -class IndexPair { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; -}; - -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; - -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists -}; - -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; - -gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; - -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - -#include -bool linear_independent(Matrix A, Matrix B, double tol); - -#include -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s = "") const; - - // Manifold - size_t dim() const; -}; - -#include -template -virtual class GenericValue : gtsam::Value { - void serializable() const; -}; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Point2& point, double tol) const; - - // Group - static gtsam::Point2 identity(); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - -#include -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; - - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - - // Operator Overloads - gtsam::StereoPoint2 operator-() const; - // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet supported - gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const; - - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; - - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); - - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -class Point3Pairs { - Point3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s = "theta") const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Operator Overloads - gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - Vector logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class SO3 { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO3& other, double tol) const; - - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3& R) const; - gtsam::SO3 compose(const gtsam::SO3& R) const; - - // Operator Overloads - gtsam::SO3 operator*(const gtsam::SO3& R) const; - - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3& R) const; - static gtsam::SO3 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SO4 { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO4& other, double tol) const; - - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4& Q) const; - gtsam::SO4 compose(const gtsam::SO4& Q) const; - - // Operator Overloads - gtsam::SO4 operator*(const gtsam::SO4& Q) const; - - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4& Q) const; - static gtsam::SO4 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SOn { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SOn& other, double tol) const; - - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn& Q) const; - gtsam::SOn compose(const gtsam::SOn& Q) const; - - // Operator Overloads - gtsam::SOn operator*(const gtsam::SOn& Q) const; - - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn& Q) const; - static gtsam::SOn Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Quaternion { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); - - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Operator Overloads - gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Vector logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& other); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Operator Overloads - gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Vector logmap(const gtsam::Pose2& p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2& v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& other); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); - Pose3(Matrix mat); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& pose) const; - gtsam::Pose3 between(const gtsam::Pose3& pose) const; - - // Operator Overloads - gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& pose) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& pose); - Vector logmap(const gtsam::Pose3& pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3& xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& point) const; - gtsam::Point3 transformTo(const gtsam::Point3& point) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -class Pose3Pairs { - Pose3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Pose3Pair at(size_t n) const; - void push_back(const gtsam::Pose3Pair& pose_pair); -}; - -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& pose); -}; - -#include -class Unit3 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Unit3& pose, double tol) const; - - // Other functionality - Matrix basis() const; - Matrix skew() const; - gtsam::Point3 point3() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::Unit3& expected, double tol) const; -}; - -#include -class EssentialMatrix { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); -}; - -#include -class Cal3_S2 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); - - // Testable - void print(string s = "Cal3_S2") const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix K() const; - Matrix inverse() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); - - // Testable - void print(string s = "") const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - Matrix K() const; - Vector k() const; - Vector vector() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); - - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); - - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; -}; - -#include -class Cal3Bundler { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Bundler& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - - // Testable - void print(string s = "CalibratedCamera") const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& point) const; - double range(const gtsam::Pose3& pose) const; - double range(const gtsam::CalibratedCamera& camera) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const CALIBRATION& K); - - // Testable - void print(string s = "PinholeCamera") const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(Vector d) const; - Vector localCoordinates(const This& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - - -#include -class Similarity3 { - // Standard Constructors - Similarity3(); - Similarity3(double s); - Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); - Similarity3(const Matrix& R, const Vector& t, double s); - Similarity3(const Matrix& T); - - gtsam::Point3 transformFrom(const gtsam::Point3& p) const; - gtsam::Pose3 transformFrom(const gtsam::Pose3& T); - - static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); - static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); - - // Standard Interface - const Matrix matrix() const; - const gtsam::Rot3& rotation(); - const gtsam::Point3& translation(); - double scale() const; -}; - - -// Forward declaration of PinholeCameraCalX is defined here. -#include -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - -template -class CameraSet { - CameraSet(); - - // structure specific methods - T at(size_t i) const; - void push_back(const T& cam); -}; - -#include -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; - - // Manifold - gtsam::StereoCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include - -// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const CameraSetCal3Bundler& cameras, - const gtsam::Point2Vector& measurements, const Point3& initialEstimate); - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s = "SymbolicFactor", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "SymbolicFactorGraph", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s = "SymbolicBayesNet", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -class SymbolicBayesTreeClique { - SymbolicBayesTreeClique(); - // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); - - bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; - void print(string s = "", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - size_t numCachedSeparatorMarginals() const; - // gtsam::sharedConditional* conditional() const; - bool isRoot() const; - size_t treeSize() const; - gtsam::SymbolicBayesTreeClique* parent() const; - -// // TODO: need wrapped versions graphs, BayesNet -// BayesNet shortcut(derived_ptr root, Eliminate function) const; -// FactorGraph marginal(derived_ptr root, Eliminate function) const; -// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// - void deleteCachedShortcuts(); -}; - -#include -class VariableIndex { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& sfg); - VariableIndex(const gtsam::GaussianFactorGraph& gfg); - VariableIndex(const gtsam::NonlinearFactorGraph& fg); - VariableIndex(const gtsam::VariableIndex& other); - - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s = "VariableIndex: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { - void print(string s = "") const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - - bool equals(gtsam::noiseModel::Base& expected, double tol); - - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; - - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - - // access to noise model - double sigma() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { - void print(string s = "") const; -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - // Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s = "VectorValues", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); - - //Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, Vector sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); - - //Testable - size_t size() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - - // From FactorGraph - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianConditional : gtsam::JacobianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s = "GaussianConditional", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianConditional& cg, double tol) const; - - // Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, - const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - - //Standard Interface - void print(string s = "GaussianDensity", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; - -#include -virtual class GaussianBayesNet { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - void saveGraph(const string& s) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -#include -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s = "Errors"); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -#include -class GaussianISAM { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); -}; - -#include -virtual class PreconditionerParameters { - PreconditionerParameters(); -}; - -virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { - DummyPreconditionerParameters(); -}; - -#include -virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { - PCGSolverParameters(); - void print(string s = ""); - void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); -}; - -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; - -#include -class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s = "") const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); -}; - -//************************************************************************* -// nonlinear -//************************************************************************* - -#include - -class Symbol { - Symbol(); - Symbol(char c, uint64_t j); - Symbol(size_t key); - - size_t key() const; - void print(const string& s = "") const; - bool equals(const gtsam::Symbol& expected, double tol) const; - - char chr() const; - uint64_t index() const; - string string() const; -}; - -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -namespace symbol_shorthand { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); -}///\namespace symbol - -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); - -#include -class LabeledSymbol { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol& key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s = "") const; -}; - -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); - -#include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s = "NonlinearFactorGraph: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor* factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - - template , - gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T& prior, - const gtsam::noiseModel::Base* noiseModel); - - // NonlinearFactorGraph - void printErrors(const gtsam::Values& values) const; - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - void saveGraph(const string& s) const; -}; - -#include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - bool equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -virtual class CustomFactor: gtsam::NoiseModelFactor { - /* - * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there. - * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`. - */ - CustomFactor(); - /* - * Example: - * ``` - * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - * - * if not H is None: - * - * H[0] = J1 # 2-d numpy array for a Jacobian block - * H[1] = J2 - * ... - * return error # 1-d numpy array - * - * cf = CustomFactor(noise_model, keys, error_func) - * ``` - */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, - const gtsam::CustomErrorFunction& errorFunction); - - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -#include -class Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2& point2); - void insert(size_t j, const gtsam::Point3& point3); - void insert(size_t j, const gtsam::Rot2& rot2); - void insert(size_t j, const gtsam::Pose2& pose2); - void insert(size_t j, const gtsam::SO3& R); - void insert(size_t j, const gtsam::SO4& Q); - void insert(size_t j, const gtsam::SOn& P); - void insert(size_t j, const gtsam::Rot3& rot3); - void insert(size_t j, const gtsam::Pose3& pose3); - void insert(size_t j, const gtsam::Unit3& unit3); - void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert(size_t j, const gtsam::NavState& nav_state); - void insert(size_t j, double c); - - void update(size_t j, const gtsam::Point2& point2); - void update(size_t j, const gtsam::Point3& point3); - void update(size_t j, const gtsam::Rot2& rot2); - void update(size_t j, const gtsam::Pose2& pose2); - void update(size_t j, const gtsam::SO3& R); - void update(size_t j, const gtsam::SO4& Q); - void update(size_t j, const gtsam::SOn& P); - void update(size_t j, const gtsam::Rot3& rot3); - void update(size_t j, const gtsam::Pose3& pose3); - void update(size_t j, const gtsam::Unit3& unit3); - void update(size_t j, const gtsam::Cal3_S2& cal3_s2); - void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void update(size_t j, const gtsam::NavState& nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); - void update(size_t j, double c); - - template , - gtsam::imuBias::ConstantBias, - gtsam::NavState, - Vector, - Matrix, - double}> - T at(size_t j); - -}; - -#include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::VectorValues& solutionvec); - - void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; -}; - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Values& linearizationPoint); - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); - - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor - -// Summarization functionality -//#include -// -//// Uses partial QR approach by default -//gtsam::GaussianFactorGraph summarize( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); -// -//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); - -//************************************************************************* -// Nonlinear optimizers -//************************************************************************* -#include -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s = "") const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - void setLinearSolverType(string solver); - - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - void setOrdering(const gtsam::Ordering& ordering); - string getOrderingType() const; - void setOrderingType(string ordering); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); -bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; - - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); - - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); - - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph& graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - - double getDeltaInitial() const; - string getVerbosityDL() const; - - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; - -#include -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph* iterate() const; -}; - -#include -virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); -}; - -#include -virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; -}; - -#include -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string s = "") const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - - //Standard Interface - Vector gradientContribution() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -class ISAM2Result { - ISAM2Result(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; -}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template , - Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; - -#include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -template }> -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); - - // enabling serialization functionality - void serialize() const; -}; - -#include -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -#include -template -virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; - -#include -template -virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingFactor BearingFactor2D; -typedef gtsam::BearingFactor BearingFactor3D; -typedef gtsam::BearingFactor BearingFactorPose2; - -#include -template -class BearingRange { - BearingRange(const BEARING& b, const RANGE& r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE& pose, const POINT& point); - static BEARING MeasureBearing(const POSE& pose, const POINT& point); - static RANGE MeasureRange(const POSE& pose, const POINT& point); - void print(string s = "") const; -}; - -typedef gtsam::BearingRange BearingRange2D; - -#include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); - - gtsam::BearingRange measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; -typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; - - -#include -template -virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, - const POSE& body_P_sensor); - - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - - -#include -template -virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; - -template -virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include - -/// Linearization mode: what factor to linearize to -enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; - -/// How to manage degeneracy -enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; - -class SmartProjectionParams { - SmartProjectionParams(); - void setLinearizationMode(gtsam::LinearizationMode linMode); - void setDegeneracyMode(gtsam::DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - -#include -template -virtual class GenericStereoFactor : gtsam::NoiseModelFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericStereoFactor GenericStereoFactor3D; - -#include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -#include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -#include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; - -#include - -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; - - double r; - double g; - double b; - - std::vector> measurements; - - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmTrack& expected, double tol) const; -}; - -class SfmData { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamera& cam); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmData& expected, double tol) const; -}; - -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model, int maxIndex); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose2] -class BetweenFactorPose2s -{ - BetweenFactorPose2s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose2s parse2DFactors(string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose3] -class BetweenFactorPose3s -{ - BetweenFactorPose3s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose3s parse3DFactors(string filename); - -pair load3D(string filename); - -pair readG2o(string filename); -pair readG2o(string filename, bool is3D); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -#include -class InitializePose3 { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -}; - -#include -template -virtual class KarcherMeanFactor : gtsam::NonlinearFactor { - KarcherMeanFactor(const gtsam::KeyVector& keys); -}; - -#include -gtsam::noiseModel::Isotropic* ConvertNoiseModel( - gtsam::noiseModel::Base* model, size_t d); - -template -virtual class FrobeniusFactor : gtsam::NoiseModelFactor { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -template -virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -#include - -virtual class ShonanFactor3 : gtsam::NoiseModelFactor { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); -}; - -#include -template -class BinaryMeasurement { - BinaryMeasurement(size_t key1, size_t key2, const T& measured, - const gtsam::noiseModel::Base* model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base* noiseModel() const; -}; - -typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; -typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; - -class BinaryMeasurementsUnit3 { - BinaryMeasurementsUnit3(); - size_t size() const; - gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement& measurement); -}; - -#include - -// TODO(frank): copy/pasta below until we have integer template paremeters in wrap! - -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveragingParameters3 { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveraging2 { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -class ShonanAveraging3 { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -#include - -class KeyPairDoubleMap { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair& keypair) const; -}; - -class MFAS { - MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::Unit3& projectionDirection); - - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; -}; - -#include -class TranslationRecovery { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 -}; - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Operator Overloads - gtsam::imuBias::ConstantBias operator-() const; - gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class NavState { - // Constructors - NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); - NavState(const gtsam::Pose3& pose, Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::NavState& expected, double tol) const; - - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; - - gtsam::NavState retract(const Vector& x) const; - Vector localCoordinates(const gtsam::NavState& g) const; -}; - -#include -virtual class PreintegratedRotationParams { - PreintegratedRotationParams(); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); - - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3& pose); - - Matrix getGyroscopeCovariance() const; - - boost::optional getOmegaCoriolis() const; - boost::optional getBodyPSensor() const; -}; - -#include -virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { - PreintegrationParams(Vector n_gravity); - - static gtsam::PreintegrationParams* MakeSharedD(double g); - static gtsam::PreintegrationParams* MakeSharedU(double g); - static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationParams& expected, double tol); - - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); - - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; -}; - -#include -class PreintegratedImuMeasurements { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias); -}; - -#include -virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { - PreintegrationCombinedParams(Vector n_gravity); - - static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); - - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); - - Matrix getBiasAccCovariance() const ; - Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; - -}; - -class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s = "Preintegrated Measurements:") const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::imuBias::ConstantBias& bias_j); -}; - -#include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - - // Testable - void print(string s = "Preintegrated Measurements: ") const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - -#include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -#include -virtual class GPSFactor : gtsam::NonlinearFactor{ - GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -virtual class GPSFactor2 : gtsam::NonlinearFactor { - GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -#include -virtual class Scenario { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; -}; - -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3& nTb0); -}; - -virtual class AcceleratingScenario : gtsam::Scenario { - AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, - Vector v0, Vector a_n, - Vector omega_b); -}; - -#include -class ScenarioRunner { - ScenarioRunner(const gtsam::Scenario& scenario, - const gtsam::PreintegrationParams* p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias& bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias& estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements& pim, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; -}; - //************************************************************************* // Utilities //************************************************************************* namespace utilities { - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - gtsam::Values allPose2s(gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - #include +gtsam::KeyList createKeyList(Vector I); +gtsam::KeyList createKeyList(string s, Vector I); +gtsam::KeyVector createKeyVector(Vector I); +gtsam::KeyVector createKeyVector(string s, Vector I); +gtsam::KeySet createKeySet(Vector I); +gtsam::KeySet createKeySet(string s, Vector I); +Matrix extractPoint2(const gtsam::Values& values); +Matrix extractPoint3(const gtsam::Values& values); +gtsam::Values allPose2s(gtsam::Values& values); +Matrix extractPose2(const gtsam::Values& values); +gtsam::Values allPose3s(gtsam::Values& values); +Matrix extractPose3(const gtsam::Values& values); +void perturbPoint2(gtsam::Values& values, double sigma, int seed); +void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, + int seed); +void perturbPoint3(gtsam::Values& values, double sigma, int seed); +void insertBackprojections(gtsam::Values& values, + const gtsam::PinholeCamera& c, + Vector J, Matrix Z, double depth); +void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, + Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, + const gtsam::Cal3_S2* K); +void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, + Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, + const gtsam::Cal3_S2* K, + const gtsam::Pose3& body_P_sensor); +Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& values); +gtsam::Values localToWorld(const gtsam::Values& local, + const gtsam::Pose2& base); +gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, + const gtsam::KeyVector& keys); + +} // namespace utilities + class RedirectCout { RedirectCout(); string str(); }; -} +} // namespace gtsam diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 316ca8ee4..edc4883e7 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include #include @@ -86,7 +86,7 @@ namespace gtsam { typedef std::pair, boost::shared_ptr<_FactorType> > EliminationResult; /// The function type that does a single dense elimination step on a subgraph. - typedef boost::function Eliminate; + typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 3390a3aac..b8d446e49 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index e1c18274a..166ae41f4 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -23,8 +23,6 @@ #include -#include - #include #include #include // for cout :-( diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index a9ca7f84d..e337e3249 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,7 +29,6 @@ #include // for Eigen::aligned_allocator #include -#include #include #include #include diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index e67c56e5e..6a1739e20 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -15,14 +15,11 @@ * @author: Alex Cunningham */ -#include +#include #include -#include - #include - -#include +#include namespace gtsam { @@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { /* ************************************************************************* */ static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} -boost::function LabeledSymbol::TypeTest(unsigned char c) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c; +std::function LabeledSymbol::TypeTest(unsigned char c) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)), + c); } -boost::function LabeledSymbol::LabelTest(unsigned char label) { - return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; +std::function LabeledSymbol::LabelTest(unsigned char label) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)), + label); } -boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && - boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; +std::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { + // Use lambda functions for && and == + auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; }; + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind(logical_and, + std::bind(equals, + std::bind(&LabeledSymbol::chr, + std::bind(make, std::placeholders::_1)), + c), + std::bind(equals, + std::bind(&LabeledSymbol::label, + std::bind(make, std::placeholders::_1)), + label)); } /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 15a2a2501..5aee4a0e2 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,8 +19,8 @@ #pragma once +#include #include -#include namespace gtsam { @@ -89,13 +89,13 @@ public: */ // Checks only the type - static boost::function TypeTest(unsigned char c); + static std::function TypeTest(unsigned char c); // Checks only the robot ID (label_) - static boost::function LabelTest(unsigned char label); + static std::function LabelTest(unsigned char label); // Checks both type and the robot ID - static boost::function TypeLabelTest(unsigned char c, unsigned char label); + static std::function TypeLabelTest(unsigned char c, unsigned char label); // Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index e6a2beee0..925ba9ce3 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,8 +18,8 @@ #include +#include #include -#include #include #include @@ -62,8 +62,11 @@ Symbol::operator std::string() const { static Symbol make(gtsam::Key key) { return Symbol(key);} -boost::function Symbol::ChrTest(unsigned char c) { - return bind(&Symbol::chr, bind(make, _1)) == c; +std::function Symbol::ChrTest(unsigned char c) { + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)), + c); } GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 89fb0d161..017d5134a 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -18,11 +18,12 @@ #pragma once -#include #include +#include + #include -#include #include +#include namespace gtsam { @@ -114,7 +115,7 @@ public: * Values::filter() function to retrieve all key-value pairs with the * requested character. */ - static boost::function ChrTest(unsigned char c); + static std::function ChrTest(unsigned char c); /// Output stream operator that can be used with key_formatter (see Key.h). GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &); diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f2ba3e31c..47438ecd6 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -182,6 +182,16 @@ protected: return item->second; } +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(nFactors_); + ar & BOOST_SERIALIZATION_NVP(nEntries_); + } + /// @} }; diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index cc5dc4b88..770b48f63 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -20,7 +20,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7bf65353b..8646a9cae 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -32,14 +32,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include #include diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2e0ffb034..635476687 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -35,14 +35,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include #include diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 746275847..6a2514b35 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include #include #include @@ -38,7 +38,8 @@ namespace gtsam { { // Merge using predicate for comparing first of pair merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), - boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); + std::bind(&less::operator(), less(), std::bind(&KeyValuePair::first, std::placeholders::_1), + std::bind(&KeyValuePair::first, std::placeholders::_2))); if(size() != first.size() + second.size()) throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i new file mode 100644 index 000000000..63047bf4f --- /dev/null +++ b/gtsam/linear/linear.i @@ -0,0 +1,653 @@ +//************************************************************************* +// linear +//************************************************************************* +namespace gtsam { + +namespace noiseModel { +#include +virtual class Base { + void print(string s = "") const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* Information(Matrix R); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + + bool equals(gtsam::noiseModel::Base& expected, double tol); + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + + // access to noise model + double sigma() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { + void print(string s = "") const; +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + // Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s = "VectorValues", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, Vector sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianConditional : gtsam::JacobianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s = "GaussianConditional", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianConditional& cg, double tol) const; + + // Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, + const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s = "GaussianDensity", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + void saveGraph(const string& s) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +#include +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s = "Errors"); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +#include +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s = ""); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s = "") const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +} \ No newline at end of file diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index eafefb3cb..00a338e54 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -25,12 +25,14 @@ #include #include #include // for operator += -using namespace boost::assign; +#include // STL/C++ #include #include +using namespace boost::assign; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -268,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 17dc6a425..c5601af27 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -21,7 +21,6 @@ #include #include // for operator += #include // for operator += -using namespace boost::assign; #include #include @@ -29,6 +28,8 @@ using namespace boost::assign; #include #include +using namespace boost::assign; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -258,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i new file mode 100644 index 000000000..48a5a35de --- /dev/null +++ b/gtsam/navigation/navigation.i @@ -0,0 +1,355 @@ +//************************************************************************* +// Navigation +//************************************************************************* + +namespace gtsam { + +namespace imuBias { +#include + +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Operator Overloads + gtsam::imuBias::ConstantBias operator-() const; + gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + +#include +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::NavState& expected, double tol) const; + + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; + + gtsam::NavState retract(const Vector& x) const; + Vector localCoordinates(const gtsam::NavState& g) const; +}; + +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + boost::optional getOmegaCoriolis() const; + boost::optional getBodyPSensor() const; +}; + +#include +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); + + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); + + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; + +#include +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; + +#include +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); + + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; + +class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); + // Testable + void print(string s = "Preintegrated Measurements:") const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + + // Testable + void print(string s = "Preintegrated Measurements: ") const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + +#include +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +#include +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +#include +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; + +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; + +#include +class ScenarioRunner { + ScenarioRunner(const gtsam::Scenario& scenario, + const gtsam::PreintegrationParams* p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias& bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias& estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements& pim, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; +}; + +} diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 74acf69da..a4d06d01a 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -25,9 +25,9 @@ #include #include -#include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -174,17 +174,17 @@ TEST(AHRSFactor, Error) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -233,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); + std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega); const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); @@ -293,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); + std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -367,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Compute numerical derivatives Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -409,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -458,8 +458,8 @@ TEST (AHRSFactor, predictTest) { // AHRSFactor::PreintegratedMeasurements::predict Matrix expectedH = numericalDerivative11( - boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, - &pim, _1, boost::none), bias); + std::bind(&AHRSFactor::PreintegratedMeasurements::predict, + &pim, std::placeholders::_1, boost::none), bias); // Actual Jacobians Matrix H; diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 38f16f55f..d49907cbf 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -21,8 +21,11 @@ #include #include #include + +#include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -47,8 +50,9 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), + Matrix expectedH = numericalDerivative11( + std::bind(&Rot3AttitudeFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), nRb); // Use the factor to calculate the derivative @@ -114,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, + std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3ef810cad..2bbc2cc7c 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -29,7 +29,6 @@ #include -#include #include #include "imuFactorTesting.h" diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 9457f501d..b784c0c94 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -20,11 +20,14 @@ #include #include +#include + #include #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -69,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -98,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 596b76f6a..b486a4a98 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -19,8 +19,8 @@ #include #include -#include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -127,8 +127,9 @@ TEST(ImuBias, operatorSubB) { TEST(ImuBias, Correct1) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = boost::bind( - &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctAccelerometer, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctAccelerometer(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); @@ -138,8 +139,9 @@ TEST(ImuBias, Correct1) { TEST(ImuBias, Correct2) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = - boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctGyroscope, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctGyroscope(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 474b00add..801d87895 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include "imuFactorTesting.h" @@ -146,8 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix9 aH1, aH2; Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3, + std::function f = + std::bind(&PreintegrationBase::computeError, actual, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -203,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), kZeroBias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::placeholders::_1, boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; Matrix96 aH2; NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, - boost::none), state1); + std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, + kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, - boost::none), kZeroBias); + std::bind(&PreintegrationBase::predict, pim, state1, + std::placeholders::_1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -277,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) { // Make sure rotation part is correct when error is interpreted as axis-angle // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias), x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias), x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); @@ -332,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), bias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::placeholders::_1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -521,7 +522,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); Matrix3 expectedD = numericalDerivative11( - boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), + measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); double dt = 0.1; @@ -532,13 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG1, G1, 1e-5)); // // Matrix93 expectedG2 = numericalDerivative22( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG2, G2, 1e-5)); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 45db58e33..5107b3b6b 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -20,10 +20,13 @@ #include #include +#include + #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -61,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -73,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index a1cc1c6eb..204c1d38f 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -17,6 +17,7 @@ #include #include +using namespace std::placeholders; using namespace gtsam; // ***************************************************************************** @@ -75,8 +76,11 @@ TEST(MagPoseFactor, JacobianPose2) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P2_b), + H2, 1e-7)); } // ***************************************************************************** @@ -86,8 +90,11 @@ TEST(MagPoseFactor, JacobianPose3) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P3_b), + H3, 1e-7)); } // ***************************************************************************** @@ -101,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7)); } // ***************************************************************************** @@ -115,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7)); } // ***************************************************************************** diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index a3f688dda..7796ccbda 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -22,10 +22,11 @@ #include #include -#include #include "imuFactorTesting.h" +using namespace std::placeholders; + namespace testing { // Create default parameters with Z-down and above noise parameters static boost::shared_ptr Params() { @@ -41,21 +42,21 @@ static boost::shared_ptr Params() { TEST(ManifoldPreintegration, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function deltaRij = + std::function deltaRij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaRij(); }; - boost::function deltaPij = + std::function deltaPij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaPij(); }; - boost::function deltaVij = + std::function deltaVij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -96,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&ManifoldPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 5bafe4392..e7adb923d 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -19,8 +19,11 @@ #include #include #include + +#include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -36,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { - boost::function create = - boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, - boost::none); + std::function create = + std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix aH1, aH2, aH3; EXPECT( assert_equal(kState1, @@ -56,9 +59,9 @@ TEST(NavState, Constructor) { /* ************************************************************************* */ TEST(NavState, Constructor2) { - boost::function construct = - boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, - boost::none); + std::function construct = + std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix aH1, aH2; EXPECT( assert_equal(kState1, @@ -73,7 +76,7 @@ TEST( NavState, Attitude) { Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( - boost::bind(&NavState::attitude, _1, boost::none), kState1); + std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -83,7 +86,8 @@ TEST( NavState, Position) { Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( - boost::bind(&NavState::position, _1, boost::none), kState1); + std::bind(&NavState::position, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -93,7 +97,8 @@ TEST( NavState, Velocity) { Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( - boost::bind(&NavState::velocity, _1, boost::none), kState1); + std::bind(&NavState::velocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -103,7 +108,8 @@ TEST( NavState, BodyVelocity) { Velocity3 actual = kState1.bodyVelocity(aH); EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( - boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -134,8 +140,9 @@ TEST( NavState, Manifold ) { // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); - boost::function retract = - boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); + std::function retract = + std::bind(&NavState::retract, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); @@ -146,9 +153,9 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); // Check localCoordinates derivatives - boost::function local = - boost::bind(&NavState::localCoordinates, _1, _2, boost::none, - boost::none); + std::function local = + std::bind(&NavState::localCoordinates, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); @@ -165,8 +172,9 @@ TEST( NavState, Manifold ) { /* ************************************************************************* */ static const double dt = 2.0; -boost::function coriolis = boost::bind( - &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); +std::function coriolis = + std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, + std::placeholders::_2, boost::none); TEST(NavState, Coriolis) { Matrix9 aH; @@ -241,9 +249,10 @@ TEST(NavState, CorrectPIM) { xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 aH1, aH2; - boost::function correctPIM = - boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, - false, boost::none, boost::none); + std::function correctPIM = + std::bind(&NavState::correctPIM, std::placeholders::_1, + std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, + boost::none, boost::none); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 9c080929d..0df51956b 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -19,9 +19,9 @@ #include #include -#include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -147,7 +147,7 @@ TEST(Scenario, Accelerating) { { // Check acceleration in nav Matrix expected = numericalDerivative11( - boost::bind(&Scenario::velocity_n, scenario, _1), T); + std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T); EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); } diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 65fd55fa3..ada059094 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -22,10 +22,11 @@ #include #include -#include #include "imuFactorTesting.h" +using namespace std::placeholders; + static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { @@ -76,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) { TEST(ImuFactor, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function preintegrated = + std::function preintegrated = [=](const Vector3& a, const Vector3& w) { TangentPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -103,10 +104,12 @@ TEST(TangentPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&TangentPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -119,7 +122,7 @@ TEST(TangentPreintegration, Compose) { TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - boost::function f = + std::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 0ba2ad446..cf2462dfc 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -21,6 +21,7 @@ #include +#include #include #include #include @@ -82,7 +83,8 @@ template Expression::Expression(const Expression& expression, T (A::*method)(typename MakeOptionalJacobian::type) const) : root_( - new internal::UnaryExpression(boost::bind(method, _1, _2), + new internal::UnaryExpression(std::bind(method, + std::placeholders::_1, std::placeholders::_2), expression)) { } @@ -95,7 +97,10 @@ Expression::Expression(const Expression& expression1, const Expression& expression2) : root_( new internal::BinaryExpression( - boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + expression1, expression2)) { } /// Construct a binary method expression @@ -109,8 +114,11 @@ Expression::Expression(const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( new internal::TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6), + expression1, expression2, expression3)) { } template @@ -247,8 +255,10 @@ template Expression operator*(const Expression& expression1, const Expression& expression2) { return Expression( - boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, - expression2); + std::bind(internal::apply_compose(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + expression1, expression2); } /// Construct an array of leaves diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 9e8aa3f29..eb828760d 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -24,7 +24,6 @@ #include #include -#include #include #include @@ -69,20 +68,20 @@ public: // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, typename MakeOptionalJacobian::type)> type; }; template struct BinaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type)> type; }; template struct TernaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, const A3&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type, @@ -240,7 +239,7 @@ class BinarySumExpression : public Expression { */ template Expression linearExpression( - const boost::function& f, const Expression& expression, + const std::function& f, const Expression& expression, const Eigen::Matrix::dimension, traits::dimension>& dTdA) { // Use lambda to endow f with a linear Jacobian typename Expression::template UnaryFunction::type g = diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9f33e757f..92c2142a7 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { void removeVariables(const KeySet& unusedKeys); void updateDelta(bool forceFullSolve = false) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object >(*this); + ar & BOOST_SERIALIZATION_NVP(theta_); + ar & BOOST_SERIALIZATION_NVP(variableIndex_); + ar & BOOST_SERIALIZATION_NVP(delta_); + ar & BOOST_SERIALIZATION_NVP(deltaNewton_); + ar & BOOST_SERIALIZATION_NVP(RgProd_); + ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_); + ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_); + ar & BOOST_SERIALIZATION_NVP(linearFactors_); + ar & BOOST_SERIALIZATION_NVP(doglegDelta_); + ar & BOOST_SERIALIZATION_NVP(fixedVariables_); + ar & BOOST_SERIALIZATION_NVP(update_count_); + } + }; // ISAM2 /// traits diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d7f8b0ef8..6b9972156 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -69,7 +69,7 @@ public: /** * Function that compares two values */ - typedef boost::function CompareFunction; + typedef std::function CompareFunction; CompareFunction compare_; // bool (*compare_)(const T& a, const T& b); @@ -87,7 +87,8 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -97,7 +98,8 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { @@ -359,4 +361,3 @@ struct traits > : Testable > }// namespace gtsam - diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 19813adba..8ebdcab17 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,6 +26,8 @@ #include +#include + #include // Only so Eclipse finds class definition namespace gtsam { @@ -101,7 +103,7 @@ namespace gtsam { boost::transform_iterator< KeyValuePair(*)(Values::KeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::iterator> > iterator; @@ -111,7 +113,7 @@ namespace gtsam { boost::transform_iterator< ConstKeyValuePair(*)(Values::ConstKeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::const_iterator> > const_const_iterator; @@ -132,7 +134,7 @@ namespace gtsam { private: Filtered( - const boost::function& filter, + const std::function& filter, Values& values) : begin_( boost::make_transform_iterator( @@ -203,7 +205,7 @@ namespace gtsam { const_iterator begin_; const_iterator end_; ConstFiltered( - const boost::function& filter, + const std::function& filter, const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. @@ -234,33 +236,35 @@ namespace gtsam { /* ************************************************************************* */ Values::Filtered - inline Values::filter(const boost::function& filterFcn) { + inline Values::filter(const std::function& filterFcn) { return filter(filterFcn); } /* ************************************************************************* */ template Values::Filtered - Values::filter(const boost::function& filterFcn) { - return Filtered(boost::bind(&filterHelper, filterFcn, _1), *this); + Values::filter(const std::function& filterFcn) { + return Filtered(std::bind(&filterHelper, filterFcn, + std::placeholders::_1), *this); } /* ************************************************************************* */ Values::ConstFiltered - inline Values::filter(const boost::function& filterFcn) const { + inline Values::filter(const std::function& filterFcn) const { return filter(filterFcn); } /* ************************************************************************* */ template Values::ConstFiltered - Values::filter(const boost::function& filterFcn) const { - return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); + Values::filter(const std::function& filterFcn) const { + return ConstFiltered(std::bind(&filterHelper, + filterFcn, std::placeholders::_1), *this); } /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, + inline bool Values::filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 9b8f7645a..ebc9c51f6 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,14 +25,6 @@ #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 893d5572f..33e9e7d82 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,15 +29,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#pragma GCC diagnostic ignored "-Wunused-local-typedefs" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include @@ -117,19 +108,19 @@ namespace gtsam { /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::iterator> iterator; + std::function, KeyValueMap::iterator> iterator; /// Const forward iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_iterator> const_iterator; + std::function, KeyValueMap::const_iterator> const_iterator; /// Mutable reverse iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + std::function, KeyValueMap::reverse_iterator> reverse_iterator; /// Const reverse iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; typedef KeyValuePair value_type; @@ -330,7 +321,7 @@ namespace gtsam { * the original Values class. */ Filtered - filter(const boost::function& filterFcn); + filter(const std::function& filterFcn); /** * Return a filtered view of this Values class, without copying any data. @@ -353,7 +344,7 @@ namespace gtsam { */ template Filtered - filter(const boost::function& filterFcn = &_truePredicate); + filter(const std::function& filterFcn = &_truePredicate); /** * Return a filtered view of this Values class, without copying any data. @@ -369,7 +360,7 @@ namespace gtsam { * the original Values class. */ ConstFiltered - filter(const boost::function& filterFcn) const; + filter(const std::function& filterFcn) const; /** * Return a filtered view of this Values class, without copying any data. @@ -391,7 +382,7 @@ namespace gtsam { */ template ConstFiltered - filter(const boost::function& filterFcn = &_truePredicate) const; + filter(const std::function& filterFcn = &_truePredicate) const; // Count values of given type \c ValueType template @@ -408,7 +399,7 @@ namespace gtsam { // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. template - static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + static bool filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { BOOST_STATIC_ASSERT((!boost::is_same::value)); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 2a1a07fb0..f6afb287e 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { * Expression node. The superclass for objects that do the heavy lifting * An Expression has a pointer to an ExpressionNode underneath * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. + * are passed by value. This is the same way std::function works. * http://loki-lib.sourceforge.net/html/a00652.html */ template diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i new file mode 100644 index 000000000..8071e8bc7 --- /dev/null +++ b/gtsam/nonlinear/nonlinear.i @@ -0,0 +1,804 @@ +//************************************************************************* +// nonlinear +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Symbol { + Symbol(); + Symbol(char c, uint64_t j); + Symbol(size_t key); + + size_t key() const; + void print(const string& s = "") const; + bool equals(const gtsam::Symbol& expected, double tol) const; + + char chr() const; + uint64_t index() const; + string string() const; +}; + +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +namespace symbol_shorthand { +size_t A(size_t j); +size_t B(size_t j); +size_t C(size_t j); +size_t D(size_t j); +size_t E(size_t j); +size_t F(size_t j); +size_t G(size_t j); +size_t H(size_t j); +size_t I(size_t j); +size_t J(size_t j); +size_t K(size_t j); +size_t L(size_t j); +size_t M(size_t j); +size_t N(size_t j); +size_t O(size_t j); +size_t P(size_t j); +size_t Q(size_t j); +size_t R(size_t j); +size_t S(size_t j); +size_t T(size_t j); +size_t U(size_t j); +size_t V(size_t j); +size_t W(size_t j); +size_t X(size_t j); +size_t Y(size_t j); +size_t Z(size_t j); +} // namespace symbol_shorthand + +// Default keyformatter +void PrintKeyList(const gtsam::KeyList& keys); +void PrintKeyList(const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet(const gtsam::KeySet& keys); +void PrintKeySet(const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s = "") const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s = "NonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + + // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const + // std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + void saveGraph(const string& s) const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; +}; + +#include +virtual class NoiseModelFactor : gtsam::NonlinearFactor { + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting + * machinery there. This is achieved by adding `gtsam::CustomFactor` to the + * ignore list in `matlab/CMakeLists.txt`. + */ + CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + const gtsam::KeyVector& keys, + const gtsam::CustomErrorFunction& errorFunction); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // New in 4.0, we have to specialize every insert/update/at to generate + // wrappers Instead of the old: void insert(size_t j, const gtsam::Value& + // value); void update(size_t j, const gtsam::Value& val); gtsam::Value + // at(size_t j) const; + + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); + void insert(size_t j, double c); + + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); + void update(size_t j, double c); + + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias, + gtsam::NavState, + Vector, + Matrix, + double}> + T at(size_t j); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); + + void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance( + const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation( + const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s = "", gtsam::KeyFormatter keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + LinearContainerFactor(gtsam::GaussianFactor* factor, + const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; + // const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph ConvertLinearGraph( + const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph ConvertLinearGraph( + const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor + +// Summarization functionality +//#include +// +//// Uses partial QR approach by default +// gtsam::GaussianFactorGraph summarize( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); +// +// gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); + +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s = "") const; + + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); + + string getLinearSolverType() const; + void setLinearSolverType(string solver); + + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); + + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; + +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); + +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; + + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); +}; + +#include +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +#include +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph* iterate() const; +}; + +#include +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::GaussNewtonParams& params); +}; + +#include +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::DoglegParams& params); + double getDelta() const; +}; + +#include +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string s = "") const; +}; + +#include +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; + +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + +class ISAM2Params { + ISAM2Params(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + void setOptimizationParams( + const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck); +}; + +class ISAM2Clique { + // Constructors + ISAM2Clique(); + + // Standard Interface + Vector gradientContribution() const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; +}; + +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); + + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys, + bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; + +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& initialValues); + void reorder_relinearize(); + + // These might be expensive as instead of a reference the wrapper will make a + // copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + +//************************************************************************* +// Nonlinear factor types +//************************************************************************* +#include +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 8fcf84a11..80262ae3f 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -58,22 +58,23 @@ TEST(Expression, Constant) { /* ************************************************************************* */ // Leaf TEST(Expression, Leaf) { - Rot3_ R(100); + const Key key = 100; + Rot3_ R(key); Values values; - values.insert(100, someR); + values.insert(key, someR); Rot3 actual2 = R.value(values); EXPECT(assert_equal(someR, actual2)); } /* ************************************************************************* */ -// Many Leaves +// Test the function `createUnknowns` to create many leaves at once. TEST(Expression, Leaves) { Values values; - Point3 somePoint(1, 2, 3); + const Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); - std::vector points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint, points.back().value(values))); + std::vector pointExpressions = createUnknowns(10, 'p', 1); + EXPECT(assert_equal(somePoint, pointExpressions.back().value(values))); } /* ************************************************************************* */ @@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { Vector f3(const Point3& p, OptionalJacobian H) { return p; } -Point3_ p(1); +Point3_ pointExpression(1); set expected = list_of(1); } // namespace unary +// Create a unary expression that takes another expression as a single argument. TEST(Expression, Unary1) { using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} -TEST(Expression, Unary2) { - using namespace unary; - Double_ e(f2, p); - EXPECT(expected == e.keys()); + Expression unaryExpression(f1, pointExpression); + EXPECT(expected == unaryExpression.keys()); +} + +// Check that also works with a scalar return value. +TEST(Expression, Unary2) { + using namespace unary; + Double_ unaryExpression(f2, pointExpression); + EXPECT(expected == unaryExpression.keys()); } -/* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; - // Expression e(f3, p); + // TODO(yetongumich): dynamic output arguments do not work yet! + // Expression unaryExpression(f3, pointExpression); + // EXPECT(expected == unaryExpression.keys()); } /* ************************************************************************* */ +// Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: enum {dimension = 3}; @@ -133,16 +139,20 @@ template<> struct traits : public internal::VectorSpace {}; // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm_(p, &Class::norm); + const Key key(67); + Expression classExpression(key); + + // Make expression from a class method, note how it differs from the function + // expressions by leading with the class expression in the constructor. + Expression norm_(classExpression, &Class::norm); // Create Values Values values; - values.insert(67, Class(3, 4, 5)); + values.insert(key, Class(3, 4, 5)); // Check dims as map std::map map; - norm_.dims(map); + norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention. LONGS_EQUAL(1, map.size()); // Get value and Jacobians @@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) { double actual = norm_.value(values, H); // Check all - EXPECT(actual == sqrt(50)); + const double norm = sqrt(3*3 + 4*4 + 5*5); + EXPECT(actual == norm); Matrix expected(1, 3); - expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); + expected << 3.0 / norm, 4.0 / norm, 5.0 / norm; EXPECT(assert_equal(expected, H[0])); } @@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p); } /* ************************************************************************* */ -// Check that creating an expression to double compiles +// Check that creating an expression to double compiles. TEST(Expression, BinaryToDouble) { using namespace binary; Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ -// keys +// Check keys of an expression created from class method. TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } /* ************************************************************************* */ -// dimensions +// Check dimensions by calling `dims` method. TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); @@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) { } /* ************************************************************************* */ -// dimensions +// Check dimensions of execution trace. TEST(Expression, BinaryTraceSize) { typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); @@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) { } /* ************************************************************************* */ +// Test compose operation with * operator. TEST(Expression, compose1) { // Create expression Rot3_ R1(1), R2(2); @@ -258,7 +270,7 @@ TEST(Expression, compose1) { } /* ************************************************************************* */ -// Test compose with arguments referring to the same rotation +// Test compose with arguments referring to the same rotation. TEST(Expression, compose2) { // Create expression Rot3_ R1(1), R2(1); @@ -270,7 +282,7 @@ TEST(Expression, compose2) { } /* ************************************************************************* */ -// Test compose with one arguments referring to constant rotation +// Test compose with one arguments referring to constant rotation. TEST(Expression, compose3) { // Create expression Rot3_ R1(Rot3::identity()), R2(3); @@ -282,7 +294,7 @@ TEST(Expression, compose3) { } /* ************************************************************************* */ -// Test with ternary function +// Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) @@ -306,6 +318,7 @@ TEST(Expression, ternary) { } /* ************************************************************************* */ +// Test scalar multiplication with * operator. TEST(Expression, ScalarMultiply) { const Key key(67); const Point3_ expr = 23 * Point3_(key); @@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) { } /* ************************************************************************* */ +// Test sum with + operator. TEST(Expression, BinarySum) { const Key key(67); const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); @@ -366,6 +380,7 @@ TEST(Expression, BinarySum) { } /* ************************************************************************* */ +// Test sum of 3 variables with + operator. TEST(Expression, TripleSum) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -387,6 +402,7 @@ TEST(Expression, TripleSum) { } /* ************************************************************************* */ +// Test sum with += operator. TEST(Expression, PlusEqual) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -461,11 +477,12 @@ TEST(Expression, WeightedSum) { EXPECT(actual_dims == expected_dims); Values values; - values.insert(key1, Point3(1, 0, 0)); - values.insert(key2, Point3(0, 1, 0)); + const Point3 point1(1, 0, 0), point2(0, 1, 0); + values.insert(key1, point1); + values.insert(key2, point2); // Check value - const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0); + const Point3 expected = 17 * point1 + 23 * point2; EXPECT(assert_equal(expected, weighted_sum_.value(values))); // Check value + Jacobians @@ -495,7 +512,7 @@ TEST(Expression, Subtract) { /* ************************************************************************* */ TEST(Expression, LinearExpression) { const Key key(67); - const boost::function f = [](const Point3& p) { return (Vector3)p; }; + const std::function f = [](const Point3& p) { return (Vector3)p; }; const Matrix3 kIdentity = I_3x3; const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 90ebcbbba..f4bb5f4f6 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -31,6 +32,30 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Create GUIDs for Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// Create GUIDs for factors +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); + + /* ************************************************************************* */ // Export all classes derived from Value GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); @@ -82,6 +107,61 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } +TEST(Serialization, ISAM2) { + + gtsam::ISAM2Params parameters; + gtsam::ISAM2 solver(parameters); + gtsam::NonlinearFactorGraph graph; + gtsam::Values initialValues; + initialValues.clear(); + + gtsam::Vector6 temp6; + for (int i = 0; i < 6; ++i) { + temp6[i] = 0.0001; + } + gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6); + + gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0)); + gtsam::Symbol symbol0('x', 0); + graph.add(gtsam::PriorFactor(symbol0, pose0, noiseModel)); + initialValues.insert(symbol0, pose0); + + solver.update(graph, initialValues, + gtsam::FastVector()); + + std::string binaryPath = "saved_solver.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << solver; + } catch(...) { + EXPECT(false); + } + + gtsam::ISAM2 solverFromDisk; + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> solverFromDisk; + } catch(...) { + EXPECT(false); + } + + gtsam::Pose3 p1, p2; + try { + p1 = solver.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + + try { + p2 = solverFromDisk.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + EXPECT(assert_equal(p1, p2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index aacceb276..b894f4816 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -28,11 +28,13 @@ #include // for operator += #include #include -using namespace boost::assign; +#include #include #include #include +using namespace boost::assign; +using namespace std::placeholders; using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); @@ -334,7 +336,7 @@ TEST(Values, filter) { // Filter by key int i = 0; - Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); for(const auto key_value: filtered) { if(i == 0) { @@ -362,7 +364,7 @@ TEST(Values, filter) { EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); // ConstFilter by Key - Values::ConstFiltered constfiltered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::ConstFiltered constfiltered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)constfiltered.size()); Values fromconstfiltered(constfiltered); EXPECT(assert_equal(expectedSubValues1, fromconstfiltered)); diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index ffc279872..4e79e20ff 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -260,30 +260,5 @@ Values localToWorld(const Values& local, const Pose2& base, } // namespace utilities -/** - * For Python __str__(). - * Redirect std cout to a string stream so we can return a string representation - * of an object when it prints to cout. - * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string - */ -struct RedirectCout { - /// constructor -- redirect stdout buffer to a stringstream buffer - RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} - - /// return the string - std::string str() const { - return ssBuffer_.str(); - } - - /// destructor -- redirect stdout buffer to its original buffer - ~RedirectCout() { - std::cout.rdbuf(coutBuffer_); - } - -private: - std::stringstream ssBuffer_; - std::streambuf* coutBuffer_; -}; - } diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i new file mode 100644 index 000000000..370e1c3ea --- /dev/null +++ b/gtsam/sam/sam.i @@ -0,0 +1,96 @@ +//************************************************************************* +// sam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include + +// ##### + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor + RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor, gtsam::Point3> + RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor + RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor, + gtsam::PinholeCamera> + RangeFactorSimpleCamera; + +#include +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel, + const POSE& body_T_sensor); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform3D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose3; + +#include +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingFactor + BearingFactor2D; +typedef gtsam::BearingFactor + BearingFactor3D; +typedef gtsam::BearingFactor + BearingFactorPose2; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + + gtsam::BearingRange measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor + BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose2; + +} // namespace gtsam diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 673d4e052..5f5d4f4dd 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -26,8 +26,9 @@ #include #include -#include +#include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -264,9 +265,9 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError2D, _1, point, factor), pose); + std::bind(&factorError2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError2D, pose, _1, factor), point); + std::bind(&factorError2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -295,9 +296,9 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -322,9 +323,9 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError3D, _1, point, factor), pose); + std::bind(&factorError3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError3D, pose, _1, factor), point); + std::bind(&factorError3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -354,9 +355,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a982ef7da..76fd1bfc7 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,6 +944,36 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +// Extract Rot2 measurement from Pose2 betweenfactors +// Modeled after similar function in dataset.cpp +static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( + const BetweenFactor::shared_ptr &f) { + auto gaussian = + boost::dynamic_pointer_cast(f->noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix3 M = gaussian->covariance(); + auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); +} + +static ShonanAveraging2::Measurements extractRot2Measurements( + const BetweenFactorPose2s &factors) { + ShonanAveraging2::Measurements result; + result.reserve(factors.size()); + for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f)); + return result; +} + +ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters) + : ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors), + parameters.getUseHuber()), + parameters) {} + /* ************************************************************************* */ // Explicit instantiation for d=3 template class ShonanAveraging<3>; diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index f00109cae..de12de478 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); + ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters = Parameters()); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i new file mode 100644 index 000000000..705892e60 --- /dev/null +++ b/gtsam/sfm/sfm.i @@ -0,0 +1,211 @@ +//************************************************************************* +// sfm +//************************************************************************* + +namespace gtsam { + +// ##### + +#include + +virtual class ShonanFactor3 : gtsam::NoiseModelFactor { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p, + gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + +#include +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base* noiseModel() const; +}; + +typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; +typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; + +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + +#include + +// TODO(frank): copy/pasta below until we have integer template paremeters in +// wrap! + +class ShonanAveragingParameters2 { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveragingParameters3 { + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveraging2 { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2& parameters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +class ShonanAveraging3 { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3& parameters); + + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, + const gtsam::ShonanAveragingParameters3& parameters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; + +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; + +#include +class TranslationRecovery { + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::LevenbergMarquardtParams& lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; + +} // namespace gtsam diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 3ff76ac5c..818f2bce5 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -22,6 +22,7 @@ #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -89,9 +90,9 @@ TEST(TranslationFactor, Jacobian) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError, _1, T2, factor), T1); + std::bind(&factorError, std::placeholders::_1, T2, factor), T1); H2Expected = numericalDerivative11( - boost::bind(&factorError, T1, _1, factor), T2); + std::bind(&factorError, T1, std::placeholders::_1, factor), T2); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i new file mode 100644 index 000000000..1c04fd14c --- /dev/null +++ b/gtsam/slam/slam.i @@ -0,0 +1,338 @@ +//************************************************************************* +// slam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include + +// ###### + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, + const gtsam::noiseModel::Base* noiseModel); + T measured() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, + const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + const POSE& body_P_sensor); + + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Unified; + +#include +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, size_t cameraKey, + size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Unified; + +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, size_t poseKey, + size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include + +/// Linearization mode: what factor to linearize to +enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; + +/// How to manage degeneracy +enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; + +class SmartProjectionParams { + SmartProjectionParams(); + void setLinearizationMode(gtsam::LinearizationMode linMode); + void setDegeneracyMode(gtsam::DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor + SmartProjectionPose3Factor; + +#include +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor + GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, + const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + +#include + +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + + std::vector> measurements; + + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; +}; + +class SfmData { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t); + void add_camera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; +}; + +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); + +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise, bool smart); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust( + string filename, gtsam::noiseModel::Base* model, int maxIndex); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose2] +class BetweenFactorPose2s { + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose3] +class BetweenFactorPose3s { + BetweenFactorPose3s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose3s parse3DFactors(string filename); + +pair load3D(string filename); + +pair readG2o(string filename); +pair readG2o(string filename, + bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + +#include +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; + +#include +gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, + size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, + gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d995bf8e7..6897943ec 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,16 +1,20 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief - * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @author Duy-Nguyen Ta, Varun Agrawal + * @date Aug 2, 2013 */ +#include +#include #include +#include #include #include +#include #include -#include +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -31,42 +35,86 @@ TEST(BetweenFactor, Rot3) { Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail - Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21( + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + R1, R2, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none, boost::none)), R1, R2, 1e-5); 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); + double measured = 0.0; + BetweenFactor factor(1, 2, measured, 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); + Vector3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, 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); + Vector measured(5); measured << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured, model); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Point3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Point3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); + + Values values; + values.insert(1, Point3(0, 0, 0)); + values.insert(2, Point3(1, 2, 3)); + Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Rot3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Rot3 measured = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured, model); + + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3::Ry(M_PI_2)); + Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Pose3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); + Pose3 measured(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured, model); + + Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3)); + Values values; + values.insert(1, pose1); + values.insert(2, pose2); + Vector6 error = factor.evaluateError(pose1, pose2); + EXPECT(assert_equal(Vector6::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } -*/ /* ************************************************************************* */ int main() { diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 16197ab03..080239b35 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -22,8 +22,10 @@ #include #include #include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -50,12 +52,14 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, - boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, - boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 78857262f..03775a70f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -17,6 +17,7 @@ #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -101,8 +102,8 @@ TEST(EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + std::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -127,9 +128,9 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::function, OptionalJacobian<5, 2>)> g; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index a0ef7b91d..35c750408 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -27,9 +27,9 @@ #include #include -#include using namespace boost::assign; +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -144,8 +144,9 @@ TEST( OrientedPlane3Factor, Derivatives ) { OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); // Calculate numerical derivatives - boost::function f = boost::bind( - &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + std::function f = std::bind( + &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); @@ -183,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // Calculate numerical derivatives Matrix expectedH1 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T1); Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T2); Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T3); // Use the factor to calculate the derivative diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 217ff2178..b5800a414 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -16,8 +16,6 @@ #include -#include - #include #include @@ -32,6 +30,7 @@ using namespace std; using namespace boost; +using namespace std::placeholders; using namespace gtsam; typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; @@ -69,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); @@ -101,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index caf3d31df..b8fd01730 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -12,12 +12,12 @@ #include #include -#include #include #include using namespace std; using namespace boost::assign; +using namespace std::placeholders; using namespace gtsam; static const double kDegree = M_PI / 180; @@ -72,13 +72,13 @@ TEST (RotateFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } @@ -143,14 +143,14 @@ TEST (RotateDirectionsFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 0cc5e8f55..c7f220c10 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -30,6 +30,7 @@ #include using namespace boost::assign; +using namespace std::placeholders; static const double rankTol = 1.0; // Create a noise model for the pixel error @@ -130,8 +131,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) - boost::function f = // - boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP Matrix actualE; diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 4bbef6530..ad88c88fc 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -27,10 +27,12 @@ #include #include +#include using namespace std; using namespace gtsam; using namespace boost::assign; +using namespace std::placeholders; // Some common constants static const boost::shared_ptr sharedCal = // @@ -60,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); @@ -84,13 +86,15 @@ TEST( triangulation, TriangulationFactorStereo ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); // compare same problem against expression factor - Expression::UnaryFunction::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2); + Expression::UnaryFunction::type f = + std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, + boost::none, std::placeholders::_2); Expression point_(pointKey); Expression project2_(f, point_); diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i new file mode 100644 index 000000000..4e7cca68a --- /dev/null +++ b/gtsam/symbolic/symbolic.i @@ -0,0 +1,201 @@ +//************************************************************************* +// Symbolic +//************************************************************************* +namespace gtsam { + +#include +#include + +// ################### + +#include +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, + size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + + // From Factor + size_t size() const; + void print(string s = "SymbolicFactor", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; + +#include +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s = "SymbolicFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; + + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); + + // Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal( + const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::KeyVector& keys); + pair + eliminatePartialMultifrontal(const gtsam::Ordering& ordering); + pair + eliminatePartialMultifrontal(const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); +}; + +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, + size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, + size_t nrFrontals); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; + + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; + +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s = "SymbolicBayesNet", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; + +#include +class SymbolicBayesTree { + // Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + // Standard Interface + // size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +class SymbolicBayesTreeClique { + SymbolicBayesTreeClique(); + // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); + + bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + size_t numCachedSeparatorMarginals() const; + // gtsam::sharedConditional* conditional() const; + bool isRoot() const; + size_t treeSize() const; + gtsam::SymbolicBayesTreeClique* parent() const; + + // // TODO: need wrapped versions graphs, BayesNet + // BayesNet shortcut(derived_ptr root, Eliminate function) + // const; FactorGraph marginal(derived_ptr root, Eliminate + // function) const; FactorGraph joint(derived_ptr C2, derived_ptr + // root, Eliminate function) const; + // + void deleteCachedShortcuts(); +}; + +#include +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + // template + // VariableIndex(const T& factorGraph, size_t nVariables); + // VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); + VariableIndex(const gtsam::VariableIndex& other); + + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s = "VariableIndex: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; + +} // namespace gtsam diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index 96424324b..9d854a169 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -260,7 +260,7 @@ namespace gtsam { } /** iterate over tree */ - void iter(boost::function f) const { + void iter(std::function f) const { if (!root_) return; left().iter(f); f(key(), value()); @@ -269,7 +269,7 @@ namespace gtsam { /** map key-values in tree over function f that computes a new value */ template - BTree map(boost::function f) const { + BTree map(std::function f) const { if (empty()) return BTree (); std::pair xd(key(), f(key(), value())); return BTree (left().map(f), xd, right().map(f)); @@ -282,7 +282,7 @@ namespace gtsam { * The associated values are passed to [f] in reverse sort order */ template - ACC fold(boost::function f, + ACC fold(std::function f, const ACC& a) const { if (!root_) return a; ACC ar = right().fold(f, a); // fold over right subtree diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index c7b8cd417..4ad7d3ea8 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -122,7 +122,7 @@ public: } // maps f over all keys, must be invertible - DSF map(boost::function func) const { + DSF map(std::function func) const { DSF t; for(const KeyLabel& pair: (Tree)*this) t = t.add(func(pair.first), func(pair.second)); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 531d54af1..9f00f81d6 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** @@ -89,9 +91,9 @@ public: z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5); return z - predict_proxy(x1, x2, dt_); } diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 62a6abcd9..9a742b4f0 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** @@ -79,9 +81,9 @@ public: boost::optional H2 = boost::none) const override { const Vector6 meas = z(); if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 976f448c5..bf3b95c0f 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -166,24 +166,24 @@ public: boost::optional H3 = boost::none) const { if (H1) { (*H1) = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H2) { (*H2) = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H3) { (*H3) = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 56171a728..d24d06e79 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { @@ -84,9 +86,11 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const override { if (H1) *H1 = gtsam::numericalDerivative21( - boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); if (H2) *H2 = gtsam::numericalDerivative22( - boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index fe21d5e00..882d5423a 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -9,6 +9,7 @@ #include /* ************************************************************************* */ +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -56,19 +57,28 @@ TEST( Reconstruction, evaluateError) { assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); @@ -109,22 +119,22 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index f72b2cf95..8d496a30e 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 0349f3293..f1bbc3970 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -23,8 +23,7 @@ #include -#include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -66,11 +65,11 @@ TEST(Event, Derivatives) { Matrix13 actualH2; kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none), + std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - boost::bind(kToa, exampleEvent, _1, boost::none, boost::none), + std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index df21c0132..c755f2451 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -39,6 +39,7 @@ #include using boost::fusion::at_c; +using namespace std::placeholders; using namespace std; namespace bf = boost::fusion; @@ -410,50 +411,50 @@ typedef qi::grammar> base_grammar; struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; QPSVisitor *rqp_; - boost::function const &)> setName; - boost::function const &)> + std::function const &)> setName; + std::function const &)> addRow; - boost::function const &)> rhsSingle; - boost::function)> rhsDouble; - boost::function const &)> rangeSingle; - boost::function)> rangeDouble; - boost::function)> colSingle; - boost::function const &)> colDouble; - boost::function const &)> addQuadTerm; - boost::function const &)> addBound; - boost::function const &)> addFreeBound; MPSGrammar(QPSVisitor *rqp) : base_grammar(start), rqp_(rqp), - setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), - addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), - rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), - rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), - rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), - rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), - colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), - colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), - addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), - addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), - addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { + setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)), + addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)), + rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)), + rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)), + rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)), + rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)), + colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)), + colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)), + addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)), + addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)), + addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 165135114..cabbfdbe8 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -27,6 +27,7 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include +#include #include #include @@ -307,38 +308,68 @@ public: // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + Pose1); + Matrix H1_Vel = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), + Vel1); + Matrix H2_Vel = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), + Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. IMUBias1 if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Pose = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), + Bias1); + Matrix H3_Vel = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), + Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Pose2 if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Pose = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), + Pose2); + Matrix H4_Vel = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), + Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } // Jacobian w.r.t. Vel2 if (H5){ if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Pose = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), + Vel2); + Matrix H5_Vel = numericalDerivative11( + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), + Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } @@ -437,18 +468,45 @@ public: Matrix Z_3x3 = Z_3x3; Matrix I_3x3 = I_3x3; - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + std::placeholders::_1, delta_vel_in_t0), + delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + delta_pos_in_t0, std::placeholders::_1), + delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_vel = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, std::placeholders::_1, + flag_use_body_P_sensor, body_P_sensor, Bias_t0), + delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, std::placeholders::_1, delta_vel_in_t0, + flag_use_body_P_sensor, body_P_sensor, Bias_t0), + delta_angles); + Matrix H_vel_bias = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, + flag_use_body_P_sensor, body_P_sensor, + std::placeholders::_1), + Bias_t0); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_angles = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, std::placeholders::_1, flag_use_body_P_sensor, + body_P_sensor, Bias_t0), + delta_angles); + Matrix H_angles_bias = numericalDerivative11( + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, + std::placeholders::_1), + Bias_t0); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 40dc81c9a..0e2aebd7f 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -278,29 +278,29 @@ public: // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. Pose2 if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Vel2 if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -372,15 +372,15 @@ public: Matrix Z_3x3 = Z_3x3; Matrix I_3x3 = I_3x3; - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index f031f4884..0828fbd08 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -26,6 +26,7 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include +#include #include #include @@ -234,38 +235,68 @@ public: // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ - Matrix H1_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Pose = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + Pose1); + Matrix H1_Vel = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Pose = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), + Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), + Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. IMUBias1 if (H3){ - Matrix H3_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Pose = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), + Bias1); + Matrix H3_Vel = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), + Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Pose2 if (H4){ - Matrix H4_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Pose = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), + Pose2); + Matrix H4_Vel = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), + Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } // Jacobian w.r.t. Vel2 if (H5){ if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Pose = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), + Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11( + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), + Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 7a34ab1bc..40c09416c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -17,6 +17,8 @@ #include #include +#include + namespace gtsam { /** @@ -106,13 +108,14 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, - landmark), pose); + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, + std::placeholders::_1, landmark), + pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, - _1), landmark); + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 0462aefad..b1169580e 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -18,6 +18,8 @@ #include #include +#include + namespace gtsam { /** @@ -109,13 +111,13 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, - landmark), pose); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, - _1), landmark); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 454358a0e..98f2db2f3 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -17,6 +17,8 @@ #include #include +#include + namespace gtsam { /** @@ -108,10 +110,16 @@ public: boost::optional H2=boost::none) const override { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11( + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, + std::placeholders::_1, landmark), + pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11( + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, + std::placeholders::_1), + landmark); } return inverseDepthError(pose, landmark); @@ -229,13 +237,22 @@ public: boost::optional H3=boost::none) const override { if(H1) - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + (*H1) = numericalDerivative11( + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, + std::placeholders::_1, pose2, landmark), + pose1); if(H2) - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + (*H2) = numericalDerivative11( + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + std::placeholders::_1, landmark), + pose2); if(H3) - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11( + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + pose2, std::placeholders::_1), + landmark); return inverseDepthError(pose1, pose2, landmark); } diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 70368cc0e..4d6e1912a 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -190,12 +190,12 @@ TEST (BetweenFactorEM, jacobian ) { // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); - Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); // try to check numerical derivatives of a standard between factor - Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); + Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index e4112f53d..59c4fdf53 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -9,8 +9,10 @@ #include #include #include + #include +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -65,15 +67,17 @@ TEST(BiasedGPSFactor, jacobian) { factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 35bf5790e..644283512 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -21,9 +21,12 @@ #include #include #include + +#include #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 209326672..8692cf584 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -23,6 +23,7 @@ #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -105,9 +106,13 @@ TEST (GaussMarkovFactor, jacobian ) { // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function Matrix numerical_H1, numerical_H2; numerical_H1 = numericalDerivative21( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); numerical_H2 = numericalDerivative22( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); // Verify they are equal for this choice of state CHECK( assert_equal(numerical_H1, computed_H1, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index b84f7e080..e68b2fe5f 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -25,6 +25,7 @@ #include #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -249,7 +250,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, // // Vector3 v(predictionRq(angles, q)); // -// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(std::bind(&predictionRq, std::placeholders::_1, q), angles); // // cout<<"J_hyp"<( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -344,19 +345,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -642,19 +643,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; H1_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -676,19 +677,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 9f6fe5b50..b97d56c7e 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -24,8 +24,7 @@ #include -#include - +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -142,8 +141,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); // Calculate numerical derivatives - auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, - _1, _2, _3, boost::none, boost::none, boost::none); + auto f = + std::bind(&LocalOrientedPlane3Factor::evaluateError, factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); Matrix numericalH2 = numericalDerivative32 #include #include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -80,7 +82,9 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -96,13 +100,16 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) { Pose2 measurement(-6.0, 3.5, 0.123); // Prior on x component of translation. - TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); + TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(), + NM::Isotropic::Sigma(2, 0.25)); Pose2 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -124,7 +131,7 @@ TEST(PartialPriorFactor, JacobianTheta) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -175,7 +182,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -197,7 +204,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -221,7 +228,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -245,7 +252,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -268,7 +275,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index f589e932f..cd5bf1d9e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -21,8 +21,10 @@ #include #include #include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -196,8 +198,14 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -225,8 +233,14 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Point3(-3.5257579, 6.02637531, -8.98382384)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 166f058e3..dbbc02a8b 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -21,8 +21,10 @@ #include #include #include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -185,7 +187,10 @@ TEST( PosePriorFactor, Jacobian ) { Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -209,7 +214,10 @@ TEST( PosePriorFactor, JacobianWithTransform ) { Point3(-4.74767676, 7.67044942, -11.00985)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 8f8563e9d..e0e5c4581 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -63,7 +63,7 @@ TEST(PoseToPointFactor, jacobian) { PoseToPointFactor factor(pose_key, point_key, l_meas, noise); // Calculate numerical derivatives - auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, boost::none, boost::none); Matrix numerical_H1 = numericalDerivative21(f, p, l); Matrix numerical_H2 = numericalDerivative22(f, p, l); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 4e9fdcb50..c05f83a23 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -28,8 +28,7 @@ #include -#include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -177,10 +176,13 @@ TEST( ProjectionFactorPPP, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, Pose3(), point); + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, Pose3(), point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); } @@ -213,9 +215,12 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) { // Verify H2 with numerical derivative Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, body_P_sensor, point); + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, body_P_sensor, point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5eadf5fd6..6490dfc75 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -28,8 +28,7 @@ #include -#include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -137,12 +136,16 @@ TEST( ProjectionFactorPPPC, Jacobian ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, - *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, + std::placeholders::_1, point, *K1, boost::none, boost::none, + boost::none, boost::none), + Pose3()); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), + point, std::placeholders::_1, boost::none, boost::none, + boost::none, boost::none), + *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); @@ -173,12 +176,12 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index 210018ed3..25ca3339b 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -11,6 +11,7 @@ #include +using namespace std::placeholders; using namespace gtsam; SharedNoiseModel model1 = noiseModel::Unit::Create(1); @@ -35,10 +36,14 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -50,10 +55,14 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -65,10 +74,14 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -80,10 +93,14 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -95,10 +112,14 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -110,10 +131,14 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -125,10 +150,14 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose3, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 042130a24..bc5c553e0 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -25,7 +25,6 @@ #include #include -#include #include using namespace std; diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 098a0ef56..fbb21e191 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -18,8 +18,10 @@ #include #include + #include +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -45,10 +47,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none, boost::none), pose); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none, boost::none), point); // Verify the Jacobians are correct @@ -79,17 +81,17 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); H3Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1, point, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, - _1, boost::none, boost::none, boost::none, boost::none), point); + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -120,17 +122,17 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); H3Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1, pose2, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, - base2, _1, boost::none, boost::none, boost::none, boost::none), + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), pose2); // Verify the Jacobians are correct diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index 01d4b152d..36914f88f 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -18,7 +18,7 @@ #include #include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -231,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } @@ -285,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index b200a3e58..657a9fb34 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -18,7 +18,7 @@ #include #include - +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -260,8 +260,10 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); -// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); + Matrix H1_expected = gtsam::numericalDerivative11( + std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, + stepsize); + // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } /////* ************************************************************************** */ //TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) { @@ -310,12 +312,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6..7b8347da5 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -50,8 +50,22 @@ set(ignore gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) +set(interface_headers + ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i + ${PROJECT_SOURCE_DIR}/gtsam/base/base.i + ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i + ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i + ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i + ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i + ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i + ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i +) + + pybind_wrap(gtsam_py # target - ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header + "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name "gtsam" # top_namespace @@ -102,6 +116,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target diff --git a/python/README.md b/python/README.md index ec9808ce0..54436df93 100644 --- a/python/README.md +++ b/python/README.md @@ -24,6 +24,7 @@ For instructions on updating the version of the [wrap library](https://github.co ```bash cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 ``` + If you do not have TBB installed, you should also provide the argument `-DGTSAM_WITH_TBB=OFF`. - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`). - To install, simply run `make python-install` (`ninja python-install`). diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b800f7c35..93e7ffbaf 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -18,7 +18,7 @@ #include #include "gtsam/config.h" #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam.i` {includes} @@ -32,20 +32,24 @@ // Preamble for STL classes // TODO(fan): make this automatic -#include "python/gtsam/preamble.h" +#include "python/gtsam/preamble/{module_name}.h" using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} // Specializations for STL classes // TODO(fan): make this automatic -#include "python/gtsam/specializations.h" +#include "python/gtsam/specializations/{module_name}.h" }} diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h deleted file mode 100644 index 7294a6ac8..000000000 --- a/python/gtsam/preamble.h +++ /dev/null @@ -1,30 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - */ -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif -PYBIND11_MAKE_OPAQUE(std::vector >); -PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector - -// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this. -namespace std { - using gtsam::operator<<; -} diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h new file mode 100644 index 000000000..626b47ae4 --- /dev/null +++ b/python/gtsam/preamble/base.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE(std::vector); + +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h new file mode 100644 index 000000000..498c7dc58 --- /dev/null +++ b/python/gtsam/preamble/geometry.h @@ -0,0 +1,21 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE( + gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h new file mode 100644 index 000000000..ec39c410a --- /dev/null +++ b/python/gtsam/preamble/gtsam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ +#ifdef GTSAM_ALLOCATOR_TBB +PYBIND11_MAKE_OPAQUE(std::vector>); +#else +PYBIND11_MAKE_OPAQUE(std::vector); +#endif diff --git a/python/gtsam/preamble/linear.h b/python/gtsam/preamble/linear.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/navigation.h b/python/gtsam/preamble/navigation.h new file mode 100644 index 000000000..b647ef029 --- /dev/null +++ b/python/gtsam/preamble/navigation.h @@ -0,0 +1,18 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. +// We should find a way to NOT do this. +namespace std { +using gtsam::operator<<; +} diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sam.h b/python/gtsam/preamble/sam.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/sfm.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h new file mode 100644 index 000000000..34dbb4b7a --- /dev/null +++ b/python/gtsam/preamble/slam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector > >); +PYBIND11_MAKE_OPAQUE( + std::vector > >); diff --git a/python/gtsam/preamble/symbolic.h b/python/gtsam/preamble/symbolic.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h deleted file mode 100644 index be8eb8a6c..000000000 --- a/python/gtsam/specializations.h +++ /dev/null @@ -1,35 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - * - * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but - * without the `` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this - * allows the types to be modified with Python, and saves one copy operation. - */ -#ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); -py::implicitly_convertible > >(); -#else -py::bind_vector >(m_, "KeyVector"); -py::implicitly_convertible >(); -#endif - -py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "Point3Pairs"); -py::bind_vector >(m_, "Pose3Pairs"); -py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); -py::bind_vector > > >(m_, "BetweenFactorPose2s"); -py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); -py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); -py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector > >(m_, "CameraSetCal3_S2"); -py::bind_vector > >(m_, "CameraSetCal3Bundler"); -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h new file mode 100644 index 000000000..9eefdeca8 --- /dev/null +++ b/python/gtsam/specializations/base.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); + +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h new file mode 100644 index 000000000..e11d3cc4f --- /dev/null +++ b/python/gtsam/specializations/geometry.h @@ -0,0 +1,27 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector>>( + m_, "Point2Vector"); +py::bind_vector>(m_, "Point3Pairs"); +py::bind_vector>(m_, "Pose3Pairs"); +py::bind_vector>(m_, "Pose3Vector"); +py::bind_vector>>( + m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3Bundler"); +py::bind_vector>>( + m_, "CameraSetCal3Unified"); +py::bind_vector>>( + m_, "CameraSetCal3Fisheye"); diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h new file mode 100644 index 000000000..490d71902 --- /dev/null +++ b/python/gtsam/specializations/gtsam.h @@ -0,0 +1,20 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +#ifdef GTSAM_ALLOCATOR_TBB +py::bind_vector > >(m_, "KeyVector"); +py::implicitly_convertible > >(); +#else +py::bind_vector >(m_, "KeyVector"); +py::implicitly_convertible >(); +#endif diff --git a/python/gtsam/specializations/linear.h b/python/gtsam/specializations/linear.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/navigation.h b/python/gtsam/specializations/navigation.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/navigation.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sam.h b/python/gtsam/specializations/sam.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h new file mode 100644 index 000000000..6de15217f --- /dev/null +++ b/python/gtsam/specializations/sfm.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector > >( + m_, "BinaryMeasurementsUnit3"); +py::bind_map(m_, "KeyPairDoubleMap"); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h new file mode 100644 index 000000000..198485a72 --- /dev/null +++ b/python/gtsam/specializations/slam.h @@ -0,0 +1,19 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose3s"); +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose2s"); diff --git a/python/gtsam/specializations/symbolic.h b/python/gtsam/specializations/symbolic.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py new file mode 100644 index 000000000..298c6e57b --- /dev/null +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -0,0 +1,133 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Fisheye unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +Refactored: Roderick Koehle +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P + +class TestCal3Fisheye(GtsamTestCase): + + @classmethod + def setUpClass(cls): + """ + Equidistant fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = tan(theta), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + u, v = np.arctan2(x, z), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + + def test_Cal3Fisheye(self): + K = gtsam.Cal3Fisheye() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fy(), 1.) + + def test_distortion(self): + """Fisheye distortion and rectification""" + equidistant = gtsam.Cal3Fisheye() + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = equidistant.uncalibrate(perspective_pt) + rectified_pt = equidistant.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) + + def test_pinhole(self): + """Spatial equidistant camera projection""" + camera = gtsam.PinholeCameraCal3Fisheye() + pt1 = camera.Project(self.obj_point) # Perspective projection + pt2 = camera.project(self.obj_point) # Equidistant projection + x, y, z = self.obj_point + obj1 = camera.backproject(self.img_point, z) + r1 = camera.range(self.obj_point) + r = np.linalg.norm(self.obj_point) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + self.gtsamAssertEquals(pt2, self.img_point) + self.gtsamAssertEquals(obj1, self.obj_point) + self.assertEqual(r1, r) + + def test_generic_factor(self): + """Evaluate residual using pose and point as state variables""" + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = gtsam.Cal3Fisheye() + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + """Evaluate residual with camera, pose and point as state variables""" + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = gtsam.Cal3Fisheye() + state.insert_cal3fisheye(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation_skipped(self): + """Estimate spatial point from image measurements""" + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(triangulated, self.origin) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + shared_cal = gtsam.Cal3_S2() + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(triangulated, self.origin) + + def test_retract(self): + expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) + k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') + actual = k.retract(d) + self.gtsamAssertEquals(actual, expected) + np.testing.assert_allclose(d, k.localCoordinates(actual)) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index fbf5f3565..dab1ae446 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -14,15 +14,122 @@ import numpy as np import gtsam from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): + @classmethod + def setUpClass(cls): + """ + Stereographic fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = 2*tan(theta/2), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2*x/(z+r), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) + def test_distortion(self): + """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)""" + x, y, z = self.obj_point + r = np.linalg.norm([x, y, z]) + # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r)) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = self.stereographic.uncalibrate(perspective_pt) + rectified_pt = self.stereographic.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) + + def test_pinhole(self): + """Spatial stereographic camera projection""" + x, y, z = self.obj_point + u, v = self.img_point + r = np.linalg.norm(self.obj_point) + pose = gtsam.Pose3() + camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic) + pt1 = camera.Project(self.obj_point) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + pt2 = camera.project(self.obj_point) + self.gtsamAssertEquals(pt2, self.img_point) + obj1 = camera.backproject(self.img_point, z) + self.gtsamAssertEquals(obj1, self.obj_point) + r1 = camera.range(self.obj_point) + self.assertEqual(r1, r) + + def test_generic_factor(self): + """Evaluate residual using pose and point as state variables""" + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = self.stereographic + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + """Evaluate residual with camera, pose and point as state variables""" + r = np.linalg.norm(self.obj_point) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = self.stereographic + state.insert_cal3unified(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, self.obj_point) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation(self): + """Estimate spatial point from image measurements""" + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(triangulated, self.origin) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + shared_cal = gtsam.Cal3_S2() + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(triangulated, self.origin) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 4c423574d..19b9f8dc1 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,14 +13,27 @@ Author: Frank Dellaert import unittest import gtsam -from gtsam import ShonanAveraging3, ShonanAveragingParameters3 +import numpy as np +from gtsam import ( + BetweenFactorPose2, + LevenbergMarquardtParams, + Rot2, + Pose2, + ShonanAveraging2, + ShonanAveragingParameters2, + ShonanAveraging3, + ShonanAveragingParameters3, +) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( - gtsam.LevenbergMarquardtParams.CeresDefaults()) + gtsam.LevenbergMarquardtParams.CeresDefaults() +) -def fromExampleName(name: str, parameters=DEFAULT_PARAMS): +def fromExampleName( + name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS +) -> ShonanAveraging3: g2oFile = gtsam.findExampleDataFile(name) return ShonanAveraging3(g2oFile, parameters) @@ -133,7 +146,64 @@ class TestShonanAveraging(GtsamTestCase): self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + + + def test_constructorBetweenFactorPose2s(self) -> None: + """Check if ShonanAveraging2 constructor works when not initialized from g2o file. + + GT pose graph: + + | cam 1 = (0,4) + --o + | . + . . + . . + | | + o-- ... o-- + cam 0 cam 2 = (4,0) + (0,0) + """ + num_images = 3 + + wTi_list = [ + Pose2(Rot2.fromDegrees(0), np.array([0, 0])), + Pose2(Rot2.fromDegrees(90), np.array([0, 4])), + Pose2(Rot2.fromDegrees(0), np.array([4, 0])), + ] + + edges = [(0, 1), (1, 2), (0, 2)] + i2Ri1_dict = { + (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation() + for (i1, i2) in edges + } + + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters2(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + + noise_model = gtsam.noiseModel.Unit.Create(3) + between_factors = gtsam.BetweenFactorPose2s() + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + i2Ti1 = Pose2(i2Ri1, np.zeros(2)) + between_factors.append( + BetweenFactorPose2(i2, i1, i2Ti1, noise_model) + ) + + obj = ShonanAveraging2(between_factors, shonan_params) + initial = obj.initializeRandomly() + result_values, _ = obj.run(initial, min_p=2, max_p=100) + + wRi_list = [result_values.atRot2(i) for i in range(num_images)] + thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) + + # map all angles to [0,360) + thetas_deg = thetas_deg % 360 + thetas_deg -= thetas_deg[0] + + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index c1033ba43..aa7ac6bdb 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -16,7 +16,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam_unstable.i` {includes} diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index b0978feb9..ec41bf678 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -29,14 +29,6 @@ #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include // for 'list_of()' #include #include diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index c31baeadf..66dbed1eb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -17,20 +17,19 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include -#include -#include -#include +#include +#include #include #include +#include #include -#include - -#include +#include +#include +#include #include using boost::assign::list_of; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -59,40 +58,42 @@ Point2_ p(2); TEST(ExpressionFactor, Leaf) { using namespace leaf; - // Create old-style factor to create expected value and derivatives + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. 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(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ -// non-zero noise model +// Test leaf expression with noise model of different variance. TEST(ExpressionFactor, Model) { using namespace leaf; SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - // Create old-style factor to create expected value and derivatives + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor f(model, Point2(0, 0), p); - // Check values and derivatives + // 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(assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ -// Constrained noise model +// Test leaf expression with constrained noise model. TEST(ExpressionFactor, Constrained) { using namespace leaf; @@ -106,7 +107,7 @@ TEST(ExpressionFactor, Constrained) { 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(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ @@ -130,7 +131,7 @@ TEST(ExpressionFactor, Unary) { boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); + EXPECT(assert_equal(expected, *jf, 1e-9)); } /* ************************************************************************* */ @@ -143,11 +144,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { if (H) *H << I_3x3, I_3x3, I_3x3; return v; } + typedef Eigen::Matrix Matrix9; Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } + TEST(ExpressionFactor, Wide) { // Create some values Values values; @@ -208,6 +211,7 @@ TEST(ExpressionFactor, Binary) { EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } + /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) TEST(ExpressionFactor, Shallow) { @@ -264,7 +268,7 @@ TEST(ExpressionFactor, Shallow) { EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); } /* ************************************************************************* */ @@ -297,7 +301,7 @@ TEST(ExpressionFactor, tree) { EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); + EXPECT(assert_equal(*expected, *gf, 1e-9)); // Concise version ExpressionFactor f2(model, measured, @@ -305,14 +309,14 @@ TEST(ExpressionFactor, tree) { EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f3.dim()); boost::shared_ptr gf3 = f3.linearize(values); - EXPECT( assert_equal(*expected, *gf3, 1e-9)); + EXPECT(assert_equal(*expected, *gf3, 1e-9)); } /* ************************************************************************* */ @@ -333,15 +337,15 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -363,14 +367,14 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); + EXPECT(assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -392,14 +396,14 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -435,16 +439,16 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); - EXPECT( assert_equal(I_3x3, H[2],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[2],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } TEST(ExpressionFactor, tree_finite_differences) { @@ -619,9 +623,10 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { Matrix3 A; const Vector Ab = f(a, b, H1, A); CHECK(assert_equal(A * b, Ab)); - CHECK(assert_equal(numericalDerivative11( - boost::bind(f, _1, b, boost::none, boost::none), a), - H1)); + CHECK(assert_equal( + numericalDerivative11( + std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a), + H1)); Values values; values.insert(0, a); @@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { class TestNaryFactor : public gtsam::ExpressionFactorN { + gtsam::Rot3, gtsam::Point3> { private: using This = TestNaryFactor; using Base = diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 02f6ed762..2bc381f7a 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -21,10 +21,12 @@ #include #include +#include #include #include +using namespace std::placeholders; using namespace gtsam; // Convenience for named keys @@ -44,7 +46,7 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(boost::bind(simulated3D::prior, _1, boost::none),x); + Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); Matrix computed; simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); @@ -53,13 +55,19 @@ TEST( simulated3D, Dprior ) /* ************************************************************************* */ TEST( simulated3D, DOdo ) { - Point3 x1(1,-9,7),x2(-5,6,7); - Matrix H1,H2; - simulated3D::odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A2,H2,1e-9)); + Point3 x1(1, -9, 7), x2(-5, 6, 7); + Matrix H1, H2; + simulated3D::odo(x1, x2, H1, H2); + Matrix A1 = numericalDerivative21( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A1, H1, 1e-9)); + Matrix A2 = numericalDerivative22( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A2, H2, 1e-9)); } diff --git a/wrap/.github/workflows/linux-ci.yml b/wrap/.github/workflows/linux-ci.yml index 0ca9ba8f5..34623385e 100644 --- a/wrap/.github/workflows/linux-ci.yml +++ b/wrap/.github/workflows/linux-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/.github/workflows/macos-ci.yml b/wrap/.github/workflows/macos-ci.yml index b0ccb3fbe..3910d28d8 100644 --- a/wrap/.github/workflows/macos-ci.yml +++ b/wrap/.github/workflows/macos-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/.gitignore b/wrap/.gitignore index 8e2bafa7a..9f79deafa 100644 --- a/wrap/.gitignore +++ b/wrap/.gitignore @@ -8,4 +8,4 @@ __pycache__/ # Files related to code coverage stats **/.coverage -gtwrap/matlab_wrapper.tpl +gtwrap/matlab_wrapper/matlab_wrapper.tpl diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 9e03da060..2a11a760d 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -58,7 +58,7 @@ if(NOT DEFINED GTWRAP_INCLUDE_NAME) endif() configure_file(${PROJECT_SOURCE_DIR}/templates/matlab_wrapper.tpl.in - ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper.tpl) + ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper/matlab_wrapper.tpl) # Install the gtwrap python package as a directory so it can be found by CMake # for wrapping. diff --git a/wrap/DOCS.md b/wrap/DOCS.md index 8537ddd27..c8285baef 100644 --- a/wrap/DOCS.md +++ b/wrap/DOCS.md @@ -192,12 +192,14 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class. This will cause an ambiguity problem in the Pybind header file. +- Splitting wrapper over multiple files + - The Pybind11 wrapper supports splitting the wrapping code over multiple files. + - To be able to use classes from another module, simply import the C++ header file in that wrapper file. + - Unfortunately, this means that aliases can no longer be used. + - Similarly, there can be multiple `preamble.h` and `specializations.h` files. Each of these should match the module file name. ### TODO -- Default values for arguments. - - WORKAROUND: make multiple versions of the same function for different configurations of default arguments. - Handle `gtsam::Rot3M` conversions to quaternions. - Parse return of const ref arguments. - Parse `std::string` variants and convert directly to special string. -- Add enum support. - Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load. diff --git a/wrap/README.md b/wrap/README.md index 442fc2f93..a04a2ef2d 100644 --- a/wrap/README.md +++ b/wrap/README.md @@ -29,8 +29,10 @@ Using `wrap` in your project is straightforward from here. In your `CMakeLists.t ```cmake find_package(gtwrap) +set(interface_files ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h) + pybind_wrap(${PROJECT_NAME}_py # target - ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file + "${interface_files}" # list of interface header files "${PROJECT_NAME}.cpp" # the generated cpp "${PROJECT_NAME}" # module_name "${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 331dfff8c..f341c2f98 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) message(STATUS "Setting Python version for wrapper") set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) -# User-friendly Pybind11 wrapping and installing function. -# Builds a Pybind11 module from the provided interface_header. -# For example, for the interface header gtsam.h, this will -# build the wrap module 'gtsam_py.cc'. +# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11 +# module from the provided interface_headers. For example, for the interface +# header gtsam.h, this will build the wrap module 'gtsam_py.cc'. # # Arguments: # ~~~ # target: The Make target -# interface_header: The relative path to the wrapper interface definition file. +# interface_headers: List of paths to the wrapper interface definition files. The top level interface file should be first. # generated_cpp: The name of the cpp file which is generated from the tpl file. # module_name: The name of the Python module to use. # top_namespace: The C++ namespace under which the code to be wrapped exists. @@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) # libs: Libraries to link with. # dependencies: Dependencies which need to be built before the wrapper. # use_boost (optional): Flag indicating whether to include Boost. -function(pybind_wrap - target - interface_header - generated_cpp - module_name - top_namespace - ignore_classes - module_template - libs - dependencies) +function( + pybind_wrap + target + interface_headers + generated_cpp + module_name + top_namespace + ignore_classes + module_template + libs + dependencies) set(ExtraMacroArgs ${ARGN}) list(GET ExtraMacroArgs 0 USE_BOOST) if(USE_BOOST) @@ -49,57 +49,62 @@ function(pybind_wrap set(_WRAP_BOOST_ARG "") endif(USE_BOOST) - if (UNIX) + if(UNIX) set(GTWRAP_PATH_SEPARATOR ":") else() set(GTWRAP_PATH_SEPARATOR ";") endif() - add_custom_command(OUTPUT ${generated_cpp} - COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} - ${PYBIND_WRAP_SCRIPT} - --src - ${interface_header} - --out - ${generated_cpp} - --module_name - ${module_name} - --top_module_namespaces - "${top_namespace}" - --ignore - ${ignore_classes} - --template - ${module_template} - ${_WRAP_BOOST_ARG} - DEPENDS ${interface_header} ${module_template} - VERBATIM) - add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp}) + # Convert .i file names to .cpp file names. + foreach(filepath ${interface_headers}) + get_filename_component(interface ${filepath} NAME) + string(REPLACE ".i" ".cpp" cpp_file ${interface}) + list(APPEND cpp_files ${cpp_file}) + endforeach() + + add_custom_command( + OUTPUT ${cpp_files} + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}" + --out "${generated_cpp}" --module_name ${module_name} + --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes} + --template ${module_template} ${_WRAP_BOOST_ARG} + DEPENDS "${interface_headers}" ${module_template} + VERBATIM) + + add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files}) # Late dependency injection, to make sure this gets called whenever the # interface header or the wrap library are updated. # ~~~ # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes # ~~~ - add_custom_command(OUTPUT ${generated_cpp} - DEPENDS ${interface_header} - # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py - # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py - # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py - APPEND) + add_custom_command( + OUTPUT ${cpp_files} + DEPENDS ${interface_headers} + # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py + # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py + # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py + APPEND) - pybind11_add_module(${target} ${generated_cpp}) + pybind11_add_module(${target} "${cpp_files}") if(APPLE) - # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly - # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared - # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior. - # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 + # `type_info` objects will become "weak private external" if the templated + # class is initialized implicitly even if we explicitly export them with + # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated + # class will diverge between shared libraries, causing `dynamic_cast` to + # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See + # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 # https://github.com/CppMicroServices/CppMicroServices/pull/82/files # https://www.russellmcc.com/posts/2013-08-03-rtti.html target_compile_options(${target} PRIVATE "-fvisibility-ms-compat") endif() + add_dependencies(${target} pybind_wrap_${module_name}) + if(NOT "${libs}" STREQUAL "") target_link_libraries(${target} PRIVATE "${libs}") endif() @@ -121,10 +126,7 @@ endfunction() # dest_directory: The destination directory to install to. # patterns: list of file patterns to install # ~~~ -function(install_python_scripts - source_directory - dest_directory - patterns) +function(install_python_scripts source_directory dest_directory patterns) set(patterns_args "") set(exclude_patterns "") @@ -144,17 +146,19 @@ function(install_python_scripts # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(DIRECTORY "${source_directory}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" - DESTINATION "${dest_directory}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${dest_directory}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() @@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) set(build_type_tag "") - # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if - # there is one + # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there + # is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(FILES "${source_files}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}") + install( + FILES "${source_files}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}") endforeach() else() install(FILES "${source_files}" DESTINATION "${dest_directory}") @@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder) return() endif() - file(GLOB files - LIST_DIRECTORIES true - RELATIVE "${source_folder}" - "${source_folder}/*") + file( + GLOB files + LIST_DIRECTORIES true + RELATIVE "${source_folder}" + "${source_folder}/*") foreach(path_file ${files}) get_filename_component(folder ${path_file} PATH) get_filename_component(ext ${path_file} EXT) set(ignored_ext ".tpl" ".h") - list (FIND ignored_ext "${ext}" _index) - if (${_index} GREATER -1) + list(FIND ignored_ext "${ext}" _index) + if(${_index} GREATER -1) continue() - endif () + endif() # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") @@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder) endif() # cmake-format: on - execute_process(COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) + execute_process( + COMMAND ${command} + RESULT_VARIABLE result + ERROR_VARIABLE output) if(NOT ${result} EQUAL 0) message( diff --git a/wrap/gtwrap/interface_parser/__init__.py b/wrap/gtwrap/interface_parser/__init__.py index 0f87eaaa9..3be52d7d9 100644 --- a/wrap/gtwrap/interface_parser/__init__.py +++ b/wrap/gtwrap/interface_parser/__init__.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae import sys -import pyparsing +import pyparsing # type: ignore from .classes import * from .declaration import * diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index ea7a3b3c3..3e6a0fc3c 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore +from pyparsing import Literal, Optional, ZeroOrMore # type: ignore from .enum import Enum from .function import ArgumentList, ReturnType @@ -233,7 +233,7 @@ class Class: self.static_methods = [] self.properties = [] self.operators = [] - self.enums = [] + self.enums: List[Enum] = [] for m in members: if isinstance(m, Constructor): self.ctors.append(m) @@ -274,7 +274,7 @@ class Class: def __init__( self, - template: Template, + template: Union[Template, None], is_virtual: str, name: str, parent_class: list, @@ -292,16 +292,16 @@ class Class: if parent_class: # If it is in an iterable, extract the parent class. if isinstance(parent_class, Iterable): - parent_class = parent_class[0] + parent_class = parent_class[0] # type: ignore # If the base class is a TemplatedType, # we want the instantiated Typename if isinstance(parent_class, TemplatedType): - parent_class = parent_class.typename + parent_class = parent_class.typename # type: ignore self.parent_class = parent_class else: - self.parent_class = '' + self.parent_class = '' # type: ignore self.ctors = ctors self.methods = methods diff --git a/wrap/gtwrap/interface_parser/declaration.py b/wrap/gtwrap/interface_parser/declaration.py index 292d6aeaa..f47ee6e05 100644 --- a/wrap/gtwrap/interface_parser/declaration.py +++ b/wrap/gtwrap/interface_parser/declaration.py @@ -10,7 +10,7 @@ Classes and rules for declarations such as includes and forward declarations. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import CharsNotIn, Optional +from pyparsing import CharsNotIn, Optional # type: ignore from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON, VIRTUAL) diff --git a/wrap/gtwrap/interface_parser/enum.py b/wrap/gtwrap/interface_parser/enum.py index fca7080ef..265e1ad61 100644 --- a/wrap/gtwrap/interface_parser/enum.py +++ b/wrap/gtwrap/interface_parser/enum.py @@ -10,7 +10,7 @@ Parser class and rules for parsing C++ enums. Author: Varun Agrawal """ -from pyparsing import delimitedList +from pyparsing import delimitedList # type: ignore from .tokens import ENUM, IDENT, LBRACE, RBRACE, SEMI_COLON from .type import Typename diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index 3b9a5d4ad..995aba10e 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -10,9 +10,9 @@ Parser classes and rules for parsing C++ functions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from typing import Iterable, List, Union +from typing import Any, Iterable, List, Union -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .template import Template from .tokens import (COMMA, DEFAULT_ARG, EQUAL, IDENT, LOPBRACK, LPAREN, PAIR, @@ -42,12 +42,12 @@ class Argument: name: str, default: ParseResults = None): if isinstance(ctype, Iterable): - self.ctype = ctype[0] + self.ctype = ctype[0] # type: ignore else: self.ctype = ctype self.name = name self.default = default - self.parent = None # type: Union[ArgumentList, None] + self.parent: Union[ArgumentList, None] = None def __repr__(self) -> str: return self.to_cpp() @@ -70,7 +70,7 @@ class ArgumentList: arg.parent = self # The parent object which contains the argument list # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None @staticmethod def from_parse_result(parse_result: ParseResults): @@ -123,7 +123,7 @@ class ReturnType: self.type2 = type2 # The parent object which contains the return type # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None def is_void(self) -> bool: """ diff --git a/wrap/gtwrap/interface_parser/module.py b/wrap/gtwrap/interface_parser/module.py index 6412098b8..7912c40d5 100644 --- a/wrap/gtwrap/interface_parser/module.py +++ b/wrap/gtwrap/interface_parser/module.py @@ -12,7 +12,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments -from pyparsing import ParseResults, ZeroOrMore, cppStyleComment, stringEnd +from pyparsing import (ParseResults, ZeroOrMore, # type: ignore + cppStyleComment, stringEnd) from .classes import Class from .declaration import ForwardDeclaration, Include diff --git a/wrap/gtwrap/interface_parser/namespace.py b/wrap/gtwrap/interface_parser/namespace.py index 575d98237..9c135ffe8 100644 --- a/wrap/gtwrap/interface_parser/namespace.py +++ b/wrap/gtwrap/interface_parser/namespace.py @@ -14,7 +14,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import List, Union -from pyparsing import Forward, ParseResults, ZeroOrMore +from pyparsing import Forward, ParseResults, ZeroOrMore # type: ignore from .classes import Class, collect_namespaces from .declaration import ForwardDeclaration, Include @@ -93,7 +93,7 @@ class Namespace: return Namespace(t.name, content) def find_class_or_function( - self, typename: Typename) -> Union[Class, GlobalFunction]: + self, typename: Typename) -> Union[Class, GlobalFunction, ForwardDeclaration]: """ Find the Class or GlobalFunction object given its typename. We have to traverse the tree of namespaces. @@ -115,7 +115,7 @@ class Namespace: return res[0] def top_level(self) -> "Namespace": - """Return the top leve namespace.""" + """Return the top level namespace.""" if self.name == '' or self.parent == '': return self else: diff --git a/wrap/gtwrap/interface_parser/template.py b/wrap/gtwrap/interface_parser/template.py index dc9d0ce44..fd9de830a 100644 --- a/wrap/gtwrap/interface_parser/template.py +++ b/wrap/gtwrap/interface_parser/template.py @@ -12,11 +12,11 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import List -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .tokens import (EQUAL, IDENT, LBRACE, LOPBRACK, RBRACE, ROPBRACK, SEMI_COLON, TEMPLATE, TYPEDEF) -from .type import Typename, TemplatedType +from .type import TemplatedType, Typename class Template: diff --git a/wrap/gtwrap/interface_parser/tokens.py b/wrap/gtwrap/interface_parser/tokens.py index 4eba95900..0f8d38d86 100644 --- a/wrap/gtwrap/interface_parser/tokens.py +++ b/wrap/gtwrap/interface_parser/tokens.py @@ -10,9 +10,9 @@ All the token definitions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress, - Word, alphanums, alphas, nestedExpr, nums, - originalTextFor, printables) +from pyparsing import (Keyword, Literal, OneOrMore, Or, # type: ignore + QuotedString, Suppress, Word, alphanums, alphas, + nestedExpr, nums, originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) diff --git a/wrap/gtwrap/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index b9f2bd8f7..0b9be6501 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -14,7 +14,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Iterable, List, Union -from pyparsing import Forward, Optional, Or, ParseResults, delimitedList +from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore + delimitedList) from .tokens import (BASIS_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF, ROPBRACK, SHARED_POINTER) @@ -48,7 +49,7 @@ class Typename: def __init__(self, t: ParseResults, - instantiations: Union[tuple, list, str, ParseResults] = ()): + instantiations: Iterable[ParseResults] = ()): self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] diff --git a/wrap/gtwrap/interface_parser/variable.py b/wrap/gtwrap/interface_parser/variable.py index fcb02666f..3779cf74f 100644 --- a/wrap/gtwrap/interface_parser/variable.py +++ b/wrap/gtwrap/interface_parser/variable.py @@ -10,7 +10,9 @@ Parser classes and rules for parsing C++ variables. Author: Varun Agrawal, Gerry Chen """ -from pyparsing import Optional, ParseResults +from typing import List + +from pyparsing import Optional, ParseResults # type: ignore from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON from .type import TemplatedType, Type @@ -40,7 +42,7 @@ class Variable: t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, - ctype: Type, + ctype: List[Type], name: str, default: ParseResults = None, parent=''): diff --git a/wrap/gtwrap/matlab_wrapper/__init__.py b/wrap/gtwrap/matlab_wrapper/__init__.py new file mode 100644 index 000000000..f10338c1c --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/__init__.py @@ -0,0 +1,3 @@ +"""Package to wrap C++ code to Matlab via MEX.""" + +from .wrapper import MatlabWrapper diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py new file mode 100644 index 000000000..061cea283 --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -0,0 +1,222 @@ +"""Mixins for reducing the amount of boilerplate in the main wrapper class.""" + +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator + + +class CheckMixin: + """Mixin to provide various checks.""" + # Data types that are primitive types + not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] + # Ignore the namespace for these datatypes + ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] + # Methods that should be ignored + ignore_methods = ['pickle'] + # Methods that should not be wrapped directly + whitelist = ['serializable', 'serialize'] + # Datatypes that do not need to be checked in methods + not_check_type: list = [] + + def _has_serialization(self, cls): + for m in cls.methods: + if m.name in self.whitelist: + return True + return False + + def is_shared_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + shared pointer in the wrapper. + """ + return arg_type.is_shared_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + raw pointer in the wrapper. + """ + return arg_type.is_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ref(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + reference in the wrapper. + """ + return arg_type.typename.name not in self.ignore_namespace and \ + arg_type.typename.name not in self.not_ptr_type and \ + arg_type.is_ref + + +class FormatMixin: + """Mixin to provide formatting utilities.""" + def _clean_class_name(self, instantiated_class): + """Reformatted the C++ class name to fit Matlab defined naming + standards + """ + if len(instantiated_class.ctors) != 0: + return instantiated_class.ctors[0].name + + return instantiated_class.name + + def _format_type_name(self, + type_name, + separator='::', + include_namespace=True, + constructor=False, + method=False): + """ + Args: + type_name: an interface_parser.Typename to reformat + separator: the statement to add between namespaces and typename + include_namespace: whether to include namespaces when reformatting + constructor: if the typename will be in a constructor + method: if the typename will be in a method + + Raises: + constructor and method cannot both be true + """ + if constructor and method: + raise ValueError( + 'Constructor and method parameters cannot both be True') + + formatted_type_name = '' + name = type_name.name + + if include_namespace: + for namespace in type_name.namespaces: + if name not in self.ignore_namespace and namespace != '': + formatted_type_name += namespace + separator + + if constructor: + formatted_type_name += self.data_type.get(name) or name + elif method: + formatted_type_name += self.data_type_param.get(name) or name + else: + formatted_type_name += name + + if separator == "::": # C++ + templates = [] + for idx in range(len(type_name.instantiations)): + template = '{}'.format( + self._format_type_name(type_name.instantiations[idx], + include_namespace=include_namespace, + constructor=constructor, + method=method)) + templates.append(template) + + if len(templates) > 0: # If there are no templates + formatted_type_name += '<{}>'.format(','.join(templates)) + + else: + for idx in range(len(type_name.instantiations)): + formatted_type_name += '{}'.format( + self._format_type_name(type_name.instantiations[idx], + separator=separator, + include_namespace=False, + constructor=constructor, + method=method)) + + return formatted_type_name + + def _format_return_type(self, + return_type, + include_namespace=False, + separator="::"): + """Format return_type. + + Args: + return_type: an interface_parser.ReturnType to reformat + include_namespace: whether to include namespaces when reformatting + """ + return_wrap = '' + + if self._return_count(return_type) == 1: + return_wrap = self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace) + else: + return_wrap = 'pair< {type1}, {type2} >'.format( + type1=self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace), + type2=self._format_type_name( + return_type.type2.typename, + separator=separator, + include_namespace=include_namespace)) + + return return_wrap + + def _format_class_name(self, instantiated_class, separator=''): + """Format a template_instantiator.InstantiatedClass name.""" + if instantiated_class.parent == '': + parent_full_ns = [''] + else: + parent_full_ns = instantiated_class.parent.full_namespaces() + # class_name = instantiated_class.parent.name + # + # if class_name != '': + # class_name += separator + # + # class_name += instantiated_class.name + parentname = "".join([separator + x + for x in parent_full_ns]) + separator + + class_name = parentname[2 * len(separator):] + + class_name += instantiated_class.name + + return class_name + + def _format_static_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.StaticMethod): + method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ + separator + static_method.parent.name + separator + + return method[2 * len(separator):] + + def _format_instance_method(self, instance_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(instance_method, instantiator.InstantiatedMethod): + method_list = [ + separator + x + for x in instance_method.parent.parent.full_namespaces() + ] + method += "".join(method_list) + separator + + method += instance_method.parent.name + separator + method += instance_method.original.name + method += "<" + instance_method.instantiations.to_cpp() + ">" + + return method[2 * len(separator):] + + def _format_global_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.GlobalFunction): + method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ + separator + + return method[2 * len(separator):] diff --git a/wrap/gtwrap/matlab_wrapper/templates.py b/wrap/gtwrap/matlab_wrapper/templates.py new file mode 100644 index 000000000..7aaf8f487 --- /dev/null +++ b/wrap/gtwrap/matlab_wrapper/templates.py @@ -0,0 +1,166 @@ +import textwrap + + +class WrapperTemplate: + """Class to encapsulate string templates for use in wrapper generation""" + boost_headers = textwrap.dedent(""" + #include + #include + #include + """) + + typdef_collectors = textwrap.dedent('''\ + typedef std::set*> Collector_{class_name}; + static Collector_{class_name} collector_{class_name}; + ''') + + delete_obj = textwrap.indent(textwrap.dedent('''\ + {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); + iter != collector_{class_name}.end(); ) {{ + delete *iter; + collector_{class_name}.erase(iter++); + anyDeleted = true; + }} }} + '''), + prefix=' ') + + delete_all_objects = textwrap.dedent(''' + void _deleteAllObjects() + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + bool anyDeleted = false; + {delete_objs} + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n" + "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); + }} + ''') + + rtti_register = textwrap.dedent('''\ + void _{module_name}_RTTIRegister() {{ + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); + if(!alreadyCreated) {{ + std::map types; + + {rtti_classes} + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) {{ + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + }} + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) {{ + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + }} + mxDestroyArray(newAlreadyCreated); + }} + }} + ''') + + collector_function_upcast_from_void = textwrap.dedent('''\ + void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr<{cpp_name}> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; + }}\n + ''') + + class_serialize_method = textwrap.dedent('''\ + function varargout = string_serialize(this, varargin) + % STRING_SERIALIZE usage: string_serialize() : returns string + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_serialize'); + end + end\n + function sobj = saveobj(obj) + % SAVEOBJ Saves the object to a matlab-readable format + sobj = obj.string_serialize(); + end + ''') + + collector_function_serialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("string_serialize",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); + ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << *obj; + out[0] = wrap< string >(out_archive_stream.str()); + """), + prefix=' ') + + collector_function_deserialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); + string serialized = unwrap< string >(in[0]); + istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + Shared output(new {full_name}()); + in_archive >> *output; + out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); + """), + prefix=' ') + + mex_function = textwrap.dedent(''' + void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + _{module_name}_RTTIRegister();\n + int id = unwrap(in[0]);\n + try {{ + switch(id) {{ + {cases} }} + }} catch(const std::exception& e) {{ + mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); + }}\n + std::cout.rdbuf(outbuf); + }} + ''') + + collector_function_shared_return = textwrap.indent(textwrap.dedent('''\ + {{ + boost::shared_ptr<{name}> shared({shared_obj}); + out[{id}] = wrap_shared_ptr(shared,"{name}"); + }}{new_line}'''), + prefix=' ') + + matlab_deserialize = textwrap.indent(textwrap.dedent("""\ + function varargout = string_deserialize(varargin) + % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 + varargout{{1}} = {wrapper}({id}, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_deserialize'); + end + end\n + function obj = loadobj(sobj) + % LOADOBJ Saves the object to a matlab-readable format + obj = {class_name}.string_deserialize(sobj); + end + """), + prefix=' ') diff --git a/wrap/gtwrap/matlab_wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py similarity index 68% rename from wrap/gtwrap/matlab_wrapper.py rename to wrap/gtwrap/matlab_wrapper/wrapper.py index de6221bbc..b040d2731 100755 --- a/wrap/gtwrap/matlab_wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -7,16 +7,19 @@ that Matlab's MEX compiler can use. import os import os.path as osp -import sys import textwrap from functools import partial, reduce from typing import Dict, Iterable, List, Union +from loguru import logger + import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator +from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin +from gtwrap.matlab_wrapper.templates import WrapperTemplate -class MatlabWrapper(object): +class MatlabWrapper(CheckMixin, FormatMixin): """ Wrap the given C++ code into Matlab. Attributes @@ -25,89 +28,75 @@ class MatlabWrapper(object): top_module_namespace: C++ namespace for the top module (default '') ignore_classes: A list of classes to ignore (default []) """ - # Map the data type to its Matlab class. - # Found in Argument.cpp in old wrapper - data_type = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'Vector': 'double', - 'Matrix': 'double', - 'int': 'numeric', - 'size_t': 'numeric', - 'bool': 'logical' - } - # Map the data type into the type used in Matlab methods. - # Found in matlab.h in old wrapper - data_type_param = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'size_t': 'int', - 'int': 'int', - 'double': 'double', - 'Point2': 'double', - 'Point3': 'double', - 'Vector': 'double', - 'Matrix': 'double', - 'bool': 'bool' - } - # Methods that should not be wrapped directly - whitelist = ['serializable', 'serialize'] - # Methods that should be ignored - ignore_methods = ['pickle'] - # Datatypes that do not need to be checked in methods - not_check_type = [] # type: list - # Data types that are primitive types - not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] - # Ignore the namespace for these datatypes - ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] - # The amount of times the wrapper has created a call to geometry_wrapper - wrapper_id = 0 - # Map each wrapper id to what its collector function namespace, class, type, and string format - wrapper_map = {} - # Set of all the includes in the namespace - includes = {} # type: Dict[parser.Include, int] - # Set of all classes in the namespace - classes = [ - ] # type: List[Union[parser.Class, instantiator.InstantiatedClass]] - classes_elems = { - } # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] - # Id for ordering global functions in the wrapper - global_function_id = 0 - # Files and their content - content = [] # type: List[str] - - # Ensure the template file is always picked up from the correct directory. - dir_path = osp.dirname(osp.realpath(__file__)) - with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: - wrapper_file_header = f.read() - def __init__(self, module_name, top_module_namespace='', ignore_classes=()): + super().__init__() + self.module_name = module_name self.top_module_namespace = top_module_namespace self.ignore_classes = ignore_classes self.verbose = False - def _debug(self, message): - if not self.verbose: - return - print(message, file=sys.stderr) + # Map the data type to its Matlab class. + # Found in Argument.cpp in old wrapper + self.data_type = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'Vector': 'double', + 'Matrix': 'double', + 'int': 'numeric', + 'size_t': 'numeric', + 'bool': 'logical' + } + # Map the data type into the type used in Matlab methods. + # Found in matlab.h in old wrapper + self.data_type_param = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'size_t': 'int', + 'int': 'int', + 'double': 'double', + 'Point2': 'double', + 'Point3': 'double', + 'Vector': 'double', + 'Matrix': 'double', + 'bool': 'bool' + } + # The amount of times the wrapper has created a call to geometry_wrapper + self.wrapper_id = 0 + # Map each wrapper id to its collector function namespace, class, type, and string format + self.wrapper_map: Dict = {} + # Set of all the includes in the namespace + self.includes: List[parser.Include] = [] + # Set of all classes in the namespace + self.classes: List[Union[parser.Class, + instantiator.InstantiatedClass]] = [] + self.classes_elems: Dict[Union[parser.Class, + instantiator.InstantiatedClass], + int] = {} + # Id for ordering global functions in the wrapper + self.global_function_id = 0 + # Files and their content + self.content: List[str] = [] - def _add_include(self, include): - self.includes[include] = 0 + # Ensure the template file is always picked up from the correct directory. + dir_path = osp.dirname(osp.realpath(__file__)) + with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: + self.wrapper_file_headers = f.read() - def _add_class(self, instantiated_class): + def add_class(self, instantiated_class): + """Add `instantiated_class` to the list of classes.""" if self.classes_elems.get(instantiated_class) is None: self.classes_elems[instantiated_class] = 0 self.classes.append(instantiated_class) def _update_wrapper_id(self, collector_function=None, id_diff=0): - """Get and define wrapper ids. - + """ + Get and define wrapper ids. Generates the map of id -> collector function. Args: @@ -150,34 +139,6 @@ class MatlabWrapper(object): """ return x + '\n' + ('' if y == '' else ' ') + y - def _is_shared_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - shared pointer in the wrapper. - """ - return arg_type.is_shared_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - raw pointer in the wrapper. - """ - return arg_type.is_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ref(self, arg_type): - """Determine if the interface_parser.Type should be treated as a - reference in the wrapper. - """ - return arg_type.typename.name not in self.ignore_namespace and \ - arg_type.typename.name not in self.not_ptr_type and \ - arg_type.is_ref - def _group_methods(self, methods): """Group overloaded methods together""" method_map = {} @@ -190,181 +151,10 @@ class MatlabWrapper(object): method_map[method.name] = len(method_out) method_out.append([method]) else: - self._debug("[_group_methods] Merging {} with {}".format( - method_index, method.name)) method_out[method_index].append(method) return method_out - def _clean_class_name(self, instantiated_class): - """Reformatted the C++ class name to fit Matlab defined naming - standards - """ - if len(instantiated_class.ctors) != 0: - return instantiated_class.ctors[0].name - - return instantiated_class.name - - @classmethod - def _format_type_name(cls, - type_name, - separator='::', - include_namespace=True, - constructor=False, - method=False): - """ - Args: - type_name: an interface_parser.Typename to reformat - separator: the statement to add between namespaces and typename - include_namespace: whether to include namespaces when reformatting - constructor: if the typename will be in a constructor - method: if the typename will be in a method - - Raises: - constructor and method cannot both be true - """ - if constructor and method: - raise Exception( - 'Constructor and method parameters cannot both be True') - - formatted_type_name = '' - name = type_name.name - - if include_namespace: - for namespace in type_name.namespaces: - if name not in cls.ignore_namespace and namespace != '': - formatted_type_name += namespace + separator - - #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces)) - if constructor: - formatted_type_name += cls.data_type.get(name) or name - elif method: - formatted_type_name += cls.data_type_param.get(name) or name - else: - formatted_type_name += name - - if separator == "::": # C++ - templates = [] - for idx in range(len(type_name.instantiations)): - template = '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - include_namespace=include_namespace, - constructor=constructor, - method=method)) - templates.append(template) - - if len(templates) > 0: # If there are no templates - formatted_type_name += '<{}>'.format(','.join(templates)) - - else: - for idx in range(len(type_name.instantiations)): - formatted_type_name += '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - separator=separator, - include_namespace=False, - constructor=constructor, - method=method)) - - return formatted_type_name - - @classmethod - def _format_return_type(cls, - return_type, - include_namespace=False, - separator="::"): - """Format return_type. - - Args: - return_type: an interface_parser.ReturnType to reformat - include_namespace: whether to include namespaces when reformatting - """ - return_wrap = '' - - if cls._return_count(return_type) == 1: - return_wrap = cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace) - else: - return_wrap = 'pair< {type1}, {type2} >'.format( - type1=cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace), - type2=cls._format_type_name( - return_type.type2.typename, - separator=separator, - include_namespace=include_namespace)) - - return return_wrap - - def _format_class_name(self, instantiated_class, separator=''): - """Format a template_instantiator.InstantiatedClass name.""" - if instantiated_class.parent == '': - parent_full_ns = [''] - else: - parent_full_ns = instantiated_class.parent.full_namespaces() - # class_name = instantiated_class.parent.name - # - # if class_name != '': - # class_name += separator - # - # class_name += instantiated_class.name - parentname = "".join([separator + x - for x in parent_full_ns]) + separator - - class_name = parentname[2 * len(separator):] - - class_name += instantiated_class.name - - return class_name - - def _format_static_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.StaticMethod): - method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ - separator + static_method.parent.name + separator - - return method[2 * len(separator):] - - def _format_instance_method(self, instance_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(instance_method, instantiator.InstantiatedMethod): - method_list = [ - separator + x - for x in instance_method.parent.parent.full_namespaces() - ] - method += "".join(method_list) + separator - - method += instance_method.parent.name + separator - method += instance_method.original.name - method += "<" + instance_method.instantiations.to_cpp() + ">" - - return method[2 * len(separator):] - - def _format_global_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.GlobalFunction): - method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ - separator - - return method[2 * len(separator):] - def _wrap_args(self, args): """Wrap an interface_parser.ArgumentList into a list of arguments. @@ -520,7 +310,7 @@ class MatlabWrapper(object): if params != '': params += ',' - if self._is_ref(arg.ctype): # and not constructor: + if self.is_ref(arg.ctype): # and not constructor: ctype_camel = self._format_type_name(arg.ctype.typename, separator='') body_args += textwrap.indent(textwrap.dedent('''\ @@ -531,7 +321,7 @@ class MatlabWrapper(object): id=arg_id)), prefix=' ') - elif (self._is_shared_ptr(arg.ctype) or self._is_ptr(arg.ctype)) and \ + elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \ arg.ctype.typename.name not in self.ignore_namespace: if arg.ctype.is_shared_ptr: call_type = arg.ctype.is_shared_ptr @@ -665,22 +455,13 @@ class MatlabWrapper(object): return comment - def generate_matlab_wrapper(self): - """Generate the C++ file for the wrapper.""" - file_name = self._wrapper_name() + '.cpp' - - wrapper_file = self.wrapper_file_header - - return file_name, wrapper_file - def wrap_method(self, methods): - """Wrap methods in the body of a class.""" + """ + Wrap methods in the body of a class. + """ if not isinstance(methods, list): methods = [methods] - # for method in methods: - # output = '' - return '' def wrap_methods(self, methods, global_funcs=False, global_ns=None): @@ -697,10 +478,6 @@ class MatlabWrapper(object): continue if global_funcs: - self._debug("[wrap_methods] wrapping: {}..{}={}".format( - method[0].parent.name, method[0].name, - type(method[0].parent.name))) - method_text = self.wrap_global_function(method) self.content.append(("".join([ '+' + x + '/' for x in global_ns.full_namespaces()[1:] @@ -838,11 +615,6 @@ class MatlabWrapper(object): base_obj = '' - if has_parent: - self._debug("class: {} ns: {}".format( - parent_name, - self._format_class_name(inst_class.parent, separator="."))) - if has_parent: base_obj = ' obj = obj@{parent_name}(uint64(5139824614673773682), base_ptr);'.format( parent_name=parent_name) @@ -850,9 +622,6 @@ class MatlabWrapper(object): if base_obj: base_obj = '\n' + base_obj - self._debug("class: {}, name: {}".format( - inst_class.name, self._format_class_name(inst_class, - separator="."))) methods_wrap += textwrap.indent(textwrap.dedent('''\ else error('Arguments do not match any overload of {class_name_doc} constructor'); @@ -1101,27 +870,12 @@ class MatlabWrapper(object): prefix=" ") if serialize: - method_text += textwrap.indent(textwrap.dedent("""\ - function varargout = string_deserialize(varargin) - % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 - varargout{{1}} = {wrapper}({id}, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_deserialize'); - end - end\n - function obj = loadobj(sobj) - % LOADOBJ Saves the object to a matlab-readable format - obj = {class_name}.string_deserialize(sobj); - end - """).format( + method_text += WrapperTemplate.matlab_deserialize.format( class_name=namespace_name + '.' + instantiated_class.name, wrapper=self._wrapper_name(), id=self._update_wrapper_id( (namespace_name, instantiated_class, 'string_deserialize', - 'deserialize'))), - prefix=' ') + 'deserialize'))) return method_text @@ -1213,33 +967,32 @@ class MatlabWrapper(object): return file_name + '.m', content_text - def wrap_namespace(self, namespace, parent=()): + def wrap_namespace(self, namespace): """Wrap a namespace by wrapping all of its components. Args: namespace: the interface_parser.namespace instance of the namespace parent: parent namespace """ - test_output = '' namespaces = namespace.full_namespaces() inner_namespace = namespace.name != '' wrapped = [] - self._debug("wrapping ns: {}, parent: {}".format( - namespace.full_namespaces(), parent)) - matlab_wrapper = self.generate_matlab_wrapper() - self.content.append((matlab_wrapper[0], matlab_wrapper[1])) + cpp_filename = self._wrapper_name() + '.cpp' + self.content.append((cpp_filename, self.wrapper_file_headers)) current_scope = [] namespace_scope = [] for element in namespace.content: if isinstance(element, parser.Include): - self._add_include(element) + self.includes.append(element) + elif isinstance(element, parser.Namespace): - self.wrap_namespace(element, namespaces) + self.wrap_namespace(element) + elif isinstance(element, instantiator.InstantiatedClass): - self._add_class(element) + self.add_class(element) if inner_namespace: class_text = self.wrap_instantiated_class( @@ -1265,7 +1018,7 @@ class MatlabWrapper(object): if isinstance(func, parser.GlobalFunction) ] - test_output += self.wrap_methods(all_funcs, True, global_ns=namespace) + self.wrap_methods(all_funcs, True, global_ns=namespace) return wrapped @@ -1277,16 +1030,12 @@ class MatlabWrapper(object): """Wrap the collector function which returns a shared pointer.""" new_line = '\n' if new_line else '' - return textwrap.indent(textwrap.dedent('''\ - {{ - boost::shared_ptr<{name}> shared({shared_obj}); - out[{id}] = wrap_shared_ptr(shared,"{name}"); - }}{new_line}''').format(name=self._format_type_name( - return_type_name, include_namespace=False), - shared_obj=shared_obj, - id=func_id, - new_line=new_line), - prefix=' ') + return WrapperTemplate.collector_function_shared_return.format( + name=self._format_type_name(return_type_name, + include_namespace=False), + shared_obj=shared_obj, + id=func_id, + new_line=new_line) def wrap_collector_function_return_types(self, return_type, func_id): """ @@ -1296,7 +1045,7 @@ class MatlabWrapper(object): pair_value = 'first' if func_id == 0 else 'second' new_line = '\n' if func_id == 0 else '' - if self._is_shared_ptr(return_type) or self._is_ptr(return_type): + if self.is_shared_ptr(return_type) or self.is_ptr(return_type): shared_obj = 'pairResult.' + pair_value if not (return_type.is_shared_ptr or return_type.is_ptr): @@ -1355,16 +1104,12 @@ class MatlabWrapper(object): method_name = self._format_static_method(method, '::') method_name += method.name - if "MeasureRange" in method_name: - self._debug("method: {}, method: {}, inst: {}".format( - method_name, method.name, method.parent.to_cpp())) - obj = ' ' if return_1_name == 'void' else '' obj += '{}{}({})'.format(obj_start, method_name, params) if return_1_name != 'void': if return_count == 1: - if self._is_shared_ptr(return_1) or self._is_ptr(return_1): + if self.is_shared_ptr(return_1) or self.is_ptr(return_1): sep_method_name = partial(self._format_type_name, return_1.typename, include_namespace=True) @@ -1377,12 +1122,6 @@ class MatlabWrapper(object): shared_obj = '{obj},"{method_name_sep}"'.format( obj=obj, method_name_sep=sep_method_name('.')) else: - self._debug("Non-PTR: {}, {}".format( - return_1, type(return_1))) - self._debug("Inner type is: {}, {}".format( - return_1.typename.name, sep_method_name('.'))) - self._debug("Inner type instantiations: {}".format( - return_1.typename.instantiations)) method_name_sep_dot = sep_method_name('.') shared_obj_template = 'boost::make_shared<{method_name_sep_col}>({obj}),' \ '"{method_name_sep_dot}"' @@ -1417,16 +1156,8 @@ class MatlabWrapper(object): """ Add function to upcast type from void type. """ - return textwrap.dedent('''\ - void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr<{cpp_name}> Shared; - boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); - out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid)); - *reinterpret_cast(mxGetData(out[0])) = self; - }}\n - ''').format(class_name=class_name, cpp_name=cpp_name, id=func_id) + return WrapperTemplate.collector_function_upcast_from_void.format( + class_name=class_name, cpp_name=cpp_name, id=func_id) def generate_collector_function(self, func_id): """ @@ -1610,158 +1341,109 @@ class MatlabWrapper(object): else: next_case = None - mex_function = textwrap.dedent(''' - void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - {{ - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - _{module_name}_RTTIRegister();\n - int id = unwrap(in[0]);\n - try {{ - switch(id) {{ - {cases} }} - }} catch(const std::exception& e) {{ - mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); - }}\n - std::cout.rdbuf(outbuf); - }} - ''').format(module_name=self.module_name, cases=cases) + mex_function = WrapperTemplate.mex_function.format( + module_name=self.module_name, cases=cases) return mex_function - def generate_wrapper(self, namespace): - """Generate the c++ wrapper.""" - # Includes - wrapper_file = self.wrapper_file_header + textwrap.dedent(""" - #include - #include - #include \n - """) - - assert namespace - - includes_list = sorted(list(self.includes.keys()), - key=lambda include: include.header) - - # Check the number of includes. - # If no includes, do nothing, if 1 then just append newline. - # if more than one, concatenate them with newlines. - if len(includes_list) == 0: - pass - elif len(includes_list) == 1: - wrapper_file += (str(includes_list[0]) + '\n') + def get_class_name(self, cls): + """Get the name of the class `cls` taking template instantiations into account.""" + if cls.instantiations: + class_name_sep = cls.name else: - wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), - includes_list) - wrapper_file += '\n' + class_name_sep = cls.to_cpp() - typedef_instances = '\n' - typedef_collectors = '' + class_name = self._format_class_name(cls) + + return class_name, class_name_sep + + def generate_preamble(self): + """ + Generate the preamble of the wrapper file, which includes + the Boost exports, typedefs for collectors, and + the _deleteAllObjects and _RTTIRegister functions. + """ + delete_objs = '' + typedef_instances = [] boost_class_export_guid = '' - delete_objs = textwrap.dedent('''\ - void _deleteAllObjects() - { - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - bool anyDeleted = false; - ''') - rtti_reg_start = textwrap.dedent('''\ - void _{module_name}_RTTIRegister() {{ - const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); - if(!alreadyCreated) {{ - std::map types; - ''').format(module_name=self.module_name) - rtti_reg_mid = '' - rtti_reg_end = textwrap.indent( - textwrap.dedent(''' - mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); - if(!registry) - registry = mxCreateStructMatrix(1, 1, 0, NULL); - typedef std::pair StringPair; - for(const StringPair& rtti_matlab: types) { - int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); - mxSetFieldByNumber(registry, 0, fieldId, matlabName); - } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxDestroyArray(registry); - '''), - prefix=' ') + ' \n' + textwrap.dedent('''\ - mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) - mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); - mxDestroyArray(newAlreadyCreated); - } - } - ''') - ptr_ctor_frag = '' + typedef_collectors = '' + rtti_classes = '' for cls in self.classes: - uninstantiated_name = "::".join( - cls.namespaces()[1:]) + "::" + cls.name - self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name)) - + # Check if class is in ignore list. + # If so, then skip + uninstantiated_name = "::".join(cls.namespaces()[1:] + [cls.name]) if uninstantiated_name in self.ignore_classes: - self._debug("Ignoring: {} -> {}".format( - cls.name, uninstantiated_name)) continue - def _has_serialization(cls): - for m in cls.methods: - if m.name in self.whitelist: - return True - return False + class_name, class_name_sep = self.get_class_name(cls) + # If a class has instantiations, then declare the typedef for each instance if cls.instantiations: cls_insts = '' - for i, inst in enumerate(cls.instantiations): if i != 0: cls_insts += ', ' cls_insts += self._format_type_name(inst) - typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \ + typedef_instances.append('typedef {original_class_name} {class_name_sep};' \ .format(original_class_name=cls.to_cpp(), - class_name_sep=cls.name) + class_name_sep=cls.name)) - class_name_sep = cls.name - class_name = self._format_class_name(cls) + # Get the Boost exports for serialization + if cls.original.namespaces() and self._has_serialization(cls): + boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( + class_name_sep, class_name) - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - else: - class_name_sep = cls.to_cpp() - class_name = self._format_class_name(cls) + # Typedef and declare the collector objects. + typedef_collectors += WrapperTemplate.typdef_collectors.format( + class_name_sep=class_name_sep, class_name=class_name) - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - - typedef_collectors += textwrap.dedent('''\ - typedef std::set*> Collector_{class_name}; - static Collector_{class_name} collector_{class_name}; - ''').format(class_name_sep=class_name_sep, class_name=class_name) - delete_objs += textwrap.indent(textwrap.dedent('''\ - {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); - iter != collector_{class_name}.end(); ) {{ - delete *iter; - collector_{class_name}.erase(iter++); - anyDeleted = true; - }} }} - ''').format(class_name=class_name), - prefix=' ') + # Generate the _deleteAllObjects method + delete_objs += WrapperTemplate.delete_obj.format( + class_name=class_name) if cls.is_virtual: - rtti_reg_mid += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ + class_name, class_name_sep = self.get_class_name(cls) + rtti_classes += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ .format(class_name_sep, class_name) + # Generate the typedef instances string + typedef_instances = "\n".join(typedef_instances) + + # Generate the full deleteAllObjects function + delete_all_objs = WrapperTemplate.delete_all_objects.format( + delete_objs=delete_objs) + + # Generate the full RTTIRegister function + rtti_register = WrapperTemplate.rtti_register.format( + module_name=self.module_name, rtti_classes=rtti_classes) + + return typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, rtti_register + + def generate_wrapper(self, namespace): + """Generate the c++ wrapper.""" + assert namespace, "Namespace if empty" + + # Generate the header includes + includes_list = sorted(self.includes, + key=lambda include: include.header) + includes = textwrap.dedent("""\ + {wrapper_file_headers} + {boost_headers} + {includes_list} + """).format(wrapper_file_headers=self.wrapper_file_headers.strip(), + boost_headers=WrapperTemplate.boost_headers, + includes_list='\n'.join(map(str, includes_list))) + + preamble = self.generate_preamble() + typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, \ + rtti_register = preamble + + ptr_ctor_frag = '' set_next_case = False for idx in range(self.wrapper_id): @@ -1784,24 +1466,20 @@ class MatlabWrapper(object): ptr_ctor_frag += self.wrap_collector_function_upcast_from_void( id_val[1].name, idx, id_val[1].to_cpp()) - wrapper_file += textwrap.dedent('''\ + wrapper_file = textwrap.dedent('''\ + {includes} {typedef_instances} {boost_class_export_guid} {typedefs_collectors} - {delete_objs} if(anyDeleted) - cout << - "WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n" - "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n" - "module, so that your recompiled module is used instead of the old one." << endl; - std::cout.rdbuf(outbuf); - }}\n + {delete_all_objs} {rtti_register} {pointer_constructor_fragment}{mex_function}''') \ - .format(typedef_instances=typedef_instances, + .format(includes=includes, + typedef_instances=typedef_instances, boost_class_export_guid=boost_class_export_guid, typedefs_collectors=typedef_collectors, - delete_objs=delete_objs, - rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end, + delete_all_objs=delete_all_objs, + rtti_register=rtti_register, pointer_constructor_fragment=ptr_ctor_frag, mex_function=self.mex_function()) @@ -1815,23 +1493,10 @@ class MatlabWrapper(object): wrapper_id = self._update_wrapper_id( (namespace_name, inst_class, 'string_serialize', 'serialize')) - return textwrap.dedent('''\ - function varargout = string_serialize(this, varargin) - % STRING_SERIALIZE usage: string_serialize() : returns string - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 0 - varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_serialize'); - end - end\n - function sobj = saveobj(obj) - % SAVEOBJ Saves the object to a matlab-readable format - sobj = obj.string_serialize(); - end - ''').format(wrapper=self._wrapper_name(), - wrapper_id=wrapper_id, - class_name=namespace_name + '.' + class_name) + return WrapperTemplate.class_serialize_method.format( + wrapper=self._wrapper_name(), + wrapper_id=wrapper_id, + class_name=namespace_name + '.' + class_name) def wrap_collector_function_serialize(self, class_name, @@ -1840,18 +1505,8 @@ class MatlabWrapper(object): """ Wrap the serizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("string_serialize",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); - ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << *obj; - out[0] = wrap< string >(out_archive_stream.str()); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_serialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) def wrap_collector_function_deserialize(self, class_name, @@ -1860,87 +1515,85 @@ class MatlabWrapper(object): """ Wrap the deserizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); - string serialized = unwrap< string >(in[0]); - istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new {full_name}()); - in_archive >> *output; - out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_deserialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) - def wrap(self, content): + def generate_content(self, cc_content, path): + """ + Generate files and folders from matlab wrapper content. + + Args: + cc_content: The content to generate formatted as + (file_name, file_content) or + (folder_name, [(file_name, file_content)]) + path: The path to the files parent folder within the main folder + """ + for c in cc_content: + if isinstance(c, list): + if len(c) == 0: + continue + + path_to_folder = osp.join(path, c[0][0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + + for sub_content in c: + self.generate_content(sub_content[1], path_to_folder) + + elif isinstance(c[1], list): + path_to_folder = osp.join(path, c[0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + for sub_content in c[1]: + path_to_file = osp.join(path_to_folder, sub_content[0]) + with open(path_to_file, 'w') as f: + f.write(sub_content[1]) + else: + path_to_file = osp.join(path, c[0]) + + if not osp.isdir(path_to_file): + try: + os.mkdir(path) + except OSError: + pass + + with open(path_to_file, 'w') as f: + f.write(c[1]) + + def wrap(self, files, path): """High level function to wrap the project.""" - # Parse the contents of the interface file - parsed_result = parser.Module.parseString(content) - # Instantiate the module - module = instantiator.instantiate_namespace(parsed_result) - self.wrap_namespace(module) - self.generate_wrapper(module) + modules = {} + for file in files: + with open(file, 'r') as f: + content = f.read() + + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) + # print(parsed_result) + + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) + + if module.name in modules: + modules[module. + name].content[0].content += module.content[0].content + else: + modules[module.name] = module + + for module in modules.values(): + # Wrap the full namespace + self.wrap_namespace(module) + self.generate_wrapper(module) + + # Generate the corresponding .m and .cpp files + self.generate_content(self.content, path) return self.content - - -def generate_content(cc_content, path, verbose=False): - """ - Generate files and folders from matlab wrapper content. - - Args: - cc_content: The content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path: The path to the files parent folder within the main folder - """ - def _debug(message): - if not verbose: - return - print(message, file=sys.stderr) - - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - _debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) - - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - _debug("sub object: {}".format(sub_content[1][0][0])) - generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - _debug("[generate_content_global]: {}".format(path_to_folder)) - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - _debug("[generate_global_method]: {}".format(path_to_file)) - with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - else: - path_to_file = osp.join(path, c[0]) - - _debug("[generate_content]: {}".format(path_to_file)) - if not os.path.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass - - with open(path_to_file, 'w') as f: - f.write(c[1]) diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 0e1b3c7ea..40571263a 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -13,6 +13,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long import re +from pathlib import Path import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -32,7 +33,7 @@ class PybindWrapper: self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes - self._serializing_classes = list() + self._serializing_classes = [] self.module_template = module_template self.python_keywords = [ 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', @@ -160,7 +161,7 @@ class PybindWrapper: 'self->print', 'py::scoped_ostream_redirect output; self->print') - # Make __repr__() call print() internally + # Make __repr__() call .print() internally ret += '''{prefix}.def("__repr__", [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{ gtsam::RedirectCout redirect; @@ -557,8 +558,15 @@ class PybindWrapper: ) return wrapped, includes - def wrap(self, content): - """Wrap the code in the interface file.""" + def wrap_file(self, content, module_name=None, submodules=None): + """ + Wrap the code in the interface file. + + Args: + content: The contents of the interface file. + module_name: The name of the module. + submodules: List of other interface file names that should be linked to. + """ # Parse the contents of the interface file module = parser.Module.parseString(content) # Instantiate all templates @@ -574,23 +582,74 @@ class PybindWrapper: if ',' in cpp_class: new_name = re.sub("[,:<> ]", "", cpp_class) boost_class_export += "typedef {cpp_class} {new_name};\n".format( # noqa - cpp_class=cpp_class, - new_name=new_name, - ) + cpp_class=cpp_class, new_name=new_name) + boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format( new_name=new_name, ) + # Reset the serializing classes list + self._serializing_classes = [] + holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \ "{shared_ptr_type}::shared_ptr);" include_boost = "#include " if self.use_boost else "" + submodules_init = [] + + if submodules is not None: + module_def = "PYBIND11_MODULE({0}, m_)".format(module_name) + + for idx, submodule in enumerate(submodules): + submodules[idx] = "void {0}(py::module_ &);".format(submodule) + submodules_init.append("{0}(m_);".format(submodule)) + + else: + module_def = "void {0}(py::module_ &m_)".format(module_name) + submodules = [] + return self.module_template.format( include_boost=include_boost, - module_name=self.module_name, + module_def=module_def, + module_name=module_name, includes=includes, holder_type=holder_type.format( shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "", wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, + submodules="\n".join(submodules), + submodules_init="\n".join(submodules_init), ) + + def wrap(self, sources, main_output): + """ + Wrap all the source interface files. + + Args: + sources: List of all interface files. + main_output: The name for the main module. + """ + main_module = sources[0] + submodules = [] + for source in sources[1:]: + filename = Path(source).name + module_name = Path(source).stem + # Read in the complete interface (.i) file + with open(source, "r") as f: + content = f.read() + submodules.append(module_name) + cc_content = self.wrap_file(content, module_name=module_name) + + # Generate the C++ code which Pybind11 will use. + with open(filename.replace(".i", ".cpp"), "w") as f: + f.write(cc_content) + + with open(main_module, "r") as f: + content = f.read() + cc_content = self.wrap_file(content, + module_name=self.module_name, + submodules=submodules) + + # Generate the C++ code which Pybind11 will use. + with open(main_output, "w") as f: + f.write(cc_content) diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py index c47424648..87729cfa6 100644 --- a/wrap/gtwrap/template_instantiator.py +++ b/wrap/gtwrap/template_instantiator.py @@ -4,7 +4,7 @@ import itertools from copy import deepcopy -from typing import List +from typing import Iterable, List import gtwrap.interface_parser as parser @@ -29,12 +29,13 @@ def instantiate_type(ctype: parser.Type, ctype = deepcopy(ctype) # Check if the return type has template parameters - if len(ctype.typename.instantiations) > 0: + if ctype.typename.instantiations: for idx, instantiation in enumerate(ctype.typename.instantiations): if instantiation.name in template_typenames: template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[idx] = instantiations[ - template_idx] + ctype.typename.instantiations[ + idx] = instantiations[ # type: ignore + template_idx] return ctype @@ -212,7 +213,9 @@ class InstantiatedMethod(parser.Method): void func(X x, Y y); } """ - def __init__(self, original, instantiations: List[parser.Typename] = ''): + def __init__(self, + original, + instantiations: Iterable[parser.Typename] = ()): self.original = original self.instantiations = instantiations self.template = '' @@ -278,7 +281,7 @@ class InstantiatedClass(parser.Class): self.original = original self.instantiations = instantiations - self.template = '' + self.template = None self.is_virtual = original.is_virtual self.parent_class = original.parent_class self.parent = original.parent @@ -318,7 +321,7 @@ class InstantiatedClass(parser.Class): self.methods = [] for method in instantiated_methods: if not method.template: - self.methods.append(InstantiatedMethod(method, '')) + self.methods.append(InstantiatedMethod(method, ())) else: instantiations = [] # Get all combinations of template parameters @@ -342,9 +345,9 @@ class InstantiatedClass(parser.Class): ) def __repr__(self): - return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\ - "{ctors}\n{static_methods}\n{methods}".format( - virtual="virtual" if self.is_virtual else '', + return "{virtual}Class {cpp_class} : {parent_class}\n"\ + "{ctors}\n{static_methods}\n{methods}\n{operators}".format( + virtual="virtual " if self.is_virtual else '', name=self.name, cpp_class=self.to_cpp(), parent_class=self.parent, diff --git a/wrap/matlab.h b/wrap/matlab.h index 4f3bfe96e..bcdef3c8d 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -26,6 +26,7 @@ #include #include #include +#include using gtsam::Vector; using gtsam::Matrix; diff --git a/wrap/scripts/matlab_wrap.py b/wrap/scripts/matlab_wrap.py index be6043947..0f6664a63 100644 --- a/wrap/scripts/matlab_wrap.py +++ b/wrap/scripts/matlab_wrap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Helper script to wrap C++ to Matlab. This script is installed via CMake to the user's binary directory @@ -7,19 +6,24 @@ and invoked during the wrapping by CMake. """ import argparse -import os import sys -from gtwrap.matlab_wrapper import MatlabWrapper, generate_content +from gtwrap.matlab_wrapper import MatlabWrapper if __name__ == "__main__": arg_parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument("--src", type=str, required=True, + arg_parser.add_argument("--src", + type=str, + required=True, help="Input interface .h file.") - arg_parser.add_argument("--module_name", type=str, required=True, + arg_parser.add_argument("--module_name", + type=str, + required=True, help="Name of the C++ class being wrapped.") - arg_parser.add_argument("--out", type=str, required=True, + arg_parser.add_argument("--out", + type=str, + required=True, help="Name of the output folder.") arg_parser.add_argument( "--top_module_namespaces", @@ -33,28 +37,22 @@ if __name__ == "__main__": "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" ", and `from import ns4` gives you access to a Python " "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ") - arg_parser.add_argument("--ignore", - nargs='*', - type=str, - help="A space-separated list of classes to ignore. " - "Class names must include their full namespaces.") + arg_parser.add_argument( + "--ignore", + nargs='*', + type=str, + help="A space-separated list of classes to ignore. " + "Class names must include their full namespaces.") args = arg_parser.parse_args() top_module_namespaces = args.top_module_namespaces.split("::") if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - with open(args.src, 'r') as f: - content = f.read() - - if not os.path.exists(args.src): - os.mkdir(args.src) - - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) + print("[MatlabWrapper] Ignoring classes: {}".format(args.ignore), file=sys.stderr) wrapper = MatlabWrapper(module_name=args.module_name, top_module_namespace=top_module_namespaces, ignore_classes=args.ignore) - cc_content = wrapper.wrap(content) - - generate_content(cc_content, args.out) + sources = args.src.split(';') + cc_content = wrapper.wrap(sources, path=args.out) diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py index 7f2f8d419..c82a1d24c 100644 --- a/wrap/scripts/pybind_wrap.py +++ b/wrap/scripts/pybind_wrap.py @@ -67,10 +67,6 @@ def main(): if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - # Read in the complete interface (.i) file - with open(args.src, "r") as f: - content = f.read() - with open(args.template, "r") as f: template_content = f.read() @@ -83,11 +79,8 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap(content) - - # Generate the C++ code which Pybind11 will use. - with open(args.out, "w") as f: - f.write(cc_content) + sources = args.src.split(';') + wrapper.wrap(sources, args.out) if __name__ == "__main__": diff --git a/wrap/templates/pybind_wrapper.tpl.example b/wrap/templates/pybind_wrapper.tpl.example index bf5b33490..485aa8d00 100644 --- a/wrap/templates/pybind_wrapper.tpl.example +++ b/wrap/templates/pybind_wrapper.tpl.example @@ -7,7 +7,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. {includes} #include @@ -22,9 +22,13 @@ using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} #include "python/specializations.h" diff --git a/wrap/tests/expected/matlab/+gtsam/Class1.m b/wrap/tests/expected/matlab/+gtsam/Class1.m new file mode 100644 index 000000000..00dd5ca74 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/Class1.m @@ -0,0 +1,36 @@ +%class Class1, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class1() +% +classdef Class1 < handle + properties + ptr_gtsamClass1 = 0 + end + methods + function obj = Class1(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(0, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(1); + else + error('Arguments do not match any overload of gtsam.Class1 constructor'); + end + obj.ptr_gtsamClass1 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(2, obj.ptr_gtsamClass1); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/Class2.m b/wrap/tests/expected/matlab/+gtsam/Class2.m new file mode 100644 index 000000000..93279e156 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/Class2.m @@ -0,0 +1,36 @@ +%class Class2, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class2() +% +classdef Class2 < handle + properties + ptr_gtsamClass2 = 0 + end + methods + function obj = Class2(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(3, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(4); + else + error('Arguments do not match any overload of gtsam.Class2 constructor'); + end + obj.ptr_gtsamClass2 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(5, obj.ptr_gtsamClass2); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/ClassA.m b/wrap/tests/expected/matlab/+gtsam/ClassA.m new file mode 100644 index 000000000..3210e93c6 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/ClassA.m @@ -0,0 +1,36 @@ +%class ClassA, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%ClassA() +% +classdef ClassA < handle + properties + ptr_gtsamClassA = 0 + end + methods + function obj = ClassA(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(6, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(7); + else + error('Arguments do not match any overload of gtsam.ClassA constructor'); + end + obj.ptr_gtsamClassA = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(8, obj.ptr_gtsamClassA); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index e644ac00f..fab9c1450 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -7,7 +7,6 @@ #include - typedef Fun FunDouble; typedef PrimitiveRef PrimitiveRefDouble; typedef MyVector<3> MyVector3; @@ -16,7 +15,6 @@ typedef MultipleTemplates MultipleTemplatesIntDouble; typedef MultipleTemplates MultipleTemplatesIntFloat; typedef MyFactor MyFactorPosePoint2; - typedef std::set*> Collector_FunRange; static Collector_FunRange collector_FunRange; typedef std::set*> Collector_FunDouble; @@ -38,6 +36,7 @@ static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { mstream mout; @@ -104,6 +103,7 @@ void _deleteAllObjects() collector_MyFactorPosePoint2.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -117,24 +117,29 @@ void _class_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp index ae7f49c41..d0f0f8ca6 100644 --- a/wrap/tests/expected/matlab/functions_wrapper.cpp +++ b/wrap/tests/expected/matlab/functions_wrapper.cpp @@ -5,38 +5,11 @@ #include #include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { @@ -44,66 +17,7 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -117,24 +31,29 @@ void _functions_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp index 4d8a7c789..81631390c 100644 --- a/wrap/tests/expected/matlab/geometry_wrapper.cpp +++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp @@ -5,112 +5,25 @@ #include #include -#include #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; static Collector_gtsamPoint2 collector_gtsamPoint2; typedef std::set*> Collector_gtsamPoint3; static Collector_gtsamPoint3 collector_gtsamPoint3; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); iter != collector_gtsamPoint2.end(); ) { delete *iter; @@ -123,6 +36,7 @@ void _deleteAllObjects() collector_gtsamPoint3.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -136,24 +50,29 @@ void _geometry_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index 077df4830..8e61ac8c6 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -5,47 +5,11 @@ #include #include -#include -#include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; + typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; typedef std::set*> Collector_MyBase; static Collector_MyBase collector_MyBase; typedef std::set*> Collector_MyTemplatePoint2; @@ -55,84 +19,13 @@ static Collector_MyTemplateMatrix collector_MyTemplateMatrix; typedef std::set*> Collector_ForwardKinematicsFactor; static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); iter != collector_MyBase.end(); ) { delete *iter; @@ -157,6 +50,7 @@ void _deleteAllObjects() collector_ForwardKinematicsFactor.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -169,42 +63,38 @@ void _inheritance_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_inheritance_rttiRegistry_created"); if(!alreadyCreated) { std::map types; + types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } -void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr Shared; - - Shared *self = *reinterpret_cast (mxGetData(in[0])); - collector_gtsamPoint2.insert(self); -} - void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -214,6 +104,15 @@ void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin collector_MyBase.insert(self); } +void MyBase_upcastFromVoid_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; @@ -227,19 +126,6 @@ void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArr } } -void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - typedef boost::shared_ptr Shared; - checkArguments("delete_gtsamPoint2",nargout,nargin,1); - Shared *self = *reinterpret_cast(mxGetData(in[0])); - Collector_gtsamPoint2::iterator item; - item = collector_gtsamPoint2.find(self); - if(item != collector_gtsamPoint2.end()) { - delete self; - collector_gtsamPoint2.erase(item); - } -} - void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -253,6 +139,15 @@ void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplatePoint2_upcastFromVoid_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void MyTemplatePoint2_constructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -399,20 +294,6 @@ void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplatePoint2", false); } -void gtsamPoint3_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr Shared; - - double x = unwrap< double >(in[0]); - double y = unwrap< double >(in[1]); - double z = unwrap< double >(in[2]); - Shared *self = new Shared(new gtsam::Point3(x,y,z)); - collector_gtsamPoint3.insert(self); - out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - *reinterpret_cast (mxGetData(out[0])) = self; -} - void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -426,6 +307,15 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplateMatrix_upcastFromVoid_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void MyTemplateMatrix_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -572,14 +462,6 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("return_vector2",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Vector value = unwrap< Vector >(in[1]); - out[0] = wrap< Vector >(obj->return_vector2(value)); -} - void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -593,6 +475,15 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; @@ -619,19 +510,19 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) try { switch(id) { case 0: - gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); break; case 1: - MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_upcastFromVoid_1(nargout, out, nargin-1, in+1); break; case 2: MyBase_deconstructor_2(nargout, out, nargin-1, in+1); break; case 3: - gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); break; case 4: - MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_upcastFromVoid_4(nargout, out, nargin-1, in+1); break; case 5: MyTemplatePoint2_constructor_5(nargout, out, nargin-1, in+1); @@ -676,10 +567,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplatePoint2_Level_18(nargout, out, nargin-1, in+1); break; case 19: - gtsamPoint3_constructor_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); break; case 20: - MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_upcastFromVoid_20(nargout, out, nargin-1, in+1); break; case 21: MyTemplateMatrix_constructor_21(nargout, out, nargin-1, in+1); @@ -724,10 +615,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector2_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); break; case 36: - ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1); break; case 37: ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1); diff --git a/wrap/tests/expected/matlab/multiple_files_wrapper.cpp b/wrap/tests/expected/matlab/multiple_files_wrapper.cpp new file mode 100644 index 000000000..66ab7ff73 --- /dev/null +++ b/wrap/tests/expected/matlab/multiple_files_wrapper.cpp @@ -0,0 +1,229 @@ +#include +#include + +#include +#include +#include + + + + + +typedef std::set*> Collector_gtsamClass1; +static Collector_gtsamClass1 collector_gtsamClass1; +typedef std::set*> Collector_gtsamClass2; +static Collector_gtsamClass2 collector_gtsamClass2; +typedef std::set*> Collector_gtsamClassA; +static Collector_gtsamClassA collector_gtsamClassA; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_gtsamClass1::iterator iter = collector_gtsamClass1.begin(); + iter != collector_gtsamClass1.end(); ) { + delete *iter; + collector_gtsamClass1.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClass2::iterator iter = collector_gtsamClass2.begin(); + iter != collector_gtsamClass2.end(); ) { + delete *iter; + collector_gtsamClass2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClassA::iterator iter = collector_gtsamClassA.begin(); + iter != collector_gtsamClassA.end(); ) { + delete *iter; + collector_gtsamClassA.erase(iter++); + anyDeleted = true; + } } + + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _multiple_files_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_multiple_files_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(newAlreadyCreated); + } +} + +void gtsamClass1_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClass1.insert(self); +} + +void gtsamClass1_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class1()); + collector_gtsamClass1.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass1_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass1",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass1::iterator item; + item = collector_gtsamClass1.find(self); + if(item != collector_gtsamClass1.end()) { + delete self; + collector_gtsamClass1.erase(item); + } +} + +void gtsamClass2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClass2.insert(self); +} + +void gtsamClass2_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class2()); + collector_gtsamClass2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass2_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass2::iterator item; + item = collector_gtsamClass2.find(self); + if(item != collector_gtsamClass2.end()) { + delete self; + collector_gtsamClass2.erase(item); + } +} + +void gtsamClassA_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClassA.insert(self); +} + +void gtsamClassA_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::ClassA()); + collector_gtsamClassA.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClassA",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClassA::iterator item; + item = collector_gtsamClassA.find(self); + if(item != collector_gtsamClassA.end()) { + delete self; + collector_gtsamClassA.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _multiple_files_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + gtsamClass1_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + gtsamClass1_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + gtsamClass1_deconstructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + gtsamClass2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + break; + case 4: + gtsamClass2_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + gtsamClass2_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + gtsamClassA_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + gtsamClassA_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + gtsamClassA_deconstructor_8(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected/matlab/namespaces_wrapper.cpp b/wrap/tests/expected/matlab/namespaces_wrapper.cpp index 8f6e415e2..604ede5da 100644 --- a/wrap/tests/expected/matlab/namespaces_wrapper.cpp +++ b/wrap/tests/expected/matlab/namespaces_wrapper.cpp @@ -5,9 +5,6 @@ #include #include -#include -#include -#include #include #include #include @@ -15,51 +12,8 @@ #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; -typedef std::set*> Collector_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; typedef std::set*> Collector_ns1ClassA; static Collector_ns1ClassA collector_ns1ClassA; typedef std::set*> Collector_ns1ClassB; @@ -75,108 +29,13 @@ static Collector_ClassD collector_ClassD; typedef std::set*> Collector_gtsamValues; static Collector_gtsamValues collector_gtsamValues; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); - iter != collector_MyBase.end(); ) { - delete *iter; - collector_MyBase.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); - iter != collector_MyTemplatePoint2.end(); ) { - delete *iter; - collector_MyTemplatePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); iter != collector_ns1ClassA.end(); ) { delete *iter; @@ -219,6 +78,7 @@ void _deleteAllObjects() collector_gtsamValues.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -231,10 +91,8 @@ void _namespaces_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_namespaces_rttiRegistry_created"); if(!alreadyCreated) { std::map types; - types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); - types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); - types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -242,18 +100,21 @@ void _namespaces_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/expected/matlab/special_cases_wrapper.cpp b/wrap/tests/expected/matlab/special_cases_wrapper.cpp index 056ce8097..69abbf73b 100644 --- a/wrap/tests/expected/matlab/special_cases_wrapper.cpp +++ b/wrap/tests/expected/matlab/special_cases_wrapper.cpp @@ -5,78 +5,11 @@ #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; -typedef std::set*> Collector_gtsamPoint2; -static Collector_gtsamPoint2 collector_gtsamPoint2; -typedef std::set*> Collector_gtsamPoint3; -static Collector_gtsamPoint3 collector_gtsamPoint3; -typedef std::set*> Collector_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; -typedef std::set*> Collector_ns1ClassA; -static Collector_ns1ClassA collector_ns1ClassA; -typedef std::set*> Collector_ns1ClassB; -static Collector_ns1ClassB collector_ns1ClassB; -typedef std::set*> Collector_ns2ClassA; -static Collector_ns2ClassA collector_ns2ClassA; -typedef std::set*> Collector_ns2ns3ClassB; -static Collector_ns2ns3ClassB collector_ns2ns3ClassB; -typedef std::set*> Collector_ns2ClassC; -static Collector_ns2ClassC collector_ns2ClassC; -typedef std::set*> Collector_ClassD; -static Collector_ClassD collector_ClassD; -typedef std::set*> Collector_gtsamValues; -static Collector_gtsamValues collector_gtsamValues; typedef std::set*> Collector_gtsamNonlinearFactorGraph; static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph; typedef std::set*> Collector_gtsamSfmTrack; @@ -86,150 +19,13 @@ static Collector_gtsamPinholeCameraCal3Bundler collector_gtsamPinholeCameraCal3B typedef std::set*> Collector_gtsamGeneralSFMFactorCal3Bundler; static Collector_gtsamGeneralSFMFactorCal3Bundler collector_gtsamGeneralSFMFactorCal3Bundler; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_Test::iterator iter = collector_Test.begin(); - iter != collector_Test.end(); ) { - delete *iter; - collector_Test.erase(iter++); - anyDeleted = true; - } } - { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); - iter != collector_gtsamPoint2.end(); ) { - delete *iter; - collector_gtsamPoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); - iter != collector_gtsamPoint3.end(); ) { - delete *iter; - collector_gtsamPoint3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); - iter != collector_MyBase.end(); ) { - delete *iter; - collector_MyBase.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); - iter != collector_MyTemplatePoint2.end(); ) { - delete *iter; - collector_MyTemplatePoint2.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); - iter != collector_ns1ClassA.end(); ) { - delete *iter; - collector_ns1ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); - iter != collector_ns1ClassB.end(); ) { - delete *iter; - collector_ns1ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); - iter != collector_ns2ClassA.end(); ) { - delete *iter; - collector_ns2ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); - iter != collector_ns2ns3ClassB.end(); ) { - delete *iter; - collector_ns2ns3ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); - iter != collector_ns2ClassC.end(); ) { - delete *iter; - collector_ns2ClassC.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ClassD::iterator iter = collector_ClassD.begin(); - iter != collector_ClassD.end(); ) { - delete *iter; - collector_ClassD.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); - iter != collector_gtsamValues.end(); ) { - delete *iter; - collector_gtsamValues.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin(); iter != collector_gtsamNonlinearFactorGraph.end(); ) { delete *iter; @@ -254,6 +50,7 @@ void _deleteAllObjects() collector_gtsamGeneralSFMFactorCal3Bundler.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -266,10 +63,8 @@ void _special_cases_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_special_cases_rttiRegistry_created"); if(!alreadyCreated) { std::map types; - types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); - types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); - types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -277,18 +72,21 @@ void _special_cases_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + if(fieldId < 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); mxSetFieldByNumber(registry, 0, fieldId, matlabName); } - if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); - if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/wrap/tests/fixtures/part1.i b/wrap/tests/fixtures/part1.i new file mode 100644 index 000000000..b69850baf --- /dev/null +++ b/wrap/tests/fixtures/part1.i @@ -0,0 +1,11 @@ +// First file to test for multi-file support. + +namespace gtsam { +class Class1 { + Class1(); +}; + +class Class2 { + Class2(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/wrap/tests/fixtures/part2.i b/wrap/tests/fixtures/part2.i new file mode 100644 index 000000000..29ad86a7f --- /dev/null +++ b/wrap/tests/fixtures/part2.i @@ -0,0 +1,7 @@ +// Second file to test for multi-file support. + +namespace gtsam { +class ClassA { + ClassA(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index b321c4e15..fad4de16a 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -22,73 +22,31 @@ class TestWrap(unittest.TestCase): """ Test the Matlab wrapper """ - TEST_DIR = osp.dirname(osp.realpath(__file__)) - INTERFACE_DIR = osp.join(TEST_DIR, "fixtures") - MATLAB_TEST_DIR = osp.join(TEST_DIR, "expected", "matlab") - MATLAB_ACTUAL_DIR = osp.join(TEST_DIR, "actual", "matlab") + def setUp(self) -> None: + super().setUp() - # Create the `actual/matlab` directory - os.makedirs(MATLAB_ACTUAL_DIR, exist_ok=True) + # Set up all the directories + self.TEST_DIR = osp.dirname(osp.realpath(__file__)) + self.INTERFACE_DIR = osp.join(self.TEST_DIR, "fixtures") + self.MATLAB_TEST_DIR = osp.join(self.TEST_DIR, "expected", "matlab") + self.MATLAB_ACTUAL_DIR = osp.join(self.TEST_DIR, "actual", "matlab") - # set the log level to INFO by default - logger.remove() # remove the default sink - logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") + if not osp.exists(self.MATLAB_ACTUAL_DIR): + os.mkdir(self.MATLAB_ACTUAL_DIR) - def generate_content(self, cc_content, path=MATLAB_ACTUAL_DIR): - """Generate files and folders from matlab wrapper content. + # Generate the matlab.h file if it does not exist + template_file = osp.join(self.TEST_DIR, "..", "gtwrap", + "matlab_wrapper", "matlab_wrapper.tpl") + if not osp.exists(template_file): + with open(template_file, 'w') as tpl: + tpl.write("#include \n#include \n") - Keyword arguments: - cc_content -- the content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path -- the path to the files parent folder within the main folder - """ - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - logger.debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) + # Create the `actual/matlab` directory + os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - logger.debug("sub object: {}".format(sub_content[1][0][0])) - self.generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - logger.debug( - "[generate_content_global]: {}".format(path_to_folder)) - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - logger.debug( - "[generate_global_method]: {}".format(path_to_file)) - with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - - else: - path_to_file = osp.join(path, c[0]) - - logger.debug("[generate_content]: {}".format(path_to_file)) - if not osp.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass - - with open(path_to_file, 'w') as f: - f.write(c[1]) + # set the log level to INFO by default + logger.remove() # remove the default sink + logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") def compare_and_diff(self, file): """ @@ -109,11 +67,7 @@ class TestWrap(unittest.TestCase): python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h --module_name geometry --out wrap/tests/actual-matlab """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'geometry.i') # Create MATLAB wrapper instance wrapper = MatlabWrapper( @@ -122,24 +76,18 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) - - self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp'] + self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) + for file in files: self.compare_and_diff(file) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'functions.i') wrapper = MatlabWrapper( module_name='functions', @@ -147,9 +95,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m', @@ -163,11 +109,7 @@ class TestWrap(unittest.TestCase): def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'class.i') wrapper = MatlabWrapper( module_name='class', @@ -175,9 +117,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m', @@ -191,21 +131,14 @@ class TestWrap(unittest.TestCase): def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'inheritance.i') wrapper = MatlabWrapper( module_name='inheritance', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m', @@ -219,11 +152,7 @@ class TestWrap(unittest.TestCase): """ Test interface file with full namespace definition. """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'namespaces.i') wrapper = MatlabWrapper( module_name='namespaces', @@ -231,9 +160,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m', @@ -249,21 +176,14 @@ class TestWrap(unittest.TestCase): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'special_cases.i') wrapper = MatlabWrapper( module_name='special_cases', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'special_cases_wrapper.cpp', @@ -274,6 +194,31 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) + def test_multiple_files(self): + """ + Test for when multiple interface files are specified. + """ + file1 = osp.join(self.INTERFACE_DIR, 'part1.i') + file2 = osp.join(self.INTERFACE_DIR, 'part2.i') + + wrapper = MatlabWrapper( + module_name='multiple_files', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file1, file2], path=self.MATLAB_ACTUAL_DIR) + + files = [ + 'multiple_files_wrapper.cpp', + '+gtsam/Class1.m', + '+gtsam/Class2.m', + '+gtsam/ClassA.m', + ] + + for file in files: + self.compare_and_diff(file) + if __name__ == '__main__': unittest.main() diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py index 77c884b62..67c637d14 100644 --- a/wrap/tests/test_pybind_wrapper.py +++ b/wrap/tests/test_pybind_wrapper.py @@ -31,9 +31,9 @@ class TestWrap(unittest.TestCase): # Create the `actual/python` directory os.makedirs(PYTHON_ACTUAL_DIR, exist_ok=True) - def wrap_content(self, content, module_name, output_dir): + def wrap_content(self, sources, module_name, output_dir): """ - Common function to wrap content. + Common function to wrap content in `sources`. """ with open(osp.join(self.TEST_DIR, "pybind_wrapper.tpl")) as template_file: @@ -46,15 +46,12 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], module_template=module_template) - cc_content = wrapper.wrap(content) - output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp") if not osp.exists(osp.join(self.TEST_DIR, output_dir)): os.mkdir(osp.join(self.TEST_DIR, output_dir)) - with open(output, 'w') as f: - f.write(cc_content) + wrapper.wrap(sources, output) return output @@ -76,39 +73,32 @@ class TestWrap(unittest.TestCase): python3 ../pybind_wrapper.py --src geometry.h --module_name geometry_py --out output/geometry_py.cc """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'geometry_py', + source = osp.join(self.INTERFACE_DIR, 'geometry.i') + output = self.wrap_content([source], 'geometry_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('geometry_pybind.cpp', output) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'functions_py', + source = osp.join(self.INTERFACE_DIR, 'functions.i') + output = self.wrap_content([source], 'functions_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('functions_pybind.cpp', output) def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'class_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'class.i') + output = self.wrap_content([source], 'class_py', + self.PYTHON_ACTUAL_DIR) self.compare_and_diff('class_pybind.cpp', output) def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'inheritance_py', + source = osp.join(self.INTERFACE_DIR, 'inheritance.i') + output = self.wrap_content([source], 'inheritance_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('inheritance_pybind.cpp', output) @@ -119,10 +109,8 @@ class TestWrap(unittest.TestCase): python3 ../pybind_wrapper.py --src namespaces.i --module_name namespaces_py --out output/namespaces_py.cpp """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'namespaces_py', + source = osp.join(self.INTERFACE_DIR, 'namespaces.i') + output = self.wrap_content([source], 'namespaces_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('namespaces_pybind.cpp', output) @@ -131,10 +119,8 @@ class TestWrap(unittest.TestCase): """ Tests for operator overloading. """ - with open(osp.join(self.INTERFACE_DIR, 'operator.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'operator_py', + source = osp.join(self.INTERFACE_DIR, 'operator.i') + output = self.wrap_content([source], 'operator_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('operator_pybind.cpp', output) @@ -143,10 +129,8 @@ class TestWrap(unittest.TestCase): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'special_cases_py', + source = osp.join(self.INTERFACE_DIR, 'special_cases.i') + output = self.wrap_content([source], 'special_cases_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('special_cases_pybind.cpp', output) @@ -155,10 +139,8 @@ class TestWrap(unittest.TestCase): """ Test if enum generation is correct. """ - with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'enum.i') + output = self.wrap_content([source], 'enum_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('enum_pybind.cpp', output)