Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
15 
16 namespace mrpt
17 {
18  namespace maps
19  {
20  template <class OCTREE,class OCTREE_NODE>
21  template <class octomap_point3d, class octomap_pointcloud>
22  bool COctoMapBase<OCTREE,OCTREE_NODE>::internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const
23  {
24  using namespace mrpt::poses;
25  using namespace mrpt::obs;
26 
27  scan.clear();
28 
29  CPose3D robotPose3D;
30  if (robotPose) // Default values are (0,0,0)
31  robotPose3D = (*robotPose);
32 
34  {
35  /********************************************************************
36  OBSERVATION TYPE: CObservation2DRangeScan
37  ********************************************************************/
38  const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
39 
40  // Build a points-map representation of the points from the scan (coordinates are wrt the robot base)
41 
42  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
43  CPose3D sensorPose(UNINITIALIZED_POSE);
44  sensorPose.composeFrom(robotPose3D,o->sensorPose);
45  sensorPt = octomap::point3d(sensorPose.x(),sensorPose.y(),sensorPose.z());
46 
48  const size_t nPts = scanPts->size();
49 
50  // Transform 3D point cloud:
51  scan.reserve(nPts);
52 
54  for (size_t i=0;i<nPts;i++)
55  {
56  // Load the next point:
57  scanPts->getPointFast(i,pt.x,pt.y,pt.z);
58 
59  // Translation:
60  double gx,gy,gz;
61  robotPose3D.composePoint(pt.x,pt.y,pt.z, gx,gy,gz);
62 
63  // Add to this map:
64  scan.push_back(gx,gy,gz);
65  }
66  return true;
67  }
68  else if ( IS_CLASS(obs,CObservation3DRangeScan) )
69  {
70  /********************************************************************
71  OBSERVATION TYPE: CObservation3DRangeScan
72  ********************************************************************/
73  const CObservation3DRangeScan *o = static_cast<const CObservation3DRangeScan*>( obs );
74 
75  // Build a points-map representation of the points from the scan (coordinates are wrt the robot base)
76  if (!o->hasPoints3D)
77  return false;
78 
79  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
80  CPose3D sensorPose(UNINITIALIZED_POSE);
81  sensorPose.composeFrom(robotPose3D,o->sensorPose);
82  sensorPt = octomap::point3d(sensorPose.x(),sensorPose.y(),sensorPose.z());
83 
84  o->load(); // Just to make sure the points are loaded from an external source, if that's the case...
85  const size_t sizeRangeScan = o->points3D_x.size();
86 
87  // Transform 3D point cloud:
88  scan.reserve(sizeRangeScan);
89 
90  // For quicker access to values as "float" instead of "doubles":
92  robotPose3D.getHomogeneousMatrix(H);
93  const float m00 = H.get_unsafe(0,0);
94  const float m01 = H.get_unsafe(0,1);
95  const float m02 = H.get_unsafe(0,2);
96  const float m03 = H.get_unsafe(0,3);
97  const float m10 = H.get_unsafe(1,0);
98  const float m11 = H.get_unsafe(1,1);
99  const float m12 = H.get_unsafe(1,2);
100  const float m13 = H.get_unsafe(1,3);
101  const float m20 = H.get_unsafe(2,0);
102  const float m21 = H.get_unsafe(2,1);
103  const float m22 = H.get_unsafe(2,2);
104  const float m23 = H.get_unsafe(2,3);
105 
107  for (size_t i=0;i<sizeRangeScan;i++)
108  {
109  pt.x = o->points3D_x[i];
110  pt.y = o->points3D_y[i];
111  pt.z = o->points3D_z[i];
112 
113  // Valid point?
114  if ( pt.x!=0 || pt.y!=0 || pt.z!=0 )
115  {
116  // Translation:
117  const float gx = m00*pt.x + m01*pt.y + m02*pt.z + m03;
118  const float gy = m10*pt.x + m11*pt.y + m12*pt.z + m13;
119  const float gz = m20*pt.x + m21*pt.y + m22*pt.z + m23;
120 
121  // Add to this map:
122  scan.push_back(gx,gy,gz);
123  }
124  }
125  return true;
126  }
127 
128  return false;
129  }
130 
131  template <class OCTREE,class OCTREE_NODE>
133  {
134  MRPT_START
135 
136  // Save as 3D Scene:
137  {
139  mrpt::opengl::CSetOfObjectsPtr obj3D = mrpt::opengl::CSetOfObjects::Create();
140 
141  this->getAs3DObject(obj3D);
142 
143  scene.insert(obj3D);
144 
145  const std::string fil = filNamePrefix + std::string("_3D.3Dscene");
147  f << scene;
148  }
149 
150  // Save as ".bt" file (a binary format from the octomap lib):
151  {
152  const std::string fil = filNamePrefix + std::string("_binary.bt");
153  const_cast<OCTREE*>(PIMPL_GET_PTR(OCTREE, m_octomap))->writeBinary(fil);
154  }
155  MRPT_END
156  }
157 
158  template <class OCTREE,class OCTREE_NODE>
160  {
161  octomap::point3d sensorPt;
162  octomap::Pointcloud scan;
163 
164  if (!internal_build_PointCloud_for_observation(obs,&takenFrom, sensorPt, scan))
165  return 0; // Nothing to do.
166 
167  octomap::OcTreeKey key;
168  const size_t N=scan.size();
169 
170  double log_lik = 0;
171  for (size_t i=0;i<N;i+=likelihoodOptions.decimation)
172  {
173  if (PIMPL_GET_REF(OCTREE, m_octomap).coordToKeyChecked(scan.getPoint(i), key))
174  {
175  OCTREE_NODE *node = PIMPL_GET_REF(OCTREE, m_octomap).search(key,0 /*depth*/);
176  if (node)
177  log_lik += std::log(node->getOccupancy());
178  }
179  }
180 
181  return log_lik;
182  }
183 
184  template <class OCTREE,class OCTREE_NODE>
185  bool COctoMapBase<OCTREE,OCTREE_NODE>::getPointOccupancy(const float x,const float y,const float z, double &prob_occupancy) const
186  {
187  octomap::OcTreeKey key;
188  if (PIMPL_GET_REF(OCTREE, m_octomap).coordToKeyChecked(octomap::point3d(x,y,z), key))
189  {
190  OCTREE_NODE *node = PIMPL_GET_REF(OCTREE, m_octomap).search(key,0 /*depth*/);
191  if (!node) return false;
192 
193  prob_occupancy = node->getOccupancy();
194  return true;
195  }
196  else return false;
197  }
198 
199  template <class OCTREE,class OCTREE_NODE>
200  void COctoMapBase<OCTREE,OCTREE_NODE>::insertPointCloud(const CPointsMap &ptMap, const float sensor_x,const float sensor_y,const float sensor_z)
201  {
202  MRPT_START
203  const octomap::point3d sensorPt(sensor_x,sensor_y,sensor_z);
204  size_t N;
205  const float *xs,*ys,*zs;
206  ptMap.getPointsBuffer(N,xs,ys,zs);
207  for (size_t i=0;i<N;i++)
208  PIMPL_GET_REF(OCTREE, m_octomap).insertRay(sensorPt, octomap::point3d(xs[i],ys[i],zs[i]), insertionOptions.maxrange,insertionOptions.pruning);
209  MRPT_END
210  }
211 
212  template <class OCTREE,class OCTREE_NODE>
213  bool COctoMapBase<OCTREE,OCTREE_NODE>::castRay(const mrpt::math::TPoint3D & origin,const mrpt::math::TPoint3D & direction,mrpt::math::TPoint3D & end,bool ignoreUnknownCells,double maxRange) const
214  {
215  octomap::point3d _end;
216 
217  const bool ret= PIMPL_GET_REF(OCTREE, m_octomap).castRay(
218  octomap::point3d(origin.x,origin.y,origin.z),
219  octomap::point3d(direction.x,direction.y,direction.z),
220  _end,ignoreUnknownCells,maxRange);
221 
222  end.x = _end.x();
223  end.y = _end.y();
224  end.z = _end.z();
225  return ret;
226  }
227 
228 
229 
230  /*---------------------------------------------------------------
231  TInsertionOptions
232  ---------------------------------------------------------------*/
233  template <class OCTREE,class OCTREE_NODE>
235  maxrange (-1.),
236  pruning (true),
237  m_parent (&parent),
238  // Default values from octomap:
239  occupancyThres (0.5),
240  probHit(0.7),
241  probMiss(0.4),
242  clampingThresMin(0.1192),
243  clampingThresMax(0.971)
244  {
245  }
246 
247  template <class OCTREE,class OCTREE_NODE>
249  maxrange (-1.),
250  pruning (true),
251  m_parent (NULL),
252  // Default values from octomap:
253  occupancyThres (0.5),
254  probHit(0.7),
255  probMiss(0.4),
256  clampingThresMin(0.1192),
257  clampingThresMax(0.971)
258  {
259  }
260 
261  template <class OCTREE,class OCTREE_NODE>
263  decimation ( 1 )
264  {
265  }
266 
267  template <class OCTREE,class OCTREE_NODE>
269  {
270  const int8_t version = 0;
271  out << version;
272  out << decimation;
273  }
274 
275  template <class OCTREE,class OCTREE_NODE>
277  {
278  int8_t version;
279  in >> version;
280  switch(version)
281  {
282  case 0:
283  {
284  in >> decimation;
285  }
286  break;
288  }
289  }
290 
291 
292  /*---------------------------------------------------------------
293  dumpToTextStream
294  ---------------------------------------------------------------*/
295  template <class OCTREE,class OCTREE_NODE>
297  {
298  out.printf("\n----------- [COctoMapBase<>::TInsertionOptions] ------------ \n\n");
299 
300  LOADABLEOPTS_DUMP_VAR(maxrange,double);
301  LOADABLEOPTS_DUMP_VAR(pruning,bool);
302 
308 
309  out.printf("\n");
310  }
311 
312  template <class OCTREE,class OCTREE_NODE>
314  {
315  out.printf("\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ \n\n");
316 
317  LOADABLEOPTS_DUMP_VAR(decimation,int);
318  }
319 
320  /*---------------------------------------------------------------
321  loadFromConfigFile
322  ---------------------------------------------------------------*/
323  template <class OCTREE,class OCTREE_NODE>
325  const mrpt::utils::CConfigFileBase &iniFile,
326  const std::string &section)
327  {
328  MRPT_LOAD_CONFIG_VAR(maxrange,double, iniFile,section);
329  MRPT_LOAD_CONFIG_VAR(pruning,bool, iniFile,section);
330 
331  MRPT_LOAD_CONFIG_VAR(occupancyThres,double, iniFile,section);
332  MRPT_LOAD_CONFIG_VAR(probHit,double, iniFile,section);
333  MRPT_LOAD_CONFIG_VAR(probMiss,double, iniFile,section);
334  MRPT_LOAD_CONFIG_VAR(clampingThresMin,double, iniFile,section);
335  MRPT_LOAD_CONFIG_VAR(clampingThresMax,double, iniFile,section);
336 
337  // Set loaded options into the actual octomap object, if any:
338  this->setOccupancyThres(occupancyThres);
339  this->setProbHit(probHit);
340  this->setProbMiss(probMiss);
341  this->setClampingThresMin(clampingThresMin);
342  this->setClampingThresMax(clampingThresMax);
343  }
344 
345  template <class OCTREE,class OCTREE_NODE>
347  const mrpt::utils::CConfigFileBase &iniFile,
348  const std::string &section)
349  {
350  MRPT_LOAD_CONFIG_VAR(decimation,int,iniFile,section);
351  }
352 
353  /* COctoMapColoured */
354  template <class OCTREE,class OCTREE_NODE>
356  {
357  const int8_t version = 0;
358  out << version;
359  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
360  << generateFreeVoxels << visibleFreeVoxels;
361  }
362 
363  template <class OCTREE,class OCTREE_NODE>
365  {
366  int8_t version;
367  in >> version;
368  switch(version)
369  {
370  case 0:
371  {
372  in >> generateGridLines >> generateOccupiedVoxels >> visibleOccupiedVoxels
373  >> generateFreeVoxels >> visibleFreeVoxels;
374  }
375  break;
377  }
378  }
379 
380 
381  } // End of namespace
382 } // End of namespace
383 
384 
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
virtual void setProbMiss(double prob)=0
virtual double getOccupancyThres() const =0
virtual double getClampingThresMin() const =0
GLdouble GLdouble z
Definition: glext.h:3734
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
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:42
double z
X,Y,Z coordinates.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
virtual void setClampingThresMin(double thresProb)=0
TLikelihoodOptions()
Initilization of default parameters.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
A numeric matrix of compile-time fixed size.
void insert(const CRenderizablePtr &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)...
Lightweight 3D point (float version).
#define MRPT_END
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:595
This CStream derived class allow using a file as a write-only, binary stream.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLuint GLuint end
Definition: glext.h:3512
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const
Builds the list of 3D points in global coordinates for a generic observation.
#define PIMPL_GET_PTR(_TYPE, _VAR_NAME)
Definition: pimpl.h:74
This namespace contains representation of robot actions and observations.
int version
Definition: mrpt_jpeglib.h:898
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
Definition: glext.h:3919
#define PIMPL_GET_REF(_TYPE, _VAR_NAME)
Definition: pimpl.h:75
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...
Definition: CPoint.h:17
#define MRPT_START
#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...
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 NULL if there is no points in the map.
Definition: CPointsMap.cpp:252
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Declares a class that represents any robot&#39;s observation.
virtual double getProbMiss() const =0
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
void insertPointCloud(const CPointsMap &ptMap, const float sensor_x, const float sensor_y, const float sensor_z)
Update the octomap with a 2D or 3D scan, given directly as a point cloud and the 3D location of the s...
GLuint in
Definition: glext.h:6301
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
virtual void setOccupancyThres(double prob)=0
GLenum GLint GLint y
Definition: glext.h:3516
virtual double getProbHit() const =0
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
GLenum GLint x
Definition: glext.h:3516
Lightweight 3D point.
virtual void load() const MRPT_OVERRIDE
Makes sure all images and other fields which may be externally stored are loaded in memory...
virtual void setClampingThresMax(double thresProb)=0
size_t size() const
Returns the number of stored points in the map.
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:166
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
static CSetOfObjectsPtr Create()
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019