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

Added Features2d::limitKeypoints() with grid options. Odometry: if imu is provided and no guess is provided, the change of orientation of imu is used for rotation guess (overwrite rotation from Odom/GuessFromMotion). OdometryInfo: added gravity errors when imu is used. Preferences: added a second GravitySigma parameter (overwritting Optimizer/GravitySigma for odometry is not negative) for F2M odometry panel.

Browse Source
This commit is contained in:
matlabbe
2020-05-31 11:22:12 -04:00
parent 6e55525a7b
commit 415a2778f1
15 changed files with 186 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
guilib/src/CloudViewer.cpp
View File

@@ -3345,7 +3345,7 @@ void CloudViewer::handleAction(QAction * a)
else if(a == _aSetGridCellSize)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 1000, 2, &ok);
if(ok)
{
this->setGridCellSize(value);

29
guilib/src/MainWindow.cpp
View File

@@ -605,6 +605,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false);
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", false);
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
@@ -1524,14 +1526,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_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());
if(odom.info().timeParticleFiltering>0.0f)
{
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().localBundleTime > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
@@ -1943,9 +1956,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
// loop closure view
if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
!stat.loopClosureTransform().isNull() &&
@@ -5207,6 +5217,11 @@ void MainWindow::startDetection()
odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
double gravitySigma = _preferencesDialog->getOdomF2MGravitySigma();
if(gravitySigma >= 0.0)
{
uInsert(odomParameters, ParametersPair(Parameters::kOptimizerGravitySigma(), uNumber2Str(gravitySigma)));
}
if(odomStrategy != 1)
{
// Only Frame To Frame supports all VisCorType

8
guilib/src/PreferencesDialog.cpp
View File

@@ -421,6 +421,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->odom_f2m_gravitySigma, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
// Cloud rendering panel
@@ -2029,6 +2030,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
{
_ui->odom_registration->setCurrentIndex(3);
_ui->odom_f2m_gravitySigma->setValue(-1);
}
}
}
@@ -2154,6 +2156,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool());
_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
_ui->odom_f2m_gravitySigma->setValue(settings.value("odomF2MGravitySigma", _ui->odom_f2m_gravitySigma->value()).toDouble());
_ui->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
for(int i=0; i<2; ++i)
@@ -2628,6 +2631,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked());
settings.setValue("odomDisabled", _ui->checkbox_odomDisabled->isChecked());
settings.setValue("odomRegistration", _ui->odom_registration->currentIndex());
settings.setValue("odomF2MGravitySigma", _ui->odom_f2m_gravitySigma->value());
settings.setValue("gtAlign", _ui->checkbox_groundTruthAlign->isChecked());
for(int i=0; i<2; ++i)
@@ -5009,6 +5013,10 @@ int PreferencesDialog::getOdomRegistrationApproach() const
{
return _ui->odom_registration->currentIndex();
}
double PreferencesDialog::getOdomF2MGravitySigma() const
{
return _ui->odom_f2m_gravitySigma->value();
}
bool PreferencesDialog::isGroundTruthAligned() const
{
return _ui->checkbox_groundTruthAlign->isChecked();

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

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-687</y>
<y>0</y>
<width>680</width>
<height>3270</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -11004,7 +11004,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_527">
<property name="text">
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy.</string>
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11020,7 +11020,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string/>
</property>
<property name="decimals">
<number>2</number>
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
@@ -13469,6 +13469,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_598">
<property name="text">
<string>[Visual] Gravity sigma used for bundle adjustment (&lt;0, use same value than Optimizer/GravitySigma parameter)</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="QDoubleSpinBox" name="odom_f2m_gravitySigma">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>