26 : minSensorDistance(0),
27 maxSensorDistance(1e2f),
47 out << minSensorDistance << maxSensorDistance << stdError;
49 n = sensedData.size();
51 for (i = 0; i <
n; i++)
52 out << sensedData[i].sensorLocationOnRobot
53 << sensedData[i].sensedDistance << sensedData[i].beaconID;
55 out << auxEstimatePose;
57 out << sensorLabel << timestamp;
77 in >> minSensorDistance >> maxSensorDistance >> stdError;
81 for (i = 0; i <
n; i++)
83 in >> sensedData[i].sensorLocationOnRobot >>
84 sensedData[i].sensedDistance;
86 sensedData[i].beaconID =
id;
89 if (version >= 1)
in >> auxEstimatePose;
112 printf(
"[CObservationBeaconRanges::debugPrintOut] Dumping:\n");
114 "[CObservationBeaconRanges::debugPrintOut] minSensorDistance:\t%f\n",
117 "[CObservationBeaconRanges::debugPrintOut] maxSensorDistance:\t%f:\n",
120 "[CObservationBeaconRanges::debugPrintOut] stdError:\t%f\n", stdError);
122 "[CObservationBeaconRanges::debugPrintOut] %u ranges:\n",
123 static_cast<unsigned>(sensedData.size()));
125 size_t i,
n = sensedData.size();
126 for (i = 0; i <
n; i++)
128 "[CObservationBeaconRanges::debugPrintOut] \tID[%u]: %f\n",
129 sensedData[i].beaconID, sensedData[i].sensedDistance);
137 if (!sensedData.empty())
138 out_sensorPose =
CPose3D(sensedData[0].sensorLocationOnRobot);
140 out_sensorPose =
CPose3D(0, 0, 0);
148 size_t i,
n = sensedData.size();
150 for (i = 0; i <
n; i++)
151 sensedData[i].sensorLocationOnRobot =
CPoint3D(newSensorPose);
159 for (
size_t i = 0; i < sensedData.size(); i++)
160 if (sensedData[i].beaconID == beaconID)
161 return sensedData[i].sensedDistance;
170 o <<
"Auxiliary estimated pose (if available): " << auxEstimatePose << endl;
172 o <<
format(
"minSensorDistance=%f m\n", minSensorDistance);
173 o <<
format(
"maxSensorDistance=%f m\n", maxSensorDistance);
174 o <<
format(
"stdError=%f m\n\n", stdError);
177 "There are %u range measurements:\n\n", (
unsigned)sensedData.size());
179 o <<
" BEACON RANGE SENSOR POSITION ON ROBOT \n";
180 o <<
"------------------------------------------------\n";
183 it != sensedData.end(); it++)
186 " %i %.04f (%.03f,%.03f,%.03f)\n", (
int)it->beaconID,
187 it->sensedDistance, it->sensorLocationOnRobot.x(),
188 it->sensorLocationOnRobot.y(), it->sensorLocationOnRobot.z());
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
const Scalar * const_iterator
float getSensedRangeByBeaconID(int32_t beaconID)
Easy look-up into the vector sensedData, returns the range for a given beacon, or 0 if the beacon is ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
void debugPrintOut()
Prints out the contents of the object.
unsigned __int32 uint32_t
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...