12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Addressed code review

release/4.3a0
Paul Furgale 2014-11-24 12:43:30 +01:00
parent a44baac308
commit 9f68344abb
5 changed files with 100 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/base/ChartTesting.h → gtsam/base/chartTesting.h
View File

@ -10,27 +10,34 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /*
* @file ChartValue.h * @file chartTesting.h
* @brief * @brief
* @date October, 2014 * @date November, 2014
* @author Paul Furgale * @author Paul Furgale
*/ */
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestResult.h> #include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h> #include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h> #include <CppUnitLite/Failure.h>
namespace gtsam { namespace gtsam {
// Do a full concept check and test the invertibility of // Do a full concept check and test the invertibility of local() vs. retract().
// local() vs. retract().
template<typename T> template<typename T>
void testDefaultChart(TestResult& result_, void testDefaultChart(TestResult& result_,
const std::string& name_, const std::string& name_,
const T& value) { const T& value) {
typedef typename gtsam::DefaultChart<T> 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<Chart>));
T other = value; T other = value;
// Check for the existence of a print function. // Check for the existence of a print function.
gtsam::traits::print<T>()(value, "value"); gtsam::traits::print<T>()(value, "value");
@ -39,9 +46,6 @@ void testDefaultChart(TestResult& result_,
// Check for the existence of "equals" // Check for the existence of "equals"
EXPECT(gtsam::traits::equals<T>()(value, other, 1e-12)); EXPECT(gtsam::traits::equals<T>()(value, other, 1e-12));
typedef typename gtsam::DefaultChart<T> Chart;
typedef typename Chart::vector Vector;
// Check that the dimension of the local value matches the chart dimension. // Check that the dimension of the local value matches the chart dimension.
Vector dx = Chart::local(value, other); Vector dx = Chart::local(value, other);
EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size());
@ -54,6 +58,7 @@ void testDefaultChart(TestResult& result_,
Vector invdx = Chart::local(value, updated); Vector invdx = Chart::local(value, updated);
EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9)); EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
// And test that negative steps work as well.
dx = -dx; dx = -dx;
updated = Chart::retract(value, dx); updated = Chart::retract(value, dx);
invdx = Chart::local(value, updated); invdx = Chart::local(value, updated);

37
gtsam/base/numericalDerivative.h
View File

@ -34,6 +34,10 @@
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam { 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 // 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 // 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<typename FactorType> template<typename FactorType>
JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor, JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor,
const Values& values, const Values& values,
double fd_step) { double fd_step) {
Eigen::VectorXd e = factor.unwhitenedError(values); Eigen::VectorXd e = factor.unwhitenedError(values);
@ -557,5 +561,34 @@ JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor,
return JacobianFactor(jacobians, -e); return JacobianFactor(jacobians, -e);
} }
template<typename FactorType>
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<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> 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 } // 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); }

82
gtsam_unstable/nonlinear/ExpressionTesting.h
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
#include <gtsam/base/numericalDerivative.h>
namespace gtsam {
template<typename FactorType>
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<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> 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<typename T>
void testExpressionJacobians(TestResult& result_,
const std::string& name_,
const gtsam::Expression<T>& expression,
const gtsam::Values& values,
double fd_step,
double tolerance) {
// Create factor
size_t size = traits::dimension<T>::value;
ExpressionFactor<T> 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); }

53
gtsam_unstable/nonlinear/expressionTesting.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
#include <gtsam/base/numericalDerivative.h>
namespace gtsam {
template<typename T>
void testExpressionJacobians(TestResult& result_,
const std::string& name_,
const gtsam::Expression<T>& expression,
const gtsam::Values& values,
double nd_step,
double tolerance) {
// Create factor
size_t size = traits::dimension<T>::value;
ExpressionFactor<T> 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); }

1
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -29,7 +29,6 @@
#include <gtsam_unstable/nonlinear/ceres_example.h> #include <gtsam_unstable/nonlinear/ceres_example.h>
#undef CHECK
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>