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

1
corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -119,7 +119,6 @@ private:
float noiseFilteringRadius_;
int noiseFilteringMinNeighbors_;
bool scan2dUnknownSpaceFilled_;
double scan2dMaxUnknownSpaceFilledRange_;
bool rayTracing_;
bool fullUpdate_;
float minMapSize_;

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -630,8 +630,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, GroundIsObstacle, bool, false, uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "Noise filtering radius (0=disabled). Done after segmentation.");
RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors.");
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, "Unknown space filled. Only used with 2D laser scans.");
RTABMAP_PARAM(Grid, Scan2dMaxFilledRange, float, 4.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
RTABMAP_PARAM(Grid, RayTracing, bool, false, uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
RTABMAP_PARAM(GridGlobal, FullUpdate, bool, true, "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");

19
corelib/src/OccupancyGrid.cpp
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());

3
corelib/src/Parameters.cpp
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())));

56
corelib/src/util3d_mapping.cpp
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));