util3d_mapping.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/util3d_mapping.h"
29 
30 #include <rtabmap/core/util3d_transforms.h>
31 #include <rtabmap/core/util3d_filtering.h>
32 #include <rtabmap/core/util3d.h>
33 
34 #include <rtabmap/utilite/ULogger.h>
35 #include <rtabmap/utilite/UTimer.h>
36 #include <rtabmap/utilite/UStl.h>
37 #include <rtabmap/utilite/UConversion.h>
38 #include <rtabmap/utilite/UMath.h>
39 
40 #include <pcl/common/common.h>
41 #include <pcl/common/centroid.h>
42 #include <pcl/common/io.h>
43 
44 namespace rtabmap
45 {
46 
47 namespace util3d
48 {
49 
50 void occupancy2DFromLaserScan(
51  const cv::Mat & scan,
52  cv::Mat & empty,
53  cv::Mat & occupied,
54  float cellSize,
55  bool unknownSpaceFilled,
56  float scanMaxRange)
57 {
58  cv::Point3f viewpoint(0,0,0);
59  occupancy2DFromLaserScan(
60  scan,
61  cv::Mat(),
62  viewpoint,
63  empty,
64  occupied,
65  cellSize,
66  unknownSpaceFilled,
67  scanMaxRange);
68 }
69 
70 void occupancy2DFromLaserScan(
71  const cv::Mat & scan,
72  const cv::Point3f & viewpoint,
73  cv::Mat & empty,
74  cv::Mat & occupied,
75  float cellSize,
76  bool unknownSpaceFilled,
77  float scanMaxRange)
78 {
79  occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, empty, occupied, cellSize, unknownSpaceFilled, scanMaxRange);
80 }
81 
82 void occupancy2DFromLaserScan(
83  const cv::Mat & scanHitIn,
84  const cv::Mat & scanNoHitIn,
85  const cv::Point3f & viewpoint,
86  cv::Mat & empty,
87  cv::Mat & occupied,
88  float cellSize,
89  bool unknownSpaceFilled,
90  float scanMaxRange)
91 {
92  if(scanHitIn.empty() && scanNoHitIn.empty())
93  {
94  return;
95  }
96  cv::Mat scanHit;
97  cv::Mat scanNoHit;
98  // keep only XY channels
99  if(scanHitIn.channels()>2)
100  {
101  std::vector<cv::Mat> channels;
102  cv::split(scanHitIn,channels);
103  while(channels.size()>2)
104  {
105  channels.pop_back();
106  }
107  cv::merge(channels,scanHit);
108  }
109  else
110  {
111  scanHit = scanHitIn.clone(); // will be returned in occupied matrix
112  }
113  if(scanNoHitIn.channels()>2)
114  {
115  std::vector<cv::Mat> channels;
116  cv::split(scanNoHitIn,channels);
117  while(channels.size()>2)
118  {
119  channels.pop_back();
120  }
121  cv::merge(channels,scanNoHit);
122  }
123  else
124  {
125  scanNoHit = scanNoHitIn;
126  }
127 
128  std::map<int, Transform> poses;
129  poses.insert(std::make_pair(1, Transform::getIdentity()));
130 
131  std::map<int, std::pair<cv::Mat, cv::Mat> > scans;
132  scans.insert(std::make_pair(1, std::make_pair(scanHit, scanNoHit)));
133 
134  std::map<int, cv::Point3f> viewpoints;
135  viewpoints.insert(std::make_pair(1, viewpoint));
136 
137  float xMin, yMin;
138  cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
139 
140  // find empty cells
141  std::list<int> emptyIndices;
142  for(unsigned int i=0; i< map8S.total(); ++i)
143  {
144  if(map8S.data[i] == 0)
145  {
146  emptyIndices.push_back(i);
147  }
148  }
149 
150  // Convert to position matrices, get points to each center of the cells
151  empty = cv::Mat();
152  if(emptyIndices.size())
153  {
154  empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
155  int i=0;
156  for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
157  {
158  int y = *iter / map8S.cols;
159  int x = *iter - y*map8S.cols;
160  cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
161  ptr[i][0] = (float(x))*cellSize + xMin;
162  ptr[i][1] = (float(y))*cellSize + yMin;
163  ++i;
164  }
165  }
166 
167  // copy directly obstacles precise positions
168  if(scanMaxRange > cellSize)
169  {
170  occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data();
171  }
172  else
173  {
174  occupied = scanHit;
175  }
176 }
177 
191 cv::Mat create2DMapFromOccupancyLocalMaps(
192  const std::map<int, Transform> & posesIn,
193  const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
194  float cellSize,
195  float & xMin,
196  float & yMin,
197  float minMapSize,
198  bool erode,
199  float footprintRadius)
200 {
201  UASSERT(minMapSize >= 0.0f);
202  UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
203  UTimer timer;
204 
205  std::map<int, cv::Mat> emptyLocalMaps;
206  std::map<int, cv::Mat> occupiedLocalMaps;
207 
208  std::list<std::pair<int, Transform> > poses;
209  // place negative poses at the end
210  for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
211  {
212  if(iter->first>0)
213  {
214  poses.push_front(*iter);
215  }
216  else
217  {
218  poses.push_back(*iter);
219  }
220  }
221 
222  float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
223  bool undefinedSize = minMapSize == 0.0f;
224  for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
225  {
226  UASSERT(!iter->second.isNull());
227 
228  float x = iter->second.x();
229  float y =iter->second.y();
230  if(undefinedSize)
231  {
232  minX = maxX = x;
233  minY = maxY = y;
234  undefinedSize = false;
235  }
236  else
237  {
238  if(minX > x)
239  minX = x;
240  else if(maxX < x)
241  maxX = x;
242 
243  if(minY > y)
244  minY = y;
245  else if(maxY < y)
246  maxY = y;
247  }
248 
249  if(uContains(occupancy, iter->first))
250  {
251  const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
252 
253  //ground
254  if(pair.first.cols)
255  {
256  if(pair.first.rows > 1 && pair.first.cols == 1)
257  {
258  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
259  }
260  cv::Mat ground(1, pair.first.cols, CV_32FC2);
261  for(int i=0; i<ground.cols; ++i)
262  {
263  const float * vi = pair.first.ptr<float>(0,i);
264  float * vo = ground.ptr<float>(0,i);
265  cv::Point3f vt;
266  if(pair.first.channels() != 2 && pair.first.channels() != 5)
267  {
268  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
269  }
270  else
271  {
272  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
273  }
274  vo[0] = vt.x;
275  vo[1] = vt.y;
276  if(minX > vo[0])
277  minX = vo[0];
278  else if(maxX < vo[0])
279  maxX = vo[0];
280 
281  if(minY > vo[1])
282  minY = vo[1];
283  else if(maxY < vo[1])
284  maxY = vo[1];
285  }
286  emptyLocalMaps.insert(std::make_pair(iter->first, ground));
287  }
288 
289  //obstacles
290  if(pair.second.cols)
291  {
292  if(pair.second.rows > 1 && pair.second.cols == 1)
293  {
294  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
295  }
296  cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
297  for(int i=0; i<obstacles.cols; ++i)
298  {
299  const float * vi = pair.second.ptr<float>(0,i);
300  float * vo = obstacles.ptr<float>(0,i);
301  cv::Point3f vt;
302  if(pair.second.channels() != 2 && pair.second.channels() != 5)
303  {
304  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
305  }
306  else
307  {
308  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
309  }
310  vo[0] = vt.x;
311  vo[1] = vt.y;
312  if(minX > vo[0])
313  minX = vo[0];
314  else if(maxX < vo[0])
315  maxX = vo[0];
316 
317  if(minY > vo[1])
318  minY = vo[1];
319  else if(maxY < vo[1])
320  maxY = vo[1];
321  }
322  occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
323  }
324  }
325  }
326  UDEBUG("timer=%fs", timer.ticks());
327 
328  cv::Mat map;
329  if(minX != maxX && minY != maxY)
330  {
331  //Get map size
332  float margin = cellSize*10.0f;
333  xMin = minX-margin-cellSize/2.0f;
334  yMin = minY-margin-cellSize/2.0f;
335  float xMax = maxX+margin;
336  float yMax = maxY+margin;
337  if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB
338  fabs((xMax - xMin) / cellSize) > 30000)
339  {
340  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
341  "There's maybe an error with the poses provided! The map will not be created!",
342  xMin, yMin, xMax, yMax);
343  }
344  else
345  {
346  UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
347 
348 
349  map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
350  for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
351  {
352  std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
353  std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
354  if(iter!=emptyLocalMaps.end())
355  {
356  for(int i=0; i<iter->second.cols; ++i)
357  {
358  float * ptf = iter->second.ptr<float>(0, i);
359  cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
360  UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
361  uFormat("id=%d, map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
362  char & value = map.at<char>(pt.y, pt.x);
363  if(value != -2)
364  {
365  value = 0; // free space
366  }
367  }
368  }
369 
370  if(footprintRadius >= cellSize*1.5f)
371  {
372  // place free space under the footprint of the robot
373  cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
374  cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize, (kter->second.y()+footprintRadius-yMin)/cellSize);
375  if(ptBegin.x < 0)
376  ptBegin.x = 0;
377  if(ptEnd.x >= map.cols)
378  ptEnd.x = map.cols-1;
379 
380  if(ptBegin.y < 0)
381  ptBegin.y = 0;
382  if(ptEnd.y >= map.rows)
383  ptEnd.y = map.rows-1;
384  for(int i=ptBegin.x; i<ptEnd.x; ++i)
385  {
386  for(int j=ptBegin.y; j<ptEnd.y; ++j)
387  {
388  map.at<char>(j, i) = -2; // free space (footprint)
389  }
390  }
391  }
392 
393  if(jter!=occupiedLocalMaps.end())
394  {
395  for(int i=0; i<jter->second.cols; ++i)
396  {
397  float * ptf = jter->second.ptr<float>(0, i);
398  cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
399  UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
400  uFormat("id=%d: map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
401  char & value = map.at<char>(pt.y, pt.x);
402  if(value != -2)
403  {
404  value = 100; // obstacles
405  }
406  }
407  }
408 
409  //UDEBUG("empty=%d occupied=%d", empty, occupied);
410  }
411 
412  // fill holes and remove empty from obstacle borders
413  cv::Mat updatedMap = map.clone();
414  std::list<std::pair<int, int> > obstacleIndices;
415  for(int i=0; i<map.rows; ++i)
416  {
417  for(int j=0; j<map.cols; ++j)
418  {
419  if(map.at<char>(i, j) == -2)
420  {
421  updatedMap.at<char>(i, j) = 0;
422  }
423 
424  if(i >=2 && i<map.rows-2 && j>=2 && j<map.cols-2)
425  {
426  if(map.at<char>(i, j) == -1 &&
427  map.at<char>(i+1, j) != -1 &&
428  map.at<char>(i-1, j) != -1 &&
429  map.at<char>(i, j+1) != -1 &&
430  map.at<char>(i, j-1) != -1)
431  {
432  updatedMap.at<char>(i, j) = 0;
433  }
434  else if(map.at<char>(i, j) == 100)
435  {
436  // obstacle/empty/unknown -> remove empty
437  // unknown/empty/obstacle -> remove empty
438  if((map.at<char>(i-1, j) == 0 || map.at<char>(i-1, j) == -2) &&
439  map.at<char>(i-2, j) == -1)
440  {
441  updatedMap.at<char>(i-1, j) = -1;
442  }
443  else if((map.at<char>(i+1, j) == 0 || map.at<char>(i+1, j) == -2) &&
444  map.at<char>(i+2, j) == -1)
445  {
446  updatedMap.at<char>(i+1, j) = -1;
447  }
448  if((map.at<char>(i, j-1) == 0 || map.at<char>(i, j-1) == -2) &&
449  map.at<char>(i, j-2) == -1)
450  {
451  updatedMap.at<char>(i, j-1) = -1;
452  }
453  else if((map.at<char>(i, j+1) == 0 || map.at<char>(i, j+1) == -2) &&
454  map.at<char>(i, j+2) == -1)
455  {
456  updatedMap.at<char>(i, j+1) = -1;
457  }
458 
459  if(erode)
460  {
461  obstacleIndices.push_back(std::make_pair(i, j));
462  }
463  }
464  else if(map.at<char>(i, j) == 0)
465  {
466  // obstacle/empty/obstacle -> remove empty
467  if(map.at<char>(i-1, j) == 100 &&
468  map.at<char>(i+1, j) == 100)
469  {
470  updatedMap.at<char>(i, j) = -1;
471  }
472  else if(map.at<char>(i, j-1) == 100 &&
473  map.at<char>(i, j+1) == 100)
474  {
475  updatedMap.at<char>(i, j) = -1;
476  }
477  }
478  }
479  }
480  }
481  map = updatedMap;
482 
483  if(erode)
484  {
485  // remove obstacles which touch at least 3 empty cells but not unknown cells
486  cv::Mat erodedMap = map.clone();
487  for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
488  iter!= obstacleIndices.end();
489  ++iter)
490  {
491  int i = iter->first;
492  int j = iter->second;
493  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
494  (map.at<char>(i-1, j) == 0?1:0) +
495  (map.at<char>(i, j+1) == 0?1:0) +
496  (map.at<char>(i, j-1) == 0?1:0);
497  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
498  map.at<char>(i-1, j) != -1 &&
499  map.at<char>(i, j+1) != -1 &&
500  map.at<char>(i, j-1) != -1)
501  {
502  erodedMap.at<char>(i, j) = 0; // empty
503  }
504  }
505  map = erodedMap;
506  }
507  }
508  }
509 
510  UDEBUG("timer=%fs", timer.ticks());
511  return map;
512 }
513 
528 cv::Mat create2DMap(const std::map<int, Transform> & poses,
529  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
530  float cellSize,
531  bool unknownSpaceFilled,
532  float & xMin,
533  float & yMin,
534  float minMapSize,
535  float scanMaxRange)
536 {
537  std::map<int, cv::Point3f > viewpoints;
538  std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
539  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
540  {
541  scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second).data(), cv::Mat())));
542  }
543  return create2DMap(poses,
544  scansCv,
545  viewpoints,
546  cellSize,
547  unknownSpaceFilled,
548  xMin,
549  yMin,
550  minMapSize,
551  scanMaxRange);
552 }
553 
554 cv::Mat create2DMap(const std::map<int, Transform> & poses,
555  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
556  const std::map<int, cv::Point3f > & viewpoints,
557  float cellSize,
558  bool unknownSpaceFilled,
559  float & xMin,
560  float & yMin,
561  float minMapSize,
562  float scanMaxRange)
563 {
564  std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
565  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
566  {
567  scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second).data(), cv::Mat())));
568  }
569  return create2DMap(poses,
570  scansCv,
571  viewpoints,
572  cellSize,
573  unknownSpaceFilled,
574  xMin,
575  yMin,
576  minMapSize,
577  scanMaxRange);
578 }
579 
580 cv::Mat create2DMap(const std::map<int, Transform> & poses,
581  const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >
582  const std::map<int, cv::Point3f > & viewpoints,
583  float cellSize,
584  bool unknownSpaceFilled,
585  float & xMin,
586  float & yMin,
587  float minMapSize,
588  float scanMaxRange)
589 {
590  UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
591 
592  // local scans contain end points of each ray in map frame (pose+localTransform)
593  std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
594 
595  pcl::PointCloud<pcl::PointXYZ> minMax;
596  if(minMapSize > 0.0f)
597  {
598  minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
599  minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
600  }
601  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
602  {
603  std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
604  UASSERT_MSG(jter->second.first.empty() || jter->second.first.type() == CV_32FC2, "Input scans should be 2D to avoid any confusion.");
605  UASSERT_MSG(jter->second.second.empty() || jter->second.second.type() == CV_32FC2, "Input scans should be 2D to avoid any confusion.");
606  if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
607  {
608  UASSERT(!iter->second.isNull());
609  cv::Mat hit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.first), iter->second).data();
610  cv::Mat noHit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.second), iter->second).data();
611  pcl::PointXYZ min, max;
612  if(!hit.empty())
613  {
614  util3d::getMinMax3D(hit, min, max);
615  minMax.push_back(min);
616  minMax.push_back(max);
617  }
618  if(!noHit.empty())
619  {
620  util3d::getMinMax3D(noHit, min, max);
621  minMax.push_back(min);
622  minMax.push_back(max);
623  }
624  minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
625 
626  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
627  if(kter!=viewpoints.end())
628  {
629  minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
630  }
631 
632  localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit)));
633  }
634  }
635 
636  cv::Mat map;
637  if(minMax.size())
638  {
639  //Get map size
640  pcl::PointXYZ min, max;
641  pcl::getMinMax3D(minMax, min, max);
642 
643  // Added margin to make sure that all points are inside the map (when rounded to integer)
644  float margin = cellSize*10.0f;
645  xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
646  yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
647  float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
648  float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
649 
650  UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
651  xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
652 
653  UTimer timer;
654 
655  map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
656  UDEBUG("map size = %dx%d", map.cols, map.rows);
657 
658  float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
659  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
660  {
661  const Transform & pose = poses.at(iter->first);
662  cv::Point3f viewpoint(0,0,0);
663  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
664  if(kter!=viewpoints.end())
665  {
666  viewpoint = kter->second;
667  }
668  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
669 
670  // Set obstacles first
671  for(int i=0; i<iter->second.first.cols; ++i)
672  {
673  const float * ptr = iter->second.first.ptr<float>(0, i);
674  bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
675  if(!ignore)
676  {
677  cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
678  if(end!=start)
679  {
680  map.at<char>(end.y, end.x) = 100; // obstacle
681  }
682  }
683  }
684 
685  // ray tracing for hits
686  for(int i=0; i<iter->second.first.cols; ++i)
687  {
688  const float * ptr = iter->second.first.ptr<float>(0, i);
689 
690  cv::Vec2f pt(ptr[0], ptr[1]);
691  if(scanMaxRange>cellSize)
692  {
693  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
694  float n = cv::norm(v);
695  if(n > scanMaxRange+cellSize)
696  {
697  v = (v/n) * scanMaxRange;
698  pt[0] = pose.x()+viewpoint.x + v[0];
699  pt[1] = pose.y()+viewpoint.y + v[1];
700  }
701  }
702 
703  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
704  if(end!=start)
705  {
706  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
707  {
708  rayTrace(start, end, map, true); // trace free space
709  }
710  }
711  }
712  // ray tracing for no hits
713  for(int i=0; i<iter->second.second.cols; ++i)
714  {
715  const float * ptr = iter->second.second.ptr<float>(0, i);
716 
717  cv::Vec2f pt(ptr[0], ptr[1]);
718  if(scanMaxRange>cellSize)
719  {
720  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
721  float n = cv::norm(v);
722  if(n > scanMaxRange+cellSize)
723  {
724  v = (v/n) * scanMaxRange;
725  pt[0] = pose.x()+viewpoint.x + v[0];
726  pt[1] = pose.y()+viewpoint.y + v[1];
727  }
728  }
729 
730  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
731  if(end!=start)
732  {
733  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
734  {
735  rayTrace(start, end, map, true); // trace free space
736  if(map.at<char>(end.y, end.x) == -1)
737  {
738  map.at<char>(end.y, end.x) = 0; // empty
739  }
740  }
741  }
742  }
743  }
744  UDEBUG("Ray trace known space=%fs", timer.ticks());
745 
746  // now fill unknown spaces
747  if(unknownSpaceFilled && scanMaxRange > 0)
748  {
749  float angleIncrement = CV_PI/90.0f; // angle increment
750  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
751  {
752  if(iter->second.first.cols > 2)
753  {
754  if(scanMaxRange > cellSize)
755  {
756  const Transform & pose = poses.at(iter->first);
757  cv::Point3f viewpoint(0,0,0);
758  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
759  if(kter!=viewpoints.end())
760  {
761  viewpoint = kter->second;
762  }
763  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
764 
765  // As we don't know the angle_min or angle_max, ray trace between
766  // the first and last obstacle (counterclockwise).
767  cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(angleIncrement), -sin(angleIncrement),
768  sin(angleIncrement), cos(angleIncrement));
769  cv::Mat origin(2,1,CV_32F), obsFirst(2,1,CV_32F), obsLast(2,1,CV_32F);
770  origin.at<float>(0) = pose.x()+viewpoint.x;
771  origin.at<float>(1) = pose.y()+viewpoint.y;
772  obsFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
773  obsFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
774  obsLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[0];
775  obsLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[1];
776  cv::Mat firstVector(3,1,CV_32F), lastVector(3,1,CV_32F);
777  firstVector.at<float>(0) = obsFirst.at<float>(0) - origin.at<float>(0);
778  firstVector.at<float>(1) = obsFirst.at<float>(1) - origin.at<float>(1);
779  firstVector.at<float>(2) = 0.0f;
780  firstVector = firstVector/cv::norm(firstVector);
781  lastVector.at<float>(0) = obsLast.at<float>(0) - origin.at<float>(0);
782  lastVector.at<float>(1) = obsLast.at<float>(1) - origin.at<float>(1);
783  lastVector.at<float>(2) = 0.0f;
784  lastVector = lastVector / cv::norm(lastVector);
785  float maxAngle = acos(firstVector.dot(lastVector));
786  if(firstVector.cross(lastVector).at<float>(2) < 0)
787  {
788  maxAngle = 2*M_PI-maxAngle;
789  }
790  //UWARN("angle=%f v1=[%f %f 0];v2=[%f %f 0];",
791  // maxAngle,
792  // firstVector.at<float>(0), firstVector.at<float>(1),
793  // lastVector.at<float>(0), lastVector.at<float>(1));
794  float angle = angleIncrement;
795  cv::Mat tmp = (obsFirst - origin);
796  cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
797  while(angle < maxAngle-angleIncrement)
798  {
799  cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
800  //end must be inside the grid
801  end.x = end.x < 0?0:end.x;
802  end.x = end.x >= map.cols?map.cols-1:end.x;
803  end.y = end.y < 0?0:end.y;
804  end.y = end.y >= map.rows?map.rows-1:end.y;
805  rayTrace(start, end, map, true); // trace free space
806  // next point
807  endRotated = rotation*(endRotated - origin) + origin;
808 
809  angle+=angleIncrement;
810  }
811  }
812  }
813  }
814  UDEBUG("Fill empty space=%fs", timer.ticks());
815  //cv::imwrite("map.png", util3d::convertMap2Image8U(map));
816  //UWARN("saved map.png");
817  }
818  }
819  return map;
820 }
821 
822 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
823 {
824  UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
825  UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
826  UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
827  UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
828 
829  cv::Point2i ptA, ptB;
830  ptA = start;
831  ptB = end;
832 
833  float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
834 
835  bool swapped = false;
836  if(slope<-1.0f || slope>1.0f)
837  {
838  // swap x and y
839  slope = 1.0f/slope;
840 
841  int tmp = ptA.x;
842  ptA.x = ptA.y;
843  ptA.y = tmp;
844 
845  tmp = ptB.x;
846  ptB.x = ptB.y;
847  ptB.y = tmp;
848 
849  swapped = true;
850  }
851 
852  float b = ptA.y - slope*ptA.x;
853  for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
854  {
855  int upperbound = float(x)*slope + b;
856  int lowerbound = upperbound;
857  if(x != ptA.x)
858  {
859  lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
860  }
861 
862  if(lowerbound > upperbound)
863  {
864  int tmp = upperbound;
865  upperbound = lowerbound;
866  lowerbound = tmp;
867  }
868 
869  if(!swapped)
870  {
871  UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
872  UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
873  }
874  else
875  {
876  UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
877  UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
878  }
879 
880  for(int y = lowerbound; y<=(int)upperbound; ++y)
881  {
882  char * v;
883  if(swapped)
884  {
885  v = &grid.at<char>(x, y);
886  }
887  else
888  {
889  v = &grid.at<char>(y, x);
890  }
891  if(*v == 100 && stopOnObstacle)
892  {
893  return;
894  }
895  else
896  {
897  *v = 0; // free space
898  }
899  }
900  }
901 }
902 
903 //convert to gray scaled map
904 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
905 {
906  UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
907  cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
908  for (int i = 0; i < map8S.rows; ++i)
909  {
910  for (int j = 0; j < map8S.cols; ++j)
911  {
912  char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
913  unsigned char gray;
914  if(v == 0)
915  {
916  gray = pgmFormat?254:178;
917  }
918  else if(v == 100)
919  {
920  gray = 0;
921  }
922  else if(v == -2)
923  {
924  gray = pgmFormat?254:200;
925  }
926  else if(pgmFormat || v == -1)// -1
927  {
928  gray = pgmFormat?205:89;
929  }
930  else if(v>50)
931  {
932  gray = double(100-v)*2/100.0*double(89);
933  }
934  else // v<50
935  {
936  gray = double(50-v)*2/100.0*double(178-89)+89;
937  }
938  map8U.at<unsigned char>(i, j) = gray;
939  }
940  }
941  return map8U;
942 }
943 
944 //convert gray scaled image to map
945 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
946 {
947  UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
948  cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
949  for (int i = 0; i < map8U.rows; ++i)
950  {
951  for (int j = 0; j < map8U.cols; ++j)
952  {
953  unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
954  char occupancy;
955  if(pgmFormat)
956  {
957  if(v >= 254)
958  {
959  occupancy = 0;
960  }
961  else if(v == 0)
962  {
963  occupancy = 100;
964  }
965  else // 205
966  {
967  occupancy = -1;
968  }
969  }
970  else
971  {
972  if(v == 178)
973  {
974  occupancy = 0;
975  }
976  else if(v == 0)
977  {
978  occupancy = 100;
979  }
980  else if(v == 200)
981  {
982  occupancy = -2;
983  }
984  else // 89
985  {
986  occupancy = -1;
987  }
988  }
989 
990  map8S.at<char>(i, j) = occupancy;
991  }
992  }
993  return map8S;
994 }
995 
996 cv::Mat erodeMap(const cv::Mat & map)
997 {
998  UASSERT(map.type() == CV_8SC1);
999  cv::Mat erodedMap = map.clone();
1000  for(int i=0; i<map.rows; ++i)
1001  {
1002  for(int j=0; j<map.cols; ++j)
1003  {
1004  if(map.at<char>(i, j) == 100)
1005  {
1006  // remove obstacles which touch at least 3 empty cells but not unknown cells
1007  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
1008  (map.at<char>(i-1, j) == 0?1:0) +
1009  (map.at<char>(i, j+1) == 0?1:0) +
1010  (map.at<char>(i, j-1) == 0?1:0);
1011 
1012  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
1013  map.at<char>(i-1, j) != -1 &&
1014  map.at<char>(i, j+1) != -1 &&
1015  map.at<char>(i, j-1) != -1)
1016  {
1017  erodedMap.at<char>(i, j) = 0; // empty
1018  }
1019  }
1020  }
1021  }
1022  return erodedMap;
1023 }
1024 
1025 }
1026 
1027 }
int
int
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
timer
b
Array< int, 3, 1 > b
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_CORE_EXPORT transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
end
end
y
Matrix3f y
iterator
util3d.h
rtabmap::util3d::create2DMap
RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map< int, Transform > &poses, const std::map< int, pcl::PointCloud< pcl::PointXYZ >::Ptr > &scans, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:528
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
UTimer.h
UMath.h
Basic mathematics functions.
glm::acos
GLM_FUNC_DECL genType acos(genType const &x)
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:996
fabs
Real fabs(const Real &a)
n
int n
rtabmap::util3d::rayTrace
void RTABMAP_CORE_EXPORT rayTrace(const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle)
Definition: util3d_mapping.cpp:822
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
UFATAL
#define UFATAL(...)
util3d_transforms.h
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:945
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
rtabmap::util3d::create2DMapFromOccupancyLocalMaps
cv::Mat RTABMAP_CORE_EXPORT create2DMapFromOccupancyLocalMaps(const std::map< int, Transform > &poses, const std::map< int, std::pair< cv::Mat, cv::Mat > > &occupancy, float cellSize, float &xMin, float &yMin, float minMapSize=0.0f, bool erode=false, float footprintRadius=0.0f)
Definition: util3d_mapping.cpp:191
util3d_filtering.h
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UASSERT
#define UASSERT(condition)
x
x
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
util3d_mapping.h
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
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::Transform
Definition: Transform.h:41
empty
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::util3d::occupancy2DFromLaserScan
RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(const cv::Mat &scan, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:50
iter
iterator iter(handle obj)
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
float
float
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_CORE_EXPORT rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:347
handle::ptr
PyObject *& ptr()
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:904
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
value
value
i
int i
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)

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