12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update all the tests

release/4.3a0
Varun Agrawal 2021-07-10 21:03:15 -04:00
parent dc8b5e58ff
commit d5890a2d61
59 changed files with 557 additions and 482 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
}
/** I can't get this to work !
class Mul: boost::function<double(const double&, const double&)> {
class Mul: std::function<double(const double&, const double&)> {
inline double operator()(const double& a, const double& b) {
return a * b;
}

2
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -196,7 +196,7 @@ TEST(DT, conversion)
map<string, Label> ordering;
ordering[A] = X;
ordering[B] = Y;
boost::function<bool(const int&)> op = convert;
std::function<bool(const int&)> op = convert;
BDT f2(f1, ordering, op);
// f1.print("f1");
// f2.print("f2");

7
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -20,12 +20,11 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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<CalibratedCamera(Pose3)> f = //
boost::bind(CalibratedCamera::Create, _1, boost::none);
std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

27
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -5,16 +5,15 @@
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <sstream>
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<EssentialMatrix, Rot3>(
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<EssentialMatrix, Unit3>(
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<EssentialMatrix, Pose3>(
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<EssentialMatrix, Pose3>(
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));
}

12
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -21,10 +21,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
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<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> 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<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);

12
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -22,13 +22,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
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<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

4
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

22
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,14 +14,14 @@
* @brief Unit tests for Point3 class
*/
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
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<double(const Point3&, const Point3&)> f =
std::function<double(const Point3&, const Point3&)> 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<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::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<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(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<Point3, Point3>(
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));
}

11
gtsam/geometry/tests/testPose3.cpp
View File

@ -22,8 +22,7 @@
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;
using namespace std::placeholders;
#include <CppUnitLite/TestHarness.h>
#include <cmath>
@ -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<Point3, Pose3>(
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<Rot3, Pose3>(
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<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

27
gtsam/geometry/tests/testSO3.cpp
View File

@ -15,15 +15,12 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<SO3, Vector3>(
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<SO3, Vector3>(
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<SO3, Vector3>(
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<Vector, SO3>(
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<Vector, SO3>(
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<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
std::function<Vector9(const SO3&)> 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<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);

6
gtsam/geometry/tests/testSO4.cpp
View File

@ -166,7 +166,7 @@ TEST(SO4, vec) {
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
std::function<Vector16(const SO4&)> 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<Matrix3(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix3(const SO4&)> 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<Matrix43(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
return stiefel(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);

4
gtsam/geometry/tests/testSOn.cpp
View File

@ -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<Matrix(const Vector &)> h = [](const Vector &v) {
std::function<Matrix(const Vector &)> 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<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}

31
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,24 +16,22 @@
* @author Zhaoyang Lv
*/
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <functional>
#include <boost/bind/bind.hpp>
#include <boost/function.hpp>
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<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transformFrom, _1, _2, boost::none, boost::none);
// Use lambda to resolve overloaded method
std::function<Point3(const Similarity3&, const Point3&)>
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 }) {

42
gtsam/geometry/tests/testUnit3.cpp
View File

@ -32,13 +32,12 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
#include <cmath>
#include <random>
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<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
boost::none, boost::none);
std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@ -158,13 +158,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
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<Vector2,Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Vector2, Unit3, Unit3>(
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<Unit3>(
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<Unit3>(
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<Vector6, Unit3>(
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<Vector6, Unit3>(
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<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
std::function<Unit3(const Vector2&)> 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<Unit3, Point3>(
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));
}

6
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -32,7 +32,7 @@
#include <sstream>
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<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
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();

7
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -21,7 +21,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/bind/bind.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/numericalDerivative.h>
@ -30,7 +29,7 @@
#include <gtsam/linear/GaussianConditional.h>
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<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
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();

47
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -25,10 +25,9 @@
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include <list>
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<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
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<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
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<Rot3, Vector3>(
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<Vector3, Vector3>(
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<Rot3, Vector3>(
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<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
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<Vector3, Vector3>(
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;

9
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -25,7 +25,7 @@
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
Matrix expectedH = numericalDerivative11<Vector, Rot3>(
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<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
boost::none), T);
// Use the factor to calculate the derivative

6
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -27,7 +27,7 @@
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
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<Vector,Pose3>(
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<Vector,NavState>(
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;

13
gtsam/navigation/tests/testImuBias.cpp
View File

@ -19,9 +19,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> 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<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> 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));

34
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -146,9 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Matrix9 aH1, aH2;
Matrix96 aH3;
actual.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
boost::bind(&PreintegrationBase::computeError, actual,
boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3,
std::function<Vector9(const NavState&, const NavState&, const Bias&)> 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<Vector9, Bias>(
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<NavState, NavState>(
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<NavState, Bias>(
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<Rot3, Pose3>(
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<Rot3, Pose3>(
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<Vector9, Bias>(
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<Vector3, Vector3>(
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<NavState, Vector3, Vector3>(
// 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<NavState, Vector3, Vector3>(
// 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));

18
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -26,7 +26,7 @@
#include <GeographicLib/LocalCartesian.hpp>
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<Point3,Rot2> //
(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<Vector,Rot2> //
(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<Vector,Rot3> //
(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<Vector,Point3> //
(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<Vector,Point3> //
(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<Vector,double> //
(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<Vector,Unit3> //
(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<Vector,Point3> //
(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));
}

22
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -17,9 +17,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/MagPoseFactor.h>
#include <boost/bind/bind.hpp>
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<Pose2> 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<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::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<Pose3> 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<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P3_b),
H3, 1e-7));
}
// *****************************************************************************
@ -104,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(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<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7));
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
@ -118,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(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<Vector, Pose3> //
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************

19
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -22,11 +22,10 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#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<PreintegrationParams> Params() {
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
std::function<Vector3(const Vector3&, const Vector3&)> 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<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
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));

48
gtsam/navigation/tests/testNavState.cpp
View File

@ -23,7 +23,7 @@
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> 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<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
std::function<NavState(const Pose3&, const Vector3&)> 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<Rot3, NavState>(
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<Point3, NavState>(
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<Velocity3, NavState>(
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<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
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<NavState(const NavState&, const Vector9&)> retract =
boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
std::function<NavState(const NavState&, const Vector9&)> 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<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
std::function<Vector9(const NavState&, const NavState&)> 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<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
std::function<Vector9(const NavState&, const bool&)> 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<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
std::function<Vector9(const NavState&, const Vector9&)> 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));

5
gtsam/navigation/tests/testScenario.cpp
View File

@ -19,10 +19,9 @@
#include <gtsam/navigation/Scenario.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include <cmath>
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<Vector3, double>(
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));
}

17
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -22,11 +22,10 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#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<Vector9(const Vector3&, const Vector3&)> preintegrated =
std::function<Vector9(const Vector3&, const Vector3&)> 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<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
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<Vector9(const Vector9&, const Vector9&)> f =
std::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
};

2
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -495,7 +495,7 @@ TEST(Expression, Subtract) {
/* ************************************************************************* */
TEST(Expression, LinearExpression) {
const Key key(67);
const boost::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const std::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const Matrix3 kIdentity = I_3x3;
const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);

6
gtsam/nonlinear/tests/testValues.cpp
View File

@ -34,7 +34,7 @@
#include <type_traits>
using namespace boost::assign;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
@ -336,7 +336,7 @@ TEST(Values, filter) {
// Filter by key
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::Filtered<Value> filtered = values.filter(std::bind(std::greater_equal<Key>(), 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<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
Values::ConstFiltered<Value> constfiltered = values.filter(std::bind(std::greater_equal<Key>(), std::placeholders::_1, 2));
EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
Values fromconstfiltered(constfiltered);
EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));

18
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -28,7 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<Vector, Pose2>(
boost::bind(&factorError2D, _1, point, factor), pose);
std::bind(&factorError2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
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<Vector, Pose2>(
boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
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<Vector, Pose3>(
boost::bind(&factorError3D, _1, point, factor), pose);
std::bind(&factorError3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
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<Vector, Pose3>(
boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
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));

7
gtsam/sfm/tests/testTranslationFactor.cpp
View File

@ -20,10 +20,9 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector, Point3>(
boost::bind(&factorError, _1, T2, factor), T1);
std::bind(&factorError, std::placeholders::_1, T2, factor), T1);
H2Expected = numericalDerivative11<Vector, Point3>(
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));

15
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -14,7 +14,7 @@
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
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<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::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<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
&BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none)), R1, R2, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}

17
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -23,10 +23,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector5,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector5,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

14
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -17,9 +17,7 @@
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
#include <boost/bind/bind.hpp>
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<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> 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<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)>
g;

14
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -27,10 +27,9 @@
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind/bind.hpp>
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<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
&OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
&OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
@ -184,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
boost::none), T3);
// Use the factor to calculate the derivative

16
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -16,8 +16,6 @@
#include <iostream>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
@ -32,7 +30,7 @@
using namespace std;
using namespace boost;
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace gtsam;
typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
@ -70,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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<Vector,Point2,Pose2,Point2>(
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));

11
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -13,12 +13,11 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind/bind.hpp>
#include <vector>
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<Vector3,Rot3>(
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<Vector3,Rot3>(
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<Vector,Rot3>(
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<Vector,Rot3>(
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));

7
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -27,11 +27,10 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <boost/bind/bind.hpp>
#include <iostream>
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<Vector(Point3)> f = //
boost::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, _1);
std::function<Vector(Point3)> f = //
std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// Calculate using computeEP
Matrix actualE;

10
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -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<Cal3_S2> sharedCal = //
@ -62,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) {
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
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<Vector,Point3>(
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<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
Expression<StereoPoint2>::UnaryFunction<Point3>::type f =
std::bind(&StereoCamera::project2, camera2, std::placeholders::_1,
boost::none, std::placeholders::_2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);

42
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -3,14 +3,13 @@
* @author Duy-Nguyen Ta
*/
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
/* ************************************************************************* */
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3,
boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5);
std::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
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
);

8
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -23,9 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<double, Event>(
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<double, Point3>(
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));
}

6
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -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<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(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<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

17
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
View File

@ -10,10 +10,9 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/slam/BiasedGPSFactor.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
std::function<Vector(const Pose3&, const Point3&)>(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<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
std::function<Vector(const Pose3&, const Point3&)>(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));
}

2
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -26,7 +26,7 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace boost::placeholders;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;

12
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -23,9 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <boost/bind/bind.hpp>
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<Vector3, Vector3, Vector3>(
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<Vector3, Vector3, Vector3>(
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));

45
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -16,7 +16,6 @@
*/
#include <iostream>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
@ -26,7 +25,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
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<Vector3, Vector3>(boost::bind(&predictionRq, _1, q), angles);
// J_expected = numericalDerivative11<Vector3, Vector3>(std::bind(&predictionRq, std::placeholders::_1, q), angles);
//
// cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl;
@ -312,19 +311,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
H5_expectedPose;
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
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<Vector, Pose3>(
boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(
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<Pose3, Pose3>(
boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
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<Vector, Pose3>(
boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor),
Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(
boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor),
Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor),
Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(
boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor),
Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(
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

10
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -24,9 +24,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<Vector3, Pose3, Pose3,
OrientedPlane3>(f, poseLin, anchorPoseLin, pLin);
Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,

26
gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
View File

@ -16,10 +16,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector, Pose2>(
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<Vector, Pose2>(
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<Vector, Pose2>(
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<Vector, Pose3>(
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<Vector, Pose3>(
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<Vector, Pose3>(
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<Vector, Pose3>(
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<Vector, Pose3>(
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;

23
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -22,10 +22,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
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<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor,
std::placeholders::_1, pose2, boost::none, boost::none),
pose1);
Matrix expectedH2 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1,
std::placeholders::_1, boost::none, boost::none),
pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

13
gtsam_unstable/slam/tests/testPosePriorFactor.cpp
View File

@ -22,10 +22,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
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<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
std::bind(&TestPosePriorFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
pose);
// Use the factor to calculate the derivative
Matrix actualH1;

2
gtsam_unstable/slam/tests/testPoseToPointFactor.h
View File

@ -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<Vector, Pose3, Point3>(f, p, l);
Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);

24
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
View File

@ -28,9 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<Vector,Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
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<Vector, Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
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));

22
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -28,9 +28,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
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<Vector, Pose3>(
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<Vector, Cal3_S2>(
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<Vector, Pose3>(
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<Vector, Cal3_S2>(
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));

87
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -5,14 +5,13 @@
* @author Alex Cunningham
*/
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
#include <gtsam/base/numericalDerivative.h>
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
std::bind(evalFactorError, factor, std::placeholders::_1,
std::placeholders::_2),
pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
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));
}

27
gtsam_unstable/slam/tests/testTSAMFactors.cpp
View File

@ -19,10 +19,9 @@
#include <gtsam_unstable/slam/TSAMFactors.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
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<Vector2, Pose2>(
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<Vector2, Point2>(
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<Vector2, Pose2>(
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<Vector2, Pose2>(
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<Vector2, Pose2>(
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<Vector2, Point2>(
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<Vector3, Pose2>(
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<Vector3, Pose2>(
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<Vector3, Pose2>(
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<Vector3, Pose2>(
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

12
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
View File

@ -18,9 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <boost/bind/bind.hpp>
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<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(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<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(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<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

16
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -18,9 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <boost/bind/bind.hpp>
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<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(
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<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(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<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//

24
tests/testExpressionFactor.cpp
View File

@ -17,22 +17,19 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/expressions.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
#include <boost/bind/bind.hpp>
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<Vector3, Point2>(
boost::bind(f, _1, b, boost::none, boost::none), a),
H1));
CHECK(assert_equal(
numericalDerivative11<Vector3, Point2>(
std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a),
H1));
Values values;
values.insert<Point2>(0, a);

24
tests/testSimulated3D.cpp
View File

@ -26,7 +26,7 @@
#include <iostream>
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<Point3, Point3>(boost::bind(simulated3D::prior, _1, boost::none),x);
Matrix numerical = numericalDerivative11<Point3, Point3>(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<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(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<Point3, Point3, Point3>(
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<Point3, Point3, Point3>(
std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none),
x1, x2);
EXPECT(assert_equal(A2, H2, 1e-9));
}