OccupancyGrid.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, 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/OccupancyGrid.h>
29 #include <rtabmap/core/util3d.h>
30 #include <rtabmap/core/util3d_filtering.h>
31 #include <rtabmap/core/util3d_mapping.h>
32 #include <rtabmap/utilite/ULogger.h>
33 #include <rtabmap/utilite/UConversion.h>
34 #include <rtabmap/utilite/UStl.h>
35 #include <rtabmap/utilite/UTimer.h>
36 
37 #include <pcl/io/pcd_io.h>
38 
39 namespace rtabmap {
40 
41 OccupancyGrid::OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters) :
42  GlobalMap(cache, parameters),
43  minMapSize_(Parameters::defaultGridGlobalMinSize()),
44  erode_(Parameters::defaultGridGlobalEroded()),
45  footprintRadius_(Parameters::defaultGridGlobalFootprintRadius())
46 {
47  Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
48  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
49  Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
50 
51  UASSERT(minMapSize_ >= 0.0f);
52 }
53 
54 void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
55 {
56  UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
57  map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
58  this->clear();
59  if(!poses.empty() && !map.empty())
60  {
61  UASSERT(cellSize > 0.0f);
62  UASSERT(map.type() == CV_8SC1);
63  map_ = map.clone();
64  mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
65  for(int i=0; i<map_.rows; ++i)
66  {
67  for(int j=0; j<map_.cols; ++j)
68  {
69  const char value = map_.at<char>(i,j);
70  float * info = mapInfo_.ptr<float>(i,j);
71  if(value == 0)
72  {
73  info[3] = logOddsClampingMin_;
74  }
75  else if(value == 100)
76  {
77  info[3] = logOddsClampingMax_;
78  }
79  }
80  }
81  minValues_[0] = xMin;
82  minValues_[1] = yMin;
83  cellSize_ = cellSize;
84  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
85  {
86  addAssembledNode(iter->first, iter->second);
87  }
88  }
89 }
90 
91 void OccupancyGrid::clear()
92 {
93  map_ = cv::Mat();
94  mapInfo_ = cv::Mat();
95  cellCount_.clear();
96  GlobalMap::clear();
97 }
98 
99 cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
100 {
101  xMin = minValues_[0];
102  yMin = minValues_[1];
103 
104  cv::Mat map = map_;
105 
106  UTimer t;
107  if(occupancyThr_ != 0.0f && !map.empty())
108  {
109  float occThr = logodds(occupancyThr_);
110  map = cv::Mat(map.size(), map.type());
111  UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
112  for(int i=0; i<map.rows; ++i)
113  {
114  for(int j=0; j<map.cols; ++j)
115  {
116  const float * info = mapInfo_.ptr<float>(i, j);
117  if(info[3] == 0.0f)
118  {
119  map.at<char>(i, j) = -1; // unknown
120  }
121  else if(info[3] >= occThr)
122  {
123  map.at<char>(i, j) = 100; // unknown
124  }
125  else
126  {
127  map.at<char>(i, j) = 0; // empty
128  }
129  }
130  }
131  UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
132  }
133 
134  if(erode_ && !map.empty())
135  {
136  map = util3d::erodeMap(map);
137  UDEBUG("Eroding map = %fs", t.ticks());
138  }
139  return map;
140 }
141 
142 cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
143 {
144  xMin = minValues_[0];
145  yMin = minValues_[1];
146 
147  cv::Mat map;
148  if(!mapInfo_.empty())
149  {
150  map = cv::Mat(mapInfo_.size(), map_.type());
151  for(int i=0; i<map.rows; ++i)
152  {
153  for(int j=0; j<map.cols; ++j)
154  {
155  const float * info = mapInfo_.ptr<float>(i, j);
156  if(info[3] == 0.0f)
157  {
158  map.at<char>(i, j) = -1; // unknown
159  }
160  else
161  {
162  map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
163  }
164  }
165  }
166  }
167  else
168  {
169  UWARN("Map info is empty, cannot generate probabilistic occupancy grid");
170  }
171  return map;
172 }
173 
174 void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPoses)
175 {
176  UTimer timer;
177 
178  float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
179 
180  float minX=-minMapSize_/2.0f;
181  float minY=-minMapSize_/2.0f;
182  float maxX=minMapSize_/2.0f;
183  float maxY=minMapSize_/2.0f;
184  bool undefinedSize = minMapSize_ == 0.0f;
185  std::map<int, cv::Mat> emptyLocalMaps;
186  std::map<int, cv::Mat> occupiedLocalMaps;
187 
188  if(!map_.empty())
189  {
190  // update
191  minX=minValues_[0]+margin+cellSize_/2.0f;
192  minY=minValues_[1]+margin+cellSize_/2.0f;
193  maxX=minValues_[0]+float(map_.cols)*cellSize_ - margin;
194  maxY=minValues_[1]+float(map_.rows)*cellSize_ - margin;
195  undefinedSize = false;
196  }
197 
198  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
199  {
200  UASSERT(!iter->second.isNull());
201 
202  float x = iter->second.x();
203  float y =iter->second.y();
204  if(undefinedSize)
205  {
206  minX = maxX = x;
207  minY = maxY = y;
208  undefinedSize = false;
209  }
210  else
211  {
212  if(minX > x)
213  minX = x;
214  else if(maxX < x)
215  maxX = x;
216 
217  if(minY > y)
218  minY = y;
219  else if(maxY < y)
220  maxY = y;
221  }
222  }
223 
224  if(!cache().empty())
225  {
226  UDEBUG("Updating from cache");
227  for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
228  {
229  if(uContains(cache(), iter->first))
230  {
231  const LocalGrid & localGrid = cache().at(iter->first);
232 
233  UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
234 
235  //ground
236  cv::Mat ground;
237  if(localGrid.groundCells.cols || localGrid.emptyCells.cols)
238  {
239  ground = cv::Mat(1, localGrid.groundCells.cols+localGrid.emptyCells.cols, CV_32FC2);
240  }
241  if(localGrid.groundCells.cols)
242  {
243  if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
244  {
245  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
246  }
247  for(int i=0; i<localGrid.groundCells.cols; ++i)
248  {
249  const float * vi = localGrid.groundCells.ptr<float>(0,i);
250  float * vo = ground.ptr<float>(0,i);
251  cv::Point3f vt;
252  if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
253  {
254  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
255  }
256  else
257  {
258  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
259  }
260  vo[0] = vt.x;
261  vo[1] = vt.y;
262  if(minX > vo[0])
263  minX = vo[0];
264  else if(maxX < vo[0])
265  maxX = vo[0];
266 
267  if(minY > vo[1])
268  minY = vo[1];
269  else if(maxY < vo[1])
270  maxY = vo[1];
271  }
272  }
273 
274  //empty
275  if(localGrid.emptyCells.cols)
276  {
277  if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1)
278  {
279  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols);
280  }
281  for(int i=0; i<localGrid.emptyCells.cols; ++i)
282  {
283  const float * vi = localGrid.emptyCells.ptr<float>(0,i);
284  float * vo = ground.ptr<float>(0,i+localGrid.groundCells.cols);
285  cv::Point3f vt;
286  if(localGrid.emptyCells.channels() != 2 && localGrid.emptyCells.channels() != 5)
287  {
288  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
289  }
290  else
291  {
292  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
293  }
294  vo[0] = vt.x;
295  vo[1] = vt.y;
296  if(minX > vo[0])
297  minX = vo[0];
298  else if(maxX < vo[0])
299  maxX = vo[0];
300 
301  if(minY > vo[1])
302  minY = vo[1];
303  else if(maxY < vo[1])
304  maxY = vo[1];
305  }
306  }
307  uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
308 
309  //obstacles
310  if(localGrid.obstacleCells.cols)
311  {
312  if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
313  {
314  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
315  }
316  cv::Mat obstacles(1, localGrid.obstacleCells.cols, CV_32FC2);
317  for(int i=0; i<obstacles.cols; ++i)
318  {
319  const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
320  float * vo = obstacles.ptr<float>(0,i);
321  cv::Point3f vt;
322  if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
323  {
324  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
325  }
326  else
327  {
328  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
329  }
330  vo[0] = vt.x;
331  vo[1] = vt.y;
332  if(minX > vo[0])
333  minX = vo[0];
334  else if(maxX < vo[0])
335  maxX = vo[0];
336 
337  if(minY > vo[1])
338  minY = vo[1];
339  else if(maxY < vo[1])
340  maxY = vo[1];
341  }
342  uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
343  }
344  }
345  }
346  }
347 
348  cv::Mat map;
349  cv::Mat mapInfo;
350  if(minX != maxX && minY != maxY)
351  {
352  //Get map size
353  float xMin = minX-margin;
354  xMin -= cellSize_/2.0f;
355  float yMin = minY-margin;
356  yMin -= cellSize_/2.0f;
357  float xMax = maxX+margin;
358  float yMax = maxY+margin;
359 
360  if(fabs((yMax - yMin) / cellSize_) > 99999 ||
361  fabs((xMax - xMin) / cellSize_) > 99999)
362  {
363  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
364  "There's maybe an error with the poses provided! The map will not be created!",
365  xMin, yMin, xMax, yMax);
366  }
367  else
368  {
369  UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
370  cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
371  if(map_.empty())
372  {
373  UDEBUG("Map empty!");
374  map = cv::Mat::ones(newMapSize, CV_8S)*-1;
375  mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
376  }
377  else
378  {
379  if(xMin == minValues_[0] && yMin == minValues_[1] &&
380  newMapSize.width == map_.cols &&
381  newMapSize.height == map_.rows)
382  {
383  // same map size and origin, don't do anything
384  UDEBUG("Map same size!");
385  map = map_;
386  mapInfo = mapInfo_;
387  }
388  else
389  {
390  UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
391  UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
392  UASSERT_MSG(xMax >= minValues_[0]+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], map_.cols, cellSize_).c_str());
393  UASSERT_MSG(yMax >= minValues_[1]+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], map_.rows, cellSize_).c_str());
394 
395  UDEBUG("Copy map");
396  // copy the old map in the new map
397  // make sure the translation is cellSize
398  int deltaX = 0;
399  if(xMin < minValues_[0])
400  {
401  deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
402  xMin = minValues_[0]-float(deltaX)*cellSize_;
403  }
404  int deltaY = 0;
405  if(yMin < minValues_[1])
406  {
407  deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
408  yMin = minValues_[1]-float(deltaY)*cellSize_;
409  }
410  UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
411  newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
412  newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
413  UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
414  UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
415  UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
416  UASSERT(deltaX>=0 && deltaY>=0);
417  map = cv::Mat::ones(newMapSize, CV_8S)*-1;
418  mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
419  map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
420  mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
421  }
422  }
423  UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
424  UDEBUG("map %d %d", map.cols, map.rows);
425  if(newPoses.size())
426  {
427  UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
428  }
429  for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
430  {
431  std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
432  std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
433  if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
434  {
435  addAssembledNode(kter->first, kter->second);
436  std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
437  if(cter == cellCount_.end() && kter->first > 0)
438  {
439  cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
440  }
441  if(iter!=emptyLocalMaps.end())
442  {
443  for(int i=0; i<iter->second.cols; ++i)
444  {
445  float * ptf = iter->second.ptr<float>(0,i);
446  cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
447  UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
448  uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
449  kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
450  char & value = map.at<char>(pt.y, pt.x);
451  if(value != -2)
452  {
453  float * info = mapInfo.ptr<float>(pt.y, pt.x);
454  int nodeId = (int)info[0];
455  if(value != -1)
456  {
457  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
458  {
459  // cannot rewrite on cells referred by more recent nodes
460  continue;
461  }
462  if(nodeId > 0)
463  {
464  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
465  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
466  if(value == 0)
467  {
468  eter->second.first -= 1;
469  }
470  else if(value == 100)
471  {
472  eter->second.second -= 1;
473  }
474  if(kter->first < 0)
475  {
476  eter->second.first += 1;
477  }
478  }
479  }
480  if(kter->first > 0)
481  {
482  info[0] = (float)kter->first;
483  info[1] = ptf[0];
484  info[2] = ptf[1];
485  cter->second.first+=1;
486  }
487  value = 0; // free space
488 
489  // update odds
490  if(nodeId != kter->first)
491  {
492  info[3] += logOddsMiss_;
493  if (info[3] < logOddsClampingMin_)
494  {
495  info[3] = logOddsClampingMin_;
496  }
497  if (info[3] > logOddsClampingMax_)
498  {
499  info[3] = logOddsClampingMax_;
500  }
501  }
502  }
503  }
504  }
505 
506  if(footprintRadius_ >= cellSize_*1.5f)
507  {
508  // place free space under the footprint of the robot
509  cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
510  cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
511  if(ptBegin.x < 0)
512  ptBegin.x = 0;
513  if(ptEnd.x >= map.cols)
514  ptEnd.x = map.cols-1;
515 
516  if(ptBegin.y < 0)
517  ptBegin.y = 0;
518  if(ptEnd.y >= map.rows)
519  ptEnd.y = map.rows-1;
520 
521  for(int i=ptBegin.x; i<ptEnd.x; ++i)
522  {
523  for(int j=ptBegin.y; j<ptEnd.y; ++j)
524  {
525  UASSERT(j < map.rows && i < map.cols);
526  char & value = map.at<char>(j, i);
527  float * info = mapInfo.ptr<float>(j, i);
528  int nodeId = (int)info[0];
529  if(value != -1)
530  {
531  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
532  {
533  // cannot rewrite on cells referred by more recent nodes
534  continue;
535  }
536  if(nodeId>0)
537  {
538  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
539  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
540  if(value == 0)
541  {
542  eter->second.first -= 1;
543  }
544  else if(value == 100)
545  {
546  eter->second.second -= 1;
547  }
548  if(kter->first < 0)
549  {
550  eter->second.first += 1;
551  }
552  }
553  }
554  if(kter->first > 0)
555  {
556  info[0] = (float)kter->first;
557  info[1] = float(i) * cellSize_ + xMin;
558  info[2] = float(j) * cellSize_ + yMin;
559  info[3] = logOddsClampingMin_;
560  cter->second.first+=1;
561  }
562  value = -2; // free space (footprint)
563  }
564  }
565  }
566 
567  if(jter!=occupiedLocalMaps.end())
568  {
569  for(int i=0; i<jter->second.cols; ++i)
570  {
571  float * ptf = jter->second.ptr<float>(0,i);
572  cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
573  UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
574  uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
575  kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
576  char & value = map.at<char>(pt.y, pt.x);
577  if(value != -2)
578  {
579  float * info = mapInfo.ptr<float>(pt.y, pt.x);
580  int nodeId = (int)info[0];
581  if(value != -1)
582  {
583  if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
584  {
585  // cannot rewrite on cells referred by more recent nodes
586  continue;
587  }
588  if(nodeId>0)
589  {
590  std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
591  UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
592  if(value == 0)
593  {
594  eter->second.first -= 1;
595  }
596  else if(value == 100)
597  {
598  eter->second.second -= 1;
599  }
600  if(kter->first < 0)
601  {
602  eter->second.second += 1;
603  }
604  }
605  }
606  if(kter->first > 0)
607  {
608  info[0] = (float)kter->first;
609  info[1] = ptf[0];
610  info[2] = ptf[1];
611  cter->second.second+=1;
612  }
613 
614  // update odds
615  if(nodeId != kter->first || value!=100)
616  {
617  info[3] += logOddsHit_;
618  if (info[3] < logOddsClampingMin_)
619  {
620  info[3] = logOddsClampingMin_;
621  }
622  if (info[3] > logOddsClampingMax_)
623  {
624  info[3] = logOddsClampingMax_;
625  }
626  }
627 
628  value = 100; // obstacles
629  }
630  }
631  }
632  }
633  }
634 
635  if(footprintRadius_ >= cellSize_*1.5f)
636  {
637  for(int i=1; i<map.rows-1; ++i)
638  {
639  for(int j=1; j<map.cols-1; ++j)
640  {
641  char & value = map.at<char>(i, j);
642  if(value == -2)
643  {
644  value = 0;
645  }
646  }
647  }
648  }
649 
650  map_ = map;
651  mapInfo_ = mapInfo;
652  minValues_[0] = xMin;
653  minValues_[1] = yMin;
654 
655  // clean cellCount_
656  for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
657  {
658  UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
659  if(iter->second.first == 0 && iter->second.second == 0)
660  {
661  cellCount_.erase(iter++);
662  }
663  else
664  {
665  ++iter;
666  }
667  }
668  }
669  }
670 
671  UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
672 }
673 
674 unsigned long OccupancyGrid::getMemoryUsed() const
675 {
676  unsigned long memoryUsage = GlobalMap::getMemoryUsed();
677 
678  memoryUsage += map_.total() * map_.elemSize();
679  memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
680  memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
681 
682  return memoryUsage;
683 }
684 
685 }
int
int
rtabmap::GlobalMap::logOddsHit_
float logOddsHit_
Definition: GlobalMap.h:87
rtabmap::GlobalMap::logodds
static float logodds(double probability)
Definition: GlobalMap.h:43
rtabmap::GlobalMap::clear
virtual void clear()
Definition: GlobalMap.cpp:85
timer
rtabmap::OccupancyGrid::map_
cv::Mat map_
Definition: global_map/OccupancyGrid.h:58
rtabmap::LocalGrid::obstacleCells
cv::Mat obstacleCells
Definition: LocalGrid.h:50
rtabmap::GlobalMap::logOddsMiss_
float logOddsMiss_
Definition: GlobalMap.h:88
rtabmap::GlobalMap::getMemoryUsed
virtual unsigned long getMemoryUsed() const
Definition: GlobalMap.cpp:93
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::OccupancyGrid::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: OccupancyGrid.cpp:674
iterator
rtabmap::OccupancyGrid::footprintRadius_
float footprintRadius_
Definition: global_map/OccupancyGrid.h:64
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::erodeMap
cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:996
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(...)
j
std::ptrdiff_t j
rtabmap::LocalGrid::emptyCells
cv::Mat emptyCells
Definition: LocalGrid.h:51
rtabmap::GlobalMap::logOddsClampingMin_
float logOddsClampingMin_
Definition: GlobalMap.h:89
rtabmap::GlobalMap
Definition: GlobalMap.h:40
UConversion.h
Some conversion functions.
util3d_filtering.h
rtabmap::OccupancyGrid::erode_
bool erode_
Definition: global_map/OccupancyGrid.h:63
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::OccupancyGrid::minMapSize_
float minMapSize_
Definition: global_map/OccupancyGrid.h:62
rtabmap::OccupancyGrid::clear
virtual void clear()
Definition: OccupancyGrid.cpp:91
rtabmap::GlobalMap::occupancyThr_
float occupancyThr_
Definition: GlobalMap.h:86
rtabmap::OccupancyGrid::OccupancyGrid
OccupancyGrid(const LocalGridCache *cache, const ParametersMap &parameters=ParametersMap())
Definition: OccupancyGrid.cpp:41
info
else if n * info
UASSERT
#define UASSERT(condition)
x
x
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:99
rtabmap::Parameters
Definition: Parameters.h:170
util3d_mapping.h
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::OccupancyGrid::mapInfo_
cv::Mat mapInfo_
Definition: global_map/OccupancyGrid.h:59
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger.h
ULogger class and convenient macros.
rtabmap::GlobalMap::probability
static double probability(double logodds)
Definition: GlobalMap.h:48
empty
rtabmap::OccupancyGrid::cellCount_
std::map< int, std::pair< int, int > > cellCount_
Definition: global_map/OccupancyGrid.h:60
iter
iterator iter(handle obj)
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::GlobalMap::cache
const std::map< int, LocalGrid > & cache() const
Definition: GlobalMap.h:76
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:142
rtabmap::GlobalMap::logOddsClampingMax_
float logOddsClampingMax_
Definition: GlobalMap.h:90
float
float
rtabmap::GlobalMap::cellSize_
float cellSize_
Definition: GlobalMap.h:83
handle::ptr
PyObject *& ptr()
OccupancyGrid.h
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
rtabmap::OccupancyGrid::setMap
void setMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:54
rtabmap::OccupancyGrid::assemble
virtual void assemble(const std::list< std::pair< int, Transform > > &newPoses)
Definition: OccupancyGrid.cpp:174
value
value
i
int i

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