libfovis
internal_utils.hpp
1 #ifndef __fovis_internal_utils_hpp__
2 #define __fovis_internal_utils_hpp__
3 
4 #include <stdio.h>
5 
6 #include <Eigen/Core>
7 #include <Eigen/Geometry>
8 #include <emmintrin.h>
9 
10 #include "options.hpp"
11 
12 namespace fovis
13 {
14 
15 #define FOVIS_IS_ALIGNED16(x) (((uintptr_t)(x) & 0xf) == 0)
16 
17 static inline int
18 round_up_to_multiple(int x, int a)
19 {
20  int rem = x % a;
21  if(rem)
22  return x + (a - rem);
23  return x;
24 }
25 
26 static inline Eigen::Vector3d
27 _quat_to_roll_pitch_yaw(const Eigen::Quaterniond&q)
28 {
29  double roll_a = 2 * (q.w() * q.x() + q.y() * q.z());
30  double roll_b = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
31  double roll = atan2(roll_a, roll_b);
32 
33  double pitch_sin = 2 * (q.w() * q.y() - q.z() * q.x());
34  double pitch = asin(pitch_sin);
35 
36  double yaw_a = 2 * (q.w() * q.z() + q.x() * q.y());
37  double yaw_b = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
38  double yaw = atan2(yaw_a, yaw_b);
39  return Eigen::Vector3d(roll, pitch, yaw);
40 }
41 
42 static inline Eigen::Quaterniond
43 _rpy_to_quat(const Eigen::Vector3d rpy)
44 {
45  double roll = rpy(0), pitch = rpy(1), yaw = rpy(2);
46 
47  double halfroll = roll / 2;
48  double halfpitch = pitch / 2;
49  double halfyaw = yaw / 2;
50 
51  double sin_r2 = sin(halfroll);
52  double sin_p2 = sin(halfpitch);
53  double sin_y2 = sin(halfyaw);
54 
55  double cos_r2 = cos(halfroll);
56  double cos_p2 = cos(halfpitch);
57  double cos_y2 = cos(halfyaw);
58 
59  Eigen::Quaterniond q;
60  q.w() = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
61  q.x() = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
62  q.y() = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
63  q.z() = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
64  return q;
65 }
66 
67 static inline void
68 print_isometry(const Eigen::Isometry3d & iso)
69 {
70  const Eigen::Vector3d & t = iso.translation();
71  Eigen::Vector3d rpy = _quat_to_roll_pitch_yaw(Eigen::Quaterniond(iso.rotation()))*180.0/M_PI;
72  fprintf(stderr, "trans:(% 6.3f % 6.3f % 6.3f) rot:(% 6.3f % 6.3f % 6.3f)",t(0),t(1),t(2),rpy(0),rpy(1),rpy(2));
73  // dbg("rot:(% 6.3f % 6.3f % 6.3f)",rpy(0),rpy(1),rpy(2));
74 }
75 
76 bool optionsGetInt(const VisualOdometryOptions& options, std::string name,
77  int* result);
78 
79 bool optionsGetBool(const VisualOdometryOptions& options, std::string name,
80  bool* result);
81 
82 int optionsGetIntOrFromDefault(const VisualOdometryOptions& options,
83  std::string name, const VisualOdometryOptions& defaults);
84 
85 bool optionsGetBoolOrFromDefault(const VisualOdometryOptions& options,
86  std::string name, const VisualOdometryOptions& defaults);
87 
88 bool optionsGetDouble(const VisualOdometryOptions& options, std::string name,
89  double* result);
90 
91 double optionsGetDoubleOrFromDefault(const VisualOdometryOptions& options,
92  std::string name, const VisualOdometryOptions& defaults);
93 
94 }
95 
96 #endif
std::map< std::string, std::string > VisualOdometryOptions
Options.
Definition: options.hpp:106
quick and dirty profiling tool.inspired by the matlab tic/toc command
Definition: camera_intrinsics.hpp:6
Options.