1/**
2 * @file testBearingS2.cpp
3 *
4 * @brief Tests for a bearing measurement on S2 for 3D bearing measurements
5 *
6 * @date Jan 26, 2012
7 * @author Alex Cunningham
8 */
9
10#include <CppUnitLite/TestHarness.h>
11
12#include <gtsam/base/TestableAssertions.h>
13
14#include <gtsam_unstable/geometry/BearingS2.h>
15
16using namespace gtsam;
17
18static const double tol=1e-5;
19
20/* ************************************************************************* */
21TEST( testBearingS2, basics ) {
22 BearingS2 origin;
23 EXPECT(assert_equal(Rot2(), origin.elevation(), tol));
24 EXPECT(assert_equal(Rot2(), origin.azimuth(), tol));
25
26 double expAzi = 0.2, expEle = 0.3;
27 BearingS2 actual1(expAzi, expEle);
28 EXPECT(assert_equal(Rot2::fromAngle(expEle), actual1.elevation(), tol));
29 EXPECT(assert_equal(Rot2::fromAngle(expAzi), actual1.azimuth(), tol));
30}
31
32/* ************************************************************************* */
33TEST( testBearingS2, equals ) {
34 BearingS2 origin, b1(0.2, 0.3), b2(b1), b3(0.1, 0.3), b4(0.2, 0.5);
35 EXPECT(assert_equal(origin, origin, tol));
36 EXPECT(assert_equal(b1, b1, tol));
37 EXPECT(assert_equal(b1, b2, tol));
38 EXPECT(assert_equal(b2, b1, tol));
39
40 EXPECT(assert_inequal(b1, b3, tol));
41 EXPECT(assert_inequal(b3, b1, tol));
42 EXPECT(assert_inequal(b1, b4, tol));
43 EXPECT(assert_inequal(b4, b1, tol));
44}
45
46/* ************************************************************************* */
47TEST( testBearingS2, manifold ) {
48 BearingS2 origin, b1(0.2, 0.3);
49 EXPECT_LONGS_EQUAL(2, origin.dim());
50
51 EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol));
52 EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol));
53
54 EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol));
55 EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol));
56}
57
58/* ************************************************************************* */
59int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
60/* ************************************************************************* */
61