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

Updated how local scan matching is done (doing it on segmented local paths)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-27 02:43:06 -04:00
parent 2e706ed01f
commit db90da7303
16 changed files with 532 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
guilib/src/MainWindow.cpp
View File

@@ -602,7 +602,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
Statistics stats = rtabmapEvent->getStats();
int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_closure_id(), 0.0f));
int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_last_closure_id(), 0.0f));
bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
if((stats.loopClosureId() > 0 &&
@@ -1984,6 +1984,9 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
else
{
_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
_ui->statusbar->showMessage(
tr("Global path computed from %1 (%2 poses, %3 m)!").arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
5000);
}
}
@@ -2730,7 +2733,8 @@ void MainWindow::startDetection()
{
_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceDatabaseOdometryIgnored());
_preferencesDialog->getSourceDatabaseOdometryIgnored(),
_preferencesDialog->getSourceDatabaseGoalDelayIgnored());
//Create odometry thread if rgdb slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&

27
guilib/src/PreferencesDialog.cpp
View File

@@ -293,6 +293,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->groupBox_sourceDatabase, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_database_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//openni group
connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
@@ -363,6 +364,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
_ui->general_doubleSpinBox_laserScanVoxel->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str());
// Database
@@ -475,14 +477,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
_ui->graphPlan_maxDiffID->setObjectName(Parameters::kRGBDPlanVirtualLinksMaxDiffID().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
_ui->localDetection_maxNeighbors->setObjectName(Parameters::kRGBDLocalLoopDetectionNeighbors().c_str());
_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
@@ -499,7 +502,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_maxFeatures->setObjectName(Parameters::kLccReextractMaxWords().c_str());
_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
_ui->globalDetection_icpMaxDistance->setObjectName(Parameters::kLccIcpMaxDistance().c_str());
_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kLccIcpMaxTranslation().c_str());
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kLccIcpMaxRotation().c_str());
_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());
@@ -727,9 +731,12 @@ void PreferencesDialog::setupSignals()
const rtabmap::ParametersMap & parameters = Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
QObject * obj = _ui->stackedWidget->findChild<QObject*>((*iter).first.c_str());
QWidget * obj = _ui->stackedWidget->findChild<QWidget*>((*iter).first.c_str());
if(obj)
{
// set tooltip as the parameter name
obj->setToolTip(iter->first.c_str());
QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
QComboBox * combo = qobject_cast<QComboBox *>(obj);
@@ -767,12 +774,12 @@ void PreferencesDialog::setupSignals()
}
else
{
ULOGGER_WARN("QObject called %s can't be cast to a supported widget", (*iter).first.c_str());
ULOGGER_WARN("QWidget called %s can't be cast to a supported widget", (*iter).first.c_str());
}
}
else
{
ULOGGER_WARN("Can't find the related QObject for parameter %s", (*iter).first.c_str());
ULOGGER_WARN("Can't find the related QWidget for parameter %s", (*iter).first.c_str());
}
}
}
@@ -928,6 +935,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->groupBox_sourceDatabase->setChecked(false);
_ui->source_checkBox_ignoreOdometry->setChecked(false);
_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
_ui->source_spinBox_databaseStartPos->setValue(0);
_ui->groupBox_sourceOpenni->setChecked(true);
@@ -1202,6 +1210,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->groupBox_sourceDatabase->setChecked(settings.value("databaseUsed", _ui->groupBox_sourceDatabase->isChecked()).toBool());
_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
settings.endGroup(); // Database
@@ -1468,6 +1477,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("databaseUsed", _ui->groupBox_sourceDatabase->isChecked());
settings.setValue("path", _ui->source_database_lineEdit_path->text());
settings.setValue("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked());
settings.setValue("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked());
settings.setValue("startPos", _ui->source_spinBox_databaseStartPos->value());
settings.endGroup();
@@ -2030,6 +2040,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
_ui->groupBox_sourceDatabase->setChecked(true);
_ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes);
_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
_ui->source_database_lineEdit_path->setText(path);
_ui->source_spinBox_databaseStartPos->setValue(0);
}
@@ -3036,6 +3047,10 @@ bool PreferencesDialog::getSourceDatabaseOdometryIgnored() const
{
return _ui->source_checkBox_ignoreOdometry->isChecked();
}
bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const
{
return _ui->source_checkBox_ignoreGoalDelay->isChecked();
}
int PreferencesDialog::getSourceDatabaseStartPos() const
{
return _ui->source_spinBox_databaseStartPos->value();

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

@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>759</width>
<height>800</height>
<width>760</width>
<height>1002</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>26</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1603,6 +1603,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_9">
<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">
@@ -1613,14 +1620,14 @@ 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="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_58">
<property name="text">
<string>Start position (index)</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QSpinBox" name="source_spinBox_databaseStartPos">
<property name="minimum">
<number>0</number>
@@ -1644,13 +1651,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="1" column="1">
<widget class="QLabel" name="label_72">
<property name="text">
@@ -1661,6 +1661,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_80">
<property name="text">
<string>Ignore goal delay.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -2731,6 +2748,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_recentWmRatio">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.200000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="general_spinBox_maxStMemSize">
<property name="minimum">
@@ -2764,25 +2800,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_recentWmRatio">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.200000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_ratioRecent">
<property name="text">
@@ -2916,6 +2933,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_retrieved_8">
<property name="text">
<string>If &gt; 0.0, voxelize laser scans when creating a location. This feature can be used to save laser scans already voxelized.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxel">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -5157,6 +5209,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item>
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="localDetection_maxDiffID">
<property name="maximum">
<number>999</number>
@@ -5166,14 +5234,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="localDetection_maxNeighbors">
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="0" column="1">
<widget class="QLabel" name="label_space3_2">
<property name="text">
<string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
@@ -5183,16 +5244,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">
<property name="text">
<string>Maximum nearest neighbors for space detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
<property name="text">
@@ -5210,6 +5261,16 @@ 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_3">
<property name="text">
<string>Path filtering radius to avoid merging laser scans which are close.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -5263,14 +5324,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="1" column="1">
<widget class="QLabel" name="label_space3_4">
<property name="text">
<string>Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by &quot;Goal reached radius&quot; above.</string>
<string>Add virtual links. Before planning in the graph, near nodes are linked together. The maximum distance is defined by &quot;Goal reached radius&quot; above. If &quot;Maximum ID difference&quot; below is set, only close nodes in time can be linked together.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_space3_5">
<property name="text">
<string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -5280,13 +5341,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space3_6">
<property name="text">
<string>Maximum ID difference between close nodes to add virtual links. Set 0 to ignore.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="graphPlan_maxDiffID">
<property name="maximum">
<number>999</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -5754,7 +5835,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxTranslation">
<property name="suffix">
<string> m</string>
</property>
@@ -5775,13 +5856,42 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item row="0" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
<string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
<string>Maximum ICP translation correction accepted (&gt;0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_55">
<property name="text">
<string>Maximum ICP rotation correction accepted (&gt;0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxRotation">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.780000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>