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