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

Odometry: 2D scans + normals support

Browse Source
This commit is contained in:
matlabbe
2017-08-31 18:01:46 -04:00
parent 7ec58c63e9
commit 4f55b56d6b
7 changed files with 121 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Odometry.cpp
View File

@@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_pose(Transform::getIdentity()),
_resetCurrentCount(0),
previousStamp_(0),
distanceTravelled_(0)
distanceTravelled_(0),
framesProcessed_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose)
_resetCurrentCount = 0;
previousStamp_ = 0;
distanceTravelled_ = 0;
framesProcessed_ = 0;
if(_force3DoF || particleFilters_.size())
{
float x,y,z, roll,pitch,yaw;
@@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
distanceTravelled_ += t.getNorm();
info->distanceTravelled = distanceTravelled_;
}
++framesProcessed_;
return _pose *= t; // update
}

10
corelib/src/OdometryF2F.cpp
View File

@@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform(
return output;
}
bool addKeyFrame = false;
RegistrationInfo regInfo;
UASSERT(!this->getPose().isNull());
@@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
// special case for ICP-only odom, set guess to identity if we just started
!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
// special case for ICP-only odom, set guess to identity if we just started or reset
!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
&regInfo);
if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
@@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform(
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size();
if(features == 0)
if(registrationPipeline_->isImageRequired() && features == 0)
{
newFrame = Signature(data);
// this will generate features only for the first frame or if optical flow was used (no 3d words)
@@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform(
//reset motion
lastKeyFramePose_.setNull();
addKeyFrame = true;
}
else
{
@@ -249,6 +252,7 @@ Transform OdometryF2F::computeTransform(
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
info->features = newFrame.sensorData().keypoints().size();
info->keyFrameAdded = addKeyFrame;
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

59
corelib/src/OdometryF2M.cpp
View File

@@ -186,9 +186,10 @@ Transform OdometryF2M::computeTransform(
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
// special case for ICP-only odom, set guess to identity if we just started
!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
// special case for ICP-only odom, set guess to identity if we just started or reset
!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
{
tmpMap = *map_;
@@ -591,7 +592,7 @@ Transform OdometryF2M::computeTransform(
if(lastFrame_->sensorData().laserScanRaw().cols)
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -623,17 +624,6 @@ Transform OdometryF2M::computeTransform(
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_)
{
@@ -691,7 +681,16 @@ Transform OdometryF2M::computeTransform(
*mapCloudNormals += *scansBuffer_.back().first;
}
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
if(mapScan.channels() == 2 || mapScan.channels() == 5)
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint);
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint);
}
modified=true;
}
}
@@ -702,7 +701,18 @@ Transform OdometryF2M::computeTransform(
{
*map_ = tmpMap;
map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
if(mapScan.channels() == 2 || mapScan.channels() == 5)
{
map_->sensorData().setLaserScanRaw(mapScan,
LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
}
else
{
map_->sensorData().setLaserScanRaw(mapScan,
LaserScanInfo(0, 0.0f, newFramePose.translation()));
}
map_->setWords(mapWords);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
@@ -717,7 +727,7 @@ Transform OdometryF2M::computeTransform(
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(tmpMap.getWords3());
info->localScanMap = tmpMap.sensorData().laserScanRaw();
info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform());
}
}
}
@@ -831,7 +841,18 @@ Transform OdometryF2M::computeTransform(
frameValid = true;
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5)
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
LaserScanInfo(0, 0.0f, newFramePose.translation()));
}
addKeyFrame = true;
}
else
@@ -854,7 +875,7 @@ Transform OdometryF2M::computeTransform(
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(map_->getWords3());
info->localScanMap = map_->sensorData().laserScanRaw();
info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform());
}
}
}

82
corelib/src/RegistrationIcp.cpp
View File

@@ -505,18 +505,24 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
if(_voxelSize > 0.0f)
{
int pointsBeforeFiltering = fromCloudFiltered->size();
float pointsBeforeFiltering = (float)fromCloudFiltered->size();
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
pointsBeforeFiltering = toCloudFiltered->size();
pointsBeforeFiltering = (float)toCloudFiltered->size();
toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s",
UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
_voxelSize,
float(fromCloudFiltered->size()) / float(pointsBeforeFiltering),
float(toCloudFiltered->size()) / float(pointsBeforeFiltering),
ratioFrom,
(int)fromCloudFiltered->size(),
maxLaserScansFrom,
ratioTo,
(int)toCloudFiltered->size(),
maxLaserScansTo,
timer.ticks());
}
@@ -531,11 +537,22 @@ Transform RegistrationIcp::computeTransformationImpl(
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
normals = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
if(_voxelSize > 0.0f)
{
normals = util3d::computeNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
}
else
{
@@ -546,11 +563,22 @@ Transform RegistrationIcp::computeTransformationImpl(
if(toScan.channels() == 2 || toScan.channels() == 5)
{
normals = util3d::computeFastOrganizedNormals2D(
toCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointTo);
if(_voxelSize > 0.0f)
{
normals = util3d::computeNormals2D(
toCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointTo);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(
toCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointTo);
}
}
else
{
@@ -564,7 +592,7 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
if(fromScan.channels() == 2 || toScan.channels() == 5)
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
@@ -654,8 +682,22 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_voxelSize > 0.0f)
{
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
}
#ifdef RTABMAP_POINTMATCHER

8
corelib/src/util3d_registration.cpp
View File

@@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloudB);
est->setInputSource(cloudA);
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
@@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloudB);
est->setInputSource(cloudA);
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);