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 }
timing.h
Timing utilities.
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
Pose2.h
2D Pose
Rot2BetweenFactorEvaluateErrorDefault
Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:46
Rot2BetweenFactorEvaluateErrorOptimizedBetween
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:55
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:66
Matrix1
Eigen::Matrix< double, 1, 1 > Matrix1
Definition: timeRot2.cpp:78
simple::R3
static Rot3 R3
Definition: testInitializePose3.cpp:54
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
main
int main()
Definition: timeRot2.cpp:91
TEST
#define TEST(TITLE, STATEMENT)
Definition: timeRot2.cpp:28
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
n
int n
Definition: BiCGSTAB_simple.cpp:1
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
gtsam::Rot2
Definition: Rot2.h:35
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
Rot2betweenDefault
Rot2 Rot2betweenDefault(const Rot2 &r1, const Rot2 &r2)
Definition: timeRot2.cpp:35
std
Definition: BFloat16.h:88
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Rot2betweenOptimized
Rot2 Rot2betweenOptimized(const Rot2 &r1, const Rot2 &r2)
Definition: timeRot2.cpp:40
R
Rot2 R(Rot2::fromAngle(0.1))
Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2 &measured, const Rot2 &p1, const Rot2 &p2, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
Definition: timeRot2.cpp:79


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:11