Main MRPT website > C++ reference for MRPT 1.9.9
CObservationIMU.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 //#include <mrpt/math/CMatrixD.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::poses;
18 
19 // This must be added to any CSerializable class implementation file.
21 
22 uint8_t CObservationIMU::serializeGetVersion() const { return 3; }
24 {
25  // v1->v2 was only done to fix a bug in the ordering of
26  // YAW/PITCH/ROLL rates.
27  out << sensorPose << dataIsPresent << timestamp;
28  out << rawMeasurements;
29  // Version 3: Added 6 new raw measurements (IMU_MAG_X=15 to
30  // IMU_TEMPERATURE=20)
31  out << sensorLabel;
32 }
33 
36 {
37  switch (version)
38  {
39  case 0:
40  case 1:
41  case 2:
42  case 3:
43  in >> sensorPose;
44  in >> dataIsPresent;
45 
46  in >> timestamp;
47 
48  // In version 0 it was a vector of floats:
49  if (version < 1)
50  {
52  in >> tmp;
53  rawMeasurements.resize(tmp.size());
54  for (size_t i = 0; i < rawMeasurements.size(); i++)
55  rawMeasurements[i] = tmp[i];
56  }
57  else
58  {
59  in >> rawMeasurements;
60  }
61 
62  if (version < 2)
63  {
64  // A bug in the grabbing from XSens IMU's made /ROLL rates to be
65  // stored in the wrong order:
66  std::swap(
67  rawMeasurements[IMU_YAW_VEL],
68  rawMeasurements[IMU_ROLL_VEL]);
69  }
70  else
71  {
72  // v2: nothing to do, data is already in the right order.
73  }
74 
75  in >> sensorLabel;
76 
77  // Fill new entries with default values:
78  if (dataIsPresent.size() < COUNT_IMU_DATA_FIELDS)
79  {
80  const size_t nOld = dataIsPresent.size();
81  ASSERT_(rawMeasurements.size() == dataIsPresent.size());
82 
83  dataIsPresent.resize(COUNT_IMU_DATA_FIELDS);
84  rawMeasurements.resize(COUNT_IMU_DATA_FIELDS);
85  for (size_t i = nOld; i < COUNT_IMU_DATA_FIELDS; i++)
86  {
87  dataIsPresent[i] = false;
88  rawMeasurements[i] = 0;
89  }
90  }
91  break;
92  default:
94  };
95 }
96 
97 void CObservationIMU::getDescriptionAsText(std::ostream& o) const
98 {
99  using namespace std;
100  ;
102 
103  o << "Sensor pose on the robot: " << sensorPose << endl;
104 
105  o << format(
106  "Orientation (degrees): (yaw,pitch,roll)=(%.06f, %.06f, %.06f)\n\n",
107  RAD2DEG(rawMeasurements[IMU_YAW]), RAD2DEG(rawMeasurements[IMU_PITCH]),
108  RAD2DEG(rawMeasurements[IMU_ROLL]));
109 
110  // Units:
111  // Use "COUNT_IMU_DATA_FIELDS" so a compile error happens if the sizes don't
112  // fit ;-)
113  static const char* imu_units[mrpt::obs::COUNT_IMU_DATA_FIELDS] = {
114  "m/s^2", // IMU_X_ACC,
115  "m/s^2", // IMU_Y_ACC,
116  "m/s^2", // IMU_Z_ACC,
117  "rad/s", // IMU_YAW_VEL,
118  "rad/s", // IMU_PITCH_VEL,
119  "rad/s", // IMU_ROLL_VEL,
120  "m/s", // IMU_X_VEL,
121  "m/s", // IMU_Y_VEL,
122  "m/s", // IMU_Z_VEL,
123  "rad", // IMU_YAW,
124  "rad", // IMU_PITCH,
125  "rad", // IMU_ROLL,
126  "m", // IMU_X,
127  "m", // IMU_Y,
128  "m", // IMU_Z
129  "gauss", // IMU_MAG_X,
130  "gauss", // IMU_MAG_Y,
131  "gauss", // IMU_MAG_Z,
132  "Pa", // IMU_PRESSURE,
133  "m", // IMU_ALTITUDE,
134  "deg.", // IMU_TEMPERATURE,
135  "qx", // IMU_ORI_QUAT_X,
136  "qy", // IMU_ORI_QUAT_Y,
137  "qz", // IMU_ORI_QUAT_Z,
138  "qw", // IMU_ORI_QUAT_W,
139  "rad/s", // IMU_YAW_VEL_GLOBAL
140  "rad/s", // IMU_PITCH_VEL_GLOBAL
141  "rad/s", // IMU_ROLL_VEL_GLOBAL
142  "m/s^2", // IMU_X_ACC_GLOBAL
143  "m/s^2", // IMU_Y_ACC_GLOBAL
144  "m/s^2" // IMU_Z_ACC_GLOBAL
145  };
146 
147 #define DUMP_IMU_DATA(x) \
148  o << format("%15s = ", #x); \
149  if (dataIsPresent[x]) \
150  o << format("%10f %s\n", rawMeasurements[x], imu_units[x]); \
151  else \
152  o << "(not present)\n";
153 
185 }
mrpt::obs::IMU_YAW_VEL_GLOBAL
@ IMU_YAW_VEL_GLOBAL
yaw angular velocity (global/navigation frame) (rad/sec)
Definition: CObservationIMU.h:79
mrpt::obs::IMU_X_ACC_GLOBAL
@ IMU_X_ACC_GLOBAL
x-axis acceleration (global/navigation frame) (m/sec2)
Definition: CObservationIMU.h:85
mrpt::obs::IMU_MAG_Y
@ IMU_MAG_Y
y magnetic field value (local/vehicle frame) (gauss)
Definition: CObservationIMU.h:61
mrpt::obs::IMU_ALTITUDE
@ IMU_ALTITUDE
altitude from an altimeter (meters)
Definition: CObservationIMU.h:67
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::obs::IMU_Z_VEL
@ IMU_Z_VEL
z-axis velocity (global/navigation frame) (m/sec)
Definition: CObservationIMU.h:45
mrpt::obs::IMU_PITCH
@ IMU_PITCH
orientation pitch absolute value (global/navigation frame) (rad)
Definition: CObservationIMU.h:49
mrpt::obs::IMU_MAG_X
@ IMU_MAG_X
x magnetic field value (local/vehicle frame) (gauss)
Definition: CObservationIMU.h:59
mrpt::obs::IMU_ROLL_VEL
@ IMU_ROLL_VEL
roll angular velocity (local/vehicle frame) (rad/sec)
Definition: CObservationIMU.h:39
mrpt::obs::IMU_ROLL
@ IMU_ROLL
orientation roll absolute value (global/navigation frame) (rad)
Definition: CObservationIMU.h:51
obs-precomp.h
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::obs::IMU_PRESSURE
@ IMU_PRESSURE
air pressure (Pascals)
Definition: CObservationIMU.h:65
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::obs::CObservationIMU
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation,...
Definition: CObservationIMU.h:108
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::obs::IMU_ORI_QUAT_W
@ IMU_ORI_QUAT_W
Orientation Quaternion W (global/navigation frame)
Definition: CObservationIMU.h:77
mrpt::obs::IMU_PITCH_VEL
@ IMU_PITCH_VEL
pitch angular velocity (local/vehicle frame) (rad/sec)
Definition: CObservationIMU.h:37
mrpt::obs::IMU_Y_VEL
@ IMU_Y_VEL
y-axis velocity (global/navigation frame) (m/sec)
Definition: CObservationIMU.h:43
mrpt::obs::CObservationIMU::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CObservationIMU.cpp:34
mrpt::obs::CObservation::getDescriptionAsText
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservation.cpp:44
mrpt::obs::COUNT_IMU_DATA_FIELDS
@ COUNT_IMU_DATA_FIELDS
Definition: CObservationIMU.h:92
mrpt::obs::IMU_Z_ACC
@ IMU_Z_ACC
z-axis acceleration (local/vehicle frame) (m/sec2)
Definition: CObservationIMU.h:33
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::obs::IMU_MAG_Z
@ IMU_MAG_Z
z magnetic field value (local/vehicle frame) (gauss)
Definition: CObservationIMU.h:63
mrpt::obs::IMU_ORI_QUAT_Y
@ IMU_ORI_QUAT_Y
Orientation Quaternion Y (global/navigation frame)
Definition: CObservationIMU.h:73
mrpt::obs::IMU_TEMPERATURE
@ IMU_TEMPERATURE
temperature (degrees Celsius)
Definition: CObservationIMU.h:69
mrpt::obs::IMU_Y
@ IMU_Y
y absolute value (global/navigation frame) (meters)
Definition: CObservationIMU.h:55
mrpt::obs::IMU_Z_ACC_GLOBAL
@ IMU_Z_ACC_GLOBAL
z-axis acceleration (global/navigation frame) (m/sec2)
Definition: CObservationIMU.h:89
mrpt::obs::IMU_X_ACC
@ IMU_X_ACC
x-axis acceleration (local/vehicle frame) (m/sec2)
Definition: CObservationIMU.h:29
mrpt::obs::IMU_ORI_QUAT_Z
@ IMU_ORI_QUAT_Z
Orientation Quaternion Z (global/navigation frame)
Definition: CObservationIMU.h:75
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:114
CObservationIMU.h
mrpt::obs::CObservationIMU::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CObservationIMU.cpp:23
mrpt::obs::IMU_Y_ACC
@ IMU_Y_ACC
y-axis acceleration (local/vehicle frame) (m/sec2)
Definition: CObservationIMU.h:31
mrpt::obs::IMU_X
@ IMU_X
x absolute value (global/navigation frame) (meters)
Definition: CObservationIMU.h:53
mrpt::obs::IMU_PITCH_VEL_GLOBAL
@ IMU_PITCH_VEL_GLOBAL
pitch angular velocity (global/navigation frame) (rad/sec)
Definition: CObservationIMU.h:81
mrpt::obs::CObservationIMU::getDescriptionAsText
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservationIMU.cpp:97
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::obs::IMU_YAW
@ IMU_YAW
orientation yaw absolute value (global/navigation frame) (rad)
Definition: CObservationIMU.h:47
in
GLuint in
Definition: glext.h:7274
mrpt::obs::IMU_ROLL_VEL_GLOBAL
@ IMU_ROLL_VEL_GLOBAL
roll angular velocity (global/navigation frame) (rad/sec)
Definition: CObservationIMU.h:83
CArchive.h
mrpt::obs::IMU_YAW_VEL
@ IMU_YAW_VEL
yaw angular velocity (local/vehicle frame) (rad/sec)
Definition: CObservationIMU.h:35
mrpt::obs::IMU_Y_ACC_GLOBAL
@ IMU_Y_ACC_GLOBAL
y-axis acceleration (global/navigation frame) (m/sec2)
Definition: CObservationIMU.h:87
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::obs::IMU_Z
@ IMU_Z
z absolute value (global/navigation frame) (meters)
Definition: CObservationIMU.h:57
mrpt::obs::IMU_ORI_QUAT_X
@ IMU_ORI_QUAT_X
Orientation Quaternion X (global/navigation frame)
Definition: CObservationIMU.h:71
mrpt::obs::IMU_X_VEL
@ IMU_X_VEL
x-axis velocity (global/navigation frame) (m/sec)
Definition: CObservationIMU.h:41
DUMP_IMU_DATA
#define DUMP_IMU_DATA(x)



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST