i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.18.2: Added Odom/GuessSmoothingDelay parameter

Browse Source
This commit is contained in:
matlabbe
2018-11-21 11:57:04 -05:00
parent 5ae2f487b4
commit b862d6bc48
11 changed files with 184 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
corelib/src/Odometry.cpp
View File

@@ -101,6 +101,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_force3DoF(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -125,6 +126,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -182,7 +184,8 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose)
{
UASSERT(!initialPose.isNull());
previousVelocityTransform_.setNull();
previousVelocities_.clear();
velocityGuess_.setNull();
previousGroundTruthPose_.setNull();
_resetCurrentCount = 0;
previousStamp_ = 0;
@@ -232,6 +235,37 @@ void Odometry::reset(const Transform & initialPose)
}
}
const Transform & Odometry::previousVelocityTransform() const
{
return getVelocityGuess();
}
Transform getMeanVelocity(const std::list<std::pair<std::vector<float>, double> > & transforms)
{
if(transforms.size())
{
float tvx=0.0f,tvy=0.0f,tvz=0.0f, tvroll=0.0f,tvpitch=0.0f,tvyaw=0.0f;
for(std::list<std::pair<std::vector<float>, double> >::const_iterator iter=transforms.begin(); iter!=transforms.end(); ++iter)
{
UASSERT(iter->first.size() == 6);
tvx+=iter->first[0];
tvy+=iter->first[1];
tvz+=iter->first[2];
tvroll+=iter->first[3];
tvpitch+=iter->first[4];
tvyaw+=iter->first[5];
}
tvx/=float(transforms.size());
tvy/=float(transforms.size());
tvz/=float(transforms.size());
tvroll/=float(transforms.size());
tvpitch/=float(transforms.size());
tvyaw/=float(transforms.size());
return Transform(tvx, tvy, tvz, tvroll, tvpitch, tvyaw);
}
return Transform();
}
Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
return process(data, Transform(), info);
@@ -313,21 +347,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
// KITTI datasets start with stamp=0
double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
{
if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
{
UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
}
else if(_filteringStrategy==1)
{
UERROR("Kalman filtering is enabled but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
UERROR("Kalman filtering is enabled but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
}
dt=0;
previousVelocityTransform_.setNull();
previousVelocities_.clear();
velocityGuess_.setNull();
}
if(!previousVelocityTransform_.isNull())
if(!velocityGuess_.isNull())
{
if(guessFromMotion_)
{
@@ -341,7 +376,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
else
{
float vx,vy,vz, vroll,vpitch,vyaw;
previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
}
@@ -460,7 +495,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
if(_filteringStrategy == 1)
{
if(previousVelocityTransform_.isNull())
if(velocityGuess_.isNull())
{
// reset Kalman
if(dt)
@@ -482,7 +517,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
// Particle filtering
UASSERT(particleFilters_.size()==6);
if(previousVelocityTransform_.isNull())
if(velocityGuess_.isNull())
{
particleFilters_[0]->init(vx);
particleFilters_[1]->init(vy);
@@ -564,17 +599,43 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
previousStamp_ = data.stamp();
previousVelocityTransform_.setNull();
if(dt)
{
previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
if(dt >=guessSmoothingDelay_/2.0 || particleFilters_.size() || _filteringStrategy==1)
{
velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
previousVelocities_.clear();
}
else
{
// smooth velocity estimation over the past X seconds
std::vector<float> v(6);
v[0] = vx;
v[1] = vy;
v[2] = vz;
v[3] = vroll;
v[4] = vpitch;
v[5] = vyaw;
previousVelocities_.push_back(std::make_pair(v, data.stamp()));
while(previousVelocities_.size() > 1 && previousVelocities_.front().second < previousVelocities_.back().second-guessSmoothingDelay_)
{
previousVelocities_.pop_front();
}
velocityGuess_ = getMeanVelocity(previousVelocities_);
}
}
else
{
previousVelocities_.clear();
velocityGuess_.setNull();
}
if(info)
{
distanceTravelled_ += t.getNorm();
info->distanceTravelled = distanceTravelled_;
info->guessVelocity = velocityGuess_;
}
++framesProcessed_;
@@ -592,7 +653,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
previousVelocityTransform_.setNull();
previousVelocities_.clear();
velocityGuess_.setNull();
previousStamp_ = 0;
return Transform();

9
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -466,7 +466,14 @@ Transform OdometryF2M::computeTransform(
}
if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
if(guess.isNull())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Registration failed: \"%s\" (guess=%s)", regInfo.rejectedMsg.c_str(), guess.prettyPrint().c_str());
}
}
else
{