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



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo