timeRot2.cpp
Go to the documentation of this file.
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 
19 #include <gtsam/geometry/Pose2.h>
20 #include <gtsam/base/timing.h>
21 #include <iostream>
23 
24 using namespace std;
25 using 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 /* ************************************************************************* */
35 Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) {
36  return r1.inverse() * r2;
37 }
38 
39 /* ************************************************************************* */
41  // Same as compose but sign of sin for r1 is reversed
42  return Rot2::fromCosSin(r1.c() * r2.c() + r1.s() * r2.s(), -r1.s() * r2.c() + r1.c() * r2.s());
43 }
44 
45 /* ************************************************************************* */
48 {
49  Rot2 hx = p1.between(p2, H1, H2); // h(x)
50  // manifold equivalent of h(x)-z -> log(z,h(x))
51  return measured.localCoordinates(hx);
52 }
53 
54 /* ************************************************************************* */
57 {
58  Rot2 hx = Rot2betweenOptimized(p1, 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(Rot2betweenOptimized(measured, hx));
63 }
64 
65 /* ************************************************************************* */
68 {
69  // TODO: Implement
70  Rot2 hx = Rot2betweenOptimized(p1, 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(Rot2betweenOptimized(measured, hx));
75 }
76 
77 /* ************************************************************************* */
81 {
82  // TODO: Implement
83  Rot2 hx = Rot2betweenOptimized(p1, 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(Rot2betweenOptimized(measured, hx));
88 }
89 
90 /* ************************************************************************* */
91 int 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;
113 
114  // Print timings
115  tictoc_print_();
116 
117  return 0;
118 }
Point2 measured(-17, 30)
double c() const
Definition: Rot2.h:197
Rot2 Rot2betweenOptimized(const Rot2 &r1, const Rot2 &r2)
Definition: timeRot2.cpp:40
Vector3f p1
Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:46
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:79
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
#define TEST(TITLE, STATEMENT)
Definition: timeRot2.cpp:28
Eigen::Matrix< double, 1, 1 > Matrix1
Definition: timeRot2.cpp:78
int main()
Definition: timeRot2.cpp:91
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
double s() const
Definition: Rot2.h:202
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:66
Eigen::VectorXd Vector
Definition: Vector.h:38
Array< int, Dynamic, 1 > v
void tictoc_print_()
Definition: timing.h:268
static const double r2
traits
Definition: chartTesting.h:28
static Rot3 R3
static const double r1
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Class between(const Class &g) const
Definition: Lie.h:52
Rot2 inverse() const
Definition: Rot2.h:113
static Point3 p2
Special class for optional Jacobian arguments.
2D Pose
The matrix class, also used for vectors and row-vectors.
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:55
Timing utilities.
Rot2 Rot2betweenDefault(const Rot2 &r1, const Rot2 &r2)
Definition: timeRot2.cpp:35


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:03