MRPT  1.9.9
CObservationIMU.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
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 
35  mrpt::serialization::CArchive& in, uint8_t version)
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 }
x-axis acceleration (global/navigation frame) (m/sec2)
z-axis acceleration (global/navigation frame) (m/sec2)
yaw angular velocity (global/navigation frame) (rad/sec)
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Template for column vectors of dynamic size, compatible with Eigen.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
orientation pitch absolute value (global/navigation frame) (rad)
temperature (degrees Celsius)
x magnetic field value (local/vehicle frame) (gauss)
y-axis acceleration (local/vehicle frame) (m/sec2)
STL namespace.
Orientation Quaternion X (global/navigation frame)
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation, raw gyroscope and accelerometer values), altimeters or magnetometers.
z-axis acceleration (local/vehicle frame) (m/sec2)
x-axis velocity (global/navigation frame) (m/sec)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
roll angular velocity (global/navigation frame) (rad/sec)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
angular velocity - x (local/vehicle frame) (rad/sec)
This namespace contains representation of robot actions and observations.
pitch angular velocity (global/navigation frame) (rad/sec)
#define DUMP_IMU_DATA(x)
Orientation Quaternion Y (global/navigation frame)
angular velocity - z (local/vehicle frame) (rad/sec)
Orientation Quaternion Z (global/navigation frame)
z magnetic field value (local/vehicle frame) (gauss)
Orientation Quaternion W (global/navigation frame)
y absolute value (global/navigation frame) (meters)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
y magnetic field value (local/vehicle frame) (gauss)
angular velocity - y (local/vehicle frame) (rad/sec)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
constexpr double RAD2DEG(const double x)
Radians to degrees.
y-axis acceleration (global/navigation frame) (m/sec2)
air pressure (Pascals)
z absolute value (global/navigation frame) (meters)
orientation yaw absolute value (global/navigation frame) (rad)
y-axis velocity (global/navigation frame) (m/sec)
orientation roll absolute value (global/navigation frame) (rad)
void resize(std::size_t N, bool zeroNewElements=false)
yaw angular velocity (local/vehicle frame) (rad/sec)
x absolute value (global/navigation frame) (meters)
roll angular velocity (local/vehicle frame) (rad/sec)
x-axis acceleration (local/vehicle frame) (m/sec2)
z-axis velocity (global/navigation frame) (m/sec)
altitude from an altimeter (meters)
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020