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 namespace rtabmap {
43 
44 GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) :
45  GlobalMap(cache, parameters),
46  minMapSize_(Parameters::defaultGridGlobalMinSize())
47 {
48  Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
49 }
50 
51 void GridMap::clear()
52 {
53  gridMap_ = grid_map::GridMap();
54  GlobalMap::clear();
55 }
56 
57 cv::Mat GridMap::createHeightMap(float & xMin, float & yMin, float & cellSize) const
58 {
59  return toImage("elevation", xMin, yMin, cellSize);
60 }
61 
62 cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) const
63 {
64  return toImage("colors", xMin, yMin, cellSize);
65 }
66 
67 cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const
68 {
69  if( gridMap_.hasBasicLayers())
70  {
71  const grid_map::Matrix& data = gridMap_[layer];
72 
73  cv::Mat image;
74  if(layer.compare("elevation") == 0)
75  {
76  image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_32FC1);
77  for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
78  const grid_map::Index index(*iterator);
79  const float& value = data(index(0), index(1));
80  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
81  if (std::isfinite(value))
82  {
83  image.at<float>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)) = value;
84  }
85  }
86  }
87  else if(layer.compare("colors") == 0)
88  {
89  image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_8UC3);
90  for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
91  const grid_map::Index index(*iterator);
92  const float& value = data(index(0), index(1));
93  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
94  if (std::isfinite(value))
95  {
96  const int * ptr = (const int *)&value;
97  cv::Vec3b & color = image.at<cv::Vec3b>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0));
98  color[0] = (unsigned char)(*ptr & 0xFF); // B
99  color[1] = (unsigned char)((*ptr >> 8) & 0xFF); // G
100  color[2] = (unsigned char)((*ptr >> 16) & 0xFF); // R
101  }
102  }
103  }
104  else
105  {
106  UFATAL("Unknown layer \"%s\"", layer.c_str());
107  }
108 
109  xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
110  yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
111  cellSize = gridMap_.getResolution();
112 
113  return image;
114 
115  }
116  return cv::Mat();
117 }
118 
119 pcl::PointCloud<pcl::PointXYZRGB>::Ptr GridMap::createTerrainCloud() const
120 {
121  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
122  if( gridMap_.hasBasicLayers())
123  {
124  const grid_map::Matrix& dataElevation = gridMap_["elevation"];
125  const grid_map::Matrix& dataColors = gridMap_["colors"];
126 
127  cloud->width = gridMap_.getSize()(0);
128  cloud->height = gridMap_.getSize()(1);
129  cloud->resize(cloud->width * cloud->height);
130  cloud->is_dense = false;
131 
132  float xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
133  float yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
134  float cellSize = gridMap_.getResolution();
135 
136  for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator)
137  {
138  const grid_map::Index index(*iterator);
139  const float& value = dataElevation(index(0), index(1));
140  const int* color = (const int*)&dataColors(index(0), index(1));
141  const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
142  pcl::PointXYZRGB & pt = cloud->at(cloud->width-1-imageIndex(0), imageIndex(1));
143  if (std::isfinite(value))
144  {
145  pt.x = xMin + (cloud->width-1-imageIndex(0)) * cellSize;
146  pt.y = yMin + (cloud->height-1-imageIndex(1)) * cellSize;
147  pt.z = value;
148  pt.b = (unsigned char)(*color & 0xFF);
149  pt.g = (unsigned char)((*color >> 8) & 0xFF);
150  pt.r = (unsigned char)((*color >> 16) & 0xFF);
151  }
152  else
153  {
154  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
155  }
156  }
157  }
158  return cloud;
159 }
160 
161 pcl::PolygonMesh::Ptr GridMap::createTerrainMesh() const
162 {
163  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
164  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTerrainCloud();
165  if(!cloud->empty())
166  {
167  mesh->polygons = util3d::organizedFastMesh(
168  cloud,
169  M_PI,
170  true,
171  1);
172 
173  pcl::toPCLPointCloud2(*cloud, mesh->cloud);
174  }
175 
176  return mesh;
177 }
178 
179 void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
180 {
181  UTimer timer;
182 
183  float margin = cellSize_*10.0f;
184 
185  float minX=-minMapSize_/2.0f;
186  float minY=-minMapSize_/2.0f;
187  float maxX=minMapSize_/2.0f;
188  float maxY=minMapSize_/2.0f;
189  bool undefinedSize = minMapSize_ == 0.0f;
190  std::map<int, cv::Mat> occupiedLocalMaps;
191 
192  if(gridMap_.hasBasicLayers())
193  {
194  // update
195  minX=minValues_[0]+margin+cellSize_/2.0f;
196  minY=minValues_[1]+margin+cellSize_/2.0f;
197  maxX=minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - margin;
198  maxY=minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - margin;
199  undefinedSize = false;
200  }
201 
202  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
203  {
204  UASSERT(!iter->second.isNull());
205 
206  float x = iter->second.x();
207  float y =iter->second.y();
208  if(undefinedSize)
209  {
210  minX = maxX = x;
211  minY = maxY = y;
212  undefinedSize = false;
213  }
214  else
215  {
216  if(minX > x)
217  minX = x;
218  else if(maxX < x)
219  maxX = x;
220 
221  if(minY > y)
222  minY = y;
223  else if(maxY < y)
224  maxY = y;
225  }
226  }
227 
228  if(!cache().empty())
229  {
230  UDEBUG("Updating from cache");
231  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
232  {
233  if(uContains(cache(), iter->first))
234  {
235  const LocalGrid & localGrid = cache().at(iter->first);
236 
237  if(!localGrid.is3D())
238  {
239  UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
240  localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
241  continue;
242  }
243 
244  UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
245 
246  //ground
247  cv::Mat occupied;
248  if(localGrid.groundCells.cols || localGrid.obstacleCells.cols)
249  {
250  occupied = cv::Mat(1, localGrid.groundCells.cols+localGrid.obstacleCells.cols, CV_32FC4);
251  }
252  if(localGrid.groundCells.cols)
253  {
254  if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
255  {
256  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
257  }
258  for(int i=0; i<localGrid.groundCells.cols; ++i)
259  {
260  const float * vi = localGrid.groundCells.ptr<float>(0,i);
261  float * vo = occupied.ptr<float>(0,i);
262  cv::Point3f vt;
263  vo[3] = 0xFFFFFFFF; // RGBA
264  if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
265  {
266  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
267  if(localGrid.groundCells.channels() == 4)
268  {
269  vo[3] = vi[3];
270  }
271  }
272  else
273  {
274  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
275  }
276  vo[0] = vt.x;
277  vo[1] = vt.y;
278  vo[2] = vt.z;
279  if(minX > vo[0])
280  minX = vo[0];
281  else if(maxX < vo[0])
282  maxX = vo[0];
283 
284  if(minY > vo[1])
285  minY = vo[1];
286  else if(maxY < vo[1])
287  maxY = vo[1];
288  }
289  }
290 
291  //obstacles
292  if(localGrid.obstacleCells.cols)
293  {
294  if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
295  {
296  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
297  }
298  for(int i=0; i<localGrid.obstacleCells.cols; ++i)
299  {
300  const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
301  float * vo = occupied.ptr<float>(0,i+localGrid.groundCells.cols);
302  cv::Point3f vt;
303  vo[3] = 0xFFFFFFFF; // RGBA
304  if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
305  {
306  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
307  if(localGrid.obstacleCells.channels() == 4)
308  {
309  vo[3] = vi[3];
310  }
311  }
312  else
313  {
314  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
315  }
316  vo[0] = vt.x;
317  vo[1] = vt.y;
318  vo[2] = vt.z;
319  if(minX > vo[0])
320  minX = vo[0];
321  else if(maxX < vo[0])
322  maxX = vo[0];
323 
324  if(minY > vo[1])
325  minY = vo[1];
326  else if(maxY < vo[1])
327  maxY = vo[1];
328  }
329  }
330  uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
331  }
332  }
333  }
334 
335  if(minX != maxX && minY != maxY)
336  {
337  //Get map size
338  float xMin = minX-margin;
339  xMin -= cellSize_/2.0f;
340  float yMin = minY-margin;
341  yMin -= cellSize_/2.0f;
342  float xMax = maxX+margin;
343  float yMax = maxY+margin;
344 
345  if(fabs((yMax - yMin) / cellSize_) > 99999 ||
346  fabs((xMax - xMin) / cellSize_) > 99999)
347  {
348  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
349  "There's maybe an error with the poses provided! The map will not be created!",
350  xMin, yMin, xMax, yMax);
351  }
352  else
353  {
354  UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
355  cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
356  if(!gridMap_.hasBasicLayers())
357  {
358  UDEBUG("Map empty!");
359  grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
360  grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
361 
362  UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]);
363  gridMap_.setGeometry(length, cellSize_, position);
364  UDEBUG("size: %d, %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
365  // Add elevation layer
366  gridMap_.add("elevation");
367  gridMap_.add("node_ids");
368  gridMap_.add("colors");
369  gridMap_.setBasicLayers({"elevation"});
370  }
371  else
372  {
373  if(xMin == minValues_[0] && yMin == minValues_[1] &&
374  newMapSize.width == gridMap_.getSize()[0] &&
375  newMapSize.height == gridMap_.getSize()[1])
376  {
377  // same map size and origin, don't do anything
378  UDEBUG("Map same size!");
379  }
380  else
381  {
382  UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
383  UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
384  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());
385  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());
386 
387  UDEBUG("Copy map");
388  // copy the old map in the new map
389  // make sure the translation is cellSize
390  int deltaX = 0;
391  if(xMin < minValues_[0])
392  {
393  deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
394  xMin = minValues_[0]-float(deltaX)*cellSize_;
395  }
396  int deltaY = 0;
397  if(yMin < minValues_[1])
398  {
399  deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
400  yMin = minValues_[1]-float(deltaY)*cellSize_;
401  }
402  UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
403  newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
404  newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
405  UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], newMapSize.width, newMapSize.height);
406  UASSERT(newMapSize.width >= gridMap_.getSize()[0] && newMapSize.height >= gridMap_.getSize()[1]);
407  UASSERT(newMapSize.width >= gridMap_.getSize()[0]+deltaX && newMapSize.height >= gridMap_.getSize()[1]+deltaY);
408  UASSERT(deltaX>=0 && deltaY>=0);
409 
410  grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
411  grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
412 
413  grid_map::GridMap tmpExtendedMap;
414  tmpExtendedMap.setGeometry(length, cellSize_, position);
415 
416  UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);
417 
418  UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)",
419  gridMap_.getLength()[0], gridMap_.getLength()[1],
420  gridMap_.getPosition()[0], gridMap_.getPosition()[1],
421  tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1],
422  tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]);
423  if(!gridMap_.extendToInclude(tmpExtendedMap))
424  {
425  UERROR("Failed to update size of the grid map");
426  }
427  UDEBUG("Updated side: %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
428  }
429  }
430  UDEBUG("map %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
431  if(newPoses.size())
432  {
433  UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
434  }
435  grid_map::Matrix& gridMapData = gridMap_["elevation"];
436  grid_map::Matrix& gridMapNodeIds = gridMap_["node_ids"];
437  grid_map::Matrix& gridMapColors = gridMap_["colors"];
438  for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
439  {
440  std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(kter->first);
441  if(iter!=occupiedLocalMaps.end())
442  {
443  addAssembledNode(kter->first, kter->second);
444 
445  for(int i=0; i<iter->second.cols; ++i)
446  {
447  float * ptf = iter->second.ptr<float>(0,i);
448  grid_map::Position position(ptf[0], ptf[1]);
449  grid_map::Index index;
450  if(gridMap_.getIndex(position, index))
451  {
452  // If no elevation has been set, use current elevation.
453  if (!gridMap_.isValid(index))
454  {
455  gridMapData(index(0), index(1)) = ptf[2];
456  gridMapNodeIds(index(0), index(1)) = kter->first;
457  gridMapColors(index(0), index(1)) = ptf[3];
458  }
459  else
460  {
461  if ((gridMapData(index(0), index(1)) < ptf[2] && (gridMapNodeIds(index(0), index(1)) <= kter->first || kter->first == -1)) ||
462  gridMapNodeIds(index(0), index(1)) < kter->first)
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  }
469  }
470  else
471  {
472  UERROR("Outside map!? (%d) (%f,%f) -> (%d,%d)", i, ptf[0], ptf[1], index[0], index[1]);
473  }
474  }
475  }
476  }
477 
478  minValues_[0] = xMin;
479  minValues_[1] = yMin;
480  }
481  }
482  UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
483 }
484 
485 }
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:179
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::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
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(...)
rtabmap::GridMap::gridMap_
grid_map::GridMap gridMap_
Definition: GridMap.h:63
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:161
rtabmap::GridMap::createTerrainCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createTerrainCloud() const
Definition: GridMap.cpp:119
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:64
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::GridMap::createHeightMap
cv::Mat createHeightMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:57
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
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:67
empty
iter
iterator iter(handle obj)
rtabmap::GridMap::GridMap
GridMap(const LocalGridCache *cache, const ParametersMap &parameters=ParametersMap())
Definition: GridMap.cpp:44
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:51
rtabmap::GridMap::createColorMap
cv::Mat createColorMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:62
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
value
value
i
int i

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11