Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
15
16namespace mrpt
17{
18 namespace maps
19 {
20 template <class OCTREE,class OCTREE_NODE>
21 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
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>
138 {
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 }
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 }
161 }
162
163 template <class OCTREE,class OCTREE_NODE>
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;
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 {
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);
215 }
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)
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);
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
322 LOADABLEOPTS_DUMP_VAR(decimation,int);
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
#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...
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#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
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
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.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
octomap::ColorOcTreeNode octree_node_t
The type of nodes in the octree, in the "octomap" library.
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
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 void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
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().
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight,...
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.
size_t size() const
Returns the number of stored points in the map.
A numeric matrix of compile-time fixed size.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
const POINTSMAP * buildAuxPointsMap(const void *options=NULL) const
Returns a cached points map representing this laser scan, building it upon the first call.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
virtual void load() const MRPT_OVERRIDE
Makes sure all images and other fields which may be externally stored are loaded in memory.
bool hasPoints3D
true means the field points3D contains valid data.
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Declares a class that represents any robot's observation.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:50
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).
static CSetOfObjectsPtr Create()
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
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 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...
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
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
This CStream derived class allow using a file as a write-only, binary stream.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
#define MRPT_START
Definition: mrpt_macros.h:349
#define MRPT_END
Definition: mrpt_macros.h:353
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: mrpt_macros.h:200
This namespace contains representation of robot actions and observations.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
@ UNINITIALIZED_POSE
Definition: CPoseOrPoint.h:35
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations,...
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.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
TLikelihoodOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to 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.
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Lightweight 3D point (float version).



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:21:40 UTC 2021