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

OdomF2M: added support to laser scan

Browse Source
This commit is contained in:
matlabbe
2016-03-06 15:11:09 -05:00
parent eefd557ab4
commit 7a1cf84b08
22 changed files with 696 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OdometryF2F.cpp
+5 -1
View File
@@ -39,11 +39,14 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
}
OdometryF2F::~OdometryF2F()
@@ -136,7 +139,8 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))
if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size();