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
+19 -41
View File
@@ -57,28 +57,20 @@ 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;
output = cv::Mat(1, finalSize, cloud.type());
int oi = 0;
for(int i=0; i<cloud.cols-step+1; i+=step)
{ {
int finalSize = cloud.cols/step; cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
output = cv::Mat(1, finalSize, cloud.type()); ++oi;
int oi = 0;
for(int i=0; i<cloud.cols-step+1; i+=step)
{
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++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;
@@ -89,26 +81,19 @@ 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;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{ {
int finalSize = cloud->size()/step; (*output)[oi++] = cloud->at(i);
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
else if(cloud->size())
{
output->push_back(cloud->at(0));
} }
} }
return output; return output;
@@ -119,26 +104,19 @@ 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;
output->resize(finalSize);
int oi = 0;
for(int i=0; i<(int)cloud->size()-step+1; i+=step)
{ {
int finalSize = cloud->size()/step; (*output)[oi++] = cloud->at(i);
output->resize(finalSize);
int oi = 0;
for(int i=0; i<(int)cloud->size()-step+1; i+=step)
{
(*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;