i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Create2DMap: set maximum scan range to 6 meters

Browse Source
This commit is contained in:
matlabbe
2015-09-23 16:52:23 -04:00
parent fb69f770f9
commit bb2ea8a3a3
1 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_mapping.cpp
+14 -11
View File
@@ -404,6 +404,12 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UDEBUG("poses=%d, scans = %d", poses.size(), scans.size()); UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans; std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
// For computation issue, the maximum scan range allowed is 6 meters
if(scanMaxRange > 6.0f)
{
scanMaxRange = 6.0f;
}
pcl::PointCloud<pcl::PointXYZ> minMax; pcl::PointCloud<pcl::PointXYZ> minMax;
if(minMapSize > 0.0f) if(minMapSize > 0.0f)
{ {
@@ -433,13 +439,14 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
pcl::getMinMax3D(minMax, min, max); pcl::getMinMax3D(minMax, min, max);
// Added X2 to make sure that all points are inside the map (when rounded to integer) // Added X2 to make sure that all points are inside the map (when rounded to integer)
float marging = cellSize*10.0f + (scanMaxRange>0.0f?scanMaxRange/cellSize:0.0f); float margin = cellSize*10.0f + (scanMaxRange>0.0f?scanMaxRange/cellSize:0.0f);
xMin = min.x-marging; xMin = min.x-margin;
yMin = min.y-marging; yMin = min.y-margin;
float xMax = max.x+marging; float xMax = max.x+margin;
float yMax = max.y+marging; float yMax = max.y+margin;
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax); UDEBUG("map min=(%f, %f) max=(%f,%f) (margin=%f, cellSize=%f, scan range=%f, min=[%f,%f] max=[%f,%f])",
xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
UTimer timer; UTimer timer;
@@ -475,6 +482,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
if(unknownSpaceFilled) if(unknownSpaceFilled)
{ {
j=0; j=0;
float a = CV_PI/256.0f; // angle increment
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{ {
if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f) if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f)
@@ -482,11 +490,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float maxLength = sqrt(maxSquaredLength[j]); float maxLength = sqrt(maxSquaredLength[j]);
if(maxLength > cellSize) if(maxLength > cellSize)
{ {
// compute angle
float a = (CV_PI/2.0f) / (maxLength / cellSize);
//UWARN("a=%f PI/256=%f", a, CV_PI/256.0f);
UASSERT_MSG(a >= 0 && a < 5.0f*CV_PI/8.0f, uFormat("a=%f length=%f cell=%f", a, maxLength, cellSize).c_str());
const Transform & pose = poses.at(iter->first); const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f); cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);