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

create2DMap: removed hard-coded max 6 meters range for unknown space filling. util3d::downsample() now returns a copy of the scane if step is larger than the scan size

Browse Source
This commit is contained in:
matlabbe
2016-03-11 19:58:48 -05:00
parent 48644a17f0
commit 152d47e62a
2 changed files with 29 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_filtering.cpp
+3 -25
View File
@@ -57,14 +57,12 @@ cv::Mat downsample(
{ {
UASSERT(step > 0); UASSERT(step > 0);
cv::Mat output; cv::Mat output;
if(step == 1) if(step <= 1 || cloud.cols <= step)
{ {
// no sampling // no sampling
output = cloud.clone(); output = cloud.clone();
} }
else else
{
if(cloud.cols > step)
{ {
int finalSize = cloud.cols/step; int finalSize = cloud.cols/step;
output = cv::Mat(1, finalSize, cloud.type()); output = cv::Mat(1, finalSize, cloud.type());
@@ -75,12 +73,6 @@ cv::Mat downsample(
++oi; ++oi;
} }
} }
else if(cloud.cols)
{
output = cv::Mat(1, 1, cloud.type());
cv::Mat(cloud, cv::Range::all(), cv::Range(0,1)).copyTo(output); // first point
}
}
return output; return output;
} }
pcl::PointCloud<pcl::PointXYZ>::Ptr downsample( pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
@@ -89,14 +81,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
{ {
UASSERT(step > 0); UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(step == 1) if(step <= 1 || (int)cloud->size() <= step)
{ {
// no sampling // no sampling
*output = *cloud; *output = *cloud;
} }
else else
{
if((int)cloud->size() > step)
{ {
int finalSize = cloud->size()/step; int finalSize = cloud->size()/step;
output->resize(finalSize); output->resize(finalSize);
@@ -106,11 +96,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
(*output)[oi++] = cloud->at(i); (*output)[oi++] = cloud->at(i);
} }
} }
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output; return output;
} }
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample( pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
@@ -119,14 +104,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
{ {
UASSERT(step > 0); UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(step == 1) if(step <= 1 || (int)cloud->size()<=step)
{ {
// no sampling // no sampling
*output = *cloud; *output = *cloud;
} }
else else
{
if((int)cloud->size() > step)
{ {
int finalSize = cloud->size()/step; int finalSize = cloud->size()/step;
output->resize(finalSize); output->resize(finalSize);
@@ -136,11 +119,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
(*output)[oi++] = cloud->at(i); (*output)[oi++] = cloud->at(i);
} }
} }
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output; return output;
} }
corelib/src/util3d_mapping.cpp
+10 -16
View File
@@ -404,12 +404,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange); UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
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 <= 0.0f)
{
scanMaxRange = 6.0f;
}
pcl::PointCloud<pcl::PointXYZ> minMax; pcl::PointCloud<pcl::PointXYZ> minMax;
if(minMapSize > 0.0f) if(minMapSize > 0.0f)
{ {
@@ -440,10 +434,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
// Added margin to make sure that all points are inside the map (when rounded to integer) // Added margin to make sure that all points are inside the map (when rounded to integer)
float margin = cellSize*10.0f; float margin = cellSize*10.0f;
xMin = (scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin; xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
yMin = (scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin; yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
float xMax = (scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin; float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
float yMax = (scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin; float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
//UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])", //UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
// xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y); // xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
@@ -493,13 +487,13 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
origin.at<float>(1) = pose.y(); origin.at<float>(1) = pose.y();
pcl::PointXYZ ptFirst = iter->second->points[0]; pcl::PointXYZ ptFirst = iter->second->points[0];
pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1]; pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1];
if(ptFirst.y > ptLast.y) //if(ptFirst.y > ptLast.y)
{ //{
// swap to iterate counterclockwise // swap to iterate counterclockwise
pcl::PointXYZ tmp = ptLast; // pcl::PointXYZ tmp = ptLast;
ptLast = ptFirst; // ptLast = ptFirst;
ptFirst = tmp; // ptFirst = tmp;
} //}
endFirst.at<float>(0) = ptFirst.x; endFirst.at<float>(0) = ptFirst.x;
endFirst.at<float>(1) = ptFirst.y; endFirst.at<float>(1) = ptFirst.y;
endLast.at<float>(0) = ptLast.x; endLast.at<float>(0) = ptLast.x;