From 2cbba15573372c6b732fc9c17b52d9469f17dab4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 00:29:18 +0200 Subject: [PATCH 01/45] ceres style functor --- .../nonlinear/tests/testExpression.cpp | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index c66cc3b8b..cc3cf766c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -224,6 +224,60 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +// Some Ceres Snippets copied for testing +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +template inline T &RowMajorAccess(T *base, int rows, int cols, + int i, int j) { + return base[cols * i + j]; +} + +inline double RandDouble() { + double r = static_cast(rand()); + return r / RAND_MAX; +} + +// A structure for projecting a 3x4 camera matrix and a +// homogeneous 3D point, to a 2D inhomogeneous point. +struct Projective { + // Function that takes P and X as separate vectors: + // P, X -> x + template + bool operator()(A const P[12], A const X[4], A x[2]) const { + A PX[3]; + for (int i = 0; i < 3; ++i) { + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + } + if (PX[2] != 0.0) { + x[0] = PX[0] / PX[2]; + x[1] = PX[1] / PX[2]; + return true; + } + return false; + } +}; + +/* ************************************************************************* */ +//#include "/Users/frank/include/ceres/autodiff_cost_function.h" +// Test Ceres AutoDiff +TEST(Expression, AutoDiff) { + + MatrixRowMajor P(3, 4); + P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; + Vector4 X(10, 0, 5, 1); + + // Apply the mapping, to get image point b_x. + Projective projective; + Vector2 actual; + EXPECT(projective(P.data(), X.data(), actual.data())); + + Vector expected = Vector2(2, 1); + EXPECT(assert_equal(expected,actual,1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; From db037c96c5433f5d7ffdabe5d1678efe70530ef6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 12:12:25 +0200 Subject: [PATCH 02/45] Implemented manifold_traits to allow numerical derivatives wrpt Matrix arguments --- gtsam_unstable/nonlinear/CMakeLists.txt | 3 + .../nonlinear/tests/testExpression.cpp | 124 +++++++++++++++++- 2 files changed, 121 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index 9e0cb68e1..85412295a 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -2,5 +2,8 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear) +FIND_PACKAGE(Ceres REQUIRED) +INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) + # Add all tests add_subdirectory(tests) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index cc3cf766c..84a6b67f9 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -23,7 +23,11 @@ #include #include #include +//#include +#include + +#undef CHECK #include #include @@ -258,24 +262,132 @@ struct Projective { } return false; } + Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { + Vector2 x; + if (operator()(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Projective fails"); + } }; /* ************************************************************************* */ -//#include "/Users/frank/include/ceres/autodiff_cost_function.h" + +#include + +template +struct manifold_traits { + typedef T type; + static const size_t dimension = T::dimension; + typedef Eigen::Matrix tangent; + static tangent localCoordinates(const T& t1, const T& t2) { + return t1.localCoordinates(t2); + } + static type retract(const type& t, const tangent& d) { + return t.retract(d); + } +}; + +// Adapt constant size Eigen::Matrix types as manifold types +template +struct manifold_traits > { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); + typedef Eigen::Matrix type; + static const size_t dimension = M * N; + typedef Eigen::Matrix tangent; + static tangent localCoordinates(const type& t1, const type& t2) { + type diff = t2 - t1; + return tangent(Eigen::Map(diff.data())); + } + static type retract(const type& t, const tangent& d) { + type sum = t + Eigen::Map(d.data()); + return sum; + } +}; + +// Test dimension traits +TEST(Expression, Traits) { + EXPECT_LONGS_EQUAL(2, manifold_traits::dimension); + EXPECT_LONGS_EQUAL(8, manifold_traits::dimension); +} + +template +Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + Y hx = h(x1, x2); + double factor = 1.0 / (2.0 * delta); + static const size_t m = manifold_traits::dimension, n = + manifold_traits::dimension; + Eigen::Matrix d; + d.setZero(); + Matrix H = zeros(m, n); + for (size_t j = 0; j < n; j++) { + d(j) += delta; + Vector hxplus = manifold_traits::localCoordinates(hx, + h(manifold_traits::retract(x1, d), x2)); + d(j) -= 2 * delta; + Vector hxmin = manifold_traits::localCoordinates(hx, + h(manifold_traits::retract(x1, d), x2)); + d(j) += delta; + H.block(0, j) << (hxplus - hxmin) * factor; + } + return H; +} + +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + Y hx = h(x1, x2); + double factor = 1.0 / (2.0 * delta); + static const size_t m = manifold_traits::dimension, n = + manifold_traits::dimension; + Eigen::Matrix d; + d.setZero(); + Matrix H = zeros(m, n); + for (size_t j = 0; j < n; j++) { + d(j) += delta; + Vector hxplus = manifold_traits::localCoordinates(hx, + h(x1, manifold_traits::retract(x2, d))); + d(j) -= 2 * delta; + Vector hxmin = manifold_traits::localCoordinates(hx, + h(x1, manifold_traits::retract(x2, d))); + d(j) += delta; + H.block(0, j) << (hxplus - hxmin) * factor; + } + return H; +} +/* ************************************************************************* */ // Test Ceres AutoDiff TEST(Expression, AutoDiff) { + using ceres::internal::AutoDiff; - MatrixRowMajor P(3, 4); + // Instantiate function + Projective projective; + + // Make arguments + typedef Eigen::Matrix M; + M P; P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; Vector4 X(10, 0, 5, 1); // Apply the mapping, to get image point b_x. - Projective projective; - Vector2 actual; - EXPECT(projective(P.data(), X.data(), actual.data())); - Vector expected = Vector2(2, 1); + Vector2 actual = projective(P, X); EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21(Projective(), P, X); + Matrix E2 = numericalDerivative22(Projective(), P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 12), H2(2, 4); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } /* ************************************************************************* */ From 2972671064625d86c0cbf10625b38938c1e2a0c1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 12:29:14 +0200 Subject: [PATCH 03/45] Use boost::bind to avoid code duplication --- .../nonlinear/tests/testExpression.cpp | 50 ++++++++----------- 1 file changed, 21 insertions(+), 29 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 84a6b67f9..252d2c73c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -23,9 +23,9 @@ #include #include #include -//#include -#include +#include "ceres/ceres.h" +#include "ceres/rotation.h" #undef CHECK #include @@ -272,7 +272,7 @@ struct Projective { }; /* ************************************************************************* */ - +// manifold_traits prototype #include template @@ -311,51 +311,43 @@ TEST(Expression, Traits) { EXPECT_LONGS_EQUAL(8, manifold_traits::dimension); } -template -Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - Y hx = h(x1, x2); +/* ************************************************************************* */ +// New-style numerical derivatives using manifold_traits +template +Matrix numericalDerivative(boost::function h, const X& x, + double delta = 1e-5) { + Y hx = h(x); double factor = 1.0 / (2.0 * delta); static const size_t m = manifold_traits::dimension, n = - manifold_traits::dimension; + manifold_traits::dimension; Eigen::Matrix d; d.setZero(); Matrix H = zeros(m, n); for (size_t j = 0; j < n; j++) { d(j) += delta; Vector hxplus = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x1, d), x2)); + h(manifold_traits::retract(x, d))); d(j) -= 2 * delta; Vector hxmin = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x1, d), x2)); + h(manifold_traits::retract(x, d))); d(j) += delta; H.block(0, j) << (hxplus - hxmin) * factor; } return H; } -template +template +Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalDerivative(boost::bind(h,_1,x2),x1,delta); +} + +template Matrix numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - Y hx = h(x1, x2); - double factor = 1.0 / (2.0 * delta); - static const size_t m = manifold_traits::dimension, n = - manifold_traits::dimension; - Eigen::Matrix d; - d.setZero(); - Matrix H = zeros(m, n); - for (size_t j = 0; j < n; j++) { - d(j) += delta; - Vector hxplus = manifold_traits::localCoordinates(hx, - h(x1, manifold_traits::retract(x2, d))); - d(j) -= 2 * delta; - Vector hxmin = manifold_traits::localCoordinates(hx, - h(x1, manifold_traits::retract(x2, d))); - d(j) += delta; - H.block(0, j) << (hxplus - hxmin) * factor; - } - return H; + return numericalDerivative(boost::bind(h,x1,_1),x2,delta); } + /* ************************************************************************* */ // Test Ceres AutoDiff TEST(Expression, AutoDiff) { From 7018afdd58c1893e7b3addfc70e1d517d50c44b6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 12:34:05 +0200 Subject: [PATCH 04/45] Slight refactor of numerical derivatives --- .../nonlinear/tests/testExpression.cpp | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 252d2c73c..49f7f6f40 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -318,20 +318,19 @@ Matrix numericalDerivative(boost::function h, const X& x, double delta = 1e-5) { Y hx = h(x); double factor = 1.0 / (2.0 * delta); - static const size_t m = manifold_traits::dimension, n = - manifold_traits::dimension; - Eigen::Matrix d; - d.setZero(); - Matrix H = zeros(m, n); - for (size_t j = 0; j < n; j++) { - d(j) += delta; + static const size_t M = manifold_traits::dimension; + static const size_t N = manifold_traits::dimension; + Eigen::Matrix d; + Matrix H = zeros(M, N); + for (size_t j = 0; j < N; j++) { + d.setZero(); + d(j) = delta; Vector hxplus = manifold_traits::localCoordinates(hx, h(manifold_traits::retract(x, d))); - d(j) -= 2 * delta; + d(j) = -delta; Vector hxmin = manifold_traits::localCoordinates(hx, h(manifold_traits::retract(x, d))); - d(j) += delta; - H.block(0, j) << (hxplus - hxmin) * factor; + H.block(0, j) << (hxplus - hxmin) * factor; } return H; } @@ -339,13 +338,13 @@ Matrix numericalDerivative(boost::function h, const X& x, template Matrix numericalDerivative21(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h,_1,x2),x1,delta); + return numericalDerivative(boost::bind(h, _1, x2), x1, delta); } template Matrix numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h,x1,_1),x2,delta); + return numericalDerivative(boost::bind(h, x1, _1), x2, delta); } /* ************************************************************************* */ From bdf12b14b9cfbbdbf2865ba2d31aa953692074b7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 12:35:02 +0200 Subject: [PATCH 05/45] Add Snavely cost function --- .../nonlinear/tests/testExpression.cpp | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 49f7f6f40..01e493c4f 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -271,6 +271,63 @@ struct Projective { } }; +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyReprojectionError { + SnavelyReprojectionError(double observed_x, double observed_y) : + observed_x(observed_x), observed_y(observed_y) { + } + + template + bool operator()(const T* const camera, const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + ceres::AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + T xp = -p[0] / p[2]; + T yp = -p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + T r2 = xp * xp + yp * yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + const T& focal = camera[6]; + T predicted_x = focal * distortion * xp; + T predicted_y = focal * distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - T(observed_x); + residuals[1] = predicted_y - T(observed_y); + + return true; + } + + // Factory to hide the construction of the CostFunction object from + // the client code. + static ceres::CostFunction* Create(const double observed_x, + const double observed_y) { + return (new ceres::AutoDiffCostFunction( + new SnavelyReprojectionError(observed_x, observed_y))); + } + + double observed_x; + double observed_y; +}; + /* ************************************************************************* */ // manifold_traits prototype #include From 4c334444155aeeab57903ce8ef3dc47ac6f0b12c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 13:16:44 +0200 Subject: [PATCH 06/45] Snavely tested --- .../nonlinear/tests/testExpression.cpp | 74 ++++++++++++++----- 1 file changed, 55 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 01e493c4f..40c97cfca 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -262,12 +262,14 @@ struct Projective { } return false; } + + // Adapt to eigen types Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { Vector2 x; if (operator()(P.data(), X.data(), x.data())) return x; else - throw std::runtime_error("Projective fails"); + throw std::runtime_error("Projective fail"); } }; @@ -276,13 +278,10 @@ struct Projective { // focal length and 2 for radial distortion. The principal point is not modeled // (i.e. it is assumed be located at the image center). struct SnavelyReprojectionError { - SnavelyReprojectionError(double observed_x, double observed_y) : - observed_x(observed_x), observed_y(observed_y) { - } template bool operator()(const T* const camera, const T* const point, - T* residuals) const { + T* predicted) const { // camera[0,1,2] are the angle-axis rotation. T p[3]; ceres::AngleAxisRotatePoint(camera, point, p); @@ -306,26 +305,21 @@ struct SnavelyReprojectionError { // Compute final projected point position. const T& focal = camera[6]; - T predicted_x = focal * distortion * xp; - T predicted_y = focal * distortion * yp; - - // The error is the difference between the predicted and observed position. - residuals[0] = predicted_x - T(observed_x); - residuals[1] = predicted_y - T(observed_y); + predicted[0] = focal * distortion * xp; + predicted[1] = focal * distortion * yp; return true; } - // Factory to hide the construction of the CostFunction object from - // the client code. - static ceres::CostFunction* Create(const double observed_x, - const double observed_y) { - return (new ceres::AutoDiffCostFunction( - new SnavelyReprojectionError(observed_x, observed_y))); + // Adapt to GTSAM types + Vector2 operator()(const Vector9& P, const Vector3& X) const { + Vector2 x; + if (operator()(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Snavely fail"); } - double observed_x; - double observed_y; }; /* ************************************************************************* */ @@ -438,6 +432,48 @@ TEST(Expression, AutoDiff) { EXPECT(assert_equal(E2,H2,1e-8)); } +/* ************************************************************************* */ +// Test Ceres AutoDiff on Snavely +TEST(Expression, AutoDiff2) { + using ceres::internal::AutoDiff; + + // Instantiate function + SnavelyReprojectionError snavely; + + // Make arguments + Vector9 P; + P << 0, 0, 0, 0, 5, 0, 1, 0, 0; + Vector3 X(10, 0, -5); + + // Apply the mapping, to get image point b_x. + Vector expected = Vector2(2, 1); + Vector2 actual = snavely(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21( + SnavelyReprojectionError(), P, X); + Matrix E2 = numericalDerivative22( + SnavelyReprojectionError(), P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 9), H2(2, 3); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// keys +TEST(Expression, SnavelyKeys) { +// Expression expression(1); +// set expected = list_of(1)(2); +// EXPECT(expected == expression.keys()); +} /* ************************************************************************* */ int main() { TestResult tr; From f08dc6c031d1d5208eb205a3db14ed7c81b7623f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 14:16:24 +0200 Subject: [PATCH 07/45] More boost-style traits --- .../nonlinear/tests/testExpression.cpp | 47 +++++++++++++++---- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 40c97cfca..7fb764129 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -324,13 +324,38 @@ struct SnavelyReprojectionError { /* ************************************************************************* */ // manifold_traits prototype +// Same style as Boost.TypeTraits +// All meta-functions below ever only declare a single type +// or a type/value/value_type #include +#include + +// is manifold +template +struct is_manifold: public false_type { +}; + +// dimension +template +struct dimension: public integral_constant { +}; + +// Fixed size Eigen::Matrix type +template +struct is_manifold > : public true_type { +}; + +template +struct dimension > : public integral_constant< + size_t, M * N> { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); +}; template struct manifold_traits { typedef T type; - static const size_t dimension = T::dimension; - typedef Eigen::Matrix tangent; + static const size_t dim = dimension::value; + typedef Eigen::Matrix tangent; static tangent localCoordinates(const T& t1, const T& t2) { return t1.localCoordinates(t2); } @@ -344,8 +369,8 @@ template struct manifold_traits > { BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); typedef Eigen::Matrix type; - static const size_t dimension = M * N; - typedef Eigen::Matrix tangent; + static const size_t dim = dimension::value; + typedef Eigen::Matrix tangent; static tangent localCoordinates(const type& t1, const type& t2) { type diff = t2 - t1; return tangent(Eigen::Map(diff.data())); @@ -358,8 +383,8 @@ struct manifold_traits > { // Test dimension traits TEST(Expression, Traits) { - EXPECT_LONGS_EQUAL(2, manifold_traits::dimension); - EXPECT_LONGS_EQUAL(8, manifold_traits::dimension); + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); } /* ************************************************************************* */ @@ -367,10 +392,12 @@ TEST(Expression, Traits) { template Matrix numericalDerivative(boost::function h, const X& x, double delta = 1e-5) { + BOOST_STATIC_ASSERT(is_manifold::value); + BOOST_STATIC_ASSERT(is_manifold::value); Y hx = h(x); double factor = 1.0 / (2.0 * delta); - static const size_t M = manifold_traits::dimension; - static const size_t N = manifold_traits::dimension; + static const size_t M = dimension::value; + static const size_t N = dimension::value; Eigen::Matrix d; Matrix H = zeros(M, N); for (size_t j = 0; j < N; j++) { @@ -441,9 +468,9 @@ TEST(Expression, AutoDiff2) { SnavelyReprojectionError snavely; // Make arguments - Vector9 P; + Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); + Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! // Apply the mapping, to get image point b_x. Vector expected = Vector2(2, 1); From ec69949f4329e009c10212b2be35c6bbf59a05c8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 14:22:01 +0200 Subject: [PATCH 08/45] Point2 specialized --- .../nonlinear/tests/testExpression.cpp | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 7fb764129..bb3bac1af 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -337,8 +337,7 @@ struct is_manifold: public false_type { // dimension template -struct dimension: public integral_constant { -}; +struct dimension; // Fixed size Eigen::Matrix type template @@ -351,6 +350,16 @@ struct dimension > : public integral_consta BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; +// Point2 + +template<> +struct is_manifold : public true_type { +}; + +template<> +struct dimension : public integral_constant { +}; + template struct manifold_traits { typedef T type; @@ -381,8 +390,15 @@ struct manifold_traits > { } }; -// Test dimension traits -TEST(Expression, Traits) { +// is_manifold +TEST(Expression, is_manifold) { + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +// dimension +TEST(Expression, dimension) { EXPECT_LONGS_EQUAL(2, dimension::value); EXPECT_LONGS_EQUAL(8, dimension::value); } From 10cfd47404a3de972b80accf346fcd74328d9bfb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 14:29:40 +0200 Subject: [PATCH 09/45] TangentVector meta-function --- .../nonlinear/tests/testExpression.cpp | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index bb3bac1af..b7f1cae5e 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -339,7 +339,15 @@ struct is_manifold: public false_type { template struct dimension; +// TangentVector is eigen::Matrix type in tangent space +template +struct TangentVector { + BOOST_STATIC_ASSERT(is_manifold::value); + typedef Eigen::Matrix::value, 1> type; +}; + // Fixed size Eigen::Matrix type + template struct is_manifold > : public true_type { }; @@ -363,12 +371,11 @@ struct dimension : public integral_constant { template struct manifold_traits { typedef T type; - static const size_t dim = dimension::value; - typedef Eigen::Matrix tangent; - static tangent localCoordinates(const T& t1, const T& t2) { + static typename TangentVector::type localCoordinates(const T& t1, + const T& t2) { return t1.localCoordinates(t2); } - static type retract(const type& t, const tangent& d) { + static type retract(const type& t, const typename TangentVector::type& d) { return t.retract(d); } }; @@ -378,13 +385,14 @@ template struct manifold_traits > { BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); typedef Eigen::Matrix type; - static const size_t dim = dimension::value; - typedef Eigen::Matrix tangent; - static tangent localCoordinates(const type& t1, const type& t2) { + static typename TangentVector::type localCoordinates(const type& t1, + const type& t2) { type diff = t2 - t1; - return tangent(Eigen::Map(diff.data())); + return typename TangentVector::type( + Eigen::Map::type>(diff.data())); } - static type retract(const type& t, const tangent& d) { + static type retract(const type& t, + const typename TangentVector::type& d) { type sum = t + Eigen::Map(d.data()); return sum; } From 66b3081603b9744f86d4e4a28425a6dd5a3ec09a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 15:02:22 +0200 Subject: [PATCH 10/45] localCoordinates and retract --- .../nonlinear/tests/testExpression.cpp | 76 ++++++++++--------- 1 file changed, 42 insertions(+), 34 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index b7f1cae5e..28d160799 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -346,6 +346,22 @@ struct TangentVector { typedef Eigen::Matrix::value, 1> type; }; +// default localCoordinates +template +struct localCoordinates { + typename TangentVector::type operator()(const T& t1, const T& t2) { + return t1.localCoordinates(t2); + } +}; + +// default retract +template +struct retract { + T operator()(const T& t, const typename TangentVector::type& d) { + return t.retract(d); + } +}; + // Fixed size Eigen::Matrix type template @@ -358,6 +374,24 @@ struct dimension > : public integral_consta BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; +template +struct localCoordinates > { + typedef Eigen::Matrix T; + typedef typename TangentVector::type result_type; + result_type operator()(const T& t1, const T& t2) { + T diff = t2 - t1; + return result_type(Eigen::Map(diff.data())); + } +}; + +template +struct retract > { + typedef Eigen::Matrix T; + T operator()(const T& t, const typename TangentVector::type& d) { + return t + Eigen::Map(d.data()); + } +}; + // Point2 template<> @@ -368,36 +402,6 @@ template<> struct dimension : public integral_constant { }; -template -struct manifold_traits { - typedef T type; - static typename TangentVector::type localCoordinates(const T& t1, - const T& t2) { - return t1.localCoordinates(t2); - } - static type retract(const type& t, const typename TangentVector::type& d) { - return t.retract(d); - } -}; - -// Adapt constant size Eigen::Matrix types as manifold types -template -struct manifold_traits > { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); - typedef Eigen::Matrix type; - static typename TangentVector::type localCoordinates(const type& t1, - const type& t2) { - type diff = t2 - t1; - return typename TangentVector::type( - Eigen::Map::type>(diff.data())); - } - static type retract(const type& t, - const typename TangentVector::type& d) { - type sum = t + Eigen::Map(d.data()); - return sum; - } -}; - // is_manifold TEST(Expression, is_manifold) { EXPECT(!is_manifold::value); @@ -411,6 +415,12 @@ TEST(Expression, dimension) { EXPECT_LONGS_EQUAL(8, dimension::value); } +// localCoordinates +TEST(Expression, localCoordinates) { + EXPECT(localCoordinates()(Point2(0,0),Point2(1,0))==Vector2(1,0)); + EXPECT(localCoordinates()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); +} + /* ************************************************************************* */ // New-style numerical derivatives using manifold_traits template @@ -427,11 +437,9 @@ Matrix numericalDerivative(boost::function h, const X& x, for (size_t j = 0; j < N; j++) { d.setZero(); d(j) = delta; - Vector hxplus = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x, d))); + Vector hxplus = localCoordinates()(hx, h(retract()(x, d))); d(j) = -delta; - Vector hxmin = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x, d))); + Vector hxmin = localCoordinates()(hx, h(retract()(x, d))); H.block(0, j) << (hxplus - hxmin) * factor; } return H; From 9c97b1d8a0720a9a45e85ee532cdb3fedd28be32 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 16:45:04 +0200 Subject: [PATCH 11/45] Some more refactoring --- .../nonlinear/tests/testExpression.cpp | 81 +++++++++++++------ 1 file changed, 58 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 28d160799..b0abf6b6f 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -323,23 +323,39 @@ struct SnavelyReprojectionError { }; /* ************************************************************************* */ -// manifold_traits prototype -// Same style as Boost.TypeTraits + +/** + * A manifold defines a space in which there is a notion of a linear tangent space + * that can be centered around a given point on the manifold. These nonlinear + * spaces may have such properties as wrapping around (as is the case with rotations), + * which might make linear operations on parameters not return a viable element of + * the manifold. + * + * We perform optimization by computing a linear delta in the tangent space of the + * current estimate, and then apply this change using a retraction operation, which + * maps the change in tangent space back to the manifold itself. + * + * There may be multiple possible retractions for a given manifold, which can be chosen + * between depending on the computational complexity. The important criteria for + * the creation for the retract and localCoordinates functions is that they be + * inverse operations. + * + */ + +// Traits, same style as Boost.TypeTraits // All meta-functions below ever only declare a single type // or a type/value/value_type -#include -#include - -// is manifold +// is manifold, by default this is false template -struct is_manifold: public false_type { +struct is_manifold: public std::false_type { }; -// dimension +// dimension, can return Eigen::Dynamic (-1) if not known at compile time template -struct dimension; +struct dimension: public std::integral_constant { +}; -// TangentVector is eigen::Matrix type in tangent space +// TangentVector is Eigen::Matrix type in tangent space, can be Dynamic... template struct TangentVector { BOOST_STATIC_ASSERT(is_manifold::value); @@ -348,7 +364,7 @@ struct TangentVector { // default localCoordinates template -struct localCoordinates { +struct LocalCoordinates { typename TangentVector::type operator()(const T& t1, const T& t2) { return t1.localCoordinates(t2); } @@ -356,7 +372,7 @@ struct localCoordinates { // default retract template -struct retract { +struct Retract { T operator()(const T& t, const typename TangentVector::type& d) { return t.retract(d); } @@ -375,7 +391,7 @@ struct dimension > : public integral_consta }; template -struct localCoordinates > { +struct LocalCoordinates > { typedef Eigen::Matrix T; typedef typename TangentVector::type result_type; result_type operator()(const T& t1, const T& t2) { @@ -385,7 +401,7 @@ struct localCoordinates > { }; template -struct retract > { +struct Retract > { typedef Eigen::Matrix T; T operator()(const T& t, const typename TangentVector::type& d) { return t + Eigen::Map(d.data()); @@ -417,8 +433,14 @@ TEST(Expression, dimension) { // localCoordinates TEST(Expression, localCoordinates) { - EXPECT(localCoordinates()(Point2(0,0),Point2(1,0))==Vector2(1,0)); - EXPECT(localCoordinates()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); + EXPECT(LocalCoordinates()(Point2(0,0),Point2(1,0))==Vector2(1,0)); + EXPECT(LocalCoordinates()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); +} + +// retract +TEST(Expression, retract) { + EXPECT(Retract()(Point2(0,0),Vector2(1,0))==Point2(1,0)); + EXPECT(Retract()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); } /* ************************************************************************* */ @@ -426,21 +448,34 @@ TEST(Expression, localCoordinates) { template Matrix numericalDerivative(boost::function h, const X& x, double delta = 1e-5) { + BOOST_STATIC_ASSERT(is_manifold::value); - BOOST_STATIC_ASSERT(is_manifold::value); - Y hx = h(x); - double factor = 1.0 / (2.0 * delta); static const size_t M = dimension::value; + typedef typename TangentVector::type TangentY; + LocalCoordinates localCoordinates; + + BOOST_STATIC_ASSERT(is_manifold::value); static const size_t N = dimension::value; - Eigen::Matrix d; + typedef typename TangentVector::type TangentX; + Retract retract; + + // get value at x + Y hx = h(x); + + // Prepare a tangent vector to perturb x with + TangentX d; + d.setZero(); + + // Fill in Jacobian H Matrix H = zeros(M, N); + double factor = 1.0 / (2.0 * delta); for (size_t j = 0; j < N; j++) { - d.setZero(); d(j) = delta; - Vector hxplus = localCoordinates()(hx, h(retract()(x, d))); + TangentY hxplus = localCoordinates(hx, h(retract(x, d))); d(j) = -delta; - Vector hxmin = localCoordinates()(hx, h(retract()(x, d))); + TangentY hxmin = localCoordinates(hx, h(retract(x, d))); H.block(0, j) << (hxplus - hxmin) * factor; + d(j) = 0; } return H; } From ed6a2b6effaae2b9e3535dde37955b8b46de8f6e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 17:11:28 +0200 Subject: [PATCH 12/45] Charts !!!! --- .../nonlinear/tests/testExpression.cpp | 120 ++++++++++-------- 1 file changed, 69 insertions(+), 51 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index b0abf6b6f..ab2aae1c2 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -355,27 +355,22 @@ template struct dimension: public std::integral_constant { }; -// TangentVector is Eigen::Matrix type in tangent space, can be Dynamic... +// Chart is a map from T -> vector, retract is its inverse template -struct TangentVector { +struct DefaultChart { BOOST_STATIC_ASSERT(is_manifold::value); - typedef Eigen::Matrix::value, 1> type; -}; - -// default localCoordinates -template -struct LocalCoordinates { - typename TangentVector::type operator()(const T& t1, const T& t2) { - return t1.localCoordinates(t2); + typedef Eigen::Matrix::value, 1> vector; + DefaultChart(const T& t) : + t_(t) { } -}; - -// default retract -template -struct Retract { - T operator()(const T& t, const typename TangentVector::type& d) { - return t.retract(d); + vector apply(const T& other) { + return t_.localCoordinates(other); } + T retract(const vector& d) { + return t_.retract(d); + } +private: + T const & t_; }; // Fixed size Eigen::Matrix type @@ -384,28 +379,48 @@ template struct is_manifold > : public true_type { }; +// TODO: Could be more sophisticated using Eigen traits and SFINAE? + +template +struct dimension > : public integral_constant< + size_t, Eigen::Dynamic> { +}; + +template +struct dimension > : public integral_constant< + size_t, Eigen::Dynamic> { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic); +}; + +template +struct dimension > : public integral_constant< + size_t, Eigen::Dynamic> { + BOOST_STATIC_ASSERT(N!=Eigen::Dynamic); +}; + template struct dimension > : public integral_constant< size_t, M * N> { BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; +// Chart is a map from T -> vector, retract is its inverse template -struct LocalCoordinates > { +struct DefaultChart > { typedef Eigen::Matrix T; - typedef typename TangentVector::type result_type; - result_type operator()(const T& t1, const T& t2) { - T diff = t2 - t1; - return result_type(Eigen::Map(diff.data())); + typedef Eigen::Matrix::value, 1> vector; + DefaultChart(const T& t) : + t_(t) { } -}; - -template -struct Retract > { - typedef Eigen::Matrix T; - T operator()(const T& t, const typename TangentVector::type& d) { - return t + Eigen::Map(d.data()); + vector apply(const T& other) { + T diff = other - t_; + return Eigen::Map(diff.data()); } + T retract(const vector& d) { + return t_ + Eigen::Map(d.data()); + } +private: + T const & t_; }; // Point2 @@ -431,16 +446,15 @@ TEST(Expression, dimension) { EXPECT_LONGS_EQUAL(8, dimension::value); } -// localCoordinates -TEST(Expression, localCoordinates) { - EXPECT(LocalCoordinates()(Point2(0,0),Point2(1,0))==Vector2(1,0)); - EXPECT(LocalCoordinates()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); -} +// charts +TEST(Expression, Charts) { + DefaultChart chart1(Point2(0, 0)); + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); -// retract -TEST(Expression, retract) { - EXPECT(Retract()(Point2(0,0),Vector2(1,0))==Point2(1,0)); - EXPECT(Retract()(Vector2(0,0),Vector2(1,0))==Vector2(1,0)); + DefaultChart chart2(Vector2(0, 0)); + EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); } /* ************************************************************************* */ @@ -451,31 +465,35 @@ Matrix numericalDerivative(boost::function h, const X& x, BOOST_STATIC_ASSERT(is_manifold::value); static const size_t M = dimension::value; - typedef typename TangentVector::type TangentY; - LocalCoordinates localCoordinates; + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; BOOST_STATIC_ASSERT(is_manifold::value); static const size_t N = dimension::value; - typedef typename TangentVector::type TangentX; - Retract retract; + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; - // get value at x + // get chart at x + ChartX chartX(x); + + // get value at x, and corresponding chart Y hx = h(x); + ChartY chartY(hx); // Prepare a tangent vector to perturb x with - TangentX d; - d.setZero(); + TangentX dx; + dx.setZero(); // Fill in Jacobian H Matrix H = zeros(M, N); double factor = 1.0 / (2.0 * delta); for (size_t j = 0; j < N; j++) { - d(j) = delta; - TangentY hxplus = localCoordinates(hx, h(retract(x, d))); - d(j) = -delta; - TangentY hxmin = localCoordinates(hx, h(retract(x, d))); - H.block(0, j) << (hxplus - hxmin) * factor; - d(j) = 0; + dx(j) = delta; + TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + dx(j) = -delta; + TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + H.block(0, j) << (dy1 - dy2) * factor; + dx(j) = 0; } return H; } From fcda501ee2628845d50f24ab7e36b449549de91e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 18:13:33 +0200 Subject: [PATCH 13/45] double as manifold. No more LieScalar ! --- .../nonlinear/tests/testExpression.cpp | 60 +++++++++++++++++-- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ab2aae1c2..e9a1b7163 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -352,8 +352,10 @@ struct is_manifold: public std::false_type { // dimension, can return Eigen::Dynamic (-1) if not known at compile time template -struct dimension: public std::integral_constant { -}; +struct dimension; +//: public std::integral_constant { +// BOOST_STATIC_ASSERT(is_manifold::value); +//}; // Chart is a map from T -> vector, retract is its inverse template @@ -373,6 +375,34 @@ private: T const & t_; }; +// double + +template<> +struct is_manifold : public true_type { +}; + +template<> +struct dimension : public integral_constant { +}; + +template<> +struct DefaultChart { + typedef Eigen::Matrix vector; + DefaultChart(double t) : + t_(t) { + } + vector apply(double other) { + vector d; + d << other - t_; + return d; + } + double retract(const vector& d) { + return t_ + d[0]; + } +private: + double t_; +}; + // Fixed size Eigen::Matrix type template @@ -404,7 +434,6 @@ struct dimension > : public integral_consta BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; -// Chart is a map from T -> vector, retract is its inverse template struct DefaultChart > { typedef Eigen::Matrix T; @@ -414,10 +443,12 @@ struct DefaultChart > { } vector apply(const T& other) { T diff = other - t_; - return Eigen::Map(diff.data()); + Eigen::Map map(diff.data()); + return vector(map); } T retract(const vector& d) { - return t_ + Eigen::Map(d.data()); + Eigen::Map map(d.data()); + return t_ + map; } private: T const & t_; @@ -438,16 +469,23 @@ TEST(Expression, is_manifold) { EXPECT(!is_manifold::value); EXPECT(is_manifold::value); EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); } // dimension TEST(Expression, dimension) { EXPECT_LONGS_EQUAL(2, dimension::value); EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); } // charts TEST(Expression, Charts) { + DefaultChart chart1(Point2(0, 0)); EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); @@ -455,6 +493,18 @@ TEST(Expression, Charts) { DefaultChart chart2(Vector2(0, 0)); EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + DefaultChart chart3(0); + Eigen::Matrix v1; v1<<1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// DefaultChart chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); } /* ************************************************************************* */ From d436d991460100f28e2cfdda6390acbf2dea4c1c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 23:25:25 +0200 Subject: [PATCH 14/45] Moved stuff to Manifold.h --- gtsam/base/Manifold.h | 138 ++++++++++++++++-- .../nonlinear/tests/testExpression.cpp | 136 +---------------- 2 files changed, 132 insertions(+), 142 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index ceebf6bad..1eee71dfd 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -19,19 +19,12 @@ #include #include +#include +#include namespace gtsam { /** - * Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * * A manifold defines a space in which there is a notion of a linear tangent space * that can be centered around a given point on the manifold. These nonlinear * spaces may have such properties as wrapping around (as is the case with rotations), @@ -45,7 +38,130 @@ namespace gtsam { * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. + * inverse operations. The new notion of a Chart guarantees that. + * + */ + +// Traits, same style as Boost.TypeTraits +// All meta-functions below ever only declare a single type +// or a type/value/value_type +// is manifold, by default this is false +template +struct is_manifold: public std::false_type { +}; + +// dimension, can return Eigen::Dynamic (-1) if not known at compile time +template +struct dimension; +//: public std::integral_constant { +// BOOST_STATIC_ASSERT(is_manifold::value); +//}; + +// Chart is a map from T -> vector, retract is its inverse +template +struct DefaultChart { + BOOST_STATIC_ASSERT(is_manifold::value); + typedef Eigen::Matrix::value, 1> vector; + DefaultChart(const T& t) : + t_(t) { + } + vector apply(const T& other) { + return t_.localCoordinates(other); + } + T retract(const vector& d) { + return t_.retract(d); + } +private: + T const & t_; +}; + +// double + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct DefaultChart { + typedef Eigen::Matrix vector; + DefaultChart(double t) : + t_(t) { + } + vector apply(double other) { + vector d; + d << other - t_; + return d; + } + double retract(const vector& d) { + return t_ + d[0]; + } +private: + double t_; +}; + +// Fixed size Eigen::Matrix type + +template +struct is_manifold > : public std::true_type { +}; + +// TODO: Could be more sophisticated using Eigen traits and SFINAE? + +typedef std::integral_constant Dynamic; + +template +struct dimension > : public Dynamic { +}; + +template +struct dimension > : public Dynamic { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic); +}; + +template +struct dimension > : public Dynamic { + BOOST_STATIC_ASSERT(N!=Eigen::Dynamic); +}; + +template +struct dimension > : public std::integral_constant< + size_t, M * N> { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); +}; + +template +struct DefaultChart > { + typedef Eigen::Matrix T; + typedef Eigen::Matrix::value, 1> vector; + DefaultChart(const T& t) : + t_(t) { + } + vector apply(const T& other) { + T diff = other - t_; + Eigen::Map map(diff.data()); + return vector(map); + } + T retract(const vector& d) { + Eigen::Map map(d.data()); + return t_ + map; + } +private: + T const & t_; +}; + +/** + * Old Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. * * Returns dimensionality of the tangent space, which may be smaller than the number * of nonlinear parameters. @@ -61,7 +177,7 @@ namespace gtsam { * By convention, we use capital letters to designate a static function * @tparam T is a Lie type, like Point2, Pose3, etc. */ -template +template class ManifoldConcept { private: /** concept checking function - implement the functions this demands */ diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index e9a1b7163..45f8f3284 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -324,137 +324,8 @@ struct SnavelyReprojectionError { /* ************************************************************************* */ -/** - * A manifold defines a space in which there is a notion of a linear tangent space - * that can be centered around a given point on the manifold. These nonlinear - * spaces may have such properties as wrapping around (as is the case with rotations), - * which might make linear operations on parameters not return a viable element of - * the manifold. - * - * We perform optimization by computing a linear delta in the tangent space of the - * current estimate, and then apply this change using a retraction operation, which - * maps the change in tangent space back to the manifold itself. - * - * There may be multiple possible retractions for a given manifold, which can be chosen - * between depending on the computational complexity. The important criteria for - * the creation for the retract and localCoordinates functions is that they be - * inverse operations. - * - */ - -// Traits, same style as Boost.TypeTraits -// All meta-functions below ever only declare a single type -// or a type/value/value_type -// is manifold, by default this is false -template -struct is_manifold: public std::false_type { -}; - -// dimension, can return Eigen::Dynamic (-1) if not known at compile time -template -struct dimension; -//: public std::integral_constant { -// BOOST_STATIC_ASSERT(is_manifold::value); -//}; - -// Chart is a map from T -> vector, retract is its inverse -template -struct DefaultChart { - BOOST_STATIC_ASSERT(is_manifold::value); - typedef Eigen::Matrix::value, 1> vector; - DefaultChart(const T& t) : - t_(t) { - } - vector apply(const T& other) { - return t_.localCoordinates(other); - } - T retract(const vector& d) { - return t_.retract(d); - } -private: - T const & t_; -}; - -// double - -template<> -struct is_manifold : public true_type { -}; - -template<> -struct dimension : public integral_constant { -}; - -template<> -struct DefaultChart { - typedef Eigen::Matrix vector; - DefaultChart(double t) : - t_(t) { - } - vector apply(double other) { - vector d; - d << other - t_; - return d; - } - double retract(const vector& d) { - return t_ + d[0]; - } -private: - double t_; -}; - -// Fixed size Eigen::Matrix type - -template -struct is_manifold > : public true_type { -}; - -// TODO: Could be more sophisticated using Eigen traits and SFINAE? - -template -struct dimension > : public integral_constant< - size_t, Eigen::Dynamic> { -}; - -template -struct dimension > : public integral_constant< - size_t, Eigen::Dynamic> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic); -}; - -template -struct dimension > : public integral_constant< - size_t, Eigen::Dynamic> { - BOOST_STATIC_ASSERT(N!=Eigen::Dynamic); -}; - -template -struct dimension > : public integral_constant< - size_t, M * N> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); -}; - -template -struct DefaultChart > { - typedef Eigen::Matrix T; - typedef Eigen::Matrix::value, 1> vector; - DefaultChart(const T& t) : - t_(t) { - } - vector apply(const T& other) { - T diff = other - t_; - Eigen::Map map(diff.data()); - return vector(map); - } - T retract(const vector& d) { - Eigen::Map map(d.data()); - return t_ + map; - } -private: - T const & t_; -}; - // Point2 +namespace gtsam { template<> struct is_manifold : public true_type { @@ -464,6 +335,8 @@ template<> struct dimension : public integral_constant { }; +} + // is_manifold TEST(Expression, is_manifold) { EXPECT(!is_manifold::value); @@ -495,7 +368,8 @@ TEST(Expression, Charts) { EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); DefaultChart chart3(0); - Eigen::Matrix v1; v1<<1; + Eigen::Matrix v1; + v1 << 1; EXPECT(chart3.apply(1)==v1); EXPECT(chart3.retract(v1)==1); From c32d2bb3b283496be042bb2904fdac05790491b8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 18 Oct 2014 23:48:51 +0200 Subject: [PATCH 15/45] Fixed comments --- gtsam_unstable/nonlinear/Expression-inl.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 5114ac911..6a5502fd1 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -322,7 +322,7 @@ public: // C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost // and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. // to recursively generate a class, that will be the base for function nodes. -// The class generated, for two arguments A1, A2, and A3 will be +// The class generated, for three arguments A1, A2, and A3 will be // // struct Base1 : Argument, FunctionalBase { // ... storage related to A1 ... @@ -331,12 +331,12 @@ public: // // struct Base2 : Argument, Base1 { // ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A2 ... +// ... methods that work on A2 and (recursively) on A1 ... // }; // -// struct Base2 : Argument, Base2 { +// struct Base3 : Argument, Base2 { // ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... // }; // // struct FunctionalNode : Base3 { From 6e142184cc76658f767f3495e7532512ad2892ba Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 00:35:25 +0200 Subject: [PATCH 16/45] Implemented is_manifold and dimension for all types in testExpressionFactor --- gtsam/base/Manifold.h | 9 ++--- gtsam/geometry/Cal3Bundler.h | 14 +++++--- gtsam/geometry/Cal3_S2.h | 15 +++++--- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Point2.h | 17 ++++++--- gtsam/geometry/Point3.h | 16 ++++++--- gtsam/geometry/Pose2.h | 15 +++++--- gtsam/geometry/Pose3.h | 13 +++++-- gtsam/geometry/Rot3.h | 21 +++++++---- gtsam_unstable/nonlinear/Expression-inl.h | 36 ++++++++++--------- gtsam_unstable/nonlinear/Expression.h | 3 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 14 ++++---- .../nonlinear/tests/testExpression.cpp | 19 ++-------- 13 files changed, 116 insertions(+), 78 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 1eee71dfd..b8ec03402 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -53,9 +53,6 @@ struct is_manifold: public std::false_type { // dimension, can return Eigen::Dynamic (-1) if not known at compile time template struct dimension; -//: public std::integral_constant { -// BOOST_STATIC_ASSERT(is_manifold::value); -//}; // Chart is a map from T -> vector, retract is its inverse template @@ -82,7 +79,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; template<> @@ -111,7 +108,7 @@ struct is_manifold > : public std::true_typ // TODO: Could be more sophisticated using Eigen traits and SFINAE? -typedef std::integral_constant Dynamic; +typedef std::integral_constant Dynamic; template struct dimension > : public Dynamic { @@ -129,7 +126,7 @@ struct dimension > : public Dy template struct dimension > : public std::integral_constant< - size_t, M * N> { + int, M * N> { BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index e508710cd..8f321d363 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -36,8 +36,6 @@ private: double u0_, v0_; ///< image center, not a parameter to be optimized but a constant public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 3; /// @name Standard Constructors /// @{ @@ -169,6 +167,14 @@ private: /// @} - }; +}; - } // namespace gtsam +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 4e17c64f4..01cc0d916 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -36,8 +36,6 @@ private: double fx_, fy_, s_, u0_, v0_; public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 5; typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object @@ -200,12 +198,12 @@ public: /// return DOF, dimensionality of tangent space inline size_t dim() const { - return dimension; + return 5; } /// return DOF, dimensionality of tangent space static size_t Dim() { - return dimension; + return 5; } /// Given 5-dim tangent vector, create new calibration @@ -242,4 +240,13 @@ private: }; +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + + } // \ namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 86e6a9097..3df8bb97d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -303,7 +303,7 @@ public: return K_.uncalibrate(pn); } - typedef Eigen::Matrix Matrix2K; + typedef Eigen::Matrix::value> Matrix2K; /** project a point from world coordinate to the image * @param pw is a point in world coordinates diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index ccab84233..d6c7e28a2 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -33,10 +33,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point2 : public DerivedValue { -public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 2; + private: + double x_, y_; public: @@ -153,10 +152,10 @@ public: /// @{ /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 2; } /// Dimensionality of tangent space = 2 DOF - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 2; } /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } @@ -251,5 +250,13 @@ private: /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 6e5b1ea8a..151958476 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -37,11 +37,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point3 : public DerivedValue { - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 3; private: + double x_, y_, z_; public: @@ -122,10 +120,10 @@ namespace gtsam { /// @{ /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 3; } /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 3; } /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return Point3(*this + v); } @@ -244,4 +242,12 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 13773ddb4..5be9f11dd 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -36,7 +36,6 @@ namespace gtsam { class GTSAM_EXPORT Pose2 : public DerivedValue { public: - static const size_t dimension = 3; /** Pose Concept requirements */ typedef Rot2 Rotation; @@ -142,10 +141,10 @@ public: /// @{ /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 3; } /// Dimensionality of tangent space = 3 DOF - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 3; } /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose Pose2 retract(const Vector& v) const; @@ -294,6 +293,8 @@ public: */ static std::pair rotationInterval() { return std::make_pair(2, 2); } + /// @} + private: // Serialization function @@ -320,7 +321,13 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -/// @} +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 55cda05a8..d2ecee4c5 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -41,7 +41,6 @@ class Pose2; */ class GTSAM_EXPORT Pose3: public DerivedValue { public: - static const size_t dimension = 6; /** Pose Concept requirements */ typedef Rot3 Rotation; @@ -132,12 +131,12 @@ public: /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes static size_t Dim() { - return dimension; + return 6; } /// Dimensionality of the tangent space = 6 DOF size_t dim() const { - return dimension; + return 6; } /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map @@ -355,4 +354,12 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 59a09ba39..612c3c47c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,10 +59,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Rot3 : public DerivedValue { - public: - static const size_t dimension = 3; private: + #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ Quaternion quaternion_; @@ -260,10 +259,10 @@ namespace gtsam { /// @{ /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return dimension; } + static size_t Dim() { return 3; } /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return dimension; } + size_t dim() const { return 3; } /** * The method retract() is used to map from the tangent space back to the manifold. @@ -449,6 +448,8 @@ namespace gtsam { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); + /// @} + private: /** Serialization function */ @@ -478,8 +479,6 @@ namespace gtsam { }; - /// @} - /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' @@ -491,4 +490,14 @@ namespace gtsam { * @return a vector [thetax, thetay, thetaz] in radians. */ GTSAM_EXPORT std::pair RQ(const Matrix3& A); + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 6a5502fd1..0da5727c1 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -22,6 +22,8 @@ #include #include #include +#include + #include #include @@ -38,7 +40,6 @@ namespace MPL = boost::mpl::placeholders; #include // for placement new - class ExpressionFactorBinaryTest; // Forward declare for testing @@ -75,14 +76,15 @@ struct CallRecord { //----------------------------------------------------------------------------- /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians template -void handleLeafCase(const Eigen::Matrix& dTdA, +void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = jacobians.find(key); - it->second.block(0,0) += dTdA; // block makes HUGE difference + it->second.block(0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> -void handleLeafCase(const Eigen::Matrix& dTdA, +void handleLeafCase( + const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = jacobians.find(key); it->second += dTdA; @@ -98,12 +100,13 @@ void handleLeafCase(const Eigen::Matrix& d */ template class ExecutionTrace { + static const int Dim = dimension::value; enum { Constant, Leaf, Function } kind; union { Key key; - CallRecord* ptr; + CallRecord* ptr; } content; public: /// Pointer always starts out as a Constant @@ -116,7 +119,7 @@ public: content.key = key; } /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { + void setFunction(CallRecord* record) { kind = Function; content.ptr = record; } @@ -145,7 +148,7 @@ public: * *** This is the main entry point for reverseAD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ - typedef Eigen::Matrix JacobianTT; + typedef Eigen::Matrix JacobianTT; void startReverseAD(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors @@ -164,7 +167,7 @@ public: content.ptr->reverseAD(dTdA, jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) - typedef Eigen::Matrix Jacobian2T; + typedef Eigen::Matrix Jacobian2T; void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const { if (kind == Leaf) handleLeafCase(dTdA, jacobians, content.key); @@ -179,7 +182,7 @@ public: /// Primary template calls the generic Matrix reverseAD pipeline template struct Select { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD(dTdA, jacobians); @@ -189,7 +192,7 @@ struct Select { /// Partially specialized template calls the 2-dimensional output version template struct Select<2, A> { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD2(dTdA, jacobians); @@ -300,7 +303,7 @@ public: /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = T::dimension; + map[key_] = dimension::value; } /// Return value @@ -351,13 +354,13 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix type; + typedef Eigen::Matrix::value, dimension::value> type; }; /// meta-function to generate JacobianTA optional reference template struct Optional { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value, dimension::value> Jacobian; typedef boost::optional type; }; @@ -368,7 +371,7 @@ template struct FunctionalBase: ExpressionNode { static size_t const N = 0; // number of arguments - typedef CallRecord Record; + typedef CallRecord::value> Record; /// Construct an execution trace for reverse AD void trace(const Values& values, Record* record, char*& raw) const { @@ -437,7 +440,8 @@ struct GenerateFunctionalNode: Argument, Base { /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Base::Record::startReverseAD(jacobians); - Select::reverseAD(This::trace, This::dTdA, jacobians); + Select::value, A>::reverseAD(This::trace, This::dTdA, + jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process @@ -447,7 +451,7 @@ struct GenerateFunctionalNode: Argument, Base { } /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; + typedef Eigen::Matrix::value> Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD2(dFdT, jacobians); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 1ab69880e..12e101f14 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -154,7 +154,8 @@ public: template struct apply_compose { typedef T result_type; - typedef Eigen::Matrix Jacobian; + static const int Dim = dimension::value; + typedef Eigen::Matrix Jacobian; T operator()(const T& x, const T& y, boost::optional H1, boost::optional H2) const { return x.compose(y, H1, H2); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 8030165b9..66ba025d2 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -45,7 +45,7 @@ public: measurement_(measurement), expression_(expression) { if (!noiseModel) throw std::invalid_argument("ExpressionFactor: no NoiseModel."); - if (noiseModel->dim() != T::dimension) + if (noiseModel->dim() != dimension::value) throw std::invalid_argument( "ExpressionFactor was created with a NoiseModel of incorrect dimension."); noiseModel_ = noiseModel; @@ -68,7 +68,7 @@ public: #ifdef DEBUG_ExpressionFactor BOOST_FOREACH(size_t d, dimensions_) std::cout << d << " "; - std::cout << " -> " << T::dimension << "x" << augmentedCols_ << std::endl; + std::cout << " -> " << dimension::value << "x" << augmentedCols_ << std::endl; #endif } @@ -87,9 +87,9 @@ public: JacobianMap blocks; for (DenseIndex i = 0; i < size(); i++) { Matrix& Hi = H->at(i); - Hi.resize(T::dimension, dimensions_[i]); + Hi.resize(dimension::value, dimensions_[i]); Hi.setZero(); // zero out - Eigen::Block block = Hi.block(0, 0, T::dimension, + Eigen::Block block = Hi.block(0, 0, dimension::value, dimensions_[i]); blocks.insert(std::make_pair(keys_[i], block)); } @@ -105,9 +105,9 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { // Allocate memory on stack and create a view on it (saves a malloc) - double memory[T::dimension * augmentedCols_]; - Eigen::Map > // - matrix(memory, T::dimension, augmentedCols_); + double memory[dimension::value * augmentedCols_]; + Eigen::Map::value, Eigen::Dynamic> > // + matrix(memory, dimension::value, augmentedCols_); matrix.setZero(); // zero out // Construct block matrix, is of right size but un-initialized diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 45f8f3284..b830613c3 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -324,19 +324,6 @@ struct SnavelyReprojectionError { /* ************************************************************************* */ -// Point2 -namespace gtsam { - -template<> -struct is_manifold : public true_type { -}; - -template<> -struct dimension : public integral_constant { -}; - -} - // is_manifold TEST(Expression, is_manifold) { EXPECT(!is_manifold::value); @@ -506,9 +493,9 @@ TEST(Expression, AutoDiff2) { /* ************************************************************************* */ // keys TEST(Expression, SnavelyKeys) { -// Expression expression(1); -// set expected = list_of(1)(2); -// EXPECT(expected == expression.keys()); + Expression expression(1); + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); } /* ************************************************************************* */ int main() { From 768f47174bdcdb814a7fe36886b30f8229429957 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 00:50:33 +0200 Subject: [PATCH 17/45] Forgot one is_manifold/dimension --- gtsam/geometry/Cal3DS2.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 82bfa4c5f..1ebbcb132 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -46,8 +46,6 @@ protected: double p1_, p2_ ; // tangential distortion public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 9; Matrix K() const ; Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } @@ -146,11 +144,9 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable -private: - /// @} - /// @name Advanced Interface - /// @{ + +private: /** Serialization function */ friend class boost::serialization::access; @@ -170,10 +166,14 @@ private: ar & BOOST_SERIALIZATION_NVP(p2_); } - - /// @} - }; +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; } From eac76cd0f021439eff027a78497303b9c67c3168 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 11:18:36 +0200 Subject: [PATCH 18/45] Some progress on defining interface --- gtsam/geometry/PinholeCamera.h | 20 ++++++++----- .../nonlinear/tests/testExpression.cpp | 29 ++++++++++++++----- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 3df8bb97d..d39e42265 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -602,10 +602,6 @@ private: Dpi_pn * Dpn_point; } - /// @} - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template @@ -614,6 +610,16 @@ private: ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(K_); } - /// @} - } - ;} + +}; + +template +struct is_manifold > : public std::true_type { +}; + +template +struct dimension > : public std::integral_constant< + size_t, dimension::value + dimension::value> { +}; + +} diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index b830613c3..ed17d11f8 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -277,7 +278,7 @@ struct Projective { // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for // focal length and 2 for radial distortion. The principal point is not modeled // (i.e. it is assumed be located at the image center). -struct SnavelyReprojectionError { +struct SnavelyProjection { template bool operator()(const T* const camera, const T* const point, @@ -461,7 +462,7 @@ TEST(Expression, AutoDiff2) { using ceres::internal::AutoDiff; // Instantiate function - SnavelyReprojectionError snavely; + SnavelyProjection snavely; // Make arguments Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 @@ -475,9 +476,9 @@ TEST(Expression, AutoDiff2) { // Get expected derivatives Matrix E1 = numericalDerivative21( - SnavelyReprojectionError(), P, X); + SnavelyProjection(), P, X); Matrix E2 = numericalDerivative22( - SnavelyReprojectionError(), P, X); + SnavelyProjection(), P, X); // Get derivatives with AutoDiff Vector2 actual2; @@ -485,15 +486,29 @@ TEST(Expression, AutoDiff2) { double *parameters[] = { P.data(), X.data() }; double *jacobians[] = { H1.data(), H2.data() }; CHECK( - (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); + (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); EXPECT(assert_equal(E1,H1,1e-8)); EXPECT(assert_equal(E2,H2,1e-8)); } /* ************************************************************************* */ -// keys +// Adapt ceres-style autodiff +template +struct AutoDiff: Expression { + typedef boost::function Function; + AutoDiff(Function f, const Expression& e1, const Expression& e2) : + Expression(3) { + + } +}; + TEST(Expression, SnavelyKeys) { - Expression expression(1); + // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector + typedef PinholeCamera Camera; + Expression P(1); + Expression X(2); + Expression expression = // + AutoDiff(SnavelyProjection(), P, X); set expected = list_of(1)(2); EXPECT(expected == expression.keys()); } From 8ee16c90180dbad00e6596e35b944cf024167774 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 11:19:09 +0200 Subject: [PATCH 19/45] Comments for Paul --- gtsam_unstable/nonlinear/Expression-inl.h | 16 +++++++++++++++- gtsam_unstable/nonlinear/Expression.h | 5 +++++ gtsam_unstable/nonlinear/ExpressionFactor.h | 13 ++++++++++--- 3 files changed, 30 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 0da5727c1..3594ea61f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -92,11 +92,25 @@ void handleLeafCase( //----------------------------------------------------------------------------- /** - * The ExecutionTrace class records a tree-structured expression's execution + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. * It is a tagged union that obviates the need to create * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead * the key for the leaf is stored in the space normally used to store a * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution */ template class ExecutionTrace { diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 12e101f14..0e9b1034d 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -124,6 +124,11 @@ public: /// Return value and derivatives, reverse AD version T reverse(const Values& values, JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h size_t size = traceSize(); char raw[size]; ExecutionTrace trace; diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 66ba025d2..cdf2d132e 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -104,8 +104,14 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // Allocate memory on stack and create a view on it (saves a malloc) - double memory[dimension::value * augmentedCols_]; + // This method has been heavily optimized for maximum performance. + // We allocate a VerticalBlockMatrix on the stack first, and then create + // Eigen::Block views on this piece of memory which is then passed + // to [expression_.value] below, which writes directly into Ab_. + + // Another malloc saved by creating a Matrix on the stack + static const int Dim = dimension::value; + double memory[Dim * augmentedCols_]; Eigen::Map::value, Eigen::Dynamic> > // matrix(memory, dimension::value, augmentedCols_); matrix.setZero(); // zero out @@ -117,8 +123,9 @@ public: JacobianMap blocks; for (DenseIndex i = 0; i < size(); i++) blocks.insert(std::make_pair(keys_[i], Ab(i))); + // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, blocks); + T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! Ab(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now From 9a3d2747b8bea647f5d84f237b42b922280f582c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 23:13:20 +0200 Subject: [PATCH 20/45] Type of dimension::value should be int --- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Point3.h | 2 +- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/Rot3.h | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 8f321d363..dded932e8 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -174,7 +174,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 1ebbcb132..eb3bb3623 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -173,7 +173,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 01cc0d916..6f1e75bad 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -245,7 +245,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index d39e42265..02f283224 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -619,7 +619,7 @@ struct is_manifold > : public std::true_type { template struct dimension > : public std::integral_constant< - size_t, dimension::value + dimension::value> { + int, dimension::value + dimension::value> { }; } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index d6c7e28a2..ffd3c2f80 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -255,7 +255,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 151958476..b333ac1e9 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -247,7 +247,7 @@ namespace gtsam { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public std::integral_constant { }; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 5be9f11dd..13fa6aba0 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -326,7 +326,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d2ecee4c5..c5013270f 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -359,7 +359,7 @@ struct is_manifold : public std::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public std::integral_constant { }; } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 612c3c47c..eb6078ef2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -496,7 +496,7 @@ namespace gtsam { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public std::integral_constant { }; From e71f9edd37cfaf76a870c9de8f3f51318cd89414 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 23:13:30 +0200 Subject: [PATCH 21/45] dimension --- gtsam/base/LieMatrix.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 23e5fd023..ca6cf1b3f 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -174,4 +174,12 @@ private: } }; +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + } // \namespace gtsam From 63ae33088eee22b372f068eca0137c285fc56d12 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Oct 2014 23:24:41 +0200 Subject: [PATCH 22/45] Some success on the way to autodiff --- .../nonlinear/tests/testExpression.cpp | 50 ++++++++++++++++--- 1 file changed, 42 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ed17d11f8..e04f25ed2 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -493,25 +493,59 @@ TEST(Expression, AutoDiff2) { /* ************************************************************************* */ // Adapt ceres-style autodiff -template -struct AutoDiff: Expression { - typedef boost::function Function; - AutoDiff(Function f, const Expression& e1, const Expression& e2) : - Expression(3) { +template +struct AutoDiff { + static const int N = dimension::value; + static const int M1 = dimension::value; + static const int M2 = dimension::value; + + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + + Point2 operator()(const A1& a1, const A2& a2, + boost::optional H1, boost::optional H2) { + + // Instantiate function + F f; + + // Make arguments + Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 + P << 0, 0, 0, 0, 5, 0, 1, 0, 0; + Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + bool success; + Vector2 result; + + if (H1 || H2) { + + // Get derivatives with AutoDiff + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1->data(), H2->data() }; + success = ceres::internal::AutoDiff::Differentiate(f, + parameters, 2, result.data(), jacobians); + + } else { + // Apply the mapping, to get result + success = f(P.data(), X.data(), result.data()); + } + return Point2(); } }; -TEST(Expression, SnavelyKeys) { +TEST(Expression, Snavely) { // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; + Expression P(1); Expression X(2); - Expression expression = // - AutoDiff(SnavelyProjection(), P, X); +// AutoDiff f; + Expression expression( + AutoDiff(), P, X); set expected = list_of(1)(2); EXPECT(expected == expression.keys()); } + /* ************************************************************************* */ int main() { TestResult tr; From 7ebc8e969f5c4e01b5bae8233322f3d3f46d2d5e Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 09:29:45 +0200 Subject: [PATCH 23/45] Charts with default constructors --- .../nonlinear/tests/testExpression.cpp | 45 +++++++++++++------ 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index e04f25ed2..7b5824809 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -491,6 +491,11 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } +/* ************************************************************************* */ +// zero for canonical coordinates +template +struct zero; + /* ************************************************************************* */ // Adapt ceres-style autodiff template @@ -500,43 +505,55 @@ struct AutoDiff { static const int M1 = dimension::value; static const int M2 = dimension::value; + typedef DefaultChart Chart1; + typedef DefaultChart Chart2; + typedef typename Chart1::vector Vector1; + typedef typename Chart2::vector Vector2; + typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; - Point2 operator()(const A1& a1, const A2& a2, - boost::optional H1, boost::optional H2) { + T operator()(const A1& a1, const A2& a2, boost::optional H1, + boost::optional H2) { - // Instantiate function + // Instantiate function and charts + A1 z1; A2 z2; // TODO, zero + Chart1 chart1(z1); + Chart2 chart2(z2); F f; // Make arguments - Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 - P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! + Vector1 v1 = chart1.apply(a1); + Vector2 v2 = chart2.apply(a2); bool success; - Vector2 result; + double result[N]; if (H1 || H2) { // Get derivatives with AutoDiff - double *parameters[] = { P.data(), X.data() }; + double *parameters[] = { v1.data(), v2.data() }; double *jacobians[] = { H1->data(), H2->data() }; success = ceres::internal::AutoDiff::Differentiate(f, - parameters, 2, result.data(), jacobians); + parameters, 2, result, jacobians); } else { // Apply the mapping, to get result - success = f(P.data(), X.data(), result.data()); + success = f(v1.data(), v2.data(), result); } - return Point2(); + return T(result[0], result[1]); } }; -TEST(Expression, Snavely) { - // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector - typedef PinholeCamera Camera; +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; +//template <> +//zero { +// static const Camera value = Camera(); +//} + +TEST(Expression, Snavely) { Expression P(1); Expression X(2); // AutoDiff f; From 821f7761181f8e628a987b9c1973086ad1eed328 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 09:43:32 +0200 Subject: [PATCH 24/45] Wrapper works to some extent --- .../nonlinear/tests/testExpression.cpp | 66 ++++++++++++++----- 1 file changed, 48 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 7b5824809..b668fe547 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -491,11 +491,6 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } -/* ************************************************************************* */ -// zero for canonical coordinates -template -struct zero; - /* ************************************************************************* */ // Adapt ceres-style autodiff template @@ -505,19 +500,24 @@ struct AutoDiff { static const int M1 = dimension::value; static const int M2 = dimension::value; - typedef DefaultChart Chart1; - typedef DefaultChart Chart2; + typedef DefaultChart ChartT; + typedef DefaultChart Chart1; + typedef DefaultChart Chart2; + typedef typename ChartT::vector VectorT; typedef typename Chart1::vector Vector1; typedef typename Chart2::vector Vector2; typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; - T operator()(const A1& a1, const A2& a2, boost::optional H1, - boost::optional H2) { + T operator()(const A1& a1, const A2& a2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) { // Instantiate function and charts - A1 z1; A2 z2; // TODO, zero + T z; + A1 z1; + A2 z2; // TODO, zero + ChartT chartT(z); Chart1 chart1(z1); Chart2 chart2(z2); F f; @@ -525,9 +525,11 @@ struct AutoDiff { // Make arguments Vector1 v1 = chart1.apply(a1); Vector2 v2 = chart2.apply(a2); + cout << v1 << endl; + cout << v2 << endl; bool success; - double result[N]; + VectorT result; if (H1 || H2) { @@ -535,23 +537,51 @@ struct AutoDiff { double *parameters[] = { v1.data(), v2.data() }; double *jacobians[] = { H1->data(), H2->data() }; success = ceres::internal::AutoDiff::Differentiate(f, - parameters, 2, result, jacobians); + parameters, 2, result.data(), jacobians); } else { // Apply the mapping, to get result - success = f(v1.data(), v2.data(), result); + success = f(v1.data(), v2.data(), result.data()); } - return T(result[0], result[1]); + cout << result << endl; + return chartT.retract(result); } }; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; -//template <> -//zero { -// static const Camera value = Camera(); -//} +/* ************************************************************************* */ +// Test AutoDiff wrapper Snavely +TEST(Expression, AutoDiff3) { + + // Make arguments + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + AutoDiff snavely; + + // Apply the mapping, to get image point b_x. + Point2 expected(2, 1); + Point2 actual = snavely(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + +// // Get expected derivatives +// Matrix E1 = numericalDerivative21( +// SnavelyProjection(), P, X); +// Matrix E2 = numericalDerivative22( +// SnavelyProjection(), P, X); +// +// // Get derivatives with AutoDiff +// Vector2 actual2; +// MatrixRowMajor H1(2, 9), H2(2, 3); +// double *parameters[] = { P.data(), X.data() }; +// double *jacobians[] = { H1.data(), H2.data() }; +// CHECK( +// (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); +// EXPECT(assert_equal(E1,H1,1e-8)); +// EXPECT(assert_equal(E2,H2,1e-8)); +} TEST(Expression, Snavely) { Expression P(1); From 70b22150fdba4d43b9c70f778f2779389cfdf2c6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 14:21:31 +0200 Subject: [PATCH 25/45] Test vector - Cal3Bundle() focal length = 1 !! --- gtsam/geometry/tests/testCal3Bundler.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 6e62d4be0..905605b55 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -28,6 +28,13 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2,3); +/* ************************************************************************* */ +TEST( Cal3Bundler, vector) +{ + Cal3Bundler K; + CHECK(assert_equal((Vector(3)<<1,0,0),K.vector())); +} + /* ************************************************************************* */ TEST( Cal3Bundler, uncalibrate) { @@ -36,7 +43,7 @@ TEST( Cal3Bundler, uncalibrate) double g = v[0]*(1+v[1]*r+v[2]*r*r) ; Point2 expected (1000+g*p.x(), 2000+g*p.y()) ; Point2 actual = K.uncalibrate(p); - CHECK(assert_equal(actual,expected)); + CHECK(assert_equal(expected,actual)); } TEST( Cal3Bundler, calibrate ) From a423f284e9f0fab18d8d552e295a0a02d44b5b5d Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 14:23:08 +0200 Subject: [PATCH 26/45] Canonical coordinates prototype, works for Snavely --- .../nonlinear/tests/testExpression.cpp | 76 ++++++++++++++----- 1 file changed, 55 insertions(+), 21 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index b668fe547..ad25455ee 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -491,6 +491,25 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } +/* ************************************************************************* */ +// zero::value is intended to be the origin of a canonical coordinate system +// with canonical(t) == DefaultChart(zero::value).apply(t) +template struct zero; +template class Canonical { + DefaultChart chart; +public: + typedef T type; + typedef typename DefaultChart::vector vector; + Canonical() : + chart(zero::value) { + } + vector vee(const T& t) { + return chart.apply(t); + } + T hat(const vector& v) { + return chart.retract(v); + } +}; /* ************************************************************************* */ // Adapt ceres-style autodiff template @@ -500,12 +519,12 @@ struct AutoDiff { static const int M1 = dimension::value; static const int M2 = dimension::value; - typedef DefaultChart ChartT; - typedef DefaultChart Chart1; - typedef DefaultChart Chart2; - typedef typename ChartT::vector VectorT; - typedef typename Chart1::vector Vector1; - typedef typename Chart2::vector Vector2; + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical Canonical2; + typedef typename CanonicalT::vector VectorT; + typedef typename Canonical1::vector Vector1; + typedef typename Canonical2::vector Vector2; typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; @@ -513,20 +532,9 @@ struct AutoDiff { T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - // Instantiate function and charts - T z; - A1 z1; - A2 z2; // TODO, zero - ChartT chartT(z); - Chart1 chart1(z1); - Chart2 chart2(z2); - F f; - // Make arguments - Vector1 v1 = chart1.apply(a1); - Vector2 v2 = chart2.apply(a2); - cout << v1 << endl; - cout << v2 << endl; + Vector1 v1 = chart1.vee(a1); + Vector2 v2 = chart2.vee(a2); bool success; VectorT result; @@ -543,14 +551,40 @@ struct AutoDiff { // Apply the mapping, to get result success = f(v1.data(), v2.data(), result.data()); } - cout << result << endl; - return chartT.retract(result); + return chartT.hat(result); } + +private: + + // Instantiate function and charts + CanonicalT chartT; + Canonical1 chart1; + Canonical2 chart2; + F f; + }; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; +template<> +struct zero { + static const Camera value; +}; +const Camera zero::value(Camera(Pose3(),Cal3Bundler(0,0,0))); + +template<> +struct zero { + static const Point3 value; +}; +const Point3 zero::value(Point3(0,0,0)); + +template<> +struct zero { + static const Point2 value; +}; +const Point2 zero::value(Point2(0,0)); + /* ************************************************************************* */ // Test AutoDiff wrapper Snavely TEST(Expression, AutoDiff3) { From df5e584412785780e7ea9db37765dd807192d111 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 14:32:20 +0200 Subject: [PATCH 27/45] Compiles, but Jacobains not correct yet --- .../nonlinear/tests/testExpression.cpp | 62 +++++++++---------- 1 file changed, 30 insertions(+), 32 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ad25455ee..002894acd 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -513,7 +513,7 @@ public: /* ************************************************************************* */ // Adapt ceres-style autodiff template -struct AutoDiff { +class AdaptAutoDiff { static const int N = dimension::value; static const int M1 = dimension::value; @@ -522,16 +522,27 @@ struct AutoDiff { typedef Canonical CanonicalT; typedef Canonical Canonical1; typedef Canonical Canonical2; + typedef typename CanonicalT::vector VectorT; typedef typename Canonical1::vector Vector1; typedef typename Canonical2::vector Vector2; + // Instantiate function and charts + CanonicalT chartT; + Canonical1 chart1; + Canonical2 chart2; + F f; + +public: + typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { + using ceres::internal::AutoDiff; + // Make arguments Vector1 v1 = chart1.vee(a1); Vector2 v2 = chart2.vee(a2); @@ -540,13 +551,11 @@ struct AutoDiff { VectorT result; if (H1 || H2) { - // Get derivatives with AutoDiff double *parameters[] = { v1.data(), v2.data() }; double *jacobians[] = { H1->data(), H2->data() }; - success = ceres::internal::AutoDiff::Differentiate(f, - parameters, 2, result.data(), jacobians); - + success = AutoDiff::Differentiate(f, parameters, 2, + result.data(), jacobians); } else { // Apply the mapping, to get result success = f(v1.data(), v2.data(), result.data()); @@ -554,14 +563,6 @@ struct AutoDiff { return chartT.hat(result); } -private: - - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; - F f; - }; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector @@ -571,19 +572,19 @@ template<> struct zero { static const Camera value; }; -const Camera zero::value(Camera(Pose3(),Cal3Bundler(0,0,0))); +const Camera zero::value(Camera(Pose3(), Cal3Bundler(0, 0, 0))); template<> struct zero { static const Point3 value; }; -const Point3 zero::value(Point3(0,0,0)); +const Point3 zero::value(Point3(0, 0, 0)); template<> struct zero { static const Point2 value; }; -const Point2 zero::value(Point2(0,0)); +const Point2 zero::value(Point2(0, 0)); /* ************************************************************************* */ // Test AutoDiff wrapper Snavely @@ -593,7 +594,8 @@ TEST(Expression, AutoDiff3) { Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! - AutoDiff snavely; + typedef AdaptAutoDiff Adaptor; + Adaptor snavely; // Apply the mapping, to get image point b_x. Point2 expected(2, 1); @@ -601,20 +603,16 @@ TEST(Expression, AutoDiff3) { EXPECT(assert_equal(expected,actual,1e-9)); // // Get expected derivatives -// Matrix E1 = numericalDerivative21( -// SnavelyProjection(), P, X); -// Matrix E2 = numericalDerivative22( -// SnavelyProjection(), P, X); -// -// // Get derivatives with AutoDiff -// Vector2 actual2; -// MatrixRowMajor H1(2, 9), H2(2, 3); -// double *parameters[] = { P.data(), X.data() }; -// double *jacobians[] = { H1.data(), H2.data() }; -// CHECK( -// (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); -// EXPECT(assert_equal(E1,H1,1e-8)); -// EXPECT(assert_equal(E2,H2,1e-8)); + Matrix E1 = numericalDerivative21(Adaptor(), P, X); + Matrix E2 = numericalDerivative22(Adaptor(), P, X); + + // Get derivatives with AutoDiff + Matrix29 H1; + Matrix23 H2; + Point2 actual2 = snavely(P, X, H1, H2); + EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } TEST(Expression, Snavely) { @@ -622,7 +620,7 @@ TEST(Expression, Snavely) { Expression X(2); // AutoDiff f; Expression expression( - AutoDiff(), P, X); + AdaptAutoDiff(), P, X); set expected = list_of(1)(2); EXPECT(expected == expression.keys()); } From bf5580d5189a013af309dbda90c85ee65f28ab3d Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 14:39:28 +0200 Subject: [PATCH 28/45] AdaptAutoDiff now works with RowMajor Eigen matrices --- .../nonlinear/tests/testExpression.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 002894acd..11fe49dc6 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -535,8 +535,8 @@ class AdaptAutoDiff { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { @@ -606,9 +606,9 @@ TEST(Expression, AutoDiff3) { Matrix E1 = numericalDerivative21(Adaptor(), P, X); Matrix E2 = numericalDerivative22(Adaptor(), P, X); - // Get derivatives with AutoDiff - Matrix29 H1; - Matrix23 H2; + // Get derivatives with AutoDiff, not gives RowMajor results! + Eigen::Matrix H1; + Eigen::Matrix H2; Point2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expected,actual,1e-9)); EXPECT(assert_equal(E1,H1,1e-8)); @@ -616,13 +616,13 @@ TEST(Expression, AutoDiff3) { } TEST(Expression, Snavely) { - Expression P(1); - Expression X(2); -// AutoDiff f; - Expression expression( - AdaptAutoDiff(), P, X); - set expected = list_of(1)(2); - EXPECT(expected == expression.keys()); +// Expression P(1); +// Expression X(2); +//// AutoDiff f; +// Expression expression( +// AdaptAutoDiff(), P, X); +// set expected = list_of(1)(2); +// EXPECT(expected == expression.keys()); } /* ************************************************************************* */ From bce84ca4db4cc22293ff64350ba9cbc669e7a5b5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 15:38:27 +0200 Subject: [PATCH 29/45] Successfully created Expression from AutoDiff function! --- .../nonlinear/tests/testExpression.cpp | 37 +++++++++++++------ 1 file changed, 25 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 11fe49dc6..8a788b7b7 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -519,6 +519,9 @@ class AdaptAutoDiff { static const int M1 = dimension::value; static const int M2 = dimension::value; + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + typedef Canonical CanonicalT; typedef Canonical Canonical1; typedef Canonical Canonical2; @@ -535,8 +538,8 @@ class AdaptAutoDiff { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { @@ -551,15 +554,26 @@ public: VectorT result; if (H1 || H2) { + // Get derivatives with AutoDiff double *parameters[] = { v1.data(), v2.data() }; - double *jacobians[] = { H1->data(), H2->data() }; + double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double *jacobians[] = { rowMajor1, rowMajor2 }; success = AutoDiff::Differentiate(f, parameters, 2, result.data(), jacobians); + + // Convert from row-major to columnn-major + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major + *H1 = Eigen::Map(rowMajor1); + *H2 = Eigen::Map(rowMajor2); + } else { // Apply the mapping, to get result success = f(v1.data(), v2.data(), result.data()); } + if (!success) + throw std::runtime_error( + "AdaptAutoDiff: function call resulted in failure"); return chartT.hat(result); } @@ -607,8 +621,8 @@ TEST(Expression, AutoDiff3) { Matrix E2 = numericalDerivative22(Adaptor(), P, X); // Get derivatives with AutoDiff, not gives RowMajor results! - Eigen::Matrix H1; - Eigen::Matrix H2; + Matrix29 H1; + Matrix23 H2; Point2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expected,actual,1e-9)); EXPECT(assert_equal(E1,H1,1e-8)); @@ -616,13 +630,12 @@ TEST(Expression, AutoDiff3) { } TEST(Expression, Snavely) { -// Expression P(1); -// Expression X(2); -//// AutoDiff f; -// Expression expression( -// AdaptAutoDiff(), P, X); -// set expected = list_of(1)(2); -// EXPECT(expected == expression.keys()); + Expression P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); } /* ************************************************************************* */ From f39c1d72f825ffad2b1ca071b3f6f8b4cee972e3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 15:43:47 +0200 Subject: [PATCH 30/45] dimension --- gtsam/base/LieScalar.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 21a2e10ad..cb1196de0 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -111,4 +111,13 @@ namespace gtsam { private: double d_; }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public Dynamic { + }; + } // \namespace gtsam From e0841fb3e601b5bc35c7cdfe3fc3ac2d7001d7e5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Oct 2014 23:53:56 +0200 Subject: [PATCH 31/45] No more Ceres dependecy, copied relevant Ceres files here (for now) --- gtsam_unstable/nonlinear/CMakeLists.txt | 3 - gtsam_unstable/nonlinear/ceres_autodiff.h | 314 ++++++++ gtsam_unstable/nonlinear/ceres_eigen.h | 93 +++ gtsam_unstable/nonlinear/ceres_fixed_array.h | 190 +++++ gtsam_unstable/nonlinear/ceres_fpclassify.h | 87 +++ gtsam_unstable/nonlinear/ceres_jet.h | 670 ++++++++++++++++++ gtsam_unstable/nonlinear/ceres_macros.h | 170 +++++ .../nonlinear/ceres_manual_constructor.h | 208 ++++++ gtsam_unstable/nonlinear/ceres_rotation.h | 644 +++++++++++++++++ .../nonlinear/ceres_variadic_evaluate.h | 181 +++++ .../nonlinear/tests/testExpression.cpp | 4 +- 11 files changed, 2559 insertions(+), 5 deletions(-) create mode 100644 gtsam_unstable/nonlinear/ceres_autodiff.h create mode 100644 gtsam_unstable/nonlinear/ceres_eigen.h create mode 100644 gtsam_unstable/nonlinear/ceres_fixed_array.h create mode 100644 gtsam_unstable/nonlinear/ceres_fpclassify.h create mode 100644 gtsam_unstable/nonlinear/ceres_jet.h create mode 100644 gtsam_unstable/nonlinear/ceres_macros.h create mode 100644 gtsam_unstable/nonlinear/ceres_manual_constructor.h create mode 100644 gtsam_unstable/nonlinear/ceres_rotation.h create mode 100644 gtsam_unstable/nonlinear/ceres_variadic_evaluate.h diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index 85412295a..9e0cb68e1 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -2,8 +2,5 @@ file(GLOB nonlinear_headers "*.h") install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear) -FIND_PACKAGE(Ceres REQUIRED) -INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) - # Add all tests add_subdirectory(tests) diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam_unstable/nonlinear/ceres_autodiff.h new file mode 100644 index 000000000..2a0ac8987 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_autodiff.h @@ -0,0 +1,314 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template +// bool operator()(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template +// bool operator()(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian NULL pointers +// ---------------------- +// In general, the functions below will accept NULL pointers for all or some of +// the Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include + +#include +#include +#include +#include +#define DCHECK assert +#define DCHECK_GT(a,b) assert((a)>(b)) + +namespace ceres { +namespace internal { + +// Extends src by a 1st order pertubation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template +inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { + DCHECK(src); + DCHECK(dst); + for (int j = 0; j < N; ++j) { + dst[j].a = src[j]; + dst[j].v.setZero(); + dst[j].v[offset + j] = T(1.0); + } +} + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template +inline void Take0thOrderPart(int M, const JetT *src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map >(dst + N * i, N) = + src[i].v.template segment(N0); + } +} + +// This is in a struct because default template parameters on a +// function are not supported in C++03 (though it is available in +// C++0x). N0 through N5 are the dimension of the input arguments to +// the user supplied functor. +template +struct AutoDiff { + static bool Differentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T *function_value, + T **jacobians) { + // This block breaks the 80 column rule to keep it somewhat readable. + DCHECK_GT(num_outputs, 0); + DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))); + + typedef Jet JetT; + FixedArray x( + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); + + // These are the positions of the respective jets in the fixed array x. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; + const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; + const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; + + const JetT *unpacked_parameters[10] = { + x.get() + jet0, + x.get() + jet1, + x.get() + jet2, + x.get() + jet3, + x.get() + jet4, + x.get() + jet5, + x.get() + jet6, + x.get() + jet7, + x.get() + jet8, + x.get() + jet9, + }; + + JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; + +#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + internal::Make1stOrderPerturbation( \ + jet ## i, \ + parameters[i], \ + x.get() + jet ## i); \ + } + CERES_MAKE_1ST_ORDER_PERTURBATION(0); + CERES_MAKE_1ST_ORDER_PERTURBATION(1); + CERES_MAKE_1ST_ORDER_PERTURBATION(2); + CERES_MAKE_1ST_ORDER_PERTURBATION(3); + CERES_MAKE_1ST_ORDER_PERTURBATION(4); + CERES_MAKE_1ST_ORDER_PERTURBATION(5); + CERES_MAKE_1ST_ORDER_PERTURBATION(6); + CERES_MAKE_1ST_ORDER_PERTURBATION(7); + CERES_MAKE_1ST_ORDER_PERTURBATION(8); + CERES_MAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_MAKE_1ST_ORDER_PERTURBATION + + if (!VariadicEvaluate::Call( + functor, unpacked_parameters, output)) { + return false; + } + + internal::Take0thOrderPart(num_outputs, output, function_value); + +#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + if (jacobians[i]) { \ + internal::Take1stOrderPart(num_outputs, \ + output, \ + jacobians[i]); \ + } \ + } + CERES_TAKE_1ST_ORDER_PERTURBATION(0); + CERES_TAKE_1ST_ORDER_PERTURBATION(1); + CERES_TAKE_1ST_ORDER_PERTURBATION(2); + CERES_TAKE_1ST_ORDER_PERTURBATION(3); + CERES_TAKE_1ST_ORDER_PERTURBATION(4); + CERES_TAKE_1ST_ORDER_PERTURBATION(5); + CERES_TAKE_1ST_ORDER_PERTURBATION(6); + CERES_TAKE_1ST_ORDER_PERTURBATION(7); + CERES_TAKE_1ST_ORDER_PERTURBATION(8); + CERES_TAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_TAKE_1ST_ORDER_PERTURBATION + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam_unstable/nonlinear/ceres_eigen.h new file mode 100644 index 000000000..18a602cf4 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_eigen.h @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include + +namespace ceres { + +typedef Eigen::Matrix Vector; +typedef Eigen::Matrix Matrix; +typedef Eigen::Map VectorRef; +typedef Eigen::Map MatrixRef; +typedef Eigen::Map ConstVectorRef; +typedef Eigen::Map ConstMatrixRef; + +// Column major matrices for DenseSparseMatrix/DenseQRSolver +typedef Eigen::Matrix ColMajorMatrix; + +typedef Eigen::Map > ColMajorMatrixRef; + +typedef Eigen::Map > ConstColMajorMatrixRef; + + + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template +struct EigenTypes { + typedef Eigen::Matrix + Matrix; + + typedef Eigen::Map< + Eigen::Matrix > + MatrixRef; + + typedef Eigen::Matrix + Vector; + + typedef Eigen::Map < + Eigen::Matrix > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam_unstable/nonlinear/ceres_fixed_array.h new file mode 100644 index 000000000..4586fe524 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fixed_array.h @@ -0,0 +1,190 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: rennie@google.com (Jeffrey Rennie) +// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include +#include +#include +#include + +namespace ceres { +namespace internal { + +// A FixedArray represents a non-resizable array of T where the +// length of the array does not need to be a compile time constant. +// +// FixedArray allocates small arrays inline, and large arrays on +// the heap. It is a good replacement for non-standard and deprecated +// uses of alloca() and variable length arrays (a GCC extension). +// +// FixedArray keeps performance fast for small arrays, because it +// avoids heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. +// +// Also, FixedArray is useful for writing portable code. Not all +// compilers support arrays of dynamic size. + +// Most users should not specify an inline_elements argument and let +// FixedArray<> automatically determine the number of elements +// to store inline based on sizeof(T). +// +// If inline_elements is specified, the FixedArray<> implementation +// will store arrays of length <= inline_elements inline. +// +// Finally note that unlike vector FixedArray will not zero-initialize +// simple types like int, double, bool, etc. +// +// Non-POD types will be default-initialized just like regular vectors or +// arrays. + +#if defined(_WIN64) + typedef __int64 ssize_t; +#elif defined(_WIN32) + typedef __int32 ssize_t; +#endif + +template +class FixedArray { + public: + // For playing nicely with stl: + typedef T value_type; + typedef T* iterator; + typedef T const* const_iterator; + typedef T& reference; + typedef T const& const_reference; + typedef T* pointer; + typedef std::ptrdiff_t difference_type; + typedef size_t size_type; + + // REQUIRES: n >= 0 + // Creates an array object that can store "n" elements. + // + // FixedArray will not zero-initialiaze POD (simple) types like int, + // double, bool, etc. + // Non-POD types will be default-initialized just like regular vectors or + // arrays. + explicit FixedArray(size_type n); + + // Releases any resources. + ~FixedArray(); + + // Returns the length of the array. + inline size_type size() const { return size_; } + + // Returns the memory size of the array in bytes. + inline size_t memsize() const { return size_ * sizeof(T); } + + // Returns a pointer to the underlying element array. + inline const T* get() const { return &array_[0].element; } + inline T* get() { return &array_[0].element; } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline T& operator[](size_type i) { + DCHECK_LT(i, size_); + return array_[i].element; + } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline const T& operator[](size_type i) const { + DCHECK_LT(i, size_); + return array_[i].element; + } + + inline iterator begin() { return &array_[0].element; } + inline iterator end() { return &array_[size_].element; } + + inline const_iterator begin() const { return &array_[0].element; } + inline const_iterator end() const { return &array_[size_].element; } + + private: + // Container to hold elements of type T. This is necessary to handle + // the case where T is a a (C-style) array. The size of InnerContainer + // and T must be the same, otherwise callers' assumptions about use + // of this code will be broken. + struct InnerContainer { + T element; + }; + + // How many elements should we store inline? + // a. If not specified, use a default of 256 bytes (256 bytes + // seems small enough to not cause stack overflow or unnecessary + // stack pollution, while still allowing stack allocation for + // reasonably long character arrays. + // b. Never use 0 length arrays (not ISO C++) + static const size_type S1 = ((inline_elements < 0) + ? (256/sizeof(T)) : inline_elements); + static const size_type S2 = (S1 <= 0) ? 1 : S1; + static const size_type kInlineElements = S2; + + size_type const size_; + InnerContainer* const array_; + + // Allocate some space, not an array of elements of type T, so that we can + // skip calling the T constructors and destructors for space we never use. + ManualConstructor inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template +inline FixedArray::FixedArray(typename FixedArray::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast(inline_space_) + : new InnerContainer[n])) { + // Construct only the elements actually used. + if (array_ == reinterpret_cast(inline_space_)) { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template +inline FixedArray::~FixedArray() { + if (array_ != reinterpret_cast(inline_space_)) { + delete[] array_; + } else { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Destroy(); + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam_unstable/nonlinear/ceres_fpclassify.h new file mode 100644 index 000000000..da8a4d086 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fpclassify.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable floating point classification. The names are picked such that they +// do not collide with macros. For example, "isnan" in C99 is a macro and hence +// does not respect namespaces. +// +// TODO(keir): Finish porting! + +#ifndef CERES_PUBLIC_FPCLASSIFY_H_ +#define CERES_PUBLIC_FPCLASSIFY_H_ + +#if defined(_MSC_VER) +#include +#endif + +#include + +namespace ceres { + +#if defined(_MSC_VER) + +inline bool IsFinite (double x) { return _finite(x) != 0; } +inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; } +inline bool IsNaN (double x) { return _isnan(x) != 0; } +inline bool IsNormal (double x) { + int classification = _fpclass(x); + return classification == _FPCLASS_NN || + classification == _FPCLASS_PN; +} + +#elif defined(ANDROID) && defined(_STLPORT_VERSION) + +// On Android, when using the STLPort, the C++ isnan and isnormal functions +// are defined as macros. +inline bool IsNaN (double x) { return isnan(x); } +inline bool IsNormal (double x) { return isnormal(x); } +// On Android NDK r6, when using STLPort, the isinf and isfinite functions are +// not available, so reimplement them. +inline bool IsInfinite(double x) { + return x == std::numeric_limits::infinity() || + x == -std::numeric_limits::infinity(); +} +inline bool IsFinite(double x) { + return !isnan(x) && !IsInfinite(x); +} + +# else + +// These definitions are for the normal Unix suspects. +inline bool IsFinite (double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN (double x) { return std::isnan(x); } +inline bool IsNormal (double x) { return std::isnormal(x); } + +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_FPCLASSIFY_H_ diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam_unstable/nonlinear/ceres_jet.h new file mode 100644 index 000000000..ed4834caf --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_jet.h @@ -0,0 +1,670 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, augument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet x(0); // Pick the 0th dual number for x. +// Jet y(1); // Pick the 1st dual number for y. +// Jet z = f(x, y); +// +// LOG(INFO) << "df/dx = " << z.a[0] +// << "df/dy = " << z.a[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the pertubation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include +#include +#include // NOLINT +#include +#include + +#include +#include + +namespace ceres { + +template +struct Jet { + enum { DIMENSION = N }; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() { + v.setZero(); + } + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setZero(); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setZero(); + v[k] = T(1.0); + } + + // Constructor from scalar and vector part + // The use of Eigen::DenseBase allows Eigen expressions + // to be passed in without being fully evaluated until + // they are assigned to v + template + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase &v) + : a(a), v(v) { + } + + // Compound operators + Jet& operator+=(const Jet &y) { + *this = *this + y; + return *this; + } + + Jet& operator-=(const Jet &y) { + *this = *this - y; + return *this; + } + + Jet& operator*=(const Jet &y) { + *this = *this * y; + return *this; + } + + Jet& operator/=(const Jet &y) { + *this = *this / y; + return *this; + } + + // The scalar part. + T a; + + // The infinitesimal part. + // + // Note the Eigen::DontAlign bit is needed here because this object + // gets allocated on the stack and as part of other arrays and + // structs. Forcing the right alignment there is the source of much + // pain and suffering. Even if that works, passing Jets around to + // functions by value has problems because the C++ ABI does not + // guarantee alignment for function arguments. + // + // Setting the DontAlign bit prevents Eigen from using SSE for the + // various operations on Jets. This is a small performance penalty + // since the AutoDiff code will still expose much of the code as + // statically sized loops to the compiler. But given the subtle + // issues that arise due to alignment, especially when dealing with + // multiple platforms, it seems to be a trade off worth making. + Eigen::Matrix v; +}; + +// Unary + +template inline +Jet const& operator+(const Jet& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template inline +Jet operator-(const Jet&f) { + return Jet(-f.a, -f.v); +} + +// Binary + +template inline +Jet operator+(const Jet& f, + const Jet& g) { + return Jet(f.a + g.a, f.v + g.v); +} + +// Binary + with a scalar: x + s +template inline +Jet operator+(const Jet& f, T s) { + return Jet(f.a + s, f.v); +} + +// Binary + with a scalar: s + x +template inline +Jet operator+(T s, const Jet& f) { + return Jet(f.a + s, f.v); +} + +// Binary - +template inline +Jet operator-(const Jet& f, + const Jet& g) { + return Jet(f.a - g.a, f.v - g.v); +} + +// Binary - with a scalar: x - s +template inline +Jet operator-(const Jet& f, T s) { + return Jet(f.a - s, f.v); +} + +// Binary - with a scalar: s - x +template inline +Jet operator-(T s, const Jet& f) { + return Jet(s - f.a, -f.v); +} + +// Binary * +template inline +Jet operator*(const Jet& f, + const Jet& g) { + return Jet(f.a * g.a, f.a * g.v + f.v * g.a); +} + +// Binary * with a scalar: x * s +template inline +Jet operator*(const Jet& f, T s) { + return Jet(f.a * s, f.v * s); +} + +// Binary * with a scalar: s * x +template inline +Jet operator*(T s, const Jet& f) { + return Jet(f.a * s, f.v * s); +} + +// Binary / +template inline +Jet operator/(const Jet& f, + const Jet& g) { + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + const T g_a_inverse = T(1.0) / g.a; + const T f_a_by_g_a = f.a * g_a_inverse; + return Jet(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); +} + +// Binary / with a scalar: s / x +template inline +Jet operator/(T s, const Jet& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + return Jet(s / g.a, g.v * minus_s_g_a_inverse2); +} + +// Binary / with a scalar: x / s +template inline +Jet operator/(const Jet& f, T s) { + const T s_inverse = 1.0 / s; + return Jet(f.a * s_inverse, f.v * s_inverse); +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template inline \ +bool operator op(const Jet& f, const Jet& g) { \ + return f.a op g.a; \ +} \ +template inline \ +bool operator op(const T& s, const Jet& g) { \ + return s op g.a; \ +} \ +template inline \ +bool operator op(const Jet& f, const T& s) { \ + return f.a op s; \ +} +CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +// +// TODO(keir): Switch to "using". +inline double abs (double x) { return std::abs(x); } +inline double log (double x) { return std::log(x); } +inline double exp (double x) { return std::exp(x); } +inline double sqrt (double x) { return std::sqrt(x); } +inline double cos (double x) { return std::cos(x); } +inline double acos (double x) { return std::acos(x); } +inline double sin (double x) { return std::sin(x); } +inline double asin (double x) { return std::asin(x); } +inline double tan (double x) { return std::tan(x); } +inline double atan (double x) { return std::atan(x); } +inline double sinh (double x) { return std::sinh(x); } +inline double cosh (double x) { return std::cosh(x); } +inline double tanh (double x) { return std::tanh(x); } +inline double pow (double x, double y) { return std::pow(x, y); } +inline double atan2(double y, double x) { return std::atan2(y, x); } + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= x + h or -(x + h) +template inline +Jet abs(const Jet& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template inline +Jet log(const Jet& f) { + const T a_inverse = T(1.0) / f.a; + return Jet(log(f.a), f.v * a_inverse); +} + +// exp(a + h) ~= exp(a) + exp(a) h +template inline +Jet exp(const Jet& f) { + const T tmp = exp(f.a); + return Jet(tmp, tmp * f.v); +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template inline +Jet sqrt(const Jet& f) { + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet(tmp, f.v * two_a_inverse); +} + +// cos(a + h) ~= cos(a) - sin(a) h +template inline +Jet cos(const Jet& f) { + return Jet(cos(f.a), - sin(f.a) * f.v); +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template inline +Jet acos(const Jet& f) { + const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(acos(f.a), tmp * f.v); +} + +// sin(a + h) ~= sin(a) + cos(a) h +template inline +Jet sin(const Jet& f) { + return Jet(sin(f.a), cos(f.a) * f.v); +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template inline +Jet asin(const Jet& f) { + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(asin(f.a), tmp * f.v); +} + +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template inline +Jet tan(const Jet& f) { + const T tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + return Jet(tan_a, tmp * f.v); +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template inline +Jet atan(const Jet& f) { + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + return Jet(atan(f.a), tmp * f.v); +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template inline +Jet sinh(const Jet& f) { + return Jet(sinh(f.a), cosh(f.a) * f.v); +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template inline +Jet cosh(const Jet& f) { + return Jet(cosh(f.a), sinh(f.a) * f.v); +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template inline +Jet tanh(const Jet& f) { + const T tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + return Jet(tanh_a, tmp * f.v); +} + +// Jet Classification. It is not clear what the appropriate semantics are for +// these classifications. This picks that IsFinite and isnormal are "all" +// operations, i.e. all elements of the jet must be finite for the jet itself +// to be finite (or normal). For IsNaN and IsInfinite, the answer is less +// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any +// part of a jet is nan or inf, then the entire jet is nan or inf. This leads +// to strange situations like a jet can be both IsInfinite and IsNaN, but in +// practice the "any" semantics are the most useful for e.g. checking that +// derivatives are sane. + +// The jet is finite if all parts of the jet are finite. +template inline +bool IsFinite(const Jet& f) { + if (!IsFinite(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsFinite(f.v[i])) { + return false; + } + } + return true; +} + +// The jet is infinite if any part of the jet is infinite. +template inline +bool IsInfinite(const Jet& f) { + if (IsInfinite(f.a)) { + return true; + } + for (int i = 0; i < N; i++) { + if (IsInfinite(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is NaN if any part of the jet is NaN. +template inline +bool IsNaN(const Jet& f) { + if (IsNaN(f.a)) { + return true; + } + for (int i = 0; i < N; ++i) { + if (IsNaN(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is normal if all parts of the jet are normal. +template inline +bool IsNormal(const Jet& f) { + if (!IsNormal(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsNormal(f.v[i])) { + return false; + } + } + return true; +} + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template inline +Jet atan2(const Jet& g, const Jet& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); + return Jet(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); +} + + +// pow -- base is a differentiable function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template inline +Jet pow(const Jet& f, double g) { + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet(pow(f.a, g), tmp * f.v); +} + +// pow -- base is a constant, exponent is a differentiable function. +// (a)^(p+dp) ~= a^p + a^p log(a) dp +template inline +Jet pow(double f, const Jet& g) { + T const tmp = pow(f, g.a); + return Jet(tmp, log(f) * tmp * g.v); +} + + +// pow -- both base and exponent are differentiable functions. +// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db +template inline +Jet pow(const Jet& f, const Jet& g) { + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + + return Jet(tmp1, tmp2 * f.v + tmp3 * g.v); +} + +// Define the helper functions Eigen needs to embed Jet types. +// +// NOTE(keir): machine_epsilon() and precision() are missing, because they don't +// work with nested template types (e.g. where the scalar is itself templated). +// Among other things, this means that decompositions of Jet's does not work, +// for example +// +// Matrix ... > A, x, b; +// ... +// A.solve(b, &x) +// +// does not work and will fail with a strange compiler error. +// +// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we +// switch to 3.0, also add the rest of the specialization functionality. +template inline const Jet& ei_conj(const Jet& x) { return x; } // NOLINT +template inline const Jet& ei_real(const Jet& x) { return x; } // NOLINT +template inline Jet ei_imag(const Jet& ) { return Jet(0.0); } // NOLINT +template inline Jet ei_abs (const Jet& x) { return fabs(x); } // NOLINT +template inline Jet ei_abs2(const Jet& x) { return x * x; } // NOLINT +template inline Jet ei_sqrt(const Jet& x) { return sqrt(x); } // NOLINT +template inline Jet ei_exp (const Jet& x) { return exp(x); } // NOLINT +template inline Jet ei_log (const Jet& x) { return log(x); } // NOLINT +template inline Jet ei_sin (const Jet& x) { return sin(x); } // NOLINT +template inline Jet ei_cos (const Jet& x) { return cos(x); } // NOLINT +template inline Jet ei_tan (const Jet& x) { return tan(x); } // NOLINT +template inline Jet ei_atan(const Jet& x) { return atan(x); } // NOLINT +template inline Jet ei_sinh(const Jet& x) { return sinh(x); } // NOLINT +template inline Jet ei_cosh(const Jet& x) { return cosh(x); } // NOLINT +template inline Jet ei_tanh(const Jet& x) { return tanh(x); } // NOLINT +template inline Jet ei_pow (const Jet& x, Jet y) { return pow(x, y); } // NOLINT + +// Note: This has to be in the ceres namespace for argument dependent lookup to +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template +inline std::ostream &operator<<(std::ostream &s, const Jet& z) { + return s << "[" << z.a << " ; " << z.v.transpose() << "]"; +} + +} // namespace ceres + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template +struct NumTraits > { + typedef ceres::Jet Real; + typedef ceres::Jet NonInteger; + typedef ceres::Jet Nested; + + static typename ceres::Jet dummy_precision() { + return ceres::Jet(1e-12); + } + + static inline Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1, + RequireInitialization = 1 + }; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam_unstable/nonlinear/ceres_macros.h new file mode 100644 index 000000000..1ed55be6e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_macros.h @@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Various Google-specific macros. +// +// This code is compiled directly on many platforms, including client +// platforms like Windows, Mac, and embedded systems. Before making +// any changes here, make sure that you're not breaking any platforms. + +#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ +#define CERES_PUBLIC_INTERNAL_MACROS_H_ + +#include // For size_t. + +// A macro to disallow the copy constructor and operator= functions +// This should be used in the private: declarations for a class +// +// For disallowing only assign or copy, write the code directly, but declare +// the intend in a comment, for example: +// +// void operator=(const TypeName&); // _DISALLOW_ASSIGN + +// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY +// are broken semantically, one should either use disallow both or +// neither. Try to avoid these in new code. +#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +// A macro to disallow all the implicit constructors, namely the +// default constructor, copy constructor and operator= functions. +// +// This should be used in the private: declarations for a class +// that wants to prevent anyone from instantiating it. This is +// especially useful for classes containing only static methods. +#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ + TypeName(); \ + CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) + +// The arraysize(arr) macro returns the # of elements in an array arr. +// The expression is a compile-time constant, and therefore can be +// used in defining new arrays, for example. If you use arraysize on +// a pointer by mistake, you will get a compile-time error. +// +// One caveat is that arraysize() doesn't accept any array of an +// anonymous type or a type defined inside a function. In these rare +// cases, you have to use the unsafe ARRAYSIZE() macro below. This is +// due to a limitation in C++'s template system. The limitation might +// eventually be removed, but it hasn't happened yet. + +// This template function declaration is used in defining arraysize. +// Note that the function doesn't need an implementation, as we only +// use its type. +template +char (&ArraySizeHelper(T (&array)[N]))[N]; + +// That gcc wants both of these prototypes seems mysterious. VC, for +// its part, can't decide which to use (another mystery). Matching of +// template overloads: the final frontier. +#ifndef _WIN32 +template +char (&ArraySizeHelper(const T (&array)[N]))[N]; +#endif + +#define arraysize(array) (sizeof(ArraySizeHelper(array))) + +// ARRAYSIZE performs essentially the same calculation as arraysize, +// but can be used on anonymous types or types defined inside +// functions. It's less safe than arraysize as it accepts some +// (although not all) pointers. Therefore, you should use arraysize +// whenever possible. +// +// The expression ARRAYSIZE(a) is a compile-time constant of type +// size_t. +// +// ARRAYSIZE catches a few type errors. If you see a compiler error +// +// "warning: division by zero in ..." +// +// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. +// You should only use ARRAYSIZE on statically allocated arrays. +// +// The following comments are on the implementation details, and can +// be ignored by the users. +// +// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in +// the array) and sizeof(*(arr)) (the # of bytes in one array +// element). If the former is divisible by the latter, perhaps arr is +// indeed an array, in which case the division result is the # of +// elements in the array. Otherwise, arr cannot possibly be an array, +// and we generate a compiler error to prevent the code from +// compiling. +// +// Since the size of bool is implementation-defined, we need to cast +// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final +// result has type size_t. +// +// This macro is not perfect as it wrongfully accepts certain +// pointers, namely where the pointer size is divisible by the pointee +// size. Since all our code has to go through a 32-bit compiler, +// where a pointer is 4 bytes, this means all pointers to a type whose +// size is 3 or greater than 4 will be (righteously) rejected. +// +// Kudos to Jorg Brown for this simple and elegant implementation. +// +// - wan 2005-11-16 +// +// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, +// the definition comes from the over-broad windows.h header that +// introduces a macro, ERROR, that conflicts with the logging framework +// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. +#define CERES_ARRAYSIZE(a) \ + ((sizeof(a) / sizeof(*(a))) / \ + static_cast(!(sizeof(a) % sizeof(*(a))))) + +// Tell the compiler to warn about unused return values for functions +// declared with this macro. The macro should be used on function +// declarations following the argument list: +// +// Sprocket* AllocateSprocket() MUST_USE_RESULT; +// +#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ + && !defined(COMPILER_ICC) +#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result)) +#else +#define CERES_MUST_USE_RESULT +#endif + +// Platform independent macros to get aligned memory allocations. +// For example +// +// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16); +// +// Gives us an instance of MyFoo which is aligned at a 16 byte +// boundary. +#if defined(_MSC_VER) +#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n)) +#define CERES_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n))) +#define CERES_ALIGN_OF(T) __alignof(T) +#endif + +#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam_unstable/nonlinear/ceres_manual_constructor.h new file mode 100644 index 000000000..7ea723d2a --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_manual_constructor.h @@ -0,0 +1,208 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kenton@google.com (Kenton Varda) +// +// ManualConstructor statically-allocates space in which to store some +// object, but does not initialize it. You can then call the constructor +// and destructor for the object yourself as you see fit. This is useful +// for memory management optimizations, where you want to initialize and +// destroy an object multiple times but only allocate it once. +// +// (When I say ManualConstructor statically allocates space, I mean that +// the ManualConstructor object itself is forced to be the right size.) + +#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ +#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ + +#include + +namespace ceres { +namespace internal { + +// ------- Define CERES_ALIGNED_CHAR_ARRAY -------------------------------- + +#ifndef CERES_ALIGNED_CHAR_ARRAY + +// Because MSVC and older GCCs require that the argument to their alignment +// construct to be a literal constant integer, we use a template instantiated +// at all the possible powers of two. +template struct AlignType { }; +template struct AlignType<0, size> { typedef char result[size]; }; + +#if !defined(CERES_ALIGN_ATTRIBUTE) +#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler +#else // !defined(CERES_ALIGN_ATTRIBUTE) + +#define CERES_ALIGN_TYPE_TEMPLATE(X) \ + template struct AlignType { \ + typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +CERES_ALIGN_TYPE_TEMPLATE(1); +CERES_ALIGN_TYPE_TEMPLATE(2); +CERES_ALIGN_TYPE_TEMPLATE(4); +CERES_ALIGN_TYPE_TEMPLATE(8); +CERES_ALIGN_TYPE_TEMPLATE(16); +CERES_ALIGN_TYPE_TEMPLATE(32); +CERES_ALIGN_TYPE_TEMPLATE(64); +CERES_ALIGN_TYPE_TEMPLATE(128); +CERES_ALIGN_TYPE_TEMPLATE(256); +CERES_ALIGN_TYPE_TEMPLATE(512); +CERES_ALIGN_TYPE_TEMPLATE(1024); +CERES_ALIGN_TYPE_TEMPLATE(2048); +CERES_ALIGN_TYPE_TEMPLATE(4096); +CERES_ALIGN_TYPE_TEMPLATE(8192); +// Any larger and MSVC++ will complain. + +#undef CERES_ALIGN_TYPE_TEMPLATE + +#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType::result + +#endif // !defined(CERES_ALIGN_ATTRIBUTE) + +#endif // CERES_ALIGNED_CHAR_ARRAY + +template +class ManualConstructor { + public: + // No constructor or destructor because one of the most useful uses of + // this class is as part of a union, and members of a union cannot have + // constructors or destructors. And, anyway, the whole point of this + // class is to bypass these. + + inline Type* get() { + return reinterpret_cast(space_); + } + inline const Type* get() const { + return reinterpret_cast(space_); + } + + inline Type* operator->() { return get(); } + inline const Type* operator->() const { return get(); } + + inline Type& operator*() { return *get(); } + inline const Type& operator*() const { return *get(); } + + // This is needed to get around the strict aliasing warning GCC generates. + inline void* space() { + return reinterpret_cast(space_); + } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space()) Type; + } + + template + inline void Init(const T1& p1) { + new(space()) Type(p1); + } + + template + inline void Init(const T1& p1, const T2& p2) { + new(space()) Type(p1, p2); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space()) Type(p1, p2, p3); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space()) Type(p1, p2, p3, p4); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5) { + new(space()) Type(p1, p2, p3, p4, p5); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6) { + new(space()) Type(p1, p2, p3, p4, p5, p6); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10, const T11& p11) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); + } + + inline void Destroy() { + get()->~Type(); + } + + private: + CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_; +}; + +#undef CERES_ALIGNED_CHAR_ARRAY + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam_unstable/nonlinear/ceres_rotation.h new file mode 100644 index 000000000..896761296 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_rotation.h @@ -0,0 +1,644 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include +#include + +namespace ceres { + +// Trivial wrapper to index linear arrays as matrices, given a fixed +// column and row stride. When an array "T* array" is wrapped by a +// +// (const) MatrixAdapter M" +// +// the expression M(i, j) is equivalent to +// +// arrary[i * row_stride + j * col_stride] +// +// Conversion functions to and from rotation matrices accept +// MatrixAdapters to permit using row-major and column-major layouts, +// and rotation matrices embedded in larger matrices (such as a 3x4 +// projection matrix). +template +struct MatrixAdapter; + +// Convenience functions to create a MatrixAdapter that treats the +// array pointed to by "pointer" as a 3x3 (contiguous) column-major or +// row-major matrix. +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer); + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer); + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implemention may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template +void RotationMatrixToAngleAxis(const T* R, T* angle_axis); + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, + T* angle_axis); + +template +void AngleAxisToRotationMatrix(const T* angle_axis, T* R); + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// The rotation matrix is row-major. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +template inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& R); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +template inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToRotation( + const T q[4], + const MatrixAdapter& R); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +template inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +template inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +template inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +template +struct MatrixAdapter { + T* pointer_; + explicit MatrixAdapter(T* pointer) + : pointer_(pointer) + {} + + T& operator()(int r, int c) const { + return pointer_[r * row_stride + c * col_stride]; + } +}; + +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T& a0 = angle_axis[0]; + const T& a1 = angle_axis[1]; + const T& a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared_theta > T(0.0)) { + const T sin_theta = sqrt(sin_squared_theta); + const T& cos_theta = quaternion[0]; + + // If cos_theta is negative, theta is greater than pi/2, which + // means that angle for the angle_axis vector which is 2 * theta + // would be greater than pi. + // + // While this will result in the correct rotation, it does not + // result in a normalized angle-axis vector. + // + // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, + // which is equivalent saying + // + // theta - pi = atan(sin(theta - pi), cos(theta - pi)) + // = atan(-sin(theta), -cos(theta)) + // + const T two_theta = + T(2.0) * ((cos_theta < 0.0) + ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); + const T k = two_theta / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template +inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { + RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); +} + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, + T* angle_axis) { + // x = k * 2 * sin(theta), where k is the axis of rotation. + angle_axis[0] = R(2, 1) - R(1, 2); + angle_axis[1] = R(0, 2) - R(2, 0); + angle_axis[2] = R(1, 0) - R(0, 1); + + static const T kOne = T(1.0); + static const T kTwo = T(2.0); + + // Since the right hand side may give numbers just above 1.0 or + // below -1.0 leading to atan misbehaving, we threshold. + T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo, + T(-1.0)), + kOne); + + // sqrt is guaranteed to give non-negative results, so we only + // threshold above. + T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + + angle_axis[1] * angle_axis[1] + + angle_axis[2] * angle_axis[2]) / kTwo, + kOne); + + // Use the arctan2 to get the right sign on theta + const T theta = atan2(sintheta, costheta); + + // Case 1: sin(theta) is large enough, so dividing by it is not a + // problem. We do not use abs here, because while jets.h imports + // std::abs into the namespace, here in this file, abs resolves to + // the int version of the function, which returns zero always. + // + // We use a threshold much larger then the machine epsilon, because + // if sin(theta) is small, not only do we risk overflow but even if + // that does not occur, just dividing by a small number will result + // in numerical garbage. So we play it safe. + static const double kThreshold = 1e-12; + if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { + const T r = theta / (kTwo * sintheta); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= r; + } + return; + } + + // Case 2: theta ~ 0, means sin(theta) ~ theta to a good + // approximation. + if (costheta > 0.0) { + const T kHalf = T(0.5); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= kHalf; + } + return; + } + + // Case 3: theta ~ pi, this is the hard case. Since theta is large, + // and sin(theta) is small. Dividing by theta by sin(theta) will + // either give an overflow or worse still numerically meaningless + // results. Thus we use an alternate more complicated formula + // here. + + // Since cos(theta) is negative, division by (1-cos(theta)) cannot + // overflow. + const T inv_one_minus_costheta = kOne / (kOne - costheta); + + // We now compute the absolute value of coordinates of the axis + // vector using the diagonal entries of R. To resolve the sign of + // these entries, we compare the sign of angle_axis[i]*sin(theta) + // with the sign of sin(theta). If they are the same, then + // angle_axis[i] should be positive, otherwise negative. + for (int i = 0; i < 3; ++i) { + angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta); + if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || + ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { + angle_axis[i] = -angle_axis[i]; + } + } +} + +template +inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { + AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); +} + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + R(0, 0) = costheta + wx*wx*(kOne - costheta); + R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); + R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); + R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; + R(1, 1) = costheta + wy*wy*(kOne - costheta); + R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); + R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); + R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); + R(2, 2) = costheta + wz*wz*(kOne - costheta); + } else { + // Near zero, we switch to using the first order Taylor expansion. + R(0, 0) = kOne; + R(1, 0) = angle_axis[2]; + R(2, 0) = -angle_axis[1]; + R(0, 1) = -angle_axis[2]; + R(1, 1) = kOne; + R(2, 1) = angle_axis[0]; + R(0, 2) = angle_axis[1]; + R(1, 2) = -angle_axis[0]; + R(2, 2) = kOne; + } +} + +template +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride_parameter, + T* R) { + DCHECK(row_stride_parameter==3); + EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); +} + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& R) { + const double kPi = 3.14159265358979323846; + const T degrees_to_radians(kPi / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + R(0, 0) = c1*c2; + R(0, 1) = -s1*c3 + c1*s2*s3; + R(0, 2) = s1*s3 + c1*s2*c3; + + R(1, 0) = s1*c2; + R(1, 1) = c1*c3 + s1*s2*s3; + R(1, 2) = -c1*s3 + s1*s2*c3; + + R(2, 0) = -s2; + R(2, 1) = c2*s3; + R(2, 2) = c2*c3; +} + +template inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& R) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT +} + +template inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToRotation(const T q[4], + const MatrixAdapter& R) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } +} + +template inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[0] * q[1]; + const T t3 = q[0] * q[2]; + const T t4 = q[0] * q[3]; + const T t5 = -q[1] * q[1]; + const T t6 = q[1] * q[2]; + const T t7 = q[1] * q[3]; + const T t8 = -q[2] * q[2]; + const T t9 = q[2] * q[3]; + const T t1 = -q[3] * q[3]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT +} + +template inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +// xy = x cross y; +template inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template inline +T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T theta_inverse = 1.0 / theta; + + const T w[3] = { angle_axis[0] * theta_inverse, + angle_axis[1] * theta_inverse, + angle_axis[2] * theta_inverse }; + + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], + w[2] * pt[0] - w[0] * pt[2], + w[0] * pt[1] - w[1] * pt[0] }; + const T tmp = + (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); + + result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; + result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; + result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion near zero provides meaningful + // derivatives when evaluated using Jets. + // + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], + angle_axis[2] * pt[0] - angle_axis[0] * pt[2], + angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; + + result[0] = pt[0] + w_cross_pt[0]; + result[1] = pt[1] + w_cross_pt[1]; + result[2] = pt[2] + w_cross_pt[2]; + } +} + +} // namespace ceres + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h new file mode 100644 index 000000000..7d22fe22e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h @@ -0,0 +1,181 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ +#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ + +#include + +#include +#include +#include + +namespace ceres { +namespace internal { + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + input[9], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 8a788b7b7..d8aa80535 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -25,8 +25,8 @@ #include #include -#include "ceres/ceres.h" -#include "ceres/rotation.h" +#include +#include #undef CHECK #include From e60ad9d2b22f978ae72d5db7c5aeb85d4ebd7b28 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 01:25:47 +0200 Subject: [PATCH 32/45] Re-factor traits, has its own namespace now, as well is_group and zero --- gtsam/base/LieMatrix.h | 6 ++ gtsam/base/LieScalar.h | 5 ++ gtsam/base/Manifold.h | 127 +++++++++++++++++++++++---------- gtsam/geometry/Cal3Bundler.h | 12 ++++ gtsam/geometry/Cal3DS2.h | 6 ++ gtsam/geometry/Cal3_S2.h | 9 +++ gtsam/geometry/PinholeCamera.h | 15 +++- gtsam/geometry/Point2.h | 9 +++ gtsam/geometry/Point3.h | 8 +++ gtsam/geometry/Pose2.h | 5 ++ gtsam/geometry/Pose3.h | 9 +++ gtsam/geometry/Rot3.h | 9 ++- 12 files changed, 181 insertions(+), 39 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index ca6cf1b3f..2a8d4bc41 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -174,6 +174,10 @@ private: } }; + +// Define GTSAM traits +namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -182,4 +186,6 @@ template<> struct dimension : public Dynamic { }; +} + } // \namespace gtsam diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index cb1196de0..2ed81b1df 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -112,6 +112,9 @@ namespace gtsam { double d_; }; + // Define GTSAM traits + namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -120,4 +123,6 @@ namespace gtsam { struct dimension : public Dynamic { }; + } + } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index b8ec03402..4bea1c919 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -13,14 +13,15 @@ * @file Manifold.h * @brief Base class and basic functions for Manifold types * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once -#include #include #include #include +#include namespace gtsam { @@ -45,6 +46,21 @@ namespace gtsam { // Traits, same style as Boost.TypeTraits // All meta-functions below ever only declare a single type // or a type/value/value_type +namespace traits { + +// is group, by default this is false +template +struct is_group: public std::false_type { +}; + +// identity, no default provided, by default given by default constructor +template +struct identity { + static T value() { + return T(); + } +}; + // is manifold, by default this is false template struct is_manifold: public std::false_type { @@ -54,22 +70,13 @@ struct is_manifold: public std::false_type { template struct dimension; -// Chart is a map from T -> vector, retract is its inverse -template -struct DefaultChart { - BOOST_STATIC_ASSERT(is_manifold::value); - typedef Eigen::Matrix::value, 1> vector; - DefaultChart(const T& t) : - t_(t) { - } - vector apply(const T& other) { - return t_.localCoordinates(other); - } - T retract(const vector& d) { - return t_.retract(d); - } -private: - T const & t_; +/** + * zero::value is intended to be the origin of a canonical coordinate system + * with canonical(t) == DefaultChart(zero::value).apply(t) + * Below we provide the group identity as zero *in case* it is a group + */ +template struct zero: public identity { + BOOST_STATIC_ASSERT(is_group::value); }; // double @@ -82,24 +89,6 @@ template<> struct dimension : public std::integral_constant { }; -template<> -struct DefaultChart { - typedef Eigen::Matrix vector; - DefaultChart(double t) : - t_(t) { - } - vector apply(double other) { - vector d; - d << other - t_; - return d; - } - double retract(const vector& d) { - return t_ + d[0]; - } -private: - double t_; -}; - // Fixed size Eigen::Matrix type template @@ -130,10 +119,74 @@ struct dimension > : public std::integral_c BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; +} // \ namespace traits + +// Chart is a map from T -> vector, retract is its inverse +template +struct DefaultChart { + BOOST_STATIC_ASSERT(traits::is_manifold::value); + typedef Eigen::Matrix::value, 1> vector; + DefaultChart(const T& t) : + t_(t) { + } + vector apply(const T& other) { + return t_.localCoordinates(other); + } + T retract(const vector& d) { + return t_.retract(d); + } +private: + T const & t_; +}; + +/** + * Canonical::value is a chart around zero::value + * An example is Canonical + */ +template class Canonical { + DefaultChart chart; +public: + typedef T type; + typedef typename DefaultChart::vector vector; + Canonical() : + chart(traits::zero::value()) { + } + // Convert t of type T into canonical coordinates + vector apply(const T& t) { + return chart.apply(t); + } + // Convert back from canonical coordinates to T + T retract(const vector& v) { + return chart.retract(v); + } +}; + +// double + +template<> +struct DefaultChart { + typedef Eigen::Matrix vector; + DefaultChart(double t) : + t_(t) { + } + vector apply(double other) { + vector d; + d << other - t_; + return d; + } + double retract(const vector& d) { + return t_ + d[0]; + } +private: + double t_; +}; + +// Fixed size Eigen::Matrix type + template struct DefaultChart > { typedef Eigen::Matrix T; - typedef Eigen::Matrix::value, 1> vector; + typedef Eigen::Matrix::value, 1> vector; DefaultChart(const T& t) : t_(t) { } @@ -202,7 +255,7 @@ private: } }; -} // namespace gtsam +} // \ namespace gtsam /** * Macros for using the ManifoldConcept diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index dded932e8..2de5a808d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -169,6 +169,9 @@ private: }; +// Define GTSAM traits +namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -177,4 +180,13 @@ template<> struct dimension : public std::integral_constant { }; +template<> +struct zero { + static Cal3Bundler value() { + return Cal3Bundler(0, 0, 0); + } +}; + +} + } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index eb3bb3623..d716d398e 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -168,6 +168,9 @@ private: }; +// Define GTSAM traits +namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -175,5 +178,8 @@ struct is_manifold : public std::true_type { template<> struct dimension : public std::integral_constant { }; + +} + } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6f1e75bad..a87a30e36 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -240,6 +240,9 @@ private: }; +// Define GTSAM traits +namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -248,5 +251,11 @@ template<> struct dimension : public std::integral_constant { }; +template<> +struct zero { + static Cal3_S2 value() { return Cal3_S2();} +}; + +} } // \ namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 02f283224..4b93ca70c 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -303,7 +303,7 @@ public: return K_.uncalibrate(pn); } - typedef Eigen::Matrix::value> Matrix2K; + typedef Eigen::Matrix::value> Matrix2K; /** project a point from world coordinate to the image * @param pw is a point in world coordinates @@ -613,6 +613,9 @@ private: }; +// Define GTSAM traits +namespace traits { + template struct is_manifold > : public std::true_type { }; @@ -622,4 +625,14 @@ struct dimension > : public std::integral_constant< int, dimension::value + dimension::value> { }; +template +struct zero > { + static PinholeCamera value() { + return PinholeCamera(zero::value(), + zero::value()); + } +}; + } + +} // \ gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index ffd3c2f80..7d1fab133 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -250,6 +250,13 @@ private: /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + template<> struct is_manifold : public std::true_type { }; @@ -260,3 +267,5 @@ struct dimension : public std::integral_constant { } +} + diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b333ac1e9..d69ceb861 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -242,6 +242,13 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + template<> struct is_manifold : public std::true_type { }; @@ -250,4 +257,5 @@ namespace gtsam { struct dimension : public std::integral_constant { }; + } } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 13fa6aba0..b6a2314ff 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -321,6 +321,9 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +// Define GTSAM traits +namespace traits { + template<> struct is_manifold : public std::true_type { }; @@ -329,5 +332,7 @@ template<> struct dimension : public std::integral_constant { }; +} + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c5013270f..5f99b25ac 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -354,6 +354,13 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + template<> struct is_manifold : public std::true_type { }; @@ -362,4 +369,6 @@ template<> struct dimension : public std::integral_constant { }; +} + } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index eb6078ef2..62ac9f3f9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -491,6 +491,13 @@ namespace gtsam { */ GTSAM_EXPORT std::pair RQ(const Matrix3& A); + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + template<> struct is_manifold : public std::true_type { }; @@ -499,5 +506,5 @@ namespace gtsam { struct dimension : public std::integral_constant { }; - + } } From bf16446f92edc4f4c5c3f2b1870638878f6dc554 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 01:26:17 +0200 Subject: [PATCH 33/45] Deal with traits changes --- gtsam_unstable/nonlinear/Expression-inl.h | 20 +++---- gtsam_unstable/nonlinear/Expression.h | 2 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 16 +++--- .../nonlinear/tests/testExpression.cpp | 52 ++++--------------- 4 files changed, 29 insertions(+), 61 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 3594ea61f..d5c5f1279 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -114,7 +114,7 @@ void handleLeafCase( */ template class ExecutionTrace { - static const int Dim = dimension::value; + static const int Dim = traits::dimension::value; enum { Constant, Leaf, Function } kind; @@ -196,7 +196,7 @@ public: /// Primary template calls the generic Matrix reverseAD pipeline template struct Select { - typedef Eigen::Matrix::value> Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD(dTdA, jacobians); @@ -206,7 +206,7 @@ struct Select { /// Partially specialized template calls the 2-dimensional output version template struct Select<2, A> { - typedef Eigen::Matrix::value> Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD2(dTdA, jacobians); @@ -317,7 +317,7 @@ public: /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = dimension::value; + map[key_] = traits::dimension::value; } /// Return value @@ -368,13 +368,15 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix::value, dimension::value> type; + typedef Eigen::Matrix::value, + traits::dimension::value> type; }; /// meta-function to generate JacobianTA optional reference template struct Optional { - typedef Eigen::Matrix::value, dimension::value> Jacobian; + typedef Eigen::Matrix::value, + traits::dimension::value> Jacobian; typedef boost::optional type; }; @@ -385,7 +387,7 @@ template struct FunctionalBase: ExpressionNode { static size_t const N = 0; // number of arguments - typedef CallRecord::value> Record; + typedef CallRecord::value> Record; /// Construct an execution trace for reverse AD void trace(const Values& values, Record* record, char*& raw) const { @@ -454,7 +456,7 @@ struct GenerateFunctionalNode: Argument, Base { /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Base::Record::startReverseAD(jacobians); - Select::value, A>::reverseAD(This::trace, This::dTdA, + Select::value, A>::reverseAD(This::trace, This::dTdA, jacobians); } @@ -465,7 +467,7 @@ struct GenerateFunctionalNode: Argument, Base { } /// Version specialized to 2-dimensional output - typedef Eigen::Matrix::value> Jacobian2T; + typedef Eigen::Matrix::value> Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD2(dFdT, jacobians); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 0e9b1034d..6ac7d9ce8 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -159,7 +159,7 @@ public: template struct apply_compose { typedef T result_type; - static const int Dim = dimension::value; + static const int Dim = traits::dimension::value; typedef Eigen::Matrix Jacobian; T operator()(const T& x, const T& y, boost::optional H1, boost::optional H2) const { diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index cdf2d132e..84456c934 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -37,6 +37,8 @@ class ExpressionFactor: public NoiseModelFactor { std::vector dimensions_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) + static const int Dim = traits::dimension::value; + public: /// Constructor @@ -45,7 +47,7 @@ public: measurement_(measurement), expression_(expression) { if (!noiseModel) throw std::invalid_argument("ExpressionFactor: no NoiseModel."); - if (noiseModel->dim() != dimension::value) + if (noiseModel->dim() != Dim) throw std::invalid_argument( "ExpressionFactor was created with a NoiseModel of incorrect dimension."); noiseModel_ = noiseModel; @@ -68,7 +70,7 @@ public: #ifdef DEBUG_ExpressionFactor BOOST_FOREACH(size_t d, dimensions_) std::cout << d << " "; - std::cout << " -> " << dimension::value << "x" << augmentedCols_ << std::endl; + std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; #endif } @@ -87,10 +89,9 @@ public: JacobianMap blocks; for (DenseIndex i = 0; i < size(); i++) { Matrix& Hi = H->at(i); - Hi.resize(dimension::value, dimensions_[i]); + Hi.resize(Dim, dimensions_[i]); Hi.setZero(); // zero out - Eigen::Block block = Hi.block(0, 0, dimension::value, - dimensions_[i]); + Eigen::Block block = Hi.block(0, 0, Dim, dimensions_[i]); blocks.insert(std::make_pair(keys_[i], block)); } @@ -110,10 +111,9 @@ public: // to [expression_.value] below, which writes directly into Ab_. // Another malloc saved by creating a Matrix on the stack - static const int Dim = dimension::value; double memory[Dim * augmentedCols_]; - Eigen::Map::value, Eigen::Dynamic> > // - matrix(memory, dimension::value, augmentedCols_); + Eigen::Map > // + matrix(memory, Dim, augmentedCols_); matrix.setZero(); // zero out // Construct block matrix, is of right size but un-initialized diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index d8aa80535..68765ecc0 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -327,6 +327,7 @@ struct SnavelyProjection { // is_manifold TEST(Expression, is_manifold) { + using namespace traits; EXPECT(!is_manifold::value); EXPECT(is_manifold::value); EXPECT(is_manifold::value); @@ -337,6 +338,7 @@ TEST(Expression, is_manifold) { // dimension TEST(Expression, dimension) { + using namespace traits; EXPECT_LONGS_EQUAL(2, dimension::value); EXPECT_LONGS_EQUAL(8, dimension::value); EXPECT_LONGS_EQUAL(1, dimension::value); @@ -374,6 +376,7 @@ TEST(Expression, Charts) { template Matrix numericalDerivative(boost::function h, const X& x, double delta = 1e-5) { + using namespace traits; BOOST_STATIC_ASSERT(is_manifold::value); static const size_t M = dimension::value; @@ -491,33 +494,14 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } -/* ************************************************************************* */ -// zero::value is intended to be the origin of a canonical coordinate system -// with canonical(t) == DefaultChart(zero::value).apply(t) -template struct zero; -template class Canonical { - DefaultChart chart; -public: - typedef T type; - typedef typename DefaultChart::vector vector; - Canonical() : - chart(zero::value) { - } - vector vee(const T& t) { - return chart.apply(t); - } - T hat(const vector& v) { - return chart.retract(v); - } -}; /* ************************************************************************* */ // Adapt ceres-style autodiff template class AdaptAutoDiff { - static const int N = dimension::value; - static const int M1 = dimension::value; - static const int M2 = dimension::value; + static const int N = traits::dimension::value; + static const int M1 = traits::dimension::value; + static const int M2 = traits::dimension::value; typedef Eigen::Matrix RowMajor1; typedef Eigen::Matrix RowMajor2; @@ -547,8 +531,8 @@ public: using ceres::internal::AutoDiff; // Make arguments - Vector1 v1 = chart1.vee(a1); - Vector2 v2 = chart2.vee(a2); + Vector1 v1 = chart1.apply(a1); + Vector2 v2 = chart2.apply(a2); bool success; VectorT result; @@ -574,7 +558,7 @@ public: if (!success) throw std::runtime_error( "AdaptAutoDiff: function call resulted in failure"); - return chartT.hat(result); + return chartT.retract(result); } }; @@ -582,24 +566,6 @@ public: // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; -template<> -struct zero { - static const Camera value; -}; -const Camera zero::value(Camera(Pose3(), Cal3Bundler(0, 0, 0))); - -template<> -struct zero { - static const Point3 value; -}; -const Point3 zero::value(Point3(0, 0, 0)); - -template<> -struct zero { - static const Point2 value; -}; -const Point2 zero::value(Point2(0, 0)); - /* ************************************************************************* */ // Test AutoDiff wrapper Snavely TEST(Expression, AutoDiff3) { From 13b433ad89c4fcb5a10501b84c0904943679a5f4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 10:42:05 +0200 Subject: [PATCH 34/45] zero for double and fixed matrices --- gtsam/base/Manifold.h | 30 +++++++++++++++++++++++++----- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 4bea1c919..4ed371803 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -81,6 +81,10 @@ template struct zero: public identity { // double +template<> +struct is_group : public std::true_type { +}; + template<> struct is_manifold : public std::true_type { }; @@ -89,8 +93,17 @@ template<> struct dimension : public std::integral_constant { }; +template<> +struct zero { + static double value() { return 0;} +}; + // Fixed size Eigen::Matrix type +template +struct is_group > : public std::true_type { +}; + template struct is_manifold > : public std::true_type { }; @@ -119,6 +132,15 @@ struct dimension > : public std::integral_c BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; +template +struct zero > : public std::integral_constant< + int, M * N> { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); + static Eigen::Matrix value() { + return Eigen::Matrix::Zero(); + } +}; + } // \ namespace traits // Chart is a map from T -> vector, retract is its inverse @@ -126,6 +148,7 @@ template struct DefaultChart { BOOST_STATIC_ASSERT(traits::is_manifold::value); typedef Eigen::Matrix::value, 1> vector; + T const & t_; DefaultChart(const T& t) : t_(t) { } @@ -135,19 +158,16 @@ struct DefaultChart { T retract(const vector& d) { return t_.retract(d); } -private: - T const & t_; }; /** * Canonical::value is a chart around zero::value * An example is Canonical */ -template class Canonical { - DefaultChart chart; -public: +template struct Canonical { typedef T type; typedef typename DefaultChart::vector vector; + DefaultChart chart; Canonical() : chart(traits::zero::value()) { } From 25ad9ade05d1645ac2d9cebcf65b98695287142a Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 10:42:30 +0200 Subject: [PATCH 35/45] Moved AdaptAutoDiff into its own test --- .cproject | 106 ++-- .../nonlinear/tests/testAdaptAutoDiff.cpp | 460 ++++++++++++++++++ .../nonlinear/tests/testExpression.cpp | 381 --------------- 3 files changed, 522 insertions(+), 425 deletions(-) create mode 100644 gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp diff --git a/.cproject b/.cproject index 7223156ff..38c4c66f4 100644 --- a/.cproject +++ b/.cproject @@ -600,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -607,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -654,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -661,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -668,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -683,6 +688,7 @@ make + tests/testBayesTree true false @@ -1042,6 +1048,7 @@ make + testErrors.run true false @@ -1271,6 +1278,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1353,7 +1400,6 @@ make - testSimulated2DOriented.run true false @@ -1393,7 +1439,6 @@ make - testSimulated2D.run true false @@ -1401,7 +1446,6 @@ make - testSimulated3D.run true false @@ -1415,46 +1459,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1712,6 +1716,7 @@ cpack + -G DEB true false @@ -1719,6 +1724,7 @@ cpack + -G RPM true false @@ -1726,6 +1732,7 @@ cpack + -G TGZ true false @@ -1733,6 +1740,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2459,6 +2467,7 @@ make + testGraph.run true false @@ -2466,6 +2475,7 @@ make + testJunctionTree.run true false @@ -2473,6 +2483,7 @@ make + testSymbolicBayesNetB.run true false @@ -2566,6 +2577,14 @@ true true + + make + -j5 + testAdaptAutoDiff.run + true + true + true + make -j2 @@ -2968,7 +2987,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp new file mode 100644 index 000000000..45267bf81 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -0,0 +1,460 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#undef CHECK +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +/* ************************************************************************* */ +// Some Ceres Snippets copied for testing +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +template inline T &RowMajorAccess(T *base, int rows, int cols, + int i, int j) { + return base[cols * i + j]; +} + +inline double RandDouble() { + double r = static_cast(rand()); + return r / RAND_MAX; +} + +// A structure for projecting a 3x4 camera matrix and a +// homogeneous 3D point, to a 2D inhomogeneous point. +struct Projective { + // Function that takes P and X as separate vectors: + // P, X -> x + template + bool operator()(A const P[12], A const X[4], A x[2]) const { + A PX[3]; + for (int i = 0; i < 3; ++i) { + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + } + if (PX[2] != 0.0) { + x[0] = PX[0] / PX[2]; + x[1] = PX[1] / PX[2]; + return true; + } + return false; + } + + // Adapt to eigen types + Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { + Vector2 x; + if (operator()(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Projective fail"); + } +}; + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyProjection { + + template + bool operator()(const T* const camera, const T* const point, + T* predicted) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + ceres::AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + T xp = -p[0] / p[2]; + T yp = -p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + T r2 = xp * xp + yp * yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + const T& focal = camera[6]; + predicted[0] = focal * distortion * xp; + predicted[1] = focal * distortion * yp; + + return true; + } + + // Adapt to GTSAM types + Vector2 operator()(const Vector9& P, const Vector3& X) const { + Vector2 x; + if (operator()(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Snavely fail"); + } + +}; + +/* ************************************************************************* */ +// is_manifold +TEST(Manifold, _is_manifold) { + using namespace traits; + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +/* ************************************************************************* */ +// dimension +TEST(Manifold, _dimension) { + using namespace traits; + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, DefaultChart) { + + DefaultChart chart1(Point2(0, 0)); + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + DefaultChart chart2(Vector2(0, 0)); + EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + DefaultChart chart3(0); + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// DefaultChart chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); +} + +/* ************************************************************************* */ +// zero +TEST(Manifold, _zero) { + EXPECT(assert_equal(Pose3(),traits::zero::value())); + Cal3Bundler cal(0,0,0); + EXPECT(assert_equal(cal,traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, Canonical) { + + Canonical chart1; + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + Canonical chart2; + EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// Canonical chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); +} + +/* ************************************************************************* */ +// New-style numerical derivatives using manifold_traits +template +Matrix numericalDerivative(boost::function h, const X& x, + double delta = 1e-5) { + using namespace traits; + + BOOST_STATIC_ASSERT(is_manifold::value); + static const size_t M = dimension::value; + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; + + BOOST_STATIC_ASSERT(is_manifold::value); + static const size_t N = dimension::value; + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get chart at x + ChartX chartX(x); + + // get value at x, and corresponding chart + Y hx = h(x); + ChartY chartY(hx); + + // Prepare a tangent vector to perturb x with + TangentX dx; + dx.setZero(); + + // Fill in Jacobian H + Matrix H = zeros(M, N); + double factor = 1.0 / (2.0 * delta); + for (size_t j = 0; j < N; j++) { + dx(j) = delta; + TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + dx(j) = -delta; + TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + H.block(0, j) << (dy1 - dy2) * factor; + dx(j) = 0; + } + return H; +} + +template +Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalDerivative(boost::bind(h, _1, x2), x1, delta); +} + +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalDerivative(boost::bind(h, x1, _1), x2, delta); +} + +/* ************************************************************************* */ +// Test Ceres AutoDiff +TEST(Expression, AutoDiff) { + using ceres::internal::AutoDiff; + + // Instantiate function + Projective projective; + + // Make arguments + typedef Eigen::Matrix M; + M P; + P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; + Vector4 X(10, 0, 5, 1); + + // Apply the mapping, to get image point b_x. + Vector expected = Vector2(2, 1); + Vector2 actual = projective(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21(Projective(), P, X); + Matrix E2 = numericalDerivative22(Projective(), P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 12), H2(2, 4); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// Test Ceres AutoDiff on Snavely +TEST(Expression, AutoDiff2) { + using ceres::internal::AutoDiff; + + // Instantiate function + SnavelyProjection snavely; + + // Make arguments + Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 + P << 0, 0, 0, 0, 5, 0, 1, 0, 0; + Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + // Apply the mapping, to get image point b_x. + Vector expected = Vector2(2, 1); + Vector2 actual = snavely(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21( + SnavelyProjection(), P, X); + Matrix E2 = numericalDerivative22( + SnavelyProjection(), P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 9), H2(2, 3); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// Adapt ceres-style autodiff +template +class AdaptAutoDiff { + + static const int N = traits::dimension::value; + static const int M1 = traits::dimension::value; + static const int M2 = traits::dimension::value; + + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical Canonical2; + + typedef typename CanonicalT::vector VectorT; + typedef typename Canonical1::vector Vector1; + typedef typename Canonical2::vector Vector2; + + // Instantiate function and charts + CanonicalT chartT; + Canonical1 chart1; + Canonical2 chart2; + F f; + +public: + + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + + T operator()(const A1& a1, const A2& a2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) { + + using ceres::internal::AutoDiff; + + // Make arguments + Vector1 v1 = chart1.apply(a1); + Vector2 v2 = chart2.apply(a2); + + bool success; + VectorT result; + + if (H1 || H2) { + + // Get derivatives with AutoDiff + double *parameters[] = { v1.data(), v2.data() }; + double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double *jacobians[] = { rowMajor1, rowMajor2 }; + success = AutoDiff::Differentiate(f, parameters, 2, + result.data(), jacobians); + + // Convert from row-major to columnn-major + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major + *H1 = Eigen::Map(rowMajor1); + *H2 = Eigen::Map(rowMajor2); + + } else { + // Apply the mapping, to get result + success = f(v1.data(), v2.data(), result.data()); + } + if (!success) + throw std::runtime_error( + "AdaptAutoDiff: function call resulted in failure"); + return chartT.retract(result); + } + +}; + +/* ************************************************************************* */ +// Test AutoDiff wrapper Snavely +TEST(Expression, AutoDiff3) { + + // Make arguments + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + typedef AdaptAutoDiff Adaptor; + Adaptor snavely; + + // Apply the mapping, to get image point b_x. + Point2 expected(2, 1); + Point2 actual = snavely(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + +// // Get expected derivatives + Matrix E1 = numericalDerivative21(Adaptor(), P, X); + Matrix E2 = numericalDerivative22(Adaptor(), P, X); + + // Get derivatives with AutoDiff, not gives RowMajor results! + Matrix29 H1; + Matrix23 H2; + Point2 actual2 = snavely(P, X, H1, H2); + EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// Test AutoDiff wrapper in an expression +TEST(Expression, Snavely) { + Expression P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 68765ecc0..1997bdb53 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -18,17 +18,11 @@ */ #include -#include #include -#include #include #include #include -#include -#include - -#undef CHECK #include #include @@ -229,381 +223,6 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } -/* ************************************************************************* */ -// Some Ceres Snippets copied for testing -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. -template inline T &RowMajorAccess(T *base, int rows, int cols, - int i, int j) { - return base[cols * i + j]; -} - -inline double RandDouble() { - double r = static_cast(rand()); - return r / RAND_MAX; -} - -// A structure for projecting a 3x4 camera matrix and a -// homogeneous 3D point, to a 2D inhomogeneous point. -struct Projective { - // Function that takes P and X as separate vectors: - // P, X -> x - template - bool operator()(A const P[12], A const X[4], A x[2]) const { - A PX[3]; - for (int i = 0; i < 3; ++i) { - PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] - + RowMajorAccess(P, 3, 4, i, 1) * X[1] - + RowMajorAccess(P, 3, 4, i, 2) * X[2] - + RowMajorAccess(P, 3, 4, i, 3) * X[3]; - } - if (PX[2] != 0.0) { - x[0] = PX[0] / PX[2]; - x[1] = PX[1] / PX[2]; - return true; - } - return false; - } - - // Adapt to eigen types - Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { - Vector2 x; - if (operator()(P.data(), X.data(), x.data())) - return x; - else - throw std::runtime_error("Projective fail"); - } -}; - -// Templated pinhole camera model for used with Ceres. The camera is -// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for -// focal length and 2 for radial distortion. The principal point is not modeled -// (i.e. it is assumed be located at the image center). -struct SnavelyProjection { - - template - bool operator()(const T* const camera, const T* const point, - T* predicted) const { - // camera[0,1,2] are the angle-axis rotation. - T p[3]; - ceres::AngleAxisRotatePoint(camera, point, p); - - // camera[3,4,5] are the translation. - p[0] += camera[3]; - p[1] += camera[4]; - p[2] += camera[5]; - - // Compute the center of distortion. The sign change comes from - // the camera model that Noah Snavely's Bundler assumes, whereby - // the camera coordinate system has a negative z axis. - T xp = -p[0] / p[2]; - T yp = -p[1] / p[2]; - - // Apply second and fourth order radial distortion. - const T& l1 = camera[7]; - const T& l2 = camera[8]; - T r2 = xp * xp + yp * yp; - T distortion = T(1.0) + r2 * (l1 + l2 * r2); - - // Compute final projected point position. - const T& focal = camera[6]; - predicted[0] = focal * distortion * xp; - predicted[1] = focal * distortion * yp; - - return true; - } - - // Adapt to GTSAM types - Vector2 operator()(const Vector9& P, const Vector3& X) const { - Vector2 x; - if (operator()(P.data(), X.data(), x.data())) - return x; - else - throw std::runtime_error("Snavely fail"); - } - -}; - -/* ************************************************************************* */ - -// is_manifold -TEST(Expression, is_manifold) { - using namespace traits; - EXPECT(!is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); -} - -// dimension -TEST(Expression, dimension) { - using namespace traits; - EXPECT_LONGS_EQUAL(2, dimension::value); - EXPECT_LONGS_EQUAL(8, dimension::value); - EXPECT_LONGS_EQUAL(1, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); -} - -// charts -TEST(Expression, Charts) { - - DefaultChart chart1(Point2(0, 0)); - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); - - DefaultChart chart2(Vector2(0, 0)); - EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); - - DefaultChart chart3(0); - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); - - // Dynamic does not work yet ! -// Vector z = zero(2), v(2); -// v << 1, 0; -// DefaultChart chart4(z); -// EXPECT(chart4.apply(v)==v); -// EXPECT(chart4.retract(v)==v); -} - -/* ************************************************************************* */ -// New-style numerical derivatives using manifold_traits -template -Matrix numericalDerivative(boost::function h, const X& x, - double delta = 1e-5) { - using namespace traits; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t M = dimension::value; - typedef DefaultChart ChartY; - typedef typename ChartY::vector TangentY; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t N = dimension::value; - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; - - // get chart at x - ChartX chartX(x); - - // get value at x, and corresponding chart - Y hx = h(x); - ChartY chartY(hx); - - // Prepare a tangent vector to perturb x with - TangentX dx; - dx.setZero(); - - // Fill in Jacobian H - Matrix H = zeros(M, N); - double factor = 1.0 / (2.0 * delta); - for (size_t j = 0; j < N; j++) { - dx(j) = delta; - TangentY dy1 = chartY.apply(h(chartX.retract(dx))); - dx(j) = -delta; - TangentY dy2 = chartY.apply(h(chartX.retract(dx))); - H.block(0, j) << (dy1 - dy2) * factor; - dx(j) = 0; - } - return H; -} - -template -Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h, _1, x2), x1, delta); -} - -template -Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h, x1, _1), x2, delta); -} - -/* ************************************************************************* */ -// Test Ceres AutoDiff -TEST(Expression, AutoDiff) { - using ceres::internal::AutoDiff; - - // Instantiate function - Projective projective; - - // Make arguments - typedef Eigen::Matrix M; - M P; - P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; - Vector4 X(10, 0, 5, 1); - - // Apply the mapping, to get image point b_x. - Vector expected = Vector2(2, 1); - Vector2 actual = projective(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - - // Get expected derivatives - Matrix E1 = numericalDerivative21(Projective(), P, X); - Matrix E2 = numericalDerivative22(Projective(), P, X); - - // Get derivatives with AutoDiff - Vector2 actual2; - MatrixRowMajor H1(2, 12), H2(2, 4); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); -} - -/* ************************************************************************* */ -// Test Ceres AutoDiff on Snavely -TEST(Expression, AutoDiff2) { - using ceres::internal::AutoDiff; - - // Instantiate function - SnavelyProjection snavely; - - // Make arguments - Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 - P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! - - // Apply the mapping, to get image point b_x. - Vector expected = Vector2(2, 1); - Vector2 actual = snavely(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - - // Get expected derivatives - Matrix E1 = numericalDerivative21( - SnavelyProjection(), P, X); - Matrix E2 = numericalDerivative22( - SnavelyProjection(), P, X); - - // Get derivatives with AutoDiff - Vector2 actual2; - MatrixRowMajor H1(2, 9), H2(2, 3); - double *parameters[] = { P.data(), X.data() }; - double *jacobians[] = { H1.data(), H2.data() }; - CHECK( - (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); -} - -/* ************************************************************************* */ -// Adapt ceres-style autodiff -template -class AdaptAutoDiff { - - static const int N = traits::dimension::value; - static const int M1 = traits::dimension::value; - static const int M2 = traits::dimension::value; - - typedef Eigen::Matrix RowMajor1; - typedef Eigen::Matrix RowMajor2; - - typedef Canonical CanonicalT; - typedef Canonical Canonical1; - typedef Canonical Canonical2; - - typedef typename CanonicalT::vector VectorT; - typedef typename Canonical1::vector Vector1; - typedef typename Canonical2::vector Vector2; - - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; - F f; - -public: - - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { - - using ceres::internal::AutoDiff; - - // Make arguments - Vector1 v1 = chart1.apply(a1); - Vector2 v2 = chart2.apply(a2); - - bool success; - VectorT result; - - if (H1 || H2) { - - // Get derivatives with AutoDiff - double *parameters[] = { v1.data(), v2.data() }; - double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack - double *jacobians[] = { rowMajor1, rowMajor2 }; - success = AutoDiff::Differentiate(f, parameters, 2, - result.data(), jacobians); - - // Convert from row-major to columnn-major - // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major - *H1 = Eigen::Map(rowMajor1); - *H2 = Eigen::Map(rowMajor2); - - } else { - // Apply the mapping, to get result - success = f(v1.data(), v2.data(), result.data()); - } - if (!success) - throw std::runtime_error( - "AdaptAutoDiff: function call resulted in failure"); - return chartT.retract(result); - } - -}; - -// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector -typedef PinholeCamera Camera; - -/* ************************************************************************* */ -// Test AutoDiff wrapper Snavely -TEST(Expression, AutoDiff3) { - - // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); - Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! - - typedef AdaptAutoDiff Adaptor; - Adaptor snavely; - - // Apply the mapping, to get image point b_x. - Point2 expected(2, 1); - Point2 actual = snavely(P, X); - EXPECT(assert_equal(expected,actual,1e-9)); - -// // Get expected derivatives - Matrix E1 = numericalDerivative21(Adaptor(), P, X); - Matrix E2 = numericalDerivative22(Adaptor(), P, X); - - // Get derivatives with AutoDiff, not gives RowMajor results! - Matrix29 H1; - Matrix23 H2; - Point2 actual2 = snavely(P, X, H1, H2); - EXPECT(assert_equal(expected,actual,1e-9)); - EXPECT(assert_equal(E1,H1,1e-8)); - EXPECT(assert_equal(E2,H2,1e-8)); -} - -TEST(Expression, Snavely) { - Expression P(1); - Expression X(2); - typedef AdaptAutoDiff Adaptor; - Expression expression(Adaptor(), P, X); - set expected = list_of(1)(2); - EXPECT(expected == expression.keys()); -} - /* ************************************************************************* */ int main() { TestResult tr; From 0acffe5ae9b9533d587c00ffdaaad209a481ff85 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 11:13:14 +0200 Subject: [PATCH 36/45] Fixed bug in DefaultChart: keeping a reference s never a good idea. --- gtsam/base/Manifold.h | 8 ++--- .../nonlinear/tests/testAdaptAutoDiff.cpp | 32 +++++++++++++++---- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 4ed371803..c4420bb7d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -148,7 +148,7 @@ template struct DefaultChart { BOOST_STATIC_ASSERT(traits::is_manifold::value); typedef Eigen::Matrix::value, 1> vector; - T const & t_; + T t_; DefaultChart(const T& t) : t_(t) { } @@ -186,6 +186,7 @@ template struct Canonical { template<> struct DefaultChart { typedef Eigen::Matrix vector; + double t_; DefaultChart(double t) : t_(t) { } @@ -197,8 +198,6 @@ struct DefaultChart { double retract(const vector& d) { return t_ + d[0]; } -private: - double t_; }; // Fixed size Eigen::Matrix type @@ -207,6 +206,7 @@ template struct DefaultChart > { typedef Eigen::Matrix T; typedef Eigen::Matrix::value, 1> vector; + T t_; DefaultChart(const T& t) : t_(t) { } @@ -219,8 +219,6 @@ struct DefaultChart > { Eigen::Map map(d.data()); return t_ + map; } -private: - T const & t_; }; /** diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index 45267bf81..d697a382f 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -202,7 +202,7 @@ TEST(Manifold, Canonical) { EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); Canonical chart2; - EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); Canonical chart3; @@ -211,12 +211,30 @@ TEST(Manifold, Canonical) { EXPECT(chart3.apply(1)==v1); EXPECT(chart3.retract(v1)==1); - // Dynamic does not work yet ! -// Vector z = zero(2), v(2); -// v << 1, 0; -// Canonical chart4(z); -// EXPECT(chart4.apply(v)==v); -// EXPECT(chart4.retract(v)==v); + Canonical chart4; + Point3 point(1,2,3); + Vector3 v3(1,2,3); + EXPECT(assert_equal((Vector)chart4.apply(point),v3)); + EXPECT(assert_equal(chart4.retract(v3),point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(),point); + Vector6 v6; v6 << 0,0,0,1,2,3; + EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); + EXPECT(assert_equal(chart5.retract(v6),pose)); + + Canonical chart6; + Cal3Bundler cal0(0,0,0); + Camera camera(Pose3(),cal0); + Vector9 z9 = Vector9::Zero(); + EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); + EXPECT(assert_equal(chart6.retract(z9),camera)); + + Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() + Camera camera2(pose,cal); + Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; + EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); + EXPECT(assert_equal(chart6.retract(v9),camera2)); } /* ************************************************************************* */ From 224b71d696bcba7703dd30450c9b5a2fa43ee7ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 12:54:28 +0200 Subject: [PATCH 37/45] Created testManifold --- .../nonlinear/tests/testAdaptAutoDiff.cpp | 102 ------------ tests/testManifold.cpp | 149 ++++++++++++++++++ 2 files changed, 149 insertions(+), 102 deletions(-) create mode 100644 tests/testManifold.cpp diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index d697a382f..397c91c5f 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -135,108 +135,6 @@ struct SnavelyProjection { }; -/* ************************************************************************* */ -// is_manifold -TEST(Manifold, _is_manifold) { - using namespace traits; - EXPECT(!is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); -} - -/* ************************************************************************* */ -// dimension -TEST(Manifold, _dimension) { - using namespace traits; - EXPECT_LONGS_EQUAL(2, dimension::value); - EXPECT_LONGS_EQUAL(8, dimension::value); - EXPECT_LONGS_EQUAL(1, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); -} - -/* ************************************************************************* */ -// charts -TEST(Manifold, DefaultChart) { - - DefaultChart chart1(Point2(0, 0)); - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); - - DefaultChart chart2(Vector2(0, 0)); - EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); - - DefaultChart chart3(0); - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); - - // Dynamic does not work yet ! -// Vector z = zero(2), v(2); -// v << 1, 0; -// DefaultChart chart4(z); -// EXPECT(chart4.apply(v)==v); -// EXPECT(chart4.retract(v)==v); -} - -/* ************************************************************************* */ -// zero -TEST(Manifold, _zero) { - EXPECT(assert_equal(Pose3(),traits::zero::value())); - Cal3Bundler cal(0,0,0); - EXPECT(assert_equal(cal,traits::zero::value())); - EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); -} - -/* ************************************************************************* */ -// charts -TEST(Manifold, Canonical) { - - Canonical chart1; - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); - - Canonical chart2; - EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); - - Canonical chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); - - Canonical chart4; - Point3 point(1,2,3); - Vector3 v3(1,2,3); - EXPECT(assert_equal((Vector)chart4.apply(point),v3)); - EXPECT(assert_equal(chart4.retract(v3),point)); - - Canonical chart5; - Pose3 pose(Rot3::identity(),point); - Vector6 v6; v6 << 0,0,0,1,2,3; - EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); - EXPECT(assert_equal(chart5.retract(v6),pose)); - - Canonical chart6; - Cal3Bundler cal0(0,0,0); - Camera camera(Pose3(),cal0); - Vector9 z9 = Vector9::Zero(); - EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); - EXPECT(assert_equal(chart6.retract(z9),camera)); - - Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() - Camera camera2(pose,cal); - Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; - EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); - EXPECT(assert_equal(chart6.retract(v9),camera2)); -} - /* ************************************************************************* */ // New-style numerical derivatives using manifold_traits template diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp new file mode 100644 index 000000000..e43cde102 --- /dev/null +++ b/tests/testManifold.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include + +#undef CHECK +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +/* ************************************************************************* */ +// is_manifold +TEST(Manifold, _is_manifold) { + using namespace traits; + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +/* ************************************************************************* */ +// dimension +TEST(Manifold, _dimension) { + using namespace traits; + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, DefaultChart) { + + DefaultChart chart1(Point2(0, 0)); + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + DefaultChart chart2(Vector2(0, 0)); + EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + DefaultChart chart3(0); + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// DefaultChart chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); +} + +/* ************************************************************************* */ +// zero +TEST(Manifold, _zero) { + EXPECT(assert_equal(Pose3(),traits::zero::value())); + Cal3Bundler cal(0,0,0); + EXPECT(assert_equal(cal,traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, Canonical) { + + Canonical chart1; + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + Canonical chart2; + EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + Canonical chart4; + Point3 point(1,2,3); + Vector3 v3(1,2,3); + EXPECT(assert_equal((Vector)chart4.apply(point),v3)); + EXPECT(assert_equal(chart4.retract(v3),point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(),point); + Vector6 v6; v6 << 0,0,0,1,2,3; + EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); + EXPECT(assert_equal(chart5.retract(v6),pose)); + + Canonical chart6; + Cal3Bundler cal0(0,0,0); + Camera camera(Pose3(),cal0); + Vector9 z9 = Vector9::Zero(); + EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); + EXPECT(assert_equal(chart6.retract(z9),camera)); + + Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() + Camera camera2(pose,cal); + Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; + EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); + EXPECT(assert_equal(chart6.retract(v9),camera2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From fcbc1e90cf4b13f9b28ee44571edfd7dad0afb42 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 18:02:33 +0200 Subject: [PATCH 38/45] More traits --- gtsam/base/LieVector.h | 15 ++++++++++++++- gtsam/geometry/Cal3Unified.h | 22 +++++++++++++++++----- gtsam/geometry/EssentialMatrix.h | 18 ++++++++++++++++++ gtsam/geometry/Rot2.h | 22 ++++++++++++++++------ gtsam/geometry/StereoCamera.h | 18 ++++++++++++++++++ gtsam/geometry/StereoPoint2.h | 16 ++++++++++++++++ gtsam/geometry/Unit3.h | 20 ++++++++++++++++++++ gtsam/navigation/ImuBias.h | 17 +++++++++++++++++ gtsam_unstable/dynamics/PoseRTV.h | 19 +++++++++++++++++++ 9 files changed, 155 insertions(+), 12 deletions(-) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index a8bfe3007..8286c95a6 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -128,6 +128,19 @@ private: ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } - }; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + +} + } // \namespace gtsam diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index eacbf7053..ad8e7b904 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -126,10 +126,6 @@ public: private: - /// @} - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template @@ -140,9 +136,25 @@ private: ar & BOOST_SERIALIZATION_NVP(xi_); } - /// @} +}; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Cal3Unified value() { return Cal3Unified();} }; } +} + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 32b966261..a973f9cec 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -196,5 +196,23 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static EssentialMatrix value() { return EssentialMatrix();} +}; + +} + } // gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index d121beb12..4142d4473 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -230,10 +230,6 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /// @} - /// @name Advanced Interface - /// @{ - private: /** Serialization function */ friend class boost::serialization::access; @@ -245,8 +241,22 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(s_); } - /// @} - }; + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } // gtsam diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 70917b2c4..82914f6ab 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -155,4 +155,22 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static StereoCamera value() { return StereoCamera();} +}; + +} + } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8ce2e49bf..000f7d16f 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -173,4 +173,20 @@ namespace gtsam { }; + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8d2c024c0..bb2ee318a 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -156,5 +156,25 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Unit3 value() { + return Unit3(); + } +}; + +} + } // namespace gtsam diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 32911e589..8301a0a6b 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -218,6 +218,23 @@ namespace imuBias { } // namespace ImuBias +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + } // namespace gtsam diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 51e09ca5f..80729e8a2 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -183,4 +183,23 @@ private: } }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static PoseRTV value() { + return PoseRTV(); + } +}; + +} } // \namespace gtsam From f46aa7cd8c3a420bde656d33ba4339bd52c7c406 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 18:04:19 +0200 Subject: [PATCH 39/45] DefaultChart for dynamically sized Vector --- gtsam/base/Manifold.h | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index c4420bb7d..63390ec1f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -95,7 +95,9 @@ struct dimension : public std::integral_constant { template<> struct zero { - static double value() { return 0;} + static double value() { + return 0; + } }; // Fixed size Eigen::Matrix type @@ -118,24 +120,22 @@ struct dimension template struct dimension > : public Dynamic { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic); }; template struct dimension > : public Dynamic { - BOOST_STATIC_ASSERT(N!=Eigen::Dynamic); }; template struct dimension > : public std::integral_constant< int, M * N> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; template struct zero > : public std::integral_constant< int, M * N> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "traits::zero is only supported for fixed-size matrices"); static Eigen::Matrix value() { return Eigen::Matrix::Zero(); } @@ -206,6 +206,8 @@ template struct DefaultChart > { typedef Eigen::Matrix T; typedef Eigen::Matrix::value, 1> vector; + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "DefaultChart has not been implemented yet for dynamically sized matrices"); T t_; DefaultChart(const T& t) : t_(t) { @@ -221,6 +223,23 @@ struct DefaultChart > { } }; +// Dynamically sized Vector +template<> +struct DefaultChart { + typedef Vector T; + typedef T vector; + T t_; + DefaultChart(const T& t) : + t_(t) { + } + vector apply(const T& other) { + return other - t_; + } + T retract(const vector& d) { + return t_ + d; + } +}; + /** * Old Concept check class for Manifold types * Requires a mapping between a linear tangent space and the underlying From 1eb5e185e5548a0b3026c99fd5bf3cccbcd476b5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 18:50:52 +0200 Subject: [PATCH 40/45] New numericalDerivatives with traits an Charts - still some segfaults, *and* there should be no need for (a) multiple prototypes to match against c++ pointers, (b) the use of explicit template arguments. A task for someone... --- .cproject | 8 + gtsam/base/numericalDerivative.h | 1079 ++++++++--------- gtsam/base/tests/testNumericalDerivative.cpp | 108 +- gtsam/geometry/tests/testRot3M.cpp | 18 +- gtsam/geometry/tests/testTriangulation.cpp | 6 +- gtsam/geometry/tests/testUnit3.cpp | 4 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 13 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 4 +- .../tests/testCombinedImuFactor.cpp | 4 +- gtsam/navigation/tests/testGPSFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 36 +- gtsam/navigation/tests/testMagFactor.cpp | 14 +- .../tests/testEssentialMatrixConstraint.cpp | 4 +- .../slam/tests/testEssentialMatrixFactor.cpp | 18 +- gtsam/slam/tests/testRotateFactor.cpp | 8 +- gtsam_unstable/geometry/Pose3Upright.h | 20 +- .../geometry/tests/testInvDepthCamera3.cpp | 6 +- .../nonlinear/tests/testAdaptAutoDiff.cpp | 55 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 12 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 12 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 10 +- .../slam/tests/testPoseBetweenFactor.cpp | 8 +- .../slam/tests/testPosePriorFactor.cpp | 4 +- .../slam/tests/testProjectionFactorPPP.cpp | 4 +- 24 files changed, 667 insertions(+), 790 deletions(-) diff --git a/.cproject b/.cproject index 38c4c66f4..700b82ce6 100644 --- a/.cproject +++ b/.cproject @@ -2329,6 +2329,14 @@ true true + + make + -j5 + testNumericalDerivative.run + true + true + true + make -j5 diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 036f23f68..01ed3f09a 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -16,7 +16,6 @@ */ // \callgraph - #pragma once #include @@ -31,595 +30,497 @@ #include #include +#include namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) - * - * For additional details, see the documentation: - * http://www.boost.org/doc/libs/release/libs/bind/bind.html - */ - - - /** global functions for converting to a LieVector for use with numericalDerivative */ - inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } - - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } - -// /** remapping for double valued functions */ -// template -// Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { -// return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); -// } - - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } - - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative22 - (boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 1 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative31 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 2 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative32 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 3 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative33 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative33 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar - * function. This is implemented simply as the derivative of the gradient. - * @param f A function taking a Lie object as input and returning a scalar - * @param x The center point for computing the Hessian - * @param delta The numerical derivative step size - * @return n*n Hessian matrix computed via central differencing - */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } - - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(f), x, delta); - } - - - /** Helper class that computes the derivative of f w.r.t. x1, centered about - * x1_, as a function of x2 - */ - template - class G_x1 { - const boost::function& f_; - const X1& x1_; - double delta_; - public: - G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} - Vector operator()(const X2& x2) { - return numericalGradient(boost::function(boost::bind(f_, _1, x2)), x1_, delta_); - } - }; - - template - inline Matrix numericalHessian212(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - G_x1 g_x1(f, x1, delta); - return numericalDerivative11(boost::function(boost::bind(boost::ref(g_x1), _1)), x2, delta); - } - - - template - inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian212(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian211(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - - template - inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian211(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian222(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - - template - inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian222(boost::function(f), x1, x2, delta); - } - - /** - * Numerical Hessian for tenary functions - */ - /* **************************************************************** */ - template - inline Matrix numericalHessian311(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - template - inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian311(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian322(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - template - inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian322(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian333(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, x2, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); - } - - template - inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian333(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, _2, x3)), x1, x2, delta ); - } - - template - inline Matrix numericalHessian313(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, x2, _2)), x1, x3, delta ); - } - - template - inline Matrix numericalHessian323(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, x1, _1, _2)), x2, x3, delta ); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian312(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian313(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian323(boost::function(f), x1, x2, x3, delta); - } +/* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * Use boost bind as follows to create a function pointer that uses the member function: + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * + * For additional details, see the documentation: + * http://www.boost.org/doc/libs/release/libs/bind/bind.html + */ + +/** global functions for converting to a LieVector for use with numericalDerivative */ +inline LieVector makeLieVector(const Vector& v) { + return LieVector(v); +} +inline LieVector makeLieVectorD(double d) { + return LieVector((Vector) (Vector(1) << d)); +} + +/** + * Numerically compute gradient of scalar function + * Class X is the input argument + * The class X needs to have dim, expmap, logmap + */ +template +Vector numericalGradient(boost::function h, const X& x, + double delta = 1e-5) { + double factor = 1.0 / (2.0 * delta); + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get chart at x + ChartX chartX(x); + TangentX zeroX = chartX.apply(x); + size_t n = zeroX.size(); // hack to get size if dynamic type + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX d; + d.setZero(); + + Vector g = zero(n); + for (int j = 0; j < n; j++) { + d(j) = delta; + double hxplus = h(chartX.retract(d)); + d(j) = -delta; + double hxmin = h(chartX.retract(d)); + d(j) = 0; + g(j) = (hxplus - hxmin) * factor; + } + return g; +} + +/** + * @brief New-style numerical derivatives using manifold_traits + * @brief Computes numerical derivative in argument 1 of unary function + * @param h unary function yielding m-vector + * @param x n-dimensional value at which to evaluate h + * @param delta increment for numerical derivative + * Class Y is the output argument + * Class X is the input argument + * @return m*n Jacobian computed via central differencing + */ +template +// TODO Should compute fixed-size matrix +Matrix numericalDerivative11(boost::function h, const X& x, + double delta = 1e-5) { + using namespace traits; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get value at x, and corresponding chart + Y hx = h(x); + ChartY chartY(hx); + TangentY zeroY = chartY.apply(hx); + size_t m = zeroY.size(); + + // get chart at x + ChartX chartX(x); + TangentX zeroX = chartX.apply(x); + size_t n = zeroX.size(); + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX dx; + dx.setZero(); + + // Fill in Jacobian H + Matrix H = zeros(m,n); + double factor = 1.0 / (2.0 * delta); + for (int j = 0; j < n; j++) { + dx(j) = delta; + TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + dx(j) = -delta; + TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + dx(j) = 0; + H.col(j) << (dy1 - dy2) * factor; + } + return H; +} + +/** use a raw C++ function pointer */ +template +Matrix numericalDerivative11(Y (*h)(const X&), const X& x, + double delta = 1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); +} + +/** + * Compute numerical derivative in argument 1 of binary function + * @param h binary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative21(const boost::function& h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 2 of binary function + * @param h binary function yielding m-vector + * @param x1 first argument value + * @param x2 n-dimensional second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 1 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative31( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); +} + +template +inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative32( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); +} + +template +inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative33( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X3 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); +} + +template +inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar + * function. This is implemented simply as the derivative of the gradient. + * @param f A function taking a Lie object as input and returning a scalar + * @param x The center point for computing the Hessian + * @param delta The numerical derivative step size + * @return n*n Hessian matrix computed via central differencing + */ +template +inline Matrix numericalHessian(boost::function f, const X& x, + double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef boost::function F; + typedef boost::function G; + G ng = static_cast(numericalGradient ); + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + delta); +} + +template +inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = + 1e-5) { + return numericalHessian(boost::function(f), x, delta); +} + +/** Helper class that computes the derivative of f w.r.t. x1, centered about + * x1_, as a function of x2 + */ +template +class G_x1 { + const boost::function& f_; + X1 x1_; + double delta_; +public: + G_x1(const boost::function& f, const X1& x1, + double delta) : + f_(f), x1_(x1), delta_(delta) { + } + Vector operator()(const X2& x2) { + return numericalGradient(boost::bind(f_, _1, x2), x1_, delta_); + } +}; + +template +inline Matrix numericalHessian212( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + G_x1 g_x1(f, x1, delta); + return numericalDerivative11( + boost::function( + boost::bind(boost::ref(g_x1), _1)), x2, delta); +} + +template +inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian212(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian211( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian211(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian222( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian222(boost::function(f), + x1, x2, delta); +} + +/** + * Numerical Hessian for tenary functions + */ +/* **************************************************************** */ +template +inline Matrix numericalHessian311( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian311( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian322( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian322( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian333( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X3&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, x2, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x3, delta); +} + +template +inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian333( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, _2, x3)), + x1, x2, delta); +} + +template +inline Matrix numericalHessian313( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, x2, _2)), + x1, x3, delta); +} + +template +inline Matrix numericalHessian323( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, x1, _1, _2)), + x2, x3, delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian312( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian313( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian323( + boost::function(f), x1, x2, x3, + delta); +} } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index f7e4d3baa..b4a9b91d6 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -15,115 +15,123 @@ * @date Apr 8, 2011 */ +#include #include -#include - +using namespace std; using namespace gtsam; /* ************************************************************************* */ -double f(const LieVector& x) { +double f(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) + cos(x(1)); } /* ************************************************************************* */ -TEST(testNumericalDerivative, numericalHessian) { - LieVector center = ones(2); +// +TEST(testNumericalDerivative, numericalGradient) { + Vector2 x(1, 1); - Matrix expected = (Matrix(2,2) << - -sin(center(0)), 0.0, - 0.0, -cos(center(1))); + Vector expected(2); + expected << cos(x(1)), -sin(x(0)); - Matrix actual = numericalHessian(f, center); + Vector actual = numericalGradient(f, x); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -double f2(const LieVector& x) { +TEST(testNumericalDerivative, numericalHessian) { + Vector2 x(1, 1); + + Matrix expected(2, 2); + expected << -sin(x(0)), 0.0, 0.0, -cos(x(1)); + + Matrix actual = numericalHessian(f, x); + + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +/* ************************************************************************* */ +double f2(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) * cos(x(1)); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian2) { - Vector v_center = (Vector(2) << 0.5, 1.0); - LieVector center(v_center); + Vector2 v(0.5, 1.0); + Vector2 x(v); - Matrix expected = (Matrix(2,2) << - -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), - -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1))); + Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1)) + * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))); - Matrix actual = numericalHessian(f2, center); + Matrix actual = numericalHessian(f2, x); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -double f3(const LieVector& x1, const LieVector& x2) { - assert(x1.size() == 1 && x2.size() == 1); - return sin(x1(0)) * cos(x2(0)); +double f3(double x1, double x2) { + return sin(x1) * cos(x2); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian211) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 5.0); - LieVector center1(v_center1), center2(v_center2); + double x1 = 1, x2 = 5; - Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); - Matrix actual11 = numericalHessian211(f3, center1, center2); + Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual11 = numericalHessian211(f3, x1, x2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); - Matrix actual12 = numericalHessian212(f3, center1, center2); + Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)); + Matrix actual12 = numericalHessian212(f3, x1, x2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); - Matrix actual22 = numericalHessian222(f3, center1, center2); + Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual22 = numericalHessian222(f3, x1, x2); EXPECT(assert_equal(expected22, actual22, 1e-5)); } /* ************************************************************************* */ -double f4(const LieVector& x, const LieVector& y, const LieVector& z) { - assert(x.size() == 1 && y.size() == 1 && z.size() == 1); - return sin(x(0)) * cos(y(0)) * z(0)*z(0); +double f4(double x, double y, double z) { + return sin(x) * cos(y) * z * z; } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian311) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 2.0); - Vector v_center3 = (Vector(1) << 3.0); - LieVector center1(v_center1), center2(v_center2), center3(v_center3); - - double x = center1(0), y = center2(0), z = center3(0); - Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); - Matrix actual11 = numericalHessian311(f4, center1, center2, center3); + double x = 1, y = 2, z = 3; + Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual11 = numericalHessian311(f4, x, y, z); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); - Matrix actual12 = numericalHessian312(f4, center1, center2, center3); + Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z); + Matrix actual12 = numericalHessian312(f4, x, y, z); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); - Matrix actual13 = numericalHessian313(f4, center1, center2, center3); + Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z); + Matrix actual13 = numericalHessian313(f4, x, y, z); EXPECT(assert_equal(expected13, actual13, 1e-5)); - Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); - Matrix actual22 = numericalHessian322(f4, center1, center2, center3); + Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual22 = numericalHessian322(f4, x, y, z); EXPECT(assert_equal(expected22, actual22, 1e-5)); - Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); - Matrix actual23 = numericalHessian323(f4, center1, center2, center3); + Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z); + Matrix actual23 = numericalHessian323(f4, x, y, z); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); - Matrix actual33 = numericalHessian333(f4, center1, center2, center3); + Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2); + Matrix actual33 = numericalHessian333(f4, x, y, z); EXPECT(assert_equal(expected33, actual33, 1e-5)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index b0890057e..7707e9161 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -229,7 +229,8 @@ TEST( Rot3, rightJacobianExpMapSO3inverse ) Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias Vector3 deltatheta; deltatheta << 0, 0, 0; - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedJacobian = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); } @@ -439,19 +440,18 @@ TEST( Rot3, between ) /* ************************************************************************* */ Vector w = (Vector(3) << 0.1, 0.27, -0.2); -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 // => y = log (exp(-w) * exp(w+dw)) -Vector testDexpL(const LieVector& dw) { - Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); - return y; +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); } TEST( Rot3, dexpL) { Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11( - boost::function( - boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); Matrix actualDexpInvL = Rot3::dexpInvL(w); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 51c195d32..0bd553a40 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) { Matrix HActual; factor.evaluateError(landmark, HActual); -// Matrix expectedH1 = numericalDerivative11( -// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, -// boost::none, boost::none), pose1); - // The expected Jacobian - Matrix HExpected = numericalDerivative11( + Matrix HExpected = numericalDerivative11( boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); // Verify the Jacobians are correct diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4aa553df2..53ae2fc58 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index d65f96f7f..7726f2280 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -150,7 +149,7 @@ TEST( GaussianBayesNet, DeterminantTest ) /* ************************************************************************* */ namespace { - double computeError(const GaussianBayesNet& gbn, const LieVector& values) + double computeError(const GaussianBayesNet& gbn, const Vector& values) { pair Rd = GaussianFactorGraph(gbn).jacobian(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); @@ -180,14 +179,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Matrix hessian = numericalHessian(boost::bind(&computeError, gbn, _1), + Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Vector gradient = numericalGradient(boost::bind(&computeError, gbn, _1), + Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index c4155ea16..790080556 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), nRb); @@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::none), T); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 1a8b6160d..279c20e69 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -239,12 +239,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index f02047aa9..8c93020c9 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a6894898b..ad97d9433 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -192,15 +192,15 @@ TEST( ImuFactor, Error ) EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -276,15 +276,15 @@ TEST( ImuFactor, ErrorWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -341,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); const Vector3 x = thetahat; // parametrization of so(3) @@ -417,12 +417,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); @@ -531,15 +531,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1875e4f1c..5599c93d6 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 5184393ac..4c0edf5c9 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1e5674599..c889fb1e9 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index f36405318..15e8cf984 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 2a814b915..c1e12b4a7 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -126,11 +126,9 @@ public: /// Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose3Upright& p); -private: - /// @} - /// @name Advanced Interface - /// @{ + +private: // Serialization function friend class boost::serialization::access; @@ -142,4 +140,18 @@ private: }; // \class Pose3Upright +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index b477d3e44..6e0b92fd7 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -90,7 +90,7 @@ TEST( InvDepthFactor, Dproject_pose) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); @@ -102,7 +102,7 @@ TEST( InvDepthFactor, Dproject_landmark) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); @@ -114,7 +114,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index 397c91c5f..053acdd34 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -135,60 +136,6 @@ struct SnavelyProjection { }; -/* ************************************************************************* */ -// New-style numerical derivatives using manifold_traits -template -Matrix numericalDerivative(boost::function h, const X& x, - double delta = 1e-5) { - using namespace traits; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t M = dimension::value; - typedef DefaultChart ChartY; - typedef typename ChartY::vector TangentY; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t N = dimension::value; - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; - - // get chart at x - ChartX chartX(x); - - // get value at x, and corresponding chart - Y hx = h(x); - ChartY chartY(hx); - - // Prepare a tangent vector to perturb x with - TangentX dx; - dx.setZero(); - - // Fill in Jacobian H - Matrix H = zeros(M, N); - double factor = 1.0 / (2.0 * delta); - for (size_t j = 0; j < N; j++) { - dx(j) = delta; - TangentY dy1 = chartY.apply(h(chartX.retract(dx))); - dx(j) = -delta; - TangentY dy2 = chartY.apply(h(chartX.retract(dx))); - H.block(0, j) << (dy1 - dy2) * factor; - dx(j) = 0; - } - return H; -} - -template -Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h, _1, x2), x1, delta); -} - -template -Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalDerivative(boost::bind(h, x1, _1), x2, delta); -} - /* ************************************************************************* */ // Test Ceres AutoDiff TEST(Expression, AutoDiff) { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d61787358..6a8c297b7 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -104,11 +104,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 9d4113431..8d55d1ce5 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -107,11 +107,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index aef15638f..e4b282550 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -108,10 +108,10 @@ public: boost::optional H2=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -228,13 +228,13 @@ public: boost::optional H3=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); } if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); } return inverseDepthError(pose1, pose2, landmark); diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index bf9dc0e8e..d0af4af62 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -205,8 +205,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Point3(-3.5257579, 6.02637531, -8.98382384)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index cbfa45431..a01cfedba 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -166,7 +166,7 @@ TEST( PosePriorFactor, Jacobian ) { Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -190,7 +190,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { Point3(-4.74767676, 7.67044942, -11.00985)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 3a7bf0cd0..aacca18ea 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -170,7 +170,7 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, Pose3(), point); @@ -205,7 +205,7 @@ TEST( ProjectionFactor, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, body_P_sensor, point); From 06af482d617d1aa819cb7e7bf91d1504c784c275 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 23:07:38 +0200 Subject: [PATCH 41/45] Added test for Rot3 - all is good --- .cproject | 8 +++++ tests/testManifold.cpp | 72 ++++++++++++++++++++++++++---------------- 2 files changed, 53 insertions(+), 27 deletions(-) diff --git a/.cproject b/.cproject index 700b82ce6..97cdc3bcb 100644 --- a/.cproject +++ b/.cproject @@ -2553,6 +2553,14 @@ true true + + make + -j5 + testManifold.run + true + true + true + make -j5 diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index e43cde102..2c3b20434 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -69,29 +69,42 @@ TEST(Manifold, DefaultChart) { EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + Vector v2(2); + v2 << 1, 0; DefaultChart chart2(Vector2(0, 0)); - EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); + EXPECT(chart2.retract(v2)==Vector2(1,0)); DefaultChart chart3(0); - Eigen::Matrix v1; + Vector v1(1); v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); + EXPECT(assert_equal(v1,chart3.apply(1))); + EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); // Dynamic does not work yet ! -// Vector z = zero(2), v(2); -// v << 1, 0; -// DefaultChart chart4(z); -// EXPECT(chart4.apply(v)==v); -// EXPECT(chart4.retract(v)==v); + Vector z = zero(2), v(2); + v << 1, 0; + DefaultChart chart4(z); + EXPECT(assert_equal(chart4.apply(v),v)); + EXPECT(assert_equal(chart4.retract(v),v)); + + Vector v3(3); + v3 << 1, 1, 1; + Rot3 I = Rot3::identity(); + Rot3 R = I.retractCayley(v3); + DefaultChart chart5(I); + EXPECT(assert_equal(v3,chart5.apply(R))); + EXPECT(assert_equal(chart5.retract(v3),R)); + // Check zero vector + DefaultChart chart6(R); + EXPECT(assert_equal(zero(3),chart6.apply(R))); } /* ************************************************************************* */ // zero TEST(Manifold, _zero) { EXPECT(assert_equal(Pose3(),traits::zero::value())); - Cal3Bundler cal(0,0,0); + Cal3Bundler cal(0, 0, 0); EXPECT(assert_equal(cal,traits::zero::value())); EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); } @@ -104,39 +117,44 @@ TEST(Manifold, Canonical) { EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + Vector v2(2); + v2 << 1, 0; Canonical chart2; - EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); + EXPECT(chart2.retract(v2)==Vector2(1,0)); Canonical chart3; Eigen::Matrix v1; v1 << 1; EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); + EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); Canonical chart4; - Point3 point(1,2,3); - Vector3 v3(1,2,3); - EXPECT(assert_equal((Vector)chart4.apply(point),v3)); + Point3 point(1, 2, 3); + Vector v3(3); + v3 << 1, 2, 3; + EXPECT(assert_equal(v3,chart4.apply(point))); EXPECT(assert_equal(chart4.retract(v3),point)); Canonical chart5; - Pose3 pose(Rot3::identity(),point); - Vector6 v6; v6 << 0,0,0,1,2,3; - EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); + Pose3 pose(Rot3::identity(), point); + Vector v6(6); + v6 << 0, 0, 0, 1, 2, 3; + EXPECT(assert_equal(v6,chart5.apply(pose))); EXPECT(assert_equal(chart5.retract(v6),pose)); Canonical chart6; - Cal3Bundler cal0(0,0,0); - Camera camera(Pose3(),cal0); - Vector9 z9 = Vector9::Zero(); - EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); + Cal3Bundler cal0(0, 0, 0); + Camera camera(Pose3(), cal0); + Vector z9 = Vector9::Zero(); + EXPECT(assert_equal(z9,chart6.apply(camera))); EXPECT(assert_equal(chart6.retract(z9),camera)); Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() - Camera camera2(pose,cal); - Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; - EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); + Camera camera2(pose, cal); + Vector v9(9); + v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; + EXPECT(assert_equal(v9,chart6.apply(camera2))); EXPECT(assert_equal(chart6.retract(v9),camera2)); } From b1aa7148c79b22a908f0d4fa1cac3d01eb23681e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Oct 2014 00:39:56 +0200 Subject: [PATCH 42/45] Fix dimensions, add is_group --- gtsam/base/LieScalar.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 2ed81b1df..750a8a21f 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -115,12 +115,16 @@ namespace gtsam { // Define GTSAM traits namespace traits { + template<> + struct is_group : public std::true_type { + }; + template<> struct is_manifold : public std::true_type { }; template<> - struct dimension : public Dynamic { + struct dimension : public std::integral_constant { }; } From 4b3e0dbcc070175344d2bcc5d97ac9022a303152 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Oct 2014 00:40:08 +0200 Subject: [PATCH 43/45] Some new targets --- .cproject | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/.cproject b/.cproject index 97cdc3bcb..b01317650 100644 --- a/.cproject +++ b/.cproject @@ -902,6 +902,14 @@ true true + + make + -j5 + testGaussianBayesTree.run + true + true + true + make -j5 @@ -2073,6 +2081,22 @@ true true + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + make -j2 From 3b0d2a5f472c236fb399bf943265376c1e1dd322 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Oct 2014 00:40:28 +0200 Subject: [PATCH 44/45] Make it clear that argument types must be fixed-size (for now). --- gtsam/base/numericalDerivative.h | 39 +++++++++++++------------------- 1 file changed, 16 insertions(+), 23 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 01ed3f09a..87c912e57 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -28,7 +28,6 @@ #pragma GCC diagnostic pop #endif -#include #include #include @@ -56,14 +55,6 @@ namespace gtsam { * http://www.boost.org/doc/libs/release/libs/bind/bind.html */ -/** global functions for converting to a LieVector for use with numericalDerivative */ -inline LieVector makeLieVector(const Vector& v) { - return LieVector(v); -} -inline LieVector makeLieVectorD(double d) { - return LieVector((Vector) (Vector(1) << d)); -} - /** * Numerically compute gradient of scalar function * Class X is the input argument @@ -76,20 +67,20 @@ Vector numericalGradient(boost::function h, const X& x, BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); typedef DefaultChart ChartX; typedef typename ChartX::vector TangentX; // get chart at x ChartX chartX(x); - TangentX zeroX = chartX.apply(x); - size_t n = zeroX.size(); // hack to get size if dynamic type // Prepare a tangent vector to perturb x with, only works for fixed size TangentX d; d.setZero(); - Vector g = zero(n); - for (int j = 0; j < n; j++) { + Vector g = zero(N); // Can be fixed size + for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(chartX.retract(d)); d(j) = -delta; @@ -123,28 +114,30 @@ Matrix numericalDerivative11(boost::function h, const X& x, BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); typedef DefaultChart ChartX; typedef typename ChartX::vector TangentX; // get value at x, and corresponding chart Y hx = h(x); ChartY chartY(hx); + + // Bit of a hack for now to find number of rows TangentY zeroY = chartY.apply(hx); size_t m = zeroY.size(); // get chart at x ChartX chartX(x); - TangentX zeroX = chartX.apply(x); - size_t n = zeroX.size(); // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; dx.setZero(); // Fill in Jacobian H - Matrix H = zeros(m,n); + Matrix H = zeros(m, N); double factor = 1.0 / (2.0 * delta); - for (int j = 0; j < n; j++) { + for (int j = 0; j < N; j++) { dx(j) = delta; TangentY dy1 = chartY.apply(h(chartX.retract(dx))); dx(j) = -delta; @@ -345,7 +338,7 @@ inline Matrix numericalHessian212( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { G_x1 g_x1(f, x1, delta); - return numericalDerivative11( + return numericalDerivative11( boost::function( boost::bind(boost::ref(g_x1), _1)), x2, delta); } @@ -366,7 +359,7 @@ inline Matrix numericalHessian211( double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2)); - return numericalDerivative11( + return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); } @@ -387,7 +380,7 @@ inline Matrix numericalHessian222( double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1)); - return numericalDerivative11( + return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); } @@ -412,7 +405,7 @@ inline Matrix numericalHessian311( double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2, x3)); - return numericalDerivative11( + return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); } @@ -435,7 +428,7 @@ inline Matrix numericalHessian322( double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1, x3)); - return numericalDerivative11( + return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); } @@ -458,7 +451,7 @@ inline Matrix numericalHessian333( double) = &numericalGradient; boost::function f2(boost::bind(f, x1, x2, _1)); - return numericalDerivative11( + return numericalDerivative11( boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); } From 113b9d2e746d4ef96829161c860f27cb1b8588bf Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Oct 2014 01:32:59 +0200 Subject: [PATCH 45/45] Got rid of unnecessary LieVector usage that broke fixed-code --- .cproject | 56 ++++ gtsam/geometry/tests/testPoint2.cpp | 8 +- gtsam/geometry/tests/testPose2.cpp | 8 +- gtsam/geometry/tests/testPose3.cpp | 46 +-- gtsam/geometry/tests/testRot3M.cpp | 2 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 11 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 23 +- gtsam/navigation/tests/testImuFactor.cpp | 45 ++- gtsam/slam/tests/testPoseRotationPrior.cpp | 20 +- gtsam/slam/tests/testPoseTranslationPrior.cpp | 32 +- gtsam/slam/tests/testRangeFactor.cpp | 21 +- gtsam/slam/tests/testReferenceFrameFactor.cpp | 16 +- gtsam_unstable/dynamics/VelocityConstraint.h | 6 +- .../dynamics/tests/testSimpleHelicopter.cpp | 28 +- .../geometry/tests/testInvDepthCamera3.cpp | 24 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 10 +- .../slam/InertialNavFactor_GlobalVelocity.h | 10 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 +- .../testInertialNavFactor_GlobalVelocity.cpp | 308 +++++++++--------- .../slam/tests/testInvDepthFactorVariant1.cpp | 36 +- .../slam/tests/testInvDepthFactorVariant2.cpp | 35 +- .../slam/tests/testInvDepthFactorVariant3.cpp | 2 +- .../slam/tests/testProjectionFactorPPPC.cpp | 8 +- .../tests/testRelativeElevationFactor.cpp | 32 +- gtsam_unstable/slam/tests/testTSAMFactors.cpp | 20 +- 27 files changed, 424 insertions(+), 391 deletions(-) diff --git a/.cproject b/.cproject index b01317650..895e2667a 100644 --- a/.cproject +++ b/.cproject @@ -806,6 +806,54 @@ true true + + make + -j5 + testInertialNavFactor_GlobalVelocity.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant3.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant1.run + true + true + true + + + make + -j5 + testEquivInertialNavFactor_GlobalVel.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant2.run + true + true + true + + + make + -j5 + testRelativeElevationFactor.run + true + true + true + make -j5 @@ -2193,6 +2241,14 @@ true true + + make + -j5 + testVelocityConstraint.run + true + true + true + make -j1 diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 66ee5a387..215f376f6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -84,8 +84,8 @@ namespace { Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); /* ************************************************************************* */ - LieVector norm_proxy(const Point2& point) { - return LieVector(point.norm()); + double norm_proxy(const Point2& point) { + return point.norm(); } } TEST( Point2, norm ) { @@ -112,8 +112,8 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { - LieVector distance_proxy(const Point2& location, const Point2& point) { - return LieVector(location.distance(point)); + double distance_proxy(const Point2& location, const Point2& point) { + return location.distance(point); } } TEST( Point2, distance ) { diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 42b548f5a..12266c16f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose ) /* ************************************************************************* */ namespace { - LieVector range_proxy(const Pose2& pose, const Point2& point) { - return LieVector(pose.range(point)); + double range_proxy(const Pose2& pose, const Point2& point) { + return pose.range(point); } } TEST( Pose2, range ) @@ -611,8 +611,8 @@ TEST( Pose2, range ) /* ************************************************************************* */ namespace { - LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { - return LieVector(pose.range(point)); + double range_pose_proxy(const Pose2& pose, const Pose2& point) { + return pose.range(point); } } TEST( Pose2, range_pose ) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 175a11ff1..2a775379d 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -547,8 +547,8 @@ Pose3 xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ -LieVector range_proxy(const Pose3& pose, const Point3& point) { - return LieVector(pose.range(point)); +double range_proxy(const Pose3& pose, const Point3& point) { + return pose.range(point); } TEST( Pose3, range ) { @@ -582,8 +582,8 @@ TEST( Pose3, range ) } /* ************************************************************************* */ -LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) { - return LieVector(pose.range(point)); +double range_pose_proxy(const Pose3& pose, const Pose3& point) { + return pose.range(point); } TEST( Pose3, range_pose ) { @@ -674,30 +674,24 @@ TEST(Pose3, align_2) { /* ************************************************************************* */ /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) - Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); -Vector testDerivExpmapInv(const LieVector& dxi) { - Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); - return y; +Vector testDerivExpmapInv(const Vector6& dxi) { + return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); } TEST( Pose3, dExpInv_TLN) { Matrix res = Pose3::dExpInv_exp(xi); - Matrix numericalDerivExpmapInv = numericalDerivative11( - boost::function( - boost::bind(testDerivExpmapInv, _1) - ), - LieVector(Vector::Zero(6)), 1e-5 - ); + Matrix numericalDerivExpmapInv = numericalDerivative11( + testDerivExpmapInv, Vector6::Zero(), 1e-5); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi)*v; +Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi) * v; } TEST( Pose3, adjoint) { @@ -706,20 +700,16 @@ TEST( Pose3, adjoint) { Matrix actualH; Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjoint, _1, _2) - ), - LieVector(screw::xi), LieVector(screw::xi), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjoint, screw::xi, screw::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5)); } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi).transpose()*v; +Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi).transpose() * v; } TEST( Pose3, adjointTranspose) { @@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjointTranspose, _1, _2) - ), - LieVector(xi), LieVector(v), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(numericalH,actualH,1e-5)); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 7707e9161..08f5a42e9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 ) // Linearization point Vector3 thetahat; thetahat << 0.1, 0, 0; - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); + Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 7726f2280..cd5231e49 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -148,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - double computeError(const GaussianBayesNet& gbn, const Vector& values) + double computeError(const GaussianBayesNet& gbn, const Vector10& values) { pair Rd = GaussianFactorGraph(gbn).jacobian(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); @@ -179,12 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically - Matrix hessian = numericalHessian(boost::bind(&computeError, gbn, _1), - Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); + Matrix hessian = numericalHessian( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient(boost::bind(&computeError, gbn, _1), - Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); + Vector gradient = numericalGradient( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 8f37cac0c..217fab543 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -24,7 +24,6 @@ using namespace boost::assign; #include -#include #include #include #include @@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) { EXPECT(assert_equal(expectedCov, actualCov, 1e1)); } +/* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - /* ************************************************************************* */ - double computeError(const GaussianBayesTree& gbt, const LieVector& values) - { - pair Rd = GaussianFactorGraph(gbt).jacobian(); - return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); - } +double computeError(const GaussianBayesTree& gbt, const Vector10& values) { + pair Rd = GaussianFactorGraph(gbt).jacobian(); + return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); +} } /* ************************************************************************* */ @@ -243,14 +242,12 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ad97d9433..823ce8306 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -39,14 +38,14 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; @@ -168,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -194,11 +193,11 @@ TEST( ImuFactor, Error ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); @@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -278,11 +277,11 @@ TEST( ImuFactor, ErrorWithBiases ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); @@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap ) // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( + &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); @@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -533,11 +532,11 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index b40da2e2a..3cac377eb 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -37,13 +37,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -52,8 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -63,8 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -74,8 +72,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -85,8 +82,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 36d94c9a3..47ff708d9 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -34,13 +34,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 4fa6164a1..cba9300f5 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,12 +36,12 @@ typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; /* ************************************************************************* */ -LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { return factor.evaluateError(pose, point); } @@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 078bf85cd..309801ccb 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -53,9 +53,9 @@ TEST( ReferenceFrameFactor, equals ) { } /* ************************************************************************* */ -LieVector evaluateError_(const PointReferenceFrameFactor& c, +Vector evaluateError_(const PointReferenceFrameFactor& c, const Point2& global, const Pose2& trans, const Point2& local) { - return LieVector(c.evaluateError(global, trans, local)); + return Vector(c.evaluateError(global, trans, local)); } TEST( ReferenceFrameFactor, jacobians ) { @@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); @@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 252f9dbfe..c9db449f9 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -83,9 +83,9 @@ public: virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if (H1) *H1 = gtsam::numericalDerivative21( + if (H1) *H1 = gtsam::numericalDerivative21( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); - if (H2) *H2 = gtsam::numericalDerivative22( + if (H2) *H2 = gtsam::numericalDerivative22( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } @@ -103,7 +103,7 @@ public: } private: - static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, + static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, double dt, const dynamics::IntegrationMode& mode) { const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 82197302b..475d5285c 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,11 +18,11 @@ const double h = 0.01; //const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); -LieVector V1_g1 = g1.inverse().Adjoint(V1_w); +//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); -//LieVector v2 = Pose3::Logmap(g1.between(g2)); +//Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape @@ -46,16 +46,16 @@ Vector computeFu(const Vector& gamma, const Vector& control) { } /* ************************************************************************* */ -Vector testExpmapDeriv(const LieVector& v) { +Vector testExpmapDeriv(const Vector6& v) { return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); } TEST(Reconstruction, ExpmapInvDeriv) { Matrix numericalExpmap = numericalDerivative11( - boost::function( + boost::function( boost::bind(testExpmapDeriv, _1) ), - LieVector(Vector::Zero(6)), 1e-5 + Vector6(Vector::Zero(6)), 1e-5 ); Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); @@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) { Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 @@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Pose3 g21 = g2.between(g1); Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame - LieVector expectedv2(V2_g2); + Vector6 expectedv2(V2_g2); // hard constraints don't need a noise model DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h, @@ -130,21 +130,21 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 6e0b92fd7..3385998b0 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -83,12 +83,12 @@ TEST( InvDepthFactor, Project4) { /* ************************************************************************* */ -Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) { +Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) { return InvDepthCamera3(pose,K).project(landmark, inv_depth); } TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,12 +124,12 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); + Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; + Vector5 actual_vec; LieScalar actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); EXPECT(assert_equal(expected,actual_vec,1e-7)); @@ -140,12 +140,12 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); + Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; + Vector5 actual_vec; LieScalar actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); EXPECT(assert_equal(expected,actual_vec,1e-7)); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index b2174f8a9..41f216b77 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -315,8 +315,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -336,8 +337,9 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 5ca736c01..76f6d02d5 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -242,8 +242,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -263,8 +264,9 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 6a8c297b7..d2a784b0f 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -110,7 +110,7 @@ public: landmark), pose); } if (H2) { - (*H2) = numericalDerivative11( + (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 8d55d1ce5..3c95a5010 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -113,7 +113,7 @@ public: landmark), pose); } if (H2) { - (*H2) = numericalDerivative11( + (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index e4b282550..2f94227dc 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -111,7 +111,7 @@ public: (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -234,7 +234,7 @@ public: (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); } if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); } return inverseDepthError(pose1, pose2, landmark); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 7756e79d3..57a19ee78 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -29,31 +29,37 @@ using namespace std; using namespace gtsam; -gtsam::Rot3 world_R_ECEF( +Rot3 world_R_ECEF( 0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); -gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); +Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ -gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { +Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, + const InertialNavFactor_GlobalVelocity& factor) { return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); } -gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { - return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); +Vector predictionErrorVel(const Pose3& p1, const LieVector& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, + const InertialNavFactor_GlobalVelocity& factor) { + return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); } /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Constructor) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -61,8 +67,8 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -72,11 +78,11 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Equals) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -84,8 +90,8 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -97,17 +103,17 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Predict) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -134,17 +140,17 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -170,17 +176,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -205,17 +211,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -262,33 +268,33 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) // -q[1], q[0],0.0); // Matrix J_hyp( -(R1*qx).matrix() ); // -// gtsam::Matrix J_expected; +// Matrix J_expected; // // LieVector v(predictionRq(angles, q)); // -// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // // cout<<"J_hyp"<(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** @@ -347,19 +353,19 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } @@ -368,11 +374,11 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -380,8 +386,8 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -394,11 +400,11 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -406,8 +412,8 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -422,17 +428,17 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -462,17 +468,17 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -501,17 +507,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -540,17 +546,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -589,17 +595,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) /* ************************************************************************* */ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.01); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -640,19 +646,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** @@ -663,19 +669,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 24535854d..503a4b953 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); @@ -76,18 +76,18 @@ TEST( InvDepthFactorVariant1, optimize) { LieVector actual = result.at(landmarkKey); - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// cout << endl << endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// cout << endl << endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// cout << endl << endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; @@ -105,10 +105,10 @@ TEST( InvDepthFactorVariant1, optimize) { world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// cout << endl << endl; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index e99c9bcdf..49e5fc2aa 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -73,19 +73,18 @@ TEST( InvDepthFactorVariant2, optimize) { Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); LieVector actual = result.at(landmarkKey); - - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; @@ -101,12 +100,10 @@ TEST( InvDepthFactorVariant2, optimize) { world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; - - +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Test that the correct landmark parameters have been recovered EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index e65b7cacb..11a91f687 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -39,7 +39,7 @@ TEST( InvDepthFactorVariant3, optimize) { // Create expected landmark Point3 landmark_p1 = pose1.transform_to(landmark); - landmark_p1.print("Landmark in Pose1 Frame:\n"); + // landmark_p1.print("Landmark in Pose1 Frame:\n"); double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5b572fb69..20490a8e4 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, _1, boost::none, boost::none, boost::none, boost::none), *K1); @@ -172,11 +172,11 @@ TEST( ProjectionFactor, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, _1, boost::none, boost::none, boost::none, boost::none), *K1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index ffc7344cf..0d5cb2e36 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -24,8 +24,8 @@ const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; /* ************************************************************************* */ -LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { - return LieVector(factor.evaluateError(x, p)); +Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { + return factor.evaluateError(x, p); } /* ************************************************************************* */ @@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 44dca9b8e..a8a3ae5e9 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::none), point); @@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, _1, boost::none, boost::none, boost::none, boost::none), point); @@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2);