diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index 230836f8..04db359b 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -170,6 +170,10 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, int minNeighborsInRadius); +pcl::IndicesPtr RTABMAP_EXP radiusFiltering( + const pcl::PointCloud::Ptr & cloud, + float radiusSearch, + int minNeighborsInRadius); pcl::IndicesPtr RTABMAP_EXP radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, @@ -196,6 +200,11 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering( const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius); +pcl::IndicesPtr RTABMAP_EXP radiusFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float radiusSearch, + int minNeighborsInRadius); pcl::IndicesPtr RTABMAP_EXP radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 443cee72..debab6b8 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -159,6 +159,11 @@ pcl::PointCloud::Ptr RTABMAP_EXP mls( float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION int dilationIterations = 0); // VOXEL_GRID_DILATION +void RTABMAP_EXP adjustNormalsToViewPoints( + const std::map & poses, + const pcl::PointCloud::Ptr & rawCloud, + const std::vector & rawCameraIndices, + pcl::PointCloud::Ptr & cloud); void RTABMAP_EXP adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 8423717a..1c24a0f1 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -420,6 +420,14 @@ pcl::IndicesPtr radiusFiltering( pcl::IndicesPtr indices(new std::vector); return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius); } +pcl::IndicesPtr radiusFiltering( + const pcl::PointCloud::Ptr & cloud, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::IndicesPtr indices(new std::vector); + return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius); +} pcl::IndicesPtr radiusFiltering( const pcl::PointCloud::Ptr & cloud, float radiusSearch, @@ -483,6 +491,51 @@ pcl::IndicesPtr radiusFiltering( return output; } } +pcl::IndicesPtr radiusFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree(false)); + + if(indices->size()) + { + pcl::IndicesPtr output(new std::vector(indices->size())); + int oi = 0; // output iterator + tree->setInputCloud(cloud, indices); + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector kDistances; + int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances); + if(k > minNeighborsInRadius) + { + output->at(oi++) = indices->at(i); + } + } + output->resize(oi); + return output; + } + else + { + pcl::IndicesPtr output(new std::vector(cloud->size())); + int oi = 0; // output iterator + tree->setInputCloud(cloud); + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector kDistances; + int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances); + if(k > minNeighborsInRadius) + { + output->at(oi++) = i; + } + } + output->resize(oi); + return output; + } +} pcl::IndicesPtr radiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index c87fd13c..2572e09b 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -724,6 +724,52 @@ pcl::PointCloud::Ptr mls( return cloud_with_normals; } +void adjustNormalsToViewPoints( + const std::map & poses, + const pcl::PointCloud::Ptr & rawCloud, + const std::vector & rawCameraIndices, + pcl::PointCloud::Ptr & cloud) +{ + if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size()) + { + pcl::search::KdTree::Ptr rawTree (new pcl::search::KdTree); + rawTree->setInputCloud (rawCloud); + + for(unsigned int i=0; isize(); ++i) + { + pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z); + if(pcl::isFinite(normal)) + { + std::vector indices; + std::vector dist; + rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist); + UASSERT(indices.size() == 1); + if(indices.size() && indices[0]>=0) + { + Transform p = poses.at(rawCameraIndices[indices[0]]); + pcl::PointXYZ viewpoint(p.x(), p.y(), p.z()); + Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap(); + + Eigen::Vector3f n(normal.x, normal.y, normal.z); + + float result = v.dot(n); + if(result < 0) + { + //reverse normal + cloud->points[i].normal_x *= -1.0f; + cloud->points[i].normal_y *= -1.0f; + cloud->points[i].normal_z *= -1.0f; + } + } + else + { + UWARN("Not found camera viewpoint for point %d", i); + } + } + } + } +} + void adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, @@ -737,30 +783,34 @@ void adjustNormalsToViewPoints( for(unsigned int i=0; isize(); ++i) { - std::vector indices; - std::vector dist; - rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist); - UASSERT(indices.size() == 1); - if(indices.size() && indices[0]>=0) + pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z); + if(pcl::isFinite(normal)) { - Transform p = poses.at(rawCameraIndices[indices[0]]); - pcl::PointXYZ viewpoint(p.x(), p.y(), p.z()); - Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap(); - - Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z); - - float result = v.dot(n); - if(result < 0) + std::vector indices; + std::vector dist; + rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist); + UASSERT(indices.size() == 1); + if(indices.size() && indices[0]>=0) { - //reverse normal - cloud->points[i].normal_x *= -1.0f; - cloud->points[i].normal_y *= -1.0f; - cloud->points[i].normal_z *= -1.0f; + Transform p = poses.at(rawCameraIndices[indices[0]]); + pcl::PointXYZ viewpoint(p.x(), p.y(), p.z()); + Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap(); + + Eigen::Vector3f n(normal.x, normal.y, normal.z); + + float result = v.dot(n); + if(result < 0) + { + //reverse normal + cloud->points[i].normal_x *= -1.0f; + cloud->points[i].normal_y *= -1.0f; + cloud->points[i].normal_z *= -1.0f; + } + } + else + { + UWARN("Not found camera viewpoint for point %d", i); } - } - else - { - UWARN("Not found camera viewpoint for point %d", i); } } } diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 83875f21..6aa6928e 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -68,6 +68,7 @@ class StatsToolBox; class ProgressDialog; class TwistGridWidget; class ExportCloudsDialog; +class ExportScansDialog; class PostProcessingDialog; class DataRecorder; @@ -245,9 +246,6 @@ private: void exportPoses(int format); QString captureScreen(bool cacheInRAM = false); - bool getExportedScans(std::map::Ptr > & scans); - void saveScans(const std::map::Ptr> & clouds, bool binaryMode = true); - private: Ui_mainWindow * _ui; @@ -259,7 +257,8 @@ private: //Dialogs PreferencesDialog * _preferencesDialog; AboutDialog * _aboutDialog; - ExportCloudsDialog * _exportDialog; + ExportCloudsDialog * _exportCloudsDialog; + ExportScansDialog * _exportScansDialog; PostProcessingDialog * _postProcessingDialog; DataRecorder * _dataRecorder; @@ -287,7 +286,7 @@ private: std::map::Ptr, pcl::IndicesPtr> > _createdClouds; std::pair::Ptr, pcl::IndicesPtr> > _previousCloud; // used for subtraction - std::map::Ptr > _createdScans; + std::map _createdScans; std::map > _projectionLocalMaps; // std::map > _gridLocalMaps; // diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 59660f49..e3c98f56 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -22,6 +22,7 @@ SET(headers_ui ./ExportDialog.h ./PostProcessingDialog.h ./ExportCloudsDialog.h + ./ExportScansDialog.h ./MapVisibilityWidget.h ../include/${PROJECT_PREFIX}/gui/GraphViewer.h ./CreateSimpleCalibrationDialog.h @@ -38,6 +39,7 @@ SET(uis ./ui/exportDialog.ui ./ui/postProcessingDialog.ui ./ui/exportCloudsDialog.ui + ./ui/exportScansDialog.ui ./ui/calibrationDialog.ui ./ui/createSimpleCalibrationDialog.ui ) @@ -84,6 +86,7 @@ SET(SRC_FILES ./ExportDialog.cpp ./PostProcessingDialog.cpp ./ExportCloudsDialog.cpp + ./ExportScansDialog.cpp ./MapVisibilityWidget.cpp ./GraphViewer.cpp ./CreateSimpleCalibrationDialog.cpp diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index d3cce145..9d81e150 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -333,6 +333,8 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group) { settings.endGroup(); } + + this->update(); } bool CloudViewer::updateCloudPose( diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 52830d27..f63ee2d7 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -484,7 +484,7 @@ void ExportCloudsDialog::viewClouds( } } -bool removeDir(const QString & dirName) +bool removeDirRecursively(const QString & dirName) { bool result = true; QDir dir(dirName); @@ -492,7 +492,7 @@ bool removeDir(const QString & dirName) if (dir.exists(dirName)) { Q_FOREACH(QFileInfo info, dir.entryInfoList(QDir::NoDotAndDotDot | QDir::System | QDir::Hidden | QDir::AllDirs | QDir::Files, QDir::DirsFirst)) { if (info.isDir()) { - result = removeDir(info.absoluteFilePath()); + result = removeDirRecursively(info.absoluteFilePath()); } else { result = QFile::remove(info.absoluteFilePath()); @@ -850,7 +850,7 @@ bool ExportCloudsDialog::getExportedClouds( if(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()) { QDir dir(workingDirectory); - removeDir(workingDirectory+QDir::separator()+"tmp_textures"); + removeDirRecursively(workingDirectory+QDir::separator()+"tmp_textures"); dir.mkdir("tmp_textures"); int i=0; for(std::map::iterator iter=meshes.begin(); @@ -1360,7 +1360,7 @@ void ExportCloudsDialog::saveTextureMeshes( pcl::TextureMesh mesh; mesh.tex_coordinates = meshes.begin()->second->tex_coordinates; mesh.tex_materials = meshes.begin()->second->tex_materials; - removeDir(QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+QFileInfo(path).baseName()); + removeDirRecursively(QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+QFileInfo(path).baseName()); QDir(QFileInfo(path).absoluteDir().absolutePath()).mkdir(QFileInfo(path).baseName()); for(unsigned int i=0;isecond->tex_materials.size(); ++i) { diff --git a/guilib/src/ExportScansDialog.cpp b/guilib/src/ExportScansDialog.cpp new file mode 100644 index 00000000..05166dd4 --- /dev/null +++ b/guilib/src/ExportScansDialog.cpp @@ -0,0 +1,597 @@ +/* +Copyright (c) 2010-2014, 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 "ExportScansDialog.h" +#include "ui_ExportScansDialog.h" + +#include "rtabmap/gui/ProgressDialog.h" +#include "rtabmap/gui/CloudViewer.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UThread.h" +#include "rtabmap/utilite/UStl.h" + +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_surface.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/Graph.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rtabmap { + +ExportScansDialog::ExportScansDialog(QWidget *parent) : + QDialog(parent) +{ + _ui = new Ui_ExportScansDialog(); + _ui->setupUi(this); + + connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults())); + + restoreDefaults(); + + connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + + connect(_ui->groupBox_regenerate, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + + connect(_ui->groupBox_filtering, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_filteringRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_filteringMinNeighbors, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + + connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + + _progressDialog = new ProgressDialog(this); + _progressDialog->setVisible(false); + _progressDialog->setAutoClose(true, 2); + _progressDialog->setMinimumWidth(600); +} + +ExportScansDialog::~ExportScansDialog() +{ + delete _ui; +} + +void ExportScansDialog::saveSettings(QSettings & settings, const QString & group) const +{ + if(!group.isEmpty()) + { + settings.beginGroup(group); + } + settings.setValue("binary", _ui->checkBox_binary->isChecked()); + settings.setValue("normals_k", _ui->spinBox_normalKSearch->value()); + + settings.setValue("regenerate", _ui->groupBox_regenerate->isChecked()); + settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value()); + + settings.setValue("filtering", _ui->groupBox_filtering->isChecked()); + settings.setValue("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()); + settings.setValue("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value()); + + settings.setValue("assemble", _ui->checkBox_assemble->isChecked()); + settings.setValue("assemble_voxel",_ui->doubleSpinBox_voxelSize_assembled->value()); + + if(!group.isEmpty()) + { + settings.endGroup(); + } +} + +void ExportScansDialog::loadSettings(QSettings & settings, const QString & group) +{ + if(!group.isEmpty()) + { + settings.beginGroup(group); + } + + _ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool()); + _ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt()); + + _ui->groupBox_regenerate->setChecked(settings.value("regenerate", _ui->groupBox_regenerate->isChecked()).toBool()); + _ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt()); + + _ui->groupBox_filtering->setChecked(settings.value("filtering", _ui->groupBox_filtering->isChecked()).toBool()); + _ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble()); + _ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value()).toInt()); + + _ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool()); + _ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble()); + + if(!group.isEmpty()) + { + settings.endGroup(); + } +} + +void ExportScansDialog::restoreDefaults() +{ + _ui->checkBox_binary->setChecked(true); + _ui->spinBox_normalKSearch->setValue(20); + + _ui->groupBox_regenerate->setChecked(false); + _ui->spinBox_decimation->setValue(1); + + _ui->groupBox_filtering->setChecked(false); + _ui->doubleSpinBox_filteringRadius->setValue(0.02); + _ui->spinBox_filteringMinNeighbors->setValue(2); + + _ui->checkBox_assemble->setChecked(true); + _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01); + + this->update(); +} + +void ExportScansDialog::setSaveButton() +{ + _ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(false); + _ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(true); + _ui->checkBox_binary->setVisible(true); + _ui->label_binaryFile->setVisible(true); +} + +void ExportScansDialog::setOkButton() +{ + _ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(true); + _ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(false); + _ui->checkBox_binary->setVisible(false); + _ui->label_binaryFile->setVisible(false); +} + +void ExportScansDialog::enableRegeneration(bool enabled) +{ + if(!enabled) + { + _ui->groupBox_regenerate->setChecked(false); + } + _ui->groupBox_regenerate->setEnabled(enabled); +} + +void ExportScansDialog::exportScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdScans, + const QString & workingDirectory) +{ + std::map::Ptr> clouds; + + setSaveButton(); + + if(getExportedScans( + poses, + mapIds, + cachedSignatures, + createdScans, + workingDirectory, + clouds)) + { + saveScans(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked()); + _progressDialog->setValue(_progressDialog->maximumSteps()); + } +} + +void ExportScansDialog::viewScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdScans, + const QString & workingDirectory) +{ + std::map::Ptr> clouds; + + setOkButton(); + if(getExportedScans( + poses, + mapIds, + cachedSignatures, + createdScans, + workingDirectory, + clouds)) + { + QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window); + window->setAttribute(Qt::WA_DeleteOnClose, true); + window->setWindowTitle(tr("Scans (%1 nodes)").arg(clouds.size())); + window->setMinimumWidth(800); + window->setMinimumHeight(600); + + CloudViewer * viewer = new CloudViewer(window); + viewer->setCameraLockZ(false); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(viewer); + window->setLayout(layout); + connect(window, SIGNAL(finished(int)), viewer, SLOT(clear())); + + window->show(); + + uSleep(500); + if(clouds.size()) + { + for(std::map::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) + { + _progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size())); + _progressDialog->incrementStep(); + + QColor color = Qt::gray; + int mapId = uValue(mapIds, iter->first, -1); + if(mapId >= 0) + { + color = (Qt::GlobalColor)(mapId % 12 + 7 ); + } + viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?poses.at(iter->first):Transform::getIdentity()); + _progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size())); + } + } + + _progressDialog->setValue(_progressDialog->maximumSteps()); + viewer->update(); + } +} + +bool ExportScansDialog::getExportedScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdClouds, + const QString & workingDirectory, + std::map::Ptr> & cloudsWithNormals) +{ + enableRegeneration(cachedSignatures.size()); + if(this->exec() == QDialog::Accepted) + { + _progressDialog->resetProgress(); + _progressDialog->show(); + int mul = 1; + if(_ui->checkBox_assemble->isChecked()) + { + mul+=1; + } + mul+=1; // normals + _progressDialog->setMaximumSteps(int(poses.size())*mul+1); + + std::map::Ptr> clouds = this->getScans( + poses, + cachedSignatures, + createdClouds); + + if(_ui->checkBox_assemble->isChecked()) + { + _progressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size())); + QApplication::processEvents(); + + pcl::PointCloud::Ptr rawAssembledCloud(new pcl::PointCloud); + std::vector rawCameraIndices; + int i =0; + pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + for(std::map::Ptr>::iterator iter=clouds.begin(); + iter!= clouds.end(); + ++iter) + { + pcl::PointCloud::Ptr transformed(new pcl::PointCloud); + transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first)); + + *assembledCloud += *transformed; + + rawCameraIndices.resize(assembledCloud->size(), iter->first); + + _progressDialog->appendText(tr("Assembled cloud %1, total=%2 (%3/%4).").arg(iter->first).arg(assembledCloud->size()).arg(++i).arg(clouds.size())); + _progressDialog->incrementStep(); + QApplication::processEvents(); + } + + pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); + + if(_ui->doubleSpinBox_voxelSize_assembled->value()) + { + _progressDialog->appendText(tr("Voxelize cloud (%1 points, voxel size = %2 m)...") + .arg(assembledCloud->size()) + .arg(_ui->doubleSpinBox_voxelSize_assembled->value())); + QApplication::processEvents(); + + assembledCloud = util3d::voxelize( + assembledCloud, + _ui->doubleSpinBox_voxelSize_assembled->value()); + + if(_ui->spinBox_normalKSearch->value() > 0) + { + _progressDialog->appendText(tr("Compute normals (%1 points)...") + .arg(assembledCloud->size())); + QApplication::processEvents(); + + pcl::PointCloud::Ptr cloudXYZ(new pcl::PointCloud); + pcl::copyPointCloud(*assembledCloud, *cloudXYZ); + assembledCloud = util3d::computeNormals( + cloudXYZ, + _ui->spinBox_normalKSearch->value()); + + _progressDialog->appendText(tr("Update %1 normals with %2 camera views...") + .arg(assembledCloud->size()).arg(poses.size())); + util3d::adjustNormalsToViewPoints( + poses, + rawAssembledCloud, + rawCameraIndices, + assembledCloud); + } + } + + clouds.clear(); + clouds.insert(std::make_pair(0, assembledCloud)); + } + + //fill cloudWithNormals + for(std::map::Ptr>::iterator iter=clouds.begin(); + iter!= clouds.end(); + ++iter) + { + pcl::PointCloud::Ptr cloudWithNormals = iter->second; + + cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals)); + + _progressDialog->incrementStep(); + QApplication::processEvents(); + } + + return true; + } + return false; +} + +std::map::Ptr> ExportScansDialog::getScans( + const std::map & poses, + const QMap & cachedSignatures, + const std::map & createdScans) const +{ + std::map::Ptr> clouds; + int i=0; + pcl::PointCloud::Ptr previousCloud; + pcl::IndicesPtr previousIndices; + Transform previousPose; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + int points = 0; + if(!iter->second.isNull()) + { + cv::Mat scan; + if(_ui->groupBox_regenerate->isChecked()) + { + if(cachedSignatures.contains(iter->first)) + { + const Signature & s = cachedSignatures.find(iter->first).value(); + SensorData d = s.sensorData(); + d.uncompressData(0, 0, &scan); + if(!scan.empty()) + { + if(_ui->spinBox_decimation->value() > 1) + { + scan = util3d::downsample(scan, _ui->spinBox_decimation->value()); + } + } + } + else + { + UERROR("Scan %d not found in cache!", iter->first); + } + } + else + { + scan = uValue(createdScans, iter->first, cv::Mat()); + } + + if(!scan.empty()) + { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if(scan.channels() == 6 && _ui->doubleSpinBox_voxelSize_assembled->value() == 0.0) + { + cloud = util3d::laserScanToPointCloudNormal(scan); + } + else + { + pcl::PointCloud::Ptr cloudXYZ = util3d::laserScanToPointCloud(scan); + + if(_ui->doubleSpinBox_voxelSize_assembled->value() > 0.0) + { + cloudXYZ = util3d::voxelize( + cloudXYZ, + _ui->doubleSpinBox_voxelSize_assembled->value()); + } + + if(!_ui->checkBox_assemble->isChecked() && _ui->spinBox_normalKSearch->value() > 0) + { + cloud = util3d::computeNormals( + cloudXYZ, + _ui->spinBox_normalKSearch->value()); + } + else + { + pcl::copyPointCloud(*cloudXYZ, *cloud); + } + } + + if(cloud->size()) + { + if(_ui->groupBox_filtering->isChecked() && + _ui->doubleSpinBox_filteringRadius->value() > 0.0f && + _ui->spinBox_filteringMinNeighbors->value() > 0) + { + pcl::IndicesPtr indices = util3d::radiusFiltering(cloud, _ui->doubleSpinBox_filteringRadius->value(), _ui->spinBox_filteringMinNeighbors->value()); + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *indices, *tmp); + cloud = tmp; + } + + clouds.insert(std::make_pair(iter->first, cloud)); + points = cloud->size(); + } + } + } + else + { + UERROR("transform is null!?"); + } + + if(points>0) + { + _progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).") + .arg(iter->first).arg(points).arg(++i).arg(poses.size())); + } + else + { + _progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); + } + _progressDialog->incrementStep(); + QApplication::processEvents(); + } + + return clouds; +} + + +void ExportScansDialog::saveScans( + const QString & workingDirectory, + const std::map & poses, + const std::map::Ptr> & clouds, + bool binaryMode) +{ + if(clouds.size() == 1) + { + QString path = QFileDialog::getSaveFileName(this, tr("Save scan to ..."), workingDirectory+QDir::separator()+"scan.ply", tr("Point cloud data (*.ply *.pcd)")); + if(!path.isEmpty()) + { + if(clouds.begin()->second->size()) + { + _progressDialog->appendText(tr("Saving the scan (%1 points)...").arg(clouds.begin()->second->size())); + + bool success =false; + if(QFileInfo(path).suffix() == "pcd") + { + success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; + } + else if(QFileInfo(path).suffix() == "ply") + { + success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; + } + else if(QFileInfo(path).suffix() == "") + { + //use ply by default + path += ".ply"; + success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; + } + else + { + UERROR("Extension not recognized! (%s) Should be one of (*.ply *.pcd).", QFileInfo(path).suffix().toStdString().c_str()); + } + if(success) + { + _progressDialog->incrementStep(); + _progressDialog->appendText(tr("Saving the scan (%1 points)... done.").arg(clouds.begin()->second->size())); + + QMessageBox::information(this, tr("Save successful!"), tr("Scan saved to \"%1\"").arg(path)); + } + else + { + QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path)); + } + } + else + { + QMessageBox::warning(this, tr("Save failed!"), tr("Scan is empty...")); + } + } + } + else if(clouds.size()) + { + QString path = QFileDialog::getExistingDirectory(this, tr("Save scans to (*.ply *.pcd)..."), workingDirectory, 0); + if(!path.isEmpty()) + { + bool ok = false; + QStringList items; + items.push_back("ply"); + items.push_back("pcd"); + QString suffix = QInputDialog::getItem(this, tr("File format"), tr("Which format?"), items, 0, false, &ok); + + if(ok) + { + QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "scan", &ok); + + if(ok) + { + for(std::map::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) + { + if(iter->second->size()) + { + pcl::PointCloud::Ptr transformedCloud; + transformedCloud = util3d::transformPointCloud(iter->second, poses.at(iter->first)); + + QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); + bool success =false; + if(suffix == "pcd") + { + success = pcl::io::savePCDFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0; + } + else if(suffix == "ply") + { + success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0; + } + else + { + UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str()); + } + if(success) + { + _progressDialog->appendText(tr("Saved scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile)); + } + else + { + _progressDialog->appendText(tr("Failed saving scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile)); + } + } + else + { + _progressDialog->appendText(tr("Scan %1 is empty!").arg(iter->first)); + } + _progressDialog->incrementStep(); + QApplication::processEvents(); + } + } + } + } + } +} + +} diff --git a/guilib/src/ExportScansDialog.h b/guilib/src/ExportScansDialog.h new file mode 100644 index 00000000..bf56c26d --- /dev/null +++ b/guilib/src/ExportScansDialog.h @@ -0,0 +1,109 @@ +/* +Copyright (c) 2010-2014, 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 EXPORTSCANSDIALOG_H_ +#define EXPORTSCANSDIALOG_H_ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +class Ui_ExportScansDialog; +class QAbstractButton; + +namespace rtabmap { +class ProgressDialog; + +class ExportScansDialog : public QDialog +{ + Q_OBJECT + +public: + ExportScansDialog(QWidget *parent = 0); + + virtual ~ExportScansDialog(); + + void saveSettings(QSettings & settings, const QString & group = "") const; + void loadSettings(QSettings & settings, const QString & group = ""); + + void exportScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdScans, + const QString & workingDirectory); + + void viewScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdScans, + const QString & workingDirectory); + +signals: + void configChanged(); + +public slots: + void restoreDefaults(); + +private: + std::map::Ptr> getScans( + const std::map & poses, + const QMap & cachedSignatures, + const std::map & createdScans) const; + bool getExportedScans( + const std::map & poses, + const std::map & mapIds, + const QMap & cachedSignatures, + const std::map & createdScans, + const QString & workingDirectory, + std::map::Ptr> & clouds); + void saveScans(const QString & workingDirectory, + const std::map & poses, + const std::map::Ptr> & clouds, + bool binaryMode = true); + + void setSaveButton(); + void setOkButton(); + void enableRegeneration(bool enabled); + +private: + Ui_ExportScansDialog * _ui; + ProgressDialog * _progressDialog; +}; + +} + +#endif /* EXPORTSCANSDIALOG_H_ */ diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index a6ab72da..3cb38205 100644 --- a/guilib/src/ImageView.cpp +++ b/guilib/src/ImageView.cpp @@ -726,9 +726,13 @@ void ImageView::setImage(const QImage & image) this->updateOpacity(); } } - else + + if(image.rect().isValid()) { this->setSceneRect(image.rect()); + } + else if(!_graphicsView->isVisible()) + { this->update(); } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 29247028..f3882228 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UCv2Qt.h" #include "ExportCloudsDialog.h" +#include "ExportScansDialog.h" #include "AboutDialog.h" #include "PostProcessingDialog.h" @@ -127,7 +128,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _odomThread(0), _preferencesDialog(0), _aboutDialog(0), - _exportDialog(0), + _exportCloudsDialog(0), + _exportScansDialog(0), _dataRecorder(0), _lastId(0), _processingStatistics(false), @@ -165,8 +167,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : // Create dialogs _aboutDialog = new AboutDialog(this); _aboutDialog->setObjectName("AboutDialog"); - _exportDialog = new ExportCloudsDialog(this); - _exportDialog->setObjectName("ExportCloudsDialog"); + _exportCloudsDialog = new ExportCloudsDialog(this); + _exportCloudsDialog->setObjectName("ExportCloudsDialog"); + _exportScansDialog = new ExportScansDialog(this); + _exportScansDialog->setObjectName("ExportScansDialog"); _postProcessingDialog = new PostProcessingDialog(this); _postProcessingDialog->setObjectName("PostProcessingDialog"); @@ -199,7 +203,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : bool statusBarShown = false; _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown); _preferencesDialog->loadWindowGeometry(_preferencesDialog); - _preferencesDialog->loadWindowGeometry(_exportDialog); + _preferencesDialog->loadWindowGeometry(_exportCloudsDialog); + _preferencesDialog->loadWindowGeometry(_exportScansDialog); _preferencesDialog->loadWindowGeometry(_postProcessingDialog); _preferencesDialog->loadWindowGeometry(_aboutDialog); setupMainLayout(_preferencesDialog->isVerticalLayoutUsed()); @@ -411,7 +416,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_ui->widget_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified())); - connect(_exportDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); + connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); + connect(_exportScansDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified())); connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified())); connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified())); @@ -466,7 +472,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _preferencesDialog->loadWidgetState(_ui->widget_cloudViewer); //dialog states - _preferencesDialog->loadWidgetState(_exportDialog); + _preferencesDialog->loadWidgetState(_exportCloudsDialog); + _preferencesDialog->loadWidgetState(_exportScansDialog); _preferencesDialog->loadWidgetState(_postProcessingDialog); if(_ui->statsToolBox->findChildren().size() == 0) @@ -1288,9 +1295,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->imageView_source->setBackgroundColor(Qt::gray); } // Set color code as tooltip - if(_ui->imageView_source->toolTip().isEmpty()) + if(_ui->label_refId->toolTip().isEmpty()) { - _ui->imageView_source->setToolTip( + _ui->label_refId->setToolTip( "Background Color Code:\n" " Blue = Weight Update Merged\n" " Dark Blue = Weight Update\n" @@ -1299,9 +1306,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) " Gray = Small Movement"); } // Set color code as tooltip - if(_ui->imageView_loopClosure->toolTip().isEmpty()) + if(_ui->label_matchId->toolTip().isEmpty()) { - _ui->imageView_loopClosure->setToolTip( + _ui->label_matchId->setToolTip( "Background Color Code:\n" " Green = Accepted Loop Closure Detection\n" " Red = Rejected Loop Closure Detection\n" @@ -2321,9 +2328,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } else { - pcl::PointCloud::Ptr cloudXYZ(new pcl::PointCloud); - pcl::copyPointCloud(*cloud, *cloudXYZ); - _createdScans.insert(std::make_pair(nodeId, cloudXYZ)); + if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) + { + //reconvert the voxelized cloud + scan = util3d::laserScanFromPointCloud(*cloud); + } + _createdScans.insert(std::make_pair(nodeId, scan)); } } else @@ -2345,7 +2355,19 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } else { - _createdScans.insert(std::make_pair(nodeId, cloud)); + if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) + { + //reconvert the voxelized cloud + if(scan.channels() == 2) + { + scan = util3d::laserScan2dFromPointCloud(*cloud); + } + else + { + scan = util3d::laserScanFromPointCloud(*cloud); + } + } + _createdScans.insert(std::make_pair(nodeId, scan)); if(scan.channels() == 2) { @@ -3229,7 +3251,8 @@ void MainWindow::saveConfigGUI() _preferencesDialog->saveWidgetState(_ui->imageView_source); _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure); _preferencesDialog->saveWidgetState(_ui->imageView_odometry); - _preferencesDialog->saveWidgetState(_exportDialog); + _preferencesDialog->saveWidgetState(_exportCloudsDialog); + _preferencesDialog->saveWidgetState(_exportScansDialog); _preferencesDialog->saveWidgetState(_postProcessingDialog); _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView); _preferencesDialog->saveSettings(); @@ -4934,164 +4957,43 @@ void MainWindow::exportGridMap() void MainWindow::exportScans() { - std::map::Ptr> scans; - if(getExportedScans(scans)) - { - if(scans.size()) - { - QMessageBox::StandardButton b = QMessageBox::question(this, - tr("Binary file?"), - tr("Do you want to save in binary mode?"), - QMessageBox::No | QMessageBox::Yes, - QMessageBox::Yes); - - if(b == QMessageBox::No || b == QMessageBox::Yes) - { - this->saveScans(scans, b == QMessageBox::Yes); - } - } - _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); - } -} - -void MainWindow::viewScans() -{ - std::map::Ptr> scans; - if(getExportedScans(scans)) - { - QDialog * window = new QDialog(this, Qt::Window); - window->setWindowFlags(Qt::Dialog); - window->setWindowTitle(tr("Scans (%1 nodes)").arg(scans.size())); - window->setMinimumWidth(800); - window->setMinimumHeight(600); - - CloudViewer * viewer = new CloudViewer(window); - viewer->setCameraLockZ(false); - - QVBoxLayout *layout = new QVBoxLayout(); - layout->addWidget(viewer); - window->setLayout(layout); - connect(window, SIGNAL(finished(int)), viewer, SLOT(clear())); - - window->show(); - - uSleep(500); - - for(std::map::Ptr>::iterator iter = scans.begin(); iter!=scans.end(); ++iter) - { - _initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)...").arg(iter->first).arg(iter->second->size())); - _initProgressDialog->incrementStep(); - - QColor color = Qt::red; - int mapId = uValue(_currentMapIds, iter->first, -1); - if(mapId >= 0) - { - color = (Qt::GlobalColor)(mapId % 12 + 7 ); - } - viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity()); - _initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size())); - } - - _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); - } -} - -bool MainWindow::getExportedScans(std::map::Ptr > & scans) -{ - QMessageBox::StandardButton b = QMessageBox::question(this, - tr("Assemble scans?"), - tr("Do you want to assemble the scans in only one cloud?"), - QMessageBox::No | QMessageBox::Yes, - QMessageBox::Yes); - - if(b != QMessageBox::No && b != QMessageBox::Yes) - { - return false; - } - - double voxel = 0.01; - bool assemble = b == QMessageBox::Yes; - - if(assemble) - { - bool ok; - voxel = QInputDialog::getDouble(this, tr("Voxel size"), tr("Voxel size (m):"), voxel, 0.00, 0.1, 2, &ok); - if(!ok) - { - return false; - } - } - - pcl::PointCloud::Ptr assembledScans(new pcl::PointCloud()); - std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); - - _initProgressDialog->resetProgress(); - _initProgressDialog->show(); - _initProgressDialog->setMaximumSteps(int(poses.size())*(assemble?1:2)+1); - - int count = 1; - int i = 0; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - bool inserted = false; - if(_createdScans.find(iter->first) != _createdScans.end()) - { - pcl::PointCloud::Ptr scan = _createdScans.at(iter->first); - if(scan->size()) - { - if(assemble) - { - *assembledScans += *util3d::transformPointCloud(scan, iter->second);; - - if(count++ % 100 == 0) - { - if(assembledScans->size() && voxel) - { - assembledScans = util3d::voxelize(assembledScans, voxel); - } - } - } - else - { - scans.insert(std::make_pair(iter->first, scan)); - } - inserted = true; - } - } - if(inserted) - { - _initProgressDialog->appendText(tr("Generated scan %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); - } - else - { - _initProgressDialog->appendText(tr("Ignored scan %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); - } - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - } - - if(assemble) - { - if(voxel && assembledScans->size()) - { - assembledScans = util3d::voxelize(assembledScans, voxel); - } - if(assembledScans->size()) - { - scans.insert(std::make_pair(0, assembledScans)); - } - } - return true; -} - -void MainWindow::exportClouds() -{ - if(_exportDialog->isVisible()) + if(_exportScansDialog->isVisible()) { return; } - _exportDialog->exportClouds( + _exportScansDialog->exportScans( + _currentPosesMap, + _currentMapIds, + _cachedSignatures, + _createdScans, + _preferencesDialog->getWorkingDirectory()); +} + +void MainWindow::viewScans() +{ + if(_exportScansDialog->isVisible()) + { + return; + } + + _exportScansDialog->viewScans( + _currentPosesMap, + _currentMapIds, + _cachedSignatures, + _createdScans, + _preferencesDialog->getWorkingDirectory()); + +} + +void MainWindow::exportClouds() +{ + if(_exportCloudsDialog->isVisible()) + { + return; + } + + _exportCloudsDialog->exportClouds( _currentPosesMap, _currentMapIds, _cachedSignatures, @@ -5101,12 +5003,12 @@ void MainWindow::exportClouds() void MainWindow::viewClouds() { - if(_exportDialog->isVisible()) + if(_exportCloudsDialog->isVisible()) { return; } - _exportDialog->viewClouds( + _exportCloudsDialog->viewClouds( _currentPosesMap, _currentMapIds, _cachedSignatures, @@ -5449,115 +5351,6 @@ void MainWindow::dataRecorderDestroyed() //END ACTIONS - -void MainWindow::saveScans(const std::map::Ptr> & scans, bool binaryMode) -{ - if(scans.size() == 1) - { - QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+QDir::separator()+"scan.ply", tr("Point cloud data (*.ply *.pcd)")); - if(!path.isEmpty()) - { - if(scans.begin()->second->size()) - { - _initProgressDialog->appendText(tr("Saving the scan (%1 points)...").arg(scans.begin()->second->size())); - - bool success =false; - if(QFileInfo(path).suffix() == "pcd") - { - success = pcl::io::savePCDFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0; - } - else if(QFileInfo(path).suffix() == "ply") - { - success = pcl::io::savePLYFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0; - } - else if(QFileInfo(path).suffix() == "") - { - //use ply by default - path += ".ply"; - success = pcl::io::savePLYFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0; - } - else - { - UERROR("Extension not recognized! (%s) Should be one of (*.ply *.pcd).", QFileInfo(path).suffix().toStdString().c_str()); - } - if(success) - { - _initProgressDialog->incrementStep(); - _initProgressDialog->appendText(tr("Saving the scan (%1 points)... done.").arg(scans.begin()->second->size())); - - QMessageBox::information(this, tr("Save successful!"), tr("Scan saved to \"%1\"").arg(path)); - } - else - { - QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path)); - } - } - else - { - QMessageBox::warning(this, tr("Save failed!"), tr("Scan is empty...")); - } - } - } - else if(scans.size()) - { - QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply *.pcd)..."), _preferencesDialog->getWorkingDirectory(), 0); - if(!path.isEmpty()) - { - bool ok = false; - QStringList items; - items.push_back("ply"); - items.push_back("pcd"); - QString suffix = QInputDialog::getItem(this, tr("File format"), tr("Which format?"), items, 0, false, &ok); - - if(ok) - { - QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "scan", &ok); - - if(ok) - { - for(std::map::Ptr >::const_iterator iter=scans.begin(); iter!=scans.end(); ++iter) - { - if(iter->second->size()) - { - pcl::PointCloud::Ptr transformedCloud; - transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first)); - - QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); - bool success =false; - if(suffix == "pcd") - { - success = pcl::io::savePCDFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0; - } - else if(suffix == "ply") - { - success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0; - } - else - { - UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str()); - } - if(success) - { - _initProgressDialog->appendText(tr("Saved scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile)); - } - else - { - _initProgressDialog->appendText(tr("Failed saving scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile)); - } - } - else - { - _initProgressDialog->appendText(tr("Scan %1 is empty!").arg(iter->first)); - } - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - } - } - } - } - } -} - // STATES // in monitoring state, only some actions are enabled diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 0f74bb7a..b02093ba 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -67,6 +67,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 "ExportScansDialog.h" #include "PostProcessingDialog.h" #include "CreateSimpleCalibrationDialog.h" @@ -2402,6 +2403,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 ExportScansDialog * exportScansDialog = qobject_cast(widget); const PostProcessingDialog * postProcessingDialog = qobject_cast(widget); const GraphViewer * graphViewer = qobject_cast(widget); const CalibrationDialog * calibrationDialog = qobject_cast(widget); @@ -2418,6 +2420,10 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget) { exportCloudsDialog->saveSettings(settings); } + else if(exportScansDialog) + { + exportScansDialog->saveSettings(settings); + } else if(postProcessingDialog) { postProcessingDialog->saveSettings(settings); @@ -2452,6 +2458,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget) CloudViewer * cloudViewer = qobject_cast(widget); ImageView * imageView = qobject_cast(widget); ExportCloudsDialog * exportCloudsDialog = qobject_cast(widget); + ExportScansDialog * exportScansDialog = qobject_cast(widget); PostProcessingDialog * postProcessingDialog = qobject_cast(widget); GraphViewer * graphViewer = qobject_cast(widget); CalibrationDialog * calibrationDialog = qobject_cast(widget); @@ -2468,6 +2475,10 @@ void PreferencesDialog::loadWidgetState(QWidget * widget) { exportCloudsDialog->loadSettings(settings); } + else if(exportScansDialog) + { + exportScansDialog->loadSettings(settings); + } else if(postProcessingDialog) { postProcessingDialog->loadSettings(settings); diff --git a/guilib/src/ui/exportScansDialog.ui b/guilib/src/ui/exportScansDialog.ui new file mode 100644 index 00000000..7280571a --- /dev/null +++ b/guilib/src/ui/exportScansDialog.ui @@ -0,0 +1,305 @@ + + + ExportScansDialog + + + + 0 + 0 + 545 + 439 + + + + Export Scans + + + + + + true + + + + + 0 + 0 + 519 + 363 + + + + + + + + + Assemble scans to a single output cloud. + + + true + + + + + + + + + + true + + + + + + + + + + + + + + Voxel size. Set 0 to disable. + + + true + + + + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + Set the number of k nearest neighbors to use for the normal estimation. Set 0 to disable normal estimation. + + + true + + + + + + + 0 + + + 20 + + + + + + + Binary file. + + + true + + + + + + + + + Regenerate Scans + + + true + + + + + + + + Downsampling step. + + + true + + + + + + + 1 + + + 999 + + + 1 + + + + + + + + + + + + Cloud Filtering (remove noisy points) + + + true + + + + + + m + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + Radius search. + + + true + + + + + + + 1 + + + 100 + + + 2 + + + + + + + Minimum neighbors in the search radius. + + + true + + + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults|QDialogButtonBox::Save + + + + + + + + + buttonBox + accepted() + ExportScansDialog + accept() + + + 248 + 254 + + + 157 + 274 + + + + + buttonBox + rejected() + ExportScansDialog + reject() + + + 316 + 260 + + + 286 + 274 + + + + +