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 timePose3.cpp
14 * @brief time Pose3 functions
15 * @author Frank Dellaert
16 */
17
18#include <iostream>
19
20#include <gtsam/base/timing.h>
21#include <gtsam/geometry/Pose3.h>
22
23using namespace std;
24using 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
33int main()
34{
35 int n = 5000000;
36 cout << "NOTE: Times are reported for " << n << " calls" << endl;
37
38 double norm=sqrt(x: 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(xi: (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))
45 TEST(Expmap, T*Pose3::Expmap(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}
56