Main MRPT website > C++ reference for MRPT 1.5.6
COctoMap.cpp
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 #include "maps-precomp.h" // Precomp header
11 
12 #include <octomap/octomap.h>
13 #include <octomap/OcTree.h>
14 #include <mrpt/utils/pimpl.h>
15 PIMPL_IMPLEMENT(octomap::OcTree);
16 
17 #include <mrpt/maps/COctoMap.h>
18 #include <mrpt/maps/CPointsMap.h>
21 #include <mrpt/utils/CStream.h>
22 #include <mrpt/system/filesystem.h>
24 
25 #include "COctoMapBase_impl.h"
26 
27 // Explicit instantiation:
29 
30 using namespace std;
31 using namespace mrpt;
32 using namespace mrpt::maps;
33 using namespace mrpt::obs;
34 using namespace mrpt::utils;
35 using namespace mrpt::poses;
36 using namespace mrpt::math;
37 using namespace mrpt::opengl;
38 
39 // =========== Begin of Map definition ============
41 
43  resolution(0.10)
44 {
45 }
46 
47 void COctoMap::TMapDefinition::loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
48 {
49  // [<sectionNamePrefix>+"_creationOpts"]
50  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
51  MRPT_LOAD_CONFIG_VAR(resolution, double, source,sSectCreation);
52 
53  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
54  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix+string("_likelihoodOpts") );
55 }
56 
57 void COctoMap::TMapDefinition::dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
58 {
59  LOADABLEOPTS_DUMP_VAR(resolution , double);
60 
61  this->insertionOpts.dumpToTextStream(out);
62  this->likelihoodOpts.dumpToTextStream(out);
63 }
64 
65 mrpt::maps::CMetricMap* COctoMap::internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &_def)
66 {
67  const COctoMap::TMapDefinition &def = *dynamic_cast<const COctoMap::TMapDefinition*>(&_def);
68  COctoMap *obj = new COctoMap(def.resolution);
71  return obj;
72 }
73 // =========== End of Map definition Block =========
74 
76 
77 /*---------------------------------------------------------------
78  Constructor
79  ---------------------------------------------------------------*/
80 COctoMap::COctoMap(const double resolution)
81 {
82  m_octomap.ptr.reset(new octomap::OcTree(resolution));
83 }
84 
85 /*---------------------------------------------------------------
86  Destructor
87  ---------------------------------------------------------------*/
88 COctoMap::~COctoMap()
89 {
90 }
91 
92 
93 /*---------------------------------------------------------------
94  writeToStream
95  Implements the writing to a CStream capability of
96  CSerializable objects
97  ---------------------------------------------------------------*/
98 void COctoMap::writeToStream(mrpt::utils::CStream &out, int *version) const
99 {
100  if (version)
101  *version = 2;
102  else
103  {
104  this->likelihoodOptions.writeToStream(out);
105  this->renderingOptions.writeToStream(out); // Added in v1
106  out << genericMapParams; // v2
107 
108  CMemoryChunk chunk;
109  const string tmpFil = mrpt::system::getTempFileName();
110  const_cast<octomap::OcTree*>(&PIMPL_GET_REF(OcTree, m_octomap))->writeBinary(tmpFil);
111  chunk.loadBufferFromFile(tmpFil);
112  mrpt::system::deleteFile(tmpFil);
113 
114  out << chunk;
115  }
116 }
117 
118 /*---------------------------------------------------------------
119  readFromStream
120  Implements the reading from a CStream capability of
121  CSerializable objects
122  ---------------------------------------------------------------*/
123 void COctoMap::readFromStream(mrpt::utils::CStream &in, int version)
124 {
125  switch(version)
126  {
127  case 0:
128  case 1:
129  case 2:
130  {
131  this->likelihoodOptions.readFromStream(in);
132  if (version>=1) this->renderingOptions.readFromStream(in);
133  if (version>=2) in >> genericMapParams;
134 
135  this->clear();
136 
137  CMemoryChunk chunk;
138  in >> chunk;
139 
140  if (chunk.getTotalBytesCount())
141  {
142  const string tmpFil = mrpt::system::getTempFileName();
143  if (!chunk.saveBufferToFile( tmpFil ) ) THROW_EXCEPTION("Error saving temporary file");
144  PIMPL_GET_REF(OcTree, m_octomap).readBinary(tmpFil);
145  mrpt::system::deleteFile( tmpFil );
146  }
147 
148  } break;
149  default:
151  };
152 
153 }
154 
155 
156 bool COctoMap::internal_insertObservation(const mrpt::obs::CObservation *obs,const CPose3D *robotPose)
157 {
158  octomap::point3d sensorPt;
159  octomap::Pointcloud scan;
160  if (!internal_build_PointCloud_for_observation(obs,robotPose, sensorPt, scan))
161  return false; // Nothing to do.
162  // Insert rays:
163  PIMPL_GET_REF(OcTree, m_octomap).insertPointCloud(scan, sensorPt, insertionOptions.maxrange, insertionOptions.pruning);
164  return true;
165 }
166 
167 
168 
169 /** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object. */
170 void COctoMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
171 {
172  // Go thru all voxels:
173  //OcTreeVolume voxel; // current voxel, possibly transformed
174  octomap::OcTree::tree_iterator it_end = PIMPL_GET_REF(OcTree, m_octomap).end_tree();
175 
176  const unsigned char max_depth = 0; // all
177  const TColorf general_color = gl_obj.getColor();
178  const TColor general_color_u(general_color.R*255,general_color.G*255,general_color.B*255,general_color.A*255);
179 
180  gl_obj.clear();
181  gl_obj.reserveGridCubes( this->calcNumNodes() );
182 
183  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
184 
185  gl_obj.showVoxels(VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels );
186  gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels );
187 
188  const size_t nLeafs = this->getNumLeafNodes();
189  gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
190  gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
191 
192  double xmin, xmax, ymin, ymax, zmin, zmax, inv_dz;
193  this->getMetricMin(xmin, ymin, zmin);
194  this->getMetricMax(xmax, ymax, zmax);
195  inv_dz = 1/(zmax-zmin + 0.01);
196 
197  for(octomap::OcTree::tree_iterator it = PIMPL_GET_REF(OcTree, m_octomap).begin_tree(max_depth);it!=it_end; ++it)
198  {
199  const octomap::point3d vx_center = it.getCoordinate();
200  const double vx_length = it.getSize();
201  const double L = 0.5*vx_length;
202 
203  if (it.isLeaf())
204  {
205  // voxels for leaf nodes
206  const double occ = it->getOccupancy();
207  if ( (occ>=0.5 && renderingOptions.generateOccupiedVoxels) ||
208  (occ<0.5 && renderingOptions.generateFreeVoxels) )
209  {
210  mrpt::utils::TColor vx_color;
211  double coefc, coeft;
212  switch (gl_obj.getVisualizationMode()) {
213  case COctoMapVoxels::FIXED:
214  vx_color = general_color_u;
215  break;
216  case COctoMapVoxels::COLOR_FROM_HEIGHT:
217  coefc = 255*inv_dz*(vx_center.z()-zmin);
218  vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 255.0*general_color.A);
219  break;
220 
221  case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
222  coefc = 240*(1-occ) + 15;
223  vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 255.0*general_color.A);
224  break;
225 
226  case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
227  coeft = 255 - 510*(1-occ);
228  if (coeft < 0) { coeft = 0; }
229  vx_color = TColor(255*general_color.R, 255*general_color.G, 255*general_color.B, coeft);
230  break;
231 
232  case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
233  coefc = 240*(1-occ) + 15;
234  vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, 50);
235  break;
236 
237  case COctoMapVoxels::MIXED:
238  coefc = 255*inv_dz*(vx_center.z()-zmin);
239  coeft = 255 - 510*(1-occ);
240  if (coeft < 0) { coeft = 0; }
241  vx_color = TColor(coefc*general_color.R, coefc*general_color.G, coefc*general_color.B, coeft);
242  break;
243 
244  default:
245  THROW_EXCEPTION("Unknown coloring scheme!")
246  }
247 
248  const size_t vx_set = (PIMPL_GET_REF(OcTree, m_octomap).isNodeOccupied(*it)) ? VOXEL_SET_OCCUPIED:VOXEL_SET_FREESPACE;
249 
250  gl_obj.push_back_Voxel(vx_set,COctoMapVoxels::TVoxel( TPoint3D(vx_center.x(),vx_center.y(),vx_center.z()) ,vx_length, vx_color) );
251  }
252  }
253  else if (renderingOptions.generateGridLines)
254  {
255  // Not leaf-nodes:
256  const mrpt::math::TPoint3D pt_min ( vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
257  const mrpt::math::TPoint3D pt_max ( vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
258  gl_obj.push_back_GridCube( COctoMapVoxels::TGridCube( pt_min, pt_max ) );
259  }
260  } // end for each voxel
261 
262  // if we use transparency, sort cubes by "Z" as an approximation to far-to-near render ordering:
263  if (gl_obj.isCubeTransparencyEnabled())
264  gl_obj.sort_voxels_by_z();
265 
266  // Set bounding box:
267  {
268  mrpt::math::TPoint3D bbmin, bbmax;
269  this->getMetricMin(bbmin.x,bbmin.y,bbmin.z);
270  this->getMetricMax(bbmax.x,bbmax.y,bbmax.z);
271  gl_obj.setBoundingBox(bbmin, bbmax);
272  }
273 }
274 
275 void COctoMap::insertRay(const float end_x, const float end_y, const float end_z, const float sensor_x, const float sensor_y, const float sensor_z)
276 {
277  PIMPL_GET_REF(OcTree, m_octomap).insertRay(octomap::point3d(sensor_x, sensor_y, sensor_z), octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange, insertionOptions.pruning);
278 }
279 void COctoMap::updateVoxel(const double x, const double y, const double z, bool occupied)
280 {
281  PIMPL_GET_REF(OcTree, m_octomap).updateNode(x, y, z, occupied);
282 }
283 bool COctoMap::isPointWithinOctoMap(const float x, const float y, const float z) const
284 {
285  octomap::OcTreeKey key;
286  return PIMPL_GET_CONSTREF(OcTree, m_octomap).coordToKeyChecked(octomap::point3d(x, y, z), key);
287 }
288 
289 double COctoMap::getResolution() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getResolution(); }
290 unsigned int COctoMap::getTreeDepth() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getTreeDepth(); }
291 size_t COctoMap::size() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).size(); }
292 size_t COctoMap::memoryUsage() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).memoryUsage(); }
293 size_t COctoMap::memoryUsageNode() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).memoryUsageNode(); }
294 size_t COctoMap::memoryFullGrid() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).memoryFullGrid(); }
295 double COctoMap::volume() { return PIMPL_GET_REF(OcTree, m_octomap).volume(); }
296 void COctoMap::getMetricSize(double& x, double& y, double& z) { return PIMPL_GET_REF(OcTree, m_octomap).getMetricSize(x, y, z); }
297 void COctoMap::getMetricSize(double& x, double& y, double& z) const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getMetricSize(x, y, z); }
298 void COctoMap::getMetricMin(double& x, double& y, double& z) { return PIMPL_GET_REF(OcTree, m_octomap).getMetricMin(x, y, z); }
299 void COctoMap::getMetricMin(double& x, double& y, double& z) const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getMetricMin(x, y, z); }
300 void COctoMap::getMetricMax(double& x, double& y, double& z) { return PIMPL_GET_REF(OcTree, m_octomap).getMetricMax(x, y, z); }
301 void COctoMap::getMetricMax(double& x, double& y, double& z) const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getMetricMax(x, y, z); }
302 size_t COctoMap::calcNumNodes() const { return PIMPL_GET_REF(OcTree, m_octomap).calcNumNodes(); }
303 size_t COctoMap::getNumLeafNodes() const { return PIMPL_GET_CONSTREF(OcTree, m_octomap).getNumLeafNodes(); }
304 void COctoMap::setOccupancyThres(double prob) { PIMPL_GET_REF(OcTree, m_octomap).setOccupancyThres(prob); }
305 void COctoMap::setProbHit(double prob) { PIMPL_GET_REF(OcTree, m_octomap).setProbHit(prob); }
306 void COctoMap::setProbMiss(double prob) { PIMPL_GET_REF(OcTree, m_octomap).setProbMiss(prob); }
307 void COctoMap::setClampingThresMin(double thresProb) { PIMPL_GET_REF(OcTree, m_octomap).setClampingThresMin(thresProb); }
308 void COctoMap::setClampingThresMax(double thresProb) { PIMPL_GET_REF(OcTree, m_octomap).setClampingThresMax(thresProb); }
309 double COctoMap::getOccupancyThres() const { return PIMPL_GET_REF(OcTree, m_octomap).getOccupancyThres(); }
310 float COctoMap::getOccupancyThresLog() const { return PIMPL_GET_REF(OcTree, m_octomap).getOccupancyThresLog(); }
311 double COctoMap::getProbHit() const { return PIMPL_GET_REF(OcTree, m_octomap).getProbHit(); }
312 float COctoMap::getProbHitLog() const { return PIMPL_GET_REF(OcTree, m_octomap).getProbHitLog(); }
313 double COctoMap::getProbMiss() const { return PIMPL_GET_REF(OcTree, m_octomap).getProbMiss(); }
314 float COctoMap::getProbMissLog() const { return PIMPL_GET_REF(OcTree, m_octomap).getProbMissLog(); }
315 double COctoMap::getClampingThresMin() const { return PIMPL_GET_REF(OcTree, m_octomap).getClampingThresMin(); }
316 float COctoMap::getClampingThresMinLog() const { return PIMPL_GET_REF(OcTree, m_octomap).getClampingThresMinLog(); }
317 double COctoMap::getClampingThresMax() const { return PIMPL_GET_REF(OcTree, m_octomap).getClampingThresMax(); }
318 float COctoMap::getClampingThresMaxLog() const { return PIMPL_GET_REF(OcTree, m_octomap).getClampingThresMaxLog(); }
319 void COctoMap::internal_clear() { PIMPL_GET_REF(OcTree, m_octomap).clear(); }
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void showVoxels(unsigned int voxel_set, bool show)
Shows/hides the voxels (voxel_set is a 0-based index for the set of voxels to modify, e.g.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void reserveVoxels(const size_t set_index, const size_t nVoxels)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double resolution
The finest resolution of the octomap (default: 0.10 meters)
Definition: maps/COctoMap.h:49
#define THROW_EXCEPTION(msg)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn't need to call this)
void reserveGridCubes(const size_t nCubes)
double z
X,Y,Z coordinates.
void clear()
Clears everything.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
GLuint in
Definition: glew.h:7146
mrpt::utils::TColorf getColor() const
Returns the object color property as a TColorf.
This class allows loading and storing values and vectors of different types from a configuration text...
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: COctoMap.cpp:98
void push_back_GridCube(const TGridCube &c)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
std::string BASE_IMPEXP getTempFileName()
Returns the name of a proposed temporary file name.
Definition: filesystem.cpp:273
TLikelihoodOptions likelihoodOptions
#define PIMPL_GET_CONSTREF(_TYPE, _VAR_NAME)
Definition: pimpl.h:76
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::maps::COctoMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: maps/COctoMap.h:50
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: maps/COctoMap.h:51
A RGB color - 8bit.
Definition: TColor.h:26
GLhandleARB obj
Definition: glew.h:3276
int version
Definition: mrpt_jpeglib.h:898
A memory buffer (implements CStream) which can be itself serialized.
Definition: CMemoryChunk.h:27
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: maps/COctoMap.h:37
bool isCubeTransparencyEnabled() const
#define PIMPL_GET_REF(_TYPE, _VAR_NAME)
Definition: pimpl.h:75
The info of each grid block.
void push_back_Voxel(const size_t set_index, const TVoxel &v)
GLsizeiptr size
Definition: glew.h:1586
#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...
The info of each of the voxels.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
GLdouble GLdouble z
Definition: glew.h:1464
GLclampd zmax
Definition: glew.h:5006
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
visualization_mode_t getVisualizationMode() const
Declares a class that represents any robot's observation.
void resizeVoxelSets(const size_t nVoxelSets)
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
bool BASE_IMPEXP deleteFile(const std::string &fileName)
Deletes a single file.
Definition: filesystem.cpp:175
Lightweight 3D point.
bool loadBufferFromFile(const std::string &file_name)
Loads the entire buffer from a file *.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
PIMPL_IMPLEMENT(octomap::OcTree)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018