i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

ZED driver: added option to use visual odometry approach from zed sdk. RtabmapThread: fixed thread state change on new map trigger on Odometry init (variance=9999). Odometry: on init, verify that the first frame is ok before sending first pose. Parameters: Mem/SaveDepth16Format is now false by default

Browse Source
This commit is contained in:
matlabbe
2016-06-24 18:49:34 -04:00
parent af6e17fce8
commit e90c97f8a4
14 changed files with 335 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/include/rtabmap/gui/PreferencesDialog.h
+2
View File
@@ -224,6 +224,7 @@ public:
float getTimeLimit() const;
float getDetectionRate() const;
bool isSLAMMode() const;
bool isRGBDMode() const;
//specific
bool isStatisticsPublished() const;
@@ -270,6 +271,7 @@ private slots:
void updatePredictionPlot();
void updateKpROI();
void updateG2oVisibility();
void updateStereoDisparityVisibility();
void useOdomFeatures();
void changeWorkingDirectory();
void changeDictionaryPath();
guilib/src/MainWindow.cpp
+20 -2
View File
@@ -761,6 +761,25 @@ void MainWindow::handleEvent(UEvent* anEvent)
else
{
emit cameraInfoReceived(cameraEvent->info());
if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
{
if (!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
emit odometryReceived(tmp, false);
}
else
{
// we receive too many odometry events! just send without data
SensorData data(cv::Mat(), cameraEvent->data().id(), cameraEvent->data().stamp());
data.setCameraModels(cameraEvent->data().cameraModels());
data.setStereoCameraModel(cameraEvent->data().stereoCameraModel());
data.setGroundTruth(cameraEvent->data().groundTruth());
OdometryEvent tmp(data, cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
emit odometryReceived(tmp, true);
}
}
}
}
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
@@ -3861,8 +3880,7 @@ void MainWindow::startDetection()
_odomThread = 0;
}
DBReader* dbReader = dynamic_cast<DBReader*>(camera);
if(dbReader == 0 || dbReader->isOdometryIgnored())
if(!camera->odomProvided())
{
Odometry * odom = Odometry::create(parameters);
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
guilib/src/PreferencesDialog.cpp
+23
View File
@@ -485,8 +485,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->comboBox_stereoZed_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoDisparityVisibility()));
connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoDisparityVisibility()));
connect(_ui->comboBox_stereoZed_sensingMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereoZed_confidenceThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereoZed_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_zedSvoPath, SIGNAL(clicked()), this, SLOT(selectSourceSvoPath()));
connect(_ui->lineEdit_zedSvoPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1312,6 +1315,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(1);
_ui->spinBox_stereoZed_confidenceThr->setValue(100);
_ui->checkbox_stereoZed_odom->setChecked(false);
_ui->lineEdit_zedSvoPath->clear();
_ui->checkBox_cameraImages_timestamps->setChecked(false);
@@ -1654,6 +1658,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->comboBox_stereoZed_quality->setCurrentIndex(settings.value("quality", _ui->comboBox_stereoZed_quality->currentIndex()).toInt());
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(settings.value("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex()).toInt());
_ui->spinBox_stereoZed_confidenceThr->setValue(settings.value("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value()).toInt());
_ui->checkbox_stereoZed_odom->setChecked(settings.value("odom", _ui->checkbox_stereoZed_odom->isChecked()).toBool());
_ui->lineEdit_zedSvoPath->setText(settings.value("svo_path", _ui->lineEdit_zedSvoPath->text()).toString());
settings.endGroup(); // StereoZed
@@ -2062,6 +2067,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("quality", _ui->comboBox_stereoZed_quality->currentIndex());
settings.setValue("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex());
settings.setValue("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value());
settings.setValue("odom", _ui->checkbox_stereoZed_odom->isChecked());
settings.setValue("svo_path", _ui->lineEdit_zedSvoPath->text());
settings.endGroup(); // StereoZed
@@ -3447,6 +3453,17 @@ void PreferencesDialog::updateG2oVisibility()
_ui->groupBox_g2o->setVisible(_ui->graphOptimization_type->currentIndex() == 1);
}
void PreferencesDialog::updateStereoDisparityVisibility()
{
Src driver = this->getSourceDriver();
_ui->checkbox_stereo_depthGenerated->setVisible(
driver != PreferencesDialog::kSrcStereoZed ||
_ui->comboBox_stereoZed_quality->currentIndex() == 0);
_ui->label_stereo_depthGenerated->setVisible(
driver != PreferencesDialog::kSrcStereoZed ||
_ui->comboBox_stereoZed_quality->currentIndex() == 0);
}
void PreferencesDialog::useOdomFeatures()
{
if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked())
@@ -4105,6 +4122,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
_ui->checkbox_stereoZed_odom->isChecked(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -4116,6 +4134,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
_ui->checkbox_stereoZed_odom->isChecked(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -4285,6 +4304,10 @@ bool PreferencesDialog::isSLAMMode() const
{
return _ui->general_checkBox_SLAM_mode->isChecked();
}
bool PreferencesDialog::isRGBDMode() const
{
return _ui->general_checkBox_activateRGBD->isChecked();
}
/*** SETTERS ***/
void PreferencesDialog::setInputRate(double value)
guilib/src/ui/preferencesDialog.ui
+110 -90
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-360</y>
<width>686</width>
<height>2023</height>
<y>0</y>
<width>685</width>
<height>1826</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -2316,7 +2316,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>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3104,7 +3104,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_247">
<widget class="QLabel" name="label_stereo_depthGenerated">
<property name="text">
<string>Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can found under StereoBM tab.</string>
</property>
@@ -3340,37 +3340,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Zed sdk</string>
</property>
<layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
<item row="2" column="0">
<widget class="QComboBox" name="comboBox_stereoZed_sensingMode">
<item>
<property name="text">
<string>FILL</string>
</property>
</item>
<item>
<property name="text">
<string>STANDARD</string>
</property>
</item>
</widget>
<item row="6" column="0">
<spacer name="verticalSpacer_54">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>13</height>
</size>
</property>
</spacer>
</item>
<item row="4" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QToolButton" name="toolButton_zedSvoPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_zedSvoPath">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
<item row="5" column="1">
<widget class="QLabel" name="label_305">
<property name="text">
<string>Path to a *.SVO file.</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="0">
<widget class="QComboBox" name="comboBox_stereoZed_resolution">
@@ -3396,10 +3390,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_274">
<item row="2" column="1">
<widget class="QLabel" name="label_291">
<property name="text">
<string>Resolution. Not used when a SVO file is used.</string>
<string>Sensing mode.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -3409,6 +3403,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QComboBox" name="comboBox_stereoZed_sensingMode">
<item>
<property name="text">
<string>FILL</string>
</property>
</item>
<item>
<property name="text">
<string>STANDARD</string>
</property>
</item>
</widget>
</item>
<item row="1" column="0">
<widget class="QComboBox" name="comboBox_stereoZed_quality">
<item>
@@ -3433,58 +3441,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_290">
<property name="text">
<string>Quality. If NONE, the disparity is not computed on the GPU.</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_291">
<property name="text">
<string>Sensing mode.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_305">
<property name="text">
<string>Path to a *.SVO file.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<spacer name="verticalSpacer_54">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>13</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_306">
<property name="text">
@@ -3498,6 +3454,50 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_274">
<property name="text">
<string>Resolution. Not used when a SVO file is used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QToolButton" name="toolButton_zedSvoPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_zedSvoPath">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_290">
<property name="text">
<string>Quality. If NONE, the disparity is not computed on the GPU.</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="QSpinBox" name="spinBox_stereoZed_confidenceThr">
<property name="maximum">
@@ -3505,6 +3505,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_316">
<property name="text">
<string>Use ZED visual odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkbox_stereoZed_odom">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>