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

Added check to make sure input odometry poses are invertible. Source/DB: added stereo to depth option.

Browse Source
This commit is contained in:
matlabbe
2020-11-06 21:37:59 -05:00
parent 4b527f9c36
commit 7be22d1b67
7 changed files with 96 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Transform.h
View File

@@ -98,6 +98,7 @@ public:
float theta() const;
bool isInvertible() const;
Transform inverse() const;
Transform rotation() const;
Transform translation() const;

2
corelib/src/Memory.cpp
View File

@@ -886,7 +886,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
// add signature on top of the short-term memory
if(signature)
{
UDEBUG("adding %d", signature->id());
UDEBUG("adding %d (pose=%s)", signature->id(), signature->getPose().prettyPrint().c_str());
// Update neighbors
if(_stMem.size())
{

38
corelib/src/Rtabmap.cpp
View File

@@ -1071,6 +1071,43 @@ bool Rtabmap::process(
bool fakeOdom = false;
if(_rgbdSlamMode)
{
if(!odomPose.isNull())
{
// this will make sure that all inverse operations will work!
if(!odomPose.isInvertible())
{
UWARN("Input odometry is not invertible! pose = %s\n"
"[%f %f %f %f;\n"
" %f %f %f %f;\n"
" %f %f %f %f;\n"
" 0 0 0 1]\n"
"Trying to normalize rotation to see if it makes it invertible...",
odomPose.prettyPrint().c_str(),
odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
odomPose.normalizeRotation();
UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
"[%f %f %f %f;\n"
" %f %f %f %f;\n"
" %f %f %f %f;\n"
" 0 0 0 1]", odomPose.prettyPrint().c_str(),
odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str());
UWARN("Normalizing rotation succeeded! fixed pose = %s\n"
"[%f %f %f %f;\n"
" %f %f %f %f;\n"
" %f %f %f %f;\n"
" 0 0 0 1]\n"
"If the resulting rotation is very different from original one, try to fix the odometry or TF.",
odomPose.prettyPrint().c_str(),
odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
}
}
if(!_memory->isIncremental() &&
!odomPose.isNull() &&
_optimizedPoses.size() &&
@@ -1238,6 +1275,7 @@ bool Rtabmap::process(
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
}
}
}

23
corelib/src/Transform.cpp
View File

@@ -166,9 +166,30 @@ float Transform::theta() const
return yaw;
}
bool Transform::isInvertible() const
{
bool invertible = false;
Eigen::Matrix4f inverse;
Eigen::Matrix4f::RealScalar det;
toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
return invertible;
}
Transform Transform::inverse() const
{
return fromEigen4f(toEigen4f().inverse());
bool invertible = false;
Eigen::Matrix4f inverse;
Eigen::Matrix4f::RealScalar det;
toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
UASSERT_MSG(invertible, uFormat("This transform is not invertible! %s \n"
"[%f %f %f %f;\n"
" %f %f %f %f;\n"
" %f %f %f %f;\n"
" 0 0 0 1]", prettyPrint().c_str(),
r11(), r12(), r13(), o14(),
r21(), r22(), r23(), o24(),
r31(), r32(), r33(), o34()).c_str());
return fromEigen4f(inverse);
}
Transform Transform::rotation() const