Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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>
22  {
23  using namespace mrpt::poses;
24  using namespace mrpt::obs;
25 
26  scan.clear();
27 
28  CPose3D robotPose3D;
29  if (robotPose) // Default values are (0,0,0)
30  robotPose3D = (*robotPose);
31 
33  {
34  /********************************************************************
35  OBSERVATION TYPE: CObservation2DRangeScan
36  ********************************************************************/
37  const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
38 
39  // Build a points-map representation of the points from the scan (coordinates are wrt the robot base)
40 
41  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
42  CPose3D sensorPose(UNINITIALIZED_POSE);
43  sensorPose.composeFrom(robotPose3D,o->sensorPose);
44  sensorPt = octomap::point3d(sensorPose.x(),sensorPose.y(),sensorPose.z());
45 
47  const size_t nPts = scanPts->size();
48 
49  // Transform 3D point cloud:
50  scan.reserve(nPts);
51 
53  for (size_t i=0;i<nPts;i++)
54  {
55  // Load the next point:
56  scanPts->getPointFast(i,pt.x,pt.y,pt.z);
57 
58  // Translation:
59  double gx,gy,gz;
60  robotPose3D.composePoint(pt.x,pt.y,pt.z, gx,gy,gz);
61 
62  // Add to this map:
63  scan.push_back(gx,gy,gz);
64  }
65  return true;
66  }
67  else if ( IS_CLASS(obs,CObservation3DRangeScan) )
68  {
69  /********************************************************************
70  OBSERVATION TYPE: CObservation3DRangeScan
71  ********************************************************************/
72  const CObservation3DRangeScan *o = static_cast<const CObservation3DRangeScan*>( obs );
73 
74  // Build a points-map representation of the points from the scan (coordinates are wrt the robot base)
75  if (!o->hasPoints3D)
76  return false;
77 
78  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
79  CPose3D sensorPose(UNINITIALIZED_POSE);
80  sensorPose.composeFrom(robotPose3D,o->sensorPose);
81  sensorPt = octomap::point3d(sensorPose.x(),sensorPose.y(),sensorPose.z());
82 
83  o->load(); // Just to make sure the points are loaded from an external source, if that's the case...
84  const size_t sizeRangeScan = o->points3D_x.size();
85 
86  // Transform 3D point cloud:
87  scan.reserve(sizeRangeScan);
88 
89  // For quicker access to values as "float" instead of "doubles":
91  robotPose3D.getHomogeneousMatrix(H);
92  const float m00 = H.get_unsafe(0,0);
93  const float m01 = H.get_unsafe(0,1);
94  const float m02 = H.get_unsafe(0,2);
95  const float m03 = H.get_unsafe(0,3);
96  const float m10 = H.get_unsafe(1,0);
97  const float m11 = H.get_unsafe(1,1);
98  const float m12 = H.get_unsafe(1,2);
99  const float m13 = H.get_unsafe(1,3);
100  const float m20 = H.get_unsafe(2,0);
101  const float m21 = H.get_unsafe(2,1);
102  const float m22 = H.get_unsafe(2,2);
103  const float m23 = H.get_unsafe(2,3);
104 
106  for (size_t i=0;i<sizeRangeScan;i++)
107  {
108  pt.x = o->points3D_x[i];
109  pt.y = o->points3D_y[i];
110  pt.z = o->points3D_z[i];
111 
112  // Valid point?
113  if ( pt.x!=0 || pt.y!=0 || pt.z!=0 )
114  {
115  // Translation:
116  const float gx = m00*pt.x + m01*pt.y + m02*pt.z + m03;
117  const float gy = m10*pt.x + m11*pt.y + m12*pt.z + m13;
118  const float gz = m20*pt.x + m21*pt.y + m22*pt.z + m23;
119 
120  // Add to this map:
121  scan.push_back(gx,gy,gz);
122  }
123  }
124  return true;
125  }
126 
127  return false;
128  }
129 
130  template <class OCTREE,class OCTREE_NODE>
132  {
133  return m_octomap.size()==1;
134  }
135 
136  template <class OCTREE,class OCTREE_NODE>
137  void COctoMapBase<OCTREE,OCTREE_NODE>::saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
138  {
139  MRPT_START
140 
141  // Save as 3D Scene:
142  {
144  mrpt::opengl::CSetOfObjectsPtr obj3D = mrpt::opengl::CSetOfObjects::Create();
145 
146  this->getAs3DObject(obj3D);
147 
148  scene.insert(obj3D);
149 
150  const std::string fil = filNamePrefix + std::string("_3D.3Dscene");
152  f << scene;
153  }
154 
155  // Save as ".bt" file (a binary format from the octomap lib):
156  {
157  const std::string fil = filNamePrefix + std::string("_binary.bt");
158  const_cast<OCTREE*>(&m_octomap)->writeBinary(fil);
159  }
160  MRPT_END
161  }
162 
163  template <class OCTREE,class OCTREE_NODE>
165  {
166  octomap::point3d sensorPt;
167  octomap::Pointcloud scan;
168 
169  if (!internal_build_PointCloud_for_observation(obs,&takenFrom, sensorPt, scan))
170  return 0; // Nothing to do.
171 
172  octomap::OcTreeKey key;
173  const size_t N=scan.size();
174 
175  double log_lik = 0;
176  for (size_t i=0;i<N;i+=likelihoodOptions.decimation)
177  {
178  if (m_octomap.coordToKeyChecked(scan.getPoint(i), key))
179  {
180  octree_node_t *node = m_octomap.search(key,0 /*depth*/);
181  if (node)
182  log_lik += std::log(node->getOccupancy());
183  }
184  }
185 
186  return log_lik;
187  }
188 
189  template <class OCTREE,class OCTREE_NODE>
190  bool COctoMapBase<OCTREE,OCTREE_NODE>::getPointOccupancy(const float x,const float y,const float z, double &prob_occupancy) const
191  {
192  octomap::OcTreeKey key;
193  if (m_octomap.coordToKeyChecked(octomap::point3d(x,y,z), key))
194  {
195  octree_node_t *node = m_octomap.search(key,0 /*depth*/);
196  if (!node) return false;
197 
198  prob_occupancy = node->getOccupancy();
199  return true;
200  }
201  else return false;
202  }
203 
204  template <class OCTREE,class OCTREE_NODE>
205  void COctoMapBase<OCTREE,OCTREE_NODE>::insertPointCloud(const CPointsMap &ptMap, const float sensor_x,const float sensor_y,const float sensor_z)
206  {
207  MRPT_START
208  const octomap::point3d sensorPt(sensor_x,sensor_y,sensor_z);
209  size_t N;
210  const float *xs,*ys,*zs;
211  ptMap.getPointsBuffer(N,xs,ys,zs);
212  for (size_t i=0;i<N;i++)
213  m_octomap.insertRay(sensorPt, octomap::point3d(xs[i],ys[i],zs[i]), insertionOptions.maxrange,insertionOptions.pruning);
214  MRPT_END
215  }
216 
217  template <class OCTREE,class OCTREE_NODE>
218  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
219  {
220  octomap::point3d _end;
221 
222  const bool ret=m_octomap.castRay(
223  octomap::point3d(origin.x,origin.y,origin.z),
224  octomap::point3d(direction.x,direction.y,direction.z),
225  _end,ignoreUnknownCells,maxRange);
226 
227  end.x = _end.x();
228  end.y = _end.y();
229  end.z = _end.z();
230  return ret;
231  }
232 
233 
234 
235  /*---------------------------------------------------------------
236  TInsertionOptions
237  ---------------------------------------------------------------*/
238  template <class OCTREE,class OCTREE_NODE>
240  maxrange (-1.),
241  pruning (true),
242  m_parent (&parent),
243  // Default values from octomap:
244  occupancyThres (0.5),
245  probHit(0.7),
246  probMiss(0.4),
247  clampingThresMin(0.1192),
248  clampingThresMax(0.971)
249  {
250  }
251 
252  template <class OCTREE,class OCTREE_NODE>
254  maxrange (-1.),
255  pruning (true),
256  m_parent (NULL),
257  // Default values from octomap:
258  occupancyThres (0.5),
259  probHit(0.7),
260  probMiss(0.4),
261  clampingThresMin(0.1192),
262  clampingThresMax(0.971)
263  {
264  }
265 
266  template <class OCTREE,class OCTREE_NODE>
268  decimation ( 1 )
269  {
270  }
271 
272  template <class OCTREE,class OCTREE_NODE>
274  {
275  const int8_t version = 0;
276  out << version;
277  out << decimation;
278  }
279 
280  template <class OCTREE,class OCTREE_NODE>
282  {
283  int8_t version;
284  in >> version;
285  switch(version)
286  {
287  case 0:
288  {
289  in >> decimation;
290  }
291  break;
293  }
294  }
295 
296 
297  /*---------------------------------------------------------------
298  dumpToTextStream
299  ---------------------------------------------------------------*/
300  template <class OCTREE,class OCTREE_NODE>
302  {
303  out.printf("\n----------- [COctoMapBase<>::TInsertionOptions] ------------ \n\n");
304 
305  LOADABLEOPTS_DUMP_VAR(maxrange,double);
306  LOADABLEOPTS_DUMP_VAR(pruning,bool);
307 
308  LOADABLEOPTS_DUMP_VAR(getOccupancyThres(),double);
309  LOADABLEOPTS_DUMP_VAR(getProbHit(),double);
310  LOADABLEOPTS_DUMP_VAR(getProbMiss(),double);
311  LOADABLEOPTS_DUMP_VAR(getClampingThresMin(),double);
312  LOADABLEOPTS_DUMP_VAR(getClampingThresMax(),double);
313 
314  out.printf("\n");
315  }
316 
317  template <class OCTREE,class OCTREE_NODE>
319  {
320  out.printf("\n----------- [COctoMapBase<>::TLikelihoodOptions] ------------ \n\n");
321 
323  }
324 
325  /*---------------------------------------------------------------
326  loadFromConfigFile
327  ---------------------------------------------------------------*/
328  template <class OCTREE,class OCTREE_NODE>
330  const mrpt::utils::CConfigFileBase &iniFile,
331  const std::string &section)
332  {
333  MRPT_LOAD_CONFIG_VAR(maxrange,double, iniFile,section);
334  MRPT_LOAD_CONFIG_VAR(pruning,bool, iniFile,section);
335 
336  MRPT_LOAD_CONFIG_VAR(occupancyThres,double, iniFile,section);
337  MRPT_LOAD_CONFIG_VAR(probHit,double, iniFile,section);
338  MRPT_LOAD_CONFIG_VAR(probMiss,double, iniFile,section);
339  MRPT_LOAD_CONFIG_VAR(clampingThresMin,double, iniFile,section);
340  MRPT_LOAD_CONFIG_VAR(clampingThresMax,double, iniFile,section);
341 
342  // Set loaded options into the actual octomap object, if any:
343  this->setOccupancyThres(occupancyThres);
344  this->setProbHit(probHit);
345  this->setProbMiss(probMiss);
346  this->setClampingThresMin(clampingThresMin);
347  this->setClampingThresMax(clampingThresMax);
348  }
349 
350  template <class OCTREE,class OCTREE_NODE>
352  const mrpt::utils::CConfigFileBase &iniFile,
353  const std::string &section)
354  {
355  MRPT_LOAD_CONFIG_VAR(decimation,int,iniFile,section);
356  }
357 
358  /* COctoMapColoured */
359  template <class OCTREE,class OCTREE_NODE>
361  {
362  const int8_t version = 0;
363  out << version;
364  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
365  << generateFreeVoxels << visibleFreeVoxels;
366  }
367 
368  template <class OCTREE,class OCTREE_NODE>
370  {
371  int8_t version;
372  in >> version;
373  switch(version)
374  {
375  case 0:
376  {
377  in >> generateGridLines >> generateOccupiedVoxels >> visibleOccupiedVoxels
378  >> generateFreeVoxels >> visibleFreeVoxels;
379  }
380  break;
382  }
383  }
384 
385 
386  } // End of namespace
387 } // End of namespace
388 
389 
point3d getPoint(unsigned int i)
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap::point3d &point3d_sensorPt, octomap::Pointcloud &ptr_scan) const
Builds the list of 3D points in global coordinates for a generic observation.
#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.
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
float & z()
Definition: Vector3.h:155
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.
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()
OCTREE_NODE octree_node_t
The type of nodes in the octree, in the "octomap" library.
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 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++ ...
TLikelihoodOptions()
Initilization of default parameters.
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...
A numeric matrix of compile-time fixed size.
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...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
std::vector< float > points3D_y
If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera.
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.
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
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
size_t size() const
Definition: Pointcloud.h:73
This namespace contains algorithms for SLAM, localization, map building, representation of robot&#39;s ac...
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call...
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.
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
bool hasPoints3D
true means the field points3D contains valid data.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
virtual bool isEmpty() const
Returns true if the map is empty/no observation has been inserted.
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
#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...
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().
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void reserve(size_t size)
Definition: Pointcloud.h:75
virtual void load() const
Makes sure all images and other fields which may be externally stored are loaded in memory...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
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)...
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
float & y()
Definition: Vector3.h:150
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam) ...
Declares a class that represents any robot&#39;s observation.
#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:99
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...
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...
size_t size() const
Returns the number of stored points in the map.
std::vector< float > points3D_x
If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera.
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
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.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:63
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1) ...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
Lightweight 3D point.
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
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.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016