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