MRPT  2.0.4
COctoMapBase_impl.h
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 // This file is to be included from <mrpt/maps/COctoMapBase.h>
12 #include <mrpt/maps/CPointsMap.h>
18 
19 namespace mrpt::maps
20 {
21 template <class OCTREE, class OCTREE_NODE>
22 struct mrpt::maps::COctoMapBase<OCTREE, OCTREE_NODE>::Impl
23 {
24  OCTREE m_octomap;
25 };
26 
27 template <class OCTREE, class OCTREE_NODE>
29  : insertionOptions(*this), m_impl(new Impl({resolution}))
30 {
31 }
32 
33 template <class OCTREE, class OCTREE_NODE>
34 template <class octomap_point3d, class octomap_pointcloud>
35 bool COctoMapBase<OCTREE, OCTREE_NODE>::
36  internal_build_PointCloud_for_observation(
37  const mrpt::obs::CObservation& obs,
38  const mrpt::poses::CPose3D* robotPose, octomap_point3d& sensorPt,
39  octomap_pointcloud& scan) const
40 {
41  using namespace mrpt::poses;
42  using namespace mrpt::obs;
43 
44  scan.clear();
45 
46  CPose3D robotPose3D;
47  if (robotPose) // Default values are (0,0,0)
48  robotPose3D = (*robotPose);
49 
51  {
52  /********************************************************************
53  OBSERVATION TYPE: CObservation2DRangeScan
54  ********************************************************************/
55  const auto& o = static_cast<const CObservation2DRangeScan&>(obs);
56 
57  // Build a points-map representation of the points from the scan
58  // (coordinates are wrt the robot base)
59 
60  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
61  CPose3D sensorPose(UNINITIALIZED_POSE);
62  sensorPose.composeFrom(robotPose3D, o.sensorPose);
63  sensorPt =
64  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
65 
66  const auto* scanPts = o.buildAuxPointsMap<mrpt::maps::CPointsMap>();
67  const size_t nPts = scanPts->size();
68 
69  // Transform 3D point cloud:
70  scan.reserve(nPts);
71 
73  for (size_t i = 0; i < nPts; i++)
74  {
75  // Load the next point:
76  scanPts->getPointFast(i, pt.x, pt.y, pt.z);
77 
78  // Translation:
79  double gx, gy, gz;
80  robotPose3D.composePoint(pt.x, pt.y, pt.z, gx, gy, gz);
81 
82  // Add to this map:
83  scan.push_back(gx, gy, gz);
84  }
85  return true;
86  }
87  else if (
91  {
92  // Observations that can be converted into 3D point clouds:
93  const auto* o_scan3D =
94  dynamic_cast<const CObservation3DRangeScan*>(&obs);
95  const auto* o_pc = dynamic_cast<const CObservationPointCloud*>(&obs);
96  const auto* o_velo =
97  dynamic_cast<const CObservationVelodyneScan*>(&obs);
98 
99  // No points?
100  if (o_scan3D && !o_scan3D->hasPoints3D) return false;
101  if (o_pc && (!o_pc->pointcloud || !o_pc->pointcloud->size()))
102  return false;
103  if (o_velo && !o_velo->point_cloud.size()) return false;
104 
105  // Build a points-map representation of the points from the scan
106  // (coordinates are wrt the robot base)
107 
108  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
109  CPose3D sensorPose(UNINITIALIZED_POSE);
110  obs.getSensorPose(sensorPose);
111  sensorPose.composeFrom(robotPose3D, sensorPose);
112  sensorPt =
113  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
114 
115  obs.load(); // ensure points are loaded from an external source
116 
117  // size:
118  std::size_t sizeRangeScan = 0;
119  const float *xs = nullptr, *ys = nullptr, *zs = nullptr;
120  if (o_scan3D)
121  {
122  sizeRangeScan = o_scan3D->points3D_x.size();
123  xs = &o_scan3D->points3D_x[0];
124  ys = &o_scan3D->points3D_y[0];
125  zs = &o_scan3D->points3D_z[0];
126  }
127  if (o_pc)
128  {
129  sizeRangeScan = o_pc->pointcloud->size();
130  xs = &o_pc->pointcloud->getPointsBufferRef_x()[0];
131  ys = &o_pc->pointcloud->getPointsBufferRef_y()[0];
132  zs = &o_pc->pointcloud->getPointsBufferRef_z()[0];
133  }
134  if (o_velo)
135  {
136  sizeRangeScan = o_velo->point_cloud.size();
137  xs = &o_velo->point_cloud.x[0];
138  ys = &o_velo->point_cloud.y[0];
139  zs = &o_velo->point_cloud.z[0];
140  }
141 
142  // Transform 3D point cloud:
143  scan.reserve(sizeRangeScan);
144 
145  // For quicker access to values as "float" instead of "doubles":
147  robotPose3D.getHomogeneousMatrix(H);
148  const float m00 = H(0, 0);
149  const float m01 = H(0, 1);
150  const float m02 = H(0, 2);
151  const float m03 = H(0, 3);
152  const float m10 = H(1, 0);
153  const float m11 = H(1, 1);
154  const float m12 = H(1, 2);
155  const float m13 = H(1, 3);
156  const float m20 = H(2, 0);
157  const float m21 = H(2, 1);
158  const float m22 = H(2, 2);
159  const float m23 = H(2, 3);
160 
162  for (size_t i = 0; i < sizeRangeScan; i++)
163  {
164  pt.x = xs[i];
165  pt.y = ys[i];
166  pt.z = zs[i];
167 
168  // Valid point?
169  if (pt.x != 0 || pt.y != 0 || pt.z != 0)
170  {
171  // Translation:
172  const float gx = m00 * pt.x + m01 * pt.y + m02 * pt.z + m03;
173  const float gy = m10 * pt.x + m11 * pt.y + m12 * pt.z + m13;
174  const float gz = m20 * pt.x + m21 * pt.y + m22 * pt.z + m23;
175 
176  // Add to this map:
177  scan.push_back(gx, gy, gz);
178  }
179  }
180  return true;
181  }
182 
183  return false;
184 }
185 
186 template <class OCTREE, class OCTREE_NODE>
188  const std::string& filNamePrefix) const
189 {
190  MRPT_START
191 
192  // Save as 3D Scene:
193  {
197 
198  this->getAs3DObject(obj3D);
199 
200  scene.insert(obj3D);
201 
202  const std::string fil = filNamePrefix + std::string("_3D.3Dscene");
203  scene.saveToFile(fil);
204  }
205 
206  // Save as ".bt" file (a binary format from the octomap lib):
207  {
208  const std::string fil = filNamePrefix + std::string("_binary.bt");
209  m_impl->m_octomap.writeBinaryConst(fil);
210  }
211  MRPT_END
212 }
213 
214 template <class OCTREE, class OCTREE_NODE>
216  const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom)
217 {
218  octomap::point3d sensorPt;
219  octomap::Pointcloud scan;
220 
221  if (!internal_build_PointCloud_for_observation(
222  obs, &takenFrom, sensorPt, scan))
223  return 0; // Nothing to do.
224 
225  octomap::OcTreeKey key;
226  const size_t N = scan.size();
227 
228  double log_lik = 0;
229  for (size_t i = 0; i < N; i += likelihoodOptions.decimation)
230  {
231  if (m_impl->m_octomap.coordToKeyChecked(scan.getPoint(i), key))
232  {
233  OCTREE_NODE* node = m_impl->m_octomap.search(key, 0 /*depth*/);
234  if (node) log_lik += std::log(node->getOccupancy());
235  }
236  }
237 
238  return log_lik;
239 }
240 
241 template <class OCTREE, class OCTREE_NODE>
243  const float x, const float y, const float z, double& prob_occupancy) const
244 {
245  octomap::OcTreeKey key;
246  if (m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key))
247  {
248  OCTREE_NODE* node = m_impl->m_octomap.search(key, 0 /*depth*/);
249  if (!node) return false;
250 
251  prob_occupancy = node->getOccupancy();
252  return true;
253  }
254  else
255  return false;
256 }
257 
258 template <class OCTREE, class OCTREE_NODE>
260  const CPointsMap& ptMap, const float sensor_x, const float sensor_y,
261  const float sensor_z)
262 {
263  MRPT_START
264  const octomap::point3d sensorPt(sensor_x, sensor_y, sensor_z);
265  size_t N;
266  const float *xs, *ys, *zs;
267  ptMap.getPointsBuffer(N, xs, ys, zs);
268  for (size_t i = 0; i < N; i++)
269  m_impl->m_octomap.insertRay(
270  sensorPt, octomap::point3d(xs[i], ys[i], zs[i]),
271  insertionOptions.maxrange, insertionOptions.pruning);
272  MRPT_END
273 }
274 
275 template <class OCTREE, class OCTREE_NODE>
277  const mrpt::math::TPoint3D& origin, const mrpt::math::TPoint3D& direction,
278  mrpt::math::TPoint3D& end, bool ignoreUnknownCells, double maxRange) const
279 {
280  octomap::point3d _end;
281 
282  const bool ret = m_impl->m_octomap.castRay(
283  octomap::point3d(origin.x, origin.y, origin.z),
284  octomap::point3d(direction.x, direction.y, direction.z), _end,
285  ignoreUnknownCells, maxRange);
286 
287  end.x = _end.x();
288  end.y = _end.y();
289  end.z = _end.z();
290  return ret;
291 }
292 
293 /*---------------------------------------------------------------
294  TInsertionOptions
295  ---------------------------------------------------------------*/
296 template <class OCTREE, class OCTREE_NODE>
299  : maxrange(-1.),
300  pruning(true),
301  m_parent(&parent),
302  // Default values from octomap:
303  occupancyThres(0.5),
304  probHit(0.7),
305  probMiss(0.4),
306  clampingThresMin(0.1192),
307  clampingThresMax(0.971)
308 {
309 }
310 
311 template <class OCTREE, class OCTREE_NODE>
313  : m_parent(nullptr)
314 
315 {
316 }
317 
318 template <class OCTREE, class OCTREE_NODE>
320 
321  = default;
322 
323 template <class OCTREE, class OCTREE_NODE>
326 {
327  const int8_t version = 0;
328  out << version;
329  out << decimation;
330 }
331 
332 template <class OCTREE, class OCTREE_NODE>
335 {
336  int8_t version;
337  in >> version;
338  switch (version)
339  {
340  case 0:
341  {
342  in >> decimation;
343  }
344  break;
345  default:
347  }
348 }
349 
350 template <class OCTREE, class OCTREE_NODE>
352  std::ostream& out) const
353 {
354  out << "\n----------- [COctoMapBase<>::TInsertionOptions] ------------ "
355  "\n\n";
356 
357  LOADABLEOPTS_DUMP_VAR(maxrange, double);
358  LOADABLEOPTS_DUMP_VAR(pruning, bool);
359 
365 
366  out << "\n";
367 }
368 
369 template <class OCTREE, class OCTREE_NODE>
371  std::ostream& out) const
372 {
373  out << "\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ "
374  "\n\n";
375 
376  LOADABLEOPTS_DUMP_VAR(decimation, int);
377 }
378 
379 /*---------------------------------------------------------------
380  loadFromConfigFile
381  ---------------------------------------------------------------*/
382 template <class OCTREE, class OCTREE_NODE>
384  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
385 {
386  MRPT_LOAD_CONFIG_VAR(maxrange, double, iniFile, section);
387  MRPT_LOAD_CONFIG_VAR(pruning, bool, iniFile, section);
388 
389  MRPT_LOAD_CONFIG_VAR(occupancyThres, double, iniFile, section);
390  MRPT_LOAD_CONFIG_VAR(probHit, double, iniFile, section);
391  MRPT_LOAD_CONFIG_VAR(probMiss, double, iniFile, section);
392  MRPT_LOAD_CONFIG_VAR(clampingThresMin, double, iniFile, section);
393  MRPT_LOAD_CONFIG_VAR(clampingThresMax, double, iniFile, section);
394 
395  // Set loaded options into the actual octomap object, if any:
396  this->setOccupancyThres(occupancyThres);
397  this->setProbHit(probHit);
398  this->setProbMiss(probMiss);
399  this->setClampingThresMin(clampingThresMin);
400  this->setClampingThresMax(clampingThresMax);
401 }
402 
403 template <class OCTREE, class OCTREE_NODE>
405  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
406 {
407  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
408 }
409 
410 /* COctoMapColoured */
411 template <class OCTREE, class OCTREE_NODE>
414 {
415  const int8_t version = 0;
416  out << version;
417  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
418  << generateFreeVoxels << visibleFreeVoxels;
419 }
420 
421 template <class OCTREE, class OCTREE_NODE>
424 {
425  int8_t version;
426  in >> version;
427  switch (version)
428  {
429  case 0:
430  {
431  in >> generateGridLines >> generateOccupiedVoxels >>
432  visibleOccupiedVoxels >> generateFreeVoxels >>
433  visibleFreeVoxels;
434  }
435  break;
436  default:
438  }
439 }
440 
441 } // namespace mrpt::maps
virtual void setProbMiss(double prob)=0
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
virtual double getOccupancyThres() const =0
#define MRPT_START
Definition: exceptions.h:241
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
virtual double getClampingThresMin() const =0
An observation from any sensor that can be summarized as a pointcloud.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
virtual void setProbHit(double prob)=0
virtual double getClampingThresMax() const =0
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMapBase.h:45
virtual void setClampingThresMin(double thresProb)=0
virtual void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const =0
A general method to retrieve the sensor pose on the robot.
TLikelihoodOptions()
Initilization of default parameters.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:67
This class allows loading and storing values and vectors of different types from a configuration text...
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:556
virtual void load() const
Makes sure all images and other fields which may be externally stored are loaded in memory...
Definition: CObservation.h:157
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
COctoMapBase(double resolution)
Constructor, defines the resolution of the octomap (length of each voxel side)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool castRay(const mrpt::math::TPoint3D &origin, const mrpt::math::TPoint3D &direction, mrpt::math::TPoint3D &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
const_iterator end() const
Definition: ts_hash_map.h:246
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:222
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
virtual double getProbMiss() const =0
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_END
Definition: exceptions.h:245
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:56
virtual void setOccupancyThres(double prob)=0
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
virtual double getProbHit() const =0
virtual void setClampingThresMax(double thresProb)=0
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:459
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:781



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020