timeRot3.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 <time.h>
19 #include <iostream>
20 
21 #include <gtsam/geometry/Rot3.h>
22 
23 using namespace std;
24 using namespace gtsam;
25 
26 #define TEST(TITLE, STATEMENT) \
27  cout << endl << TITLE << endl; \
28  timeLog = clock(); \
29  for (int i = 0; i < n; i++) STATEMENT; \
30  timeLog2 = clock(); \
31  seconds = static_cast<double>(timeLog2 - timeLog) / CLOCKS_PER_SEC; \
32  cout << 1000 * seconds << " milliseconds" << endl; \
33  cout << (1e9 * seconds / static_cast<double>(n)) << " nanosecs/call" << endl;
34 
35 int main() {
36  int n = 100000;
37  clock_t timeLog, timeLog2;
38  double seconds;
39  // create a random direction:
40  double norm = sqrt(1.0 + 16.0 + 4.0);
41  double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
42  Vector v = (Vector(3) << x, y, z).finished();
43  Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v);
44 
45  TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001))
46  TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v))
47  TEST("Expmap", R * Rot3::Expmap(v))
48  TEST("Retract", R.retract(v))
49  TEST("Logmap", Rot3::Logmap(R.between(R2)))
50  TEST("localCoordinates", R.localCoordinates(R2))
51  TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x))
52  TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z))
53 
54  return 0;
55 }
Scalar * y
int n
Rot2 R(Rot2::fromAngle(0.1))
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
#define TEST(TITLE, STATEMENT)
Definition: timeRot3.cpp:26
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
int main()
Definition: timeRot3.cpp:35
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
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
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
3D rotation represented as a rotation matrix or quaternion


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