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 = scanHitIn;
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;
334  yMin = minY-margin;
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), 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), 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 
595 cv::Mat create2DMap(const std::map<int, Transform> & poses,
596  const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >
597  const std::map<int, cv::Point3f > & viewpoints,
598  float cellSize,
599  bool unknownSpaceFilled,
600  float & xMin,
601  float & yMin,
602  float minMapSize,
603  float scanMaxRange)
604 {
605  UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
606 
607  // local scans contain end points of each ray in map frame (pose+localTransform)
608  std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
609 
610  pcl::PointCloud<pcl::PointXYZ> minMax;
611  if(minMapSize > 0.0f)
612  {
613  minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
614  minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
615  }
616  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
617  {
618  std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
619  if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
620  {
621  UASSERT(!iter->second.isNull());
622  cv::Mat hit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.first), iter->second).data();
623  cv::Mat noHit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.second), iter->second).data();
624  pcl::PointXYZ min, max;
625  if(!hit.empty())
626  {
627  util3d::getMinMax3D(hit, min, max);
628  minMax.push_back(min);
629  minMax.push_back(max);
630  }
631  if(!noHit.empty())
632  {
633  util3d::getMinMax3D(noHit, min, max);
634  minMax.push_back(min);
635  minMax.push_back(max);
636  }
637  minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
638 
639  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
640  if(kter!=viewpoints.end())
641  {
642  minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
643  }
644 
645  localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit)));
646  }
647  }
648 
649  cv::Mat map;
650  if(minMax.size())
651  {
652  //Get map size
653  pcl::PointXYZ min, max;
654  pcl::getMinMax3D(minMax, min, max);
655 
656  // Added margin to make sure that all points are inside the map (when rounded to integer)
657  float margin = cellSize*10.0f;
658  xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
659  yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
660  float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
661  float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
662 
663  UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
664  xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
665 
666  UTimer timer;
667 
668  map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
669  UDEBUG("map size = %dx%d", map.cols, map.rows);
670 
671  int j=0;
672  float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
673  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
674  {
675  const Transform & pose = poses.at(iter->first);
676  cv::Point3f viewpoint(0,0,0);
677  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
678  if(kter!=viewpoints.end())
679  {
680  viewpoint = kter->second;
681  }
682  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
683 
684  // Set obstacles first
685  for(int i=0; i<iter->second.first.cols; ++i)
686  {
687  const float * ptr = iter->second.first.ptr<float>(0, i);
688  bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
689  if(!ignore)
690  {
691  cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
692  if(end!=start)
693  {
694  map.at<char>(end.y, end.x) = 100; // obstacle
695  }
696  }
697  }
698 
699  // ray tracing for hits
700  for(int i=0; i<iter->second.first.cols; ++i)
701  {
702  const float * ptr = iter->second.first.ptr<float>(0, i);
703 
704  cv::Vec2f pt(ptr[0], ptr[1]);
705  if(scanMaxRange>cellSize)
706  {
707  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
708  float n = cv::norm(v);
709  if(n > scanMaxRange+cellSize)
710  {
711  v = (v/n) * scanMaxRange;
712  pt[0] = pose.x()+viewpoint.x + v[0];
713  pt[1] = pose.y()+viewpoint.y + v[1];
714  }
715  }
716 
717  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
718  if(end!=start)
719  {
720  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
721  {
722  rayTrace(start, end, map, true); // trace free space
723  }
724  }
725  }
726  // ray tracing for no hits
727  for(int i=0; i<iter->second.second.cols; ++i)
728  {
729  const float * ptr = iter->second.second.ptr<float>(0, i);
730 
731  cv::Vec2f pt(ptr[0], ptr[1]);
732  if(scanMaxRange>cellSize)
733  {
734  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
735  float n = cv::norm(v);
736  if(n > scanMaxRange+cellSize)
737  {
738  v = (v/n) * scanMaxRange;
739  pt[0] = pose.x()+viewpoint.x + v[0];
740  pt[1] = pose.y()+viewpoint.y + v[1];
741  }
742  }
743 
744  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
745  if(end!=start)
746  {
747  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
748  {
749  rayTrace(start, end, map, true); // trace free space
750  if(map.at<char>(end.y, end.x) == -1)
751  {
752  map.at<char>(end.y, end.x) = 0; // empty
753  }
754  }
755  }
756  }
757  ++j;
758  }
759  UDEBUG("Ray trace known space=%fs", timer.ticks());
760 
761  // now fill unknown spaces
762  if(unknownSpaceFilled && scanMaxRange > 0)
763  {
764  j=0;
765  float a = CV_PI/256.0f; // angle increment
766  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
767  {
768  if(iter->second.first.cols > 1)
769  {
770  if(scanMaxRange > cellSize)
771  {
772  const Transform & pose = poses.at(iter->first);
773  cv::Point3f viewpoint(0,0,0);
774  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
775  if(kter!=viewpoints.end())
776  {
777  viewpoint = kter->second;
778  }
779  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
780 
781  //UWARN("maxLength = %f", maxLength);
782  //rotate counterclockwise from the first point until we pass the last point
783  // Note: assuming that first laser scan is negative y
784  cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
785  sin(a), cos(a));
786  cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
787  origin.at<float>(0) = pose.x()+viewpoint.x;
788  origin.at<float>(1) = pose.y()+viewpoint.y;
789  endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
790  endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
791  endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
792  endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
793  //UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
794  //UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
795  //UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
796  cv::Mat tmp = (endFirst - origin);
797  cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
798  cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
799  endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
800  endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
801  endLastVector.at<float>(2) = 0.0f;
802  endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
803  endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
804  endRotatedVector.at<float>(2) = 0.0f;
805  //UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
806  float normEndRotatedVector = cv::norm(endRotatedVector);
807  endLastVector = endLastVector / cv::norm(endLastVector);
808  float angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
809  angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
810  while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
811  {
812  cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
813  //end must be inside the grid
814  end.x = end.x < 0?0:end.x;
815  end.x = end.x >= map.cols?map.cols-1:end.x;
816  end.y = end.y < 0?0:end.y;
817  end.y = end.y >= map.rows?map.rows-1:end.y;
818  rayTrace(start, end, map, true); // trace free space
819  // next point
820  endRotated = rotation*(endRotated - origin) + origin;
821  endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
822  endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
823  angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
824  angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
825 
826  //UWARN("endRotated = %f %f (%f %f %f)",
827  // endRotated.at<float>(0), endRotated.at<float>(1),
828  // acos(angle),
829  // angle,
830  // endRotatedVector.cross(endLastVector).at<float>(2));
831  }
832  }
833  }
834  ++j;
835  }
836  UDEBUG("Fill empty space=%fs", timer.ticks());
837  //cv::imwrite("map.png", util3d::convertMap2Image8U(map));
838  //UWARN("saved map.png");
839  }
840  }
841  return map;
842 }
843 
844 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
845 {
846  UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
847  UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
848  UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
849  UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
850 
851  cv::Point2i ptA, ptB;
852  ptA = start;
853  ptB = end;
854 
855  float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
856 
857  bool swapped = false;
858  if(slope<-1.0f || slope>1.0f)
859  {
860  // swap x and y
861  slope = 1.0f/slope;
862 
863  int tmp = ptA.x;
864  ptA.x = ptA.y;
865  ptA.y = tmp;
866 
867  tmp = ptB.x;
868  ptB.x = ptB.y;
869  ptB.y = tmp;
870 
871  swapped = true;
872  }
873 
874  float b = ptA.y - slope*ptA.x;
875  for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
876  {
877  int upperbound = float(x)*slope + b;
878  int lowerbound = upperbound;
879  if(x != ptA.x)
880  {
881  lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
882  }
883 
884  if(lowerbound > upperbound)
885  {
886  int tmp = upperbound;
887  upperbound = lowerbound;
888  lowerbound = tmp;
889  }
890 
891  if(!swapped)
892  {
893  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());
894  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());
895  }
896  else
897  {
898  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());
899  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());
900  }
901 
902  for(int y = lowerbound; y<=(int)upperbound; ++y)
903  {
904  char * v;
905  if(swapped)
906  {
907  v = &grid.at<char>(x, y);
908  }
909  else
910  {
911  v = &grid.at<char>(y, x);
912  }
913  if(*v == 100 && stopOnObstacle)
914  {
915  return;
916  }
917  else
918  {
919  *v = 0; // free space
920  }
921  }
922  }
923 }
924 
925 //convert to gray scaled map
926 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
927 {
928  UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
929  cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
930  for (int i = 0; i < map8S.rows; ++i)
931  {
932  for (int j = 0; j < map8S.cols; ++j)
933  {
934  char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
935  unsigned char gray;
936  if(v == 0)
937  {
938  gray = pgmFormat?254:178;
939  }
940  else if(v == 100)
941  {
942  gray = 0;
943  }
944  else if(v == -2)
945  {
946  gray = pgmFormat?254:200;
947  }
948  else // -1
949  {
950  gray = pgmFormat?205:89;
951  }
952  map8U.at<unsigned char>(i, j) = gray;
953  }
954  }
955  return map8U;
956 }
957 
958 //convert gray scaled image to map
959 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
960 {
961  UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
962  cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
963  for (int i = 0; i < map8U.rows; ++i)
964  {
965  for (int j = 0; j < map8U.cols; ++j)
966  {
967  unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
968  char occupancy;
969  if(pgmFormat)
970  {
971  if(v >= 254)
972  {
973  occupancy = 0;
974  }
975  else if(v == 0)
976  {
977  occupancy = 100;
978  }
979  else // 205
980  {
981  occupancy = -1;
982  }
983  }
984  else
985  {
986  if(v == 178)
987  {
988  occupancy = 0;
989  }
990  else if(v == 0)
991  {
992  occupancy = 100;
993  }
994  else if(v == 200)
995  {
996  occupancy = -2;
997  }
998  else // 89
999  {
1000  occupancy = -1;
1001  }
1002  }
1003 
1004  map8S.at<char>(i, j) = occupancy;
1005  }
1006  }
1007  return map8S;
1008 }
1009 
1010 cv::Mat erodeMap(const cv::Mat & map)
1011 {
1012  UASSERT(map.type() == CV_8SC1);
1013  cv::Mat erodedMap = map.clone();
1014  for(int i=0; i<map.rows; ++i)
1015  {
1016  for(int j=0; j<map.cols; ++j)
1017  {
1018  if(map.at<char>(i, j) == 100)
1019  {
1020  // remove obstacles which touch at least 3 empty cells but not unknown cells
1021  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
1022  (map.at<char>(i-1, j) == 0?1:0) +
1023  (map.at<char>(i, j+1) == 0?1:0) +
1024  (map.at<char>(i, j-1) == 0?1:0);
1025 
1026  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
1027  map.at<char>(i-1, j) != -1 &&
1028  map.at<char>(i, j+1) != -1 &&
1029  map.at<char>(i, j-1) != -1)
1030  {
1031  erodedMap.at<char>(i, j) = 0; // empty
1032  }
1033  }
1034  }
1035  }
1036  return erodedMap;
1037 }
1038 
1039 }
1040 
1041 }
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:1010
UTimer
Definition: UTimer.h:46
rtabmap::util3d::create2DMapFromOccupancyLocalMaps
cv::Mat RTABMAP_EXP 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
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2666
f
f
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::util3d::create2DMap
cv::Mat RTABMAP_EXP create2DMap(const std::map< int, Transform > &poses, const std::map< int, std::pair< cv::Mat, cv::Mat > > &scans, const std::map< int, cv::Point3f > &viewpoints, float cellSize, bool unknownSpaceFilled, float &xMin, float &yMin, float minMapSize=0.0f, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:595
rtabmap::util3d::occupancy2DFromLaserScan
void RTABMAP_EXP occupancy2DFromLaserScan(const cv::Mat &scanHit, const cv::Mat &scanNoHit, const cv::Point3f &viewpoint, cv::Mat &empty, cv::Mat &occupied, float cellSize, bool unknownSpaceFilled=false, float scanMaxRange=0.0f)
Definition: util3d_mapping.cpp:82
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
UConversion.h
Some conversion functions.
UFATAL
#define UFATAL(...)
util3d_mapping.h
UASSERT
#define UASSERT(condition)
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
UStl.h
Wrappers of STL for convenient functions.
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_EXP transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
util3d_filtering.h
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:959
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:926
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
glm::dot
GLM_FUNC_DECL T dot(vecType< T, P > const &x, vecType< T, P > const &y)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
glm::acos
GLM_FUNC_DECL genType acos(genType const &x)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:317
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::util3d::rayTrace
void RTABMAP_EXP rayTrace(const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle)
Definition: util3d_mapping.cpp:844
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:125
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())
Definition: util3d.cpp:1266

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06