ExportScansDialog.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "ExportScansDialog.h"
00029 #include "ui_exportScansDialog.h"
00030 
00031 #include "rtabmap/gui/ProgressDialog.h"
00032 #include "rtabmap/gui/CloudViewer.h"
00033 #include "rtabmap/utilite/ULogger.h"
00034 #include "rtabmap/utilite/UConversion.h"
00035 #include "rtabmap/utilite/UThread.h"
00036 #include "rtabmap/utilite/UStl.h"
00037 
00038 #include "rtabmap/core/util3d_filtering.h"
00039 #include "rtabmap/core/util3d_surface.h"
00040 #include "rtabmap/core/util3d_transforms.h"
00041 #include "rtabmap/core/util3d.h"
00042 #include "rtabmap/core/Graph.h"
00043 
00044 #include <pcl/conversions.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <pcl/io/ply_io.h>
00047 
00048 #include <QPushButton>
00049 #include <QDir>
00050 #include <QFileInfo>
00051 #include <QMessageBox>
00052 #include <QFileDialog>
00053 #include <QInputDialog>
00054 
00055 namespace rtabmap {
00056 
00057 ExportScansDialog::ExportScansDialog(QWidget *parent) :
00058         QDialog(parent)
00059 {
00060         _ui = new Ui_ExportScansDialog();
00061         _ui->setupUi(this);
00062 
00063         connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
00064 
00065         restoreDefaults();
00066 
00067         connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
00068         connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00069 
00070         connect(_ui->groupBox_regenerate, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
00071         connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00072 
00073         connect(_ui->groupBox_filtering, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
00074         connect(_ui->doubleSpinBox_filteringRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00075         connect(_ui->spinBox_filteringMinNeighbors, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00076 
00077         connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
00078         connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00079 
00080         _progressDialog = new ProgressDialog(this);
00081         _progressDialog->setVisible(false);
00082         _progressDialog->setAutoClose(true, 2);
00083         _progressDialog->setMinimumWidth(600);
00084 }
00085 
00086 ExportScansDialog::~ExportScansDialog()
00087 {
00088         delete _ui;
00089 }
00090 
00091 void ExportScansDialog::saveSettings(QSettings & settings, const QString & group) const
00092 {
00093         if(!group.isEmpty())
00094         {
00095                 settings.beginGroup(group);
00096         }
00097         settings.setValue("binary", _ui->checkBox_binary->isChecked());
00098         settings.setValue("normals_k", _ui->spinBox_normalKSearch->value());
00099 
00100         settings.setValue("regenerate", _ui->groupBox_regenerate->isChecked());
00101         settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
00102 
00103         settings.setValue("filtering", _ui->groupBox_filtering->isChecked());
00104         settings.setValue("filtering_radius", _ui->doubleSpinBox_filteringRadius->value());
00105         settings.setValue("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value());
00106 
00107         settings.setValue("assemble", _ui->checkBox_assemble->isChecked());
00108         settings.setValue("assemble_voxel",_ui->doubleSpinBox_voxelSize_assembled->value());
00109 
00110         if(!group.isEmpty())
00111         {
00112                 settings.endGroup();
00113         }
00114 }
00115 
00116 void ExportScansDialog::loadSettings(QSettings & settings, const QString & group)
00117 {
00118         if(!group.isEmpty())
00119         {
00120                 settings.beginGroup(group);
00121         }
00122 
00123         _ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool());
00124         _ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt());
00125 
00126         _ui->groupBox_regenerate->setChecked(settings.value("regenerate", _ui->groupBox_regenerate->isChecked()).toBool());
00127         _ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
00128 
00129         _ui->groupBox_filtering->setChecked(settings.value("filtering", _ui->groupBox_filtering->isChecked()).toBool());
00130         _ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble());
00131         _ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value()).toInt());
00132 
00133         _ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
00134         _ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble());
00135 
00136         if(!group.isEmpty())
00137         {
00138                 settings.endGroup();
00139         }
00140 }
00141 
00142 void ExportScansDialog::restoreDefaults()
00143 {
00144         _ui->checkBox_binary->setChecked(true);
00145         _ui->spinBox_normalKSearch->setValue(20);
00146 
00147         _ui->groupBox_regenerate->setChecked(false);
00148         _ui->spinBox_decimation->setValue(1);
00149 
00150         _ui->groupBox_filtering->setChecked(false);
00151         _ui->doubleSpinBox_filteringRadius->setValue(0.02);
00152         _ui->spinBox_filteringMinNeighbors->setValue(2);
00153 
00154         _ui->checkBox_assemble->setChecked(true);
00155         _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01);
00156 
00157         this->update();
00158 }
00159 
00160 void ExportScansDialog::setSaveButton()
00161 {
00162         _ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(false);
00163         _ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(true);
00164         _ui->checkBox_binary->setVisible(true);
00165         _ui->label_binaryFile->setVisible(true);
00166 }
00167 
00168 void ExportScansDialog::setOkButton()
00169 {
00170         _ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(true);
00171         _ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(false);
00172         _ui->checkBox_binary->setVisible(false);
00173         _ui->label_binaryFile->setVisible(false);
00174 }
00175 
00176 void ExportScansDialog::enableRegeneration(bool enabled)
00177 {
00178         if(!enabled)
00179         {
00180                 _ui->groupBox_regenerate->setChecked(false);
00181         }
00182         _ui->groupBox_regenerate->setEnabled(enabled);
00183 }
00184 
00185 void ExportScansDialog::exportScans(
00186                 const std::map<int, Transform> & poses,
00187                 const std::map<int, int> & mapIds,
00188                 const QMap<int, Signature> & cachedSignatures,
00189                 const std::map<int, cv::Mat> & createdScans,
00190                 const QString & workingDirectory)
00191 {
00192         std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> clouds;
00193 
00194         setSaveButton();
00195 
00196         if(getExportedScans(
00197                         poses,
00198                         mapIds,
00199                         cachedSignatures,
00200                         createdScans,
00201                         workingDirectory,
00202                         clouds))
00203         {
00204                 saveScans(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked());
00205                 _progressDialog->setValue(_progressDialog->maximumSteps());
00206         }
00207 }
00208 
00209 void ExportScansDialog::viewScans(
00210                 const std::map<int, Transform> & poses,
00211                 const std::map<int, int> & mapIds,
00212                 const QMap<int, Signature> & cachedSignatures,
00213                 const std::map<int, cv::Mat> & createdScans,
00214                 const QString & workingDirectory)
00215 {
00216         std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> clouds;
00217 
00218         setOkButton();
00219         if(getExportedScans(
00220                         poses,
00221                         mapIds,
00222                         cachedSignatures,
00223                         createdScans,
00224                         workingDirectory,
00225                         clouds))
00226         {
00227                 QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
00228                 window->setAttribute(Qt::WA_DeleteOnClose, true);
00229                 window->setWindowTitle(tr("Scans (%1 nodes)").arg(clouds.size()));
00230                 window->setMinimumWidth(800);
00231                 window->setMinimumHeight(600);
00232 
00233                 CloudViewer * viewer = new CloudViewer(window);
00234                 viewer->setCameraLockZ(false);
00235 
00236                 QVBoxLayout *layout = new QVBoxLayout();
00237                 layout->addWidget(viewer);
00238                 window->setLayout(layout);
00239                 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
00240 
00241                 window->show();
00242 
00243                 uSleep(500);
00244                 if(clouds.size())
00245                 {
00246                         for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
00247                         {
00248                                 _progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
00249                                 _progressDialog->incrementStep();
00250 
00251                                 QColor color = Qt::gray;
00252                                 int mapId = uValue(mapIds, iter->first, -1);
00253                                 if(mapId >= 0)
00254                                 {
00255                                         color = (Qt::GlobalColor)(mapId % 12 + 7 );
00256                                 }
00257                                 viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?poses.at(iter->first):Transform::getIdentity());
00258                                 _progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
00259                         }
00260                 }
00261 
00262                 _progressDialog->setValue(_progressDialog->maximumSteps());
00263                 viewer->update();
00264         }
00265 }
00266 
00267 bool ExportScansDialog::getExportedScans(
00268                 const std::map<int, Transform> & poses,
00269                 const std::map<int, int> & mapIds,
00270                 const QMap<int, Signature> & cachedSignatures,
00271                 const std::map<int, cv::Mat> & createdClouds,
00272                 const QString & workingDirectory,
00273                 std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> & cloudsWithNormals)
00274 {
00275         enableRegeneration(cachedSignatures.size());
00276         if(this->exec() == QDialog::Accepted)
00277         {
00278                 _progressDialog->resetProgress();
00279                 _progressDialog->show();
00280                 int mul = 1;
00281                 if(_ui->checkBox_assemble->isChecked())
00282                 {
00283                         mul+=1;
00284                 }
00285                 mul+=1; // normals
00286                 _progressDialog->setMaximumSteps(int(poses.size())*mul+1);
00287 
00288                 std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> clouds = this->getScans(
00289                                 poses,
00290                                 cachedSignatures,
00291                                 createdClouds);
00292 
00293                 if(_ui->checkBox_assemble->isChecked())
00294                 {
00295                         _progressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size()));
00296                         QApplication::processEvents();
00297 
00298                         pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
00299                         std::vector<int> rawCameraIndices;
00300                         int i =0;
00301                         pcl::PointCloud<pcl::PointNormal>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointNormal>);
00302                         for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::iterator iter=clouds.begin();
00303                                 iter!= clouds.end();
00304                                 ++iter)
00305                         {
00306                                 pcl::PointCloud<pcl::PointNormal>::Ptr transformed(new pcl::PointCloud<pcl::PointNormal>);
00307                                 transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first));
00308 
00309                                 *assembledCloud += *transformed;
00310 
00311                                 rawCameraIndices.resize(assembledCloud->size(), iter->first);
00312 
00313                                 _progressDialog->appendText(tr("Assembled cloud %1, total=%2 (%3/%4).").arg(iter->first).arg(assembledCloud->size()).arg(++i).arg(clouds.size()));
00314                                 _progressDialog->incrementStep();
00315                                 QApplication::processEvents();
00316                         }
00317 
00318                         pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud);
00319 
00320                         if(_ui->doubleSpinBox_voxelSize_assembled->value())
00321                         {
00322                                 _progressDialog->appendText(tr("Voxelize cloud (%1 points, voxel size = %2 m)...")
00323                                                 .arg(assembledCloud->size())
00324                                                 .arg(_ui->doubleSpinBox_voxelSize_assembled->value()));
00325                                 QApplication::processEvents();
00326 
00327                                 assembledCloud = util3d::voxelize(
00328                                                 assembledCloud,
00329                                                 _ui->doubleSpinBox_voxelSize_assembled->value());
00330 
00331                                 if(_ui->spinBox_normalKSearch->value() > 0)
00332                                 {
00333                                         _progressDialog->appendText(tr("Compute normals (%1 points)...")
00334                                                         .arg(assembledCloud->size()));
00335                                         QApplication::processEvents();
00336 
00337                                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
00338                                         pcl::copyPointCloud(*assembledCloud, *cloudXYZ);
00339 
00340                                         pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudXYZ, _ui->spinBox_normalKSearch->value());
00341                                         pcl::concatenateFields(*cloudXYZ, *normals, *assembledCloud);
00342 
00343                                         _progressDialog->appendText(tr("Update %1 normals with %2 camera views...")
00344                                                         .arg(assembledCloud->size()).arg(poses.size()));
00345                                                         util3d::adjustNormalsToViewPoints(
00346                                                                         poses,
00347                                                                         rawAssembledCloud,
00348                                                                         rawCameraIndices,
00349                                                                         assembledCloud);
00350                                 }
00351                         }
00352 
00353                         clouds.clear();
00354                         clouds.insert(std::make_pair(0, assembledCloud));
00355                 }
00356 
00357                 //fill cloudWithNormals
00358                 for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::iterator iter=clouds.begin();
00359                         iter!= clouds.end();
00360                         ++iter)
00361                 {
00362                         pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = iter->second;
00363 
00364                         cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals));
00365 
00366                         _progressDialog->incrementStep();
00367                         QApplication::processEvents();
00368                 }
00369 
00370                 return true;
00371         }
00372         return false;
00373 }
00374 
00375 std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> ExportScansDialog::getScans(
00376                 const std::map<int, Transform> & poses,
00377                 const QMap<int, Signature> & cachedSignatures,
00378                 const std::map<int, cv::Mat> & createdScans) const
00379 {
00380         std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> clouds;
00381         int i=0;
00382         pcl::PointCloud<pcl::PointNormal>::Ptr previousCloud;
00383         pcl::IndicesPtr previousIndices;
00384         Transform previousPose;
00385         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00386         {
00387                 int points = 0;
00388                 if(!iter->second.isNull())
00389                 {
00390                         cv::Mat scan;
00391                         if(_ui->groupBox_regenerate->isChecked())
00392                         {
00393                                 if(cachedSignatures.contains(iter->first))
00394                                 {
00395                                         const Signature & s = cachedSignatures.find(iter->first).value();
00396                                         SensorData d = s.sensorData();
00397                                         d.uncompressData(0, 0, &scan);
00398                                         if(!scan.empty())
00399                                         {
00400                                                 if(_ui->spinBox_decimation->value() > 1)
00401                                                 {
00402                                                         scan = util3d::downsample(scan, _ui->spinBox_decimation->value());
00403                                                 }
00404                                         }
00405                                 }
00406                                 else
00407                                 {
00408                                         UERROR("Scan %d not found in cache!", iter->first);
00409                                 }
00410                         }
00411                         else
00412                         {
00413                                 scan = uValue(createdScans, iter->first, cv::Mat());
00414                         }
00415 
00416                         if(!scan.empty())
00417                         {
00418                                 pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
00419                                 if(scan.channels() == 6 && _ui->doubleSpinBox_voxelSize_assembled->value() == 0.0)
00420                                 {
00421                                         cloud = util3d::laserScanToPointCloudNormal(scan);
00422                                 }
00423                                 else
00424                                 {
00425                                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ = util3d::laserScanToPointCloud(scan);
00426 
00427                                         if(_ui->doubleSpinBox_voxelSize_assembled->value() > 0.0)
00428                                         {
00429                                                 cloudXYZ = util3d::voxelize(
00430                                                                 cloudXYZ,
00431                                                                 _ui->doubleSpinBox_voxelSize_assembled->value());
00432                                         }
00433 
00434                                         if(!_ui->checkBox_assemble->isChecked() && _ui->spinBox_normalKSearch->value() > 0)
00435                                         {
00436                                                 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudXYZ, _ui->spinBox_normalKSearch->value());
00437                                                 pcl::concatenateFields(*cloudXYZ, *normals, *cloud);
00438                                         }
00439                                         else
00440                                         {
00441                                                 pcl::copyPointCloud(*cloudXYZ, *cloud);
00442                                         }
00443                                 }
00444 
00445                                 if(cloud->size())
00446                                 {
00447                                         if(_ui->groupBox_filtering->isChecked() &&
00448                                                 _ui->doubleSpinBox_filteringRadius->value() > 0.0f &&
00449                                                 _ui->spinBox_filteringMinNeighbors->value() > 0)
00450                                         {
00451                                                 pcl::IndicesPtr indices = util3d::radiusFiltering(cloud, _ui->doubleSpinBox_filteringRadius->value(), _ui->spinBox_filteringMinNeighbors->value());
00452                                                 pcl::PointCloud<pcl::PointNormal>::Ptr tmp(new pcl::PointCloud<pcl::PointNormal>);
00453                                                 pcl::copyPointCloud(*cloud, *indices, *tmp);
00454                                                 cloud = tmp;
00455                                         }
00456 
00457                                         clouds.insert(std::make_pair(iter->first, cloud));
00458                                         points = cloud->size();
00459                                 }
00460                         }
00461                 }
00462                 else
00463                 {
00464                         UERROR("transform is null!?");
00465                 }
00466 
00467                 if(points>0)
00468                 {
00469                         _progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
00470                                         .arg(iter->first).arg(points).arg(++i).arg(poses.size()));
00471                 }
00472                 else
00473                 {
00474                         _progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
00475                 }
00476                 _progressDialog->incrementStep();
00477                 QApplication::processEvents();
00478         }
00479 
00480         return clouds;
00481 }
00482 
00483 
00484 void ExportScansDialog::saveScans(
00485                 const QString & workingDirectory,
00486                 const std::map<int, Transform> & poses,
00487                 const std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> & clouds,
00488                 bool binaryMode)
00489 {
00490         if(clouds.size() == 1)
00491         {
00492                 QString path = QFileDialog::getSaveFileName(this, tr("Save scan to ..."), workingDirectory+QDir::separator()+"scan.ply", tr("Point cloud data (*.ply *.pcd)"));
00493                 if(!path.isEmpty())
00494                 {
00495                         if(clouds.begin()->second->size())
00496                         {
00497                                 _progressDialog->appendText(tr("Saving the scan (%1 points)...").arg(clouds.begin()->second->size()));
00498 
00499                                 bool success =false;
00500                                 if(QFileInfo(path).suffix() == "pcd")
00501                                 {
00502                                         success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
00503                                 }
00504                                 else if(QFileInfo(path).suffix() == "ply")
00505                                 {
00506                                         success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
00507                                 }
00508                                 else if(QFileInfo(path).suffix() == "")
00509                                 {
00510                                         //use ply by default
00511                                         path += ".ply";
00512                                         success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
00513                                 }
00514                                 else
00515                                 {
00516                                         UERROR("Extension not recognized! (%s) Should be one of (*.ply *.pcd).", QFileInfo(path).suffix().toStdString().c_str());
00517                                 }
00518                                 if(success)
00519                                 {
00520                                         _progressDialog->incrementStep();
00521                                         _progressDialog->appendText(tr("Saving the scan (%1 points)... done.").arg(clouds.begin()->second->size()));
00522 
00523                                         QMessageBox::information(this, tr("Save successful!"), tr("Scan saved to \"%1\"").arg(path));
00524                                 }
00525                                 else
00526                                 {
00527                                         QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path));
00528                                 }
00529                         }
00530                         else
00531                         {
00532                                 QMessageBox::warning(this, tr("Save failed!"), tr("Scan is empty..."));
00533                         }
00534                 }
00535         }
00536         else if(clouds.size())
00537         {
00538                 QString path = QFileDialog::getExistingDirectory(this, tr("Save scans to (*.ply *.pcd)..."), workingDirectory, 0);
00539                 if(!path.isEmpty())
00540                 {
00541                         bool ok = false;
00542                         QStringList items;
00543                         items.push_back("ply");
00544                         items.push_back("pcd");
00545                         QString suffix = QInputDialog::getItem(this, tr("File format"), tr("Which format?"), items, 0, false, &ok);
00546 
00547                         if(ok)
00548                         {
00549                                 QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "scan", &ok);
00550 
00551                                 if(ok)
00552                                 {
00553                                         for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
00554                                         {
00555                                                 if(iter->second->size())
00556                                                 {
00557                                                         pcl::PointCloud<pcl::PointNormal>::Ptr transformedCloud;
00558                                                         transformedCloud = util3d::transformPointCloud(iter->second, poses.at(iter->first));
00559 
00560                                                         QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
00561                                                         bool success =false;
00562                                                         if(suffix == "pcd")
00563                                                         {
00564                                                                 success = pcl::io::savePCDFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
00565                                                         }
00566                                                         else if(suffix == "ply")
00567                                                         {
00568                                                                 success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
00569                                                         }
00570                                                         else
00571                                                         {
00572                                                                 UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str());
00573                                                         }
00574                                                         if(success)
00575                                                         {
00576                                                                 _progressDialog->appendText(tr("Saved scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
00577                                                         }
00578                                                         else
00579                                                         {
00580                                                                 _progressDialog->appendText(tr("Failed saving scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
00581                                                         }
00582                                                 }
00583                                                 else
00584                                                 {
00585                                                         _progressDialog->appendText(tr("Scan %1 is empty!").arg(iter->first));
00586                                                 }
00587                                                 _progressDialog->incrementStep();
00588                                                 QApplication::processEvents();
00589                                         }
00590                                 }
00591                         }
00592                 }
00593         }
00594 }
00595 
00596 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:16