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

More tests

release/4.3a0
Frank Dellaert 2015-07-12 00:12:06 -07:00
parent 48169e7776
commit fcbceaf746
1 changed files with 23 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/geometry/tests/testUnit3.cpp
View File

@ -158,20 +158,39 @@ TEST(Unit3, distance) {
TEST(Unit3, localCoordinates) {
{
Unit3 p;
Vector2 actual = p.localCoordinates(p);
Unit3 p, q;
Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{
Unit3 p, q(1, 6.12385e-21, 0);
Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q);
CHECK(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{
Unit3 p, q(-1, 0, 0);
Vector2 expected(M_PI, 0);
Vector2 actual = p.localCoordinates(q);
CHECK(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{
Unit3 p, q(0, 1, 0);
Vector2 expected(0,-M_PI_2);
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{
Unit3 p, q(0, -1, 0);
Vector2 expected(0, M_PI_2);
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
double twist = 1e-4;