From 4790bade85e14f609235264a59776d8587beecc0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 00:19:01 +0100 Subject: [PATCH] Moved and refactored testing --- gtsam/base/numericalDerivative.h | 66 --------------- gtsam_unstable/nonlinear/expressionTesting.h | 87 ++++++++++++++++++-- 2 files changed, 78 insertions(+), 75 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 30136d9f2..f2c4566bb 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const 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 numerical derivatives out the other side. -template -JacobianFactor computeNumericalDerivativeJacobianFactor(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); -} - -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 index ae2c1e7e5..92c8f71e8 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -30,19 +30,88 @@ namespace gtsam { +/** + * Linearize a nonlinear factor using numerical differentiation + * 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 numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a map of Jacobians + std::map jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // 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] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +namespace internal { +// CPPUnitLite-style test for linearization of a factor +void testFactorJacobians(TestResult& result_, const std::string& name_, + const NoiseModelFactor& factor, const gtsam::Values& values, double delta, + double tolerance) { + + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, delta); + + // Create actual value by linearize + boost::shared_ptr actual = // + boost::dynamic_pointer_cast(factor.linearize(values)); + + // Check cast result and then equality + CHECK(actual); + EXPECT( assert_equal(expected, *actual, tolerance)); +} +} + +/// \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::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + +namespace internal { +// CPPUnitLite-style test for linearization of an ExpressionFactor template -void testExpressionJacobians(TestResult& result_, - const std::string& name_, - const gtsam::Expression& expression, - const gtsam::Values& values, - double nd_step, - double tolerance) { +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); + ExpressionFactor f(noiseModel::Unit::Create(size), + expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} // namespace gtsam +} +} // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. /// \param expression The expression to test. @@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_, /// \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); } + { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }