1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
|
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file timePose2.cpp
* @brief Time Pose2 geometry
* @author Richard Roberts
*/
#include <iostream>
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose2.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
#define TEST(TITLE,STATEMENT) \
gttic_(TITLE); \
for(int i = 0; i < n; i++) \
STATEMENT; \
gttoc_(TITLE);
/* ************************************************************************* */
Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
return r1.inverse() * r2;
}
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = (Matrix(3,3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0).finished();
}
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix3(); (*H1) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
{
Pose2 hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return measured.localCoordinates(hx);
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
{
Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return Pose2::Logmap(Pose2betweenOptimized(measured, hx));
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// TODO: Implement
Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return Pose2::Logmap(Pose2betweenOptimized(measured, hx));
}
/* ************************************************************************* */
int main()
{
int n = 50000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
// create a random pose:
double x = 4.0, y = 2.0, r = 0.3;
Vector v = (Vector(3) << x, y, r).finished();
Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
TEST(Expmap, Pose2::Expmap(v));
TEST(Retract, X.retract(v));
TEST(Logmap, Pose2::Logmap(X2));
TEST(localCoordinates, X.localCoordinates(X2));
Matrix H1, H2;
Matrix3 H1f, H2f;
TEST(Pose2BetweenFactorEvaluateErrorDefault, Pose2BetweenFactorEvaluateErrorDefault(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetween, Pose2BetweenFactorEvaluateErrorOptimizedBetween(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(X3, X, X2, H1f, H2f));
// Print timings
tictoc_print_();
return 0;
}
|
