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

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_;
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)
{
// arc trajectory around ICR
float tmpY = 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;
}
vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(_force3DoF)
{
vz = 0.0f;

21
corelib/src/OdometryF2F.cpp
View File

@@ -39,8 +39,7 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
motionSinceLastKeyFrame_(Transform::getIdentity())
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
@@ -58,7 +57,7 @@ void OdometryF2F::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = Signature();
motionSinceLastKeyFrame_.setIdentity();
lastKeyFramePose_.setNull();
}
// return not null transform if odometry is correctly computed
@@ -83,6 +82,13 @@ Transform OdometryF2F::computeTransform(
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);
if(refFrame_.sensorData().isValid())
{
@@ -90,7 +96,7 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
&regInfo);
if(info && this->isInfoDataFilled())
@@ -117,7 +123,7 @@ Transform OdometryF2F::computeTransform(
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)
{
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
@@ -135,8 +141,7 @@ Transform OdometryF2F::computeTransform(
if(!output.isNull())
{
output = motionSinceLastKeyFrame_.inverse() * output;
motionSinceLastKeyFrame_ *= output;
output = motionSinceLastKeyFrame.inverse() * output;
// new key-frame?
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>());
//reset motion
motionSinceLastKeyFrame_.setIdentity();
lastKeyFramePose_.setNull();
}
else
{