From 1e335e53ba4c29534d823702b10e6e782caab640 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 7 Aug 2017 16:31:49 -0400 Subject: [PATCH] OdometryEvent: fixed velocity() returning always empty. Export: added options to filter blurred cameras based on laplacian threshold or velocity (#225). --- app/android/AndroidManifest.xml.in | 2 +- corelib/include/rtabmap/core/OdometryEvent.h | 1 + guilib/include/rtabmap/gui/MainWindow.h | 2 + guilib/src/CMakeLists.txt | 3 + guilib/src/ExportBundlerDialog.cpp | 128 +++++++++++ guilib/src/ExportBundlerDialog.h | 71 ++++++ guilib/src/ExportCloudsDialog.cpp | 107 ++++++++- guilib/src/MainWindow.cpp | 192 ++++++++++++---- guilib/src/PreferencesDialog.cpp | 13 ++ guilib/src/ui/exportBundlerDialog.ui | 218 +++++++++++++++++++ guilib/src/ui/exportCloudsDialog.ui | 80 ++++++- guilib/src/ui/exportDialog.ui | 2 +- 12 files changed, 753 insertions(+), 66 deletions(-) create mode 100644 guilib/src/ExportBundlerDialog.cpp create mode 100644 guilib/src/ExportBundlerDialog.h create mode 100644 guilib/src/ui/exportBundlerDialog.ui diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index 66e016df..196a4c4a 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/corelib/include/rtabmap/core/OdometryEvent.h b/corelib/include/rtabmap/core/OdometryEvent.h index 067b54e0..d6535498 100644 --- a/corelib/include/rtabmap/core/OdometryEvent.h +++ b/corelib/include/rtabmap/core/OdometryEvent.h @@ -82,6 +82,7 @@ public: velocity[3] = roll/_info.interval; velocity[4] = pitch/_info.interval; velocity[5] = yaw/_info.interval; + return velocity; } return std::vector(); } diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index a6b8336e..f8a16d0c 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -67,6 +67,7 @@ class StatsToolBox; class ProgressDialog; class TwistGridWidget; class ExportCloudsDialog; +class ExportBundlerDialog; class PostProcessingDialog; class DepthCalibrationDialog; class DataRecorder; @@ -265,6 +266,7 @@ private: PreferencesDialog * _preferencesDialog; AboutDialog * _aboutDialog; ExportCloudsDialog * _exportCloudsDialog; + ExportBundlerDialog * _exportBundlerDialog; PostProcessingDialog * _postProcessingDialog; DepthCalibrationDialog * _depthCalibrationDialog; DataRecorder * _dataRecorder; diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index e7f04b2b..693bb45f 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -22,6 +22,7 @@ SET(headers_ui ./ExportDialog.h ./PostProcessingDialog.h ./ExportCloudsDialog.h + ./ExportBundlerDialog.h ./MapVisibilityWidget.h ../include/${PROJECT_PREFIX}/gui/GraphViewer.h ./CreateSimpleCalibrationDialog.h @@ -45,6 +46,7 @@ SET(uis ./ui/calibrationDialog.ui ./ui/createSimpleCalibrationDialog.ui ./ui/depthCalibrationDialog.ui + ./ui/exportBundlerDialog.ui ) SET(qrc @@ -87,6 +89,7 @@ SET(SRC_FILES ./CameraViewer.cpp ./CalibrationDialog.cpp ./ExportDialog.cpp + ./ExportBundlerDialog.cpp ./PostProcessingDialog.cpp ./ExportCloudsDialog.cpp ./MapVisibilityWidget.cpp diff --git a/guilib/src/ExportBundlerDialog.cpp b/guilib/src/ExportBundlerDialog.cpp new file mode 100644 index 00000000..c6efcbe3 --- /dev/null +++ b/guilib/src/ExportBundlerDialog.cpp @@ -0,0 +1,128 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "ExportBundlerDialog.h" +#include "ui_exportBundlerDialog.h" + +#include +#include + +namespace rtabmap { + +ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) : + QDialog(parent) +{ + _ui = new Ui_ExportBundlerDialog(); + _ui->setupUi(this); + + connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath())); + + restoreDefaults(); + connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults())); + + connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + + _ui->lineEdit_path->setText(QDir::currentPath()); +} + +ExportBundlerDialog::~ExportBundlerDialog() +{ + delete _ui; +} + +void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & group) const +{ + if(!group.isEmpty()) + { + settings.beginGroup(group); + } + settings.setValue("maxLinearSpeed", this->maxLinearSpeed()); + settings.setValue("maxAngularSpeed", this->maxAngularSpeed()); + settings.setValue("laplacianThr", this->laplacianThreshold()); + if(!group.isEmpty()) + { + settings.endGroup(); + } +} + +void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & group) +{ + if(!group.isEmpty()) + { + settings.beginGroup(group); + } + _ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", this->maxLinearSpeed()).toDouble()); + _ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", this->maxAngularSpeed()).toDouble()); + _ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", this->laplacianThreshold()).toDouble()); + if(!group.isEmpty()) + { + settings.endGroup(); + } +} + +void ExportBundlerDialog::setWorkingDirectory(const QString & path) +{ + _ui->lineEdit_path->setText((path.isEmpty()?QDir::currentPath():path) + "/bundler"); +} + +void ExportBundlerDialog::restoreDefaults() +{ + _ui->doubleSpinBox_linearSpeed->setValue(0); + _ui->doubleSpinBox_angularSpeed->setValue(0); + _ui->doubleSpinBox_laplacianVariance->setValue(0); +} + +void ExportBundlerDialog::getPath() +{ + QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _ui->lineEdit_path->text()); + if(!path.isEmpty()) + { + _ui->lineEdit_path->setText(path); + } +} + +QString ExportBundlerDialog::outputPath() const +{ + return _ui->lineEdit_path->text(); +} + +double ExportBundlerDialog::maxLinearSpeed() const +{ + return _ui->doubleSpinBox_linearSpeed->value(); +} +double ExportBundlerDialog::maxAngularSpeed() const +{ + return _ui->doubleSpinBox_angularSpeed->value(); +} +double ExportBundlerDialog::laplacianThreshold() const +{ + return _ui->doubleSpinBox_laplacianVariance->value(); +} + +} diff --git a/guilib/src/ExportBundlerDialog.h b/guilib/src/ExportBundlerDialog.h new file mode 100644 index 00000000..2135314d --- /dev/null +++ b/guilib/src/ExportBundlerDialog.h @@ -0,0 +1,71 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef EXPORTBUNDLERDIALOG_H_ +#define EXPORTBUNDLERDIALOG_H_ + +#include +#include + +class Ui_ExportBundlerDialog; + +namespace rtabmap { + +class ExportBundlerDialog : public QDialog +{ + Q_OBJECT + +public: + ExportBundlerDialog(QWidget * parent = 0); + + virtual ~ExportBundlerDialog(); + + void saveSettings(QSettings & settings, const QString & group = "") const; + void loadSettings(QSettings & settings, const QString & group = ""); + + void setWorkingDirectory(const QString & path); + + QString outputPath() const; + + double maxLinearSpeed() const; + double maxAngularSpeed() const; + double laplacianThreshold() const; + +signals: + void configChanged(); + +private slots: + void getPath(); + void restoreDefaults(); + +private: + Ui_ExportBundlerDialog * _ui; +}; + +} + +#endif /* EXPORTBUNDLERDIALOG_H_ */ diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index bbc0691a..8d8b90a1 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -178,6 +178,9 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); connect(_ui->doubleSpinBox_cameraFilterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_cameraFilterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_cameraFilterVel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_cameraFilterVelRad, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->checkBox_poisson_outputPolygons, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_poisson_manifold, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); @@ -319,6 +322,9 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()); settings.setValue("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value()); settings.setValue("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value()); + settings.setValue("mesh_textureCameraFilteringVel", _ui->doubleSpinBox_cameraFilterVel->value()); + settings.setValue("mesh_textureCameraFilteringVelRad", _ui->doubleSpinBox_cameraFilterVelRad->value()); + settings.setValue("mesh_textureCameraFilteringLaplacian", _ui->doubleSpinBox_laplacianVariance->value()); settings.setValue("mesh_textureBrightnessConstrastRatioLow", _ui->spinBox_textureBrightnessContrastRatioLow->value()); settings.setValue("mesh_textureBrightnessConstrastRatioHigh", _ui->spinBox_textureBrightnessContrastRatioHigh->value()); settings.setValue("mesh_textureExposureFusion", _ui->checkBox_exposureFusion->isChecked()); @@ -432,6 +438,9 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->checkBox_cameraFilter->setChecked(settings.value("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()).toBool()); _ui->doubleSpinBox_cameraFilterRadius->setValue(settings.value("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value()).toDouble()); _ui->doubleSpinBox_cameraFilterAngle->setValue(settings.value("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value()).toDouble()); + _ui->doubleSpinBox_cameraFilterVel->setValue(settings.value("mesh_textureCameraFilteringVel", _ui->doubleSpinBox_cameraFilterVel->value()).toDouble()); + _ui->doubleSpinBox_cameraFilterVelRad->setValue(settings.value("mesh_textureCameraFilteringVelRad", _ui->doubleSpinBox_cameraFilterVelRad->value()).toDouble()); + _ui->doubleSpinBox_laplacianVariance->setValue(settings.value("mesh_textureCameraFilteringLaplacian", _ui->doubleSpinBox_laplacianVariance->value()).toDouble()); _ui->spinBox_textureBrightnessContrastRatioLow->setValue(settings.value("mesh_textureBrightnessConstrastRatioLow", _ui->spinBox_textureBrightnessContrastRatioLow->value()).toDouble()); _ui->spinBox_textureBrightnessContrastRatioHigh->setValue(settings.value("mesh_textureBrightnessConstrastRatioHigh", _ui->spinBox_textureBrightnessContrastRatioHigh->value()).toDouble()); if(_ui->checkBox_exposureFusion->isEnabled()) @@ -543,8 +552,11 @@ void ExportCloudsDialog::restoreDefaults() _ui->spinBox_mesh_minTextureClusterSize->setValue(50); _ui->lineEdit_meshingTextureRoiRatios->setText("0.0 0.0 0.0 0.0"); _ui->checkBox_cameraFilter->setChecked(false); - _ui->doubleSpinBox_cameraFilterRadius->setValue(0.1); + _ui->doubleSpinBox_cameraFilterRadius->setValue(0); _ui->doubleSpinBox_cameraFilterAngle->setValue(30); + _ui->doubleSpinBox_cameraFilterVel->setValue(0); + _ui->doubleSpinBox_cameraFilterVelRad->setValue(0); + _ui->doubleSpinBox_laplacianVariance->setValue(0); _ui->spinBox_textureBrightnessContrastRatioLow->setValue(0); _ui->spinBox_textureBrightnessContrastRatioHigh->setValue(0); _ui->checkBox_exposureFusion->setChecked(false); @@ -2012,6 +2024,7 @@ bool ExportCloudsDialog::getExportedClouds( std::map cameraPoses; std::map > cameraModels; std::map cameraDepths; + int ignoredCameras = 0; for(std::map::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter) { if(validCameras.find(jter->first) != validCameras.end()) @@ -2046,21 +2059,34 @@ bool ExportCloudsDialog::getExportedClouds( if(!jter->second.isNull() && models.size()) { cv::Mat depth; + bool blurryImage = false; bool getDepth = !stereo && _ui->doubleSpinBox_meshingTextureMaxDepthError->value() >= 0.0f; + cv::Mat img; + std::vector velocity; if(models[0].imageWidth() == 0 || models[0].imageHeight() == 0) { // we are using an old database format (image size not saved in calibrations), we should // uncompress images to get their size - cv::Mat img; if(cacheHasCompressedImage) { cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, getDepth?&depth:0); + velocity = cachedSignatures.find(jter->first)->getVelocity(); } else if(_dbDriver) { SensorData data; _dbDriver->getNodeData(jter->first, data, true, false, false, false); data.uncompressDataConst(&img, getDepth?&depth:0); + + if(_ui->checkBox_cameraFilter->isChecked() && + (_ui->doubleSpinBox_cameraFilterVel->value()>0.0 || _ui->doubleSpinBox_cameraFilterVelRad->value()>0.0)) + { + Transform p,gt; + int m,w; + std::string l; + double s; + _dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity); + } } cv::Size imageSize = img.size(); imageSize.width /= models.size(); @@ -2071,20 +2097,80 @@ bool ExportCloudsDialog::getExportedClouds( } else { + bool getImg = _ui->checkBox_cameraFilter->isChecked() && _ui->doubleSpinBox_laplacianVariance->value()>0.0; // get just the depth if(cacheHasCompressedImage) { - cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(0, getDepth?&depth:0); + cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(getImg?&img:0, getDepth?&depth:0); + velocity = cachedSignatures.find(jter->first)->getVelocity(); } else if(_dbDriver) { SensorData data; _dbDriver->getNodeData(jter->first, data, true, false, false, false); - data.uncompressDataConst(0, getDepth?&depth:0); + data.uncompressDataConst(getImg?&img:0, getDepth?&depth:0); + + if(_ui->checkBox_cameraFilter->isChecked() && + (_ui->doubleSpinBox_cameraFilterVel->value()>0.0 || _ui->doubleSpinBox_cameraFilterVelRad->value()>0.0)) + { + Transform p,gt; + int m,w; + std::string l; + double s; + _dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity); + } + } + } + if(_ui->checkBox_cameraFilter->isChecked()) + { + std::string msg; + if(!blurryImage && + (_ui->doubleSpinBox_cameraFilterVel->value()>0.0 || _ui->doubleSpinBox_cameraFilterVelRad->value()>0.0)) + { + if(velocity.size() == 6) + { + float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2])); + float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5])); + if(_ui->doubleSpinBox_cameraFilterVel->value()>0.0 && transVel > _ui->doubleSpinBox_cameraFilterVel->value()) + { + msg = uFormat("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", jter->first, transVel, _ui->doubleSpinBox_cameraFilterVel->value()); + blurryImage = true; + } + else if(_ui->doubleSpinBox_cameraFilterVelRad->value()>0.0 && rotVel > _ui->doubleSpinBox_cameraFilterVelRad->value()) + { + msg = uFormat("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", jter->first, rotVel, _ui->doubleSpinBox_cameraFilterVelRad->value()); + blurryImage = true; + } + } + else + { + UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", jter->first); + } + } + + if(!blurryImage && !img.empty() && _ui->doubleSpinBox_laplacianVariance->value()>0.0) + { + cv::Mat imgLaplacian; + cv::Laplacian(img, imgLaplacian, CV_16S); + cv::Mat m, s; + cv::meanStdDev(imgLaplacian, m, s); + double stddev_pxl = s.at(0); + double var = stddev_pxl*stddev_pxl; + if(var < _ui->doubleSpinBox_laplacianVariance->value()) + { + blurryImage = true; + msg = uFormat("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", jter->first, var, _ui->doubleSpinBox_laplacianVariance->value()); + } + } + if(blurryImage) + { + _progressDialog->appendText(msg.c_str()); + QApplication::processEvents(); + ++ignoredCameras; } } - if(models[0].imageWidth() != 0 && models[0].imageHeight() != 0) + if(!blurryImage && models[0].imageWidth() != 0 && models[0].imageHeight() != 0) { cameraPoses.insert(std::make_pair(jter->first, jter->second)); cameraModels.insert(std::make_pair(jter->first, models)); @@ -2096,6 +2182,17 @@ bool ExportCloudsDialog::getExportedClouds( } } } + if(ignoredCameras > validCameras.size()/2) + { + std::string msg = uFormat("More than 50%% of the cameras (%d/%d) have been filtered for " + "too fast motion and/or blur level. You may adjust the corresponding thresholds.", + ignoredCameras, (int)validCameras.size()); + UWARN(msg.c_str()); + _progressDialog->appendText(msg.c_str(), Qt::darkYellow); + _progressDialog->setAutoClose(false); + QApplication::processEvents(); + } + if(cameraPoses.size() && iter->second->polygons.size()) { pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index e315936d..d204c83a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -62,6 +62,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UCv2Qt.h" #include "ExportCloudsDialog.h" +#include "ExportBundlerDialog.h" #include "AboutDialog.h" #include "PostProcessingDialog.h" #include "DepthCalibrationDialog.h" @@ -136,6 +137,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _preferencesDialog(0), _aboutDialog(0), _exportCloudsDialog(0), + _exportBundlerDialog(0), _dataRecorder(0), _lastId(0), _firstStamp(0.0f), @@ -183,6 +185,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _aboutDialog->setObjectName("AboutDialog"); _exportCloudsDialog = new ExportCloudsDialog(this); _exportCloudsDialog->setObjectName("ExportCloudsDialog"); + _exportBundlerDialog = new ExportBundlerDialog(this); + _exportBundlerDialog->setObjectName("ExportBundlerDialog"); _postProcessingDialog = new PostProcessingDialog(this); _postProcessingDialog->setObjectName("PostProcessingDialog"); _depthCalibrationDialog = new DepthCalibrationDialog(this); @@ -234,6 +238,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown); _preferencesDialog->loadWindowGeometry(_preferencesDialog); _preferencesDialog->loadWindowGeometry(_exportCloudsDialog); + _preferencesDialog->loadWindowGeometry(_exportBundlerDialog); _preferencesDialog->loadWindowGeometry(_postProcessingDialog); _preferencesDialog->loadWindowGeometry(_depthCalibrationDialog); _preferencesDialog->loadWindowGeometry(_aboutDialog); @@ -460,6 +465,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); + connect(_exportBundlerDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified())); @@ -515,11 +521,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->statsToolBox->setNewFigureMaxItems(50); _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); + _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _cloudViewer->setBackfaceCulling(true, false); _preferencesDialog->loadWidgetState(_cloudViewer); //dialog states _preferencesDialog->loadWidgetState(_exportCloudsDialog); + _preferencesDialog->loadWidgetState(_exportBundlerDialog); _preferencesDialog->loadWidgetState(_postProcessingDialog); _preferencesDialog->loadWidgetState(_depthCalibrationDialog); @@ -3740,6 +3748,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo { _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); + _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); } if(_state != kIdle && parametersModified.size()) @@ -4126,6 +4135,7 @@ void MainWindow::saveConfigGUI() _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure); _preferencesDialog->saveWidgetState(_ui->imageView_odometry); _preferencesDialog->saveWidgetState(_exportCloudsDialog); + _preferencesDialog->saveWidgetState(_exportBundlerDialog); _preferencesDialog->saveWidgetState(_postProcessingDialog); _preferencesDialog->saveWidgetState(_depthCalibrationDialog); _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView); @@ -6497,6 +6507,11 @@ void MainWindow::exportImages() void MainWindow::exportBundlerFormat() { + if(_exportBundlerDialog->isVisible()) + { + return; + } + std::map posesIn = _ui->widget_mapVisibility->getVisiblePoses(); // Use ground truth poses if current clouds are using them @@ -6544,9 +6559,18 @@ void MainWindow::exportBundlerFormat() if(poses.size()) { - QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _preferencesDialog->getWorkingDirectory()); + if(_exportBundlerDialog->exec() != QDialog::Accepted) + { + return; + } + QString path = _exportBundlerDialog->outputPath(); if(!path.isEmpty()) { + if(!QDir(path).mkpath(".")) + { + QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path)); + return; + } // export cameras and images QFile fileOut(path+QDir::separator()+"cameras.out"); QFile fileList(path+QDir::separator()+"list.txt"); @@ -6555,15 +6579,10 @@ void MainWindow::exportBundlerFormat() { if(fileList.open(QIODevice::WriteOnly | QIODevice::Text)) { - QTextStream out(&fileOut); - QTextStream list(&fileList); - out << "# Bundle file v0.3\n"; - out << poses.size() << " 0\n"; - + std::set ignoredCameras; for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first); - list << p << "\n"; p = path+QDir::separator()+p; cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw(); if(image.empty()) @@ -6571,56 +6590,133 @@ void MainWindow::exportBundlerFormat() _cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0); } - if(cv::imwrite(p.toStdString(), image)) + double maxLinearVel = _exportBundlerDialog->maxLinearSpeed(); + double maxAngularVel = _exportBundlerDialog->maxAngularSpeed(); + double laplacianThr = _exportBundlerDialog->laplacianThreshold(); + bool blurryImage = false; + const std::vector & velocity = _cachedSignatures[iter->first].getVelocity(); + if(maxLinearVel>0.0 || maxAngularVel>0.0) { - UINFO("saved image %s", p.toStdString().c_str()); + if(velocity.size() == 6) + { + float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2])); + float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5])); + if(maxLinearVel>0.0 && transVel > maxLinearVel) + { + UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel); + blurryImage = true; + } + else if(maxAngularVel>0.0 && rotVel > maxAngularVel) + { + UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel); + blurryImage = true; + } + } + else + { + UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first); + } + } + + if(!blurryImage && !image.empty() && laplacianThr>0.0) + { + cv::Mat imgLaplacian; + cv::Laplacian(image, imgLaplacian, CV_16S); + cv::Mat m, s; + cv::meanStdDev(imgLaplacian, m, s); + double stddev_pxl = s.at(0); + double var = stddev_pxl*stddev_pxl; + if(var < laplacianThr) + { + blurryImage = true; + UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr); + } + } + if(blurryImage) + { + ignoredCameras.insert(iter->first); } else { - UERROR("Failed to save image %s", p.toStdString().c_str()); + if(cv::imwrite(p.toStdString(), image)) + { + UINFO("saved image %s", p.toStdString().c_str()); + } + else + { + UERROR("Failed to save image %s", p.toStdString().c_str()); + } + } + } + + QTextStream out(&fileOut); + QTextStream list(&fileList); + out << "# Bundle file v0.3\n"; + out << poses.size()-ignoredCameras.size() << " 0\n"; + + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(ignoredCameras.find(iter->first) == ignoredCameras.end()) + { + QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first); + list << p << "\n"; + + Transform localTransform; + if(_cachedSignatures[iter->first].sensorData().cameraModels().size()) + { + out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n"; + localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform(); + } + else + { + out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n"; + localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform(); + } + + static const Transform opengl_world_T_rtabmap_world( + 0.0f, -1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + -1.0f, 0.0f, 0.0f, 0.0f); + + static const Transform optical_rotation_inv( + 0.0f, -1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, -1.0f, 0.0f, + 1.0f, 0.0f, 0.0f, 0.0f); + + Transform pose = iter->second; + if(!localTransform.isNull()) + { + pose*=localTransform*optical_rotation_inv; + } + Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse(); + + out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n"; + out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n"; + out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n"; + out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n"; + } - - Transform localTransform; - if(_cachedSignatures[iter->first].sensorData().cameraModels().size()) - { - out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n"; - localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform(); - } - else - { - out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n"; - localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform(); - } - - static const Transform opengl_world_T_rtabmap_world( - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, - -1.0f, 0.0f, 0.0f, 0.0f); - - static const Transform optical_rotation_inv( - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f, - 1.0f, 0.0f, 0.0f, 0.0f); - - Transform pose = iter->second; - if(!localTransform.isNull()) - { - pose*=localTransform*optical_rotation_inv; - } - Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse(); - - out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n"; - out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n"; - out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n"; - out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n"; } - QMessageBox::question(this, - tr("Exporting cameras in Bundler format..."), - tr("%1 cameras/images exported to directory \"%2\".").arg(poses.size()).arg(path)); fileList.close(); + fileOut.close(); + + QMessageBox::information(this, + tr("Exporting cameras in Bundler format..."), + tr("%1 cameras/images exported to directory \"%2\".%3") + .arg(poses.size()) + .arg(path) + .arg(ignoredCameras.size()>0?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(ignoredCameras.size()).arg(poses.size()):"")); } - fileOut.close(); + else + { + fileOut.close(); + QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt")); + } + } + else + { + QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out")); } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 6e4f2b2e..0b6219fc 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/ImageView.h" #include "rtabmap/gui/GraphViewer.h" #include "ExportCloudsDialog.h" +#include "ExportBundlerDialog.h" #include "PostProcessingDialog.h" #include "CreateSimpleCalibrationDialog.h" #include "DepthCalibrationDialog.h" @@ -2789,6 +2790,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget) const CloudViewer * cloudViewer = qobject_cast(widget); const ImageView * imageView = qobject_cast(widget); const ExportCloudsDialog * exportCloudsDialog = qobject_cast(widget); + const ExportBundlerDialog * exportBundlerDialog = qobject_cast(widget); const PostProcessingDialog * postProcessingDialog = qobject_cast(widget); const GraphViewer * graphViewer = qobject_cast(widget); const CalibrationDialog * calibrationDialog = qobject_cast(widget); @@ -2809,6 +2811,11 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget) exportCloudsDialog->saveSettings(settings); exportCloudsDialog->saveSettings(settingsTmp); } + else if(exportBundlerDialog) + { + exportBundlerDialog->saveSettings(settings); + exportBundlerDialog->saveSettings(settingsTmp); + } else if(postProcessingDialog) { postProcessingDialog->saveSettings(settings); @@ -2857,6 +2864,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget) CloudViewer * cloudViewer = qobject_cast(widget); ImageView * imageView = qobject_cast(widget); ExportCloudsDialog * exportCloudsDialog = qobject_cast(widget); + ExportBundlerDialog * exportBundlerDialog = qobject_cast(widget); PostProcessingDialog * postProcessingDialog = qobject_cast(widget); GraphViewer * graphViewer = qobject_cast(widget); CalibrationDialog * calibrationDialog = qobject_cast(widget); @@ -2877,6 +2885,11 @@ void PreferencesDialog::loadWidgetState(QWidget * widget) exportCloudsDialog->loadSettings(settings); exportCloudsDialog->saveSettings(settingsTmp); } + else if(exportBundlerDialog) + { + exportBundlerDialog->loadSettings(settings); + exportBundlerDialog->saveSettings(settingsTmp); + } else if(postProcessingDialog) { postProcessingDialog->loadSettings(settings); diff --git a/guilib/src/ui/exportBundlerDialog.ui b/guilib/src/ui/exportBundlerDialog.ui new file mode 100644 index 00000000..c54145a8 --- /dev/null +++ b/guilib/src/ui/exportBundlerDialog.ui @@ -0,0 +1,218 @@ + + + ExportBundlerDialog + + + + 0 + 0 + 521 + 320 + + + + Export Bundler + + + + + + Motion Blur Filtering + + + + + + These optional parameters can be used to avoid exporting blurred images. 0 means not used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + 0 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 0.000000000000000 + + + + + + + Laplacian variance threshold. Below this threshold, the image is considered blurred. 50 can be good default. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m/s + + + 0.000000000000000 + + + 0.100000000000000 + + + + + + + Maximum linear speed. Images taken on fast motions would be more blurry. 0.1 m/s can be good default. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + rad/s + + + 0.000000000000000 + + + 0.100000000000000 + + + + + + + Maximum angular speed. Images taken on fast motions would be more blurry. 0.4 rad/s can be good default. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Output folder: + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + true + + + + + + + ... + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults + + + + + + + + + buttonBox + accepted() + ExportBundlerDialog + accept() + + + 248 + 254 + + + 157 + 274 + + + + + buttonBox + rejected() + ExportBundlerDialog + reject() + + + 316 + 260 + + + 286 + 274 + + + + + diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 4af9fed3..b87e173a 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - -2146 + -2684 773 - 4045 + 4103 @@ -1778,7 +1778,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - Camera filtering. By comparing poses in the same area, only one camera in a fixed radius and angle is used for texturing. + Camera filtering. Some criteria to select which cameras are used for texturing. true @@ -1876,24 +1876,24 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. Camera Filtering - + m - 0.010000000000000 + 0.000000000000000 0.100000000000000 - + - Radius. + Only one camera in a fixed radius and angle is used for texturing. Radius of 0 m means disabled. true @@ -1903,7 +1903,26 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - + + + + + + + 0 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 0.000000000000000 + + + + deg @@ -1919,10 +1938,49 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - - + + - Angle. + Laplacian variance threshold. Below this threshold, the image is considered blurred. 0 means disabled. 50 can be good default. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + rad/s + + + 0.000000000000000 + + + 0.100000000000000 + + + + + + + m/s + + + 0.000000000000000 + + + 0.100000000000000 + + + + + + + Maximum speed to keep a camera view for texturing. Images taken on fast motions would be more blurry. 0 means disabled. 0.1 m/s and 0.4 rad/s can be good defaults. true diff --git a/guilib/src/ui/exportDialog.ui b/guilib/src/ui/exportDialog.ui index 0d8e1d94..1f034c37 100644 --- a/guilib/src/ui/exportDialog.ui +++ b/guilib/src/ui/exportDialog.ui @@ -142,7 +142,7 @@ - Depth 2D (laser scans) + Laser scans true