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

Updated how odometry guess is done (adapt to time between timestamps) and Kalman filtering (using velocity)

Browse Source
This commit is contained in:
matlabbe
2016-02-27 18:30:00 -05:00
parent a8d1b91083
commit 6d06b66496
16 changed files with 311 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OdometryF2F.cpp
+3 -3
View File
@@ -37,12 +37,10 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
@@ -60,6 +58,7 @@ void OdometryF2F::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
UTimer timer;
@@ -84,7 +83,7 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_&&!this->previousTransform().isNull()?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
&regInfo);
if(info && this->isInfoDataFilled())
@@ -188,6 +187,7 @@ Transform OdometryF2F::computeTransform(
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
info->features = refFrame_.sensorData().keypoints().size();
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",