i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

60
corelib/src/util3d_filtering.cpp
View File

@@ -57,28 +57,20 @@ cv::Mat downsample(
{
UASSERT(step > 0);
cv::Mat output;
if(step == 1)
if(step <= 1 || cloud.cols <= step)
{
// no sampling
output = cloud.clone();
}
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;
output = cv::Mat(1, finalSize, cloud.type());
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
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
}
return output;
@@ -89,26 +81,19 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(step == 1)
if(step <= 1 || (int)cloud->size() <= step)
{
// no sampling
*output = *cloud;
}
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->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));
(*output)[oi++] = cloud->at(i);
}
}
return output;
@@ -119,26 +104,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(step == 1)
if(step <= 1 || (int)cloud->size()<=step)
{
// no sampling
*output = *cloud;
}
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->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));
(*output)[oi++] = cloud->at(i);
}
}
return output;

26
corelib/src/util3d_mapping.cpp
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);
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;
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)
float margin = cellSize*10.0f;
xMin = (scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
yMin = (scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
float xMax = (scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
float yMax = (scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
xMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.x?-scanMaxRange:min.x) - margin;
yMin = (unknownSpaceFilled && scanMaxRange > 0 && -scanMaxRange < min.y?-scanMaxRange:min.y) - margin;
float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + 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])",
// 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();
pcl::PointXYZ ptFirst = iter->second->points[0];
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
pcl::PointXYZ tmp = ptLast;
ptLast = ptFirst;
ptFirst = tmp;
}
// pcl::PointXYZ tmp = ptLast;
// ptLast = ptFirst;
// ptFirst = tmp;
//}
endFirst.at<float>(0) = ptFirst.x;
endFirst.at<float>(1) = ptFirst.y;
endLast.at<float>(0) = ptLast.x;