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

Added a function for testing charts

release/4.3a0
Paul Furgale 2014-11-24 09:29:14 +01:00
parent 6fc3c450a7
commit a44baac308
4 changed files with 80 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/base/ChartTesting.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam {
// Do a full concept check and test the invertibility of
// local() vs. retract().
template<typename T>
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<T>()(value, "value");
gtsam::traits::print<T>()(other, "other");
// Check for the existence of "equals"
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.
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); }

9
gtsam/geometry/tests/testRot3.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/ChartTesting.h>
#include <boost/math/constants/constants.hpp>
@ -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)
{

46
gtsam_unstable/nonlinear/ExpressionTesting.h
View File

@ -63,41 +63,7 @@ void testExpressionJacobians(TestResult& result_,
ExpressionFactor<T> 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<typename T>
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<T>()(value, "value");
gtsam::traits::print<T>()(other, "other");
// Check for the existence of "equals"
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.
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); }

5
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -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;