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

OdometryF2M: updated how local scan map is updated

Browse Source
This commit is contained in:
matlabbe
2016-06-25 13:18:09 -04:00
parent ca95c9de97
commit 517bfa5272
7 changed files with 138 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

149
corelib/src/OdometryF2M.cpp
View File

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/OdometryF2M.h>
#include <pcl/common/io.h>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
@@ -60,7 +61,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubstractRadius_(Parameters::defaultOdomF2MScanSubstractRadius()),
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regPipeline_(Registration::create(parameters)),
map_(new Signature(-1)),
@@ -72,7 +73,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubstractRadius(), scanSubstractRadius_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
@@ -327,55 +328,115 @@ Transform OdometryF2M::computeTransform(
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
{
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
if(mapCloudNormals->size() && scanSubstractRadius_ > 0.0f)
UTimer tmpTimer;
if(lastFrame_->sensorData().laserScanRaw().cols)
{
frameCloudNormals = util3d::subtractFiltering(frameCloudNormals, mapCloudNormals, scanSubstractRadius_, 0.0f);
}
if(frameCloudNormals->size())
{
scansBuffer_.insert(std::make_pair(lastFrame_->id(), frameCloudNormals));
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d maxPoints=%d", (int)scansBuffer_.size(), int(mapCloudNormals->size() + frameCloudNormals->size()), scanMaximumMapSize_);
if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + frameCloudNormals->size()) > scanMaximumMapSize_)
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
{
//asssemble
mapCloudNormals->clear();
std::list<int> toRemove;
for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::reverse_iterator iter=scansBuffer_.rbegin();
iter!=scansBuffer_.rend();
++iter)
{
if(mapCloudNormals->empty())
{
*mapCloudNormals = *iter->second;
}
else if((int)mapCloudNormals->size() < scanMaximumMapSize_)
{
*mapCloudNormals += *iter->second;
}
else
{
toRemove.push_back(iter->first);
}
}
for(std::list<int>::iterator iter=toRemove.begin(); iter!=toRemove.end(); ++iter)
{
scansBuffer_.erase(*iter);
}
frameCloudNormalsIndices = util3d::subtractFiltering(
frameCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
mapCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
scanSubtractRadius_,
0.0f);
newPoints = frameCloudNormalsIndices->size();
}
else
{
//assemble
*mapCloudNormals += *frameCloudNormals;
newPoints = mapCloudNormals->size();
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
modified=true;
if(newPoints)
{
scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
(int)scansBuffer_.size(),
int(mapCloudNormals->size()),
newPoints,
scanMaximumMapSize_);
if(newPoints < 20)
{
UWARN("The number of new scan points added to local odometry "
"map is low (%d), you may want to decrease the parameter \"%s\" "
"(current value=%f and ICP inliers ratio is %f)",
newPoints,
Parameters::kOdomScanKeyFrameThr().c_str(),
scanKeyFrameThr_,
regInfo.icpInliersRatio);
}
if(scansBuffer_.size() > 1 &&
int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
{
//regenerate the local map
mapCloudNormals->clear();
std::list<int> toRemove;
int i = int(scansBuffer_.size())-1;
for(; i>=0; --i)
{
int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
i == 0)
{
*mapCloudNormals += *scansBuffer_[i].first;
break;
}
else
{
if(scansBuffer_[i].second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_[i].first;
}
}
}
// remove old clouds
if(i > 0)
{
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
int oi = 0;
for(; i<(int)scansBuffer_.size(); ++i)
{
UASSERT(oi < (int)scansTmp.size());
scansTmp[oi++] = scansBuffer_[i];
}
scansBuffer_ = scansTmp;
}
}
else
{
// just append the last cloud
if(scansBuffer_.back().second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_.back().first;
}
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
modified=true;
}
}
UDEBUG("Update local map = %fs", tmpTimer.ticks());
}
if(modified)
@@ -473,13 +534,13 @@ Transform OdometryF2M::computeTransform(
if (fixedMapPath_.empty())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0, 0);
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
}
}
else
{
UWARN("Mising scan to initialize odometry.");
UWARN("Missing scan to initialize odometry.");
}
}

1
corelib/src/Parameters.cpp
View File

@@ -221,6 +221,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// 0.11.8
removedParameters_.insert(std::make_pair("Reg/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("OdomF2M/ScanSubstractRadius", std::make_pair(true, Parameters::kOdomF2MScanSubtractRadius())));
// 0.11.6
removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false, "")));

30
corelib/src/RegistrationIcp.cpp
View File

@@ -113,14 +113,16 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
// ICP with guess transform
int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
int maxLaserScansTo = dataTo.laserScanMaxPts();
int maxLaserScansFrom = dataFrom.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
if(_downsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _downsamplingStep);
toScan = util3d::downsample(toScan, _downsamplingStep);
maxLaserScans/=_downsamplingStep;
maxLaserScansTo/=_downsamplingStep;
maxLaserScansFrom/=_downsamplingStep;
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
}
@@ -140,6 +142,7 @@ Transform RegistrationIcp::computeTransformationImpl(
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
@@ -186,14 +189,19 @@ Transform RegistrationIcp::computeTransformationImpl(
bool filtered = false;
if(_voxelSize > 0.0f)
{
int pointsBeforeFiltering = fromCloudFiltered->size();
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
int pointsBeforeFiltering = toCloudFiltered->size();
maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
pointsBeforeFiltering = toCloudFiltered->size();
toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
filtered = true;
UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
//Adjust maxLaserScans
maxLaserScans = maxLaserScans * toCloudFiltered->size() / pointsBeforeFiltering;
}
bool correspondencesComputed = false;
@@ -214,6 +222,10 @@ Transform RegistrationIcp::computeTransformationImpl(
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
UDEBUG("Compute normals time = %f s", timer.ticks());
if(toCloudNormals->size() && fromCloudNormals->size())
@@ -244,6 +256,13 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else // ICP Point to Point
{
if(_voxelSize > 0.0f)
{
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
}
icpT = util3d::icp(
fromCloudFiltered,
toCloudFiltered,
@@ -310,6 +329,7 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
// verify if there are enough correspondences
int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
if(maxLaserScans)
{
correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -331,7 +351,7 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged?"true":"false",
variance,
correspondences,
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
correspondencesRatio*100.0f);
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform