i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-13 23:10:20 +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/include/rtabmap/core/Transform.h
+1
View File
@@ -98,6 +98,7 @@ public:
float theta() const;
bool isInvertible() const;
Transform inverse() const;
Transform rotation() const;
Transform translation() const;
corelib/src/Memory.cpp
+1 -1
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())
{
corelib/src/Rtabmap.cpp
+38
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);
}
}
}
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
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -249,6 +249,7 @@ public:
QString getSourceDevice() const;
bool isSourceDatabaseStampsUsed() const;
bool isSourceDatabaseStereoToDepth() const;
bool isSourceRGBDColorOnly() const;
int getIMUFilteringStrategy() const;
bool isDepthFilteringAvailable() const;
guilib/src/PreferencesDialog.cpp
+2
View File
@@ -645,6 +645,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));
//openni group
_ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex());
guilib/src/ui/preferencesDialog.ui
+31 -11
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-27</y>
<y>-498</y>
<width>686</width>
<height>3236</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>0</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -5877,6 +5877,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QToolButton" name="toolButton_dbViewer">
<property name="toolTip">
@@ -5891,13 +5898,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="source_database_toolButton_selectSource">
<property name="text">
@@ -5908,7 +5908,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="0" column="1">
<widget class="QLineEdit" name="source_database_lineEdit_path"/>
</item>
<item row="8" column="1">
<item row="9" column="1">
<spacer name="verticalSpacer_35">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -6040,6 +6040,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_557">
<property name="text">
<string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>