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;