65 lines
1.9 KiB
C++
65 lines
1.9 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
|
|
|
|
* -------------------------------1------------------------------------------- */
|
|
|
|
/**
|
|
* @file testLie.cpp
|
|
* @date May, 2015
|
|
* @author Frank Dellaert
|
|
* @brief unit tests for Lie group type machinery
|
|
*/
|
|
|
|
#include <gtsam/base/ProductLieGroup.h>
|
|
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/base/testLie.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
//******************************************************************************
|
|
typedef ProductLieGroup<Point2, Point2> MyPoint2Pair;
|
|
|
|
// Define any direct product group to be a model of the multiplicative Group concept
|
|
namespace gtsam {
|
|
template<> struct traits<MyPoint2Pair> : internal::LieGroupTraits<MyPoint2Pair> {
|
|
static void Print(const MyPoint2Pair& m, const string& s = "") {
|
|
cout << s << "(" << m.first << "," << m.second << ")" << endl;
|
|
}
|
|
static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2,
|
|
double tol = 1e-8) {
|
|
return m1 == m2;
|
|
}
|
|
};
|
|
}
|
|
|
|
TEST(Lie, ProductLieGroup) {
|
|
BOOST_CONCEPT_ASSERT((IsGroup<MyPoint2Pair>));
|
|
BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
|
|
BOOST_CONCEPT_ASSERT((IsLieGroup<MyPoint2Pair>));
|
|
MyPoint2Pair pair1;
|
|
Vector4 d;
|
|
d << 1, 2, 3, 4;
|
|
MyPoint2Pair expected(Point2(1, 2), Point2(3, 4));
|
|
MyPoint2Pair pair2 = pair1.retract(d);
|
|
EXPECT(assert_equal(expected, pair2, 1e-9));
|
|
EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
|
|
}
|
|
|
|
//******************************************************************************
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
//******************************************************************************
|
|
|