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

rtabmap:

Browse Source 
-Fixed map id not incremented on Odometry reset (identity transform was missed)
-Keeping maximum odometry variance between two loop closure updates
Odometry:
-removed parameters Odom/FeaturesRatio, Odom/LinearUpdate, Odom/AngularUpdate
-added parameter Odom/FillInfoData
-expended OdometryInfo class with features stuff
Gui:
-show inliers/outliers features in Odometry view
This commit is contained in:
Mathieu Labbe
2014-12-18 16:54:46 -05:00
parent 744e2fb3c7
commit 86a6fe202c
17 changed files with 325 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+9 -19
View File
@@ -771,30 +771,20 @@ bool Rtabmap::process(const SensorData & data)
if(_memory->getLastWorkingSignature())
{
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
if(!lastPose.isIdentity() && data.pose().isIdentity())
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
else
{
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
}
}
}
}