i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

F2F nonholonomic bad transforms fixed (https://github.com/introlab/rtabmap_ros/issues/74)

Browse Source
This commit is contained in:
matlabbe
2016-05-17 18:10:24 -04:00
parent 0ae131108d
commit a6bad6d2a5
3 changed files with 15 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+1 -9
View File
@@ -401,15 +401,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
else if(!_holonomic)
{
// 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;
}
vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(_force3DoF)
{
vz = 0.0f;