Testing machinery
dellaert
parent
5c0db68152
commit
2624e9876d
|
|
@ -0,0 +1,125 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 chartTesting.h
|
||||
* @brief
|
||||
* @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) {
|
||||
|
||||
G id;
|
||||
Matrix H1, H2;
|
||||
typedef traits_x<G> T;
|
||||
|
||||
// Inverse
|
||||
|
||||
EXPECT(assert_equal(id,T::Inverse(id, H1)));
|
||||
EXPECT(assert_equal(numericalDerivative11(T::Inverse, id),H1));
|
||||
|
||||
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
|
||||
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1));
|
||||
|
||||
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
|
||||
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1));
|
||||
|
||||
// Compose
|
||||
|
||||
EXPECT(assert_equal(t1, T::Compose(id, t1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t1), H2));
|
||||
|
||||
EXPECT(assert_equal(t2, T::Compose(id, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2));
|
||||
|
||||
// Between
|
||||
|
||||
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(id.inverse() * t1,T::Between(id, t1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Between, id, t1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Between, id, t1), H2));
|
||||
|
||||
EXPECT(assert_equal(id.inverse() * t2,T::Between(id, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Between, id, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Between, id, t2), H2));
|
||||
|
||||
// Retract
|
||||
|
||||
typename G::TangentVector z = T::Local(id, id);
|
||||
EXPECT(assert_equal(id, T::Retract(id,z, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, z), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, z), H2));
|
||||
|
||||
typename G::TangentVector w1 = T::Local(id, t1);
|
||||
EXPECT(assert_equal(t1, T::Retract(id,w1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w1), H2));
|
||||
|
||||
typename G::TangentVector w2 = T::Local(id, t2);
|
||||
EXPECT(assert_equal(t2, T::Retract(id,w2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w2), H2));
|
||||
|
||||
typename G::TangentVector w12 = T::Local(t1, t2);
|
||||
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2));
|
||||
|
||||
// Local
|
||||
|
||||
EXPECT(assert_equal(z, id.localCoordinates(id, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, id), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, id), H2));
|
||||
|
||||
EXPECT(assert_equal(w1, id.localCoordinates(t1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t1), H2));
|
||||
|
||||
EXPECT(assert_equal(w2, id.localCoordinates(t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
|
||||
|
||||
}
|
||||
} // 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_LIE_GROUP_DERIVATIVES(t1,t2) \
|
||||
{ gtsam::testLieGroupDerivatives(result_, name_, t1,t2); }
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -769,69 +769,11 @@ TEST(Pose2, align_4) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Pose2 , Traits) {
|
||||
Pose2 id;
|
||||
Pose2 t1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
|
||||
Pose2 t2(M_PI / 2.0, Point2(0.0, 2.0));
|
||||
check_group_invariants(t1, t2);
|
||||
check_manifold_invariants(t1, t2);
|
||||
|
||||
Matrix H1, H2;
|
||||
typedef traits_x<Pose2> T;
|
||||
|
||||
EXPECT(assert_equal(t1, T::Compose(id, t1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t1), H2));
|
||||
|
||||
EXPECT(assert_equal(t2, T::Compose(id, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2));
|
||||
|
||||
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
|
||||
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1));
|
||||
|
||||
Vector3 z = T::Local(id, id);
|
||||
EXPECT(assert_equal(id, T::Retract(id,z, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, z), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, z), H2));
|
||||
|
||||
EXPECT(assert_equal(z, id.localCoordinates(id, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, id), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, id), H2));
|
||||
|
||||
Vector3 w1 = T::Local(id, t1);
|
||||
EXPECT(assert_equal(t1, T::Retract(id,w1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w1), H2));
|
||||
|
||||
EXPECT(assert_equal(w1, id.localCoordinates(t1, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t1), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t1), H2));
|
||||
|
||||
Vector3 w2 = T::Local(id, t2);
|
||||
EXPECT(assert_equal(t2, T::Retract(id,w2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w2), H2));
|
||||
|
||||
EXPECT(assert_equal(w2, id.localCoordinates(t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t2), H2));
|
||||
|
||||
Vector3 w12 = T::Local(t1, t2);
|
||||
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2));
|
||||
|
||||
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1));
|
||||
EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
|
||||
CHECK_LIE_GROUP_DERIVATIVES(t1,t2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue