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  signed char & value = map.at<signed 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<signed 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  signed char & value = map.at<signed 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<signed char>(i, j) == -2)
420  {
421  updatedMap.at<signed 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<signed char>(i, j) == -1 &&
427  map.at<signed char>(i+1, j) != -1 &&
428  map.at<signed char>(i-1, j) != -1 &&
429  map.at<signed char>(i, j+1) != -1 &&
430  map.at<signed char>(i, j-1) != -1)
431  {
432  updatedMap.at<signed char>(i, j) = 0;
433  }
434  else if(map.at<signed char>(i, j) == 100)
435  {
436  // obstacle/empty/unknown -> remove empty
437  // unknown/empty/obstacle -> remove empty
438  if((map.at<signed char>(i-1, j) == 0 || map.at<signed char>(i-1, j) == -2) &&
439  map.at<signed char>(i-2, j) == -1)
440  {
441  updatedMap.at<signed char>(i-1, j) = -1;
442  }
443  else if((map.at<signed char>(i+1, j) == 0 || map.at<signed char>(i+1, j) == -2) &&
444  map.at<signed char>(i+2, j) == -1)
445  {
446  updatedMap.at<signed char>(i+1, j) = -1;
447  }
448  if((map.at<signed char>(i, j-1) == 0 || map.at<signed char>(i, j-1) == -2) &&
449  map.at<signed char>(i, j-2) == -1)
450  {
451  updatedMap.at<signed char>(i, j-1) = -1;
452  }
453  else if((map.at<signed char>(i, j+1) == 0 || map.at<signed char>(i, j+1) == -2) &&
454  map.at<signed char>(i, j+2) == -1)
455  {
456  updatedMap.at<signed 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<signed char>(i, j) == 0)
465  {
466  // obstacle/empty/obstacle -> remove empty
467  if(map.at<signed char>(i-1, j) == 100 &&
468  map.at<signed char>(i+1, j) == 100)
469  {
470  updatedMap.at<signed char>(i, j) = -1;
471  }
472  else if(map.at<signed char>(i, j-1) == 100 &&
473  map.at<signed char>(i, j+1) == 100)
474  {
475  updatedMap.at<signed 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<signed char>(i+1, j) == 0?1:0) +
494  (map.at<signed char>(i-1, j) == 0?1:0) +
495  (map.at<signed char>(i, j+1) == 0?1:0) +
496  (map.at<signed char>(i, j-1) == 0?1:0);
497  if(touchEmpty>=3 && map.at<signed char>(i+1, j) != -1 &&
498  map.at<signed char>(i-1, j) != -1 &&
499  map.at<signed char>(i, j+1) != -1 &&
500  map.at<signed char>(i, j-1) != -1)
501  {
502  erodedMap.at<signed 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 = (scanMaxRange > 0 ? -scanMaxRange : min.x) - margin;
646  yMin = (scanMaxRange > 0 ? -scanMaxRange : min.y) - margin;
647  float xMax = (scanMaxRange > 0 ? scanMaxRange : max.x) + margin;
648  float yMax = (scanMaxRange > 0 ? 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<signed 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  rayTrace(start, end, map, true); // trace free space
707  }
708  }
709  // ray tracing for no hits
710  for(int i=0; i<iter->second.second.cols; ++i)
711  {
712  const float * ptr = iter->second.second.ptr<float>(0, i);
713 
714  cv::Vec2f pt(ptr[0], ptr[1]);
715  if(scanMaxRange>cellSize)
716  {
717  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
718  float n = cv::norm(v);
719  if(n > scanMaxRange+cellSize)
720  {
721  v = (v/n) * scanMaxRange;
722  pt[0] = pose.x()+viewpoint.x + v[0];
723  pt[1] = pose.y()+viewpoint.y + v[1];
724  }
725  }
726 
727  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
728  if(end!=start)
729  {
730  rayTrace(start, end, map, true); // trace free space
731  }
732  }
733  }
734  UDEBUG("Ray trace known space=%fs", timer.ticks());
735 
736  // now fill unknown spaces
737  if(unknownSpaceFilled && scanMaxRange > 0)
738  {
739  float angleIncrement = CV_PI/90.0f; // angle increment
740  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
741  {
742  if(iter->second.first.cols > 2)
743  {
744  if(scanMaxRange > cellSize)
745  {
746  const Transform & pose = poses.at(iter->first);
747  cv::Point3f viewpoint(0,0,0);
748  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
749  if(kter!=viewpoints.end())
750  {
751  viewpoint = kter->second;
752  }
753  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
754 
755  // As we don't know the angle_min or angle_max, ray trace between
756  // the first and last obstacle (counterclockwise).
757  cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(angleIncrement), -sin(angleIncrement),
758  sin(angleIncrement), cos(angleIncrement));
759  cv::Mat origin(2,1,CV_32F), obsFirst(2,1,CV_32F), obsLast(2,1,CV_32F);
760  origin.at<float>(0) = pose.x()+viewpoint.x;
761  origin.at<float>(1) = pose.y()+viewpoint.y;
762  obsFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
763  obsFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
764  obsLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[0];
765  obsLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-2)[1];
766  cv::Mat firstVector(3,1,CV_32F), lastVector(3,1,CV_32F);
767  firstVector.at<float>(0) = obsFirst.at<float>(0) - origin.at<float>(0);
768  firstVector.at<float>(1) = obsFirst.at<float>(1) - origin.at<float>(1);
769  firstVector.at<float>(2) = 0.0f;
770  firstVector = firstVector/cv::norm(firstVector);
771  lastVector.at<float>(0) = obsLast.at<float>(0) - origin.at<float>(0);
772  lastVector.at<float>(1) = obsLast.at<float>(1) - origin.at<float>(1);
773  lastVector.at<float>(2) = 0.0f;
774  lastVector = lastVector / cv::norm(lastVector);
775  float maxAngle = acos(firstVector.dot(lastVector));
776  if(firstVector.cross(lastVector).at<float>(2) < 0)
777  {
778  maxAngle = 2*M_PI-maxAngle;
779  }
780  //UWARN("angle=%f v1=[%f %f 0];v2=[%f %f 0];",
781  // maxAngle,
782  // firstVector.at<float>(0), firstVector.at<float>(1),
783  // lastVector.at<float>(0), lastVector.at<float>(1));
784  float angle = angleIncrement;
785  cv::Mat tmp = (obsFirst - origin);
786  cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
787  while(angle < maxAngle-angleIncrement)
788  {
789  cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
790  //end must be inside the grid
791  end.x = end.x < 0?0:end.x;
792  end.x = end.x >= map.cols?map.cols-1:end.x;
793  end.y = end.y < 0?0:end.y;
794  end.y = end.y >= map.rows?map.rows-1:end.y;
795  rayTrace(start, end, map, true); // trace free space
796  // next point
797  endRotated = rotation*(endRotated - origin) + origin;
798 
799  angle+=angleIncrement;
800  }
801  }
802  }
803  }
804  UDEBUG("Fill empty space=%fs", timer.ticks());
805  //cv::imwrite("map.png", util3d::convertMap2Image8U(map));
806  //UWARN("saved map.png");
807  }
808  }
809  return map;
810 }
811 
812 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
813 {
814  UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
815  UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
816 
817  cv::Point2i ptA, ptB;
818  ptA = start;
819  ptB = end;
820 
821  float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
822 
823  // clip end point
824  ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
825  ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
826 
827  bool swapped = false;
828  if(slope<-1.0f || slope>1.0f)
829  {
830  // swap x and y
831  slope = 1.0f/slope;
832 
833  int tmp = ptA.x;
834  ptA.x = ptA.y;
835  ptA.y = tmp;
836 
837  tmp = ptB.x;
838  ptB.x = ptB.y;
839  ptB.y = tmp;
840 
841  swapped = true;
842  }
843 
844  float b = ptA.y - slope*ptA.x;
845  for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
846  {
847  int upperbound = float(x)*slope + b;
848  int lowerbound = upperbound;
849  if(x != ptA.x)
850  {
851  lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
852  }
853 
854  if(lowerbound > upperbound)
855  {
856  int tmp = upperbound;
857  upperbound = lowerbound;
858  lowerbound = tmp;
859  }
860 
861  if(!swapped)
862  {
863  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());
864  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());
865  }
866  else
867  {
868  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());
869  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());
870  }
871 
872  for(int y = lowerbound; y<=(int)upperbound; ++y)
873  {
874  signed char * v;
875  if(swapped)
876  {
877  v = &grid.at<signed char>(x, y);
878  }
879  else
880  {
881  v = &grid.at<signed char>(y, x);
882  }
883  if(*v == 100 && stopOnObstacle)
884  {
885  return;
886  }
887  else
888  {
889  *v = 0; // free space
890  }
891  }
892  }
893 }
894 
895 //convert to gray scaled map
896 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
897 {
898  UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
899  cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
900  for (int i = 0; i < map8S.rows; ++i)
901  {
902  for (int j = 0; j < map8S.cols; ++j)
903  {
904  signed char v = pgmFormat?map8S.at<signed char>((map8S.rows-1)-i, j):map8S.at<signed char>(i, j);
905  unsigned char gray;
906  if(v == 0)
907  {
908  gray = pgmFormat?254:178;
909  }
910  else if(v == 100)
911  {
912  gray = 0;
913  }
914  else if(v == -2)
915  {
916  gray = pgmFormat?254:200;
917  }
918  else if(pgmFormat || v == -1)// -1
919  {
920  gray = pgmFormat?205:89;
921  }
922  else if(v>50)
923  {
924  gray = double(100-v)*2/100.0*double(89);
925  }
926  else // v<50
927  {
928  gray = double(50-v)*2/100.0*double(178-89)+89;
929  }
930  map8U.at<unsigned char>(i, j) = gray;
931  }
932  }
933  return map8U;
934 }
935 
936 //convert gray scaled image to map
937 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
938 {
939  UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
940  cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
941  for (int i = 0; i < map8U.rows; ++i)
942  {
943  for (int j = 0; j < map8U.cols; ++j)
944  {
945  unsigned char v = pgmFormat?map8U.at<signed char>((map8U.rows-1)-i, j):map8U.at<signed char>(i, j);
946  char occupancy;
947  if(pgmFormat)
948  {
949  if(v >= 254)
950  {
951  occupancy = 0;
952  }
953  else if(v == 0)
954  {
955  occupancy = 100;
956  }
957  else // 205
958  {
959  occupancy = -1;
960  }
961  }
962  else
963  {
964  if(v == 178)
965  {
966  occupancy = 0;
967  }
968  else if(v == 0)
969  {
970  occupancy = 100;
971  }
972  else if(v == 200)
973  {
974  occupancy = -2;
975  }
976  else // 89
977  {
978  occupancy = -1;
979  }
980  }
981 
982  map8S.at<signed char>(i, j) = occupancy;
983  }
984  }
985  return map8S;
986 }
987 
988 cv::Mat erodeMap(const cv::Mat & map)
989 {
990  UASSERT(map.type() == CV_8SC1);
991  cv::Mat erodedMap = map.clone();
992  for(int i=0; i<map.rows; ++i)
993  {
994  for(int j=0; j<map.cols; ++j)
995  {
996  if(map.at<signed char>(i, j) == 100)
997  {
998  // remove obstacles which touch at least 3 empty cells but not unknown cells
999  int touchEmpty = (map.at<signed char>(i+1, j) == 0?1:0) +
1000  (map.at<signed char>(i-1, j) == 0?1:0) +
1001  (map.at<signed char>(i, j+1) == 0?1:0) +
1002  (map.at<signed char>(i, j-1) == 0?1:0);
1003 
1004  if(touchEmpty>=3 && map.at<signed char>(i+1, j) != -1 &&
1005  map.at<signed char>(i-1, j) != -1 &&
1006  map.at<signed char>(i, j+1) != -1 &&
1007  map.at<signed char>(i, j-1) != -1)
1008  {
1009  erodedMap.at<signed char>(i, j) = 0; // empty
1010  }
1011  }
1012  }
1013  }
1014  return erodedMap;
1015 }
1016 
1017 }
1018 
1019 }
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:988
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:812
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:937
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:2633
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:896
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 Mon Apr 28 2025 02:46:07