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() {