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 timePose2.cpp
14 * @brief Time Pose2 geometry
15 * @author Richard Roberts
16 */
17
18#include <iostream>
19
20#include <gtsam/base/timing.h>
21#include <gtsam/geometry/Pose2.h>
22
23using namespace std;
24using namespace gtsam;
25
26/* ************************************************************************* */
27#define TEST(TITLE,STATEMENT) \
28 gttic_(TITLE); \
29 for(int i = 0; i < n; i++) \
30 STATEMENT; \
31 gttoc_(TITLE);
32
33/* ************************************************************************* */
34Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
35 return r1.inverse() * r2;
36}
37
38/* ************************************************************************* */
39Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
40 OptionalJacobian<3,3> H1 = {}, OptionalJacobian<3,3> H2 = {}) {
41 // get cosines and sines from rotation matrices
42 const Rot2& R1 = r1.r(), R2 = r2.r();
43 double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
44
45 // Assert that R1 and R2 are normalized
46 assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
47
48 // Calculate delta rotation = between(R1,R2)
49 double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
50 Rot2 R(Rot2::atan2(y: s,x: c)); // normalizes
51
52 // Calculate delta translation = unrotate(R1, dt);
53 Point2 dt = r2.t() - r1.t();
54 double x = dt.x(), y = dt.y();
55 Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
56
57 // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
58 if (H1) {
59 double dt1 = -s2 * x + c2 * y;
60 double dt2 = -c2 * x - s2 * y;
61 *H1 = (Matrix(3,3) <<
62 -c, -s, dt1,
63 s, -c, dt2,
64 0.0, 0.0,-1.0).finished();
65 }
66 if (H2) *H2 = I_3x3;
67
68 return Pose2(R,t);
69}
70
71/* ************************************************************************* */
72Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
73 OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
74{
75 Pose2 hx = p1.between(g: p2, H1, H2); // h(x)
76 // manifold equivalent of h(x)-z -> log(z,h(x))
77 return measured.localCoordinates(g: hx);
78}
79
80/* ************************************************************************* */
81Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2,
82 OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
83{
84 Pose2 hx = Pose2betweenOptimized(r1: p1, r2: p2, H1, H2); // h(x)
85 // manifold equivalent of h(x)-z -> log(z,h(x))
86 return Pose2::Logmap(p: Pose2betweenOptimized(r1: measured, r2: hx));
87}
88
89/* ************************************************************************* */
90Vector Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Pose2& measured, const Pose2& p1, const Pose2& p2,
91 OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
92{
93 // TODO: Implement
94 Pose2 hx = Pose2betweenOptimized(r1: p1, r2: p2, H1, H2); // h(x)
95 // manifold equivalent of h(x)-z -> log(z,h(x))
96 return Pose2::Logmap(p: Pose2betweenOptimized(r1: measured, r2: hx));
97}
98
99/* ************************************************************************* */
100int main()
101{
102 int n = 50000000;
103 cout << "NOTE: Times are reported for " << n << " calls" << endl;
104
105 // create a random pose:
106 double x = 4.0, y = 2.0, r = 0.3;
107 Vector v = (Vector(3) << x, y, r).finished();
108 Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
109
110 TEST(Expmap, Pose2::Expmap(v));
111 TEST(Retract, X.retract(v));
112 TEST(Logmap, Pose2::Logmap(X2));
113 TEST(localCoordinates, X.localCoordinates(X2));
114
115 Matrix H1, H2;
116 Matrix3 H1f, H2f;
117 TEST(Pose2BetweenFactorEvaluateErrorDefault, Pose2BetweenFactorEvaluateErrorDefault(X3, X, X2, H1, H2));
118 TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetween, Pose2BetweenFactorEvaluateErrorOptimizedBetween(X3, X, X2, H1, H2));
119 TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(X3, X, X2, H1f, H2f));
120
121 // Print timings
122 tictoc_print_();
123
124 return 0;
125}
126