i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +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
corelib/src/OdometryF2F.cpp
+7 -3
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",