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

F2F nonholonomic bad transforms fixed (https://github.com/introlab/rtabmap_ros/issues/74)

Browse Source
This commit is contained in:
matlabbe
2016-05-17 18:08:20 -04:00
parent 0ae131108d
commit a6bad6d2a5
3 changed files with 15 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/OdometryF2F.h
View File

@@ -55,7 +55,7 @@ private:
Registration * registrationPipeline_; Registration * registrationPipeline_;
Signature refFrame_; Signature refFrame_;
Transform motionSinceLastKeyFrame_; Transform lastKeyFramePose_;
}; };
} }

10
corelib/src/Odometry.cpp
View File

@@ -401,15 +401,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
else if(!_holonomic) else if(!_holonomic)
{ {
// arc trajectory around ICR // arc trajectory around ICR
float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f; vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
{
vy = tmpY;
}
else
{
vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
}
if(_force3DoF) if(_force3DoF)
{ {
vz = 0.0f; vz = 0.0f;

21
corelib/src/OdometryF2F.cpp
View File

@@ -39,8 +39,7 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) : OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters), Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()), keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()), scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
motionSinceLastKeyFrame_(Transform::getIdentity())
{ {
registrationPipeline_ = Registration::create(parameters); registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_); Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
@@ -58,7 +57,7 @@ void OdometryF2F::reset(const Transform & initialPose)
{ {
Odometry::reset(initialPose); Odometry::reset(initialPose);
refFrame_ = Signature(); refFrame_ = Signature();
motionSinceLastKeyFrame_.setIdentity(); lastKeyFramePose_.setNull();
} }
// return not null transform if odometry is correctly computed // return not null transform if odometry is correctly computed
@@ -83,6 +82,13 @@ Transform OdometryF2F::computeTransform(
RegistrationInfo regInfo; RegistrationInfo regInfo;
UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull())
{
lastKeyFramePose_ = this->getPose(); // reset to current pose
}
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
Signature newFrame(data); Signature newFrame(data);
if(refFrame_.sensorData().isValid()) if(refFrame_.sensorData().isValid())
{ {
@@ -90,7 +96,7 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod( output = registrationPipeline_->computeTransformationMod(
tmpRefFrame, tmpRefFrame,
newFrame, newFrame,
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(), !guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
&regInfo); &regInfo);
if(info && this->isInfoDataFilled()) if(info && this->isInfoDataFilled())
@@ -117,7 +123,7 @@ Transform OdometryF2F::computeTransform(
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]); info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
} }
Transform t = this->getPose()*motionSinceLastKeyFrame_.inverse(); Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter) for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
{ {
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t))); info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
@@ -135,8 +141,7 @@ Transform OdometryF2F::computeTransform(
if(!output.isNull()) if(!output.isNull())
{ {
output = motionSinceLastKeyFrame_.inverse() * output; output = motionSinceLastKeyFrame.inverse() * output;
motionSinceLastKeyFrame_ *= output;
// new key-frame? // new key-frame?
if( (registrationPipeline_->isImageRequired() && (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()))) ||
@@ -167,7 +172,7 @@ Transform OdometryF2F::computeTransform(
refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>()); refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
//reset motion //reset motion
motionSinceLastKeyFrame_.setIdentity(); lastKeyFramePose_.setNull();
} }
else else
{ {