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
12/**
13 * @file timeRot3.cpp
14 * @brief time Rot3 functions
15 * @author Frank Dellaert
16 */
17
18#include <time.h>
19#include <iostream>
20
21#include <gtsam/geometry/Rot3.h>
22
23using namespace std;
24using 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
35int main() {
36 int n = 100000;
37 clock_t timeLog, timeLog2;
38 double seconds;
39 // create a random direction:
40 double norm = sqrt(x: 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(wx: 0.1, wy: 0.4, wz: 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}
56