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

reverted modif on z when honolonomic but added modif on yaw depending on the estimated y value

Browse Source
This commit is contained in:
matlabbe
2015-06-16 10:50:49 -04:00
parent 7cd0d0cd53
commit 6a7a9fb9b0
4 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
corelib/src/Odometry.cpp
View File

@@ -213,28 +213,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
else
{
x = filters_[0]->filter(x);
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
if(_holonomic)
if(!_holonomic)
{
y = filters_[1]->filter(y);
}
else
{
y = x * tan(yaw);
float tmpY = x * tan(yaw);
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
y = tmpY;
}
else
{
yaw = atan(y/x);
}
}
if(!_force2D)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
if(_holonomic)
{
z = filters_[2]->filter(z);
}
else
{
z = x * tan(pitch);
}
}
}
@@ -245,10 +244,14 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
}
else if(!_holonomic)
{
y = x * tan(yaw);
if(!_force2D)
float tmpY = x * tan(yaw);
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
z = x * tan(pitch);
y = tmpY;
}
else
{
yaw = atan(y/x);
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&