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

Fixed PnP camera matrix empty when computing loop closure. Updated odometry nonholomic motion estimation (using arc around ICR).

Browse Source
This commit is contained in:
matlabbe
2015-06-18 01:24:19 -04:00
parent e9bb80abcc
commit 82943e85e8
3 changed files with 43 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+6 -4
View File
@@ -218,14 +218,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
if(!_holonomic)
{
float tmpY = x * tan(yaw);
// arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
y = tmpY;
}
else
{
yaw = atan(y/x);
yaw = (atan(x/y)*2.0f-CV_PI)*-1;
}
}
@@ -244,14 +245,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
}
else if(!_holonomic)
{
float tmpY = x * tan(yaw);
// arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
y = tmpY;
}
else
{
yaw = atan(y/x);
yaw = (atan(x/y)*2.0f-CV_PI)*-1;
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&