From e5fe5676b183faf389b62daa3990755fc5433883 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 22 Nov 2014 14:10:25 +0100 Subject: [PATCH 1/9] Working on a prototype of wrapping external types --- gtsam/base/Manifold.h | 2 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 13 ++++++++----- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index fb926c630..ba597ccc6 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -235,7 +235,7 @@ struct DefaultChart { static double retract(double origin, const vector& d) { return origin + d[0]; } - static const int getDimension(double) { + static int getDimension(double) { return 1; } }; diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 37a89af6b..f609071cb 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -81,6 +81,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; if (H) { // H should be pre-allocated assert(H->size()==size()); @@ -95,18 +97,19 @@ public: T value = expression_.value(x, map); // <<< Reverse AD happens here ! // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < size(); i++) + for (DenseIndex i = 0; i < static_cast(size()); i++) H->at(i) = Ab(i); - return measurement_.localCoordinates(value); + return chart.local(measurement_, value); } else { const T& value = expression_.value(x); - return measurement_.localCoordinates(value); + return chart.local(measurement_, value); } } virtual boost::shared_ptr linearize(const Values& x) const { - + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); @@ -128,7 +131,7 @@ public: // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + Ab(size()).col(0) = -chart.local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); From 3ef0eabff614bcb06a580572dc915026ff20c7a0 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 22 Nov 2014 14:55:32 +0100 Subject: [PATCH 2/9] Merged in changes from develop --- gtsam_unstable/nonlinear/ExpressionFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index f609071cb..22a2aefeb 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -17,6 +17,8 @@ * @brief Expressions for Block Automatic Differentiation */ +#pragma once + #include #include #include From df7470866f35a7a515ce741861f29c6639083c18 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 07:44:06 +0100 Subject: [PATCH 3/9] More progress on easy Jacobian testing --- gtsam/base/numericalDerivative.h | 42 +++++++ gtsam_unstable/nonlinear/ExpressionTesting.h | 111 ++++++++++++++++++ .../nonlinear/tests/testExpressionFactor.cpp | 25 ++++ 3 files changed, 178 insertions(+) create mode 100644 gtsam_unstable/nonlinear/ExpressionTesting.h diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9339c9c7b..8854b37ba 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -30,6 +30,9 @@ #include #include +#include +#include +#include namespace gtsam { @@ -516,4 +519,43 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), boost::function(f), x1, x2, x3, delta); } + +// The benefit of this method is that it does not need to know what types are involved +// to evaluate the factor. If all the machinery of gtsam is working correctly, we should +// get the correct finite differences out the other side. +template +JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor, + const Values& values, + double fd_step) { + Eigen::VectorXd e = factor.unwhitenedError(values); + const size_t rows = e.size(); + + std::map jacobians; + typename FactorType::const_iterator key_it = factor.begin(); + VectorValues dX = values.zeroVectors(); + for(; key_it != factor.end(); ++key_it) { + size_t key = *key_it; + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for(size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = fd_step; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.unwhitenedError(eval_values); + dx[col] = -fd_step; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.unwhitenedError(eval_values); + J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); } + +} // namespace gtsam + diff --git a/gtsam_unstable/nonlinear/ExpressionTesting.h b/gtsam_unstable/nonlinear/ExpressionTesting.h new file mode 100644 index 000000000..cb825e244 --- /dev/null +++ b/gtsam_unstable/nonlinear/ExpressionTesting.h @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Expression.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Expressions for Block Automatic Differentiation + */ + +#pragma once + +#include "Expression.h" +#include "ExpressionFactor.h" +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +template +void testFactorJacobians(TestResult& result_, + const std::string& name_, + const FactorType& f, + const gtsam::Values& values, + double fd_step, + double tolerance) { + // Check linearization + JacobianFactor expected = computeFiniteDifferenceJacobianFactor(f, values, fd_step); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + + typedef std::pair Jacobian; + Jacobian evalJ = jf->jacobianUnweighted(); + Jacobian estJ = expected.jacobianUnweighted(); + EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); + EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); + EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); +} + +template +void testExpressionJacobians(TestResult& result_, + const std::string& name_, + const gtsam::Expression& expression, + const gtsam::Values& values, + double fd_step, + double tolerance) { + // Create factor + size_t size = traits::dimension::value; + ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + testFactorJacobians(result_, name_, f, values, fd_step, tolerance); +} + + + +// Do a full concept check and test the invertibility of +// local() vs. retract(). +template +void testDefaultChart(const T& value) { + T other = value; + gtsam::traits::print()(value, "value"); + gtsam::traits::print()(other, "other"); + EXPECT_TRUE(gtsam::traits::equals()(value, other, 1e-12)); + + typedef typename gtsam::DefaultChart Chart; + typedef typename Chart::vector Vector; + + Vector dx = Chart::local(value, other); + EXPECT_EQ(Chart::getDimension(value), dx.size()); + + dx.setRandom(); + T updated = Chart::retract(value, dx); + Vector invdx = Chart::local(value, updated); + EXPECT_TRUE(assert_equal(dx, invdx, 1e-9)); + + dx = -dx; + updated = Chart::retract(value, dx); + invdx = Chart::local(value, updated); + EXPECT_TRUE(assert_equal(dx, invdx, 1e-9)); +} + +/// \brief Check the Jacobians produced by a factor against finite differences. +/// \param factor The factor to test. +/// \param values Values filled in for testing the Jacobians. +/// \param finite_difference_step The step to use when computing the finite difference Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, finite_difference_step, tolerance) \ + { testFactorJacobians(result_, name_, factor, values, finite_difference_step, tolerance); } + +/// \brief Check the Jacobians produced by an expression against finite differences. +/// \param expression The expression to test. +/// \param values Values filled in for testing the Jacobians. +/// \param finite_difference_step The step to use when computing the finite difference Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, finite_difference_step, tolerance) \ + { testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); } + +} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c63bfeb6f..d18ef90db 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -425,6 +426,30 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +TEST(ExpressionFactor, tree_finite_differences) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + + const double fd_step = 1e-5; + const double tolerance = 1e-5; + EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); +} + + /* ************************************************************************* */ int main() { TestResult tr; From a8f942f19d6dd1bf9940c773bfb78092f7186a7f Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 07:57:47 +0100 Subject: [PATCH 4/9] Fixing the mag factor tests --- gtsam/navigation/MagFactor.h | 10 +++++----- gtsam/navigation/tests/testMagFactor.cpp | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 96a0971c5..e2a1ebd8c 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -74,7 +74,7 @@ public: */ Vector evaluateError(const Rot2& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b + // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; return (hx - measured_).vector(); } @@ -112,7 +112,7 @@ public: */ Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b + // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_).vector(); } @@ -151,7 +151,7 @@ public: Vector evaluateError(const Point3& nM, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias + // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); @@ -189,11 +189,11 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(double scale, const Unit3& direction, + virtual Vector evaluateError(const double& scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias + // measured bM = nRb� * nM + b, where b is unknown bias Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5599c93d6..6f4040929 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include From 59536e4ff4818a92553bc05fad23c63f4d1ef065 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 08:00:36 +0100 Subject: [PATCH 5/9] Fixing the mag factor tests --- gtsam/navigation/MagFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index e2a1ebd8c..f70bec8c6 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -189,7 +189,7 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - virtual Vector evaluateError(const double& scale, const Unit3& direction, + Vector evaluateError(const double& scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { From 6fc3c450a7c2253dd5dd554c3f16d5617e993561 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 08:18:55 +0100 Subject: [PATCH 6/9] Fixed the chart concept check and cleaned up a bit --- gtsam_unstable/nonlinear/ExpressionTesting.h | 27 +++++++++++++------ .../nonlinear/tests/testExpressionFactor.cpp | 4 +++ 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionTesting.h b/gtsam_unstable/nonlinear/ExpressionTesting.h index cb825e244..29817f885 100644 --- a/gtsam_unstable/nonlinear/ExpressionTesting.h +++ b/gtsam_unstable/nonlinear/ExpressionTesting.h @@ -21,8 +21,8 @@ #include "Expression.h" #include "ExpressionFactor.h" -#include #include +#include #include #include #include @@ -64,32 +64,39 @@ void testExpressionJacobians(TestResult& result_, testFactorJacobians(result_, name_, f, values, fd_step, tolerance); } - - // Do a full concept check and test the invertibility of // local() vs. retract(). template -void testDefaultChart(const T& value) { +void testDefaultChart(TestResult& result_, + const std::string& name_, + const T& value) { T other = value; + // Check for the existence of a print function. gtsam::traits::print()(value, "value"); gtsam::traits::print()(other, "other"); - EXPECT_TRUE(gtsam::traits::equals()(value, other, 1e-12)); + + // Check for the existence of "equals" + EXPECT(gtsam::traits::equals()(value, other, 1e-12)); typedef typename gtsam::DefaultChart Chart; typedef typename Chart::vector Vector; + // Check that the dimension of the local value matches the chart dimension. Vector dx = Chart::local(value, other); - EXPECT_EQ(Chart::getDimension(value), dx.size()); + EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); + // And that the "local" of a value vs. itself is zero. + EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); + // Test the invertibility of retract/local dx.setRandom(); T updated = Chart::retract(value, dx); Vector invdx = Chart::local(value, updated); - EXPECT_TRUE(assert_equal(dx, invdx, 1e-9)); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); dx = -dx; updated = Chart::retract(value, dx); invdx = Chart::local(value, updated); - EXPECT_TRUE(assert_equal(dx, invdx, 1e-9)); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); } /// \brief Check the Jacobians produced by a factor against finite differences. @@ -108,4 +115,8 @@ void testDefaultChart(const T& value) { #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, finite_difference_step, tolerance) \ { testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); } +/// \brief Perform a concept check on the default chart for a type. +/// \param value An instantiation of the type to be tested. +#define CHECK_CHART_CONCEPT(value) \ + { testDefaultChart(result_, name_, value); } } // namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d18ef90db..9634ad24d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -449,6 +449,10 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } +TEST(ExpressionFactor, Pose3Chart) { + Pose3 p3; + CHECK_CHART_CONCEPT(p3); +} /* ************************************************************************* */ int main() { From a44baac308954a10231ec1c0fd60b787bff1cfd2 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 09:29:14 +0100 Subject: [PATCH 7/9] Added a function for testing charts --- gtsam/base/ChartTesting.h | 68 +++++++++++++++++++ gtsam/geometry/tests/testRot3.cpp | 9 +++ gtsam_unstable/nonlinear/ExpressionTesting.h | 46 +------------ .../nonlinear/tests/testExpressionFactor.cpp | 5 -- 4 files changed, 80 insertions(+), 48 deletions(-) create mode 100644 gtsam/base/ChartTesting.h diff --git a/gtsam/base/ChartTesting.h b/gtsam/base/ChartTesting.h new file mode 100644 index 000000000..28c6e85b2 --- /dev/null +++ b/gtsam/base/ChartTesting.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * @file ChartValue.h + * @brief + * @date October, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { +// Do a full concept check and test the invertibility of +// local() vs. retract(). +template +void testDefaultChart(TestResult& result_, + const std::string& name_, + const T& value) { + T other = value; + // Check for the existence of a print function. + gtsam::traits::print()(value, "value"); + gtsam::traits::print()(other, "other"); + + // Check for the existence of "equals" + EXPECT(gtsam::traits::equals()(value, other, 1e-12)); + + typedef typename gtsam::DefaultChart Chart; + typedef typename Chart::vector Vector; + + // Check that the dimension of the local value matches the chart dimension. + Vector dx = Chart::local(value, other); + EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); + // And that the "local" of a value vs. itself is zero. + EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); + + // Test the invertibility of retract/local + dx.setRandom(); + T updated = Chart::retract(value, dx); + Vector invdx = Chart::local(value, updated); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); + + dx = -dx; + updated = Chart::retract(value, dx); + invdx = Chart::local(value, updated); + EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); +} +} // namespace gtsam + + +/// \brief Perform a concept check on the default chart for a type. +/// \param value An instantiation of the type to be tested. +#define CHECK_CHART_CONCEPT(value) \ + { gtsam::testDefaultChart(result_, name_, value); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 63dc75876..c462f3586 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -38,6 +39,14 @@ static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; static const Matrix I3 = eye(3); +/* ************************************************************************* */ +TEST( Rot3, chart) +{ + Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1); + Rot3 rot3(R); + CHECK_CHART_CONCEPT(rot3); +} + /* ************************************************************************* */ TEST( Rot3, constructor) { diff --git a/gtsam_unstable/nonlinear/ExpressionTesting.h b/gtsam_unstable/nonlinear/ExpressionTesting.h index 29817f885..9e45ce8cd 100644 --- a/gtsam_unstable/nonlinear/ExpressionTesting.h +++ b/gtsam_unstable/nonlinear/ExpressionTesting.h @@ -63,41 +63,7 @@ void testExpressionJacobians(TestResult& result_, ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); testFactorJacobians(result_, name_, f, values, fd_step, tolerance); } - -// Do a full concept check and test the invertibility of -// local() vs. retract(). -template -void testDefaultChart(TestResult& result_, - const std::string& name_, - const T& value) { - T other = value; - // Check for the existence of a print function. - gtsam::traits::print()(value, "value"); - gtsam::traits::print()(other, "other"); - - // Check for the existence of "equals" - EXPECT(gtsam::traits::equals()(value, other, 1e-12)); - - typedef typename gtsam::DefaultChart Chart; - typedef typename Chart::vector Vector; - - // Check that the dimension of the local value matches the chart dimension. - Vector dx = Chart::local(value, other); - EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); - // And that the "local" of a value vs. itself is zero. - EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); - - // Test the invertibility of retract/local - dx.setRandom(); - T updated = Chart::retract(value, dx); - Vector invdx = Chart::local(value, updated); - EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); - - dx = -dx; - updated = Chart::retract(value, dx); - invdx = Chart::local(value, updated); - EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); -} +} // namespace gtsam /// \brief Check the Jacobians produced by a factor against finite differences. /// \param factor The factor to test. @@ -105,7 +71,7 @@ void testDefaultChart(TestResult& result_, /// \param finite_difference_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, finite_difference_step, tolerance) \ - { testFactorJacobians(result_, name_, factor, values, finite_difference_step, tolerance); } + { gtsam::testFactorJacobians(result_, name_, factor, values, finite_difference_step, tolerance); } /// \brief Check the Jacobians produced by an expression against finite differences. /// \param expression The expression to test. @@ -113,10 +79,4 @@ void testDefaultChart(TestResult& result_, /// \param finite_difference_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, finite_difference_step, tolerance) \ - { testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); } - -/// \brief Perform a concept check on the default chart for a type. -/// \param value An instantiation of the type to be tested. -#define CHECK_CHART_CONCEPT(value) \ - { testDefaultChart(result_, name_, value); } -} // namespace gtsam + { gtsam::testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 9634ad24d..63745a088 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -449,11 +449,6 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } -TEST(ExpressionFactor, Pose3Chart) { - Pose3 p3; - CHECK_CHART_CONCEPT(p3); -} - /* ************************************************************************* */ int main() { TestResult tr; From 9f68344abbd08c90d0b7924402e074d4dfa67f70 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 12:43:30 +0100 Subject: [PATCH 8/9] Addressed code review --- gtsam/base/{ChartTesting.h => chartTesting.h} | 19 +++-- gtsam/base/numericalDerivative.h | 37 ++++++++- gtsam_unstable/nonlinear/ExpressionTesting.h | 82 ------------------- gtsam_unstable/nonlinear/expressionTesting.h | 53 ++++++++++++ .../nonlinear/tests/testAdaptAutoDiff.cpp | 1 - 5 files changed, 100 insertions(+), 92 deletions(-) rename gtsam/base/{ChartTesting.h => chartTesting.h} (82%) delete mode 100644 gtsam_unstable/nonlinear/ExpressionTesting.h create mode 100644 gtsam_unstable/nonlinear/expressionTesting.h diff --git a/gtsam/base/ChartTesting.h b/gtsam/base/chartTesting.h similarity index 82% rename from gtsam/base/ChartTesting.h rename to gtsam/base/chartTesting.h index 28c6e85b2..d0b8913b5 100644 --- a/gtsam/base/ChartTesting.h +++ b/gtsam/base/chartTesting.h @@ -10,27 +10,34 @@ * -------------------------------------------------------------------------- */ /* - * @file ChartValue.h + * @file chartTesting.h * @brief - * @date October, 2014 + * @date November, 2014 * @author Paul Furgale */ #pragma once #include +#include #include #include #include #include namespace gtsam { -// Do a full concept check and test the invertibility of -// local() vs. retract(). +// Do a full concept check and test the invertibility of local() vs. retract(). template void testDefaultChart(TestResult& result_, const std::string& name_, const T& value) { + typedef typename gtsam::DefaultChart Chart; + typedef typename Chart::vector Vector; + + // First, check the basic chart concept. This checks that the interface is satisfied. + // The rest of the function is even more detailed, checking the correctness of the chart. + BOOST_CONCEPT_ASSERT((ChartConcept)); + T other = value; // Check for the existence of a print function. gtsam::traits::print()(value, "value"); @@ -39,9 +46,6 @@ void testDefaultChart(TestResult& result_, // Check for the existence of "equals" EXPECT(gtsam::traits::equals()(value, other, 1e-12)); - typedef typename gtsam::DefaultChart Chart; - typedef typename Chart::vector Vector; - // Check that the dimension of the local value matches the chart dimension. Vector dx = Chart::local(value, other); EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); @@ -54,6 +58,7 @@ void testDefaultChart(TestResult& result_, Vector invdx = Chart::local(value, updated); EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); + // And test that negative steps work as well. dx = -dx; updated = Chart::retract(value, dx); invdx = Chart::local(value, updated); diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 8854b37ba..30136d9f2 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -34,6 +34,10 @@ #include #include +#include +#include +#include + namespace gtsam { /* @@ -522,9 +526,9 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), // The benefit of this method is that it does not need to know what types are involved // to evaluate the factor. If all the machinery of gtsam is working correctly, we should -// get the correct finite differences out the other side. +// get the correct numerical derivatives out the other side. template -JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor, +JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor, const Values& values, double fd_step) { Eigen::VectorXd e = factor.unwhitenedError(values); @@ -557,5 +561,34 @@ JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor, return JacobianFactor(jacobians, -e); } +template +void testFactorJacobians(TestResult& result_, + const std::string& name_, + const FactorType& f, + const gtsam::Values& values, + double fd_step, + double tolerance) { + // Check linearization + JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + + typedef std::pair Jacobian; + Jacobian evalJ = jf->jacobianUnweighted(); + Jacobian estJ = expected.jacobianUnweighted(); + EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); + EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); + EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); +} + } // namespace gtsam +/// \brief Check the Jacobians produced by a factor against finite differences. +/// \param factor The factor to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ + { gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + diff --git a/gtsam_unstable/nonlinear/ExpressionTesting.h b/gtsam_unstable/nonlinear/ExpressionTesting.h deleted file mode 100644 index 9e45ce8cd..000000000 --- a/gtsam_unstable/nonlinear/ExpressionTesting.h +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file Expression.h - * @date September 18, 2014 - * @author Frank Dellaert - * @author Paul Furgale - * @brief Expressions for Block Automatic Differentiation - */ - -#pragma once - -#include "Expression.h" -#include "ExpressionFactor.h" -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -template -void testFactorJacobians(TestResult& result_, - const std::string& name_, - const FactorType& f, - const gtsam::Values& values, - double fd_step, - double tolerance) { - // Check linearization - JacobianFactor expected = computeFiniteDifferenceJacobianFactor(f, values, fd_step); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - - typedef std::pair Jacobian; - Jacobian evalJ = jf->jacobianUnweighted(); - Jacobian estJ = expected.jacobianUnweighted(); - EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); - EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); - EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); -} - -template -void testExpressionJacobians(TestResult& result_, - const std::string& name_, - const gtsam::Expression& expression, - const gtsam::Values& values, - double fd_step, - double tolerance) { - // Create factor - size_t size = traits::dimension::value; - ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); - testFactorJacobians(result_, name_, f, values, fd_step, tolerance); -} -} // namespace gtsam - -/// \brief Check the Jacobians produced by a factor against finite differences. -/// \param factor The factor to test. -/// \param values Values filled in for testing the Jacobians. -/// \param finite_difference_step The step to use when computing the finite difference Jacobians -/// \param tolerance The numerical tolerance to use when comparing Jacobians. -#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, finite_difference_step, tolerance) \ - { gtsam::testFactorJacobians(result_, name_, factor, values, finite_difference_step, tolerance); } - -/// \brief Check the Jacobians produced by an expression against finite differences. -/// \param expression The expression to test. -/// \param values Values filled in for testing the Jacobians. -/// \param finite_difference_step The step to use when computing the finite difference Jacobians -/// \param tolerance The numerical tolerance to use when comparing Jacobians. -#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, finite_difference_step, tolerance) \ - { gtsam::testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); } diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h new file mode 100644 index 000000000..ae2c1e7e5 --- /dev/null +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file expressionTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Test harness methods for expressions. + */ + +#pragma once + +#include "Expression.h" +#include "ExpressionFactor.h" +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +template +void testExpressionJacobians(TestResult& result_, + const std::string& name_, + const gtsam::Expression& expression, + const gtsam::Values& values, + double nd_step, + double tolerance) { + // Create factor + size_t size = traits::dimension::value; + ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + testFactorJacobians(result_, name_, f, values, nd_step, tolerance); +} +} // namespace gtsam + +/// \brief Check the Jacobians produced by an expression against finite differences. +/// \param expression The expression to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the finite difference Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ + { gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index edb26c4f4..cd281f4d8 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -29,7 +29,6 @@ #include -#undef CHECK #include #include From 386fc1e015c1f6e7653a096d2161e0c843806e01 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 12:44:24 +0100 Subject: [PATCH 9/9] Addressed code review --- gtsam/base/chartTesting.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index d0b8913b5..d2f453521 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -66,7 +66,6 @@ void testDefaultChart(TestResult& result_, } } // namespace gtsam - /// \brief Perform a concept check on the default chart for a type. /// \param value An instantiation of the type to be tested. #define CHECK_CHART_CONCEPT(value) \