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

Merge branch 'develop' into feature/wrap-multiple-interfaces

release/4.3a0
Varun Agrawal 2021-07-14 21:56:09 -04:00
parent 6919ad9277 740c9c6f39
commit 2dd22c64fd
98 changed files with 1326 additions and 874 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/Matrix.h
View File

@ -29,7 +29,7 @@
#include <gtsam/config.h>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
// The function phi should calculate f(a)*b, with derivatives in a and b.
// Naturally, the derivative in b is f(a).
typedef boost::function<VectorN(
typedef std::function<VectorN(
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
Operator;

7
gtsam/base/Testable.h
View File

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

2
gtsam/base/lieProxies.h
View File

@ -24,7 +24,7 @@
*
* These should not be used outside of tests, as they are just remappings
* of the original functions. We use these to avoid needing to do
* too much boost::bind magic or writing a bunch of separate proxy functions.
* too much std::bind magic or writing a bunch of separate proxy functions.
*
* Don't expect all classes to work for all of these functions.
*/

345
gtsam/base/numericalDerivative.h
View File

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

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

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

12
gtsam/discrete/DecisionTree.h
View File

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

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

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

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

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

14
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
// Apply inverse camera matrix to map the pixel coordinate (u, v)
// of the equidistant fisheye image to angular coordinate space (xd, yd)
// with radius theta given in radians.
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
Point2 pi(xd, yd);
const double theta = sqrt(xd * xd + yd * yd);
// Provide initial guess for the Gauss-Newton search.
// The angular coordinates given by (xd, yd) are mapped back to
// the focal plane of the perspective undistorted projection pi.
// See Cal3Unified.calibrate() using the same pattern for the
// undistortion of omnidirectional fisheye projection.
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
Point2 pi(scale * xd, scale * yd);
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached

2
gtsam/geometry/Cyclic.h
View File

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

1
gtsam/geometry/SimpleCamera.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>

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

@ -20,12 +20,11 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) {
EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
// Check derivative
boost::function<CalibratedCamera(Pose3)> f = //
boost::bind(CalibratedCamera::Create, _1, boost::none);
std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

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

@ -5,16 +5,15 @@
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <sstream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -42,14 +41,14 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
boost::none),
std::bind(EssentialMatrix::FromRotationAndDirection,
std::placeholders::_1, trueDirection, boost::none, boost::none),
trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
boost::none),
std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
std::placeholders::_1, boost::none, boost::none),
trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

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

@ -21,10 +21,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using boost::none;
@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
boost::function<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);

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

@ -22,13 +22,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) {
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
// Check derivative
boost::function<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

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

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

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

@ -14,14 +14,14 @@
* @brief Unit tests for Point3 class
*/
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
@ -101,7 +101,7 @@ TEST( Point3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Point3&, const Point3&)> f =
std::function<double(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
{
gtsam::dot(p, q, H1, H2);
@ -123,8 +123,9 @@ TEST( Point3, dot) {
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@ -142,7 +143,8 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
gtsam::cross(p, q, H1, H2);
@ -163,7 +165,7 @@ TEST (Point3, normalize) {
Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(gtsam::normalize, _1, boost::none), point);
std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

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

@ -22,8 +22,7 @@
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
#include <CppUnitLite/TestHarness.h>
#include <cmath>
@ -215,7 +214,7 @@ TEST(Pose3, translation) {
EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Point3, Pose3>(
boost::bind(&Pose3::translation, _1, boost::none), T);
std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -226,7 +225,7 @@ TEST(Pose3, rotation) {
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
boost::bind(&Pose3::rotation, _1, boost::none), T);
std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -1052,7 +1051,9 @@ TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

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

@ -15,15 +15,12 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) {
TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) {
TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) {
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) {
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) {
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
@ -357,7 +354,7 @@ TEST(SO3, vec) {
Matrix actualH;
const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
std::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
CHECK(assert_equal(numericalH, actualH));
}
@ -371,7 +368,7 @@ TEST(Matrix, compose) {
Matrix actualH;
const Matrix3 actual = so3::compose(M, R, actualH);
CHECK(assert_equal(expected, actual));
boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);

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

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

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

@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
/// Test Jacobian of Retract at origin
TEST(SOn, RetractJacobian) {
Matrix actualH = RetractJacobian(3);
boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
std::function<Matrix(const Vector &)> h = [](const Vector &v) {
return SOn::ChartAtOrigin::Retract(v).matrix();
};
Vector3 v;
@ -205,7 +205,7 @@ TEST(SOn, vec) {
SOn Q = SOn::ChartAtOrigin::Retract(v);
Matrix actualH;
const Vector actual = Q.vec(actualH);
boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}

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

@ -16,24 +16,22 @@
* @author Zhaoyang Lv
*/
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <functional>
#include <boost/bind/bind.hpp>
#include <boost/function.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using symbol_shorthand::X;
@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) {
EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
// Test derivative
boost::function<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transformFrom, _1, _2, boost::none, boost::none);
// Use lambda to resolve overloaded method
std::function<Point3(const Similarity3&, const Point3&)>
f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
Point3 q(1, 2, 3);
for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {

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

@ -32,13 +32,12 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
#include <cmath>
#include <random>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using gtsam::symbol_shorthand::U;
@ -127,7 +126,8 @@ TEST(Unit3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
@ -158,13 +158,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -185,25 +185,33 @@ TEST(Unit3, error2) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -221,13 +229,13 @@ TEST(Unit3, distance) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), q);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q);
p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), r);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r);
p.distance(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -319,7 +327,7 @@ TEST(Unit3, basis) {
Matrix62 actualH;
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
// without H, first time
EXPECT(assert_equal(expected, p.basis(), 1e-6));
@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) {
p.basis(actualH);
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
}
@ -376,8 +384,8 @@ TEST(Unit3, retract) {
TEST (Unit3, jacobian_retract) {
Matrix22 H;
Unit3 p;
boost::function<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
std::function<Unit3(const Vector2&)> f =
std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none);
{
Vector2 v (-0.2, 0.1);
p.retract(v, H);
@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) {
Unit3 expected(point);
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Unit3, Point3>(
boost::bind(Unit3::FromPoint3, _1, boost::none), point);
std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

4
gtsam/inference/EliminateableFactorGraph.h
View File

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

45
gtsam/inference/LabeledSymbol.cpp
View File

@ -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));
}
/* ************************************************************************* */

8
gtsam/inference/LabeledSymbol.h
View File

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

7
gtsam/inference/Symbol.cpp
View File

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

7
gtsam/inference/Symbol.h
View File

@ -18,11 +18,12 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <cstdint>
#include <functional>
namespace gtsam {
@ -114,7 +115,7 @@ public:
* Values::filter() function to retrieve all key-value pairs with the
* requested character.
*/
static boost::function<bool(Key)> ChrTest(unsigned char c);
static std::function<bool(Key)> ChrTest(unsigned char c);
/// Output stream operator that can be used with key_formatter (see Key.h).
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &);

4
gtsam/linear/VectorValues.cpp
View File

@ -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.");
}

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

@ -32,7 +32,7 @@
#include <sstream>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -270,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

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

@ -21,7 +21,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/bind/bind.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/numericalDerivative.h>
@ -30,7 +29,7 @@
#include <gtsam/linear/GaussianConditional.h>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -260,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();

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

@ -25,10 +25,9 @@
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include <list>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -175,17 +174,17 @@ TEST(AHRSFactor, Error) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -234,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -269,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
@ -294,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -368,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Compute numerical derivatives
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
@ -410,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -459,8 +458,8 @@ TEST (AHRSFactor, predictTest) {
// AHRSFactor::PreintegratedMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pim, _1, boost::none), bias);
std::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias);
// Actual Jacobians
Matrix H;

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

@ -25,7 +25,7 @@
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -51,7 +51,8 @@ TEST( Rot3AttitudeFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
std::bind(&Rot3AttitudeFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
nRb);
// Use the factor to calculate the derivative
@ -117,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
boost::none), T);
// Use the factor to calculate the derivative

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

@ -27,7 +27,7 @@
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative
Matrix actualH;
@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>(
boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative
Matrix actualH;

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

@ -19,9 +19,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -128,8 +127,9 @@ TEST(ImuBias, operatorSubB) {
TEST(ImuBias, Correct1) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctAccelerometer, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
@ -139,8 +139,9 @@ TEST(ImuBias, Correct1) {
TEST(ImuBias, Correct2) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctGyroscope, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));

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

@ -146,9 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Matrix9 aH1, aH2;
Matrix96 aH3;
actual.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
boost::bind(&PreintegrationBase::computeError, actual,
boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3,
std::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
std::bind(&PreintegrationBase::computeError, actual,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
boost::none, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@ -204,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
Matrix96 actualH;
pim.biasCorrectedDelta(kZeroBias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
boost::placeholders::_1, boost::none), kZeroBias);
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), kZeroBias);
EXPECT(assert_equal(expectedH, actualH));
Matrix9 aH1;
Matrix96 aH2;
NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>(
boost::bind(&PreintegrationBase::predict, pim, boost::placeholders::_1,
std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
kZeroBias, boost::none, boost::none), state1);
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, Bias>(
boost::bind(&PreintegrationBase::predict, pim, state1,
boost::placeholders::_1, boost::none, boost::none), kZeroBias);
std::bind(&PreintegrationBase::predict, pim, state1,
std::placeholders::_1, boost::none, boost::none), kZeroBias);
EXPECT(assert_equal(eH2, aH2));
}
@ -278,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Make sure rotation part is correct when error is interpreted as axis-angle
// Jacobians are around zero, so the rotation part is the same as:
Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, boost::placeholders::_1, v1, x2, v2, kZeroBias),
std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias),
x1);
EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias),
std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias),
x2);
EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
@ -333,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
Matrix96 actualH;
pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim,
boost::placeholders::_1, boost::none), bias);
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
// Create factor
@ -522,7 +522,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
boost::none, D_correctedAcc_measuredOmega, boost::none);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1),
std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
measuredOmega, 1e-6);
EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
@ -534,15 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
//
// Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// boost::placeholders::_1, boost::placeholders::_2,
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG1, G1, 1e-5));
//
// Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// boost::placeholders::_1, boost::placeholders::_2,
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG2, G2, 1e-5));

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

@ -26,7 +26,7 @@
#include <GeographicLib/LocalCartesian.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) {
Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
(boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
(std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
}
// *************************************************************************
@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) {
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
(std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
(std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
(std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
(std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
// MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
(std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
(std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
(std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7));
}

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

@ -17,9 +17,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/MagPoseFactor.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
// *****************************************************************************
@ -79,7 +77,10 @@ TEST(MagPoseFactor, JacobianPose2) {
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P2_b),
H2, 1e-7));
}
// *****************************************************************************
@ -90,7 +91,10 @@ TEST(MagPoseFactor, JacobianPose3) {
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P3_b),
H3, 1e-7));
}
// *****************************************************************************
@ -104,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
@ -118,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************

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

@ -22,11 +22,10 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include "imuFactorTesting.h"
using namespace boost::placeholders;
using namespace std::placeholders;
namespace testing {
// Create default parameters with Z-down and above noise parameters
@ -43,21 +42,21 @@ static boost::shared_ptr<PreintegrationParams> Params() {
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
@ -98,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) {
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
f = std::bind(&ManifoldPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));

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

@ -23,7 +23,7 @@
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -39,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */
TEST(NavState, Constructor) {
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none);
Matrix aH1, aH2, aH3;
EXPECT(
assert_equal(kState1,
@ -59,9 +59,9 @@ TEST(NavState, Constructor) {
/* ************************************************************************* */
TEST(NavState, Constructor2) {
boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
std::function<NavState(const Pose3&, const Vector3&)> construct =
std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
Matrix aH1, aH2;
EXPECT(
assert_equal(kState1,
@ -76,7 +76,7 @@ TEST( NavState, Attitude) {
Rot3 actual = kState1.attitude(aH);
EXPECT(assert_equal(actual, kAttitude));
eH = numericalDerivative11<Rot3, NavState>(
boost::bind(&NavState::attitude, _1, boost::none), kState1);
std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -86,7 +86,8 @@ TEST( NavState, Position) {
Point3 actual = kState1.position(aH);
EXPECT(assert_equal(actual, kPosition));
eH = numericalDerivative11<Point3, NavState>(
boost::bind(&NavState::position, _1, boost::none), kState1);
std::bind(&NavState::position, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -96,7 +97,8 @@ TEST( NavState, Velocity) {
Velocity3 actual = kState1.velocity(aH);
EXPECT(assert_equal(actual, kVelocity));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::velocity, _1, boost::none), kState1);
std::bind(&NavState::velocity, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -106,7 +108,8 @@ TEST( NavState, BodyVelocity) {
Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -137,8 +140,9 @@ TEST( NavState, Manifold ) {
// Check retract derivatives
Matrix9 aH1, aH2;
kState1.retract(xi, aH1, aH2);
boost::function<NavState(const NavState&, const Vector9&)> retract =
boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
std::function<NavState(const NavState&, const Vector9&)> retract =
std::bind(&NavState::retract, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
@ -149,9 +153,9 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
// Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
std::function<Vector9(const NavState&, const NavState&)> local =
std::bind(&NavState::localCoordinates, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@ -168,8 +172,9 @@ TEST( NavState, Manifold ) {
/* ************************************************************************* */
static const double dt = 2.0;
boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
std::function<Vector9(const NavState&, const bool&)> coriolis =
std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
std::placeholders::_2, boost::none);
TEST(NavState, Coriolis) {
Matrix9 aH;
@ -244,9 +249,10 @@ TEST(NavState, CorrectPIM) {
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5;
Matrix9 aH1, aH2;
boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
std::bind(&NavState::correctPIM, std::placeholders::_1,
std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
boost::none, boost::none);
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));

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

@ -19,10 +19,9 @@
#include <gtsam/navigation/Scenario.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include <cmath>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -148,7 +147,7 @@ TEST(Scenario, Accelerating) {
{
// Check acceleration in nav
Matrix expected = numericalDerivative11<Vector3, double>(
boost::bind(&Scenario::velocity_n, scenario, _1), T);
std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T);
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
}

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

@ -22,11 +22,10 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include "imuFactorTesting.h"
using namespace boost::placeholders;
using namespace std::placeholders;
static const double kDt = 0.1;
@ -78,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) {
TEST(ImuFactor, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
TangentPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
@ -105,10 +104,12 @@ TEST(TangentPreintegration, computeError) {
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
f = std::bind(&TangentPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@ -121,7 +122,7 @@ TEST(TangentPreintegration, Compose) {
TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
boost::function<Vector9(const Vector9&, const Vector9&)> f =
std::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
};

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

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

8
gtsam/nonlinear/Expression.h
View File

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

10
gtsam/nonlinear/NonlinearEquality.h
View File

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

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

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

18
gtsam/nonlinear/Values.h
View File

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

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

@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> underneath
* allowing Expressions to have polymorphic behaviour even though they
* are passed by value. This is the same way boost::function works.
* are passed by value. This is the same way std::function works.
* http://loki-lib.sourceforge.net/html/a00652.html
*/
template<class T>

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

@ -495,7 +495,7 @@ TEST(Expression, Subtract) {
/* ************************************************************************* */
TEST(Expression, LinearExpression) {
const Key key(67);
const boost::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const std::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const Matrix3 kIdentity = I_3x3;
const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);

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

@ -34,7 +34,7 @@
#include <type_traits>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
@ -336,7 +336,7 @@ TEST(Values, filter) {
// Filter by key
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::Filtered<Value> filtered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
EXPECT_LONGS_EQUAL(2, (long)filtered.size());
for(const auto key_value: filtered) {
if(i == 0) {
@ -364,7 +364,7 @@ TEST(Values, filter) {
EXPECT(assert_equal(expectedSubValues1, actualSubValues1));
// ConstFilter by Key
Values::ConstFiltered<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::ConstFiltered<Value> constfiltered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
Values fromconstfiltered(constfiltered);
EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));

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

@ -28,7 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -265,9 +265,9 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&factorError2D, _1, point, factor), pose);
std::bind(&factorError2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorError2D, pose, _1, factor), point);
std::bind(&factorError2D, pose, std::placeholders::_1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -296,9 +296,9 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -323,9 +323,9 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&factorError3D, _1, point, factor), pose);
std::bind(&factorError3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError3D, pose, _1, factor), point);
std::bind(&factorError3D, pose, std::placeholders::_1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -355,9 +355,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));

30
gtsam/sfm/ShonanAveraging.cpp
View File

@ -944,6 +944,36 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
parameters.getUseHuber()),
parameters) {}
// Extract Rot2 measurement from Pose2 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
const BetweenFactor<Pose2>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot2> can only convert Pose2 measurements "
"with Gaussian noise models.");
const Matrix3 M = gaussian->covariance();
auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2));
return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
model);
}
static ShonanAveraging2::Measurements extractRot2Measurements(
const BetweenFactorPose2s &factors) {
ShonanAveraging2::Measurements result;
result.reserve(factors.size());
for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f));
return result;
}
ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors,
const Parameters &parameters)
: ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors),
parameters.getUseHuber()),
parameters) {}
/* ************************************************************************* */
// Explicit instantiation for d=3
template class ShonanAveraging<3>;

2
gtsam/sfm/ShonanAveraging.h
View File

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

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

@ -20,10 +20,9 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -91,9 +90,9 @@ TEST(TranslationFactor, Jacobian) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, _1, T2, factor), T1);
std::bind(&factorError, std::placeholders::_1, T2, factor), T1);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, T1, _1, factor), T2);
std::bind(&factorError, T1, std::placeholders::_1, factor), T2);
// Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));

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

@ -14,7 +14,7 @@
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
@ -36,14 +36,15 @@ TEST(BetweenFactor, Rot3) {
EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none)),
R1, R2, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none)), R1, R2, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}

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

@ -23,10 +23,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -54,11 +53,13 @@ TEST( EssentialMatrixConstraint, test ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);
std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

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

@ -17,9 +17,7 @@
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -104,8 +102,8 @@ TEST(EssentialMatrixFactor, factor) {
TEST(EssentialMatrixFactor, ExpressionFactor) {
Key key(1);
for (size_t i = 0; i < 5; i++) {
boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression
@ -130,9 +128,9 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
Key key(1);
for (size_t i = 0; i < 5; i++) {
boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;

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

@ -27,10 +27,9 @@
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
@ -145,8 +144,9 @@ TEST( OrientedPlane3Factor, Derivatives ) {
OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
// Calculate numerical derivatives
boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
&OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
@ -184,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T3);
// Use the factor to calculate the derivative

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

@ -16,8 +16,6 @@
#include <iostream>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
@ -32,7 +30,7 @@
using namespace std;
using namespace boost;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
@ -70,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
EXPECT(assert_equal(numericalDF, actualDF));
@ -102,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
global, trans, local, 1e-5);
EXPECT(assert_equal(numericalDF, actualDF));

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

@ -13,12 +13,11 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
#include <vector>
using namespace std;
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
static const double kDegree = M_PI / 180;
@ -73,13 +72,13 @@ TEST (RotateFactor, test) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector3,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Vector3,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
@ -144,14 +143,14 @@ TEST (RotateDirectionsFactor, test) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));

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

@ -27,11 +27,10 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <boost/bind/bind.hpp>
#include <iostream>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
static const double rankTol = 1.0;
// Create a noise model for the pixel error
@ -132,8 +131,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
// Calculate expected derivative for point (easiest to check)
boost::function<Vector(Point3)> f = //
boost::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, _1);
std::function<Vector(Point3)> f = //
std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// Calculate using computeEP
Matrix actualE;

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

@ -32,7 +32,7 @@
using namespace std;
using namespace gtsam;
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
@ -62,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) {
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
@ -86,13 +86,15 @@ TEST( triangulation, TriangulationFactorStereo ) {
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
// compare same problem against expression factor
Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
boost::none, std::placeholders::_2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);

8
gtsam_unstable/base/BTree.h
View File

@ -20,7 +20,7 @@
#include <stack>
#include <sstream>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <functional>
namespace gtsam {
@ -260,7 +260,7 @@ namespace gtsam {
}
/** iterate over tree */
void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
void iter(std::function<void(const KEY&, const VALUE&)> f) const {
if (!root_) return;
left().iter(f);
f(key(), value());
@ -269,7 +269,7 @@ namespace gtsam {
/** map key-values in tree over function f that computes a new value */
template<class TO>
BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
BTree<KEY, TO> map(std::function<TO(const KEY&, const VALUE&)> f) const {
if (empty()) return BTree<KEY, TO> ();
std::pair<KEY, TO> xd(key(), f(key(), value()));
return BTree<KEY, TO> (left().map(f), xd, right().map(f));
@ -282,7 +282,7 @@ namespace gtsam {
* The associated values are passed to [f] in reverse sort order
*/
template<class ACC>
ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
ACC fold(std::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
const ACC& a) const {
if (!root_) return a;
ACC ar = right().fold(f, a); // fold over right subtree

2
gtsam_unstable/base/DSF.h
View File

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

4
gtsam_unstable/dynamics/FullIMUFactor.h
View File

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

4
gtsam_unstable/dynamics/IMUFactor.h
View File

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

12
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

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

8
gtsam_unstable/dynamics/VelocityConstraint.h
View File

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

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

@ -3,14 +3,13 @@
* @author Duy-Nguyen Ta
*/
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
/* ************************************************************************* */
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -58,19 +57,28 @@ TEST( Reconstruction, evaluateError) {
assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
std::bind(&Reconstruction::evaluateError, constraint,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
g2, g1, V1_g1, 1e-5);
Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
std::bind(&Reconstruction::evaluateError, constraint,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
g2, g1, V1_g1, 1e-5);
Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
std::bind(&Reconstruction::evaluateError, constraint,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
g2, g1, V1_g1, 1e-5);
EXPECT(assert_equal(numericalH1,H1,1e-5));
EXPECT(assert_equal(numericalH2,H2,1e-5));
@ -111,22 +119,22 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);

8
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -23,9 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -67,11 +65,11 @@ TEST(Event, Derivatives) {
Matrix13 actualH2;
kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>(
boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none),
exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>(
boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none),
microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}

47
gtsam_unstable/linear/QPSParser.cpp
View File

@ -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 | '_' | '-' | '.'];

68
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

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

26
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

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

40
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

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

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

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

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

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

20
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

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

6
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -190,12 +190,12 @@ TEST (BetweenFactorEM, jacobian ) {
// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
// try to check numerical derivatives of a standard between factor
Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

17
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
View File

@ -10,10 +10,9 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/slam/BiasedGPSFactor.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
@ -68,15 +67,17 @@ TEST(BiasedGPSFactor, jacobian) {
factor.evaluateError(pose,bias, actualH1, actualH2);
Matrix numericalH1 = numericalDerivative21(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
std::function<Vector(const Pose3&, const Point3&)>(std::bind(
&BiasedGPSFactor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none)),
pose, bias, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
std::function<Vector(const Pose3&, const Point3&)>(std::bind(
&BiasedGPSFactor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none)),
pose, bias, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}

2
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -26,7 +26,7 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;

12
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -23,9 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -108,9 +106,13 @@ TEST (GaussMarkovFactor, jacobian ) {
// Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
Matrix numerical_H1, numerical_H2;
numerical_H1 = numericalDerivative21<Vector3, Vector3, Vector3>(
boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2,
factor),
v1_upd, v2_upd);
numerical_H2 = numericalDerivative22<Vector3, Vector3, Vector3>(
boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2,
factor),
v1_upd, v2_upd);
// Verify they are equal for this choice of state
CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));

45
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -16,7 +16,6 @@
*/
#include <iostream>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
@ -26,7 +25,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -251,7 +250,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
//
// Vector3 v(predictionRq(angles, q));
//
// J_expected = numericalDerivative11<Vector3, Vector3>(boost::bind(&predictionRq, _1, q), angles);
// J_expected = numericalDerivative11<Vector3, Vector3>(std::bind(&predictionRq, std::placeholders::_1, q), angles);
//
// cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl;
@ -312,19 +311,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
H5_expectedPose;
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
Vel2);
// Verify they are equal for this choice of state
@ -346,19 +345,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
H5_expectedVel;
H1_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
Vel2);
// Verify they are equal for this choice of state
@ -644,19 +643,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
H5_expectedPose;
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
Vel2);
// Verify they are equal for this choice of state
@ -678,19 +677,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
H5_expectedVel;
H1_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor),
Vel2);
// Verify they are equal for this choice of state

10
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -24,9 +24,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
@ -143,8 +141,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) {
LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey);
// Calculate numerical derivatives
auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
_1, _2, _3, boost::none, boost::none, boost::none);
auto f =
std::bind(&LocalOrientedPlane3Factor::evaluateError, factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,

26
gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
View File

@ -16,10 +16,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -83,7 +82,9 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor2::evaluateError, &factor,
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -99,13 +100,16 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) {
Pose2 measurement(-6.0, 3.5, 0.123);
// Prior on x component of translation.
TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25));
TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
NM::Isotropic::Sigma(2, 0.25));
Pose2 pose = measurement; // Zero-error linearization point.
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor2::evaluateError, &factor,
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -127,7 +131,7 @@ TEST(PartialPriorFactor, JacobianTheta) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -178,7 +182,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -200,7 +204,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -224,7 +228,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -248,7 +252,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
@ -271,7 +275,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) {
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;

23
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -22,10 +22,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -199,8 +198,14 @@ TEST( PoseBetweenFactor, Jacobian ) {
Point3(-3.37493895, 6.14660244, -8.93650986));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
@ -228,8 +233,14 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
Point3(-3.5257579, 6.02637531, -8.98382384));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

13
gtsam_unstable/slam/tests/testPosePriorFactor.cpp
View File

@ -22,10 +22,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -188,7 +187,10 @@ TEST( PosePriorFactor, Jacobian ) {
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPosePriorFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative
Matrix actualH1;
@ -212,7 +214,10 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
Point3(-4.74767676, 7.67044942, -11.00985));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPosePriorFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative
Matrix actualH1;

2
gtsam_unstable/slam/tests/testPoseToPointFactor.h
View File

@ -63,7 +63,7 @@ TEST(PoseToPointFactor, jacobian) {
PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
// Calculate numerical derivatives
auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
boost::none, boost::none);
Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);

22
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
View File

@ -28,9 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -179,9 +177,12 @@ TEST( ProjectionFactorPPP, Jacobian ) {
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&TestProjectionFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose, Pose3(), point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
}
@ -214,9 +215,12 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) {
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&TestProjectionFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
pose, body_P_sensor, point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));

22
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -28,9 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -138,12 +136,16 @@ TEST( ProjectionFactorPPPC, Jacobian ) {
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), Pose3());
std::bind(&TestProjectionFactor::evaluateError, &factor, pose,
std::placeholders::_1, point, *K1, boost::none, boost::none,
boost::none, boost::none),
Pose3());
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(),
point, std::placeholders::_1, boost::none, boost::none,
boost::none, boost::none),
*K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
@ -174,12 +176,12 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point,
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));

59
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -5,14 +5,13 @@
* @author Alex Cunningham
*/
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
#include <gtsam/base/numericalDerivative.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
SharedNoiseModel model1 = noiseModel::Unit::Create(1);
@ -38,9 +37,13 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
Matrix actH1, actH2;
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -53,9 +56,13 @@ TEST( testRelativeElevationFactor, level_positive ) {
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -68,9 +75,13 @@ TEST( testRelativeElevationFactor, level_negative ) {
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -83,9 +94,13 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
Matrix actH1, actH2;
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -98,9 +113,13 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -113,9 +132,13 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}
@ -128,9 +151,13 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose3, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
}

27
gtsam_unstable/slam/tests/testTSAMFactors.cpp
View File

@ -19,10 +19,9 @@
#include <gtsam_unstable/slam/TSAMFactors.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -48,10 +47,10 @@ TEST( DeltaFactor, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector2, Pose2>(
boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none,
boost::none), pose);
H2Expected = numericalDerivative11<Vector2, Point2>(
boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none,
boost::none), point);
// Verify the Jacobians are correct
@ -82,17 +81,17 @@ TEST( DeltaFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<Vector2, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2,
point, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<Vector2, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2,
point, boost::none, boost::none, boost::none, boost::none), pose);
H3Expected = numericalDerivative11<Vector2, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1,
point, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<Vector2, Point2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
_1, boost::none, boost::none, boost::none, boost::none), point);
std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point);
// Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
@ -123,17 +122,17 @@ TEST( OdometryFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<Vector3, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<Vector3, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), pose1);
H3Expected = numericalDerivative11<Vector3, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1,
pose2, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<Vector3, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
base2, _1, boost::none, boost::none, boost::none, boost::none),
std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none),
pose2);
// Verify the Jacobians are correct

12
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
View File

@ -18,9 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -233,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
Matrix H1_actual = H_actual[0];
double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
}
@ -287,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
//
// double stepsize = 1.0e-9;
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
//
//
// // try to check numerical derivatives of a standard between factor
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

14
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -18,9 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -262,7 +260,9 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
Matrix H1_actual = H_actual[0];
double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(
std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB,
stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
}
/////* ************************************************************************** */
@ -312,12 +312,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
//
// double stepsize = 1.0e-9;
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
//
//
// // try to check numerical derivatives of a standard between factor
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

133
python/gtsam/tests/test_Cal3Fisheye.py Normal file
View File

@ -0,0 +1,133 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Cal3Fisheye unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
Refactored: Roderick Koehle
"""
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
from gtsam.symbol_shorthand import K, L, P
class TestCal3Fisheye(GtsamTestCase):
@classmethod
def setUpClass(cls):
"""
Equidistant fisheye projection
An equidistant fisheye projection with focal length f is defined
as the relation r/f = tan(theta), with r being the radius in the
image plane and theta the incident angle of the object point.
"""
x, y, z = 1.0, 0.0, 1.0
u, v = np.arctan2(x, z), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
p1 = [-1.0, 0.0, -1.0]
p2 = [ 1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2])
cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye()
self.assertEqual(K.fx(), 1.)
self.assertEqual(K.fy(), 1.)
def test_distortion(self):
"""Fisheye distortion and rectification"""
equidistant = gtsam.Cal3Fisheye()
perspective_pt = self.obj_point[0:2]/self.obj_point[2]
distorted_pt = equidistant.uncalibrate(perspective_pt)
rectified_pt = equidistant.calibrate(distorted_pt)
self.gtsamAssertEquals(distorted_pt, self.img_point)
self.gtsamAssertEquals(rectified_pt, perspective_pt)
def test_pinhole(self):
"""Spatial equidistant camera projection"""
camera = gtsam.PinholeCameraCal3Fisheye()
pt1 = camera.Project(self.obj_point) # Perspective projection
pt2 = camera.project(self.obj_point) # Equidistant projection
x, y, z = self.obj_point
obj1 = camera.backproject(self.img_point, z)
r1 = camera.range(self.obj_point)
r = np.linalg.norm(self.obj_point)
self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
self.gtsamAssertEquals(pt2, self.img_point)
self.gtsamAssertEquals(obj1, self.obj_point)
self.assertEqual(r1, r)
def test_generic_factor(self):
"""Evaluate residual using pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
pose_key, point_key = P(0), L(0)
k = gtsam.Cal3Fisheye()
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(point_key, self.obj_point)
factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
def test_sfm_factor2(self):
"""Evaluate residual with camera, pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
k = gtsam.Cal3Fisheye()
state.insert_cal3fisheye(camera_key, k)
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(landmark_key, gtsam.Point3(self.obj_point))
factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation_skipped(self):
"""Estimate spatial point from image measurements"""
triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(triangulated, self.origin)
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(triangulated, self.origin)
def test_retract(self):
expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10)
k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240,
1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3)
d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F')
actual = k.retract(d)
self.gtsamAssertEquals(actual, expected)
np.testing.assert_allclose(d, k.localCoordinates(actual))
if __name__ == "__main__":
unittest.main()

107
python/gtsam/tests/test_Cal3Unified.py
View File

@ -14,15 +14,122 @@ import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
from gtsam.symbol_shorthand import K, L, P
class TestCal3Unified(GtsamTestCase):
@classmethod
def setUpClass(cls):
"""
Stereographic fisheye projection
An equidistant fisheye projection with focal length f is defined
as the relation r/f = 2*tan(theta/2), with r being the radius in the
image plane and theta the incident angle of the object point.
"""
x, y, z = 1.0, 0.0, 1.0
r = np.linalg.norm([x, y, z])
u, v = 2*x/(z+r), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
k1, k2, p1, p2 = 0, 0, 0, 0
xi = 1
cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
p1 = [-1.0, 0.0, -1.0]
p2 = [ 1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2])
cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
def test_Cal3Unified(self):
K = gtsam.Cal3Unified()
self.assertEqual(K.fx(), 1.)
self.assertEqual(K.fx(), 1.)
def test_distortion(self):
"""Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)"""
x, y, z = self.obj_point
r = np.linalg.norm([x, y, z])
# Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2))
self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r))
perspective_pt = self.obj_point[0:2]/self.obj_point[2]
distorted_pt = self.stereographic.uncalibrate(perspective_pt)
rectified_pt = self.stereographic.calibrate(distorted_pt)
self.gtsamAssertEquals(distorted_pt, self.img_point)
self.gtsamAssertEquals(rectified_pt, perspective_pt)
def test_pinhole(self):
"""Spatial stereographic camera projection"""
x, y, z = self.obj_point
u, v = self.img_point
r = np.linalg.norm(self.obj_point)
pose = gtsam.Pose3()
camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic)
pt1 = camera.Project(self.obj_point)
self.gtsamAssertEquals(pt1, np.array([x/z, y/z]))
pt2 = camera.project(self.obj_point)
self.gtsamAssertEquals(pt2, self.img_point)
obj1 = camera.backproject(self.img_point, z)
self.gtsamAssertEquals(obj1, self.obj_point)
r1 = camera.range(self.obj_point)
self.assertEqual(r1, r)
def test_generic_factor(self):
"""Evaluate residual using pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
pose_key, point_key = P(0), L(0)
k = self.stereographic
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(point_key, self.obj_point)
factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
def test_sfm_factor2(self):
"""Evaluate residual with camera, pose and point as state variables"""
r = np.linalg.norm(self.obj_point)
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
k = self.stereographic
state.insert_cal3unified(camera_key, k)
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(landmark_key, self.obj_point)
factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation(self):
"""Estimate spatial point from image measurements"""
triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(triangulated, self.origin)
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(triangulated, self.origin)
def test_retract(self):
expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)

78
python/gtsam/tests/test_ShonanAveraging.py
View File

@ -13,14 +13,27 @@ Author: Frank Dellaert
import unittest
import gtsam
from gtsam import ShonanAveraging3, ShonanAveragingParameters3
import numpy as np
from gtsam import (
BetweenFactorPose2,
LevenbergMarquardtParams,
Rot2,
Pose2,
ShonanAveraging2,
ShonanAveragingParameters2,
ShonanAveraging3,
ShonanAveragingParameters3,
)
from gtsam.utils.test_case import GtsamTestCase
DEFAULT_PARAMS = ShonanAveragingParameters3(
gtsam.LevenbergMarquardtParams.CeresDefaults())
gtsam.LevenbergMarquardtParams.CeresDefaults()
)
def fromExampleName(name: str, parameters=DEFAULT_PARAMS):
def fromExampleName(
name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS
) -> ShonanAveraging3:
g2oFile = gtsam.findExampleDataFile(name)
return ShonanAveraging3(g2oFile, parameters)
@ -135,5 +148,62 @@ class TestShonanAveraging(GtsamTestCase):
self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
if __name__ == '__main__':
def test_constructorBetweenFactorPose2s(self) -> None:
"""Check if ShonanAveraging2 constructor works when not initialized from g2o file.
GT pose graph:
| cam 1 = (0,4)
--o
| .
. .
. .
| |
o-- ... o--
cam 0 cam 2 = (4,0)
(0,0)
"""
num_images = 3
wTi_list = [
Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
]
edges = [(0, 1), (1, 2), (0, 2)]
i2Ri1_dict = {
(i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
for (i1, i2) in edges
}
lm_params = LevenbergMarquardtParams.CeresDefaults()
shonan_params = ShonanAveragingParameters2(lm_params)
shonan_params.setUseHuber(False)
shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(3)
between_factors = gtsam.BetweenFactorPose2s()
for (i1, i2), i2Ri1 in i2Ri1_dict.items():
i2Ti1 = Pose2(i2Ri1, np.zeros(2))
between_factors.append(
BetweenFactorPose2(i2, i1, i2Ti1, noise_model)
)
obj = ShonanAveraging2(between_factors, shonan_params)
initial = obj.initializeRandomly()
result_values, _ = obj.run(initial, min_p=2, max_p=100)
wRi_list = [result_values.atRot2(i) for i in range(num_images)]
thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
# map all angles to [0,360)
thetas_deg = thetas_deg % 360
thetas_deg -= thetas_deg[0]
expected_thetas_deg = np.array([0.0, 90.0, 0.0])
np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
if __name__ == "__main__":
unittest.main()

22
tests/testExpressionFactor.cpp
View File

@ -17,22 +17,19 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/expressions.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -621,8 +618,9 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
Matrix3 A;
const Vector Ab = f(a, b, H1, A);
CHECK(assert_equal(A * b, Ab));
CHECK(assert_equal(numericalDerivative11<Vector3, Point2>(
boost::bind(f, _1, b, boost::none, boost::none), a),
CHECK(assert_equal(
numericalDerivative11<Vector3, Point2>(
std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a),
H1));
Values values;

14
tests/testSimulated3D.cpp
View File

@ -26,7 +26,7 @@
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
// Convenience for named keys
@ -46,7 +46,7 @@ TEST( simulated3D, Values )
TEST( simulated3D, Dprior )
{
Point3 x(1,-9, 7);
Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(simulated3D::prior, _1, boost::none),x);
Matrix numerical = numericalDerivative11<Point3, Point3>(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x);
Matrix computed;
simulated3D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9));
@ -58,9 +58,15 @@ TEST( simulated3D, DOdo )
Point3 x1(1, -9, 7), x2(-5, 6, 7);
Matrix H1, H2;
simulated3D::odo(x1, x2, H1, H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none),
x1, x2);
EXPECT(assert_equal(A1, H1, 1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none),
x1, x2);
EXPECT(assert_equal(A2, H2, 1e-9));
}