Loading...
Searching...
No Matches
testLie.h
/* ----------------------------------------------------------------------------
* 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 testLie.h
* @brief Test utilities for Lie groups
* @date November, 2014
* @author Paul Furgale
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam {
// Do a comprehensive test of Lie Group derivatives
template<typename G>
void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2) {
Matrix H1, H2;
typedef traits<G> T;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Inverse
OJ none;
EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
// Compose
EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
// Between
EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2) {
Matrix H1, H2;
typedef traits<G> T;
typedef typename T::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract
OJ none;
V w12 = T::Local(t1, t2);
EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local
EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
}
} // namespace gtsam
#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
#define CHECK_CHART_DERIVATIVES(t1,t2) \
{ gtsam::testChartDerivatives(result_, name_, t1, t2); }
Lie.h
Base class and basic functions for Lie types.
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 1 of binary function.
Definition numericalDerivative.h:166
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 1 of 4-argument function.
Definition numericalDerivative.h:325
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Compute numerical derivative in argument 2 of 4-argument function.
Definition numericalDerivative.h:359
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41