MRPT  2.0.4
CObservationRange.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 
14 
15 using namespace mrpt::obs;
16 using namespace mrpt::poses;
17 
18 // This must be added to any CSerializable class implementation file.
20 
21 uint8_t CObservationRange::serializeGetVersion() const { return 3; }
23 {
24  // The data
25  out << minSensorDistance << maxSensorDistance << sensorConeApperture;
26  const uint32_t n = sensedData.size();
27  out << n;
28  for (uint32_t i = 0; i < n; i++)
29  out << sensedData[i].sensorID << CPose3D(sensedData[i].sensorPose)
30  << sensedData[i].sensedDistance;
31  out << sensorLabel << timestamp;
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  {
44  uint32_t i, n;
45 
46  // The data
47  in >> minSensorDistance >> maxSensorDistance >> sensorConeApperture;
48 
49  in >> n;
50  sensedData.resize(n);
51  CPose3D aux;
52  for (i = 0; i < n; i++)
53  {
54  if (version >= 3)
55  in >> sensedData[i].sensorID;
56  else
57  sensedData[i].sensorID = i;
58 
59  in >> aux >> sensedData[i].sensedDistance;
60  sensedData[i].sensorPose = aux.asTPose();
61  }
62 
63  if (version >= 1)
64  in >> sensorLabel;
65  else
66  sensorLabel = "";
67 
68  if (version >= 2)
69  in >> timestamp;
70  else
71  timestamp = INVALID_TIMESTAMP;
72  }
73  break;
74  default:
76  };
77 }
78 
79 /*---------------------------------------------------------------
80  getSensorPose
81  ---------------------------------------------------------------*/
82 void CObservationRange::getSensorPose(CPose3D& out_sensorPose) const
83 {
84  if (!sensedData.empty())
85  out_sensorPose = CPose3D(sensedData[0].sensorPose);
86  else
87  out_sensorPose = CPose3D(0, 0, 0);
88 }
89 
90 /*---------------------------------------------------------------
91  setSensorPose
92  ---------------------------------------------------------------*/
93 void CObservationRange::setSensorPose(const CPose3D& newSensorPose)
94 {
95  for (auto& sd : sensedData) sd.sensorPose = newSensorPose.asTPose();
96 }
97 
98 void CObservationRange::getDescriptionAsText(std::ostream& o) const
99 {
100  using namespace std;
102 
103  o << "minSensorDistance = " << minSensorDistance << " m" << endl;
104  o << "maxSensorDistance = " << maxSensorDistance << " m" << endl;
105  o << "sensorConeApperture = " << RAD2DEG(sensorConeApperture) << " deg"
106  << endl;
107 
108  // For each entry in this sequence:
109  o << " SENSOR_ID RANGE (m) SENSOR POSE (on the robot)" << endl;
110  o << "-------------------------------------------------------" << endl;
111  for (const auto& q : sensedData)
112  {
113  o << format(" %7u", (unsigned int)q.sensorID);
114  o << format(" %4.03f ", q.sensedDistance);
115  o << q.sensorPose << endl;
116  }
117 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
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.
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This namespace contains representation of robot actions and observations.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
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 2.0.4 Git: 02a75dd25 Thu Jun 4 01:49:55 2020 +0200 at jue jun 4 02:00:12 CEST 2020