1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file timeRot2.cpp
14 * @brief Time Rot2 geometry
15 * @author Richard Roberts
16 */
17
18
19#include <gtsam/geometry/Pose2.h>
20#include <gtsam/base/timing.h>
21#include <iostream>
22#include "gtsam/base/OptionalJacobian.h"
23
24using namespace std;
25using namespace gtsam;
26
27/* ************************************************************************* */
28#define TEST(TITLE,STATEMENT) \
29 gttic_(TITLE); \
30 for(int i = 0; i < n; i++) \
31 STATEMENT; \
32 gttoc_(TITLE);
33
34/* ************************************************************************* */
35Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) {
36 return r1.inverse() * r2;
37}
38
39/* ************************************************************************* */
40Rot2 Rot2betweenOptimized(const Rot2& r1, const Rot2& r2) {
41 // Same as compose but sign of sin for r1 is reversed
42 return Rot2::fromCosSin(c: r1.c() * r2.c() + r1.s() * r2.s(), s: -r1.s() * r2.c() + r1.c() * r2.s());
43}
44
45/* ************************************************************************* */
46Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p1, const Rot2& p2,
47 OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
48{
49 Rot2 hx = p1.between(g: p2, H1, H2); // h(x)
50 // manifold equivalent of h(x)-z -> log(z,h(x))
51 return measured.localCoordinates(g: hx);
52}
53
54/* ************************************************************************* */
55Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, const Rot2& p1, const Rot2& p2,
56 OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
57{
58 Rot2 hx = Rot2betweenOptimized(r1: p1, r2: p2); // h(x)
59 if (H1) *H1 = -I_1x1;
60 if (H2) *H2 = I_1x1;
61 // manifold equivalent of h(x)-z -> log(z,h(x))
62 return Rot2::Logmap(r: Rot2betweenOptimized(r1: measured, r2: hx));
63}
64
65/* ************************************************************************* */
66Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, const Rot2& p1, const Rot2& p2,
67 OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
68{
69 // TODO: Implement
70 Rot2 hx = Rot2betweenOptimized(r1: p1, r2: p2); // h(x)
71 if (H1) *H1 = -I_1x1;
72 if (H2) *H2 = I_1x1;
73 // manifold equivalent of h(x)-z -> log(z,h(x))
74 return Rot2::Logmap(r: Rot2betweenOptimized(r1: measured, r2: hx));
75}
76
77/* ************************************************************************* */
78typedef Eigen::Matrix<double,1,1> Matrix1;
79Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2& measured, const Rot2& p1, const Rot2& p2,
80 OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
81{
82 // TODO: Implement
83 Rot2 hx = Rot2betweenOptimized(r1: p1, r2: p2); // h(x)
84 if (H1) *H1 = -Matrix1::Identity();
85 if (H2) *H2 = Matrix1::Identity();
86 // manifold equivalent of h(x)-z -> log(z,h(x))
87 return Rot2::Logmap(r: Rot2betweenOptimized(r1: measured, r2: hx));
88}
89
90/* ************************************************************************* */
91int main()
92{
93 int n = 50000000;
94 cout << "NOTE: Times are reported for " << n << " calls" << endl;
95
96 Vector1 v; v << 0.1;
97 Rot2 R = Rot2(0.4), R2(0.5), R3(0.6);
98
99 TEST(Rot2_Expmap, Rot2::Expmap(v));
100 TEST(Rot2_Retract, R.retract(v));
101 TEST(Rot2_Logmap, Rot2::Logmap(R2));
102 TEST(Rot2_between, R.between(R2));
103 TEST(betweenDefault, Rot2betweenDefault(R, R2));
104 TEST(betweenOptimized, Rot2betweenOptimized(R, R2));
105 TEST(Rot2_localCoordinates, R.localCoordinates(R2));
106
107 Matrix H1, H2;
108 Matrix1 H1f, H2f;
109 TEST(Rot2BetweenFactorEvaluateErrorDefault, Rot2BetweenFactorEvaluateErrorDefault(R3, R, R2, H1, H2));
110 TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetween, Rot2BetweenFactorEvaluateErrorOptimizedBetween(R3, R, R2, H1, H2));
111 TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye, Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(R3, R, R2, H1, H2));
112 TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(R3, R, R2, H1f, H2f));
113
114 // Print timings
115 tictoc_print_();
116
117 return 0;
118}
119