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 parameter "IcpPointToPlaneMaxComplexity". Added util3d::computeNormalsComplexity(). OdomInfo has now RegistrationInfo field to avoid duplicating members.

Browse Source
This commit is contained in:
matlabbe
2017-09-11 13:17:53 -04:00
parent 380fc2cbde
commit aca005c287
25 changed files with 581 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
guilib/src/MainWindow.cpp
View File

@@ -560,6 +560,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -812,7 +815,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
{
OdometryInfo odomInfo;
odomInfo.covariance = cameraEvent->info().odomCovariance;
odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
if (!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
@@ -927,11 +930,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
pose = _lastOdomPose;
lost = true;
}
else if(odom.info().inliers>0 &&
else if(odom.info().reg.inliers>0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -1202,9 +1205,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(_preferencesDialog->isOdomOnlyInliersShown())
{
std::multimap<int, cv::KeyPoint> kpInliers;
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
}
_ui->imageView_odometry->setFeatures(
kpInliers,
@@ -1271,13 +1274,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
_ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
_ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
}
}
}
@@ -1323,15 +1326,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
//Process info
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.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()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());

16
guilib/src/OdometryViewer.cpp
View File

@@ -179,7 +179,7 @@ void OdometryViewer::clear()
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = odom.info().inliers;
int quality = odom.info().reg.inliers;
bool lost = false;
bool lostStateChanged = false;
@@ -193,11 +193,11 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
lost = true;
}
else if(odom.info().inliers>0 &&
else if(odom.info().reg.inliers>0 &&
qualityWarningThr_ &&
odom.info().inliers < qualityWarningThr_)
odom.info().reg.inliers < qualityWarningThr_)
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkYellow);
cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -425,13 +425,13 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
}
}
}

1
guilib/src/PreferencesDialog.cpp
View File

@@ -863,6 +863,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());

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

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-531</y>
<width>673</width>
<height>2749</height>
<y>-215</y>
<width>678</width>
<height>2736</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>18</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -13497,6 +13497,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</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="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
@@ -13598,35 +13653,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</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="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpVoxelSize">
<property name="suffix">
@@ -13723,25 +13749,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
@@ -13803,8 +13810,27 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
<item row="11" column="1">
<widget class="QLabel" name="label_429">
<property name="text">
<string>Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>