i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 13:00:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed #275. Removed Grid/Scan2dMaxFilledRange parameter (use Grid/RangeMax instead).

Browse Source
This commit is contained in:
matlabbe
2018-04-14 16:48:29 -04:00
parent 344dc165bc
commit bfce5cceb5
8 changed files with 74 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OccupancyGrid.cpp
+13 -6
View File
@@ -68,7 +68,6 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
rayTracing_(Parameters::defaultGridRayTracing()),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
minMapSize_(Parameters::defaultGridGlobalMinSize()),
@@ -125,7 +124,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
@@ -259,11 +257,20 @@ void OccupancyGrid::createLocalMap(
node.sensorData().laserScanRaw().localTransform().z());
LaserScan scan = node.sensorData().laserScanRaw();
if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
if(cloudMinDepth_ > 0.0f)
{
scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f);
}
float maxRange = cloudMaxDepth_;
if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
{
maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
}
else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
{
maxRange = node.sensorData().laserScanRaw().maxRange();
}
util3d::occupancy2DFromLaserScan(
util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
cv::Mat(),
@@ -272,7 +279,7 @@ void OccupancyGrid::createLocalMap(
obstacleCells,
cellSize_,
scan2dUnknownSpaceFilled_,
node.sensorData().laserScanRaw().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanRaw().maxRange());
maxRange);
UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
}
@@ -533,7 +540,7 @@ void OccupancyGrid::createLocalMap(
obstacleCells,
cellSize_,
false, // don't fill unknown space
0);
cloudMaxDepth_);
}
}
UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
corelib/src/Parameters.cpp
+3
View File
@@ -226,6 +226,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.17.0
removedParameters_.insert(std::make_pair("Grid/Scan2dMaxFilledRange", std::make_pair(false, Parameters::kGridRangeMax())));
// 0.16.0
removedParameters_.insert(std::make_pair("Grid/ProjRayTracing", std::make_pair(true, Parameters::kGridRayTracing())));
removedParameters_.insert(std::make_pair("Grid/DepthMin", std::make_pair(true, Parameters::kGridRangeMin())));
corelib/src/util3d_mapping.cpp
+46 -10
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
@@ -133,7 +134,14 @@ void occupancy2DFromLaserScan(
}
// copy directly obstacles precise positions
occupied = scanHit.clone();
if(scanMaxRange > cellSize)
{
occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
}
else
{
occupied = scanHit.clone();
}
}
/**
@@ -628,6 +636,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
int j=0;
float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
const Transform & pose = poses.at(iter->first);
@@ -638,15 +647,20 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
cv::Point2f startf(pose.x()+viewpoint.x, pose.y()+viewpoint.y);
// Set obstacles first
for(int i=0; i<iter->second.first.cols; ++i)
{
const float * ptr = iter->second.first.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
if(end!=start)
bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]+cellSize, ptr[1]+cellSize) > scanMaxRangeSqr;
if(!ignore)
{
map.at<char>(end.y, end.x) = 100; // obstacle
cv::Point2i end((ptr[0]+startf.x-xMin)/cellSize, (ptr[1]+startf.y-yMin)/cellSize);
if(end!=start)
{
map.at<char>(end.y, end.x) = 100; // obstacle
}
}
}
@@ -654,7 +668,18 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
for(int i=0; i<iter->second.first.cols; ++i)
{
const float * ptr = iter->second.first.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
cv::Vec2f v(ptr[0], ptr[1]);
if(scanMaxRange>cellSize)
{
float n = cv::norm(v);
if(n > scanMaxRange+cellSize)
{
v = (v/n) * scanMaxRange;
}
}
cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -667,7 +692,18 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
for(int i=0; i<iter->second.second.cols; ++i)
{
const float * ptr = iter->second.second.ptr<float>(0, i);
cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
cv::Vec2f v(ptr[0], ptr[1]);
if(scanMaxRange>cellSize)
{
float n = cv::norm(v);
if(n > scanMaxRange+cellSize)
{
v = (v/n) * scanMaxRange;
}
}
cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
if(end!=start)
{
if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -712,10 +748,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
origin.at<float>(0) = pose.x()+viewpoint.x;
origin.at<float>(1) = pose.y()+viewpoint.y;
endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0]+origin.at<float>(0);
endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1]+origin.at<float>(1);
endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0]+origin.at<float>(0);
endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1]+origin.at<float>(1);
//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));