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

Fixed not invertible error ofter wrong graph optimization (seen on localization mode, gtsam returning inf poses)

Browse Source
This commit is contained in:
matlabbe
2023-05-22 17:56:59 -07:00
parent 263eb6fbde
commit ae3fda37a9
1 changed files with 27 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/src/Graph.cpp
View File

@@ -917,6 +917,33 @@ void computeMaxGraphErrors(
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
if( t1.isNull() ||
t2.isNull() ||
!t1.isInvertible() ||
!t2.isInvertible())
{
UWARN("Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)",
iter->second.from(),
t1.prettyPrint().c_str(),
iter->second.to(),
t2.prettyPrint().c_str());
if(maxLinearErrorLink)
{
*maxLinearErrorLink = 0;
}
if(maxAngularErrorLink)
{
*maxAngularErrorLink = 0;
}
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
return;
}
Transform t = t1.inverse()*t2;
float linearError = uMax3(