i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-08 19:17:45 +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); updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
} }
} }
else if(particleFilters_.size()) else
{
if(particleFilters_.size())
{ {
// Particle filtering // Particle filtering
UASSERT(particleFilters_.size()==6); UASSERT(particleFilters_.size()==6);
@@ -637,18 +639,13 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{ {
info->timeParticleFiltering = time.ticks(); info->timeParticleFiltering = time.ticks();
} }
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
} }
else if(!_holonomic) else if(!_holonomic)
{ {
// arc trajectory around ICR // arc trajectory around ICR
vy = 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(_force3DoF) if(_force3DoF)
{ {
vz = 0.0f; vz = 0.0f;