GridMap.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/global_map/GridMap.h>
29 #include <rtabmap/core/util3d_transforms.h>
30 #include <rtabmap/core/util3d.h>
31 #include <rtabmap/core/util3d_surface.h>
32 #include <rtabmap/core/CameraModel.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <rtabmap/utilite/ULogger.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <list>
37 
38 #include <opencv2/photo.hpp>
39 #include <grid_map_core/iterators/GridMapIterator.hpp>
40 #include <pcl/io/pcd_io.h>
41 
42 #include <grid_map_core/GridMap.hpp>
43 
44 namespace rtabmap {
45 
46 GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) :
47  GlobalMap(cache, parameters),
48  gridMap_(new grid_map::GridMap()),
49  minMapSize_(Parameters::defaultGridGlobalMinSize())
50 {
51  Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
52 }
53 
54 GridMap::~GridMap()
55 {
56  delete gridMap_;
57 }
58 
59 void GridMap::clear()
60 {
61  delete gridMap_;
62  gridMap_ = new grid_map::GridMap();
63  GlobalMap::clear();
64 }
65 
66 cv::Mat GridMap::createHeightMap(float & xMin, float & yMin, float & cellSize) const
67 {
68  return toImage("elevation", xMin, yMin, cellSize);
69 }
70 
71 cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) const
72 {
73  return toImage("colors", xMin, yMin, cellSize);
74 }
75 
76 cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const
77 {
78  if( gridMap_->hasBasicLayers())
79  {
80  const grid_map::Matrix& data = (*gridMap_)[layer];
81 
82  cv::Mat image;
83  if(layer.compare("elevation") == 0)
84  {
85  image = cv::Mat::zeros(gridMap_->getSize()(1), gridMap_->getSize()(0), CV_32FC1);
86  for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator) {
87  const grid_map::Index index(*iterator);
88  const float& value = data(index(0), index(1));
89  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
90  if (std::isfinite(value))
91  {
92  image.at<float>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)) = value;
93  }
94  }
95  }
96  else if(layer.compare("colors") == 0)
97  {
98  image = cv::Mat::zeros(gridMap_->getSize()(1), gridMap_->getSize()(0), CV_8UC3);
99  for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator) {
100  const grid_map::Index index(*iterator);
101  const float& value = data(index(0), index(1));
102  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
103  if (std::isfinite(value))
104  {
105  const int * ptr = (const int *)&value;
106  cv::Vec3b & color = image.at<cv::Vec3b>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0));
107  color[0] = (unsigned char)(*ptr & 0xFF); // B
108  color[1] = (unsigned char)((*ptr >> 8) & 0xFF); // G
109  color[2] = (unsigned char)((*ptr >> 16) & 0xFF); // R
110  }
111  }
112  }
113  else
114  {
115  UFATAL("Unknown layer \"%s\"", layer.c_str());
116  }
117 
118  xMin = gridMap_->getPosition().x() - gridMap_->getLength().x()/2.0f;
119  yMin = gridMap_->getPosition().y() - gridMap_->getLength().y()/2.0f;
120  cellSize = gridMap_->getResolution();
121 
122  return image;
123 
124  }
125  return cv::Mat();
126 }
127 
128 pcl::PointCloud<pcl::PointXYZRGB>::Ptr GridMap::createTerrainCloud() const
129 {
130  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
131  if( gridMap_->hasBasicLayers())
132  {
133  const grid_map::Matrix& dataElevation = (*gridMap_)["elevation"];
134  const grid_map::Matrix& dataColors = (*gridMap_)["colors"];
135 
136  cloud->width = gridMap_->getSize()(0);
137  cloud->height = gridMap_->getSize()(1);
138  cloud->resize(cloud->width * cloud->height);
139  cloud->is_dense = false;
140 
141  float xMin = gridMap_->getPosition().x() - gridMap_->getLength().x()/2.0f;
142  float yMin = gridMap_->getPosition().y() - gridMap_->getLength().y()/2.0f;
143  float cellSize = gridMap_->getResolution();
144 
145  for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator)
146  {
147  const grid_map::Index index(*iterator);
148  const float& value = dataElevation(index(0), index(1));
149  const int* color = (const int*)&dataColors(index(0), index(1));
150  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
151  pcl::PointXYZRGB & pt = cloud->at(cloud->width-1-imageIndex(0), imageIndex(1));
152  if (std::isfinite(value))
153  {
154  pt.x = xMin + (cloud->width-1-imageIndex(0)) * cellSize;
155  pt.y = yMin + (cloud->height-1-imageIndex(1)) * cellSize;
156  pt.z = value;
157  pt.b = (unsigned char)(*color & 0xFF);
158  pt.g = (unsigned char)((*color >> 8) & 0xFF);
159  pt.r = (unsigned char)((*color >> 16) & 0xFF);
160  }
161  else
162  {
163  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
164  }
165  }
166  }
167  return cloud;
168 }
169 
170 pcl::PolygonMesh::Ptr GridMap::createTerrainMesh() const
171 {
172  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
173  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTerrainCloud();
174  if(!cloud->empty())
175  {
176  mesh->polygons = util3d::organizedFastMesh(
177  cloud,
178  M_PI,
179  true,
180  1);
181 
182  pcl::toPCLPointCloud2(*cloud, mesh->cloud);
183  }
184 
185  return mesh;
186 }
187 
188 void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
189 {
190  UTimer timer;
191 
192  float margin = cellSize_*10.0f;
193 
194  float minX=-minMapSize_/2.0f;
195  float minY=-minMapSize_/2.0f;
196  float maxX=minMapSize_/2.0f;
197  float maxY=minMapSize_/2.0f;
198  bool undefinedSize = minMapSize_ == 0.0f;
199  std::map<int, cv::Mat> occupiedLocalMaps;
200 
201  if(gridMap_->hasBasicLayers())
202  {
203  // update
204  minX=minValues_[0]+margin+cellSize_/2.0f;
205  minY=minValues_[1]+margin+cellSize_/2.0f;
206  maxX=minValues_[0]+float(gridMap_->getSize()[0])*cellSize_ - margin;
207  maxY=minValues_[1]+float(gridMap_->getSize()[1])*cellSize_ - margin;
208  undefinedSize = false;
209  }
210 
211  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
212  {
213  UASSERT(!iter->second.isNull());
214 
215  float x = iter->second.x();
216  float y =iter->second.y();
217  if(undefinedSize)
218  {
219  minX = maxX = x;
220  minY = maxY = y;
221  undefinedSize = false;
222  }
223  else
224  {
225  if(minX > x)
226  minX = x;
227  else if(maxX < x)
228  maxX = x;
229 
230  if(minY > y)
231  minY = y;
232  else if(maxY < y)
233  maxY = y;
234  }
235  }
236 
237  if(!cache().empty())
238  {
239  UDEBUG("Updating from cache");
240  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
241  {
242  if(uContains(cache(), iter->first))
243  {
244  const LocalGrid & localGrid = cache().at(iter->first);
245 
246  if(!localGrid.is3D())
247  {
248  UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
249  localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
250  continue;
251  }
252 
253  UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
254 
255  //ground
256  cv::Mat occupied;
257  if(localGrid.groundCells.cols || localGrid.obstacleCells.cols)
258  {
259  occupied = cv::Mat(1, localGrid.groundCells.cols+localGrid.obstacleCells.cols, CV_32FC4);
260  }
261  if(localGrid.groundCells.cols)
262  {
263  if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
264  {
265  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
266  }
267  for(int i=0; i<localGrid.groundCells.cols; ++i)
268  {
269  const float * vi = localGrid.groundCells.ptr<float>(0,i);
270  float * vo = occupied.ptr<float>(0,i);
271  cv::Point3f vt;
272  vo[3] = 0xFFFFFFFF; // RGBA
273  if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
274  {
275  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
276  if(localGrid.groundCells.channels() == 4)
277  {
278  vo[3] = vi[3];
279  }
280  }
281  else
282  {
283  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
284  }
285  vo[0] = vt.x;
286  vo[1] = vt.y;
287  vo[2] = vt.z;
288  if(minX > vo[0])
289  minX = vo[0];
290  else if(maxX < vo[0])
291  maxX = vo[0];
292 
293  if(minY > vo[1])
294  minY = vo[1];
295  else if(maxY < vo[1])
296  maxY = vo[1];
297  }
298  }
299 
300  //obstacles
301  if(localGrid.obstacleCells.cols)
302  {
303  if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
304  {
305  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
306  }
307  for(int i=0; i<localGrid.obstacleCells.cols; ++i)
308  {
309  const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
310  float * vo = occupied.ptr<float>(0,i+localGrid.groundCells.cols);
311  cv::Point3f vt;
312  vo[3] = 0xFFFFFFFF; // RGBA
313  if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
314  {
315  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
316  if(localGrid.obstacleCells.channels() == 4)
317  {
318  vo[3] = vi[3];
319  }
320  }
321  else
322  {
323  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
324  }
325  vo[0] = vt.x;
326  vo[1] = vt.y;
327  vo[2] = vt.z;
328  if(minX > vo[0])
329  minX = vo[0];
330  else if(maxX < vo[0])
331  maxX = vo[0];
332 
333  if(minY > vo[1])
334  minY = vo[1];
335  else if(maxY < vo[1])
336  maxY = vo[1];
337  }
338  }
339  uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
340  }
341  }
342  }
343 
344  if(minX != maxX && minY != maxY)
345  {
346  //Get map size
347  float xMin = minX-margin;
348  xMin -= cellSize_/2.0f;
349  float yMin = minY-margin;
350  yMin -= cellSize_/2.0f;
351  float xMax = maxX+margin;
352  float yMax = maxY+margin;
353 
354  if(fabs((yMax - yMin) / cellSize_) > 99999 ||
355  fabs((xMax - xMin) / cellSize_) > 99999)
356  {
357  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
358  "There's maybe an error with the poses provided! The map will not be created!",
359  xMin, yMin, xMax, yMax);
360  }
361  else
362  {
363  UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
364  cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
365  if(!gridMap_->hasBasicLayers())
366  {
367  UDEBUG("Map empty!");
368  grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
369  grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
370 
371  UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]);
372  gridMap_->setGeometry(length, cellSize_, position);
373  UDEBUG("size: %d, %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
374  // Add elevation layer
375  gridMap_->add("elevation");
376  gridMap_->add("node_ids");
377  gridMap_->add("colors");
378  gridMap_->setBasicLayers({"elevation"});
379  }
380  else
381  {
382  if(xMin == minValues_[0] && yMin == minValues_[1] &&
383  newMapSize.width == gridMap_->getSize()[0] &&
384  newMapSize.height == gridMap_->getSize()[1])
385  {
386  // same map size and origin, don't do anything
387  UDEBUG("Map same size!");
388  }
389  else
390  {
391  UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
392  UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
393  UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_->getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_->getSize()[0], cellSize_).c_str());
394  UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_->getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_->getSize()[1], cellSize_).c_str());
395 
396  UDEBUG("Copy map");
397  // copy the old map in the new map
398  // make sure the translation is cellSize
399  int deltaX = 0;
400  if(xMin < minValues_[0])
401  {
402  deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
403  xMin = minValues_[0]-float(deltaX)*cellSize_;
404  }
405  int deltaY = 0;
406  if(yMin < minValues_[1])
407  {
408  deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
409  yMin = minValues_[1]-float(deltaY)*cellSize_;
410  }
411  UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
412  newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
413  newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
414  UDEBUG("%d/%d -> %d/%d", gridMap_->getSize()[0], gridMap_->getSize()[1], newMapSize.width, newMapSize.height);
415  UASSERT(newMapSize.width >= gridMap_->getSize()[0] && newMapSize.height >= gridMap_->getSize()[1]);
416  UASSERT(newMapSize.width >= gridMap_->getSize()[0]+deltaX && newMapSize.height >= gridMap_->getSize()[1]+deltaY);
417  UASSERT(deltaX>=0 && deltaY>=0);
418 
419  grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
420  grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
421 
422  grid_map::GridMap tmpExtendedMap;
423  tmpExtendedMap.setGeometry(length, cellSize_, position);
424 
425  UDEBUG("%d/%d -> %d/%d", gridMap_->getSize()[0], gridMap_->getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);
426 
427  UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)",
428  gridMap_->getLength()[0], gridMap_->getLength()[1],
429  gridMap_->getPosition()[0], gridMap_->getPosition()[1],
430  tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1],
431  tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]);
432  if(!gridMap_->extendToInclude(tmpExtendedMap))
433  {
434  UERROR("Failed to update size of the grid map");
435  }
436  UDEBUG("Updated side: %d %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
437  }
438  }
439  UDEBUG("map %d %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
440  if(newPoses.size())
441  {
442  UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
443  }
444  grid_map::Matrix& gridMapData = (*gridMap_)["elevation"];
445  grid_map::Matrix& gridMapNodeIds = (*gridMap_)["node_ids"];
446  grid_map::Matrix& gridMapColors = (*gridMap_)["colors"];
447  for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
448  {
449  std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(kter->first);
450  if(iter!=occupiedLocalMaps.end())
451  {
452  addAssembledNode(kter->first, kter->second);
453 
454  for(int i=0; i<iter->second.cols; ++i)
455  {
456  float * ptf = iter->second.ptr<float>(0,i);
457  grid_map::Position position(ptf[0], ptf[1]);
458  grid_map::Index index;
459  if(gridMap_->getIndex(position, index))
460  {
461  // If no elevation has been set, use current elevation.
462  if (!gridMap_->isValid(index))
463  {
464  gridMapData(index(0), index(1)) = ptf[2];
465  gridMapNodeIds(index(0), index(1)) = kter->first;
466  gridMapColors(index(0), index(1)) = ptf[3];
467  }
468  else
469  {
470  if ((gridMapData(index(0), index(1)) < ptf[2] && (gridMapNodeIds(index(0), index(1)) <= kter->first || kter->first == -1)) ||
471  gridMapNodeIds(index(0), index(1)) < kter->first)
472  {
473  gridMapData(index(0), index(1)) = ptf[2];
474  gridMapNodeIds(index(0), index(1)) = kter->first;
475  gridMapColors(index(0), index(1)) = ptf[3];
476  }
477  }
478  }
479  else
480  {
481  UERROR("Outside map!? (%d) (%f,%f) -> (%d,%d)", i, ptf[0], ptf[1], index[0], index[1]);
482  }
483  }
484  }
485  }
486 
487  minValues_[0] = xMin;
488  minValues_[1] = yMin;
489  }
490  }
491  UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
492 }
493 
494 }
rtabmap::GlobalMap::clear
virtual void clear()
Definition: GlobalMap.cpp:85
rtabmap::GridMap::assemble
virtual void assemble(const std::list< std::pair< int, Transform > > &newPoses)
Definition: GridMap.cpp:188
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
util3d_surface.h
timer
rtabmap::LocalGrid::obstacleCells
cv::Mat obstacleCells
Definition: LocalGrid.h:50
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
iterator
rtabmap::LocalGrid::groundCells
cv::Mat groundCells
Definition: LocalGrid.h:49
util3d.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::GridMap
Definition: GridMap.h:44
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:503
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_CORE_EXPORT organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:168
fabs
Real fabs(const Real &a)
rtabmap::LocalGrid
Definition: LocalGrid.h:38
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
UFATAL
#define UFATAL(...)
data
int data[]
util3d_transforms.h
rtabmap::LocalGrid::emptyCells
cv::Mat emptyCells
Definition: LocalGrid.h:51
rtabmap::GlobalMap
Definition: GlobalMap.h:40
rtabmap::LocalGrid::is3D
bool is3D() const
Definition: LocalGrid.cpp:48
GridMap.h
rtabmap::GlobalMap::addAssembledNode
void addAssembledNode(int id, const Transform &pose)
Definition: GlobalMap.cpp:160
rtabmap::GlobalMap::minValues_
double minValues_[3]
Definition: GlobalMap.h:92
rtabmap::GridMap::createTerrainMesh
pcl::PolygonMesh::Ptr createTerrainMesh() const
Definition: GridMap.cpp:170
rtabmap::GridMap::createTerrainCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createTerrainCloud() const
Definition: GridMap.cpp:128
UASSERT
#define UASSERT(condition)
x
x
rtabmap::Parameters
Definition: Parameters.h:170
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
glm::isfinite
GLM_FUNC_DECL bool isfinite(genType const &x)
Test whether or not a scalar or each vector component is a finite value. (From GLM_GTX_compatibility)
UWARN
#define UWARN(...)
rtabmap::GridMap::minMapSize_
float minMapSize_
Definition: GridMap.h:67
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::GridMap::createHeightMap
cv::Mat createHeightMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:66
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::GridMap::gridMap_
grid_map::GridMap * gridMap_
Definition: GridMap.h:66
ULogger.h
ULogger class and convenient macros.
rtabmap::GridMap::toImage
cv::Mat toImage(const std::string &layer, float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:76
empty
iter
iterator iter(handle obj)
rtabmap::GridMap::GridMap
GridMap(const LocalGridCache *cache, const ParametersMap &parameters=ParametersMap())
Definition: GridMap.cpp:46
Index
struct Index Index
Definition: sqlite3.c:8577
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::GlobalMap::cache
const std::map< int, LocalGrid > & cache() const
Definition: GlobalMap.h:76
float
float
rtabmap::GlobalMap::cellSize_
float cellSize_
Definition: GlobalMap.h:83
CameraModel.h
handle::ptr
PyObject *& ptr()
rtabmap::GridMap::clear
virtual void clear()
Definition: GridMap.cpp:59
rtabmap::GridMap::createColorMap
cv::Mat createColorMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:71
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
value
value
i
int i
rtabmap::GridMap::~GridMap
virtual ~GridMap()
Definition: GridMap.cpp:54
grid_map
Definition: GridMap.h:38

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:46