replaced boost with std for placeholders, bind and function
Varun Agrawal
parent
4ac4302871
commit
dc8b5e58ff
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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_);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -18,8 +18,7 @@
|
|||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/bind/bind.hpp>
|
||||
#include <functional>
|
||||
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
|
@ -38,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, boost::placeholders::_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), boost::placeholders::_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
|
||||
|
|
@ -69,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(
|
||||
|
|
@ -109,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),
|
||||
|
|
@ -150,7 +149,7 @@ 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, boost::placeholders::_1), x,
|
||||
return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
|
||||
delta);
|
||||
}
|
||||
|
||||
|
|
@ -164,14 +163,14 @@ 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, boost::placeholders::_1, boost::cref(x2)), x1, delta);
|
||||
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
|
||||
}
|
||||
|
||||
/** use a raw C++ function pointer */
|
||||
|
|
@ -179,7 +178,7 @@ 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, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
|
||||
delta);
|
||||
}
|
||||
|
||||
|
|
@ -193,14 +192,14 @@ 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), boost::placeholders::_1), x2, delta);
|
||||
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
|
||||
}
|
||||
|
||||
/** use a raw C++ function pointer */
|
||||
|
|
@ -208,7 +207,7 @@ 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, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
|
||||
delta);
|
||||
}
|
||||
|
||||
|
|
@ -225,14 +224,14 @@ 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, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)),
|
||||
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
|
||||
x1, delta);
|
||||
}
|
||||
|
||||
|
|
@ -240,8 +239,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3),
|
||||
x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -258,14 +257,14 @@ 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), boost::placeholders::_1, boost::cref(x3)),
|
||||
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
|
||||
x2, delta);
|
||||
}
|
||||
|
||||
|
|
@ -273,8 +272,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3),
|
||||
x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -291,14 +290,14 @@ 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), boost::placeholders::_1),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
|
||||
x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -306,8 +305,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3),
|
||||
x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -324,15 +323,15 @@ 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, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
|
||||
boost::cref(x4)),
|
||||
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
|
||||
std::cref(x4)),
|
||||
x1, delta);
|
||||
}
|
||||
|
||||
|
|
@ -340,8 +339,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4),
|
||||
x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
|
|
@ -358,15 +357,15 @@ 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), boost::placeholders::_1, boost::cref(x3),
|
||||
boost::cref(x4)),
|
||||
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
|
||||
std::cref(x4)),
|
||||
x2, delta);
|
||||
}
|
||||
|
||||
|
|
@ -374,8 +373,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4),
|
||||
x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
|
|
@ -392,15 +391,15 @@ 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), boost::placeholders::_1,
|
||||
boost::cref(x4)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
|
||||
std::cref(x4)),
|
||||
x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -408,8 +407,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4),
|
||||
x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
|
|
@ -426,15 +425,15 @@ 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),
|
||||
boost::placeholders::_1),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::placeholders::_1),
|
||||
x4, delta);
|
||||
}
|
||||
|
||||
|
|
@ -442,8 +441,8 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4),
|
||||
x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
|
|
@ -461,15 +460,15 @@ 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, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
|
||||
boost::cref(x4), boost::cref(x5)),
|
||||
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
|
||||
std::cref(x4), std::cref(x5)),
|
||||
x1, delta);
|
||||
}
|
||||
|
||||
|
|
@ -477,9 +476,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4,
|
||||
std::placeholders::_5),
|
||||
x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
|
|
@ -497,15 +496,15 @@ 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), boost::placeholders::_1, boost::cref(x3),
|
||||
boost::cref(x4), boost::cref(x5)),
|
||||
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
|
||||
std::cref(x4), std::cref(x5)),
|
||||
x2, delta);
|
||||
}
|
||||
|
||||
|
|
@ -513,9 +512,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4,
|
||||
std::placeholders::_5),
|
||||
x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
|
|
@ -533,15 +532,15 @@ 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), boost::placeholders::_1,
|
||||
boost::cref(x4), boost::cref(x5)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
|
||||
std::cref(x4), std::cref(x5)),
|
||||
x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -549,9 +548,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4,
|
||||
std::placeholders::_5),
|
||||
x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
|
|
@ -569,15 +568,15 @@ 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),
|
||||
boost::placeholders::_1, boost::cref(x5)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::placeholders::_1, std::cref(x5)),
|
||||
x4, delta);
|
||||
}
|
||||
|
||||
|
|
@ -585,9 +584,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4,
|
||||
std::placeholders::_5),
|
||||
x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
|
|
@ -605,15 +604,15 @@ 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), boost::placeholders::_1),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::cref(x4), std::placeholders::_1),
|
||||
x5, delta);
|
||||
}
|
||||
|
||||
|
|
@ -621,9 +620,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5),
|
||||
std::bind(h, std::placeholders::_1, std::placeholders::_2,
|
||||
std::placeholders::_3, std::placeholders::_4,
|
||||
std::placeholders::_5),
|
||||
x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
|
|
@ -642,15 +641,15 @@ 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, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
|
||||
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
|
||||
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
|
||||
std::cref(x4), std::cref(x5), std::cref(x6)),
|
||||
x1, delta);
|
||||
}
|
||||
|
||||
|
|
@ -658,9 +657,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -679,15 +678,15 @@ 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), boost::placeholders::_1, boost::cref(x3),
|
||||
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
|
||||
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
|
||||
std::cref(x4), std::cref(x5), std::cref(x6)),
|
||||
x2, delta);
|
||||
}
|
||||
|
||||
|
|
@ -695,9 +694,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -716,15 +715,15 @@ 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), boost::placeholders::_1,
|
||||
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
|
||||
std::cref(x4), std::cref(x5), std::cref(x6)),
|
||||
x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -732,9 +731,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -753,15 +752,15 @@ 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),
|
||||
boost::placeholders::_1, boost::cref(x5), boost::cref(x6)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::placeholders::_1, std::cref(x5), std::cref(x6)),
|
||||
x4, delta);
|
||||
}
|
||||
|
||||
|
|
@ -769,9 +768,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -790,15 +789,15 @@ 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), boost::placeholders::_1, boost::cref(x6)),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::cref(x4), std::placeholders::_1, std::cref(x6)),
|
||||
x5, delta);
|
||||
}
|
||||
|
||||
|
|
@ -806,9 +805,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -827,7 +826,7 @@ 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),
|
||||
|
|
@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
|
|||
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), boost::placeholders::_1),
|
||||
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
|
||||
std::cref(x4), std::cref(x5), std::placeholders::_1),
|
||||
x6, delta);
|
||||
}
|
||||
|
||||
|
|
@ -844,9 +843,9 @@ 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, boost::placeholders::_1, boost::placeholders::_2,
|
||||
boost::placeholders::_3, boost::placeholders::_4,
|
||||
boost::placeholders::_5, boost::placeholders::_6),
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -859,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, boost::placeholders::_1, delta), x, delta);
|
||||
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
|
||||
|
|
@ -882,86 +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_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
|
||||
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), boost::placeholders::_1)),
|
||||
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, boost::placeholders::_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, boost::placeholders::_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), boost::placeholders::_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, boost::placeholders::_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);
|
||||
}
|
||||
|
||||
|
|
@ -971,17 +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, boost::placeholders::_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, boost::placeholders::_1, delta)),
|
||||
std::function<Vector(const X1&)>(
|
||||
std::bind(numGrad, f2, std::placeholders::_1, delta)),
|
||||
x1, delta);
|
||||
}
|
||||
|
||||
|
|
@ -989,24 +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), boost::placeholders::_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, boost::placeholders::_1, delta)),
|
||||
std::function<Vector(const X2&)>(
|
||||
std::bind(numGrad, f2, std::placeholders::_1, delta)),
|
||||
x2, delta);
|
||||
}
|
||||
|
||||
|
|
@ -1014,24 +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), boost::placeholders::_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, boost::placeholders::_1, delta)),
|
||||
std::function<Vector(const X3&)>(
|
||||
std::bind(numGrad, f2, std::placeholders::_1, delta)),
|
||||
x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -1039,41 +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, boost::placeholders::_1, boost::placeholders::_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, boost::placeholders::_1, boost::cref(x2),
|
||||
boost::placeholders::_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), boost::placeholders::_1,
|
||||
boost::placeholders::_2)),
|
||||
std::function<double(const X2&, const X3&)>(
|
||||
std::bind(f, std::cref(x1), std::placeholders::_1,
|
||||
std::placeholders::_2)),
|
||||
x2, x3, delta);
|
||||
}
|
||||
|
||||
|
|
@ -1082,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);
|
||||
}
|
||||
|
||||
|
|
@ -1090,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);
|
||||
}
|
||||
|
||||
|
|
@ -1098,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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#include <gtsam/base/Group.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <iostream> // for cout :-(
|
||||
|
||||
namespace gtsam {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -15,15 +15,12 @@
|
|||
* @author: Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/bind/bind.hpp>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/inference/LabeledSymbol.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -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, boost::placeholders::_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, boost::placeholders::_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, boost::placeholders::_1)) == c &&
|
||||
boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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_); }
|
||||
|
|
|
|||
|
|
@ -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 boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_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) {
|
||||
|
|
|
|||
|
|
@ -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 &);
|
||||
|
|
|
|||
|
|
@ -38,8 +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, boost::placeholders::_1),
|
||||
boost::bind(&KeyValuePair::first, boost::placeholders::_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.");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -83,8 +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,
|
||||
boost::placeholders::_1, boost::placeholders::_2),
|
||||
new internal::UnaryExpression<T, A>(std::bind(method,
|
||||
std::placeholders::_1, std::placeholders::_2),
|
||||
expression)) {
|
||||
}
|
||||
|
||||
|
|
@ -97,9 +97,9 @@ Expression<T>::Expression(const Expression<A1>& expression1,
|
|||
const Expression<A2>& expression2) :
|
||||
root_(
|
||||
new internal::BinaryExpression<T, A1, A2>(
|
||||
boost::bind(method, boost::placeholders::_1,
|
||||
boost::placeholders::_2, boost::placeholders::_3,
|
||||
boost::placeholders::_4),
|
||||
std::bind(method, std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3,
|
||||
std::placeholders::_4),
|
||||
expression1, expression2)) {
|
||||
}
|
||||
|
||||
|
|
@ -114,10 +114,10 @@ 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, boost::placeholders::_1,
|
||||
boost::placeholders::_2, boost::placeholders::_3,
|
||||
boost::placeholders::_4, boost::placeholders::_5,
|
||||
boost::placeholders::_6),
|
||||
std::bind(method, std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3,
|
||||
std::placeholders::_4, std::placeholders::_5,
|
||||
std::placeholders::_6),
|
||||
expression1, expression2, expression3)) {
|
||||
}
|
||||
|
||||
|
|
@ -255,9 +255,9 @@ template<typename T>
|
|||
Expression<T> operator*(const Expression<T>& expression1,
|
||||
const Expression<T>& expression2) {
|
||||
return Expression<T>(
|
||||
boost::bind(internal::apply_compose<T>(), boost::placeholders::_1,
|
||||
boost::placeholders::_2, boost::placeholders::_3,
|
||||
boost::placeholders::_4),
|
||||
std::bind(internal::apply_compose<T>(), std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3,
|
||||
std::placeholders::_4),
|
||||
expression1, expression2);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -68,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,
|
||||
|
|
@ -239,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 =
|
||||
|
|
|
|||
|
|
@ -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,8 +87,8 @@ public:
|
|||
* Constructor - forces exact evaluation
|
||||
*/
|
||||
NonlinearEquality(Key j, const T& feasible,
|
||||
const CompareFunction &_compare = boost::bind(traits<T>::Equals,
|
||||
boost::placeholders::_1, boost::placeholders::_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) {
|
||||
|
|
@ -98,8 +98,8 @@ public:
|
|||
* Constructor - allows inexact evaluation
|
||||
*/
|
||||
NonlinearEquality(Key j, const T& feasible, double error_gain,
|
||||
const CompareFunction &_compare = boost::bind(traits<T>::Equals,
|
||||
boost::placeholders::_1, boost::placeholders::_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) {
|
||||
|
|
|
|||
|
|
@ -103,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;
|
||||
|
||||
|
|
@ -113,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;
|
||||
|
||||
|
|
@ -134,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(
|
||||
|
|
@ -205,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.
|
||||
|
|
@ -236,35 +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,
|
||||
boost::placeholders::_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, boost::placeholders::_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);
|
||||
|
|
|
|||
|
|
@ -108,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;
|
||||
|
||||
|
|
@ -321,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.
|
||||
|
|
@ -344,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.
|
||||
|
|
@ -360,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.
|
||||
|
|
@ -382,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>
|
||||
|
|
@ -399,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));
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -91,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, boost::placeholders::_1, boost::placeholders::_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, boost::placeholders::_1, boost::placeholders::_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_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -81,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, boost::placeholders::_1, boost::placeholders::_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, boost::placeholders::_1, boost::placeholders::_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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
);
|
||||
|
|
|
|||
|
|
@ -86,11 +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_, boost::placeholders::_1,
|
||||
boost::placeholders::_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_, boost::placeholders::_1,
|
||||
boost::placeholders::_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_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include <gtsam_unstable/linear/QPSParser.h>
|
||||
#include <gtsam_unstable/linear/QPSParserException.h>
|
||||
|
||||
#include <boost/bind/bind.hpp>
|
||||
#include <boost/fusion/include/vector.hpp>
|
||||
#include <boost/fusion/sequence.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
|
@ -40,7 +39,7 @@
|
|||
#include <vector>
|
||||
|
||||
using boost::fusion::at_c;
|
||||
using namespace boost::placeholders;
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
|
||||
namespace bf = boost::fusion;
|
||||
|
|
@ -412,50 +411,50 @@ typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
|
|||
struct QPSParser::MPSGrammar : base_grammar {
|
||||
typedef std::vector<char> Chars;
|
||||
QPSVisitor *rqp_;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars> const &)> setName;
|
||||
boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)>
|
||||
std::function<void(bf::vector<Chars, Chars, Chars> const &)> setName;
|
||||
std::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)>
|
||||
addRow;
|
||||
boost::function<void(
|
||||
std::function<void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
|
||||
rhsSingle;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars, Chars, Chars, double>)>
|
||||
rhsDouble;
|
||||
boost::function<void(
|
||||
std::function<void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
|
||||
rangeSingle;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars, Chars, Chars, double>)>
|
||||
rangeDouble;
|
||||
boost::function<void(
|
||||
std::function<void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)>
|
||||
colSingle;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
|
||||
std::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
|
||||
Chars, Chars, double> const &)>
|
||||
colDouble;
|
||||
boost::function<void(
|
||||
std::function<void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
|
||||
addQuadTerm;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
|
||||
std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
|
||||
Chars, double> const &)>
|
||||
addBound;
|
||||
boost::function<void(
|
||||
std::function<void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)>
|
||||
addFreeBound;
|
||||
MPSGrammar(QPSVisitor *rqp)
|
||||
: base_grammar(start),
|
||||
rqp_(rqp),
|
||||
setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)),
|
||||
addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)),
|
||||
rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)),
|
||||
rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)),
|
||||
rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)),
|
||||
rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)),
|
||||
colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)),
|
||||
colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)),
|
||||
addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)),
|
||||
addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)),
|
||||
addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) {
|
||||
setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)),
|
||||
addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)),
|
||||
rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)),
|
||||
rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)),
|
||||
rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)),
|
||||
rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)),
|
||||
colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)),
|
||||
colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)),
|
||||
addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)),
|
||||
addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)),
|
||||
addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) {
|
||||
using namespace boost::spirit;
|
||||
using namespace boost::spirit::qi;
|
||||
character = lexeme[alnum | '_' | '-' | '.'];
|
||||
|
|
|
|||
|
|
@ -309,12 +309,12 @@ public:
|
|||
// Jacobian w.r.t. Pose1
|
||||
if (H1){
|
||||
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
Pose1);
|
||||
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
Pose1);
|
||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||
}
|
||||
|
|
@ -323,12 +323,12 @@ public:
|
|||
if (H2){
|
||||
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
|
||||
Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
Vel1);
|
||||
Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
Vel1);
|
||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||
}
|
||||
|
|
@ -336,12 +336,12 @@ public:
|
|||
// Jacobian w.r.t. IMUBias1
|
||||
if (H3){
|
||||
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
|
||||
Bias1);
|
||||
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
|
||||
Bias1);
|
||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||
}
|
||||
|
|
@ -349,12 +349,12 @@ public:
|
|||
// Jacobian w.r.t. Pose2
|
||||
if (H4){
|
||||
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
|
||||
Pose2);
|
||||
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
|
||||
Pose2);
|
||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||
}
|
||||
|
|
@ -363,12 +363,12 @@ public:
|
|||
if (H5){
|
||||
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
|
||||
Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
|
||||
Vel2);
|
||||
Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
|
||||
std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
|
||||
Vel2);
|
||||
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
||||
}
|
||||
|
|
@ -469,43 +469,43 @@ public:
|
|||
Matrix I_3x3 = I_3x3;
|
||||
|
||||
Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
|
||||
boost::placeholders::_1, delta_vel_in_t0),
|
||||
std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
|
||||
std::placeholders::_1, delta_vel_in_t0),
|
||||
delta_pos_in_t0);
|
||||
Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
|
||||
delta_pos_in_t0, boost::placeholders::_1),
|
||||
std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
|
||||
delta_pos_in_t0, std::placeholders::_1),
|
||||
delta_vel_in_t0);
|
||||
Matrix H_pos_angles = Z_3x3;
|
||||
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
|
||||
|
||||
Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1,
|
||||
std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
msr_acc_t, msr_dt, delta_angles, std::placeholders::_1,
|
||||
flag_use_body_P_sensor, body_P_sensor, Bias_t0),
|
||||
delta_vel_in_t0);
|
||||
Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0,
|
||||
std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
msr_acc_t, msr_dt, std::placeholders::_1, delta_vel_in_t0,
|
||||
flag_use_body_P_sensor, body_P_sensor, Bias_t0),
|
||||
delta_angles);
|
||||
Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
|
||||
msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0,
|
||||
flag_use_body_P_sensor, body_P_sensor,
|
||||
boost::placeholders::_1),
|
||||
std::placeholders::_1),
|
||||
Bias_t0);
|
||||
Matrix H_vel_pos = Z_3x3;
|
||||
|
||||
Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
|
||||
msr_dt, boost::placeholders::_1, flag_use_body_P_sensor,
|
||||
std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
|
||||
msr_dt, std::placeholders::_1, flag_use_body_P_sensor,
|
||||
body_P_sensor, Bias_t0),
|
||||
delta_angles);
|
||||
Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(
|
||||
boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
|
||||
std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
|
||||
msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor,
|
||||
boost::placeholders::_1),
|
||||
std::placeholders::_1),
|
||||
Bias_t0);
|
||||
Matrix H_angles_pos = Z_3x3;
|
||||
Matrix H_angles_vel = Z_3x3;
|
||||
|
|
|
|||
|
|
@ -278,29 +278,29 @@ public:
|
|||
// TODO: Write analytical derivative calculations
|
||||
// Jacobian w.r.t. Pose1
|
||||
if (H1){
|
||||
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
|
||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Vel1
|
||||
if (H2){
|
||||
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
|
||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Pose2
|
||||
if (H3){
|
||||
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
|
||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Vel2
|
||||
if (H4){
|
||||
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
|
||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||
}
|
||||
|
||||
|
|
@ -372,15 +372,15 @@ public:
|
|||
Matrix Z_3x3 = Z_3x3;
|
||||
Matrix I_3x3 = I_3x3;
|
||||
|
||||
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
||||
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
||||
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
|
||||
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
|
||||
Matrix H_pos_angles = Z_3x3;
|
||||
|
||||
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
|
||||
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
|
||||
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||
Matrix H_vel_pos = Z_3x3;
|
||||
|
||||
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
|
||||
Matrix H_angles_pos = Z_3x3;
|
||||
Matrix H_angles_vel = Z_3x3;
|
||||
|
||||
|
|
|
|||
|
|
@ -236,12 +236,12 @@ public:
|
|||
// Jacobian w.r.t. Pose1
|
||||
if (H1){
|
||||
Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
Pose1);
|
||||
Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
|
||||
Pose1);
|
||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||
}
|
||||
|
|
@ -250,12 +250,12 @@ public:
|
|||
if (H2){
|
||||
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
|
||||
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
Vel1);
|
||||
Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2),
|
||||
Vel1);
|
||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||
}
|
||||
|
|
@ -263,12 +263,12 @@ public:
|
|||
// Jacobian w.r.t. IMUBias1
|
||||
if (H3){
|
||||
Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
|
||||
Bias1);
|
||||
Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2),
|
||||
Bias1);
|
||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||
}
|
||||
|
|
@ -276,12 +276,12 @@ public:
|
|||
// Jacobian w.r.t. Pose2
|
||||
if (H4){
|
||||
Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
|
||||
Pose2);
|
||||
Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2),
|
||||
Pose2);
|
||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||
}
|
||||
|
|
@ -290,12 +290,12 @@ public:
|
|||
if (H5){
|
||||
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
|
||||
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
|
||||
Vel2);
|
||||
Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
|
||||
std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
|
||||
this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1),
|
||||
Vel2);
|
||||
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -108,14 +108,14 @@ public:
|
|||
|
||||
if (H1) {
|
||||
(*H1) = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this,
|
||||
boost::placeholders::_1, landmark),
|
||||
std::bind(&InvDepthFactorVariant1::inverseDepthError, this,
|
||||
std::placeholders::_1, landmark),
|
||||
pose);
|
||||
}
|
||||
if (H2) {
|
||||
(*H2) = numericalDerivative11<Vector, Vector6>(
|
||||
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
|
||||
boost::placeholders::_1), landmark);
|
||||
std::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
|
||||
std::placeholders::_1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose, landmark);
|
||||
|
|
|
|||
|
|
@ -111,13 +111,13 @@ public:
|
|||
|
||||
if (H1) {
|
||||
(*H1) = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this,
|
||||
boost::placeholders::_1, landmark), pose);
|
||||
std::bind(&InvDepthFactorVariant2::inverseDepthError, this,
|
||||
std::placeholders::_1, landmark), pose);
|
||||
}
|
||||
if (H2) {
|
||||
(*H2) = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
|
||||
boost::placeholders::_1), landmark);
|
||||
std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
|
||||
std::placeholders::_1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose, landmark);
|
||||
|
|
|
|||
|
|
@ -111,14 +111,14 @@ public:
|
|||
|
||||
if(H1) {
|
||||
(*H1) = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
|
||||
boost::placeholders::_1, landmark),
|
||||
std::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
|
||||
std::placeholders::_1, landmark),
|
||||
pose);
|
||||
}
|
||||
if(H2) {
|
||||
(*H2) = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
|
||||
boost::placeholders::_1),
|
||||
std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
|
||||
std::placeholders::_1),
|
||||
landmark);
|
||||
}
|
||||
|
||||
|
|
@ -238,20 +238,20 @@ public:
|
|||
|
||||
if(H1)
|
||||
(*H1) = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
|
||||
boost::placeholders::_1, pose2, landmark),
|
||||
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
|
||||
std::placeholders::_1, pose2, landmark),
|
||||
pose1);
|
||||
|
||||
if(H2)
|
||||
(*H2) = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
||||
boost::placeholders::_1, landmark),
|
||||
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
||||
std::placeholders::_1, landmark),
|
||||
pose2);
|
||||
|
||||
if(H3)
|
||||
(*H3) = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
||||
pose2, boost::placeholders::_1),
|
||||
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
||||
pose2, std::placeholders::_1),
|
||||
landmark);
|
||||
|
||||
return inverseDepthError(pose1, pose2, landmark);
|
||||
|
|
|
|||
Loading…
Reference in New Issue