From c0cd024b2a9ba5a2bebeede6e9e53022637641c2 Mon Sep 17 00:00:00 2001
From: Asa Hammond <asahammond@gmail.com>
Date: Tue, 23 Feb 2021 18:27:20 -0800
Subject: [PATCH] bind a lambda instead of a static function

---
 gtsam/navigation/ConstantVelocityFactor.h | 46 +++++++++++------------
 1 file changed, 22 insertions(+), 24 deletions(-)

diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h
index 2188cc76e..08cd531b6 100644
--- a/gtsam/navigation/ConstantVelocityFactor.h
+++ b/gtsam/navigation/ConstantVelocityFactor.h
@@ -20,35 +20,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
     gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
                                 boost::optional<gtsam::Matrix &> H1 = boost::none,
                                 boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
+        // so we can reuse this calculation below
+        auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector {
+            const Velocity3 &v1 = x1.v();
+            const Velocity3 &v2 = x2.v();
+            const Point3 &p1 = x1.t();
+            const Point3 &p2 = x2.t();
+
+            // trapezoidal integration constant accelleration
+            // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0);
+
+            // euler start
+            // const Point3 hx = p1 + Point3(v1 * dt);
+
+            // euler end
+            const Point3 hx = p1 + Point3(v2 * dt);
+
+            return p2 - hx;
+        };
+
         if (H1) {
-            (*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(
-                boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
+            (*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
         }
         if (H2) {
-            (*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(
-                boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5);
+            (*H2) = numericalDerivative22<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5);
         }
 
-        return evaluateError_(x1, x2, dt_);
-    }
-
-   private:
-    static gtsam::Vector evaluateError_(const NavState &x1, const NavState &x2, double dt) {
-        const Velocity3 &v1 = x1.v();
-        const Velocity3 &v2 = x2.v();
-        const Point3 &p1 = x1.t();
-        const Point3 &p2 = x2.t();
-
-        // trapezoidal integration constant accelleration
-        // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0);
-
-        // euler start
-        // const Point3 hx = p1 + Point3(v1 * dt);
-
-        // euler end
-        const Point3 hx = p1 + Point3(v2 * dt);
-
-        return p2 - hx;
+        return evaluateErrorInner(x1, x2);
     }
 };