i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 21:40:19 +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
corelib/src/Transform.cpp
+22 -1
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