i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Export Scans dialog

Browse Source
This commit is contained in:
matlabbe
2016-03-14 17:27:53 -04:00
parent 04df6ba41c
commit 5c92025742
14 changed files with 1251 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_filtering.h
+9
View File
@@ -170,6 +170,10 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
corelib/include/rtabmap/core/util3d_surface.h
+5
View File
@@ -159,6 +159,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
corelib/src/util3d_filtering.cpp
+53
View File
@@ -420,6 +420,14 @@ pcl::IndicesPtr radiusFiltering(
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
@@ -483,6 +491,51 @@ pcl::IndicesPtr radiusFiltering(
return output;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> 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<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> 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<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
corelib/src/util3d_surface.cpp
+52 -2
View File
@@ -728,7 +728,7 @@ void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
{
if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
{
@@ -736,6 +736,9 @@ void adjustNormalsToViewPoints(
rawTree->setInputCloud (rawCloud);
for(unsigned int i=0; i<cloud->size(); ++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<int> indices;
std::vector<float> dist;
@@ -747,7 +750,7 @@ void adjustNormalsToViewPoints(
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);
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0)
@@ -765,6 +768,53 @@ void adjustNormalsToViewPoints(
}
}
}
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
{
if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
rawTree->setInputCloud (rawCloud);
for(unsigned int i=0; i<cloud->size(); ++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<int> indices;
std::vector<float> 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);
}
}
}
}
}
pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
{
guilib/include/rtabmap/gui/MainWindow.h
+4 -5
View File
@@ -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<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans);
void saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::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<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > _createdClouds;
std::pair<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > _previousCloud; // used for subtraction
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
std::map<int, cv::Mat> _createdScans;
std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
guilib/src/CMakeLists.txt
+3
View File
@@ -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
guilib/src/CloudViewer.cpp
+2
View File
@@ -333,6 +333,8 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
{
settings.endGroup();
}
this->update();
}
bool CloudViewer::updateCloudPose(
guilib/src/ExportCloudsDialog.cpp
+4 -4
View File
@@ -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<int, pcl::PolygonMesh::Ptr>::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;i<meshes.begin()->second->tex_materials.size(); ++i)
{
guilib/src/ExportScansDialog.cpp
+597
View File
@@ -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 <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <QPushButton>
#include <QDir>
#include <QFileInfo>
#include <QMessageBox>
#include <QFileDialog>
#include <QInputDialog>
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<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans,
const QString & workingDirectory)
{
std::map<int, pcl::PointCloud<pcl::PointNormal>::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<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans,
const QString & workingDirectory)
{
std::map<int, pcl::PointCloud<pcl::PointNormal>::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<int, pcl::PointCloud<pcl::PointNormal>::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<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdClouds,
const QString & workingDirectory,
std::map<int, pcl::PointCloud<pcl::PointNormal>::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<int, pcl::PointCloud<pcl::PointNormal>::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<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> rawCameraIndices;
int i =0;
pcl::PointCloud<pcl::PointNormal>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointNormal>);
for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::iterator iter=clouds.begin();
iter!= clouds.end();
++iter)
{
pcl::PointCloud<pcl::PointNormal>::Ptr transformed(new pcl::PointCloud<pcl::PointNormal>);
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<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
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<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::iterator iter=clouds.begin();
iter!= clouds.end();
++iter)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = iter->second;
cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals));
_progressDialog->incrementStep();
QApplication::processEvents();
}
return true;
}
return false;
}
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> ExportScansDialog::getScans(
const std::map<int, Transform> & poses,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans) const
{
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> clouds;
int i=0;
pcl::PointCloud<pcl::PointNormal>::Ptr previousCloud;
pcl::IndicesPtr previousIndices;
Transform previousPose;
for(std::map<int, Transform>::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<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
if(scan.channels() == 6 && _ui->doubleSpinBox_voxelSize_assembled->value() == 0.0)
{
cloud = util3d::laserScanToPointCloudNormal(scan);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointNormal>::Ptr tmp(new pcl::PointCloud<pcl::PointNormal>);
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<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointNormal>::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<int, pcl::PointCloud<pcl::PointNormal>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
{
if(iter->second->size())
{
pcl::PointCloud<pcl::PointNormal>::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();
}
}
}
}
}
}
}
guilib/src/ExportScansDialog.h
+109
View File
@@ -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 <QDialog>
#include <QMap>
#include <QtCore/QSettings>
#include <rtabmap/core/Signature.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PolygonMesh.h>
#include <pcl/TextureMesh.h>
#include <pcl/pcl_base.h>
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<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans,
const QString & workingDirectory);
void viewScans(
const std::map<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans,
const QString & workingDirectory);
signals:
void configChanged();
public slots:
void restoreDefaults();
private:
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> getScans(
const std::map<int, Transform> & poses,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans) const;
bool getExportedScans(
const std::map<int, Transform> & poses,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
const std::map<int, cv::Mat> & createdScans,
const QString & workingDirectory,
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> & clouds);
void saveScans(const QString & workingDirectory,
const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> & clouds,
bool binaryMode = true);
void setSaveButton();
void setOkButton();
void enableRegeneration(bool enabled);
private:
Ui_ExportScansDialog * _ui;
ProgressDialog * _progressDialog;
};
}
#endif /* EXPORTSCANSDIALOG_H_ */
guilib/src/ImageView.cpp
+5 -1
View File
@@ -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();
}
}
guilib/src/MainWindow.cpp
+73 -280
View File
@@ -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<StatItem*>().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<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
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<int, pcl::PointCloud<pcl::PointXYZ>::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<int, pcl::PointCloud<pcl::PointXYZ>::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<int, pcl::PointCloud<pcl::PointXYZ>::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<int, pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>());
std::map<int, Transform> 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<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(_createdScans.find(iter->first) != _createdScans.end())
{
pcl::PointCloud<pcl::PointXYZ>::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<int, pcl::PointCloud<pcl::PointXYZ>::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<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter=scans.begin(); iter!=scans.end(); ++iter)
{
if(iter->second->size())
{
pcl::PointCloud<pcl::PointXYZ>::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
guilib/src/PreferencesDialog.cpp
+11
View File
@@ -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<const CloudViewer*>(widget);
const ImageView * imageView = qobject_cast<const ImageView*>(widget);
const ExportCloudsDialog * exportCloudsDialog = qobject_cast<const ExportCloudsDialog*>(widget);
const ExportScansDialog * exportScansDialog = qobject_cast<const ExportScansDialog*>(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);
@@ -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<CloudViewer*>(widget);
ImageView * imageView = qobject_cast<ImageView*>(widget);
ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
ExportScansDialog * exportScansDialog = qobject_cast<ExportScansDialog*>(widget);
PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
CalibrationDialog * calibrationDialog = qobject_cast<CalibrationDialog *>(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);
guilib/src/ui/exportScansDialog.ui
+305
View File
@@ -0,0 +1,305 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>ExportScansDialog</class>
<widget class="QDialog" name="ExportScansDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>545</width>
<height>439</height>
</rect>
</property>
<property name="windowTitle">
<string>Export Scans</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0,0">
<item>
<widget class="QScrollArea" name="scrollArea">
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>519</width>
<height>363</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
<item row="1" column="1">
<widget class="QLabel" name="label_binaryFile_2">
<property name="text">
<string>Assemble scans to a single output cloud.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_binary">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_assemble">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_109">
<property name="text">
<string>Voxel size. Set 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.005000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_111">
<property name="text">
<string>Set the number of k nearest neighbors to use for the normal estimation. Set 0 to disable normal estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_normalKSearch">
<property name="minimum">
<number>0</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_binaryFile">
<property name="text">
<string>Binary file.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_regenerate">
<property name="title">
<string>Regenerate Scans</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_14">
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_108">
<property name="text">
<string>Downsampling step.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_filtering">
<property name="title">
<string>Cloud Filtering (remove noisy points)</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_filteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_110">
<property name="text">
<string>Radius search.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_filteringMinNeighbors">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>2</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_112">
<property name="text">
<string>Minimum neighbors in the search radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_9">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</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|QDialogButtonBox::Save</set>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections>
<connection>
<sender>buttonBox</sender>
<signal>accepted()</signal>
<receiver>ExportScansDialog</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>ExportScansDialog</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>