timePose3.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 
18 #include <iostream>
19 
20 #include <gtsam/base/timing.h>
21 #include <gtsam/geometry/Pose3.h>
22 
23 using namespace std;
24 using 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 int main()
34 {
35  int n = 5000000;
36  cout << "NOTE: Times are reported for " << n << " calls" << endl;
37 
38  double norm=sqrt(1.0+16.0+4.0);
39  double x=1.0/norm, y=4.0/norm, z=2.0/norm;
40  Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1).finished();
41  Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2).finished()), T2 = T.retract(v);
42  Matrix H1,H2;
43 
44  TEST(retract, T.retract(v))
46  TEST(localCoordinates, T.localCoordinates(T2))
47  TEST(between, T.between(T2))
48  TEST(between_derivatives, T.between(T2,H1,H2))
49  TEST(Logmap, Pose3::Logmap(T.between(T2)))
50 
51  // Print timings
52  tictoc_print_();
53 
54  return 0;
55 }
Scalar * y
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Pose2_ Expmap(const Vector3_ &xi)
int main()
Definition: timePose3.cpp:33
Definition: BFloat16.h:88
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
Eigen::VectorXd Vector
Definition: Vector.h:38
Array< int, Dynamic, 1 > v
void tictoc_print_()
Definition: timing.h:268
#define TEST(TITLE, STATEMENT)
Definition: timePose3.cpp:27
traits
Definition: chartTesting.h:28
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
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Timing utilities.


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