Main MRPT website > C++ reference for MRPT 1.9.9
COctoMapBase_impl.h
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 // This file is to be included from <mrpt/maps/COctoMapBase.h>
14 #include <mrpt/maps/CPointsMap.h>
16 
17 namespace mrpt
18 {
19 namespace maps
20 {
21 
22 template <class OCTREE, class OCTREE_NODE>
23 struct mrpt::maps::COctoMapBase<OCTREE, OCTREE_NODE>::Impl
24 {
25  OCTREE m_octomap;
26 };
27 
28 template <class OCTREE, class OCTREE_NODE>
30  : insertionOptions(*this),
31  m_impl(new Impl({resolution}))
32 {}
33 
34 template <class OCTREE, class OCTREE_NODE>
35 template <class octomap_point3d, class octomap_pointcloud>
36 bool COctoMapBase<OCTREE, OCTREE_NODE>::
37  internal_build_PointCloud_for_observation(
38  const mrpt::obs::CObservation* obs,
39  const mrpt::poses::CPose3D* robotPose, octomap_point3d& sensorPt,
40  octomap_pointcloud& scan) const
41 {
42  using namespace mrpt::poses;
43  using namespace mrpt::obs;
44 
45  scan.clear();
46 
47  CPose3D robotPose3D;
48  if (robotPose) // Default values are (0,0,0)
49  robotPose3D = (*robotPose);
50 
52  {
53  /********************************************************************
54  OBSERVATION TYPE: CObservation2DRangeScan
55  ********************************************************************/
56  const CObservation2DRangeScan* o =
57  static_cast<const CObservation2DRangeScan*>(obs);
58 
59  // Build a points-map representation of the points from the scan
60  // (coordinates are wrt the robot base)
61 
62  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
63  CPose3D sensorPose(UNINITIALIZED_POSE);
64  sensorPose.composeFrom(robotPose3D, o->sensorPose);
65  sensorPt =
66  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
67 
68  const CPointsMap* scanPts =
70  const size_t nPts = scanPts->size();
71 
72  // Transform 3D point cloud:
73  scan.reserve(nPts);
74 
76  for (size_t i = 0; i < nPts; i++)
77  {
78  // Load the next point:
79  scanPts->getPointFast(i, pt.x, pt.y, pt.z);
80 
81  // Translation:
82  double gx, gy, gz;
83  robotPose3D.composePoint(pt.x, pt.y, pt.z, gx, gy, gz);
84 
85  // Add to this map:
86  scan.push_back(gx, gy, gz);
87  }
88  return true;
89  }
90  else if (IS_CLASS(obs, CObservation3DRangeScan))
91  {
92  /********************************************************************
93  OBSERVATION TYPE: CObservation3DRangeScan
94  ********************************************************************/
95  const CObservation3DRangeScan* o =
96  static_cast<const CObservation3DRangeScan*>(obs);
97 
98  // Build a points-map representation of the points from the scan
99  // (coordinates are wrt the robot base)
100  if (!o->hasPoints3D) return false;
101 
102  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
103  CPose3D sensorPose(UNINITIALIZED_POSE);
104  sensorPose.composeFrom(robotPose3D, o->sensorPose);
105  sensorPt =
106  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
107 
108  o->load(); // Just to make sure the points are loaded from an external
109  // source, if that's the case...
110  const size_t sizeRangeScan = o->points3D_x.size();
111 
112  // Transform 3D point cloud:
113  scan.reserve(sizeRangeScan);
114 
115  // For quicker access to values as "float" instead of "doubles":
117  robotPose3D.getHomogeneousMatrix(H);
118  const float m00 = H.get_unsafe(0, 0);
119  const float m01 = H.get_unsafe(0, 1);
120  const float m02 = H.get_unsafe(0, 2);
121  const float m03 = H.get_unsafe(0, 3);
122  const float m10 = H.get_unsafe(1, 0);
123  const float m11 = H.get_unsafe(1, 1);
124  const float m12 = H.get_unsafe(1, 2);
125  const float m13 = H.get_unsafe(1, 3);
126  const float m20 = H.get_unsafe(2, 0);
127  const float m21 = H.get_unsafe(2, 1);
128  const float m22 = H.get_unsafe(2, 2);
129  const float m23 = H.get_unsafe(2, 3);
130 
132  for (size_t i = 0; i < sizeRangeScan; i++)
133  {
134  pt.x = o->points3D_x[i];
135  pt.y = o->points3D_y[i];
136  pt.z = o->points3D_z[i];
137 
138  // Valid point?
139  if (pt.x != 0 || pt.y != 0 || pt.z != 0)
140  {
141  // Translation:
142  const float gx = m00 * pt.x + m01 * pt.y + m02 * pt.z + m03;
143  const float gy = m10 * pt.x + m11 * pt.y + m12 * pt.z + m13;
144  const float gz = m20 * pt.x + m21 * pt.y + m22 * pt.z + m23;
145 
146  // Add to this map:
147  scan.push_back(gx, gy, gz);
148  }
149  }
150  return true;
151  }
152 
153  return false;
154 }
155 
156 template <class OCTREE, class OCTREE_NODE>
158  const std::string& filNamePrefix) const
159 {
160  MRPT_START
161 
162  // Save as 3D Scene:
163  {
166  mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
167 
168  this->getAs3DObject(obj3D);
169 
170  scene.insert(obj3D);
171 
172  const std::string fil = filNamePrefix + std::string("_3D.3Dscene");
173  scene.saveToFile(fil);
174  }
175 
176  // Save as ".bt" file (a binary format from the octomap lib):
177  {
178  const std::string fil = filNamePrefix + std::string("_binary.bt");
179  m_impl->m_octomap.writeBinaryConst(fil);
180  }
181  MRPT_END
182 }
183 
184 template <class OCTREE, class OCTREE_NODE>
186  const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D& takenFrom)
187 {
188  octomap::point3d sensorPt;
189  octomap::Pointcloud scan;
190 
191  if (!internal_build_PointCloud_for_observation(
192  obs, &takenFrom, sensorPt, scan))
193  return 0; // Nothing to do.
194 
195  octomap::OcTreeKey key;
196  const size_t N = scan.size();
197 
198  double log_lik = 0;
199  for (size_t i = 0; i < N; i += likelihoodOptions.decimation)
200  {
201  if (m_impl->m_octomap
202  .coordToKeyChecked(scan.getPoint(i), key))
203  {
204  OCTREE_NODE* node =
205  m_impl->m_octomap.search(key, 0 /*depth*/);
206  if (node) log_lik += std::log(node->getOccupancy());
207  }
208  }
209 
210  return log_lik;
211 }
212 
213 template <class OCTREE, class OCTREE_NODE>
215  const float x, const float y, const float z, double& prob_occupancy) const
216 {
217  octomap::OcTreeKey key;
218  if (m_impl->m_octomap
219  .coordToKeyChecked(octomap::point3d(x, y, z), key))
220  {
221  OCTREE_NODE* node =
222  m_impl->m_octomap.search(key, 0 /*depth*/);
223  if (!node) return false;
224 
225  prob_occupancy = node->getOccupancy();
226  return true;
227  }
228  else
229  return false;
230 }
231 
232 template <class OCTREE, class OCTREE_NODE>
234  const CPointsMap& ptMap, const float sensor_x, const float sensor_y,
235  const float sensor_z)
236 {
237  MRPT_START
238  const octomap::point3d sensorPt(sensor_x, sensor_y, sensor_z);
239  size_t N;
240  const float *xs, *ys, *zs;
241  ptMap.getPointsBuffer(N, xs, ys, zs);
242  for (size_t i = 0; i < N; i++)
243  m_impl->m_octomap
244  .insertRay(
245  sensorPt, octomap::point3d(xs[i], ys[i], zs[i]),
246  insertionOptions.maxrange, insertionOptions.pruning);
247  MRPT_END
248 }
249 
250 template <class OCTREE, class OCTREE_NODE>
252  const mrpt::math::TPoint3D& origin, const mrpt::math::TPoint3D& direction,
253  mrpt::math::TPoint3D& end, bool ignoreUnknownCells, double maxRange) const
254 {
255  octomap::point3d _end;
256 
257  const bool ret =
258  m_impl->m_octomap
259  .castRay(
260  octomap::point3d(origin.x, origin.y, origin.z),
261  octomap::point3d(direction.x, direction.y, direction.z), _end,
262  ignoreUnknownCells, maxRange);
263 
264  end.x = _end.x();
265  end.y = _end.y();
266  end.z = _end.z();
267  return ret;
268 }
269 
270 /*---------------------------------------------------------------
271  TInsertionOptions
272  ---------------------------------------------------------------*/
273 template <class OCTREE, class OCTREE_NODE>
276  : maxrange(-1.),
277  pruning(true),
278  m_parent(&parent),
279  // Default values from octomap:
280  occupancyThres(0.5),
281  probHit(0.7),
282  probMiss(0.4),
283  clampingThresMin(0.1192),
284  clampingThresMax(0.971)
285 {
286 }
287 
288 template <class OCTREE, class OCTREE_NODE>
290  : maxrange(-1.),
291  pruning(true),
292  m_parent(nullptr),
293  // Default values from octomap:
294  occupancyThres(0.5),
295  probHit(0.7),
296  probMiss(0.4),
297  clampingThresMin(0.1192),
298  clampingThresMax(0.971)
299 {
300 }
301 
302 template <class OCTREE, class OCTREE_NODE>
304  : decimation(1)
305 {
306 }
307 
308 template <class OCTREE, class OCTREE_NODE>
311 {
312  const int8_t version = 0;
313  out << version;
314  out << decimation;
315 }
316 
317 template <class OCTREE, class OCTREE_NODE>
320 {
321  int8_t version;
322  in >> version;
323  switch (version)
324  {
325  case 0:
326  {
327  in >> decimation;
328  }
329  break;
330  default:
332  }
333 }
334 
335 template <class OCTREE, class OCTREE_NODE>
337  std::ostream& out) const
338 {
339  out << mrpt::format(
340  "\n----------- [COctoMapBase<>::TInsertionOptions] ------------ \n\n");
341 
342  LOADABLEOPTS_DUMP_VAR(maxrange, double);
343  LOADABLEOPTS_DUMP_VAR(pruning, bool);
344 
350 
351  out << mrpt::format("\n");
352 }
353 
354 template <class OCTREE, class OCTREE_NODE>
356  std::ostream& out) const
357 {
358  out << mrpt::format(
359  "\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ \n\n");
360 
361  LOADABLEOPTS_DUMP_VAR(decimation, int);
362 }
363 
364 /*---------------------------------------------------------------
365  loadFromConfigFile
366  ---------------------------------------------------------------*/
367 template <class OCTREE, class OCTREE_NODE>
369  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
370 {
371  MRPT_LOAD_CONFIG_VAR(maxrange, double, iniFile, section);
372  MRPT_LOAD_CONFIG_VAR(pruning, bool, iniFile, section);
373 
374  MRPT_LOAD_CONFIG_VAR(occupancyThres, double, iniFile, section);
375  MRPT_LOAD_CONFIG_VAR(probHit, double, iniFile, section);
376  MRPT_LOAD_CONFIG_VAR(probMiss, double, iniFile, section);
377  MRPT_LOAD_CONFIG_VAR(clampingThresMin, double, iniFile, section);
378  MRPT_LOAD_CONFIG_VAR(clampingThresMax, double, iniFile, section);
379 
380  // Set loaded options into the actual octomap object, if any:
381  this->setOccupancyThres(occupancyThres);
382  this->setProbHit(probHit);
383  this->setProbMiss(probMiss);
384  this->setClampingThresMin(clampingThresMin);
385  this->setClampingThresMax(clampingThresMax);
386 }
387 
388 template <class OCTREE, class OCTREE_NODE>
390  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
391 {
392  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
393 }
394 
395 /* COctoMapColoured */
396 template <class OCTREE, class OCTREE_NODE>
399 {
400  const int8_t version = 0;
401  out << version;
402  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
403  << generateFreeVoxels << visibleFreeVoxels;
404 }
405 
406 template <class OCTREE, class OCTREE_NODE>
409 {
410  int8_t version;
411  in >> version;
412  switch (version)
413  {
414  case 0:
415  {
416  in >> generateGridLines >> generateOccupiedVoxels >>
417  visibleOccupiedVoxels >> generateFreeVoxels >>
418  visibleFreeVoxels;
419  }
420  break;
421  default:
423  }
424 }
425 
426 } // namespace maps
427 } // namespace mrpt
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
virtual void setProbMiss(double prob)=0
virtual double getOccupancyThres() const =0
#define MRPT_START
Definition: exceptions.h:262
virtual double getClampingThresMin() const =0
GLdouble GLdouble z
Definition: glext.h:3872
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
virtual void setProbHit(double prob)=0
virtual double getClampingThresMax() const =0
signed char int8_t
Definition: rptypes.h:40
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:90
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMapBase.h:48
virtual void setClampingThresMin(double thresProb)=0
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:64
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
Lightweight 3D point (float version).
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:565
GLuint GLuint end
Definition: glext.h:3528
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"))
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
COctoMapBase(double resolution)
Constructor, defines the resolution of the octomap (length of each voxel side)
double x
X,Y,Z coordinates.
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 hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
Definition: glext.h:4101
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:48
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
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:245
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
virtual double getProbMiss() const =0
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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...
Definition: CPose3D.cpp:379
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
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:266
GLuint in
Definition: glext.h:7274
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
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.
GLenum GLint GLint y
Definition: glext.h:3538
virtual double getProbHit() const =0
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
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
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:223
virtual void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019