From 9967c59ed0c9a1112bbffa7d84908154439e22cf Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 14:30:16 +0200 Subject: [PATCH 01/41] Forward declaration for Set of Fisheye Cameras Forward declaration of camera vector for PinholeCamera and PinholeCamera. --- gtsam/geometry/triangulation.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22e..b18c55818d 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include #include @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam From dfd50f98c26791e68ab670c1768605fba9f20209 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:13:29 +0200 Subject: [PATCH 02/41] Extend python wrapper to include fisheye models. Extend python wrapper to include fisheye camera models Cal3Fisheye and Cal3Unified. --- gtsam/gtsam.i | 102 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 90 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953bb..327574bf89 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -231,7 +231,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -977,6 +977,52 @@ class Cal3Bundler { void pickle() const; }; +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + + #include class CalibratedCamera { // Standard Constructors and Named Constructors @@ -1085,6 +1131,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -1145,7 +1192,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -2118,8 +2171,11 @@ class NonlinearFactorGraph { template , + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -2252,9 +2308,13 @@ class Values { void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -2272,9 +2332,13 @@ class Values { void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -2294,10 +2358,14 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, - gtsam::EssentialMatrix, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, - gtsam::PinholeCamera, - gtsam::imuBias::ConstantBias, + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, @@ -2603,7 +2671,9 @@ class ISAM2 { template , + gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, + gtsam::Cal3Fisheye, gtsam::Cal3Unified, + gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2655,10 +2725,14 @@ template }> + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2800,6 +2874,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; #include @@ -2810,9 +2886,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); gtsam::Point2 measured() const; From 55c12743fc29191c0e3d63c12b7c52f8284c402f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:45:11 +0200 Subject: [PATCH 03/41] Forward declaration of fisheye camera. Forward declaration of PinholeCal3Fisheye needed by Python wrapper. --- gtsam/geometry/SimpleCamera.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c77..2c34bdfa73 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -33,7 +34,8 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; - + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x From c8fc3cd216020487be3850d58b56eca9ff74ec99 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:53:41 +0200 Subject: [PATCH 04/41] Unit test for equidistant fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 105 +++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 python/gtsam/tests/test_Cal3Fisheye.py diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py new file mode 100644 index 0000000000..18c8ecd80e --- /dev/null +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -0,0 +1,105 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCal3Fisheye(GtsamTestCase): + + def test_Cal3Fisheye(self): + K = gtsam.Cal3Fisheye() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fy(), 1.) + + def test_distortion(self): + "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + equidistant = gtsam.Cal3Fisheye() + x, y, z = 1, 0, 1 + u, v = equidistant.uncalibrate([x, y]) + x2, y2 = equidistant.calibrate([u, v]) + self.assertAlmostEqual(u, np.arctan2(x, z)) + self.assertAlmostEqual(v, 0) + self.assertAlmostEqual(x2, x) + self.assertAlmostEqual(y2, 0) + + def test_pinhole(self): + "Spatial equidistant camera projection" + x, y, z = 1.0, 0.0, 1.0 + u, v = np.arctan2(x, z), 0.0 + camera = gtsam.PinholeCameraCal3Fisheye() + + pt1 = camera.Project([x, y, z]) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + + pt2 = camera.project([x, y, z]) + self.gtsamAssertEquals(pt2, np.array([u, v])) + + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, np.linalg.norm([x, y, z])) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_cal3fisheye(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_retract(self): + expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) + K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') + actual = K.retract(d) + self.gtsamAssertEquals(actual, expected) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + + +if __name__ == "__main__": + unittest.main() From 19e8cde733c40e2e6dc0beb3c313c55b34e354fa Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:59:56 +0200 Subject: [PATCH 05/41] Extend unit testing of omnidirectional projection Test projection function and factors using a stereoscopic (xi=1) reference model, i.e the image height is given by y = 2 f tan(theta/2). --- python/gtsam/tests/test_Cal3Unified.py | 83 ++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index fbf5f3565c..ff9d659607 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -23,6 +23,89 @@ def test_Cal3Unified(self): self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) + def test_distortion(self): + "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + x, y, z = 1, 0, 1 + u, v = stereographic.uncalibrate([x, y]) + r = np.linalg.norm([x, y, z]) + # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) + self.assertAlmostEqual(u, 2/(1+r)) + x2, y2 = stereographic.calibrate([u, v]) + self.assertAlmostEqual(x2, x) + + def test_pinhole(self): + "Spatial stereographic camera projection" + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2/(1+r), 0.0 + objPoint = np.array([x, y, z]) + imgPoint = np.array([u, v]) + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + pose = gtsam.Pose3() + camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) + pt1 = camera.Project(objPoint) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + pt2 = camera.project(objPoint) + self.gtsamAssertEquals(pt2, np.array([u, v])) + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, r) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_cal3unified(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From bdeb60679b7ea8f9b1a7a53166da65590b007bcb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:14:10 +0200 Subject: [PATCH 06/41] Introduce setUpClass, python snake_case variables Test case fails if object depth z is not equal 1. --- python/gtsam/tests/test_Cal3Fisheye.py | 103 +++++++++++++------------ 1 file changed, 53 insertions(+), 50 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 18c8ecd80e..23f7a9b8c3 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -5,8 +5,9 @@ See LICENSE for the license information -Cal3Unified unit tests. +Cal3Fisheye unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) +Refactored: Roderick Koehle """ import unittest @@ -14,78 +15,80 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase - +from gtsam.symbol_shorthand import K, L, P class TestCal3Fisheye(GtsamTestCase): - + + @classmethod + def setUpClass(cls): + """ + Equidistant fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = tan(theta), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + u, v = np.arctan2(x, z), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fy(), 1.) def test_distortion(self): - "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + """Fisheye distortion and rectification""" equidistant = gtsam.Cal3Fisheye() - x, y, z = 1, 0, 1 - u, v = equidistant.uncalibrate([x, y]) - x2, y2 = equidistant.calibrate([u, v]) - self.assertAlmostEqual(u, np.arctan2(x, z)) - self.assertAlmostEqual(v, 0) - self.assertAlmostEqual(x2, x) - self.assertAlmostEqual(y2, 0) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = equidistant.uncalibrate(perspective_pt) + rectified_pt = equidistant.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial equidistant camera projection" - x, y, z = 1.0, 0.0, 1.0 - u, v = np.arctan2(x, z), 0.0 + """Spatial equidistant camera projection""" camera = gtsam.PinholeCameraCal3Fisheye() - - pt1 = camera.Project([x, y, z]) + pt1 = camera.Project(self.obj_point) # Perspective projection + pt2 = camera.project(self.obj_point) # Equidistant projection + x, y, z = self.obj_point + obj1 = camera.backproject(self.img_point, z) + r1 = camera.range(self.obj_point) + r = np.linalg.norm(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - - pt2 = camera.project([x, y, z]) - self.gtsamAssertEquals(pt2, np.array([u, v])) - - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - - r1 = camera.range(np.array([x, y, z])) - self.assertEqual(r1, np.linalg.norm([x, y, z])) + self.gtsamAssertEquals(pt2, self.img_point) + self.gtsamAssertEquals(obj1, self.obj_point) + self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual with camera, pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_cal3fisheye(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + state.insert_cal3fisheye(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) @@ -93,12 +96,12 @@ def test_sfm_factor2(self): def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) - K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') - actual = K.retract(d) + actual = k.retract(d) self.gtsamAssertEquals(actual, expected) - np.testing.assert_allclose(d, K.localCoordinates(actual)) + np.testing.assert_allclose(d, k.localCoordinates(actual)) if __name__ == "__main__": From 6205057ccbd63c3705ac109b47104d14e10b41e9 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:17:38 +0200 Subject: [PATCH 07/41] Use of common setUpClass method --- python/gtsam/tests/test_Cal3Unified.py | 121 ++++++++++++------------- 1 file changed, 60 insertions(+), 61 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ff9d659607..f2c1ada488 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -14,94 +14,93 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): + @classmethod + def setUpClass(cls): + """ + Stereographic fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = 2*tan(theta/2), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2*x/(z+r), 0.0 + #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) def test_distortion(self): - "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - x, y, z = 1, 0, 1 - u, v = stereographic.uncalibrate([x, y]) + """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)""" + x, y, z = self.obj_point r = np.linalg.norm([x, y, z]) - # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) - self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) - self.assertAlmostEqual(u, 2/(1+r)) - x2, y2 = stereographic.calibrate([u, v]) - self.assertAlmostEqual(x2, x) + # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r)) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = self.stereographic.uncalibrate(perspective_pt) + rectified_pt = self.stereographic.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial stereographic camera projection" - x, y, z = 1.0, 0.0, 1.0 - r = np.linalg.norm([x, y, z]) - u, v = 2/(1+r), 0.0 - objPoint = np.array([x, y, z]) - imgPoint = np.array([u, v]) - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + """Spatial stereographic camera projection""" + x, y, z = self.obj_point + u, v = self.img_point + r = np.linalg.norm(self.obj_point) pose = gtsam.Pose3() - camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) - pt1 = camera.Project(objPoint) + camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic) + pt1 = camera.Project(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - pt2 = camera.project(objPoint) - self.gtsamAssertEquals(pt2, np.array([u, v])) - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - r1 = camera.range(np.array([x, y, z])) + pt2 = camera.project(self.obj_point) + self.gtsamAssertEquals(pt2, self.img_point) + obj1 = camera.backproject(self.img_point, z) + self.gtsamAssertEquals(obj1, self.obj_point) + r1 = camera.range(self.obj_point) self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = self.stereographic + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual with camera, pose and point as state variables""" + r = np.linalg.norm(self.obj_point) graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_cal3unified(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = self.stereographic + state.insert_cal3unified(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, self.obj_point) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) From a411b664a198776f21aa6fd9a43732841e44add7 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:25:18 +0200 Subject: [PATCH 08/41] Correct tab to spaces to fix formatting --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 327574bf89..ce251802ce 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2360,12 +2360,12 @@ class Values { gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, - gtsam::EssentialMatrix, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, - gtsam::imuBias::ConstantBias, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, From 66af0079baa5de84618fdc1927dc55e8e9fcff81 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 12:39:31 +0200 Subject: [PATCH 09/41] Improved accuracy for analytic undistortion --- gtsam/geometry/Cal3Fisheye.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index b9e60aceec..3ee036efff 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; - Point2 pi(xd, yd); + const double theta = sqrt(xd * xd + yd * yd); + const double scale = (theta > 0) ? tan(theta) / theta; + Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, // throw exception if max iterations are reached From 15478bf2783fbf4bac53e570bcd3bb61376a7d68 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:33:33 -0400 Subject: [PATCH 10/41] Update ShonanAveraging.h --- gtsam/sfm/ShonanAveraging.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index f00109caee..bff5ab471a 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 3c1823349b6b90a7fce71bfeca229a56e26bed70 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:38:10 -0400 Subject: [PATCH 11/41] add interface in C++, and helper extractRot2Measurements() --- gtsam/sfm/ShonanAveraging.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a982ef7da9..f933f368d8 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,6 +944,20 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +static ShonanAveraging2::Measurements extractRot2Measurements( + const BetweenFactorPose2s &factors) { + ShonanAveraging2::Measurements result; + result.reserve(factors.size()); + for (auto f : factors) result.push_back(convert(f)); + return result; +} + +ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters) + : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + parameters.getUseHuber()), + parameters) {} + /* ************************************************************************* */ // Explicit instantiation for d=3 template class ShonanAveraging<3>; From 0e0d630c917ec6f80a8a037f61bd030842f7feb0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:39:38 -0400 Subject: [PATCH 12/41] fix typo --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index f933f368d8..0de37be1e6 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static ShonanAveraging2::Measurements extractRot2Measurements( ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + : ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors), parameters.getUseHuber()), parameters) {} From 3c8cdb4eeee70f0a95a8ccea54373262ea24b618 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:42:59 -0400 Subject: [PATCH 13/41] add ShonanAveraging2 constructor to wrapper, that accepts BetweenFactorPose2s as input --- gtsam/gtsam.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953bb..f17679739f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3123,6 +3123,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); // Query properties size_t nrUnknowns() const; From 4bf2308ec595a96270952d81ce25e8d2d2d447fa Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:48:26 -0400 Subject: [PATCH 14/41] add conversion function for Pose2 -> BinaryMeasurement --- gtsam/sfm/ShonanAveraging.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 0de37be1e6..7810d0d470 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +// Extract Rot2 measurement from Pose2 betweenfactors +// Modeled after similar function in dataset.cpp +static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( + const BetweenFactor::shared_ptr &f) { + auto gaussian = + boost::dynamic_pointer_cast(f->noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix6 M = gaussian->covariance(); + auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); +} + static ShonanAveraging2::Measurements extractRot2Measurements( const BetweenFactorPose2s &factors) { ShonanAveraging2::Measurements result; result.reserve(factors.size()); - for (auto f : factors) result.push_back(convert(f)); + for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f)); return result; } From 641a01c72620c6736d917c470e85db26ae471980 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:54:14 -0400 Subject: [PATCH 15/41] fix typo on 3x3 matrix def --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 7810d0d470..88c393f16d 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( throw std::invalid_argument( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); - const Matrix6 M = gaussian->covariance(); + const Matrix3 M = gaussian->covariance(); auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); From 7fc8f23367eeba02c7ac20464eaeee78ce9d5702 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 23:34:55 -0400 Subject: [PATCH 16/41] use default parameters if none provided, and remove gtsam namespace prefix in .h file --- gtsam/sfm/ShonanAveraging.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index bff5ab471a..de12de4782 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,8 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters = Parameters()); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 64514387b42e04249b467c9d2b5bbb6807ded4b5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 00:57:01 -0400 Subject: [PATCH 17/41] check in python unit test for new functionality --- python/gtsam/tests/test_ShonanAveraging.py | 66 +++++++++++++++++++++- 1 file changed, 64 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 4c423574d8..fe0d7fc2b0 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,14 +13,25 @@ import unittest import gtsam -from gtsam import ShonanAveraging3, ShonanAveragingParameters3 +from gtsam import ( + BetweenFactorPose2, + LevenbergMarquardtParams, + Rot2, + Pose2, + ShonanAveraging2, + ShonanAveragingParameters2, + ShonanAveraging3, + ShonanAveragingParameters3 +) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( gtsam.LevenbergMarquardtParams.CeresDefaults()) -def fromExampleName(name: str, parameters=DEFAULT_PARAMS): +def fromExampleName( + name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS +) -> ShonanAveraging3: g2oFile = gtsam.findExampleDataFile(name) return ShonanAveraging3(g2oFile, parameters) @@ -133,6 +144,57 @@ def test_PriorWeights(self): self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + + def test_constructorBetweenFactorPose2s(self) -> None: + """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + i2Ri1_dict = { + (1, 2): -43.86342, + (1, 5): -135.06351, + (2, 4): 72.64527, + (3, 5): 117.75967, + (6, 7): -31.73934, + (7, 10): 177.45901, + (6, 9): -133.58713, + (7, 8): -90.58668, + (0, 3): 127.02978, + (8, 10): -92.16361, + (4, 8): 51.63781, + (4, 6): 173.96384, + (4, 10): 139.59445, + (2, 3): 151.04022, + (3, 4): -78.39495, + (1, 4): 28.78185, + (6, 8): -122.32602, + (0, 2): -24.01045, + (5, 7): -53.93014, + (4, 5): -163.84535, + (2, 5): -91.20009, + (1, 3): 107.17680, + (7, 9): -102.35615, + (0, 1): 19.85297, + (5, 8): -144.51682, + (5, 6): -22.19079, + (5, 10): -56.56016, + } + i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()} + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters2(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + + noise_model = gtsam.noiseModel.Unit.Create(3) + between_factors = gtsam.BetweenFactorPose2s() + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + i2Ti1 = Pose2(i2Ri1, np.zeros(2)) + between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) + + obj = ShonanAveraging2(between_factors, shonan_params) + initial = obj.initializeRandomly() + result_values, _ = obj.run(initial, p_min, self._p_max) + + for i in range(11): + wRi = result_values.atRot2(i) if __name__ == '__main__': From 03049929a55d430bc67404bf8663d792ee9d62fb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 15:21:22 +0200 Subject: [PATCH 18/41] Add comment about initial guess in undistortion For the equidistant fisheye model, r/f = tan(theta), the Gauss-Newton search to model radial distortion is expected to converge faster by mapping the angular coordinate space into the respective tangent space of the perspective plane. This is consistent to the nPlaneToSpace initial projection used in the calibrate() function of the omnidirectional model (Cal3Unified). --- gtsam/geometry/Cal3Fisheye.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 3ee036efff..52d475d5d1 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, /* ************************************************************************* */ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // initial gues just inverts the pinhole model + // Apply inverse camera matrix to map the pixel coordinate (u, v) + // of the equidistant fisheye image to angular coordinate space (xd, yd) + // with radius theta given in radians. const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; const double theta = sqrt(xd * xd + yd * yd); - const double scale = (theta > 0) ? tan(theta) / theta; + + // Provide initial guess for the Gauss-Newton search. + // The angular coordinates given by (xd, yd) are mapped back to + // the focal plane of the perspective undistorted projection pi. + // See Cal3Unified.calibrate() using the same pattern for the + // undistortion of omnidirectional fisheye projection. + const double scale = (theta > 0) ? tan(theta) / theta : 1.0; Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, From db801f164d2ff7c6c65d79058959c6dfb7832ef4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 10:31:27 -0400 Subject: [PATCH 19/41] add missing import to python unit test --- python/gtsam/tests/test_ShonanAveraging.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index fe0d7fc2b0..a873a64306 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,6 +13,7 @@ import unittest import gtsam +import numpy as np from gtsam import ( BetweenFactorPose2, LevenbergMarquardtParams, From aebb90573ab688287cf703f6ed4af97f5a2be962 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 11:41:46 -0400 Subject: [PATCH 20/41] set pmin and pmax in unit test --- python/gtsam/tests/test_ShonanAveraging.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index a873a64306..69e7055453 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -148,6 +148,7 @@ def test_PriorWeights(self): def test_constructorBetweenFactorPose2s(self) -> None: """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + num_images = 11 # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 i2Ri1_dict = { (1, 2): -43.86342, @@ -192,9 +193,9 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, p_min, self._p_max) + result_values, _ = obj.run(initial, min_p=2, max_p=10) - for i in range(11): + for i in range(num_images): wRi = result_values.atRot2(i) From d54e234f93128c3c612e0ef0a2c65cb9a1182fae Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 22:03:17 +0200 Subject: [PATCH 21/41] Add ambiguous calibrate/uncalibrate declarations. Without declaring calibrate / uncalibrated in the interface specification, the functions of the Base class Cal3DS2_Base is called. The layout of the optional Jacobian matrix is 2x10 in Cal3Unified and 2x9 in Cal3DS2_Base, so this are different function calls. --- gtsam/gtsam.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index ce251802ce..97012c2910 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -910,6 +910,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { gtsam::Cal3Unified retract(Vector v) const; Vector localCoordinates(const gtsam::Cal3Unified& c) const; + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // enabling serialization functionality void serialize() const; From 3118fde6d3dce098ffbca641afb18488f97a9dbc Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:00:24 +0200 Subject: [PATCH 22/41] Missing CameraSet binding specialisations Add pybind specialisations for CameraSetCal3Unified and CameraSetCal3Fisheye. --- python/gtsam/specializations.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index be8eb8a6cf..2e8612fec6 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -32,4 +32,6 @@ py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector > >(m_, "CameraSetCal3_S2"); py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector > >(m_, "CameraSetCal3Unified"); +py::bind_vector > >(m_, "CameraSetCal3Fisheye"); py::bind_vector >(m_, "JacobianVector"); From 0a73961f5a2762100d30147db25fde150434d1a6 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:05:53 +0200 Subject: [PATCH 23/41] Update ignore list in CMakeFile --- python/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6a..528732f4bd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target From dc8b5e58ff2b8b48ba5b5fc329271daf3b8c2468 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Jul 2021 21:01:20 -0400 Subject: [PATCH 24/41] replaced boost with std for placeholders, bind and function --- gtsam/base/Matrix.h | 4 +- gtsam/base/Testable.h | 7 +- gtsam/base/lieProxies.h | 2 +- gtsam/base/numericalDerivative.h | 345 +++++++++--------- gtsam/discrete/DecisionTree-inl.h | 4 +- gtsam/discrete/DecisionTree.h | 12 +- gtsam/geometry/Cyclic.h | 4 +- gtsam/inference/EliminateableFactorGraph.h | 4 +- gtsam/inference/LabeledSymbol.cpp | 41 ++- gtsam/inference/LabeledSymbol.h | 8 +- gtsam/inference/Symbol.cpp | 7 +- gtsam/inference/Symbol.h | 7 +- gtsam/linear/VectorValues.cpp | 4 +- gtsam/nonlinear/Expression-inl.h | 24 +- gtsam/nonlinear/Expression.h | 8 +- gtsam/nonlinear/NonlinearEquality.h | 10 +- gtsam/nonlinear/Values-inl.h | 26 +- gtsam/nonlinear/Values.h | 18 +- gtsam/nonlinear/internal/ExpressionNode.h | 2 +- gtsam_unstable/base/BTree.h | 8 +- gtsam_unstable/base/DSF.h | 2 +- gtsam_unstable/dynamics/FullIMUFactor.h | 4 +- gtsam_unstable/dynamics/IMUFactor.h | 4 +- gtsam_unstable/dynamics/SimpleHelicopter.h | 12 +- gtsam_unstable/dynamics/VelocityConstraint.h | 8 +- gtsam_unstable/linear/QPSParser.cpp | 47 ++- .../slam/EquivInertialNavFactor_GlobalVel.h | 68 ++-- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 26 +- .../slam/InertialNavFactor_GlobalVelocity.h | 40 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 8 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 8 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 20 +- 32 files changed, 408 insertions(+), 384 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a3a14c6c32..013947bbde 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction { // The function phi should calculate f(a)*b, with derivatives in a and b. // Naturally, the derivative in b is f(a). - typedef boost::function, OptionalJacobian)> Operator; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index e8cdbd8e09..6062c7ae12 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,8 +34,9 @@ #pragma once #include -#include +#include #include +#include #include #define GTSAM_PRINT(x)((x).print(#x)) @@ -119,10 +120,10 @@ namespace gtsam { * Binary predicate on shared pointers */ template - struct equals_star : public std::function&, const boost::shared_ptr&)> { + struct equals_star : public std::function&, const std::shared_ptr&)> { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} - bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + bool operator()(const std::shared_ptr& expected, const std::shared_ptr& actual) { if (!actual && !expected) return true; return actual && expected && traits::Equals(*actual,*expected, tol_); } diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 820fc73337..c811eb58a7 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -24,7 +24,7 @@ * * These should not be used outside of tests, as they are just remappings * of the original functions. We use these to avoid needing to do - * too much boost::bind magic or writing a bunch of separate proxy functions. + * too much std::bind magic or writing a bunch of separate proxy functions. * * Don't expect all classes to work for all of these functions. */ diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index c624c900eb..05b60033f8 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -18,8 +18,7 @@ // \callgraph #pragma once -#include -#include +#include #include #include @@ -38,13 +37,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, boost::placeholders::_1, boost::none) + * std::bind(bar, std::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), boost::placeholders::_1) + * std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1) * * For additional details, see the documentation: * http://www.boost.org/doc/libs/release/libs/bind/bind.html @@ -69,7 +68,7 @@ struct FixedSizeMatrix { template ::dimension> typename Eigen::Matrix numericalGradient( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); BOOST_STATIC_ASSERT_MSG( @@ -109,7 +108,7 @@ typename Eigen::Matrix numericalGradient( template ::dimension> // TODO Should compute fixed-size matrix typename internal::FixedSizeMatrix::type numericalDerivative11( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), @@ -150,7 +149,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, boost::placeholders::_1), x, + return numericalDerivative11(std::bind(h, std::placeholders::_1), x, delta); } @@ -164,14 +163,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, +typename internal::FixedSizeMatrix::type numericalDerivative21(const std::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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); + std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -179,7 +178,7 @@ 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, + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, delta); } @@ -193,14 +192,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, +typename internal::FixedSizeMatrix::type numericalDerivative22(std::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "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); + std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta); } /** use a raw C++ function pointer */ @@ -208,7 +207,7 @@ 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, + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, delta); } @@ -225,14 +224,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)), x1, delta); } @@ -240,8 +239,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -258,14 +257,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)), x2, delta); } @@ -273,8 +272,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -291,14 +290,14 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1), x3, delta); } @@ -306,8 +305,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -324,15 +323,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4)), x1, delta); } @@ -340,8 +339,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -358,15 +357,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4)), x2, delta); } @@ -374,8 +373,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -392,15 +391,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4)), x3, delta); } @@ -408,8 +407,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -426,15 +425,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1), x4, delta); } @@ -442,8 +441,8 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -461,15 +460,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5)), x1, delta); } @@ -477,9 +476,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -497,15 +496,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5)), x2, delta); } @@ -513,9 +512,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -533,15 +532,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5)), x3, delta); } @@ -549,9 +548,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -569,15 +568,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5)), x4, delta); } @@ -585,9 +584,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -605,15 +604,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1), x5, delta); } @@ -621,9 +620,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -642,15 +641,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), x1, delta); } @@ -658,9 +657,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -679,15 +678,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), x2, delta); } @@ -695,9 +694,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -716,15 +715,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5), std::cref(x6)), x3, delta); } @@ -732,9 +731,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -753,15 +752,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5), std::cref(x6)), x4, delta); } @@ -769,9 +768,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -790,15 +789,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "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)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1, std::cref(x6)), x5, delta); } @@ -806,9 +805,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -827,7 +826,7 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( - boost::function h, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), @@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( 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), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::placeholders::_1), x6, delta); } @@ -844,9 +843,9 @@ 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), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -859,22 +858,22 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (* * @return n*n Hessian matrix computed via central differencing */ template -inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, +inline typename internal::FixedSizeMatrix::type numericalHessian(std::function f, const X& x, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); typedef Eigen::Matrix::dimension, 1> VectorD; - typedef boost::function F; - typedef boost::function G; + typedef std::function F; + typedef std::function G; G ng = static_cast(numericalGradient ); return numericalDerivative11( - boost::bind(ng, f, boost::placeholders::_1, delta), x, delta); + std::bind(ng, f, std::placeholders::_1, delta), x, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f)(const X&), const X& x, double delta = 1e-5) { - return numericalHessian(boost::function(f), x, delta); + return numericalHessian(std::function(f), x, delta); } /** Helper class that computes the derivative of f w.r.t. x1, centered about @@ -882,86 +881,86 @@ inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f */ template class G_x1 { - const boost::function& f_; + const std::function& f_; X1 x1_; double delta_; public: typedef typename internal::FixedSizeMatrix::type Vector; - G_x1(const boost::function& f, const X1& x1, + G_x1(const std::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { return numericalGradient( - boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_); + std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_); } }; template inline typename internal::FixedSizeMatrix::type numericalHessian212( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; G_x1 g_x1(f, x1, delta); return numericalDerivative11( - boost::function( - boost::bind(boost::ref(g_x1), boost::placeholders::_1)), + std::function( + std::bind(std::ref(g_x1), std::placeholders::_1)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian212(boost::function(f), + return numericalHessian212(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2( - boost::bind(f, boost::placeholders::_1, boost::cref(x2))); + std::function f2( + std::bind(f, std::placeholders::_1, std::cref(x2))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian211(boost::function(f), + return numericalHessian211(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2( - boost::bind(f, boost::cref(x1), boost::placeholders::_1)); + std::function f2( + std::bind(f, std::cref(x1), std::placeholders::_1)); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian222(boost::function(f), + return numericalHessian222(std::function(f), x1, x2, delta); } @@ -971,17 +970,17 @@ inline typename internal::FixedSizeMatrix::type numericalHessian222(doubl /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian311( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3))); + std::function f2(std::bind( + f, std::placeholders::_1, std::cref(x2), std::cref(x3))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } @@ -989,24 +988,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian311( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian322( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3))); + std::function f2(std::bind( + f, std::cref(x1), std::placeholders::_1, std::cref(x3))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } @@ -1014,24 +1013,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian322( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian333( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X3&, + Vector (*numGrad)(std::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1)); + std::function f2(std::bind( + f, std::cref(x1), std::cref(x2), std::placeholders::_1)); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x3, delta); } @@ -1039,41 +1038,41 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian333( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian312( - boost::function f, const X1& x1, + std::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))), + std::function( + std::bind(f, std::placeholders::_1, std::placeholders::_2, + std::cref(x3))), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian313( - boost::function f, const X1& x1, + std::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)), + std::function( + std::bind(f, std::placeholders::_1, std::cref(x2), + std::placeholders::_2)), x1, x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian323( - boost::function f, const X1& x1, + std::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)), + std::function( + std::bind(f, std::cref(x1), std::placeholders::_1, + std::placeholders::_2)), x2, x3, delta); } @@ -1082,7 +1081,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian312( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -1090,7 +1089,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian313( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -1098,7 +1097,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian323( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 80b226e3a4..439889ebfc 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -450,7 +450,7 @@ namespace gtsam { template template DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op) { + const std::map& map, std::function op) { root_ = convert(other.root_, map, op); } @@ -568,7 +568,7 @@ namespace gtsam { template typename DecisionTree::NodePtr DecisionTree::convert( const typename DecisionTree::NodePtr& f, const std::map& map, - boost::function op) { + std::function op) { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index ecf03ad5d0..0ee0b8be0c 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,10 +20,12 @@ #pragma once #include + #include +#include #include -#include #include +#include namespace gtsam { @@ -38,8 +40,8 @@ namespace gtsam { public: /** Handy typedefs for unary and binary function types */ - typedef boost::function Unary; - typedef boost::function Binary; + typedef std::function Unary; + typedef std::function Binary; /** A label annotated with cardinality */ typedef std::pair LabelC; @@ -107,7 +109,7 @@ namespace gtsam { /** Convert to a different type */ template NodePtr convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); + L>& map, std::function op); /** Default constructor */ DecisionTree(); @@ -143,7 +145,7 @@ namespace gtsam { /** Convert from a different type */ template DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op); + const std::map& map, std::function op); /// @} /// @name Testable diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 0b81ff0f92..35578ffe0c 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -17,7 +17,9 @@ #include #include -#include // for cout :-( + +#include +#include // for cout :-( namespace gtsam { diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 316ca8ee4d..edc4883e71 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include #include @@ -86,7 +86,7 @@ namespace gtsam { typedef std::pair, boost::shared_ptr<_FactorType> > EliminationResult; /// The function type that does a single dense elimination step on a subgraph. - typedef boost::function Eliminate; + typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 17ff6fd22e..6a1739e201 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -15,14 +15,11 @@ * @author: Alex Cunningham */ -#include +#include -#include #include - #include - -#include +#include namespace gtsam { @@ -109,17 +106,37 @@ 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, boost::placeholders::_1)) == c; +std::function LabeledSymbol::TypeTest(unsigned char c) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)), + c); } -boost::function LabeledSymbol::LabelTest(unsigned char label) { - return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; +std::function LabeledSymbol::LabelTest(unsigned char label) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)), + label); } -boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c && - boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; +std::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { + // Use lambda functions for && and == + auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; }; + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind(logical_and, + std::bind(equals, + std::bind(&LabeledSymbol::chr, + std::bind(make, std::placeholders::_1)), + c), + std::bind(equals, + std::bind(&LabeledSymbol::label, + std::bind(make, std::placeholders::_1)), + label)); } /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 15a2a25014..5aee4a0e23 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,8 +19,8 @@ #pragma once +#include #include -#include namespace gtsam { @@ -89,13 +89,13 @@ class GTSAM_EXPORT LabeledSymbol { */ // Checks only the type - static boost::function TypeTest(unsigned char c); + static std::function TypeTest(unsigned char c); // Checks only the robot ID (label_) - static boost::function LabelTest(unsigned char label); + static std::function LabelTest(unsigned char label); // Checks both type and the robot ID - static boost::function TypeLabelTest(unsigned char c, unsigned char label); + static std::function TypeLabelTest(unsigned char c, unsigned char label); // Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index 61e397f403..925ba9ce3f 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -62,8 +62,11 @@ Symbol::operator std::string() const { static Symbol make(gtsam::Key key) { return Symbol(key);} -boost::function Symbol::ChrTest(unsigned char c) { - return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c; +std::function Symbol::ChrTest(unsigned char c) { + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)), + c); } GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 89fb0d161e..017d5134af 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -18,11 +18,12 @@ #pragma once -#include #include +#include + #include -#include #include +#include namespace gtsam { @@ -114,7 +115,7 @@ class GTSAM_EXPORT Symbol { * Values::filter() function to retrieve all key-value pairs with the * requested character. */ - static boost::function ChrTest(unsigned char c); + static std::function ChrTest(unsigned char c); /// Output stream operator that can be used with key_formatter (see Key.h). GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &); diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f72799c0aa..6a2514b358 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -38,8 +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, boost::placeholders::_1), - boost::bind(&KeyValuePair::first, boost::placeholders::_2))); + std::bind(&less::operator(), less(), std::bind(&KeyValuePair::first, std::placeholders::_1), + std::bind(&KeyValuePair::first, std::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/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 85f2f14bc3..cf2462dfc8 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -83,8 +83,8 @@ template Expression::Expression(const Expression& expression, T (A::*method)(typename MakeOptionalJacobian::type) const) : root_( - new internal::UnaryExpression(boost::bind(method, - boost::placeholders::_1, boost::placeholders::_2), + new internal::UnaryExpression(std::bind(method, + std::placeholders::_1, std::placeholders::_2), expression)) { } @@ -97,9 +97,9 @@ Expression::Expression(const Expression& expression1, const Expression& expression2) : root_( new internal::BinaryExpression( - boost::bind(method, boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4), + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), expression1, expression2)) { } @@ -114,10 +114,10 @@ Expression::Expression(const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( new internal::TernaryExpression( - boost::bind(method, boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4, boost::placeholders::_5, - boost::placeholders::_6), + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6), expression1, expression2, expression3)) { } @@ -255,9 +255,9 @@ template Expression operator*(const Expression& expression1, const Expression& expression2) { return Expression( - boost::bind(internal::apply_compose(), boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4), + std::bind(internal::apply_compose(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), expression1, expression2); } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index bda4575959..eb828760d4 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -68,20 +68,20 @@ class Expression { // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, typename MakeOptionalJacobian::type)> type; }; template struct BinaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type)> type; }; template struct TernaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, const A3&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type, @@ -239,7 +239,7 @@ class BinarySumExpression : public Expression { */ template Expression linearExpression( - const boost::function& f, const Expression& expression, + const std::function& f, const Expression& expression, const Eigen::Matrix::dimension, traits::dimension>& dTdA) { // Use lambda to endow f with a linear Jacobian typename Expression::template UnaryFunction::type g = diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f10ba93aee..6b99721565 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -69,7 +69,7 @@ class NonlinearEquality: public NoiseModelFactor1 { /** * Function that compares two values */ - typedef boost::function CompareFunction; + typedef std::function CompareFunction; CompareFunction compare_; // bool (*compare_)(const T& a, const T& b); @@ -87,8 +87,8 @@ class NonlinearEquality: public NoiseModelFactor1 { * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - const CompareFunction &_compare = boost::bind(traits::Equals, - boost::placeholders::_1, boost::placeholders::_2, 1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -98,8 +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, - boost::placeholders::_1, boost::placeholders::_2, 1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::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 eca7416c94..8ebdcab17c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -103,7 +103,7 @@ namespace gtsam { boost::transform_iterator< KeyValuePair(*)(Values::KeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::iterator> > iterator; @@ -113,7 +113,7 @@ namespace gtsam { boost::transform_iterator< ConstKeyValuePair(*)(Values::ConstKeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::const_iterator> > const_const_iterator; @@ -134,7 +134,7 @@ namespace gtsam { private: Filtered( - const boost::function& filter, + const std::function& filter, Values& values) : begin_( boost::make_transform_iterator( @@ -205,7 +205,7 @@ namespace gtsam { const_iterator begin_; const_iterator end_; ConstFiltered( - const boost::function& filter, + const std::function& filter, const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. @@ -236,35 +236,35 @@ namespace gtsam { /* ************************************************************************* */ Values::Filtered - inline Values::filter(const boost::function& filterFcn) { + inline Values::filter(const std::function& filterFcn) { return filter(filterFcn); } /* ************************************************************************* */ template Values::Filtered - Values::filter(const boost::function& filterFcn) { - return Filtered(boost::bind(&filterHelper, filterFcn, - boost::placeholders::_1), *this); + Values::filter(const std::function& filterFcn) { + return Filtered(std::bind(&filterHelper, filterFcn, + std::placeholders::_1), *this); } /* ************************************************************************* */ Values::ConstFiltered - inline Values::filter(const boost::function& filterFcn) const { + inline Values::filter(const std::function& filterFcn) const { return filter(filterFcn); } /* ************************************************************************* */ template Values::ConstFiltered - Values::filter(const boost::function& filterFcn) const { - return ConstFiltered(boost::bind(&filterHelper, - filterFcn, boost::placeholders::_1), *this); + Values::filter(const std::function& filterFcn) const { + return ConstFiltered(std::bind(&filterHelper, + filterFcn, std::placeholders::_1), *this); } /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, + inline bool Values::filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 3b447ede11..33e9e7d82d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -108,19 +108,19 @@ namespace gtsam { /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::iterator> iterator; + std::function, KeyValueMap::iterator> iterator; /// Const forward iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_iterator> const_iterator; + std::function, KeyValueMap::const_iterator> const_iterator; /// Mutable reverse iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + std::function, KeyValueMap::reverse_iterator> reverse_iterator; /// Const reverse iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; typedef KeyValuePair value_type; @@ -321,7 +321,7 @@ namespace gtsam { * the original Values class. */ Filtered - filter(const boost::function& filterFcn); + filter(const std::function& filterFcn); /** * Return a filtered view of this Values class, without copying any data. @@ -344,7 +344,7 @@ namespace gtsam { */ template Filtered - filter(const boost::function& filterFcn = &_truePredicate); + filter(const std::function& filterFcn = &_truePredicate); /** * Return a filtered view of this Values class, without copying any data. @@ -360,7 +360,7 @@ namespace gtsam { * the original Values class. */ ConstFiltered - filter(const boost::function& filterFcn) const; + filter(const std::function& filterFcn) const; /** * Return a filtered view of this Values class, without copying any data. @@ -382,7 +382,7 @@ namespace gtsam { */ template ConstFiltered - filter(const boost::function& filterFcn = &_truePredicate) const; + filter(const std::function& filterFcn = &_truePredicate) const; // Count values of given type \c ValueType template @@ -399,7 +399,7 @@ namespace gtsam { // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. template - static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + static bool filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { BOOST_STATIC_ASSERT((!boost::is_same::value)); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 2a1a07fb00..f6afb287e6 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { * Expression node. The superclass for objects that do the heavy lifting * An Expression has a pointer to an ExpressionNode underneath * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. + * are passed by value. This is the same way std::function works. * http://loki-lib.sourceforge.net/html/a00652.html */ template diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index 96424324bf..9d854a169f 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -260,7 +260,7 @@ namespace gtsam { } /** iterate over tree */ - void iter(boost::function f) const { + void iter(std::function f) const { if (!root_) return; left().iter(f); f(key(), value()); @@ -269,7 +269,7 @@ namespace gtsam { /** map key-values in tree over function f that computes a new value */ template - BTree map(boost::function f) const { + BTree map(std::function f) const { if (empty()) return BTree (); std::pair xd(key(), f(key(), value())); return BTree (left().map(f), xd, right().map(f)); @@ -282,7 +282,7 @@ namespace gtsam { * The associated values are passed to [f] in reverse sort order */ template - ACC fold(boost::function f, + ACC fold(std::function f, const ACC& a) const { if (!root_) return a; ACC ar = right().fold(f, a); // fold over right subtree diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index c7b8cd4177..4ad7d3ea87 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -122,7 +122,7 @@ class DSF: protected BTree { } // maps f over all keys, must be invertible - DSF map(boost::function func) const { + DSF map(std::function func) const { DSF t; for(const KeyLabel& pair: (Tree)*this) t = t.add(func(pair.first), func(pair.second)); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 337f2bc430..9f00f81d66 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -91,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, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::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 e082dee82b..9a742b4f0e 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -81,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, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 976f448c5e..bf3b95c0f2 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -166,24 +166,24 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 H3 = boost::none) const { if (H1) { (*H1) = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H2) { (*H2) = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H3) { (*H3) = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 840b7bba72..d24d06e798 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -86,11 +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_, boost::placeholders::_1, - boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); if (H2) *H2 = gtsam::numericalDerivative22( - boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1, - boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 88697e075d..c755f2451f 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include #include @@ -40,7 +39,7 @@ #include using boost::fusion::at_c; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; namespace bf = boost::fusion; @@ -412,50 +411,50 @@ typedef qi::grammar> base_grammar; struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; QPSVisitor *rqp_; - boost::function const &)> setName; - boost::function const &)> + std::function const &)> setName; + std::function const &)> addRow; - boost::function const &)> rhsSingle; - boost::function)> rhsDouble; - boost::function const &)> rangeSingle; - boost::function)> rangeDouble; - boost::function)> colSingle; - boost::function const &)> colDouble; - boost::function const &)> addQuadTerm; - boost::function const &)> addBound; - boost::function const &)> addFreeBound; MPSGrammar(QPSVisitor *rqp) : base_grammar(start), rqp_(rqp), - setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), - addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), - rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), - rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), - rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), - rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), - colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), - colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), - addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), - addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), - addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { + setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)), + addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)), + rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)), + rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)), + rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)), + rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)), + colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)), + colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)), + addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)), + addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)), + addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index a6638c1d71..cabbfdbe85 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -309,12 +309,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); Matrix H1_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } @@ -323,12 +323,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -336,12 +336,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); Matrix H3_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } @@ -349,12 +349,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); Matrix H4_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -363,12 +363,12 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); Matrix H5_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } @@ -469,43 +469,43 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5( - boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, - boost::placeholders::_1, delta_vel_in_t0), + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + std::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), + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + delta_pos_in_t0, std::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, + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, std::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, + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, std::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, + std::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), + std::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, + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, std::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, + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, - boost::placeholders::_1), + std::placeholders::_1), Bias_t0); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 40dc81c9ab..0e2aebd7fe 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -278,29 +278,29 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. Pose2 if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Vel2 if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -372,15 +372,15 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4(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(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = 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), 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), delta_angles); + Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(std::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), delta_angles); 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), delta_angles); + Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); 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 3f526e934f..0828fbd088 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -236,12 +236,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, std::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), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } @@ -250,12 +250,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -263,12 +263,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); Matrix H3_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } @@ -276,12 +276,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); Matrix H4_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -290,12 +290,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); Matrix H5_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::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 a9dcb8ceff..40c09416c8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -108,14 +108,14 @@ class InvDepthFactorVariant1: public NoiseModelFactor2 { if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, - boost::placeholders::_1), landmark); + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index d1fc92b90e..b1169580e3 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -111,13 +111,13 @@ class InvDepthFactorVariant2: public NoiseModelFactor2 { if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, - boost::placeholders::_1, landmark), pose); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, - boost::placeholders::_1), landmark); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 4595a934b1..98f2db2f37 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -111,14 +111,14 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { if(H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if(H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, - boost::placeholders::_1), + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } @@ -238,20 +238,20 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3 { if(H1) (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, - boost::placeholders::_1, pose2, landmark), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, + std::placeholders::_1, pose2, landmark), pose1); if(H2) (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + std::placeholders::_1, landmark), pose2); if(H3) (*H3) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, - pose2, boost::placeholders::_1), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + pose2, std::placeholders::_1), landmark); return inverseDepthError(pose1, pose2, landmark); From d5890a2d61a7142e745c0227e1abbe67dcc83aab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Jul 2021 21:03:15 -0400 Subject: [PATCH 25/41] update all the tests --- .../tests/testAlgebraicDecisionTree.cpp | 2 +- gtsam/discrete/tests/testDecisionTree.cpp | 2 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 7 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 25 +++--- gtsam/geometry/tests/testOrientedPlane3.cpp | 12 +-- gtsam/geometry/tests/testPinholeCamera.cpp | 12 +-- gtsam/geometry/tests/testPinholePose.cpp | 4 +- gtsam/geometry/tests/testPoint3.cpp | 22 ++--- gtsam/geometry/tests/testPose3.cpp | 11 +-- gtsam/geometry/tests/testSO3.cpp | 27 +++--- gtsam/geometry/tests/testSO4.cpp | 6 +- gtsam/geometry/tests/testSOn.cpp | 4 +- gtsam/geometry/tests/testSimilarity3.cpp | 29 +++---- gtsam/geometry/tests/testUnit3.cpp | 42 +++++---- gtsam/linear/tests/testGaussianBayesNet.cpp | 6 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 7 +- gtsam/navigation/tests/testAHRSFactor.cpp | 47 +++++----- gtsam/navigation/tests/testAttitudeFactor.cpp | 9 +- gtsam/navigation/tests/testGPSFactor.cpp | 6 +- gtsam/navigation/tests/testImuBias.cpp | 13 +-- gtsam/navigation/tests/testImuFactor.cpp | 34 ++++---- gtsam/navigation/tests/testMagFactor.cpp | 18 ++-- gtsam/navigation/tests/testMagPoseFactor.cpp | 22 +++-- .../tests/testManifoldPreintegration.cpp | 19 ++-- gtsam/navigation/tests/testNavState.cpp | 48 +++++----- gtsam/navigation/tests/testScenario.cpp | 5 +- .../tests/testTangentPreintegration.cpp | 17 ++-- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testValues.cpp | 6 +- gtsam/sam/tests/testRangeFactor.cpp | 18 ++-- gtsam/sfm/tests/testTranslationFactor.cpp | 7 +- gtsam/slam/tests/testBetweenFactor.cpp | 15 ++-- .../tests/testEssentialMatrixConstraint.cpp | 17 ++-- .../slam/tests/testEssentialMatrixFactor.cpp | 14 ++- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 14 +-- gtsam/slam/tests/testReferenceFrameFactor.cpp | 16 ++-- gtsam/slam/tests/testRotateFactor.cpp | 11 ++- .../tests/testSmartProjectionPoseFactor.cpp | 7 +- gtsam/slam/tests/testTriangulationFactor.cpp | 10 ++- .../dynamics/tests/testSimpleHelicopter.cpp | 42 +++++---- gtsam_unstable/geometry/tests/testEvent.cpp | 8 +- .../slam/tests/testBetweenFactorEM.cpp | 6 +- .../slam/tests/testBiasedGPSFactor.cpp | 17 ++-- .../testEquivInertialNavFactor_GlobalVel.cpp | 2 +- .../tests/testGaussMarkov1stOrderFactor.cpp | 12 +-- .../testInertialNavFactor_GlobalVelocity.cpp | 45 +++++----- .../tests/testLocalOrientedPlane3Factor.cpp | 10 +-- .../slam/tests/testPartialPriorFactor.cpp | 26 +++--- .../slam/tests/testPoseBetweenFactor.cpp | 23 +++-- .../slam/tests/testPosePriorFactor.cpp | 13 ++- .../slam/tests/testPoseToPointFactor.h | 2 +- .../slam/tests/testProjectionFactorPPP.cpp | 24 ++--- .../slam/tests/testProjectionFactorPPPC.cpp | 22 ++--- .../tests/testRelativeElevationFactor.cpp | 87 ++++++++++++------- gtsam_unstable/slam/tests/testTSAMFactors.cpp | 27 +++--- .../testTransformBtwRobotsUnaryFactor.cpp | 12 ++- .../testTransformBtwRobotsUnaryFactorEM.cpp | 16 ++-- tests/testExpressionFactor.cpp | 24 +++-- tests/testSimulated3D.cpp | 24 +++-- 59 files changed, 555 insertions(+), 480 deletions(-) diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 0261ef833f..be720dbca4 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) { } /** I can't get this to work ! - class Mul: boost::function { + class Mul: std::function { inline double operator()(const double& a, const double& b) { return a * b; } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 71cb4abe39..96f503abc9 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -196,7 +196,7 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - boost::function op = convert; + std::function op = convert; BDT f2(f1, ordering, op); // f1.print("f1"); // f2.print("f2"); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 1db1926bcd..0fb0754feb 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -20,12 +20,11 @@ #include #include -#include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) { EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH))); // Check derivative - boost::function f = // - boost::bind(CalibratedCamera::Create, _1, boost::none); + std::function f = // + std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f, kDefaultPose); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 7362cf7bf8..ff8c61f355 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -5,16 +5,15 @@ * @date December 17, 2013 */ -#include -#include -#include +#include #include +#include +#include +#include -#include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) { 1e-8)); Matrix expectedH1 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none, - boost::none), + std::bind(EssentialMatrix::FromRotationAndDirection, + std::placeholders::_1, trueDirection, boost::none, boost::none), trueRotation); EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); Matrix expectedH2 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none, - boost::none), - trueDirection); + std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, + std::placeholders::_1, boost::none, boost::none), + trueDirection); EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); } @@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) { Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) { Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 5c7c6142e1..533041a2cf 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -21,10 +21,9 @@ #include #include #include -#include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using boost::none; @@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) { Vector2(actual[0], actual[1]))); EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); - boost::function f = - boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); @@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) { TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; - boost::function f = - boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); + std::function f = std::bind( + &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); { Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 92deda6a53..0679a46097 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -22,13 +22,12 @@ #include #include -#include #include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) { EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); // Check derivative - boost::function f = // - boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + std::function f = // + std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f,pose,K); EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); Matrix numericalH2 = numericalDerivative22(f,pose,K); @@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 1cf2f4a3fd..acfcd9f396 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 79e44c0b35..315391ac89 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -14,14 +14,14 @@ * @brief Unit tests for Point3 class */ -#include +#include #include #include +#include -#include -#include +#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) @@ -101,7 +101,7 @@ TEST( Point3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = + std::function f = [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); }; { gtsam::dot(p, q, H1, H2); @@ -123,8 +123,9 @@ TEST( Point3, dot) { /* ************************************************************************* */ TEST(Point3, cross) { Matrix aH1, aH2; - boost::function f = - boost::bind(>sam::cross, _1, _2, boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); @@ -142,8 +143,9 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(>sam::cross, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); @@ -163,7 +165,7 @@ TEST (Point3, normalize) { Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(gtsam::normalize, _1, boost::none), point); + std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9ed76d4a67..11b8791d46 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -22,8 +22,7 @@ #include // for operator += using namespace boost::assign; -#include -using namespace boost::placeholders; +using namespace std::placeholders; #include #include @@ -215,7 +214,7 @@ TEST(Pose3, translation) { EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::translation, _1, boost::none), T); + std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -226,7 +225,7 @@ TEST(Pose3, rotation) { EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::rotation, _1, boost::none), T); + std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -1052,7 +1051,9 @@ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); EXPECT(assert_equal(T, actual)); - boost::function create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none); + std::function create = + std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 58ad967d21..910d482b06 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,15 +15,12 @@ * @author Frank Dellaert **/ -#include - +#include #include #include +#include -#include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) { TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) { TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), thetahat); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); Matrix3 Jactual; SO3::Expmap(thetahat, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } @@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); Matrix3 Jactual; SO3::Logmap(R, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) { TEST(SO3, ApplyDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); }; @@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) { TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); }; @@ -357,7 +354,7 @@ TEST(SO3, vec) { Matrix actualH; const Vector9 actual = R2.vec(actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { return Q.vec(); }; + std::function f = [](const SO3& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); CHECK(assert_equal(numericalH, actualH)); } @@ -371,7 +368,7 @@ TEST(Matrix, compose) { Matrix actualH; const Matrix3 actual = so3::compose(M, R, actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [R](const Matrix3& M) { + std::function f = [R](const Matrix3& M) { return so3::compose(M, R); }; Matrix numericalH = numericalDerivative11(f, M, 1e-2); diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index f771eea5f3..5486755f77 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -166,7 +166,7 @@ TEST(SO4, vec) { Matrix actualH; const Vector16 actual = Q2.vec(actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q) { + std::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); @@ -179,7 +179,7 @@ TEST(SO4, topLeft) { Matrix actualH; const Matrix3 actual = topLeft(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return topLeft(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); @@ -192,7 +192,7 @@ TEST(SO4, stiefel) { Matrix actualH; const Matrix43 actual = stiefel(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return stiefel(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 4d0ed98b36..d9d4da34c1 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); } /// Test Jacobian of Retract at origin TEST(SOn, RetractJacobian) { Matrix actualH = RetractJacobian(3); - boost::function h = [](const Vector &v) { + std::function h = [](const Vector &v) { return SOn::ChartAtOrigin::Retract(v).matrix(); }; Vector3 v; @@ -205,7 +205,7 @@ TEST(SOn, vec) { SOn Q = SOn::ChartAtOrigin::Retract(v); Matrix actualH; const Vector actual = Q.vec(actualH); - boost::function h = [](const SOn &Q) { return Q.vec(); }; + std::function h = [](const SOn &Q) { return Q.vec(); }; const Matrix H = numericalDerivative11(h, Q, 1e-5); CHECK(assert_equal(H, actualH)); } diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 10a9b2ac49..428422072f 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -16,24 +16,22 @@ * @author Zhaoyang Lv */ +#include +#include +#include +#include +#include #include -#include -#include +#include #include -#include #include -#include -#include -#include -#include -#include - -#include +#include +#include +#include -#include -#include +#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using symbol_shorthand::X; @@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) { EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa))); // Test derivative - boost::function f = boost::bind( - &Similarity3::transformFrom, _1, _2, boost::none, boost::none); + // Use lambda to resolve overloaded method + std::function + f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); }; Point3 q(1, 2, 3); for (const auto& T : { T1, T2, T3, T4, T5, T6 }) { diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4d609380c8..b548b93153 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -32,13 +32,12 @@ #include #include -#include #include #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using gtsam::symbol_shorthand::U; @@ -127,8 +126,9 @@ TEST(Unit3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Unit3::dot, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { p.dot(q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); @@ -158,13 +158,13 @@ TEST(Unit3, error) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), q); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), r); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -185,25 +185,33 @@ TEST(Unit3, error2) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -221,13 +229,13 @@ TEST(Unit3, distance) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), q); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), r); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -319,7 +327,7 @@ TEST(Unit3, basis) { Matrix62 actualH; Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); // without H, first time EXPECT(assert_equal(expected, p.basis(), 1e-6)); @@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) { p.basis(actualH); Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -376,8 +384,8 @@ TEST(Unit3, retract) { TEST (Unit3, jacobian_retract) { Matrix22 H; Unit3 p; - boost::function f = - boost::bind(&Unit3::retract, p, _1, boost::none); + std::function f = + std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); { Vector2 v (-0.2, 0.1); p.retract(v, H); @@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) { Unit3 expected(point); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( - boost::bind(Unit3::FromPoint3, _1, boost::none), point); + std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index c88bf87314..00a338e547 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -32,7 +32,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -270,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index a6a60c19ce..c5601af271 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -21,7 +21,6 @@ #include #include // for operator += #include // for operator += -#include #include #include @@ -30,7 +29,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -260,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 828e264f46..a4d06d01a0 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -25,10 +25,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -175,17 +174,17 @@ TEST(AHRSFactor, Error) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -234,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -269,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); + std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega); const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); @@ -294,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); + std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -368,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Compute numerical derivatives Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -410,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -459,8 +458,8 @@ TEST (AHRSFactor, predictTest) { // AHRSFactor::PreintegratedMeasurements::predict Matrix expectedH = numericalDerivative11( - boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, - &pim, _1, boost::none), bias); + std::bind(&AHRSFactor::PreintegratedMeasurements::predict, + &pim, std::placeholders::_1, boost::none), bias); // Actual Jacobians Matrix H; diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 2ab60fe6ae..d49907cbfd 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -25,7 +25,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -50,8 +50,9 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), + Matrix expectedH = numericalDerivative11( + std::bind(&Rot3AttitudeFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), nRb); // Use the factor to calculate the derivative @@ -117,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, + std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b486272abf..b784c0c94c 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -27,7 +27,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index e7da2c81c6..b486a4a98b 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -19,9 +19,8 @@ #include #include -#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -128,8 +127,9 @@ TEST(ImuBias, operatorSubB) { TEST(ImuBias, Correct1) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = boost::bind( - &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctAccelerometer, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctAccelerometer(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); @@ -139,8 +139,9 @@ TEST(ImuBias, Correct1) { TEST(ImuBias, Correct2) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = - boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctGyroscope, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctGyroscope(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f19862772c..801d87895d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -146,9 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix9 aH1, aH2; Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&PreintegrationBase::computeError, actual, - boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, + std::function f = + std::bind(&PreintegrationBase::computeError, actual, + std::placeholders::_1, std::placeholders::_2, std::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)); @@ -204,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, - boost::placeholders::_1, boost::none), kZeroBias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::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, boost::placeholders::_1, + std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, - boost::placeholders::_1, boost::none, boost::none), kZeroBias); + std::bind(&PreintegrationBase::predict, pim, state1, + std::placeholders::_1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -278,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, boost::placeholders::_1, v1, x2, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias), x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias), x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); @@ -333,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, - boost::placeholders::_1, boost::none), bias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::placeholders::_1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -522,7 +522,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); Matrix3 expectedD = numericalDerivative11( - boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1), + std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); @@ -534,15 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, -// boost::placeholders::_1, boost::placeholders::_2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::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, -// boost::placeholders::_1, boost::placeholders::_2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::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 ad193b5036..5107b3b6b3 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -26,7 +26,7 @@ #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index 1a3c5b2a97..204c1d38f9 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -17,9 +17,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; // ***************************************************************************** @@ -78,8 +76,11 @@ TEST(MagPoseFactor, JacobianPose2) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P2_b), + H2, 1e-7)); } // ***************************************************************************** @@ -89,8 +90,11 @@ TEST(MagPoseFactor, JacobianPose3) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P3_b), + H3, 1e-7)); } // ***************************************************************************** @@ -104,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7)); } // ***************************************************************************** @@ -118,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7)); } // ***************************************************************************** diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 625689ed79..7796ccbda5 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -22,11 +22,10 @@ #include #include -#include #include "imuFactorTesting.h" -using namespace boost::placeholders; +using namespace std::placeholders; namespace testing { // Create default parameters with Z-down and above noise parameters @@ -43,21 +42,21 @@ static boost::shared_ptr Params() { TEST(ManifoldPreintegration, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function deltaRij = + std::function deltaRij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaRij(); }; - boost::function deltaPij = + std::function deltaPij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaPij(); }; - boost::function deltaVij = + std::function deltaVij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -98,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&ManifoldPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::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)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 86c708f5e8..e7adb923d7 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -23,7 +23,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -39,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { - boost::function create = - boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, - boost::none); + std::function create = + std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix aH1, aH2, aH3; EXPECT( assert_equal(kState1, @@ -59,9 +59,9 @@ TEST(NavState, Constructor) { /* ************************************************************************* */ TEST(NavState, Constructor2) { - boost::function construct = - boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, - boost::none); + std::function construct = + std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix aH1, aH2; EXPECT( assert_equal(kState1, @@ -76,7 +76,7 @@ TEST( NavState, Attitude) { Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( - boost::bind(&NavState::attitude, _1, boost::none), kState1); + std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -86,7 +86,8 @@ TEST( NavState, Position) { Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( - boost::bind(&NavState::position, _1, boost::none), kState1); + std::bind(&NavState::position, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -96,7 +97,8 @@ TEST( NavState, Velocity) { Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( - boost::bind(&NavState::velocity, _1, boost::none), kState1); + std::bind(&NavState::velocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -106,7 +108,8 @@ TEST( NavState, BodyVelocity) { Velocity3 actual = kState1.bodyVelocity(aH); EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( - boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -137,8 +140,9 @@ TEST( NavState, Manifold ) { // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); - boost::function retract = - boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); + std::function retract = + std::bind(&NavState::retract, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); @@ -149,9 +153,9 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); // Check localCoordinates derivatives - boost::function local = - boost::bind(&NavState::localCoordinates, _1, _2, boost::none, - boost::none); + std::function local = + std::bind(&NavState::localCoordinates, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); @@ -168,8 +172,9 @@ TEST( NavState, Manifold ) { /* ************************************************************************* */ static const double dt = 2.0; -boost::function coriolis = boost::bind( - &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); +std::function coriolis = + std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, + std::placeholders::_2, boost::none); TEST(NavState, Coriolis) { Matrix9 aH; @@ -244,9 +249,10 @@ TEST(NavState, CorrectPIM) { xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 aH1, aH2; - boost::function correctPIM = - boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, - false, boost::none, boost::none); + std::function correctPIM = + std::bind(&NavState::correctPIM, std::placeholders::_1, + std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, + boost::none, boost::none); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index d0bae36900..0df51956bd 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -19,10 +19,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -148,7 +147,7 @@ TEST(Scenario, Accelerating) { { // Check acceleration in nav Matrix expected = numericalDerivative11( - boost::bind(&Scenario::velocity_n, scenario, _1), T); + std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T); EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); } diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 9bb988b429..ada0590945 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -22,11 +22,10 @@ #include #include -#include #include "imuFactorTesting.h" -using namespace boost::placeholders; +using namespace std::placeholders; static const double kDt = 0.1; @@ -78,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) { TEST(ImuFactor, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function preintegrated = + std::function preintegrated = [=](const Vector3& a, const Vector3& w) { TangentPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -105,10 +104,12 @@ TEST(TangentPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&TangentPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::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)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -121,7 +122,7 @@ TEST(TangentPreintegration, Compose) { TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - boost::function f = + std::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 8fcf84a113..7bb8021866 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -495,7 +495,7 @@ TEST(Expression, Subtract) { /* ************************************************************************* */ TEST(Expression, LinearExpression) { const Key key(67); - const boost::function f = [](const Point3& p) { return (Vector3)p; }; + const std::function f = [](const Point3& p) { return (Vector3)p; }; const Matrix3 kIdentity = I_3x3; const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index e9076a7d7a..b894f48169 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -34,7 +34,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); @@ -336,7 +336,7 @@ TEST(Values, filter) { // Filter by key int i = 0; - Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); for(const auto key_value: filtered) { if(i == 0) { @@ -364,7 +364,7 @@ TEST(Values, filter) { EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); // ConstFilter by Key - Values::ConstFiltered constfiltered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::ConstFiltered constfiltered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)constfiltered.size()); Values fromconstfiltered(constfiltered); EXPECT(assert_equal(expectedSubValues1, fromconstfiltered)); diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 2af5825db8..5f5d4f4dd0 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -28,7 +28,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -265,9 +265,9 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError2D, _1, point, factor), pose); + std::bind(&factorError2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError2D, pose, _1, factor), point); + std::bind(&factorError2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -296,9 +296,9 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -323,9 +323,9 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError3D, _1, point, factor), pose); + std::bind(&factorError3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError3D, pose, _1, factor), point); + std::bind(&factorError3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -355,9 +355,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 05ae76faa3..818f2bce51 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -20,10 +20,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -91,9 +90,9 @@ TEST(TranslationFactor, Jacobian) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError, _1, T2, factor), T1); + std::bind(&factorError, std::placeholders::_1, T2, factor), T1); H2Expected = numericalDerivative11( - boost::bind(&factorError, T1, _1, factor), T2); + std::bind(&factorError, T1, std::placeholders::_1, factor), T2); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index a1f8774e5a..6897943ec1 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -14,7 +14,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -35,15 +35,16 @@ TEST(BetweenFactor, Rot3) { Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail - Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21( + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + R1, R2, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none, boost::none)), R1, R2, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index fb2172107c..080239b350 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -23,10 +23,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -53,12 +52,14 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, - boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, - boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 79a86865d1..03775a70f0 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -17,9 +17,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -104,8 +102,8 @@ TEST(EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + std::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -130,9 +128,9 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::function, OptionalJacobian<5, 2>)> g; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 022dc859ea..35c7504081 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -27,10 +27,9 @@ #include #include -#include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -145,8 +144,9 @@ TEST( OrientedPlane3Factor, Derivatives ) { OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); // Calculate numerical derivatives - boost::function f = boost::bind( - &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + std::function f = std::bind( + &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); @@ -184,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // Calculate numerical derivatives Matrix expectedH1 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T1); Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T2); Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T3); // Use the factor to calculate the derivative diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 075710ae73..b5800a414b 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -16,8 +16,6 @@ #include -#include - #include #include @@ -32,7 +30,7 @@ using namespace std; using namespace boost; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; @@ -70,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); @@ -102,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index e4ecafd425..b8fd01730e 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -13,12 +13,11 @@ #include #include -#include #include using namespace std; using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; static const double kDegree = M_PI / 180; @@ -73,13 +72,13 @@ TEST (RotateFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } @@ -144,14 +143,14 @@ TEST (RotateDirectionsFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 26caa6b75d..c7f220c102 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -27,11 +27,10 @@ #include #include #include -#include #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; static const double rankTol = 1.0; // Create a noise model for the pixel error @@ -132,8 +131,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) - boost::function f = // - boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP Matrix actualE; diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 9fe087c2ff..ad88c88fcc 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -32,7 +32,7 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; // Some common constants static const boost::shared_ptr sharedCal = // @@ -62,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); @@ -86,13 +86,15 @@ TEST( triangulation, TriangulationFactorStereo ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); // compare same problem against expression factor - Expression::UnaryFunction::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2); + Expression::UnaryFunction::type f = + std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, + boost::none, std::placeholders::_2); Expression point_(pointKey); Expression project2_(f, point_); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index ede11c7fb5..882d5423ad 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -3,14 +3,13 @@ * @author Duy-Nguyen Ta */ -#include #include #include #include #include /* ************************************************************************* */ -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -58,19 +57,28 @@ TEST( Reconstruction, evaluateError) { assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); @@ -111,22 +119,22 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 3a599e6c5f..f1bbc39704 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -23,9 +23,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -67,11 +65,11 @@ TEST(Event, Derivatives) { Matrix13 actualH2; kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none), + std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - boost::bind(kToa, exampleEvent, _1, boost::none, boost::none), + std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 70368cc0e1..4d6e1912a1 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -190,12 +190,12 @@ TEST (BetweenFactorEM, jacobian ) { // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); - Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); // try to check numerical derivatives of a standard between factor - Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); + Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index 0eb8b274b3..59c4fdf536 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -10,10 +10,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -68,15 +67,17 @@ TEST(BiasedGPSFactor, jacobian) { factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 95b9b2f88f..644283512f 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -26,7 +26,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 74134612d8..8692cf5841 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -23,9 +23,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -108,9 +106,13 @@ TEST (GaussMarkovFactor, jacobian ) { // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function Matrix numerical_H1, numerical_H2; numerical_H1 = numericalDerivative21( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); numerical_H2 = numericalDerivative22( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); // Verify they are equal for this choice of state CHECK( assert_equal(numerical_H1, computed_H1, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index aae59035be..e68b2fe5f4 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include #include @@ -26,7 +25,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -251,7 +250,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, // // Vector3 v(predictionRq(angles, q)); // -// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(std::bind(&predictionRq, std::placeholders::_1, q), angles); // // cout<<"J_hyp"<( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -346,19 +345,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -644,19 +643,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; H1_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -678,19 +677,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 6bbfb55ae1..b97d56c7ea 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -24,9 +24,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -143,8 +141,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); // Calculate numerical derivatives - auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, - _1, _2, _3, boost::none, boost::none, boost::none); + auto f = + std::bind(&LocalOrientedPlane3Factor::evaluateError, factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); Matrix numericalH2 = numericalDerivative32 #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -83,7 +82,9 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -99,13 +100,16 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) { Pose2 measurement(-6.0, 3.5, 0.123); // Prior on x component of translation. - TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); + TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(), + NM::Isotropic::Sigma(2, 0.25)); Pose2 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -127,7 +131,7 @@ TEST(PartialPriorFactor, JacobianTheta) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -178,7 +182,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -200,7 +204,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -224,7 +228,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -248,7 +252,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -271,7 +275,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 6f7725eed1..cd5bf1d9e9 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -22,10 +22,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -199,8 +198,14 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -228,8 +233,14 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Point3(-3.5257579, 6.02637531, -8.98382384)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index cc26155172..dbbc02a8ba 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -22,10 +22,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -188,7 +187,10 @@ TEST( PosePriorFactor, Jacobian ) { Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -212,7 +214,10 @@ TEST( PosePriorFactor, JacobianWithTransform ) { Point3(-4.74767676, 7.67044942, -11.00985)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 8f8563e9db..e0e5c45817 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -63,7 +63,7 @@ TEST(PoseToPointFactor, jacobian) { PoseToPointFactor factor(pose_key, point_key, l_meas, noise); // Calculate numerical derivatives - auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, boost::none, boost::none); Matrix numerical_H1 = numericalDerivative21(f, p, l); Matrix numerical_H2 = numericalDerivative22(f, p, l); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 8a65e5e571..c05f83a23b 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -28,9 +28,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -178,10 +176,13 @@ TEST( ProjectionFactorPPP, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, Pose3(), point); + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, Pose3(), point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); } @@ -214,9 +215,12 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) { // Verify H2 with numerical derivative Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, body_P_sensor, point); + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, body_P_sensor, point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 232f8de17e..6490dfc753 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -28,9 +28,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -138,12 +136,16 @@ TEST( ProjectionFactorPPPC, Jacobian ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, - *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, + std::placeholders::_1, point, *K1, boost::none, boost::none, + boost::none, boost::none), + Pose3()); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), + point, std::placeholders::_1, boost::none, boost::none, + boost::none, boost::none), + *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); @@ -174,12 +176,12 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index 2fda9debb8..25ca3339b7 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -5,14 +5,13 @@ * @author Alex Cunningham */ -#include #include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; SharedNoiseModel model1 = noiseModel::Unit::Create(1); @@ -37,10 +36,14 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -52,10 +55,14 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -67,10 +74,14 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -82,10 +93,14 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -97,10 +112,14 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -112,10 +131,14 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -127,10 +150,14 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose3, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 77f82bca49..fbb21e191c 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -19,10 +19,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -48,10 +47,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none, boost::none), pose); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none, boost::none), point); // Verify the Jacobians are correct @@ -82,17 +81,17 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); H3Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1, point, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, - _1, boost::none, boost::none, boost::none, boost::none), point); + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -123,17 +122,17 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); H3Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1, pose2, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, - base2, _1, boost::none, boost::none, boost::none, boost::none), + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), pose2); // Verify the Jacobians are correct diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d9e945b78c..36914f88f2 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -18,9 +18,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -233,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } @@ -287,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 2fd282091a..657a9fb348 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -18,9 +18,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -262,8 +260,10 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); -// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); + Matrix H1_expected = gtsam::numericalDerivative11( + std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, + stepsize); + // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } /////* ************************************************************************** */ //TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) { @@ -312,12 +312,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 6bb5751bf6..e9eac22ebb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -17,22 +17,19 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include -#include -#include -#include +#include +#include #include #include +#include #include -#include - -#include +#include +#include +#include #include using boost::assign::list_of; -#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -621,9 +618,10 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { Matrix3 A; const Vector Ab = f(a, b, H1, A); CHECK(assert_equal(A * b, Ab)); - CHECK(assert_equal(numericalDerivative11( - boost::bind(f, _1, b, boost::none, boost::none), a), - H1)); + CHECK(assert_equal( + numericalDerivative11( + std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a), + H1)); Values values; values.insert(0, a); diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 342c353bca..2bc381f7a4 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -26,7 +26,7 @@ #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; // Convenience for named keys @@ -46,7 +46,7 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(boost::bind(simulated3D::prior, _1, boost::none),x); + Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); Matrix computed; simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); @@ -55,13 +55,19 @@ TEST( simulated3D, Dprior ) /* ************************************************************************* */ TEST( simulated3D, DOdo ) { - Point3 x1(1,-9,7),x2(-5,6,7); - Matrix H1,H2; - simulated3D::odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A2,H2,1e-9)); + Point3 x1(1, -9, 7), x2(-5, 6, 7); + Matrix H1, H2; + simulated3D::odo(x1, x2, H1, H2); + Matrix A1 = numericalDerivative21( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A1, H1, 1e-9)); + Matrix A2 = numericalDerivative22( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A2, H2, 1e-9)); } From ddfb45efb03544636f69268f85a703816a730c57 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 01:02:36 -0600 Subject: [PATCH 26/41] fix typo in block indexing, 3x3 covariance for Pose2 should have just 1x1 block for theta --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 88c393f16d..76fd1bfc70 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } From 941594c94be1d1223d7fbcf9d1181f2c73411574 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:11:40 +0200 Subject: [PATCH 27/41] Testing CameraSet and triangulatePoint3 Currently triangulatePoint3 returns wrong results for fisheye models. The template for PinholePose may be implemented for a fixed size of variable dimensions. --- python/gtsam/tests/test_Cal3Fisheye.py | 40 +++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 23f7a9b8c3..d731204ef8 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,7 @@ def setUpClass(cls): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) @@ -93,6 +93,44 @@ def test_sfm_factor2(self): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation_skipped(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) From c595767caeba831701298ab410a693048b2fe1db Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:14:08 +0200 Subject: [PATCH 28/41] Unittest, triangulation for Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 38 ++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index f2c1ada488..0b09fc3ae7 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -105,6 +105,44 @@ def test_sfm_factor2(self): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From 61c5e89de3bf628dccd98e95c0f955bf908ca8a9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:46:54 -0400 Subject: [PATCH 29/41] try increasing pmax to pass test --- python/gtsam/tests/test_ShonanAveraging.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 69e7055453..d07d840991 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,8 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=10) + min + result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): wRi = result_values.atRot2(i) From 690300124c5f7a162601298fca115c8f01cc531a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:47:14 -0400 Subject: [PATCH 30/41] fix typo --- python/gtsam/tests/test_ShonanAveraging.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index d07d840991..c109ce6b0a 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,6 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - min result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): From cce952fbb3fc266e89bd2599ec58de7fb92622be Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 19:35:34 -0400 Subject: [PATCH 31/41] use simple example for unit test --- python/gtsam/tests/test_ShonanAveraging.py | 84 +++++++++++----------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index c109ce6b0a..08bbe5191d 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -22,12 +22,13 @@ ShonanAveraging2, ShonanAveragingParameters2, ShonanAveraging3, - ShonanAveragingParameters3 + ShonanAveragingParameters3, ) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( - gtsam.LevenbergMarquardtParams.CeresDefaults()) + gtsam.LevenbergMarquardtParams.CeresDefaults() +) def fromExampleName( @@ -146,58 +147,59 @@ def test_PriorWeights(self): result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + def test_constructorBetweenFactorPose2s(self) -> None: - """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" - num_images = 11 - # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + """Check if ShonanAveraging2 constructor works when not initialized from g2o file. + + GT pose graph: + + | cam 1 = (0,4) + --o + | . + . . + . . + | | + o-- ... o-- + cam 0 cam 2 = (4,0) + (0,0) + """ + num_images = 3 + + wTi_list = [ + Pose2(Rot2.fromDegrees(0), np.array([0, 0])), + Pose2(Rot2.fromDegrees(90), np.array([0, 4])), + Pose2(Rot2.fromDegrees(0), np.array([4, 0])), + ] + + edges = [(0, 1), (1, 2), (0, 2)] i2Ri1_dict = { - (1, 2): -43.86342, - (1, 5): -135.06351, - (2, 4): 72.64527, - (3, 5): 117.75967, - (6, 7): -31.73934, - (7, 10): 177.45901, - (6, 9): -133.58713, - (7, 8): -90.58668, - (0, 3): 127.02978, - (8, 10): -92.16361, - (4, 8): 51.63781, - (4, 6): 173.96384, - (4, 10): 139.59445, - (2, 3): 151.04022, - (3, 4): -78.39495, - (1, 4): 28.78185, - (6, 8): -122.32602, - (0, 2): -24.01045, - (5, 7): -53.93014, - (4, 5): -163.84535, - (2, 5): -91.20009, - (1, 3): 107.17680, - (7, 9): -102.35615, - (0, 1): 19.85297, - (5, 8): -144.51682, - (5, 6): -22.19079, - (5, 10): -56.56016, + (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation() + for (i1, i2) in edges } - i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()} + lm_params = LevenbergMarquardtParams.CeresDefaults() shonan_params = ShonanAveragingParameters2(lm_params) shonan_params.setUseHuber(False) shonan_params.setCertifyOptimality(True) - + noise_model = gtsam.noiseModel.Unit.Create(3) between_factors = gtsam.BetweenFactorPose2s() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): i2Ti1 = Pose2(i2Ri1, np.zeros(2)) - between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) - + between_factors.append( + BetweenFactorPose2(i2, i1, i2Ti1, noise_model) + ) + obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=40) - - for i in range(num_images): - wRi = result_values.atRot2(i) + result_values, _ = obj.run(initial, min_p=2, max_p=100) + + wRi_list = [result_values.atRot2(i) for i in range(num_images)] + thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() From d7151ed28410e57526cf5286d820fc4ae089cf05 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 20:52:36 -0400 Subject: [PATCH 32/41] use mod when comparing angles --- python/gtsam/tests/test_ShonanAveraging.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 08bbe5191d..8c70df5dff 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -197,7 +197,9 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) thetas_deg -= max(thetas_deg) - expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + # map all angles to [0,360) + thetas_deg = thetas_deg % 360 + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg) From c4a4e13196be2fdf738a173ca4e70b2a4eb43590 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 00:16:24 -0600 Subject: [PATCH 33/41] fix assert on angles --- python/gtsam/tests/test_ShonanAveraging.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 8c70df5dff..29a14b1b6c 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -196,11 +196,13 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - thetas_deg -= max(thetas_deg) + # map all angles to [0,360) thetas_deg = thetas_deg % 360 + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) - np.testing.assert_allclose(thetas_deg, expected_thetas_deg) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) if __name__ == "__main__": From 54d34711214a438c89f173eb9878b43036876d22 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 08:18:45 -0600 Subject: [PATCH 34/41] update logic in angular error comparison --- python/gtsam/tests/test_ShonanAveraging.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 29a14b1b6c..19b9f8dc1a 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -199,7 +199,7 @@ def test_constructorBetweenFactorPose2s(self) -> None: # map all angles to [0,360) thetas_deg = thetas_deg % 360 - thetas_deg -= max(thetas_deg) + thetas_deg -= thetas_deg[0] expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) From 3402e46ad19cc6e2bef1c71cd777b167e09fe136 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:00:21 +0200 Subject: [PATCH 35/41] Shared data for triangulation unit tests --- python/gtsam/tests/test_Cal3Fisheye.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index d731204ef8..756113b930 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -33,6 +33,19 @@ def setUpClass(cls): u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) + + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() @@ -96,40 +109,17 @@ def test_sfm_factor2(self): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 17c37de7c4325bebdbfad1531ecd270f4ca80eb2 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:07:19 +0200 Subject: [PATCH 36/41] Shared setup triangulation unit test --- python/gtsam/tests/test_Cal3Unified.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 0b09fc3ae7..ca0959e44c 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -40,6 +40,19 @@ def setUpClass(cls): xi = 1 cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) @@ -108,40 +121,17 @@ def test_sfm_factor2(self): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 3e41ece75a0ba8fc923ea6f59d912fd52f3914d3 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:10:38 +0200 Subject: [PATCH 37/41] Minor fix test_Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ca0959e44c..a402ae509f 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -122,7 +122,6 @@ def test_sfm_factor2(self): def test_triangulation(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -130,7 +129,6 @@ def test_triangulation_rectify(self): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From d130387a7db0b52080905c15ddcebfa22484d0de Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:12:14 +0200 Subject: [PATCH 38/41] Minor fix test_Cal3Fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 756113b930..646b48881c 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -110,7 +110,6 @@ def test_sfm_factor2(self): def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -118,7 +117,6 @@ def test_triangulation_rectify(self): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From 16cfc7fd381a9250bb573a4613037299be770d9f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:15:10 +0200 Subject: [PATCH 39/41] Remove commented out line --- python/gtsam/tests/test_Cal3Fisheye.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 646b48881c..298c6e57b9 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,6 @@ def setUpClass(cls): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From c2bbe78e867bf1797ef33be444cc400bfe946d71 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:16:00 +0200 Subject: [PATCH 40/41] Remove comment --- python/gtsam/tests/test_Cal3Unified.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index a402ae509f..dab1ae4466 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -31,7 +31,6 @@ def setUpClass(cls): x, y, z = 1.0, 0.0, 1.0 r = np.linalg.norm([x, y, z]) u, v = 2*x/(z+r), 0.0 - #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From a115788ea5d56df6cfdbc127ab24b6ebdbb9d157 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:53:31 +0200 Subject: [PATCH 41/41] Remove spaces in empty line --- gtsam/geometry/SimpleCamera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 2c34bdfa73..119e9d1a3c 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -35,7 +35,7 @@ namespace gtsam { using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; - + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x