i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdometryEvent: fixed velocity() returning always empty. Export: added options to filter blurred cameras based on laplacian threshold or velocity (#225).

Browse Source
This commit is contained in:
matlabbe
2017-08-07 16:31:49 -04:00
parent 75854fc026
commit 1e335e53ba
12 changed files with 753 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
guilib/src/CMakeLists.txt
View File

@@ -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

128
guilib/src/ExportBundlerDialog.cpp Normal file
View File

@@ -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 <QFileDialog>
#include <QPushButton>
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();
}
}

71
guilib/src/ExportBundlerDialog.h Normal file
View File

@@ -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 <QDialog>
#include <QSettings>
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_ */

107
guilib/src/ExportCloudsDialog.cpp
View File

@@ -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<int, Transform> cameraPoses;
std::map<int, std::vector<CameraModel> > cameraModels;
std::map<int, cv::Mat > cameraDepths;
int ignoredCameras = 0;
for(std::map<int, Transform>::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<float> 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<double>(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);

192
guilib/src/MainWindow.cpp
View File

@@ -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<int, Transform> 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<int> ignoredCameras;
for(std::map<int, Transform>::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<float> & 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<double>(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<int, Transform>::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"));
}
}
}

13
guilib/src/PreferencesDialog.cpp
View File

@@ -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<const CloudViewer*>(widget);
const ImageView * imageView = qobject_cast<const ImageView*>(widget);
const ExportCloudsDialog * exportCloudsDialog = qobject_cast<const ExportCloudsDialog*>(widget);
const ExportBundlerDialog * exportBundlerDialog = qobject_cast<const ExportBundlerDialog*>(widget);
const PostProcessingDialog * postProcessingDialog = qobject_cast<const PostProcessingDialog *>(widget);
const GraphViewer * graphViewer = qobject_cast<const GraphViewer *>(widget);
const CalibrationDialog * calibrationDialog = qobject_cast<const CalibrationDialog *>(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<CloudViewer*>(widget);
ImageView * imageView = qobject_cast<ImageView*>(widget);
ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
ExportBundlerDialog * exportBundlerDialog = qobject_cast<ExportBundlerDialog*>(widget);
PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
CalibrationDialog * calibrationDialog = qobject_cast<CalibrationDialog *>(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);

218
guilib/src/ui/exportBundlerDialog.ui Normal file
View File

@@ -0,0 +1,218 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>ExportBundlerDialog</class>
<widget class="QDialog" name="ExportBundlerDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>521</width>
<height>320</height>
</rect>
</property>
<property name="windowTitle">
<string>Export Bundler</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Motion Blur Filtering</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_40">
<property name="text">
<string>These optional parameters can be used to avoid exporting blurred images. 0 means not used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_laplacianVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_38">
<property name="text">
<string>Laplacian variance threshold. Below this threshold, the image is considered blurred. 50 can be good default.</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="QDoubleSpinBox" name="doubleSpinBox_linearSpeed">
<property name="suffix">
<string> m/s</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_39">
<property name="text">
<string>Maximum linear speed. Images taken on fast motions would be more blurry. 0.1 m/s can be good default.</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="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_angularSpeed">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_41">
<property name="text">
<string>Maximum angular speed. Images taken on fast motions would be more blurry. 0.4 rad/s can be good default.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="label_42">
<property name="text">
<string>Output folder:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_path">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="toolButton_path">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults</set>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections>
<connection>
<sender>buttonBox</sender>
<signal>accepted()</signal>
<receiver>ExportBundlerDialog</receiver>
<slot>accept()</slot>
<hints>
<hint type="sourcelabel">
<x>248</x>
<y>254</y>
</hint>
<hint type="destinationlabel">
<x>157</x>
<y>274</y>
</hint>
</hints>
</connection>
<connection>
<sender>buttonBox</sender>
<signal>rejected()</signal>
<receiver>ExportBundlerDialog</receiver>
<slot>reject()</slot>
<hints>
<hint type="sourcelabel">
<x>316</x>
<y>260</y>
</hint>
<hint type="destinationlabel">
<x>286</x>
<y>274</y>
</hint>
</hints>
</connection>
</connections>
</ui>

80
guilib/src/ui/exportCloudsDialog.ui
View File

@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-2146</y>
<y>-2684</y>
<width>773</width>
<height>4045</height>
<height>4103</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -1778,7 +1778,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<item row="8" column="1">
<widget class="QLabel" name="label_meshingTextureSize_3">
<property name="text">
<string>Camera filtering. By comparing poses in the same area, only one camera in a fixed radius and angle is used for texturing.</string>
<string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -1876,24 +1876,24 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<property name="title">
<string>Camera Filtering</string>
</property>
<layout class="QGridLayout" name="gridLayout_16" columnstretch="0,1">
<layout class="QGridLayout" name="gridLayout_16" columnstretch="0,0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraFilterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
<double>0.000000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<item row="0" column="2">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Radius.</string>
<string>Only one camera in a fixed radius and angle is used for texturing. Radius of 0 m means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -1903,7 +1903,26 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_laplacianVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraFilterAngle">
<property name="suffix">
<string> deg</string>
@@ -1919,10 +1938,49 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<item row="1" column="2">
<widget class="QLabel" name="label_38">
<property name="text">
<string>Angle.</string>
<string>Laplacian variance threshold. Below this threshold, the image is considered blurred. 0 means disabled. 50 can be good default.</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="QDoubleSpinBox" name="doubleSpinBox_cameraFilterVelRad">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraFilterVel">
<property name="suffix">
<string> m/s</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_39">
<property name="text">
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>

2
guilib/src/ui/exportDialog.ui
View File

@@ -142,7 +142,7 @@
<item>
<widget class="QCheckBox" name="checkBox_depth2d">
<property name="text">
<string>Depth 2D (laser scans)</string>
<string>Laser scans</string>
</property>
<property name="checked">
<bool>true</bool>