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 }
timing.h
Timing utilities.
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
n
int n
Definition: BiCGSTAB_simple.cpp:1
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Pose3
Definition: Pose3.h:37
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
main
int main()
Definition: timePose3.cpp:33
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Eigen::Triplet< double >
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam
traits
Definition: SFMdata.h:40
std
Definition: BFloat16.h:88
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TEST
#define TEST(TITLE, STATEMENT)
Definition: timePose3.cpp:27
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Pose3.h
3D Pose


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