i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 18:47:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Odometry: fixed Reg/Force3DoF ignored if filters are not used

Browse Source
This commit is contained in:
matlabbe
2020-07-08 10:03:54 -04:00
parent 3505611fb5
commit 4a09c4bdcf
1 changed files with 39 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+39 -42
View File
@@ -592,50 +592,58 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw); updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
} }
} }
else if(particleFilters_.size()) else
{ {
// Particle filtering if(particleFilters_.size())
UASSERT(particleFilters_.size()==6);
if(velocityGuess_.isNull())
{ {
particleFilters_[0]->init(vx); // Particle filtering
particleFilters_[1]->init(vy); UASSERT(particleFilters_.size()==6);
particleFilters_[2]->init(vz); if(velocityGuess_.isNull())
particleFilters_[3]->init(vroll);
particleFilters_[4]->init(vpitch);
particleFilters_[5]->init(vyaw);
}
else
{
vx = particleFilters_[0]->filter(vx);
vy = particleFilters_[1]->filter(vy);
vyaw = particleFilters_[5]->filter(vyaw);
if(!_holonomic)
{ {
// arc trajectory around ICR particleFilters_[0]->init(vx);
float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f; particleFilters_[1]->init(vy);
if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0)) particleFilters_[2]->init(vz);
particleFilters_[3]->init(vroll);
particleFilters_[4]->init(vpitch);
particleFilters_[5]->init(vyaw);
}
else
{
vx = particleFilters_[0]->filter(vx);
vy = particleFilters_[1]->filter(vy);
vyaw = particleFilters_[5]->filter(vyaw);
if(!_holonomic)
{ {
vy = tmpY; // 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;
}
} }
else
if(!_force3DoF)
{ {
vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1; vz = particleFilters_[2]->filter(vz);
vroll = particleFilters_[3]->filter(vroll);
vpitch = particleFilters_[4]->filter(vpitch);
} }
} }
if(!_force3DoF) if(info)
{ {
vz = particleFilters_[2]->filter(vz); info->timeParticleFiltering = time.ticks();
vroll = particleFilters_[3]->filter(vroll);
vpitch = particleFilters_[4]->filter(vpitch);
} }
} }
else if(!_holonomic)
if(info)
{ {
info->timeParticleFiltering = time.ticks(); // arc trajectory around ICR
vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
} }
if(_force3DoF) if(_force3DoF)
@@ -645,17 +653,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
vpitch = 0.0f; vpitch = 0.0f;
} }
} }
else if(!_holonomic)
{
// arc trajectory around ICR
vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
}
if(dt) if(dt)
{ {