From 650e432f52f58505eba8c8f6a918f03e733a7acc Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Tue, 15 Jun 2021 13:07:08 -0400 Subject: [PATCH 1/6] update boost::bind usage use instead of deprecated use boost::placeholders:: scope in appropriate files remove and add in appropriate files --- gtsam/base/numericalDerivative.h | 89 ++++++++++++------- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 + gtsam/geometry/tests/testCameraSet.cpp | 1 - gtsam/geometry/tests/testEssentialMatrix.cpp | 3 + gtsam/geometry/tests/testOrientedPlane3.cpp | 2 + gtsam/geometry/tests/testPinholeCamera.cpp | 2 + gtsam/geometry/tests/testPinholeSet.cpp | 1 - gtsam/geometry/tests/testPoint3.cpp | 3 + gtsam/geometry/tests/testPose3.cpp | 2 + gtsam/geometry/tests/testSO3.cpp | 2 + gtsam/geometry/tests/testSimilarity3.cpp | 3 +- gtsam/geometry/tests/testUnit3.cpp | 3 +- gtsam/inference/EliminationTree-inst.h | 1 - gtsam/inference/FactorGraph-inst.h | 2 - gtsam/inference/FactorGraph.h | 1 - gtsam/inference/LabeledSymbol.cpp | 10 +-- gtsam/inference/Symbol.cpp | 4 +- gtsam/inference/inference-inst.h | 1 - gtsam/linear/HessianFactor.cpp | 8 -- gtsam/linear/JacobianFactor.cpp | 8 -- gtsam/linear/VectorValues.cpp | 5 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 2 + gtsam/linear/tests/testGaussianBayesTree.cpp | 2 + gtsam/navigation/tests/testAHRSFactor.cpp | 3 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 3 + .../tests/testCombinedImuFactor.cpp | 1 - gtsam/navigation/tests/testGPSFactor.cpp | 3 + gtsam/navigation/tests/testImuBias.cpp | 3 +- gtsam/navigation/tests/testImuFactor.cpp | 34 +++---- gtsam/navigation/tests/testMagFactor.cpp | 3 + gtsam/navigation/tests/testMagPoseFactor.cpp | 3 + .../tests/testManifoldPreintegration.cpp | 6 +- gtsam/navigation/tests/testNavState.cpp | 3 + gtsam/navigation/tests/testScenario.cpp | 3 +- .../tests/testTangentPreintegration.cpp | 6 +- gtsam/nonlinear/Expression-inl.h | 22 +++-- gtsam/nonlinear/Expression.h | 1 - gtsam/nonlinear/NonlinearEquality.h | 8 +- gtsam/nonlinear/Values-inl.h | 8 +- gtsam/nonlinear/Values.cpp | 8 -- gtsam/nonlinear/Values.h | 9 -- gtsam/nonlinear/tests/testValues.cpp | 2 + gtsam/sam/tests/testRangeFactor.cpp | 3 +- gtsam/sfm/tests/testTranslationFactor.cpp | 2 + gtsam/slam/tests/testBetweenFactor.cpp | 3 + .../tests/testEssentialMatrixConstraint.cpp | 3 + .../slam/tests/testEssentialMatrixFactor.cpp | 2 + gtsam/slam/tests/testOrientedPlane3Factor.cpp | 3 +- gtsam/slam/tests/testReferenceFrameFactor.cpp | 3 +- gtsam/slam/tests/testRotateFactor.cpp | 3 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 + gtsam/slam/tests/testTriangulationFactor.cpp | 2 + gtsam_unstable/dynamics/FullIMUFactor.h | 6 +- gtsam_unstable/dynamics/IMUFactor.h | 6 +- gtsam_unstable/dynamics/VelocityConstraint.h | 8 +- .../dynamics/tests/testSimpleHelicopter.cpp | 2 + .../examples/TimeOfArrivalExample.cpp | 1 - gtsam_unstable/geometry/tests/testEvent.cpp | 3 +- gtsam_unstable/linear/QPSParser.cpp | 2 + .../slam/EquivInertialNavFactor_GlobalVel.h | 5 ++ .../slam/InertialNavFactor_GlobalVelocity.h | 21 ++--- gtsam_unstable/slam/InvDepthFactorVariant1.h | 6 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 8 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 12 +-- .../slam/tests/testBiasedGPSFactor.cpp | 3 + .../testEquivInertialNavFactor_GlobalVel.cpp | 3 + .../tests/testGaussMarkov1stOrderFactor.cpp | 3 + .../testInertialNavFactor_GlobalVelocity.cpp | 2 + .../tests/testLocalOrientedPlane3Factor.cpp | 3 +- .../slam/tests/testPartialPriorFactor.cpp | 3 + .../slam/tests/testPoseBetweenFactor.cpp | 3 + .../slam/tests/testPosePriorFactor.cpp | 3 + .../slam/tests/testProjectionFactorPPP.cpp | 3 +- .../slam/tests/testProjectionFactorPPPC.cpp | 3 +- .../tests/testRelativeElevationFactor.cpp | 2 + gtsam_unstable/slam/tests/testTOAFactor.cpp | 1 - gtsam_unstable/slam/tests/testTSAMFactors.cpp | 3 + .../testTransformBtwRobotsUnaryFactor.cpp | 2 + .../testTransformBtwRobotsUnaryFactorEM.cpp | 2 + tests/testDoglegOptimizer.cpp | 8 -- tests/testExpressionFactor.cpp | 2 + tests/testSimulated3D.cpp | 2 + 82 files changed, 279 insertions(+), 164 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index fc5531cdc2..a05a1dda82 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 @@ class G_x1 { 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 1982b8f505..1db1926bcd 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 60cee59ee1..761ef3a8ca 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 86a498cdc7..7362cf7bf8 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 2461cea1af..5c7c6142e1 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 ad6f4e9210..92deda6a53 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 f32fe60ed1..9ca8847f85 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 a655011a03..79e44c0b35 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 594d15c91b..9ed76d4a67 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 b2c781b35f..58ad967d21 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 8f466e21bc..10a9b2ac49 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 9929df21ae..4d609380c8 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 3390a3aac8..b8d446e493 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 e1c18274a8..166ae41f41 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 a9ca7f84d3..e337e3249f 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 e67c56e5e2..17ff6fd22e 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 e6a2beee01..61e397f403 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 cc5dc4b881..770b48f63c 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 7bf65353b8..8646a9cae1 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 2e0ffb0344..6354766874 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 7462758476..f72799c0aa 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 eafefb3cb6..09a741d0b2 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 17dc6a425c..99d916123c 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 74acf69da2..828e264f46 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 38f16f55f1..2ab60fe6ae 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 3ef810cad9..2bbc2cc7c7 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 9457f501d0..b486272abf 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 596b76f6ac..e7da2c81c6 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 474b00add2..f19862772c 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 45db58e33c..ad193b5036 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 a1cc1c6eb9..1a3c5b2a97 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 a3f688dda8..16084ecbe2 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 5bafe43923..86c708f5e8 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 9c080929dd..d0bae36900 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 65fd55fa32..60a646f6ce 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 0ba2ad4463..85f2f14bc3 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 9e8aa3f291..bda4575959 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 d7f8b0ef89..b709291794 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 @@ class NonlinearEquality: public NoiseModelFactor1 { * 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 @@ class NonlinearEquality: public NoiseModelFactor1 { * 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 19813adbae..eca7416c94 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 9b8f7645a9..ebc9c51f67 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 893d5572fc..3b447ede11 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 aacceb2767..707d57aaef 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 673d4e0520..2af5825db8 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 3ff76ac5c3..05ae76faa3 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 d995bf8e7f..e99dba3bc0 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 16197ab036..fb2172107c 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 95db611d76..0c9f5c42d5 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 a0ef7b91d7..022dc859ea 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 217ff21788..075710ae73 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 caf3d31dfc..e4ecafd425 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 0cc5e8f554..26caa6b75d 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 4bbef6530f..9fe087c2ff 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 531d54af16..337f2bc430 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 @@ class FullIMUFactor : public NoiseModelFactor2 { 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 62a6abcd9f..e082dee82b 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 @@ class IMUFactor : public NoiseModelFactor2 { 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 56171a7287..840b7bba72 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 @@ class VelocityConstraint : public gtsam::NoiseModelFactor2 { 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 fe21d5e00b..ede11c7fb5 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 f72b2cf952..8d496a30ec 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 0349f32930..3a599e6c5f 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 df21c0132b..88697e075d 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 165135114d..c7b82ba985 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 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 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 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 +#include #include #include @@ -234,38 +235,38 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5(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 7a34ab1bc8..7292d702b0 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 @@ class InvDepthFactorVariant1: public NoiseModelFactor2 { 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 0462aefad5..d1fc92b90e 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 @@ class InvDepthFactorVariant2: public NoiseModelFactor2 { 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 454358a0e3..d8c790342c 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 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { 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 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3 { 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 e4112f53d8..0eb8b274b3 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 35bf5790e7..95b9b2f88f 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 2093266723..74134612d8 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 b84f7e0807..aae59035be 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 9f6fe5b50d..6bbfb55ae1 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 ae8074f432..2b99b8779a 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 f589e932fb..6f7725eed1 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 166f058e3a..cc26155172 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 4e9fdcb50d..8a65e5e571 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 5eadf5fd69..232f8de17e 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 210018ed33..2fda9debb8 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 042130a249..bc5c553e0e 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 098a0ef565..77f82bca49 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 01d4b152d8..d9e945b78c 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 b200a3e583..2fd282091a 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 b0978feb9c..ec41bf6786 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 c31baeadfe..6bb5751bf6 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 02f6ed7629..342c353bca 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 From 6dea8667fdd8dce8518b552486a7b889907a8255 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Fri, 18 Jun 2021 13:45:59 -0400 Subject: [PATCH 2/6] explicitly use boost::placeholders:_X for compilers that do not respect function scope --- gtsam/base/numericalDerivative.h | 94 +++++++------------ .../slam/EquivInertialNavFactor_GlobalVel.h | 34 +++---- 2 files changed, 50 insertions(+), 78 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index a05a1dda82..29081efba7 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -176,8 +176,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(const boost 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); + return numericalDerivative21(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta); } /** @@ -203,8 +202,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(boost::func 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); + return numericalDerivative22(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta); } /** @@ -232,8 +230,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative31( 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, + return numericalDerivative31(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, x2, x3, delta); } @@ -262,8 +259,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative32( 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, + return numericalDerivative32(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, x2, x3, delta); } @@ -292,8 +288,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative33( 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, + return numericalDerivative33(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, x2, x3, delta); } @@ -322,8 +317,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( 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); + return numericalDerivative41(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); } /** @@ -351,8 +345,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( 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); + return numericalDerivative42(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); } /** @@ -380,8 +373,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( 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); + return numericalDerivative43(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); } /** @@ -409,8 +401,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( 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); + return numericalDerivative44(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); } /** @@ -439,8 +430,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative51( 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); + return numericalDerivative51(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); } /** @@ -469,8 +459,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( 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); + return numericalDerivative52(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); } /** @@ -499,8 +488,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative53( 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); + return numericalDerivative53(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); } /** @@ -529,8 +517,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( 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); + return numericalDerivative54(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); } /** @@ -559,8 +546,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( 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); + return numericalDerivative55(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); } /** @@ -590,8 +576,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative61( 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); + return numericalDerivative61(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -621,8 +606,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative62( 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); + return numericalDerivative62(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -652,8 +636,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative63( 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); + return numericalDerivative63(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -683,8 +666,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative64( 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); + return numericalDerivative64(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -714,8 +696,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative65( 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); + return numericalDerivative65(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -746,8 +727,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( 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); + return numericalDerivative66(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); } /** @@ -820,15 +800,14 @@ 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&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, boost::cref(x2))); + boost::function f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x1, delta); } @@ -843,14 +822,13 @@ 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; - boost::function f2(boost::bind(f, boost::cref(x1), _1)); + boost::function f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x2, delta); } @@ -869,14 +847,13 @@ 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; - boost::function f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3))); + boost::function f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x1, delta); } @@ -893,14 +870,13 @@ 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; - boost::function f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3))); + boost::function f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x2, delta); } @@ -917,14 +893,13 @@ 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; - boost::function f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1)); + boost::function f2(boost::bind(f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, _1, delta)), + boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x3, delta); } @@ -941,9 +916,8 @@ 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))), + boost::function(boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, boost::cref(x3))), x1, x2, delta); } @@ -951,9 +925,8 @@ 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)), + boost::function(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::placeholders::_2)), x1, x3, delta); } @@ -961,9 +934,8 @@ 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)), + boost::function(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::placeholders::_2)), x2, x3, delta); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index c7b82ba985..3e6dbf6db3 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -310,38 +310,38 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } @@ -442,18 +442,18 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, boost::placeholders::_1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, boost::placeholders::_1), delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; From 7aeb386dbd50f86f1bb7f7491baed334ecf4ec4b Mon Sep 17 00:00:00 2001 From: Akash Patel <17132214+acxz@users.noreply.github.com> Date: Sun, 20 Jun 2021 18:04:28 -0400 Subject: [PATCH 3/6] formatting remove extraneous `using` --- gtsam/nonlinear/NonlinearEquality.h | 5 ++--- gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h | 4 ---- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index b709291794..f10ba93aee 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -88,7 +88,7 @@ class NonlinearEquality: public NoiseModelFactor1 { */ NonlinearEquality(Key j, const T& feasible, const CompareFunction &_compare = boost::bind(traits::Equals, - boost::placeholders::_1,boost::placeholders::_2,1e-9)) : + 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) { @@ -99,7 +99,7 @@ class NonlinearEquality: public NoiseModelFactor1 { */ NonlinearEquality(Key j, const T& feasible, double error_gain, const CompareFunction &_compare = boost::bind(traits::Equals, - boost::placeholders::_1,boost::placeholders::_2,1e-9)) : + 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) { @@ -361,4 +361,3 @@ struct traits > : Testable > }// namespace gtsam - diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 3e6dbf6db3..6a345a6b5d 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -305,8 +305,6 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 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){ @@ -423,8 +421,6 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 Date: Sun, 20 Jun 2021 18:31:07 -0400 Subject: [PATCH 4/6] formatting --- gtsam/linear/tests/testGaussianBayesNet.cpp | 4 ++-- gtsam/linear/tests/testGaussianBayesTree.cpp | 4 ++-- gtsam/nonlinear/tests/testValues.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 09a741d0b2..c88bf87314 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -25,14 +25,14 @@ #include #include #include // for operator += -using namespace boost::assign; #include -using namespace boost::placeholders; // STL/C++ #include #include +using namespace boost::assign; +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 99d916123c..a6a60c19ce 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -21,9 +21,7 @@ #include #include // for operator += #include // for operator += -using namespace boost::assign; #include -using namespace boost::placeholders; #include #include @@ -31,6 +29,8 @@ using namespace boost::placeholders; #include #include +using namespace boost::assign; +using namespace boost::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 707d57aaef..e9076a7d7a 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -28,13 +28,13 @@ #include // for operator += #include #include -using namespace boost::assign; #include -using namespace boost::placeholders; #include #include #include +using namespace boost::assign; +using namespace boost::placeholders; using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); From 155fafabf2faa9b3be5336a554b54e6071799b1d Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 20 Jun 2021 18:38:52 -0400 Subject: [PATCH 5/6] using using for boost placeholders in tests --- gtsam/navigation/tests/testManifoldPreintegration.cpp | 6 +++--- gtsam/navigation/tests/testTangentPreintegration.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 16084ecbe2..625689ed79 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -26,6 +26,8 @@ #include "imuFactorTesting.h" +using namespace boost::placeholders; + namespace testing { // Create default parameters with Z-down and above noise parameters static boost::shared_ptr Params() { @@ -98,9 +100,7 @@ TEST(ManifoldPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&ManifoldPreintegration::computeError, pim, - boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, + boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _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/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 60a646f6ce..9bb988b429 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -26,6 +26,8 @@ #include "imuFactorTesting.h" +using namespace boost::placeholders; + static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { @@ -105,9 +107,7 @@ TEST(TangentPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&TangentPreintegration::computeError, pim, - boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, + boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _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)); From 944b3aea297a3f0dc7b9508feb52e758725df692 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 20 Jun 2021 19:29:27 -0400 Subject: [PATCH 6/6] formatting --- gtsam/base/numericalDerivative.h | 260 ++++++++++++++---- .../slam/EquivInertialNavFactor_GlobalVel.h | 91 ++++-- .../slam/InertialNavFactor_GlobalVelocity.h | 50 +++- gtsam_unstable/slam/InvDepthFactorVariant1.h | 5 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 25 +- 5 files changed, 336 insertions(+), 95 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 29081efba7..c624c900eb 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -150,7 +150,8 @@ typename internal::FixedSizeMatrix::type numericalDerivative11( template typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { - return numericalDerivative11(boost::bind(h, boost::placeholders::_1), x, delta); + return numericalDerivative11(boost::bind(h, boost::placeholders::_1), x, + delta); } /** @@ -169,14 +170,17 @@ 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, boost::placeholders::_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) { - return numericalDerivative21(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta); + return numericalDerivative21( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, + delta); } /** @@ -195,14 +199,17 @@ 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), boost::placeholders::_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) { - return numericalDerivative22(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta); + return numericalDerivative22( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, + delta); } /** @@ -224,14 +231,18 @@ 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, boost::placeholders::_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) { - return numericalDerivative31(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, - x2, x3, delta); + return numericalDerivative31( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3), + x1, x2, x3, delta); } /** @@ -253,14 +264,18 @@ 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), boost::placeholders::_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) { - return numericalDerivative32(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, - x2, x3, delta); + return numericalDerivative32( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3), + x1, x2, x3, delta); } /** @@ -282,14 +297,18 @@ 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), boost::placeholders::_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) { - return numericalDerivative33(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1, - x2, x3, delta); + return numericalDerivative33( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3), + x1, x2, x3, delta); } /** @@ -311,13 +330,19 @@ 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, boost::placeholders::_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) { - return numericalDerivative41(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); + return numericalDerivative41( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4), + x1, x2, x3, x4); } /** @@ -339,13 +364,19 @@ 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), boost::placeholders::_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) { - return numericalDerivative42(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); + return numericalDerivative42( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4), + x1, x2, x3, x4); } /** @@ -367,13 +398,19 @@ 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), boost::placeholders::_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) { - return numericalDerivative43(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); + return numericalDerivative43( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4), + x1, x2, x3, x4); } /** @@ -395,13 +432,19 @@ 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), boost::placeholders::_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) { - return numericalDerivative44(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4); + return numericalDerivative44( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4), + x1, x2, x3, x4); } /** @@ -424,13 +467,20 @@ 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, boost::placeholders::_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) { - return numericalDerivative51(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); + return numericalDerivative51( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -453,13 +503,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative52(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); + return numericalDerivative52( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -482,13 +539,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative53(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); + return numericalDerivative53( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -511,13 +575,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative54(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); + return numericalDerivative54( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -540,13 +611,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative55(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5); + return numericalDerivative55( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5), + x1, x2, x3, x4, x5); } /** @@ -570,13 +648,20 @@ 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, boost::placeholders::_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) { - return numericalDerivative61(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative61( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -600,13 +685,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative62(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative62( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -630,13 +722,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative63(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative63( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -660,13 +759,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative64(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative64( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -690,13 +796,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative65(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative65( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -721,13 +834,20 @@ 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), boost::placeholders::_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) { - return numericalDerivative66(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6); + return numericalDerivative66( + boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, + boost::placeholders::_3, boost::placeholders::_4, + boost::placeholders::_5, boost::placeholders::_6), + x1, x2, x3, x4, x5, x6); } /** @@ -747,8 +867,8 @@ 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, boost::placeholders::_1, delta), x, - delta); + return numericalDerivative11( + boost::bind(ng, f, boost::placeholders::_1, delta), x, delta); } template @@ -773,7 +893,8 @@ class G_x1 { f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { - return numericalGradient(boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_); + return numericalGradient( + boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_); } }; @@ -785,7 +906,8 @@ inline typename internal::FixedSizeMatrix::type numericalHessian212( G_x1 g_x1(f, x1, delta); return numericalDerivative11( boost::function( - boost::bind(boost::ref(g_x1), boost::placeholders::_1)), x2, delta); + boost::bind(boost::ref(g_x1), boost::placeholders::_1)), + x2, delta); } template @@ -804,10 +926,12 @@ inline typename internal::FixedSizeMatrix::type numericalHessian211( Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2))); + boost::function f2( + boost::bind(f, boost::placeholders::_1, boost::cref(x2))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + boost::function( + boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x1, delta); } @@ -825,10 +949,12 @@ inline typename internal::FixedSizeMatrix::type numericalHessian222( typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1)); + boost::function f2( + boost::bind(f, boost::cref(x1), boost::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + boost::function( + boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x2, delta); } @@ -850,10 +976,12 @@ inline typename internal::FixedSizeMatrix::type numericalHessian311( typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3))); + boost::function f2(boost::bind( + f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + boost::function( + boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x1, delta); } @@ -873,10 +1001,12 @@ inline typename internal::FixedSizeMatrix::type numericalHessian322( typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3))); + boost::function f2(boost::bind( + f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3))); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + boost::function( + boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x2, delta); } @@ -896,10 +1026,12 @@ inline typename internal::FixedSizeMatrix::type numericalHessian333( typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1)); + boost::function f2(boost::bind( + f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1)); return numericalDerivative11( - boost::function(boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + boost::function( + boost::bind(numGrad, f2, boost::placeholders::_1, delta)), x3, delta); } @@ -917,7 +1049,9 @@ inline typename internal::FixedSizeMatrix::type numericalHessian312( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, boost::cref(x3))), + boost::function( + boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, + boost::cref(x3))), x1, x2, delta); } @@ -926,7 +1060,9 @@ inline typename internal::FixedSizeMatrix::type numericalHessian313( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::placeholders::_2)), + boost::function( + boost::bind(f, boost::placeholders::_1, boost::cref(x2), + boost::placeholders::_2)), x1, x3, delta); } @@ -935,7 +1071,9 @@ inline typename internal::FixedSizeMatrix::type numericalHessian323( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::placeholders::_2)), + boost::function( + boost::bind(f, boost::cref(x1), boost::placeholders::_1, + boost::placeholders::_2)), x2, x3, delta); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 6a345a6b5d..a6638c1d71 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -308,38 +308,68 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + Pose1); + Matrix H1_Vel = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + Vel1); + Matrix H2_Vel = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1); + Matrix H3_Pose = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + Bias1); + Matrix H3_Vel = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2); + Matrix H4_Pose = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + Pose2); + Matrix H4_Vel = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2); + Matrix H5_Pose = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + Vel2); + Matrix H5_Vel = numericalDerivative11( + boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } @@ -438,18 +468,45 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, boost::placeholders::_1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, boost::placeholders::_1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + boost::placeholders::_1, delta_vel_in_t0), + delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + delta_pos_in_t0, boost::placeholders::_1), + delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0); + Matrix H_vel_vel = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1, + flag_use_body_P_sensor, body_P_sensor, Bias_t0), + delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0, + flag_use_body_P_sensor, body_P_sensor, Bias_t0), + delta_angles); + Matrix H_vel_bias = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, + flag_use_body_P_sensor, body_P_sensor, + boost::placeholders::_1), + Bias_t0); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0); + Matrix H_angles_angles = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, + body_P_sensor, Bias_t0), + delta_angles); + Matrix H_angles_bias = numericalDerivative11( + boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, + boost::placeholders::_1), + Bias_t0); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 968810e0a3..3f526e934f 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -235,38 +235,68 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5(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); + 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, 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); + 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, 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); + 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, boost::placeholders::_1, Vel2), Pose2); - Matrix H4_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_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, boost::placeholders::_1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_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 7292d702b0..a9dcb8ceff 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -108,8 +108,9 @@ class InvDepthFactorVariant1: public NoiseModelFactor2 { if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, boost::placeholders::_1, - landmark), pose); + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, + boost::placeholders::_1, landmark), + pose); } if (H2) { (*H2) = numericalDerivative11( diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index d8c790342c..4595a934b1 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -110,10 +110,16 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { boost::optional H2=boost::none) const override { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, boost::placeholders::_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, boost::placeholders::_1), landmark); + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, + boost::placeholders::_1), + landmark); } return inverseDepthError(pose, landmark); @@ -231,13 +237,22 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3 { boost::optional H3=boost::none) const override { if(H1) - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, boost::placeholders::_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, boost::placeholders::_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, boost::placeholders::_1), landmark); + (*H3) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + pose2, boost::placeholders::_1), + landmark); return inverseDepthError(pose1, pose2, landmark); }