i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +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
+5 -8
View File
@@ -592,7 +592,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
}
}
else if(particleFilters_.size())
else
{
if(particleFilters_.size())
{
// Particle filtering
UASSERT(particleFilters_.size()==6);
@@ -637,18 +639,13 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
info->timeParticleFiltering = time.ticks();
}
if(_force3DoF)
{
vz = 0.0f;
vroll = 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;