diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index fc5531cdc..a05a1dda8 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -19,14 +19,7 @@ #pragma once #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif +#include #include #include @@ -45,13 +38,13 @@ namespace gtsam { * for a function with one relevant param and an optional derivative: * Foo bar(const Obj& a, boost::optional H1) * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) + * boost::bind(bar, boost::placeholders::_1, boost::none) * This syntax will fix the optional argument to boost::none, while using the first argument provided * * For member functions, such as below, with an instantiated copy instanceOfSomeClass * Foo SomeClass::bar(const Obj& a) * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1) * * For additional details, see the documentation: * http://www.boost.org/doc/libs/release/libs/bind/bind.html @@ -157,7 +150,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative11( template typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1), x, delta); } /** @@ -176,13 +169,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(const boost "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ template typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); } @@ -202,13 +196,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(boost::func // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta); } /** use a raw C++ function pointer */ template typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); } @@ -231,12 +226,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative31( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)), x1, delta); } template typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } @@ -260,12 +256,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative32( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } @@ -289,12 +286,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative33( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1), x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } @@ -318,12 +316,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative41(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); } @@ -346,12 +345,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative42(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); } @@ -374,12 +374,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4)), x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative43(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); } @@ -402,12 +403,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1), x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative44(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); } @@ -431,12 +433,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative51( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative51(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -460,12 +463,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative52(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -489,12 +493,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative53( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5)), x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative53(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -518,12 +523,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5)), x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative54(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -547,12 +553,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1), x5, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative55(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -577,12 +584,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative61( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative61(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -607,12 +615,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative62( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative62(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -637,12 +646,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative63( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative63(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -667,12 +677,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative64( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5), boost::cref(x6)), x4, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative64(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -697,12 +708,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative65( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1, boost::cref(x6)), x5, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative65(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -728,12 +740,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::placeholders::_1), x6, delta); } template inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + using namespace boost::placeholders; return numericalDerivative66(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); } @@ -754,7 +767,7 @@ inline typename internal::FixedSizeMatrix::type numericalHessian(boost::fun typedef boost::function F; typedef boost::function G; G ng = static_cast(numericalGradient ); - return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + return numericalDerivative11(boost::bind(ng, f, boost::placeholders::_1, delta), x, delta); } @@ -780,7 +793,7 @@ public: f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { - return numericalGradient(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_); + return numericalGradient(boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_); } }; @@ -792,7 +805,7 @@ inline typename internal::FixedSizeMatrix::type numericalHessian212( G_x1 g_x1(f, x1, delta); return numericalDerivative11( boost::function( - boost::bind(boost::ref(g_x1), _1)), x2, delta); + boost::bind(boost::ref(g_x1), boost::placeholders::_1)), x2, delta); } template @@ -807,6 +820,7 @@ inline typename internal::FixedSizeMatrix::type numericalHessian211( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + using namespace boost::placeholders; typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X1&, @@ -829,6 +843,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian222( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + using namespace boost::placeholders; typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; @@ -854,6 +869,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian311( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; @@ -877,6 +893,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian322( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; @@ -900,6 +917,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian333( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; @@ -923,6 +941,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian312( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalHessian212( boost::function(boost::bind(f, _1, _2, boost::cref(x3))), x1, x2, delta); @@ -932,6 +951,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian313( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalHessian212( boost::function(boost::bind(f, _1, boost::cref(x2), _2)), x1, x3, delta); @@ -941,6 +961,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian323( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + using namespace boost::placeholders; return numericalHessian212( boost::function(boost::bind(f, boost::cref(x1), _1, _2)), x2, x3, delta); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 1982b8f50..1db1926bc 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -20,10 +20,12 @@ #include #include +#include #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 60cee59ee..761ef3a8c 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc..7362cf7bf 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -9,9 +9,12 @@ #include #include #include + +#include #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 2461cea1a..5c7c6142e 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -21,8 +21,10 @@ #include #include #include +#include using namespace boost::assign; +using namespace boost::placeholders; using namespace gtsam; using namespace std; using boost::none; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ad6f4e921..92deda6a5 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -22,11 +22,13 @@ #include #include +#include #include #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index f32fe60ed..9ca8847f8 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a655011a0..79e44c0b3 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -17,8 +17,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 594d15c91..9ed76d4a6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -22,6 +22,8 @@ #include // for operator += using namespace boost::assign; +#include +using namespace boost::placeholders; #include #include diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index b2c781b35..58ad967d2 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -20,8 +20,10 @@ #include #include +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 8f466e21b..10a9b2ac4 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -30,9 +30,10 @@ #include +#include #include -#include +using namespace boost::placeholders; using namespace gtsam; using namespace std; using symbol_shorthand::X; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 9929df21a..4d609380c 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -31,13 +31,14 @@ #include -#include #include +#include #include #include using namespace boost::assign; +using namespace boost::placeholders; using namespace gtsam; using namespace std; using gtsam::symbol_shorthand::U; diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 3390a3aac..b8d446e49 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index e1c18274a..166ae41f4 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -23,8 +23,6 @@ #include -#include - #include #include #include // for cout :-( diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index a9ca7f84d..e337e3249 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,7 +29,6 @@ #include // for Eigen::aligned_allocator #include -#include #include #include #include diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index e67c56e5e..17ff6fd22 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -17,8 +17,8 @@ #include +#include #include -#include #include @@ -110,16 +110,16 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} boost::function LabeledSymbol::TypeTest(unsigned char c) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c; } boost::function LabeledSymbol::LabelTest(unsigned char label) { - return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; + return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; } boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && - boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c && + boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; } /* ************************************************************************* */ diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index e6a2beee0..61e397f40 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,8 +18,8 @@ #include +#include #include -#include #include #include @@ -63,7 +63,7 @@ Symbol::operator std::string() const { static Symbol make(gtsam::Key key) { return Symbol(key);} boost::function Symbol::ChrTest(unsigned char c) { - return bind(&Symbol::chr, bind(make, _1)) == c; + return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c; } GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index cc5dc4b88..770b48f63 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -20,7 +20,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7bf65353b..8646a9cae 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -32,14 +32,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include #include diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2e0ffb034..635476687 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -35,14 +35,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include #include diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 746275847..f72799c0a 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include #include #include @@ -38,7 +38,8 @@ namespace gtsam { { // Merge using predicate for comparing first of pair merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), - boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); + boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, boost::placeholders::_1), + boost::bind(&KeyValuePair::first, boost::placeholders::_2))); if(size() != first.size() + second.size()) throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index eafefb3cb..09a741d0b 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -26,6 +26,8 @@ #include #include // for operator += using namespace boost::assign; +#include +using namespace boost::placeholders; // STL/C++ #include diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 17dc6a425..99d916123 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -22,6 +22,8 @@ #include // for operator += #include // for operator += using namespace boost::assign; +#include +using namespace boost::placeholders; #include #include diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 74acf69da..828e264f4 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -25,9 +25,10 @@ #include #include -#include +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 38f16f55f..2ab60fe6a 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -21,8 +21,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3ef810cad..2bbc2cc7c 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -29,7 +29,6 @@ #include -#include #include #include "imuFactorTesting.h" diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 9457f501d..b486272ab 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -20,11 +20,14 @@ #include #include +#include + #include #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 596b76f6a..e7da2c81c 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -19,8 +19,9 @@ #include #include -#include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 474b00add..f19862772 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include "imuFactorTesting.h" @@ -147,7 +147,8 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3, + boost::bind(&PreintegrationBase::computeError, actual, + boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::none, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -203,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), kZeroBias); + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, + boost::placeholders::_1, boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; Matrix96 aH2; NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, - boost::none), state1); + boost::bind(&PreintegrationBase::predict, pim, boost::placeholders::_1, + kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, - boost::none), kZeroBias); + boost::bind(&PreintegrationBase::predict, pim, state1, + boost::placeholders::_1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -277,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) { // Make sure rotation part is correct when error is interpreted as axis-angle // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), + boost::bind(&evaluateRotationError, factor, boost::placeholders::_1, v1, x2, v2, kZeroBias), x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), + boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias), x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); @@ -332,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), bias); + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, + boost::placeholders::_1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -521,7 +522,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); Matrix3 expectedD = numericalDerivative11( - boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1), + measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); double dt = 0.1; @@ -532,13 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// boost::placeholders::_1, boost::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG1, G1, 1e-5)); // // Matrix93 expectedG2 = numericalDerivative22( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// boost::placeholders::_1, boost::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG2, G2, 1e-5)); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 45db58e33..ad193b503 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -20,10 +20,13 @@ #include #include +#include + #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index a1cc1c6eb..1a3c5b2a9 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -17,6 +17,9 @@ #include #include +#include + +using namespace boost::placeholders; using namespace gtsam; // ***************************************************************************** diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index a3f688dda..16084ecbe 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include "imuFactorTesting.h" @@ -98,7 +98,9 @@ TEST(ManifoldPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, + boost::bind(&ManifoldPreintegration::computeError, pim, + boost::placeholders::_1, boost::placeholders::_2, + boost::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)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 5bafe4392..86c708f5e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -19,8 +19,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 9c080929d..d0bae3690 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -19,9 +19,10 @@ #include #include -#include +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 65fd55fa3..60a646f6c 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include "imuFactorTesting.h" @@ -105,7 +105,9 @@ TEST(TangentPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, + boost::bind(&TangentPreintegration::computeError, pim, + boost::placeholders::_1, boost::placeholders::_2, + boost::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)); diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 0ba2ad446..85f2f14bc 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -21,6 +21,7 @@ #include +#include #include #include #include @@ -82,7 +83,8 @@ template Expression::Expression(const Expression& expression, T (A::*method)(typename MakeOptionalJacobian::type) const) : root_( - new internal::UnaryExpression(boost::bind(method, _1, _2), + new internal::UnaryExpression(boost::bind(method, + boost::placeholders::_1, boost::placeholders::_2), expression)) { } @@ -95,7 +97,10 @@ Expression::Expression(const Expression& expression1, const Expression& expression2) : root_( new internal::BinaryExpression( - boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { + boost::bind(method, boost::placeholders::_1, + boost::placeholders::_2, boost::placeholders::_3, + boost::placeholders::_4), + expression1, expression2)) { } /// Construct a binary method expression @@ -109,8 +114,11 @@ Expression::Expression(const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( new internal::TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { + boost::bind(method, boost::placeholders::_1, + boost::placeholders::_2, boost::placeholders::_3, + boost::placeholders::_4, boost::placeholders::_5, + boost::placeholders::_6), + expression1, expression2, expression3)) { } template @@ -247,8 +255,10 @@ template Expression operator*(const Expression& expression1, const Expression& expression2) { return Expression( - boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, - expression2); + boost::bind(internal::apply_compose(), boost::placeholders::_1, + boost::placeholders::_2, boost::placeholders::_3, + boost::placeholders::_4), + expression1, expression2); } /// Construct an array of leaves diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 9e8aa3f29..bda457595 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d7f8b0ef8..b70929179 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -87,7 +87,8 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + const CompareFunction &_compare = boost::bind(traits::Equals, + boost::placeholders::_1,boost::placeholders::_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -97,7 +98,8 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : + const CompareFunction &_compare = boost::bind(traits::Equals, + boost::placeholders::_1,boost::placeholders::_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 19813adba..eca7416c9 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,6 +26,8 @@ #include +#include + #include // Only so Eclipse finds class definition namespace gtsam { @@ -242,7 +244,8 @@ namespace gtsam { template Values::Filtered Values::filter(const boost::function& filterFcn) { - return Filtered(boost::bind(&filterHelper, filterFcn, _1), *this); + return Filtered(boost::bind(&filterHelper, filterFcn, + boost::placeholders::_1), *this); } /* ************************************************************************* */ @@ -255,7 +258,8 @@ namespace gtsam { template Values::ConstFiltered Values::filter(const boost::function& filterFcn) const { - return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); + return ConstFiltered(boost::bind(&filterHelper, + filterFcn, boost::placeholders::_1), *this); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 9b8f7645a..ebc9c51f6 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,14 +25,6 @@ #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 893d5572f..3b447ede1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,15 +29,6 @@ #include #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#pragma GCC diagnostic ignored "-Wunused-local-typedefs" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index aacceb276..707d57aae 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -29,6 +29,8 @@ #include #include using namespace boost::assign; +#include +using namespace boost::placeholders; #include #include #include diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 673d4e052..2af5825db 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -26,8 +26,9 @@ #include #include -#include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 3ff76ac5c..05ae76faa 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -20,8 +20,10 @@ #include #include +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d995bf8e7..e99dba3bc 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -9,8 +9,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 16197ab03..fb2172107 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -22,8 +22,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..0c9f5c42d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -17,8 +17,10 @@ #include #include +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index a0ef7b91d..022dc859e 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -27,9 +27,10 @@ #include #include -#include +#include using namespace boost::assign; +using namespace boost::placeholders; using namespace gtsam; using namespace std; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 217ff2178..075710ae7 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include @@ -32,6 +32,7 @@ using namespace std; using namespace boost; +using namespace boost::placeholders; using namespace gtsam; typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index caf3d31df..e4ecafd42 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -12,12 +12,13 @@ #include #include -#include #include +#include #include using namespace std; using namespace boost::assign; +using namespace boost::placeholders; using namespace gtsam; static const double kDegree = M_PI / 180; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 0cc5e8f55..26caa6b75 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -27,9 +27,11 @@ #include #include #include +#include #include using namespace boost::assign; +using namespace boost::placeholders; static const double rankTol = 1.0; // Create a noise model for the pixel error diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 4bbef6530..9fe087c2f 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -27,10 +27,12 @@ #include #include +#include using namespace std; using namespace gtsam; using namespace boost::assign; +using namespace boost::placeholders; // Some common constants static const boost::shared_ptr sharedCal = // diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 531d54af1..337f2bc43 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** @@ -89,9 +91,9 @@ public: z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); + boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); return z - predict_proxy(x1, x2, dt_); } diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 62a6abcd9..e082dee82 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** @@ -79,9 +81,9 @@ public: boost::optional H2 = boost::none) const override { const Vector6 meas = z(); if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); + boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 56171a728..840b7bba7 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { @@ -84,9 +86,11 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const override { if (H1) *H1 = gtsam::numericalDerivative21( - boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1, + boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); if (H2) *H2 = gtsam::numericalDerivative22( - boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); + boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1, + boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index fe21d5e00..ede11c7fb 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -3,12 +3,14 @@ * @author Duy-Nguyen Ta */ +#include #include #include #include #include /* ************************************************************************* */ +using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index f72b2cf95..8d496a30e 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 0349f3293..3a599e6c5 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -23,8 +23,9 @@ #include -#include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index df21c0132..88697e075 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -39,6 +40,7 @@ #include using boost::fusion::at_c; +using namespace boost::placeholders; using namespace std; namespace bf = boost::fusion; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 165135114..c7b82ba98 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -27,6 +27,7 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include +#include #include #include @@ -304,6 +305,8 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const override { + using namespace boost::placeholders; + // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ @@ -420,6 +423,8 @@ public: // Note: all delta terms refer to an IMU\sensor system at t0 // Note: Earth-related terms are not accounted here but are incorporated in predict functions. + using namespace boost::placeholders; + POSE body_P_sensor = POSE(); bool flag_use_body_P_sensor = false; if (p_body_P_sensor){ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index f031f4884..968810e0a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -26,6 +26,7 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include +#include #include #include @@ -234,38 +235,38 @@ public: // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ - Matrix H1_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. IMUBias1 if (H3){ - Matrix H3_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1); + Matrix H3_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Pose2 if (H4){ - Matrix H4_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2); + Matrix H4_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } // Jacobian w.r.t. Vel2 if (H5){ if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); - Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 7a34ab1bc..7292d702b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -17,6 +17,8 @@ #include #include +#include + namespace gtsam { /** @@ -106,13 +108,13 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, boost::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, - _1), landmark); + boost::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 0462aefad..d1fc92b90 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -18,6 +18,8 @@ #include #include +#include + namespace gtsam { /** @@ -109,13 +111,13 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, - landmark), pose); + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, + boost::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, - _1), landmark); + boost::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 454358a0e..d8c790342 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -17,6 +17,8 @@ #include #include +#include + namespace gtsam { /** @@ -108,10 +110,10 @@ public: boost::optional H2=boost::none) const override { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, boost::placeholders::_1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, boost::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); @@ -229,13 +231,13 @@ public: boost::optional H3=boost::none) const override { if(H1) - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, boost::placeholders::_1, pose2, landmark), pose1); if(H2) - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, boost::placeholders::_1, landmark), pose2); if(H3) - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, boost::placeholders::_1), landmark); return inverseDepthError(pose1, pose2, landmark); } diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index e4112f53d..0eb8b274b 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -9,8 +9,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 35bf5790e..95b9b2f88 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -21,9 +21,12 @@ #include #include #include + +#include #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 209326672..74134612d 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -23,6 +23,9 @@ #include #include +#include + +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index b84f7e080..aae59035b 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -25,6 +26,7 @@ #include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 9f6fe5b50..6bbfb55ae 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -24,8 +24,9 @@ #include -#include +#include +using namespace boost::placeholders; using namespace gtsam; using namespace std; diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index ae8074f43..2b99b8779 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -15,8 +15,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index f589e932f..6f7725eed 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -21,8 +21,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 166f058e3..cc2615517 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -21,8 +21,11 @@ #include #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 4e9fdcb50..8a65e5e57 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -28,8 +28,9 @@ #include -#include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5eadf5fd6..232f8de17 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -28,8 +28,9 @@ #include -#include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index 210018ed3..2fda9debb 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -5,12 +5,14 @@ * @author Alex Cunningham */ +#include #include #include #include +using namespace boost::placeholders; using namespace gtsam; SharedNoiseModel model1 = noiseModel::Unit::Create(1); diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 042130a24..bc5c553e0 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -25,7 +25,6 @@ #include #include -#include #include using namespace std; diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 098a0ef56..77f82bca4 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -18,8 +18,11 @@ #include #include + +#include #include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index 01d4b152d..d9e945b78 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -18,7 +18,9 @@ #include #include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index b200a3e58..2fd282091 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -18,7 +18,9 @@ #include #include +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index b0978feb9..ec41bf678 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -29,14 +29,6 @@ #include #include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif #include // for 'list_of()' #include #include diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index c31baeadf..6bb5751bf 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -31,6 +31,8 @@ #include using boost::assign::list_of; +#include +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 02f6ed762..342c353bc 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -21,10 +21,12 @@ #include #include +#include #include #include +using namespace boost::placeholders; using namespace gtsam; // Convenience for named keys