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  int j=0;
659  float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
660  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
661  {
662  const Transform & pose = poses.at(iter->first);
663  cv::Point3f viewpoint(0,0,0);
664  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
665  if(kter!=viewpoints.end())
666  {
667  viewpoint = kter->second;
668  }
669  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
670 
671  // Set obstacles first
672  for(int i=0; i<iter->second.first.cols; ++i)
673  {
674  const float * ptr = iter->second.first.ptr<float>(0, i);
675  bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
676  if(!ignore)
677  {
678  cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
679  if(end!=start)
680  {
681  map.at<char>(end.y, end.x) = 100; // obstacle
682  }
683  }
684  }
685 
686  // ray tracing for hits
687  for(int i=0; i<iter->second.first.cols; ++i)
688  {
689  const float * ptr = iter->second.first.ptr<float>(0, i);
690 
691  cv::Vec2f pt(ptr[0], ptr[1]);
692  if(scanMaxRange>cellSize)
693  {
694  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
695  float n = cv::norm(v);
696  if(n > scanMaxRange+cellSize)
697  {
698  v = (v/n) * scanMaxRange;
699  pt[0] = pose.x()+viewpoint.x + v[0];
700  pt[1] = pose.y()+viewpoint.y + v[1];
701  }
702  }
703 
704  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
705  if(end!=start)
706  {
707  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
708  {
709  rayTrace(start, end, map, true); // trace free space
710  }
711  }
712  }
713  // ray tracing for no hits
714  for(int i=0; i<iter->second.second.cols; ++i)
715  {
716  const float * ptr = iter->second.second.ptr<float>(0, i);
717 
718  cv::Vec2f pt(ptr[0], ptr[1]);
719  if(scanMaxRange>cellSize)
720  {
721  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
722  float n = cv::norm(v);
723  if(n > scanMaxRange+cellSize)
724  {
725  v = (v/n) * scanMaxRange;
726  pt[0] = pose.x()+viewpoint.x + v[0];
727  pt[1] = pose.y()+viewpoint.y + v[1];
728  }
729  }
730 
731  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
732  if(end!=start)
733  {
734  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
735  {
736  rayTrace(start, end, map, true); // trace free space
737  if(map.at<char>(end.y, end.x) == -1)
738  {
739  map.at<char>(end.y, end.x) = 0; // empty
740  }
741  }
742  }
743  }
744  ++j;
745  }
746  UDEBUG("Ray trace known space=%fs", timer.ticks());
747 
748  // now fill unknown spaces
749  if(unknownSpaceFilled && scanMaxRange > 0)
750  {
751  j=0;
752  float angleIncrement = CV_PI/90.0f; // angle increment
753  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
754  {
755  if(iter->second.first.cols > 2)
756  {
757  if(scanMaxRange > cellSize)
758  {
759  const Transform & pose = poses.at(iter->first);
760  cv::Point3f viewpoint(0,0,0);
761  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
762  if(kter!=viewpoints.end())
763  {
764  viewpoint = kter->second;
765  }
766  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
767 
768  // As we don't know the angle_min or angle_max, ray trace between
769  // the first and last obstacle (counterclockwise).
770  cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(angleIncrement), -sin(angleIncrement),
771  sin(angleIncrement), cos(angleIncrement));
772  cv::Mat origin(2,1,CV_32F), obsFirst(2,1,CV_32F), obsLast(2,1,CV_32F);
773  origin.at<float>(0) = pose.x()+viewpoint.x;
774  origin.at<float>(1) = pose.y()+viewpoint.y;
775  obsFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
776  obsFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
777  obsLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[0];
778  obsLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[1];
779  cv::Mat firstVector(3,1,CV_32F), lastVector(3,1,CV_32F);
780  firstVector.at<float>(0) = obsFirst.at<float>(0) - origin.at<float>(0);
781  firstVector.at<float>(1) = obsFirst.at<float>(1) - origin.at<float>(1);
782  firstVector.at<float>(2) = 0.0f;
783  firstVector = firstVector/cv::norm(firstVector);
784  lastVector.at<float>(0) = obsLast.at<float>(0) - origin.at<float>(0);
785  lastVector.at<float>(1) = obsLast.at<float>(1) - origin.at<float>(1);
786  lastVector.at<float>(2) = 0.0f;
787  lastVector = lastVector / cv::norm(lastVector);
788  float maxAngle = acos(firstVector.dot(lastVector));
789  if(firstVector.cross(lastVector).at<float>(2) < 0)
790  {
791  maxAngle = 2*M_PI-maxAngle;
792  }
793  //UWARN("angle=%f v1=[%f %f 0];v2=[%f %f 0];",
794  // maxAngle,
795  // firstVector.at<float>(0), firstVector.at<float>(1),
796  // lastVector.at<float>(0), lastVector.at<float>(1));
797  float angle = angleIncrement;
798  cv::Mat tmp = (obsFirst - origin);
799  cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
800  while(angle < maxAngle-angleIncrement)
801  {
802  cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
803  //end must be inside the grid
804  end.x = end.x < 0?0:end.x;
805  end.x = end.x >= map.cols?map.cols-1:end.x;
806  end.y = end.y < 0?0:end.y;
807  end.y = end.y >= map.rows?map.rows-1:end.y;
808  rayTrace(start, end, map, true); // trace free space
809  // next point
810  endRotated = rotation*(endRotated - origin) + origin;
811 
812  angle+=angleIncrement;
813  }
814  }
815  }
816  ++j;
817  }
818  UDEBUG("Fill empty space=%fs", timer.ticks());
819  //cv::imwrite("map.png", util3d::convertMap2Image8U(map));
820  //UWARN("saved map.png");
821  }
822  }
823  return map;
824 }
825 
826 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
827 {
828  UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
829  UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
830  UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
831  UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
832 
833  cv::Point2i ptA, ptB;
834  ptA = start;
835  ptB = end;
836 
837  float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
838 
839  bool swapped = false;
840  if(slope<-1.0f || slope>1.0f)
841  {
842  // swap x and y
843  slope = 1.0f/slope;
844 
845  int tmp = ptA.x;
846  ptA.x = ptA.y;
847  ptA.y = tmp;
848 
849  tmp = ptB.x;
850  ptB.x = ptB.y;
851  ptB.y = tmp;
852 
853  swapped = true;
854  }
855 
856  float b = ptA.y - slope*ptA.x;
857  for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
858  {
859  int upperbound = float(x)*slope + b;
860  int lowerbound = upperbound;
861  if(x != ptA.x)
862  {
863  lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
864  }
865 
866  if(lowerbound > upperbound)
867  {
868  int tmp = upperbound;
869  upperbound = lowerbound;
870  lowerbound = tmp;
871  }
872 
873  if(!swapped)
874  {
875  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());
876  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());
877  }
878  else
879  {
880  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());
881  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());
882  }
883 
884  for(int y = lowerbound; y<=(int)upperbound; ++y)
885  {
886  char * v;
887  if(swapped)
888  {
889  v = &grid.at<char>(x, y);
890  }
891  else
892  {
893  v = &grid.at<char>(y, x);
894  }
895  if(*v == 100 && stopOnObstacle)
896  {
897  return;
898  }
899  else
900  {
901  *v = 0; // free space
902  }
903  }
904  }
905 }
906 
907 //convert to gray scaled map
908 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
909 {
910  UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
911  cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
912  for (int i = 0; i < map8S.rows; ++i)
913  {
914  for (int j = 0; j < map8S.cols; ++j)
915  {
916  char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
917  unsigned char gray;
918  if(v == 0)
919  {
920  gray = pgmFormat?254:178;
921  }
922  else if(v == 100)
923  {
924  gray = 0;
925  }
926  else if(v == -2)
927  {
928  gray = pgmFormat?254:200;
929  }
930  else if(pgmFormat || v == -1)// -1
931  {
932  gray = pgmFormat?205:89;
933  }
934  else if(v>50)
935  {
936  gray = double(100-v)*2/100.0*double(89);
937  }
938  else // v<50
939  {
940  gray = double(50-v)*2/100.0*double(178-89)+89;
941  }
942  map8U.at<unsigned char>(i, j) = gray;
943  }
944  }
945  return map8U;
946 }
947 
948 //convert gray scaled image to map
949 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
950 {
951  UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
952  cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
953  for (int i = 0; i < map8U.rows; ++i)
954  {
955  for (int j = 0; j < map8U.cols; ++j)
956  {
957  unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
958  char occupancy;
959  if(pgmFormat)
960  {
961  if(v >= 254)
962  {
963  occupancy = 0;
964  }
965  else if(v == 0)
966  {
967  occupancy = 100;
968  }
969  else // 205
970  {
971  occupancy = -1;
972  }
973  }
974  else
975  {
976  if(v == 178)
977  {
978  occupancy = 0;
979  }
980  else if(v == 0)
981  {
982  occupancy = 100;
983  }
984  else if(v == 200)
985  {
986  occupancy = -2;
987  }
988  else // 89
989  {
990  occupancy = -1;
991  }
992  }
993 
994  map8S.at<char>(i, j) = occupancy;
995  }
996  }
997  return map8S;
998 }
999 
1000 cv::Mat erodeMap(const cv::Mat & map)
1001 {
1002  UASSERT(map.type() == CV_8SC1);
1003  cv::Mat erodedMap = map.clone();
1004  for(int i=0; i<map.rows; ++i)
1005  {
1006  for(int j=0; j<map.cols; ++j)
1007  {
1008  if(map.at<char>(i, j) == 100)
1009  {
1010  // remove obstacles which touch at least 3 empty cells but not unknown cells
1011  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
1012  (map.at<char>(i-1, j) == 0?1:0) +
1013  (map.at<char>(i, j+1) == 0?1:0) +
1014  (map.at<char>(i, j-1) == 0?1:0);
1015 
1016  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
1017  map.at<char>(i-1, j) != -1 &&
1018  map.at<char>(i, j+1) != -1 &&
1019  map.at<char>(i, j-1) != -1)
1020  {
1021  erodedMap.at<char>(i, j) = 0; // empty
1022  }
1023  }
1024  }
1025  }
1026  return erodedMap;
1027 }
1028 
1029 }
1030 
1031 }
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:1000
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::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2481
f
f
x
x
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
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:580
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
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:949
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
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:908
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
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:337
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:826
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
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
end
end

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58