12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made testLie a bit stronger

release/4.3a0
dellaert 2015-05-26 01:53:58 -07:00
parent d385984f26
commit 6a67ea86af
1 changed files with 62 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
tests/testLie.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/testLie.h>
@ -26,35 +27,81 @@
using namespace std;
using namespace gtsam;
static const double tol = 1e-9;
//******************************************************************************
typedef ProductLieGroup<Point2, Point2> MyPoint2Pair;
typedef ProductLieGroup<Point2, Pose2> Product;
// 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;
template<> struct traits<Product> : internal::LieGroupTraits<Product> {
static void Print(const Product& m, const string& s = "") {
cout << s << "(" << m.first << "," << m.second.translation() << "/"
<< m.second.theta() << ")" << endl;
}
static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2,
double tol = 1e-8) {
return m1 == m2;
static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) {
return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol);
}
};
}
//******************************************************************************
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);
BOOST_CONCEPT_ASSERT((IsGroup<Product>));
BOOST_CONCEPT_ASSERT((IsManifold<Product>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Product>));
Product pair1;
Vector5 d;
d << 1, 2, 0.1, 0.2, 0.3;
Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
Product pair2 = pair1.retract(d);
EXPECT(assert_equal(expected, pair2, 1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
}
/* ************************************************************************* */
Product compose_proxy(const Product& A, const Product& B) {
return A.compose(B);
}
TEST( testProduct, compose ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
Matrix actH1, actH2;
state1.compose(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
Product between_proxy(const Product& A, const Product& B) {
return A.between(B);
}
TEST( testProduct, between ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
Matrix actH1, actH2;
state1.between(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
Product inverse_proxy(const Product& A) {
return A.inverse();
}
TEST( testProduct, inverse ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5));
Matrix actH1;
state1.inverse(actH1);
Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
EXPECT(assert_equal(numericH1, actH1, tol));
}
//******************************************************************************
int main() {
TestResult tr;