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

Added RGBD/ProximityGlobalScanMap for proximity detection using the whole scan map in localization mode. Added Icp RMS statistics (CCCorelib). Updated how localization is corrected with gravity when available (simply use roll and pitch of current gravity constraint).

Browse Source
This commit is contained in:
matlabbe
2021-06-23 17:13:07 -04:00
parent dddf6378ae
commit 11e9837b82
18 changed files with 522 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
guilib/src/MainWindow.cpp
View File

@@ -604,6 +604,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
_ui->statsToolBox->updateStat("Odometry/ICPRMS/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -1441,33 +1442,32 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
}
}
if( _preferencesDialog->isIMUGravityShown(1) &&
(data->imu().orientation().val[0]!=0 ||
data->imu().orientation().val[1]!=0 ||
data->imu().orientation().val[2]!=0 ||
data->imu().orientation().val[3]!=0))
{
Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
Transform orientation(0,0,0, data->imu().orientation()[0], data->imu().orientation()[1], data->imu().orientation()[2], data->imu().orientation()[3]);
gravity = (orientation* data->imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
_cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
filteredGravityUpdated = true;
}
if( _preferencesDialog->isIMUAccShown() &&
(data->imu().linearAcceleration().val[0]!=0 ||
data->imu().linearAcceleration().val[1]!=0 ||
data->imu().linearAcceleration().val[2]!=0))
{
Eigen::Vector3f gravity(
-data->imu().linearAcceleration().val[0],
-data->imu().linearAcceleration().val[1],
-data->imu().linearAcceleration().val[2]);
gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
gravity = data->imu().localTransform().toEigen3f()*gravity;
_cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
accelerationUpdated = true;
}
}
if( _preferencesDialog->isIMUGravityShown(1) &&
(data->imu().orientation().val[0]!=0 ||
data->imu().orientation().val[1]!=0 ||
data->imu().orientation().val[2]!=0 ||
data->imu().orientation().val[3]!=0))
{
Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
Transform orientation(0,0,0, data->imu().orientation()[0], data->imu().orientation()[1], data->imu().orientation()[2], data->imu().orientation()[3]);
gravity = (orientation* data->imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
_cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
filteredGravityUpdated = true;
}
if( _preferencesDialog->isIMUAccShown() &&
(data->imu().linearAcceleration().val[0]!=0 ||
data->imu().linearAcceleration().val[1]!=0 ||
data->imu().linearAcceleration().val[2]!=0))
{
Eigen::Vector3f gravity(
-data->imu().linearAcceleration().val[0],
-data->imu().linearAcceleration().val[1],
-data->imu().linearAcceleration().val[2]);
gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
gravity = data->imu().localTransform().toEigen3f()*gravity;
_cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
accelerationUpdated = true;
}
}
if(!dataIgnored)
@@ -1713,6 +1713,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());

1
guilib/src/PreferencesDialog.cpp
View File

@@ -906,6 +906,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_createMapLabels->setObjectName(Parameters::kMemMapLabelsAdded().c_str());
_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
_ui->checkBox_localSpaceCreateGlobalScanMap->setObjectName(Parameters::kRGBDProximityGlobalScanMap().c_str());
_ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str());
_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());

192
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-132</y>
<width>686</width>
<height>3905</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>16</number>
<number>13</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -11334,26 +11334,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
<item>
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<item row="1" column="1">
<widget class="QLabel" name="label_space3_4">
<property name="text">
<string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
<string>Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11363,10 +11347,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_space3_3">
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_13">
<property name="text">
<string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
<string>Use odometry as motion guess for one-to-one proximity detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11376,13 +11360,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="localDetection_maxDiffID">
<property name="maximum">
@@ -11393,52 +11370,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space3_2">
<property name="text">
<string>Maximum graph depth between the current/last loop closure location and the proximity hypotheses. Set 0 to ignore.</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="1">
<widget class="QLabel" name="label_scanMatching_6">
<property name="text">
<string>Save scan matching IDs from one-to-many proximity detection in link's user data.</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="checkBox_localSpaceScanMatchingIDsSaved">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching_8">
<property name="text">
<string>Maximum angle (degrees) for one-to-one proximity detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="localDetection_angle">
<property name="suffix">
@@ -11455,10 +11386,30 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space3_4">
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
<property name="text">
<string>Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.</string>
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_space3_3">
<property name="text">
<string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space3_2">
<property name="text">
<string>Maximum graph depth between the current/last loop closure location and the proximity hypotheses. Set 0 to ignore.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11478,6 +11429,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<property name="text">
<string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space3_8">
<property name="text">
@@ -11491,6 +11455,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="localDetection_maxNeighbors">
<property name="maximum">
@@ -11501,10 +11481,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_13">
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
<property name="text">
<string>Use odometry as motion guess for one-to-one proximity detection.</string>
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_scanMatching_6">
<property name="text">
<string>Save scan matching IDs from one-to-many proximity detection in link's user data.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11514,8 +11501,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching_8">
<property name="text">
<string>Maximum angle (degrees) for one-to-one proximity detection.</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="checkBox_localSpaceScanMatchingIDsSaved">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_scanMatching_15">
<property name="text">
<string>Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode, otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceCreateGlobalScanMap">
<property name="text">
<string/>
</property>
@@ -13315,7 +13335,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<number>4</number>
</property>
<property name="minimum">
<double>-1.0</double>
<double>-1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>