diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 2fa6e33c6..cc1cbdb51 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -177,7 +177,7 @@ 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, x2), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -202,7 +202,7 @@ 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, x1, _1), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); } /** use a raw C++ function pointer */ @@ -230,7 +230,7 @@ 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, x2, x3), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); } template @@ -258,7 +258,7 @@ 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, x1, _1, x3), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); } template @@ -286,7 +286,7 @@ 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, x1, x2, _1), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); } template @@ -314,7 +314,7 @@ 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, x2, x3, x4), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); } template @@ -341,7 +341,7 @@ 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, x1, _1, x3, x4), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); } template @@ -368,7 +368,7 @@ 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, x1, x2, _1, x4), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); } template @@ -395,7 +395,7 @@ 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, x1, x2, x3, _1), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); } template @@ -423,7 +423,7 @@ 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, x2, x3, x4, x5), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); } template @@ -451,7 +451,7 @@ 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, x1, _1, x3, x4, x5), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); } template @@ -479,7 +479,7 @@ 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, x1, x2, _1, x4, x5), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); } template @@ -507,7 +507,7 @@ 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, x1, x2, x3, _1, x5), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); } template @@ -535,7 +535,7 @@ 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, x1, x2, x3, x4, _1), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); } template @@ -587,7 +587,7 @@ public: f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { - return numericalGradient(boost::bind(f_, _1, x2), x1_, delta_); + return numericalGradient(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_); } }; @@ -618,7 +618,7 @@ inline typename internal::FixedSizeMatrix::type numericalHessian211( Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2)); + boost::function f2(boost::bind(f, _1, boost::cref(x2))); return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), @@ -639,7 +639,7 @@ 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, x1, _1)); + boost::function f2(boost::bind(f, boost::cref(x1), _1)); return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), @@ -664,7 +664,7 @@ 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, _1, x2, x3)); + boost::function f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3))); return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), @@ -687,7 +687,7 @@ 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, x1, _1, x3)); + boost::function f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3))); return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), @@ -710,7 +710,7 @@ 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, x1, x2, _1)); + boost::function f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1)); return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), @@ -731,7 +731,7 @@ 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, _1, _2, x3)), + boost::function(boost::bind(f, _1, _2, boost::cref(x3))), x1, x2, delta); } @@ -740,7 +740,7 @@ 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, _1, x2, _2)), + boost::function(boost::bind(f, _1, boost::cref(x2), _2)), x1, x3, delta); } @@ -749,7 +749,7 @@ 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, x1, _1, _2)), + boost::function(boost::bind(f, boost::cref(x1), _1, _2)), x2, x3, delta); }