12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

merged from develop

release/4.3a0
akrishnan86 2021-07-20 23:37:23 -07:00
parent 15c29cacd2 838e74dbc8
commit bd994d07bc
200 changed files with 7474 additions and 6088 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
.github/workflows/build-python.yml vendored
View File

@ -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

7
INSTALL.md
View File

@ -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

4
gtsam/base/Matrix.h
View File

@ -29,7 +29,7 @@
#include <gtsam/config.h>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
@ -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<VectorN(
typedef std::function<VectorN(
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
Operator;

7
gtsam/base/Testable.h
View File

@ -34,8 +34,9 @@
#pragma once
#include <boost/concept_check.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#define GTSAM_PRINT(x)((x).print(#x))
@ -119,10 +120,10 @@ namespace gtsam {
* Binary predicate on shared pointers
*/
template<class V>
struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
if (!actual && !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
}

115
gtsam/base/base.i Normal file
View File

@ -0,0 +1,115 @@
//*************************************************************************
// base
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/navigation/ImuBias.h>
// #####
#include <gtsam/base/debug.h>
bool isDebugVersion();
#include <gtsam/base/DSFMap.h>
class IndexPair {
IndexPair();
IndexPair(size_t i, size_t j);
size_t i() const;
size_t j() const;
};
// template<KEY = {gtsam::IndexPair}>
// class DSFMap {
// DSFMap();
// KEY find(const KEY& key) const;
// void merge(const KEY& x, const KEY& y);
// std::map<KEY, Set> 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 <gtsam/base/Matrix.h>
bool linear_independent(Matrix A, Matrix B, double tol);
#include <gtsam/base/Value.h>
virtual class Value {
// No constructors because this is an abstract class
// Testable
void print(string s = "") const;
// Manifold
size_t dim() const;
};
#include <gtsam/base/GenericValue.h>
template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
} // namespace gtsam

2
gtsam/base/lieProxies.h
View File

@ -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.
*/

378
gtsam/base/numericalDerivative.h
View File

@ -18,15 +18,7 @@
// \callgraph
#pragma once
#include <boost/function.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <functional>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
@ -45,13 +37,13 @@ namespace gtsam {
* for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, boost::optional<Matrix&> 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 <class X, int N = traits<X>::dimension>
typename Eigen::Matrix<double, N, 1> numericalGradient(
boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
std::function<double(const X&)> 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<double, N, 1> numericalGradient(
template <class Y, class X, int N = traits<X>::dimension>
// TODO Should compute fixed-size matrix
typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@ -157,7 +149,8 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
template<class Y, class X>
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
delta);
}
/**
@ -170,20 +163,23 @@ typename internal::FixedSizeMatrix<Y,X>::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<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
return numericalDerivative21<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -196,20 +192,23 @@ typename internal::FixedSizeMatrix<Y,X1>::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<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
return numericalDerivative22<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -225,20 +224,24 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
x1, delta);
}
template<class Y, class X1, class X2, class X3>
typename internal::FixedSizeMatrix<Y,X1>::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<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative31<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -254,20 +257,24 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
x2, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X2>::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<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative32<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -283,20 +290,24 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
*/
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
x3, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X3>::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<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative33<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -312,19 +323,25 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X1>::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<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative41<Y, X1, X2, X3, X4>(
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<Y,X1>::type numericalDerivative41(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X2>::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<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative42<Y, X1, X2, X3, X4>(
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<Y,X2>::type numericalDerivative42(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X3>::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<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative43<Y, X1, X2, X3, X4>(
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<Y,X3>::type numericalDerivative43(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X4>::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<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative44<Y, X1, X2, X3, X4>(
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<Y,X4>::type numericalDerivative44(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X1>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
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<Y,X1>::type numericalDerivative51(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X2>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
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<Y,X2>::type numericalDerivative52(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X3>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
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<Y,X3>::type numericalDerivative53(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X4>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
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<Y,X4>::type numericalDerivative54(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X5>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
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<Y,X5>::type numericalDerivative55(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X1>::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<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
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<Y,X1>::type numericalDerivative61(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X2>::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<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
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<Y,X2>::type numericalDerivative62(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5), std::cref(x6)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X3>::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<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
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<Y,X3>::type numericalDerivative63(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5), std::cref(x6)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X4>::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<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
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<Y,X4>::type numericalDerivative64(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1, std::cref(x6)),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X5>::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<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
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<Y,X5>::type numericalDerivative65(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> 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<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
return numericalDerivative11<Y, X6, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::placeholders::_1),
x6, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X6>::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<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
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<Y,X6>::type numericalDerivative66(Y (*
* @return n*n Hessian matrix computed via central differencing
*/
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
typedef boost::function<double(const X&)> F;
typedef boost::function<VectorD(F, const X&, double)> G;
typedef std::function<double(const X&)> F;
typedef std::function<VectorD(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
delta);
return numericalDerivative11<VectorD, X>(
std::bind(ng, f, std::placeholders::_1, delta), x, delta);
}
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
1e-5) {
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
return numericalHessian(std::function<double(const X&)>(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<X,X>::type numericalHessian(double (*f
*/
template<class X1, class X2>
class G_x1 {
const boost::function<double(const X1&, const X2&)>& f_;
const std::function<double(const X1&, const X2&)>& f_;
X1 x1_;
double delta_;
public:
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
double delta) :
f_(f), x1_(x1), delta_(delta) {
}
Vector operator()(const X2& x2) {
return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
return numericalGradient<X1>(
std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
}
};
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
std::function<Vector(const X2&)>(
std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
std::function<double(const X1&)> f2(
std::bind(f, std::placeholders::_1, std::cref(x2)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
std::function<double(const X2&)> f2(
std::bind(f, std::cref(x1), std::placeholders::_1));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
@ -852,15 +970,17 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(doubl
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
std::function<double(const X1&)> f2(std::bind(
f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
@ -868,22 +988,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
std::function<double(const X2&)> f2(std::bind(
f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
@ -891,22 +1013,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X3>::type Vector;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
Vector (*numGrad)(std::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
std::function<double(const X3&)> f2(std::bind(
f, std::cref(x1), std::cref(x2), std::placeholders::_1));
return numericalDerivative11<Vector, X3>(
boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X3&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x3, delta);
}
@ -914,35 +1038,41 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X2>(
boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
std::function<double(const X1&, const X2&)>(
std::bind(f, std::placeholders::_1, std::placeholders::_2,
std::cref(x3))),
x1, x2, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X3>(
boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
std::function<double(const X1&, const X3&)>(
std::bind(f, std::placeholders::_1, std::cref(x2),
std::placeholders::_2)),
x1, x3, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X2, X3>(
boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
std::function<double(const X2&, const X3&)>(
std::bind(f, std::cref(x1), std::placeholders::_1,
std::placeholders::_2)),
x2, x3, delta);
}
@ -951,7 +1081,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -959,7 +1089,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -967,7 +1097,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}

5
gtsam/base/treeTraversal-inst.h
View File

@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
// Typedefs
typedef typename FOREST::Node Node;
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold);
tbb::task::spawn_root_and_wait(
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold));
#else
DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif

70
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -22,7 +22,7 @@
#include <boost/make_shared.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/task_group.h> // tbb::task_group
#include <tbb/task.h> // tbb::task, tbb::task_list
#include <tbb/scalable_allocator.h> // tbb::scalable_allocator
namespace gtsam {
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class PreOrderTask
class PreOrderTask : public tbb::task
{
public:
const boost::shared_ptr<NODE>& 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<NODE>& treeNode, const boost::shared_ptr<DATA>& 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<NODE>& 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<DATA> childData = boost::allocate_shared<DATA>(
tbb::scalable_allocator<DATA>(), 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>& node, DATA& myData) const
void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
{
for(const boost::shared_ptr<NODE>& child: node->children)
{
@ -122,7 +131,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
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<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children
tbb::task_list tasks;
for(const boost::shared_ptr<NODE>& root: roots)
{
boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), 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<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
{
typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> 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);
}
}

29
gtsam/base/utilities.h Normal file
View File

@ -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_;
};
}

4
gtsam/discrete/DecisionTree-inl.h
View File

@ -450,7 +450,7 @@ namespace gtsam {
template<typename L, typename Y>
template<typename M, typename X>
DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op) {
const std::map<M, L>& map, std::function<Y(const X&)> op) {
root_ = convert(other.root_, map, op);
}
@ -568,7 +568,7 @@ namespace gtsam {
template<typename M, typename X>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
boost::function<Y(const X&)> op) {
std::function<Y(const X&)> op) {
typedef DecisionTree<M, X> MX;
typedef typename MX::Leaf MXLeaf;

12
gtsam/discrete/DecisionTree.h
View File

@ -20,10 +20,12 @@
#pragma once
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <functional>
#include <iostream>
#include <vector>
#include <map>
#include <vector>
namespace gtsam {
@ -38,8 +40,8 @@ namespace gtsam {
public:
/** Handy typedefs for unary and binary function types */
typedef boost::function<Y(const Y&)> Unary;
typedef boost::function<Y(const Y&, const Y&)> Binary;
typedef std::function<Y(const Y&)> Unary;
typedef std::function<Y(const Y&, const Y&)> Binary;
/** A label annotated with cardinality */
typedef std::pair<L,size_t> LabelC;
@ -107,7 +109,7 @@ namespace gtsam {
/** Convert to a different type */
template<typename M, typename X> NodePtr
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
L>& map, boost::function<Y(const X&)> op);
L>& map, std::function<Y(const X&)> op);
/** Default constructor */
DecisionTree();
@ -143,7 +145,7 @@ namespace gtsam {
/** Convert from a different type */
template<typename M, typename X>
DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op);
const std::map<M, L>& map, std::function<Y(const X&)> op);
/// @}
/// @name Testable

2
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
}
/** I can't get this to work !
class Mul: boost::function<double(const double&, const double&)> {
class Mul: std::function<double(const double&, const double&)> {
inline double operator()(const double& a, const double& b) {
return a * b;
}

2
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -196,7 +196,7 @@ TEST(DT, conversion)
map<string, Label> ordering;
ordering[A] = X;
ordering[B] = Y;
boost::function<bool(const int&)> op = convert;
std::function<bool(const int&)> op = convert;
BDT f2(f1, ordering, op);
// f1.print("f1");
// f2.print("f2");

14
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -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

4
gtsam/geometry/Cyclic.h
View File

@ -17,7 +17,9 @@
#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <iostream> // for cout :-(
#include <cassert>
#include <iostream> // for cout :-(
namespace gtsam {

2
gtsam/geometry/Pose3.h
View File

@ -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

3
gtsam/geometry/SimpleCamera.h
View File

@ -21,7 +21,9 @@
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
@ -33,6 +35,7 @@ namespace gtsam {
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**

1019
gtsam/geometry/geometry.i Normal file
View File

File diff suppressed because it is too large Load Diff

5
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -24,6 +24,7 @@
#include <iostream>
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<CalibratedCamera(Pose3)> f = //
boost::bind(CalibratedCamera::Create, _1, boost::none);
std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

1
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;

24
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -5,13 +5,15 @@
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <sstream>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -39,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
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<EssentialMatrix, Unit3>(
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<EssentialMatrix, Pose3>(
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<EssentialMatrix, Pose3>(
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));
}

10
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -23,6 +23,7 @@
#include <boost/assign/std/vector.hpp>
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<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> 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<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);

10
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -27,6 +27,7 @@
#include <cmath>
#include <iostream>
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<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -79,8 +81,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

4
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

1
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;

21
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,11 +14,14 @@
* @brief Unit tests for Point3 class
*/
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Point3.h>
#include <boost/function.hpp>
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<double(const Point3&, const Point3&)> f =
std::function<double(const Point3&, const Point3&)> 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<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::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<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(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<Point3, Point3>(
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));
}

9
gtsam/geometry/tests/testPose3.cpp
View File

@ -22,6 +22,7 @@
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
using namespace std::placeholders;
#include <CppUnitLite/TestHarness.h>
#include <cmath>
@ -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<Point3, Pose3>(
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<Rot3, Pose3>(
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<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

25
gtsam/geometry/tests/testSO3.cpp
View File

@ -15,13 +15,12 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
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<SO3, Vector3>(
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<SO3, Vector3>(
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<SO3, Vector3>(
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<Vector, SO3>(
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<Vector, SO3>(
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<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
std::function<Vector9(const SO3&)> 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<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);

6
gtsam/geometry/tests/testSO4.cpp
View File

@ -166,7 +166,7 @@ TEST(SO4, vec) {
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
std::function<Vector16(const SO4&)> 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<Matrix3(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix3(const SO4&)> 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<Matrix43(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
return stiefel(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);

4
gtsam/geometry/tests/testSOn.cpp
View File

@ -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<Matrix(const Vector &)> h = [](const Vector &v) {
std::function<Matrix(const Vector &)> 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<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}

30
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,23 +16,22 @@
* @author Zhaoyang Lv
*/
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <functional>
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<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transformFrom, _1, _2, boost::none, boost::none);
// Use lambda to resolve overloaded method
std::function<Point3(const Similarity3&, const Point3&)>
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 }) {

41
gtsam/geometry/tests/testUnit3.cpp
View File

@ -31,13 +31,13 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>
#include <random>
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<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
boost::none, boost::none);
std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@ -157,13 +158,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
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<Vector2,Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Unit3>(
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<Unit3>(
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<Vector6, Unit3>(
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<Vector6, Unit3>(
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<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
std::function<Unit3(const Vector2&)> 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<Unit3, Point3>(
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));
}

10
gtsam/geometry/triangulation.h
View File

@ -18,14 +18,16 @@
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
namespace gtsam {
@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
} // \namespace gtsam

3532
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

4
gtsam/inference/EliminateableFactorGraph.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/variant.hpp>
#include <boost/optional.hpp>
@ -86,7 +86,7 @@ namespace gtsam {
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
/// The function type that does a single dense elimination step on a subgraph.
typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;

1
gtsam/inference/EliminationTree-inst.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <stack>
#include <gtsam/base/timing.h>

2
gtsam/inference/FactorGraph-inst.h
View File

@ -23,8 +23,6 @@
#include <gtsam/inference/FactorGraph.h>
#include <boost/bind.hpp>
#include <stdio.h>
#include <algorithm>
#include <iostream> // for cout :-(

1
gtsam/inference/FactorGraph.h
View File

@ -29,7 +29,6 @@
#include <Eigen/Core> // for Eigen::aligned_allocator
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>

41
gtsam/inference/LabeledSymbol.cpp
View File

@ -15,14 +15,11 @@
* @author: Alex Cunningham
*/
#include <iostream>
#include <gtsam/inference/LabeledSymbol.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/inference/LabeledSymbol.h>
#include <iostream>
namespace gtsam {
@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
/* ************************************************************************* */
static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c;
std::function<bool(gtsam::Key)> 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<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
std::function<bool(gtsam::Key)> 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<bool(gtsam::Key)> 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<bool(gtsam::Key)> 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));
}
/* ************************************************************************* */

8
gtsam/inference/LabeledSymbol.h
View File

@ -19,8 +19,8 @@
#pragma once
#include <functional>
#include <gtsam/inference/Symbol.h>
#include <boost/function.hpp>
namespace gtsam {
@ -89,13 +89,13 @@ public:
*/
// Checks only the type
static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
// Checks only the robot ID (label_)
static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
// Checks both type and the robot ID
static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
// Converts to upper/lower versions of labels
LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }

9
gtsam/inference/Symbol.cpp
View File

@ -18,8 +18,8 @@
#include <gtsam/inference/Symbol.h>
#include <boost/bind/bind.hpp>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <limits.h>
#include <list>
@ -62,8 +62,11 @@ Symbol::operator std::string() const {
static Symbol make(gtsam::Key key) { return Symbol(key);}
boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
return bind(&Symbol::chr, bind(make, _1)) == c;
std::function<bool(Key)> 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) {

7
gtsam/inference/Symbol.h
View File

@ -18,11 +18,12 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <cstdint>
#include <functional>
namespace gtsam {
@ -114,7 +115,7 @@ public:
* Values::filter() function to retrieve all key-value pairs with the
* requested character.
*/
static boost::function<bool(Key)> ChrTest(unsigned char c);
static std::function<bool(Key)> 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 &);

10
gtsam/inference/VariableIndex.h
View File

@ -182,6 +182,16 @@ protected:
return item->second;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_);
ar & BOOST_SERIALIZATION_NVP(nEntries_);
}
/// @}
};

1
gtsam/inference/inference-inst.h
View File

@ -20,7 +20,6 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>
#include <utility>
#include <gtsam/base/treeTraversal-inst.h>

8
gtsam/linear/HessianFactor.cpp
View File

@ -32,14 +32,6 @@
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>

8
gtsam/linear/JacobianFactor.cpp
View File

@ -35,14 +35,6 @@
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/array.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/adaptor/map.hpp>

5
gtsam/linear/VectorValues.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/linear/VectorValues.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
@ -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<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
std::bind(&less<Key>::operator(), less<Key>(), 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.");
}

653
gtsam/linear/linear.i Normal file
View File

@ -0,0 +1,653 @@
//*************************************************************************
// linear
//*************************************************************************
namespace gtsam {
namespace noiseModel {
#include <gtsam/linear/NoiseModel.h>
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 <gtsam/linear/Sampler.h>
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 <gtsam/linear/VectorValues.h>
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 <gtsam/linear/GaussianFactor.h>
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<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
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<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> 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 <gtsam/linear/HessianFactor.h>
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 <gtsam/linear/GaussianFactorGraph.h>
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<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::KeyVector& keys);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> 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<Matrix,Vector> jacobian() const;
pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const;
Matrix augmentedHessian() const;
Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> hessian() const;
pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianConditional.h>
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 <gtsam/linear/GaussianDensity.h>
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 <gtsam/linear/GaussianBayesNet.h>
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 <gtsam/linear/GaussianBayesTree.h>
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 <gtsam/linear/Errors.h>
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 <gtsam/linear/GaussianISAM.h>
class GaussianISAM {
//Constructor
GaussianISAM();
//Standard Interface
void update(const gtsam::GaussianFactorGraph& newFactors);
void saveGraph(string s) const;
void clear();
};
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getVerbosity() const;
void setVerbosity(string s) ;
};
//virtual class IterativeSolver {
// IterativeSolver();
// gtsam::VectorValues optimize ();
//};
#include <gtsam/linear/ConjugateGradientSolver.h>
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 <gtsam/linear/Preconditioner.h>
virtual class PreconditionerParameters {
PreconditionerParameters();
};
virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();
void print(string s = "");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
};
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
};
#include <gtsam/linear/KalmanFilter.h>
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);
};
}

8
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -25,12 +25,14 @@
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <boost/bind/bind.hpp>
// STL/C++
#include <iostream>
#include <sstream>
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<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
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();

7
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -21,7 +21,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/base/numericalDerivative.h>
@ -29,6 +28,8 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianConditional.h>
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<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
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();

355
gtsam/navigation/navigation.i Normal file
View File

@ -0,0 +1,355 @@
//*************************************************************************
// Navigation
//*************************************************************************
namespace gtsam {
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
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 <gtsam/navigation/NavState.h>
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 <gtsam/navigation/PreintegratedRotation.h>
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<Vector> getOmegaCoriolis() const;
boost::optional<gtsam::Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
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 <gtsam/navigation/ImuFactor.h>
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 <gtsam/navigation/CombinedImuFactor.h>
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 <gtsam/navigation/AHRSFactor.h>
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 <gtsam/navigation/AttitudeFactor.h>
//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 <gtsam/navigation/GPSFactor.h>
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 <gtsam/navigation/Scenario.h>
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 <gtsam/navigation/ScenarioRunner.h>
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;
};
}

46
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -25,9 +25,9 @@
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -174,17 +174,17 @@ TEST(AHRSFactor, Error) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
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<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
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<Rot3, Vector3>(
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<Vector3, Vector3>(
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<Rot3, Vector3>(
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<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
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<Vector3, Vector3>(
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;

10
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -21,8 +21,11 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
Matrix expectedH = numericalDerivative11<Vector, Rot3>(
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<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
boost::none), T);
// Use the factor to calculate the derivative

1
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -29,7 +29,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
#include "imuFactorTesting.h"

7
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -20,11 +20,14 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
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<Vector,Pose3>(
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<Vector,NavState>(
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;

12
gtsam/navigation/tests/testImuBias.cpp
View File

@ -19,8 +19,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
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<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> 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<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> 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));

36
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -30,7 +30,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <list>
#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<Vector9(const NavState&, const NavState&, const Bias&)> f =
boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3,
std::function<Vector9(const NavState&, const NavState&, const Bias&)> 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<Vector9, Bias>(
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<NavState, NavState>(
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<NavState, Bias>(
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<Rot3, Pose3>(
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<Rot3, Pose3>(
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<Vector9, Bias>(
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<Vector3, Vector3>(
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<NavState, Vector3, Vector3>(
// 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<NavState, Vector3, Vector3>(
// 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));

19
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -20,10 +20,13 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/LocalCartesian.hpp>
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<Point3,Rot2> //
(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<Vector,Rot2> //
(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<Vector,Rot3> //
(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<Vector,Point3> //
(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<Vector,Point3> //
(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<Vector,double> //
(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<Vector,Unit3> //
(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<Vector,Point3> //
(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));
}

19
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/MagPoseFactor.h>
using namespace std::placeholders;
using namespace gtsam;
// *****************************************************************************
@ -75,8 +76,11 @@ TEST(MagPoseFactor, JacobianPose2) {
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose2> 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<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::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<Pose3> 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<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P3_b),
H3, 1e-7));
}
// *****************************************************************************
@ -101,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(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<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
@ -115,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(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<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************

19
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -22,10 +22,11 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
using namespace std::placeholders;
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
@ -41,21 +42,21 @@ static boost::shared_ptr<PreintegrationParams> Params() {
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
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));

49
gtsam/navigation/tests/testNavState.cpp
View File

@ -19,8 +19,11 @@
#include <gtsam/navigation/NavState.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> 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<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
std::function<NavState(const Pose3&, const Vector3&)> 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<Rot3, NavState>(
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<Point3, NavState>(
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<Velocity3, NavState>(
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<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
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<NavState(const NavState&, const Vector9&)> retract =
boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
std::function<NavState(const NavState&, const Vector9&)> 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<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
std::function<Vector9(const NavState&, const NavState&)> 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<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
std::function<Vector9(const NavState&, const bool&)> 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<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
std::function<Vector9(const NavState&, const Vector9&)> 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));

4
gtsam/navigation/tests/testScenario.cpp
View File

@ -19,9 +19,9 @@
#include <gtsam/navigation/Scenario.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <cmath>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -147,7 +147,7 @@ TEST(Scenario, Accelerating) {
{
// Check acceleration in nav
Matrix expected = numericalDerivative11<Vector3, double>(
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));
}

17
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -22,10 +22,11 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#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<Vector9(const Vector3&, const Vector3&)> preintegrated =
std::function<Vector9(const Vector3&, const Vector3&)> 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<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
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<Vector9(const Vector9&, const Vector9&)> f =
std::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
};

22
gtsam/nonlinear/Expression-inl.h
View File

@ -21,6 +21,7 @@
#include <gtsam/nonlinear/internal/ExpressionNode.h>
#include <boost/bind/bind.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
@ -82,7 +83,8 @@ template<typename A>
Expression<T>::Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(
new internal::UnaryExpression<T, A>(boost::bind(method, _1, _2),
new internal::UnaryExpression<T, A>(std::bind(method,
std::placeholders::_1, std::placeholders::_2),
expression)) {
}
@ -95,7 +97,10 @@ Expression<T>::Expression(const Expression<A1>& expression1,
const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(
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<T>::Expression(const Expression<A1>& expression1,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(
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<typename T>
@ -247,8 +255,10 @@ template<typename T>
Expression<T> operator*(const Expression<T>& expression1,
const Expression<T>& expression2) {
return Expression<T>(
boost::bind(internal::apply_compose<T>(), _1, _2, _3, _4), expression1,
expression2);
std::bind(internal::apply_compose<T>(), std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4),
expression1, expression2);
}
/// Construct an array of leaves

9
gtsam/nonlinear/Expression.h
View File

@ -24,7 +24,6 @@
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <map>
@ -69,20 +68,20 @@ public:
// Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1>
struct UnaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
template<class A1, class A2>
struct BinaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> type;
};
template<class A1, class A2, class A3>
struct TernaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
@ -240,7 +239,7 @@ class BinarySumExpression : public Expression<T> {
*/
template <typename T, typename A>
Expression<T> linearExpression(
const boost::function<T(A)>& f, const Expression<A>& expression,
const std::function<T(A)>& f, const Expression<A>& expression,
const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
// Use lambda to endow f with a linear Jacobian
typename Expression<T>::template UnaryFunction<A>::type g =

20
gtsam/nonlinear/ISAM2.h
View File

@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
void removeVariables(const KeySet& unusedKeys);
void updateDelta(bool forceFullSolve = false) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*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

11
gtsam/nonlinear/NonlinearEquality.h
View File

@ -21,7 +21,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <limits>
#include <iostream>
@ -69,7 +69,7 @@ public:
/**
* Function that compares two values
*/
typedef boost::function<bool(const T&, const T&)> CompareFunction;
typedef std::function<bool(const T&, const T&)> 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<T>::Equals,_1,_2,1e-9)) :
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::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<T>::Equals,_1,_2,1e-9)) :
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
compare_(_compare) {
@ -359,4 +361,3 @@ struct traits<NonlinearEquality2<VALUE> > : Testable<NonlinearEquality2<VALUE> >
}// namespace gtsam

26
gtsam/nonlinear/Values-inl.h
View File

@ -26,6 +26,8 @@
#include <utility>
#include <boost/bind/bind.hpp>
#include <gtsam/nonlinear/Values.h> // 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<bool(const Values::ConstKeyValuePair&)>,
std::function<bool(const Values::ConstKeyValuePair&)>,
Values::iterator> >
iterator;
@ -111,7 +113,7 @@ namespace gtsam {
boost::transform_iterator<
ConstKeyValuePair(*)(Values::ConstKeyValuePair),
boost::filter_iterator<
boost::function<bool(const Values::ConstKeyValuePair&)>,
std::function<bool(const Values::ConstKeyValuePair&)>,
Values::const_iterator> >
const_const_iterator;
@ -132,7 +134,7 @@ namespace gtsam {
private:
Filtered(
const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
Values& values) :
begin_(
boost::make_transform_iterator(
@ -203,7 +205,7 @@ namespace gtsam {
const_iterator begin_;
const_iterator end_;
ConstFiltered(
const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
const std::function<bool(const Values::ConstKeyValuePair&)>& 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<Value>
inline Values::filter(const boost::function<bool(Key)>& filterFcn) {
inline Values::filter(const std::function<bool(Key)>& filterFcn) {
return filter<Value>(filterFcn);
}
/* ************************************************************************* */
template<class ValueType>
Values::Filtered<ValueType>
Values::filter(const boost::function<bool(Key)>& filterFcn) {
return Filtered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
Values::filter(const std::function<bool(Key)>& filterFcn) {
return Filtered<ValueType>(std::bind(&filterHelper<ValueType>, filterFcn,
std::placeholders::_1), *this);
}
/* ************************************************************************* */
Values::ConstFiltered<Value>
inline Values::filter(const boost::function<bool(Key)>& filterFcn) const {
inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
return filter<Value>(filterFcn);
}
/* ************************************************************************* */
template<class ValueType>
Values::ConstFiltered<ValueType>
Values::filter(const boost::function<bool(Key)>& filterFcn) const {
return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
Values::filter(const std::function<bool(Key)>& filterFcn) const {
return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
filterFcn, std::placeholders::_1), *this);
}
/* ************************************************************************* */
template<>
inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter,
inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
const ConstKeyValuePair& key_value) {
// Filter and check the type
return filter(key_value.key);

8
gtsam/nonlinear/Values.cpp
View File

@ -25,14 +25,6 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/iterator/transform_iterator.hpp>
#include <list>

27
gtsam/nonlinear/Values.h
View File

@ -29,15 +29,6 @@
#include <gtsam/inference/Key.h>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/ptr_container/serialize_ptr_map.hpp>
#include <boost/shared_ptr.hpp>
@ -117,19 +108,19 @@ namespace gtsam {
/// Mutable forward iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
/// Const forward iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
/// Mutable reverse iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
/// Const reverse iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
typedef KeyValuePair value_type;
@ -330,7 +321,7 @@ namespace gtsam {
* the original Values class.
*/
Filtered<Value>
filter(const boost::function<bool(Key)>& filterFcn);
filter(const std::function<bool(Key)>& filterFcn);
/**
* Return a filtered view of this Values class, without copying any data.
@ -353,7 +344,7 @@ namespace gtsam {
*/
template<class ValueType>
Filtered<ValueType>
filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
/**
* Return a filtered view of this Values class, without copying any data.
@ -369,7 +360,7 @@ namespace gtsam {
* the original Values class.
*/
ConstFiltered<Value>
filter(const boost::function<bool(Key)>& filterFcn) const;
filter(const std::function<bool(Key)>& filterFcn) const;
/**
* Return a filtered view of this Values class, without copying any data.
@ -391,7 +382,7 @@ namespace gtsam {
*/
template<class ValueType>
ConstFiltered<ValueType>
filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
// Count values of given type \c ValueType
template<class ValueType>
@ -408,7 +399,7 @@ namespace gtsam {
// Filters based on ValueType (if not Value) and also based on the user-
// supplied \c filter function.
template<class ValueType>
static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
// Filter and check the type
return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));

2
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> 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<class T>

804
gtsam/nonlinear/nonlinear.i Normal file
View File

@ -0,0 +1,804 @@
//*************************************************************************
// nonlinear
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/NavState.h>
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 <gtsam/inference/LabeledSymbol.h>
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 <gtsam/inference/Ordering.h>
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 <gtsam/nonlinear/NonlinearFactorGraph.h>
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 <T = {double,
Vector,
gtsam::Point2,
gtsam::StereoPoint2,
gtsam::Point3,
gtsam::Rot2,
gtsam::SO3,
gtsam::SO4,
gtsam::Rot3,
gtsam::Pose2,
gtsam::Pose3,
gtsam::Cal3_S2,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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<gtsam::Key,int>& 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 <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/nonlinear/CustomFactor.h>
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]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* 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 <gtsam/nonlinear/Values.h>
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<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& 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<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& 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 <T = {gtsam::Point2,
gtsam::Point3,
gtsam::Rot2,
gtsam::Pose2,
gtsam::SO3,
gtsam::SO4,
gtsam::SOn,
gtsam::Rot3,
gtsam::Pose3,
gtsam::Unit3,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::EssentialMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,
Matrix,
double}>
T at(size_t j);
};
#include <gtsam/nonlinear/Marginals.h>
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 <gtsam/nonlinear/LinearContainerFactor.h>
virtual class LinearContainerFactor : gtsam::NonlinearFactor {
LinearContainerFactor(gtsam::GaussianFactor* factor,
const gtsam::Values& linearizationPoint);
LinearContainerFactor(gtsam::GaussianFactor* factor);
gtsam::GaussianFactor* factor() const;
// const boost::optional<Values>& 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 <gtsam/nonlinear/summarization.h>
//
//// 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 <gtsam/nonlinear/NonlinearOptimizerParams.h>
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 <gtsam/nonlinear/GaussNewtonOptimizer.h>
virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
GaussNewtonParams();
};
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
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 <gtsam/nonlinear/DoglegOptimizer.h>
virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
DoglegParams();
double getDeltaInitial() const;
string getVerbosityDL() const;
void setDeltaInitial(double deltaInitial) const;
void setVerbosityDL(string verbosityDL) const;
};
#include <gtsam/nonlinear/NonlinearOptimizer.h>
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 <gtsam/nonlinear/GaussNewtonOptimizer.h>
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 <gtsam/nonlinear/DoglegOptimizer.h>
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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
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 <gtsam/nonlinear/ISAM2.h>
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 <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, 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 <gtsam/nonlinear/NonlinearISAM.h>
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 <gtsam/nonlinear/PriorFactor.h>
template <T = {double,
Vector,
gtsam::Point2,
gtsam::StereoPoint2,
gtsam::Point3,
gtsam::Rot2,
gtsam::SO3,
gtsam::SO4,
gtsam::SOn,
gtsam::Rot3,
gtsam::Pose2,
gtsam::Pose3,
gtsam::Unit3,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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 <gtsam/nonlinear/NonlinearEquality.h>
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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

83
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -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<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint, points.back().value(values)));
std::vector<Point3_> pointExpressions = createUnknowns<Point3>(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<Eigen::Dynamic, 3> H) {
return p;
}
Point3_ p(1);
Point3_ pointExpression(1);
set<Key> 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<Point2> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
using namespace unary;
Double_ e(f2, p);
EXPECT(expected == e.keys());
Expression<Point2> 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<Vector> e(f3, p);
// TODO(yetongumich): dynamic output arguments do not work yet!
// Expression<Vector> 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<Class> : public internal::VectorSpace<Class> {};
// Nullary Method
TEST(Expression, NullaryMethod) {
// Create expression
Expression<Class> p(67);
Expression<double> norm_(p, &Class::norm);
const Key key(67);
Expression<Class> 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<double> 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<Key, int> 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<Key> expected = list_of(1)(2);
EXPECT(expected == binary::p_cam.keys());
}
/* ************************************************************************* */
// dimensions
// Check dimensions by calling `dims` method.
TEST(Expression, BinaryDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(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<Point3, Pose3, Point3> 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<Point3>(key1, Point3(1, 0, 0));
values.insert<Point3>(key2, Point3(0, 1, 0));
const Point3 point1(1, 0, 0), point2(0, 1, 0);
values.insert<Point3>(key1, point1);
values.insert<Point3>(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<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const std::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const Matrix3 kIdentity = I_3x3;
const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);

80
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
@ -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::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
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<gtsam::Pose3>(symbol0, pose0, noiseModel));
initialValues.insert(symbol0, pose0);
solver.update(graph, initialValues,
gtsam::FastVector<gtsam::FactorIndex>());
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<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
try {
p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
EXPECT(assert_equal(p1, p2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

8
gtsam/nonlinear/tests/testValues.cpp
View File

@ -28,11 +28,13 @@
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
#include <boost/bind/bind.hpp>
#include <stdexcept>
#include <limits>
#include <type_traits>
using namespace boost::assign;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
@ -334,7 +336,7 @@ TEST(Values, filter) {
// Filter by key
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::Filtered<Value> filtered = values.filter(std::bind(std::greater_equal<Key>(), 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<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::ConstFiltered<Value> constfiltered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
Values fromconstfiltered(constfiltered);
EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));

25
gtsam/nonlinear/utilities.h
View File

@ -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_;
};
}

96
gtsam/sam/sam.i Normal file
View File

@ -0,0 +1,96 @@
//*************************************************************************
// sam
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
// #####
#include <gtsam/sam/RangeFactor.h>
template <POSE, POINT>
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<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>
RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera>
RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3_S2>>
RangeFactorSimpleCamera;
#include <gtsam/sam/RangeFactor.h>
template <POSE, POINT>
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<gtsam::Pose2, gtsam::Point2>
RangeFactorWithTransform2D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3>
RangeFactorWithTransform3D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2>
RangeFactorWithTransformPose2;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3>
RangeFactorWithTransformPose3;
#include <gtsam/sam/BearingFactor.h>
template <POSE, POINT, BEARING>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2>
BearingFactor2D;
typedef gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3>
BearingFactor3D;
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2>
BearingFactorPose2;
#include <gtsam/sam/BearingRangeFactor.h>
template <POSE, POINT, BEARING, RANGE>
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<POSE, POINT, BEARING, RANGE> measured() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
double>
BearingRangeFactor2D;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
double>
BearingRangeFactorPose2;
} // namespace gtsam

19
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -26,8 +26,9 @@
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
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<Vector, Pose2>(
boost::bind(&factorError2D, _1, point, factor), pose);
std::bind(&factorError2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
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<Vector, Pose2>(
boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
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<Vector, Pose3>(
boost::bind(&factorError3D, _1, point, factor), pose);
std::bind(&factorError3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
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<Vector, Pose3>(
boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
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));

30
gtsam/sfm/ShonanAveraging.cpp
View File

@ -944,6 +944,36 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
parameters.getUseHuber()),
parameters) {}
// Extract Rot2 measurement from Pose2 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
const BetweenFactor<Pose2>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot2> 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<Rot2>(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 &parameters)
: ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
template class ShonanAveraging<3>;

2
gtsam/sfm/ShonanAveraging.h
View File

@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
const Parameters &parameters = Parameters());
explicit ShonanAveraging2(std::string g2oFile,
const Parameters &parameters = Parameters());
ShonanAveraging2(const BetweenFactorPose2s &factors,
const Parameters &parameters = Parameters());
};
class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {

211
gtsam/sfm/sfm.i Normal file
View File

@ -0,0 +1,211 @@
//*************************************************************************
// sfm
//*************************************************************************
namespace gtsam {
// #####
#include <gtsam/sfm/ShonanFactor.h>
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 <gtsam/sfm/BinaryMeasurement.h>
template <T>
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<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h>
// 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<size_t, gtsam::Rot2> 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<size_t, gtsam::Rot3> 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 &parameters);
// 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<double, Vector> 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<gtsam::Values, double> 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<double, Vector> 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<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p,
size_t max_p) const;
};
#include <gtsam/sfm/MFAS.h>
class KeyPairDoubleMap {
KeyPairDoubleMap();
KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
size_t size() const;
bool empty() const;
void clear();
size_t at(const pair<size_t, size_t>& keypair) const;
};
class MFAS {
MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::Unit3& projectionDirection);
gtsam::KeyPairDoubleMap computeOutlierWeights() const;
gtsam::KeyVector computeOrdering() const;
};
#include <gtsam/sfm/TranslationRecovery.h>
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

5
gtsam/sfm/tests/testTranslationFactor.cpp
View File

@ -22,6 +22,7 @@
#include <CppUnitLite/TestHarness.h>
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<Vector, Point3>(
boost::bind(&factorError, _1, T2, factor), T1);
std::bind(&factorError, std::placeholders::_1, T2, factor), T1);
H2Expected = numericalDerivative11<Vector, Point3>(
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));

338
gtsam/slam/slam.i Normal file
View File

@ -0,0 +1,338 @@
//*************************************************************************
// slam
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/navigation/ImuBias.h>
// ######
#include <gtsam/slam/BetweenFactor.h>
template <T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
gtsam::imuBias::ConstantBias}>
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 <gtsam/slam/ProjectionFactor.h>
template <POSE, LANDMARK, CALIBRATION>
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<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3_S2>
GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3DS2>
GenericProjectionFactorCal3DS2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3Fisheye>
GenericProjectionFactorCal3Fisheye;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3Unified>
GenericProjectionFactorCal3Unified;
#include <gtsam/slam/GeneralSFMFactor.h>
template <CAMERA, LANDMARK>
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::PinholeCamera<gtsam::Cal3_S2>,
gtsam::Point3>
GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>,
gtsam::Point3>
GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::Point3>
GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::Point3>
GeneralSFMFactorCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::Point3>
GeneralSFMFactorCal3Unified;
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
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 <gtsam/slam/SmartProjectionFactor.h>
/// 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 <gtsam/slam/SmartProjectionPoseFactor.h>
template <CALIBRATION>
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<gtsam::Cal3_S2>
SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
template <POSE, LANDMARK>
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<gtsam::Pose3, gtsam::Point3>
GenericStereoFactor3D;
#include <gtsam/slam/PoseTranslationPrior.h>
template <POSE>
virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z,
const gtsam::noiseModel::Base* noiseModel);
};
typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
#include <gtsam/slam/PoseRotationPrior.h>
template <POSE>
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
PoseRotationPrior(size_t key, const POSE& pose_z,
const gtsam::noiseModel::Base* noiseModel);
};
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
#include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
const gtsam::Point2& pB,
const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/dataset.h>
class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> 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<gtsam::Cal3Bundler> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<gtsam::BetweenFactor<Pose2>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose2]
class BetweenFactorPose2s {
BetweenFactorPose2s();
size_t size() const;
gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
};
gtsam::BetweenFactorPose2s parse2DFactors(string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose3]
class BetweenFactorPose3s {
BetweenFactorPose3s();
size_t size() const;
gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
#include <gtsam/slam/InitializePose3.h>
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 <gtsam/slam/KarcherMeanFactor-inl.h>
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
};
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
size_t d);
template <T = {gtsam::SO3, gtsam::SO4}>
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 <T = {gtsam::SO3, gtsam::SO4}>
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

84
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
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<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::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<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::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<double> factor(1, 2, measured_value, model);
double measured = 0.0;
BetweenFactor<double> 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<Vector3> factor(1, 2, measured_value, model);
Vector3 measured(1, 2, 3);
BetweenFactor<Vector3> 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<Vector> factor(1, 2, measured_value, model);
Vector measured(5); measured << 1, 2, 3, 4, 5;
BetweenFactor<Vector> factor(1, 2, measured, model);
}
/* ************************************************************************* */
TEST(BetweenFactor, Point3Jacobians) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
Point3 measured(1, 2, 3);
BetweenFactor<Point3> 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>(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<Rot3> 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>(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<Pose3> 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>(Vector6::Zero(), error, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
*/
/* ************************************************************************* */
int main() {

16
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -22,8 +22,10 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
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<Vector5,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector5,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

11
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
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<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> 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<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;

13
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -27,9 +27,9 @@
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind.hpp>
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<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
&OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
@ -183,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T3);
// Use the factor to calculate the derivative

15
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -16,8 +16,6 @@
#include <iostream>
#include <boost/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
@ -32,6 +30,7 @@
using namespace std;
using namespace boost;
using namespace std::placeholders;
using namespace gtsam;
typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
@ -69,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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));

10
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -12,12 +12,12 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <vector>
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<Vector3,Rot3>(
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<Vector3,Rot3>(
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<Vector,Rot3>(
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<Vector,Rot3>(
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));

5
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -30,6 +30,7 @@
#include <iostream>
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<Vector(Point3)> f = //
boost::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, _1);
std::function<Vector(Point3)> f = //
std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// Calculate using computeEP
Matrix actualE;

10
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -27,10 +27,12 @@
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
using namespace std::placeholders;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
@ -60,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) {
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
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<Vector,Point3>(
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<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
boost::none, std::placeholders::_2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);

201
gtsam/symbolic/symbolic.i Normal file
View File

@ -0,0 +1,201 @@
//*************************************************************************
// Symbolic
//*************************************************************************
namespace gtsam {
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// ###################
#include <gtsam/symbolic/SymbolicFactor.h>
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 <gtsam/symbolic/SymbolicFactorGraph.h>
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<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
eliminatePartialSequential(const gtsam::Ordering& ordering);
pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
eliminatePartialSequential(const gtsam::KeyVector& keys);
pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*>
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 <gtsam/symbolic/SymbolicConditional.h>
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 <gtsam/symbolic/SymbolicBayesNet.h>
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 <gtsam/symbolic/SymbolicBayesTree.h>
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<ConditionalType> shortcut(derived_ptr root, Eliminate function)
// const; FactorGraph<FactorType> marginal(derived_ptr root, Eliminate
// function) const; FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr
// root, Eliminate function) const;
//
void deleteCachedShortcuts();
};
#include <gtsam/inference/VariableIndex.h>
class VariableIndex {
// Standard Constructors and Named Constructors
VariableIndex();
// TODO: Templetize constructor when wrap supports it
// template<T = {gtsam::FactorGraph}>
// 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

8
gtsam_unstable/base/BTree.h
View File

@ -20,7 +20,7 @@
#include <stack>
#include <sstream>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <functional>
namespace gtsam {
@ -260,7 +260,7 @@ namespace gtsam {
}
/** iterate over tree */
void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
void iter(std::function<void(const KEY&, const VALUE&)> 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<class TO>
BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
BTree<KEY, TO> map(std::function<TO(const KEY&, const VALUE&)> f) const {
if (empty()) return BTree<KEY, TO> ();
std::pair<KEY, TO> xd(key(), f(key(), value()));
return BTree<KEY, TO> (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<class ACC>
ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
ACC fold(std::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
const ACC& a) const {
if (!root_) return a;
ACC ar = right().fold(f, a); // fold over right subtree

2
gtsam_unstable/base/DSF.h
View File

@ -122,7 +122,7 @@ public:
}
// maps f over all keys, must be invertible
DSF map(boost::function<KEY(const KEY&)> func) const {
DSF map(std::function<KEY(const KEY&)> func) const {
DSF t;
for(const KeyLabel& pair: (Tree)*this)
t = t.add(func(pair.first), func(pair.second));

6
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <boost/bind/bind.hpp>
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<Vector9, PoseRTV, PoseRTV>(
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<Vector9, PoseRTV, PoseRTV>(
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_);
}

6
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <boost/bind/bind.hpp>
namespace gtsam {
/**
@ -79,9 +81,9 @@ public:
boost::optional<Matrix&> H2 = boost::none) const override {
const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
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<Vector6, PoseRTV, PoseRTV>(
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);
}

12
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -166,24 +166,24 @@ public:
boost::optional<Matrix&> H3 = boost::none) const {
if (H1) {
(*H1) = numericalDerivative31(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);
}
if (H2) {
(*H2) = numericalDerivative32(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);
}
if (H3) {
(*H3) = numericalDerivative33(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
),
xik, xik_1, gk, 1e-5
);

8
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <boost/bind/bind.hpp>
namespace gtsam {
namespace dynamics {
@ -84,9 +86,11 @@ public:
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const override {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
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<gtsam::Vector,PoseRTV,PoseRTV>(
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_);
}

40
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -9,6 +9,7 @@
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
/* ************************************************************************* */
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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
);

Some files were not shown because too many files have changed in this diff Show More