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 & scanHit,
84  const cv::Mat & scanNoHit,
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(scanHit.empty() && scanNoHit.empty())
93  {
94  return;
95  }
96 
97  std::map<int, Transform> poses;
98  poses.insert(std::make_pair(1, Transform::getIdentity()));
99 
100  std::map<int, std::pair<cv::Mat, cv::Mat> > scans;
101  scans.insert(std::make_pair(1, std::make_pair(scanHit, scanNoHit)));
102 
103  std::map<int, cv::Point3f> viewpoints;
104  viewpoints.insert(std::make_pair(1, viewpoint));
105 
106  float xMin, yMin;
107  cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
108 
109  // find empty cells
110  std::list<int> emptyIndices;
111  for(unsigned int i=0; i< map8S.total(); ++i)
112  {
113  if(map8S.data[i] == 0)
114  {
115  emptyIndices.push_back(i);
116  }
117  }
118 
119  // Convert to position matrices, get points to each center of the cells
120  empty = cv::Mat();
121  if(emptyIndices.size())
122  {
123  empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
124  int i=0;
125  for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
126  {
127  int y = *iter / map8S.cols;
128  int x = *iter - y*map8S.cols;
129  cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
130  ptr[i][0] = (float(x))*cellSize + xMin;
131  ptr[i][1] = (float(y))*cellSize + yMin;
132  ++i;
133  }
134  }
135 
136  // copy directly obstacles precise positions
137  if(scanMaxRange > cellSize)
138  {
139  occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
140  }
141  else
142  {
143  occupied = scanHit.clone();
144  }
145 }
146 
160 cv::Mat create2DMapFromOccupancyLocalMaps(
161  const std::map<int, Transform> & posesIn,
162  const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
163  float cellSize,
164  float & xMin,
165  float & yMin,
166  float minMapSize,
167  bool erode,
168  float footprintRadius)
169 {
170  UASSERT(minMapSize >= 0.0f);
171  UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
172  UTimer timer;
173 
174  std::map<int, cv::Mat> emptyLocalMaps;
175  std::map<int, cv::Mat> occupiedLocalMaps;
176 
177  std::list<std::pair<int, Transform> > poses;
178  // place negative poses at the end
179  for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
180  {
181  if(iter->first>0)
182  {
183  poses.push_front(*iter);
184  }
185  else
186  {
187  poses.push_back(*iter);
188  }
189  }
190 
191  float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
192  bool undefinedSize = minMapSize == 0.0f;
193  for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
194  {
195  UASSERT(!iter->second.isNull());
196 
197  float x = iter->second.x();
198  float y =iter->second.y();
199  if(undefinedSize)
200  {
201  minX = maxX = x;
202  minY = maxY = y;
203  undefinedSize = false;
204  }
205  else
206  {
207  if(minX > x)
208  minX = x;
209  else if(maxX < x)
210  maxX = x;
211 
212  if(minY > y)
213  minY = y;
214  else if(maxY < y)
215  maxY = y;
216  }
217 
218  if(uContains(occupancy, iter->first))
219  {
220  const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
221 
222  //ground
223  if(pair.first.cols)
224  {
225  if(pair.first.rows > 1 && pair.first.cols == 1)
226  {
227  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
228  }
229  cv::Mat ground(1, pair.first.cols, CV_32FC2);
230  for(int i=0; i<ground.cols; ++i)
231  {
232  const float * vi = pair.first.ptr<float>(0,i);
233  float * vo = ground.ptr<float>(0,i);
234  cv::Point3f vt;
235  if(pair.first.channels() != 2 && pair.first.channels() != 5)
236  {
237  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
238  }
239  else
240  {
241  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
242  }
243  vo[0] = vt.x;
244  vo[1] = vt.y;
245  if(minX > vo[0])
246  minX = vo[0];
247  else if(maxX < vo[0])
248  maxX = vo[0];
249 
250  if(minY > vo[1])
251  minY = vo[1];
252  else if(maxY < vo[1])
253  maxY = vo[1];
254  }
255  emptyLocalMaps.insert(std::make_pair(iter->first, ground));
256  }
257 
258  //obstacles
259  if(pair.second.cols)
260  {
261  if(pair.second.rows > 1 && pair.second.cols == 1)
262  {
263  UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
264  }
265  cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
266  for(int i=0; i<obstacles.cols; ++i)
267  {
268  const float * vi = pair.second.ptr<float>(0,i);
269  float * vo = obstacles.ptr<float>(0,i);
270  cv::Point3f vt;
271  if(pair.second.channels() != 2 && pair.second.channels() != 5)
272  {
273  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
274  }
275  else
276  {
277  vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
278  }
279  vo[0] = vt.x;
280  vo[1] = vt.y;
281  if(minX > vo[0])
282  minX = vo[0];
283  else if(maxX < vo[0])
284  maxX = vo[0];
285 
286  if(minY > vo[1])
287  minY = vo[1];
288  else if(maxY < vo[1])
289  maxY = vo[1];
290  }
291  occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
292  }
293  }
294  }
295  UDEBUG("timer=%fs", timer.ticks());
296 
297  cv::Mat map;
298  if(minX != maxX && minY != maxY)
299  {
300  //Get map size
301  float margin = cellSize*10.0f;
302  xMin = minX-margin;
303  xMin -= cellSize/2.0f;
304  yMin = minY-margin;
305  yMin += cellSize/2.0f;
306  float xMax = maxX+margin;
307  float yMax = maxY+margin;
308  if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB
309  fabs((xMax - xMin) / cellSize) > 30000)
310  {
311  UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
312  "There's maybe an error with the poses provided! The map will not be created!",
313  xMin, yMin, xMax, yMax);
314  }
315  else
316  {
317  UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
318 
319 
320  map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
321  for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
322  {
323  std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
324  std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
325  if(iter!=emptyLocalMaps.end())
326  {
327  for(int i=0; i<iter->second.cols; ++i)
328  {
329  float * ptf = iter->second.ptr<float>(0, i);
330  cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
331  UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
332  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());
333  char & value = map.at<char>(pt.y, pt.x);
334  if(value != -2)
335  {
336  value = 0; // free space
337  }
338  }
339  }
340 
341  if(footprintRadius >= cellSize*1.5f)
342  {
343  // place free space under the footprint of the robot
344  cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
345  cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize, (kter->second.y()+footprintRadius-yMin)/cellSize);
346  if(ptBegin.x < 0)
347  ptBegin.x = 0;
348  if(ptEnd.x >= map.cols)
349  ptEnd.x = map.cols-1;
350 
351  if(ptBegin.y < 0)
352  ptBegin.y = 0;
353  if(ptEnd.y >= map.rows)
354  ptEnd.y = map.rows-1;
355  for(int i=ptBegin.x; i<ptEnd.x; ++i)
356  {
357  for(int j=ptBegin.y; j<ptEnd.y; ++j)
358  {
359  map.at<char>(j, i) = -2; // free space (footprint)
360  }
361  }
362  }
363 
364  if(jter!=occupiedLocalMaps.end())
365  {
366  for(int i=0; i<jter->second.cols; ++i)
367  {
368  float * ptf = jter->second.ptr<float>(0, i);
369  cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
370  UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
371  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());
372  char & value = map.at<char>(pt.y, pt.x);
373  if(value != -2)
374  {
375  value = 100; // obstacles
376  }
377  }
378  }
379 
380  //UDEBUG("empty=%d occupied=%d", empty, occupied);
381  }
382 
383  // fill holes and remove empty from obstacle borders
384  cv::Mat updatedMap = map.clone();
385  std::list<std::pair<int, int> > obstacleIndices;
386  for(int i=0; i<map.rows; ++i)
387  {
388  for(int j=0; j<map.cols; ++j)
389  {
390  if(map.at<char>(i, j) == -2)
391  {
392  updatedMap.at<char>(i, j) = 0;
393  }
394 
395  if(i >=2 && i<map.rows-2 && j>=2 && j<map.cols-2)
396  {
397  if(map.at<char>(i, j) == -1 &&
398  map.at<char>(i+1, j) != -1 &&
399  map.at<char>(i-1, j) != -1 &&
400  map.at<char>(i, j+1) != -1 &&
401  map.at<char>(i, j-1) != -1)
402  {
403  updatedMap.at<char>(i, j) = 0;
404  }
405  else if(map.at<char>(i, j) == 100)
406  {
407  // obstacle/empty/unknown -> remove empty
408  // unknown/empty/obstacle -> remove empty
409  if((map.at<char>(i-1, j) == 0 || map.at<char>(i-1, j) == -2) &&
410  map.at<char>(i-2, j) == -1)
411  {
412  updatedMap.at<char>(i-1, j) = -1;
413  }
414  else if((map.at<char>(i+1, j) == 0 || map.at<char>(i+1, j) == -2) &&
415  map.at<char>(i+2, j) == -1)
416  {
417  updatedMap.at<char>(i+1, j) = -1;
418  }
419  if((map.at<char>(i, j-1) == 0 || map.at<char>(i, j-1) == -2) &&
420  map.at<char>(i, j-2) == -1)
421  {
422  updatedMap.at<char>(i, j-1) = -1;
423  }
424  else if((map.at<char>(i, j+1) == 0 || map.at<char>(i, j+1) == -2) &&
425  map.at<char>(i, j+2) == -1)
426  {
427  updatedMap.at<char>(i, j+1) = -1;
428  }
429 
430  if(erode)
431  {
432  obstacleIndices.push_back(std::make_pair(i, j));
433  }
434  }
435  else if(map.at<char>(i, j) == 0)
436  {
437  // obstacle/empty/obstacle -> remove empty
438  if(map.at<char>(i-1, j) == 100 &&
439  map.at<char>(i+1, j) == 100)
440  {
441  updatedMap.at<char>(i, j) = -1;
442  }
443  else if(map.at<char>(i, j-1) == 100 &&
444  map.at<char>(i, j+1) == 100)
445  {
446  updatedMap.at<char>(i, j) = -1;
447  }
448  }
449  }
450  }
451  }
452  map = updatedMap;
453 
454  if(erode)
455  {
456  // remove obstacles which touch at least 3 empty cells but not unknown cells
457  cv::Mat erodedMap = map.clone();
458  for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
459  iter!= obstacleIndices.end();
460  ++iter)
461  {
462  int i = iter->first;
463  int j = iter->second;
464  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
465  (map.at<char>(i-1, j) == 0?1:0) +
466  (map.at<char>(i, j+1) == 0?1:0) +
467  (map.at<char>(i, j-1) == 0?1:0);
468  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
469  map.at<char>(i-1, j) != -1 &&
470  map.at<char>(i, j+1) != -1 &&
471  map.at<char>(i, j-1) != -1)
472  {
473  erodedMap.at<char>(i, j) = 0; // empty
474  }
475  }
476  map = erodedMap;
477  }
478  }
479  }
480 
481  UDEBUG("timer=%fs", timer.ticks());
482  return map;
483 }
484 
499 cv::Mat create2DMap(const std::map<int, Transform> & poses,
500  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
501  float cellSize,
502  bool unknownSpaceFilled,
503  float & xMin,
504  float & yMin,
505  float minMapSize,
506  float scanMaxRange)
507 {
508  std::map<int, cv::Point3f > viewpoints;
509  std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
510  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
511  {
512  scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
513  }
514  return create2DMap(poses,
515  scansCv,
516  viewpoints,
517  cellSize,
518  unknownSpaceFilled,
519  xMin,
520  yMin,
521  minMapSize,
522  scanMaxRange);
523 }
524 
525 cv::Mat create2DMap(const std::map<int, Transform> & poses,
526  const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
527  const std::map<int, cv::Point3f > & viewpoints,
528  float cellSize,
529  bool unknownSpaceFilled,
530  float & xMin,
531  float & yMin,
532  float minMapSize,
533  float scanMaxRange)
534 {
535  std::map<int, std::pair<cv::Mat, cv::Mat> > scansCv;
536  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter = scans.begin(); iter!=scans.end(); ++iter)
537  {
538  scansCv.insert(std::make_pair(iter->first, std::make_pair(util3d::laserScanFromPointCloud(*iter->second), cv::Mat())));
539  }
540  return create2DMap(poses,
541  scansCv,
542  viewpoints,
543  cellSize,
544  unknownSpaceFilled,
545  xMin,
546  yMin,
547  minMapSize,
548  scanMaxRange);
549 }
550 
566 cv::Mat create2DMap(const std::map<int, Transform> & poses,
567  const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >
568  const std::map<int, cv::Point3f > & viewpoints,
569  float cellSize,
570  bool unknownSpaceFilled,
571  float & xMin,
572  float & yMin,
573  float minMapSize,
574  float scanMaxRange)
575 {
576  UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
577 
578  // local scans contain end points of each ray in map frame (pose+localTransform)
579  std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
580 
581  pcl::PointCloud<pcl::PointXYZ> minMax;
582  if(minMapSize > 0.0f)
583  {
584  minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
585  minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
586  }
587  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
588  {
589  std::map<int, std::pair<cv::Mat, cv::Mat> >::const_iterator jter=scans.find(iter->first);
590  if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
591  {
592  UASSERT(!iter->second.isNull());
593  cv::Mat hit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.first), iter->second).data();
594  cv::Mat noHit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.second), iter->second).data();
595  pcl::PointXYZ min, max;
596  if(!hit.empty())
597  {
598  util3d::getMinMax3D(hit, min, max);
599  minMax.push_back(min);
600  minMax.push_back(max);
601  }
602  if(!noHit.empty())
603  {
604  util3d::getMinMax3D(noHit, min, max);
605  minMax.push_back(min);
606  minMax.push_back(max);
607  }
608  minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
609 
610  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
611  if(kter!=viewpoints.end())
612  {
613  minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
614  }
615 
616  localScans.insert(std::make_pair(iter->first, std::make_pair(hit, noHit)));
617  }
618  }
619 
620  cv::Mat map;
621  if(minMax.size())
622  {
623  //Get map size
624  pcl::PointXYZ min, max;
625  pcl::getMinMax3D(minMax, min, max);
626 
627  // Added margin to make sure that all points are inside the map (when rounded to integer)
628  float margin = cellSize*10.0f;
629  xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
630  yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
631  float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
632  float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
633 
634  UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
635  xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
636 
637  UTimer timer;
638 
639  map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
640  UDEBUG("map size = %dx%d", map.cols, map.rows);
641 
642  int j=0;
643  float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
644  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
645  {
646  const Transform & pose = poses.at(iter->first);
647  cv::Point3f viewpoint(0,0,0);
648  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
649  if(kter!=viewpoints.end())
650  {
651  viewpoint = kter->second;
652  }
653  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
654 
655  // Set obstacles first
656  for(int i=0; i<iter->second.first.cols; ++i)
657  {
658  const float * ptr = iter->second.first.ptr<float>(0, i);
659  bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
660  if(!ignore)
661  {
662  cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
663  if(end!=start)
664  {
665  map.at<char>(end.y, end.x) = 100; // obstacle
666  }
667  }
668  }
669 
670  // ray tracing for hits
671  for(int i=0; i<iter->second.first.cols; ++i)
672  {
673  const float * ptr = iter->second.first.ptr<float>(0, i);
674 
675  cv::Vec2f pt(ptr[0], ptr[1]);
676  if(scanMaxRange>cellSize)
677  {
678  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
679  float n = cv::norm(v);
680  if(n > scanMaxRange+cellSize)
681  {
682  v = (v/n) * scanMaxRange;
683  pt[0] = pose.x()+viewpoint.x + v[0];
684  pt[1] = pose.y()+viewpoint.y + v[1];
685  }
686  }
687 
688  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
689  if(end!=start)
690  {
691  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
692  {
693  rayTrace(start, end, map, true); // trace free space
694  }
695  }
696  }
697  // ray tracing for no hits
698  for(int i=0; i<iter->second.second.cols; ++i)
699  {
700  const float * ptr = iter->second.second.ptr<float>(0, i);
701 
702  cv::Vec2f pt(ptr[0], ptr[1]);
703  if(scanMaxRange>cellSize)
704  {
705  cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
706  float n = cv::norm(v);
707  if(n > scanMaxRange+cellSize)
708  {
709  v = (v/n) * scanMaxRange;
710  pt[0] = pose.x()+viewpoint.x + v[0];
711  pt[1] = pose.y()+viewpoint.y + v[1];
712  }
713  }
714 
715  cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
716  if(end!=start)
717  {
718  if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
719  {
720  rayTrace(start, end, map, true); // trace free space
721  if(map.at<char>(end.y, end.x) == -1)
722  {
723  map.at<char>(end.y, end.x) = 0; // empty
724  }
725  }
726  }
727  }
728  ++j;
729  }
730  UDEBUG("Ray trace known space=%fs", timer.ticks());
731 
732  // now fill unknown spaces
733  if(unknownSpaceFilled && scanMaxRange > 0)
734  {
735  j=0;
736  float a = CV_PI/256.0f; // angle increment
737  for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
738  {
739  if(iter->second.first.cols > 1)
740  {
741  if(scanMaxRange > cellSize)
742  {
743  const Transform & pose = poses.at(iter->first);
744  cv::Point3f viewpoint(0,0,0);
745  std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
746  if(kter!=viewpoints.end())
747  {
748  viewpoint = kter->second;
749  }
750  cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
751 
752  //UWARN("maxLength = %f", maxLength);
753  //rotate counterclockwise from the first point until we pass the last point
754  // Note: assuming that first laser scan is negative y
755  cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
756  sin(a), cos(a));
757  cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
758  origin.at<float>(0) = pose.x()+viewpoint.x;
759  origin.at<float>(1) = pose.y()+viewpoint.y;
760  endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
761  endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
762  endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
763  endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
764  //UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
765  //UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
766  //UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
767  cv::Mat tmp = (endFirst - origin);
768  cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*scanMaxRange) + origin;
769  cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
770  endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
771  endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
772  endLastVector.at<float>(2) = 0.0f;
773  endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
774  endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
775  endRotatedVector.at<float>(2) = 0.0f;
776  //UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
777  float normEndRotatedVector = cv::norm(endRotatedVector);
778  endLastVector = endLastVector / cv::norm(endLastVector);
779  float angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
780  angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
781  while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
782  {
783  cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
784  //end must be inside the grid
785  end.x = end.x < 0?0:end.x;
786  end.x = end.x >= map.cols?map.cols-1:end.x;
787  end.y = end.y < 0?0:end.y;
788  end.y = end.y >= map.rows?map.rows-1:end.y;
789  rayTrace(start, end, map, true); // trace free space
790  // next point
791  endRotated = rotation*(endRotated - origin) + origin;
792  endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
793  endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
794  angle = (endRotatedVector/normEndRotatedVector).dot(endLastVector);
795  angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
796 
797  //UWARN("endRotated = %f %f (%f %f %f)",
798  // endRotated.at<float>(0), endRotated.at<float>(1),
799  // acos(angle),
800  // angle,
801  // endRotatedVector.cross(endLastVector).at<float>(2));
802  }
803  }
804  }
805  ++j;
806  }
807  UDEBUG("Fill empty space=%fs", timer.ticks());
808  //cv::imwrite("map.png", util3d::convertMap2Image8U(map));
809  //UWARN("saved map.png");
810  }
811  }
812  return map;
813 }
814 
815 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
816 {
817  UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
818  UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
819  UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
820  UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
821 
822  cv::Point2i ptA, ptB;
823  ptA = start;
824  ptB = end;
825 
826  float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
827 
828  bool swapped = false;
829  if(slope<-1.0f || slope>1.0f)
830  {
831  // swap x and y
832  slope = 1.0f/slope;
833 
834  int tmp = ptA.x;
835  ptA.x = ptA.y;
836  ptA.y = tmp;
837 
838  tmp = ptB.x;
839  ptB.x = ptB.y;
840  ptB.y = tmp;
841 
842  swapped = true;
843  }
844 
845  float b = ptA.y - slope*ptA.x;
846  for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
847  {
848  int upperbound = float(x)*slope + b;
849  int lowerbound = upperbound;
850  if(x != ptA.x)
851  {
852  lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
853  }
854 
855  if(lowerbound > upperbound)
856  {
857  int tmp = upperbound;
858  upperbound = lowerbound;
859  lowerbound = tmp;
860  }
861 
862  if(!swapped)
863  {
864  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());
865  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());
866  }
867  else
868  {
869  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());
870  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());
871  }
872 
873  for(int y = lowerbound; y<=(int)upperbound; ++y)
874  {
875  char * v;
876  if(swapped)
877  {
878  v = &grid.at<char>(x, y);
879  }
880  else
881  {
882  v = &grid.at<char>(y, x);
883  }
884  if(*v == 100 && stopOnObstacle)
885  {
886  return;
887  }
888  else
889  {
890  *v = 0; // free space
891  }
892  }
893  }
894 }
895 
896 //convert to gray scaled map
897 cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
898 {
899  UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
900  cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
901  for (int i = 0; i < map8S.rows; ++i)
902  {
903  for (int j = 0; j < map8S.cols; ++j)
904  {
905  char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
906  unsigned char gray;
907  if(v == 0)
908  {
909  gray = pgmFormat?254:178;
910  }
911  else if(v == 100)
912  {
913  gray = 0;
914  }
915  else if(v == -2)
916  {
917  gray = pgmFormat?254:200;
918  }
919  else // -1
920  {
921  gray = pgmFormat?205:89;
922  }
923  map8U.at<unsigned char>(i, j) = gray;
924  }
925  }
926  return map8U;
927 }
928 
929 //convert gray scaled image to map
930 cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
931 {
932  UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
933  cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
934  for (int i = 0; i < map8U.rows; ++i)
935  {
936  for (int j = 0; j < map8U.cols; ++j)
937  {
938  unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
939  char occupancy;
940  if(pgmFormat)
941  {
942  if(v >= 254)
943  {
944  occupancy = 0;
945  }
946  else if(v == 0)
947  {
948  occupancy = 100;
949  }
950  else // 205
951  {
952  occupancy = -1;
953  }
954  }
955  else
956  {
957  if(v == 178)
958  {
959  occupancy = 0;
960  }
961  else if(v == 0)
962  {
963  occupancy = 100;
964  }
965  else if(v == 200)
966  {
967  occupancy = -2;
968  }
969  else // 89
970  {
971  occupancy = -1;
972  }
973  }
974 
975  map8S.at<char>(i, j) = occupancy;
976  }
977  }
978  return map8S;
979 }
980 
981 cv::Mat erodeMap(const cv::Mat & map)
982 {
983  UASSERT(map.type() == CV_8SC1);
984  cv::Mat erodedMap = map.clone();
985  for(int i=0; i<map.rows; ++i)
986  {
987  for(int j=0; j<map.cols; ++j)
988  {
989  if(map.at<char>(i, j) == 100)
990  {
991  // remove obstacles which touch at least 3 empty cells but not unknown cells
992  int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
993  (map.at<char>(i-1, j) == 0?1:0) +
994  (map.at<char>(i, j+1) == 0?1:0) +
995  (map.at<char>(i, j-1) == 0?1:0);
996 
997  if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
998  map.at<char>(i-1, j) != -1 &&
999  map.at<char>(i, j+1) != -1 &&
1000  map.at<char>(i, j-1) != -1)
1001  {
1002  erodedMap.at<char>(i, j) = 0; // empty
1003  }
1004  }
1005  }
1006  }
1007  return erodedMap;
1008 }
1009 
1010 }
1011 
1012 }
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::util3d::erodeMap
cv::Mat RTABMAP_EXP erodeMap(const cv::Mat &map)
Definition: util3d_mapping.cpp:981
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:160
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:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2593
f
f
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
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:566
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:930
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:897
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:270
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::util3d::rayTrace
void RTABMAP_EXP rayTrace(const cv::Point2i &start, const cv::Point2i &end, cv::Mat &grid, bool stopOnObstacle)
Definition: util3d_mapping.cpp:815
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:84

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41