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)));
  const auto retract = [](const G& origin, const V& v, const OJ& H1,
                          const OJ& H2) { return T::Retract(origin, v, H1, H2); };
  EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(retract, t1, w12, none, none), H1));
  EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(retract, t1, w12, none, none), H2));
 
  // Local
  EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
  const auto local = [](const G& origin, const G& other, const OJ& H1,
                        const OJ& H2) { return T::Local(origin, other, H1, H2); };
  EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(local, t1, t2, none, none), H1));
  EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(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); }