69 lines
2.1 KiB
C++
69 lines
2.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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); }
|