Added ZED parameters

This commit is contained in:
matlabbe
2016-06-02 17:27:12 -04:00
parent 9430bcbf2e
commit d9611f784c
8 changed files with 403 additions and 184 deletions

View File

@@ -881,64 +881,78 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
(odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
cloud = util3d::cloudRGBFromSensorData(odom.data(),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1),
_preferencesDialog->getCloudMinDepth(1),
indices.get(),
_preferencesDialog->getAllParameters());
if(indices->size())
if(odom.data().imageRaw().cols % _preferencesDialog->getCloudDecimation(1) != 0 ||
odom.data().imageRaw().rows % _preferencesDialog->getCloudDecimation(1) != 0)
{
UERROR("Decimation (%d) is not modulo of the image resolution (%dx%d)! The cloud cannot be "
"created. Go to Preferences->3D Rendering under \"Odom\" column to modify this parameter.",
_preferencesDialog->getCloudDecimation(1),
odom.data().imageRaw().cols,
odom.data().imageRaw().rows);
}
else
{
cloud = util3d::transformPointCloud(cloud, pose);
if(_preferencesDialog->isCloudMeshing())
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
cloud = util3d::cloudRGBFromSensorData(odom.data(),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1),
_preferencesDialog->getCloudMinDepth(1),
indices.get(),
_preferencesDialog->getAllParameters());
if(indices->size())
{
// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output = util3d::extractIndices(cloud, indices, false, true);
cloud = util3d::transformPointCloud(cloud, pose);
// Fast organized mesh
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
if(_preferencesDialog->isCloudMeshing())
{
viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output = util3d::extractIndices(cloud, indices, false, true);
// Fast organized mesh
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
}
else if(!odom.data().stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
output,
_preferencesDialog->getCloudMeshingAngle(),
_preferencesDialog->isCloudMeshingQuad(),
_preferencesDialog->getCloudMeshingTriangleSize(),
Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
if(polygons.size())
{
if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
}
}
else if(!odom.data().stereoCameraModel().localTransform().isNull())
else
{
viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
output,
_preferencesDialog->getCloudMeshingAngle(),
_preferencesDialog->isCloudMeshingQuad(),
_preferencesDialog->getCloudMeshingTriangleSize(),
Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
if(polygons.size())
{
if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
}
}
else
{
if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
}
_cloudViewer->setCloudVisibility("cloudOdom", true);
_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
_cloudViewer->setCloudVisibility("cloudOdom", true);
_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
cloudUpdated = true;
cloudUpdated = true;
}
}
}
@@ -2218,6 +2232,18 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
pcl::IndicesPtr indices(new std::vector<int>);
UASSERT(nodeId == data.id());
if(image.cols % _preferencesDialog->getCloudDecimation(0) != 0 ||
image.rows % _preferencesDialog->getCloudDecimation(0) != 0)
{
UERROR("Decimation (%d) is not modulo of the image resolution (%dx%d)! The cloud cannot be "
"created. Go to Preferences->3D Rendering under \"Map\" column to modify this parameter.",
_preferencesDialog->getCloudDecimation(0),
image.cols,
image.rows);
return;
}
// Create organized cloud
cloudWithoutNormals = util3d::cloudRGBFromSensorData(data,
_preferencesDialog->getCloudDecimation(0),

View File

@@ -471,7 +471,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereoZed_computeDisparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
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_sensingMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereoZed_confidenceThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_zedSvoPath, SIGNAL(clicked()), this, SLOT(selectSourceSvoPath()));
connect(_ui->lineEdit_zedSvoPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1278,7 +1283,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_stereoImages_rectify->setChecked(false);
_ui->lineEdit_cameraStereoVideo_path->setText("");
_ui->checkBox_stereoVideo_rectify->setChecked(false);
_ui->checkBox_stereoZed_computeDisparity->setChecked(true);
_ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(1);
_ui->spinBox_stereoZed_confidenceThr->setValue(100);
_ui->lineEdit_zedSvoPath->clear();
_ui->checkBox_cameraImages_timestamps->setChecked(false);
_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -1604,7 +1613,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.endGroup(); // StereoVideo
settings.beginGroup("StereoZed");
_ui->checkBox_stereoZed_computeDisparity->setChecked(settings.value("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked()).toBool());
_ui->comboBox_stereoZed_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_stereoZed_resolution->currentIndex()).toInt());
_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->lineEdit_zedSvoPath->setText(settings.value("svo_path", _ui->lineEdit_zedSvoPath->text()).toString());
settings.endGroup(); // StereoZed
@@ -1993,7 +2006,11 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.endGroup(); // StereoVideo
settings.beginGroup("StereoZed");
settings.setValue("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked());
settings.setValue("resolution", _ui->comboBox_stereoZed_resolution->currentIndex());
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("svo_path", _ui->lineEdit_zedSvoPath->text());
settings.endGroup(); // StereoZed
@@ -2833,6 +2850,20 @@ void PreferencesDialog::selectSourceOni2Path()
}
}
void PreferencesDialog::selectSourceSvoPath()
{
QString dir = _ui->lineEdit_zedSvoPath->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_zedSvoPath->text(), tr("ZED (*.svo)"));
if(!path.isEmpty())
{
_ui->lineEdit_zedSvoPath->setText(path);
}
}
void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
{
UDEBUG("%s=%s", key.c_str(), value.c_str());
@@ -3988,10 +4019,27 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
}
else if (driver == kSrcStereoZed)
{
camera = new CameraStereoZed(
_ui->checkBox_stereoZed_computeDisparity->isChecked(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
if(!_ui->lineEdit_zedSvoPath->text().isEmpty())
{
camera = new CameraStereoZed(
_ui->lineEdit_zedSvoPath->text().toStdString(),
_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
else
{
camera = new CameraStereoZed(
this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
_ui->comboBox_stereoZed_resolution->currentIndex(),
_ui->comboBox_stereoZed_quality->currentIndex(),
_ui->comboBox_stereoZed_sensingMode->currentIndex(),
_ui->spinBox_stereoZed_confidenceThr->value(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
}
else if(driver == kSrcUsbDevice)
{

View File

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>984</width>
<height>693</height>
<height>713</height>
</rect>
</property>
<property name="sizePolicy">
@@ -63,25 +63,16 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>685</width>
<height>1826</height>
<y>-477</y>
<width>686</width>
<height>2023</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -2120,7 +2111,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex">
<number>6</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63">
@@ -2788,7 +2779,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="0" column="2">
<widget class="QLabel" name="label_240">
<property name="text">
<string>Driver</string>
<string>Driver.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -2805,7 +2796,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="2">
<widget class="QLabel" name="label_247">
<property name="text">
<string>Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images.</string>
<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>
<property name="wordWrap">
<bool>true</bool>
@@ -2817,7 +2808,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_stereo">
<property name="currentIndex">
<number>5</number>
<number>4</number>
</property>
<widget class="QWidget" name="page_49">
<layout class="QVBoxLayout" name="verticalLayout_91">
@@ -3038,32 +3029,135 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="title">
<string>Zed sdk</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_102">
<item>
<layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_stereoZed_computeDisparity">
<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>FULL</string>
</property>
</item>
<item>
<property name="text">
<string>RAW</string>
</property>
</item>
</widget>
</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>
<item row="0" column="1">
<widget class="QLabel" name="label_272">
<property name="text">
<string>Compute disparity with the GPU (using Zed sdk approach). Note that &quot;Generate disparity image...&quot; above will be ignored if set.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_stereoZed_resolution">
<item>
<property name="text">
<string>HD2K</string>
</property>
</item>
<item>
<property name="text">
<string>HD1080</string>
</property>
</item>
<item>
<property name="text">
<string>HD720</string>
</property>
</item>
<item>
<property name="text">
<string>VGA</string>
</property>
</item>
</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="1" column="0">
<widget class="QComboBox" name="comboBox_stereoZed_quality">
<item>
<property name="text">
<string>NONE</string>
</property>
</item>
<item>
<property name="text">
<string>PERFORMANCE</string>
</property>
</item>
<item>
<property name="text">
<string>QUALITY</string>
</property>
</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>
@@ -3071,11 +3165,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
<height>13</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_306">
<property name="text">
<string>Filtering value for the disparity map (and by extension the depth map). A lower value means more confidence and precision (but less density), an upper value reduces the filtering (more density, less certainty).</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">
<number>100</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -3514,16 +3628,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_93">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -3531,7 +3636,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="2">
<widget class="QLabel" name="label_255">
<property name="text">
<string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
<string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -8680,16 +8785,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
<widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -8829,16 +8925,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
<widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -8996,16 +9083,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -9085,16 +9163,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -9206,16 +9275,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>