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

Mesh reconstruction update: fixed normals computation (with and without MLS)

Browse Source
This commit is contained in:
Mathieu Labbé
2015-08-11 17:19:12 -04:00
parent 9c653655bd
commit abdd3de773
16 changed files with 580 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -72,6 +72,12 @@ public:
const std::string & id,
const Transform & pose); //including mesh
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -84,6 +90,12 @@ public:
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -101,30 +113,37 @@ public:
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb,
bool haveNormals,
const QColor & color = QColor());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addCloudMesh(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const Transform & pose = Transform::getIdentity());
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const Transform & pose = Transform::getIdentity());
bool addCloudMesh(
const std::string & id,
const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose = Transform::getIdentity());
const std::string & id,
const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose = Transform::getIdentity());
bool addOccupancyGridMap(
const cv::Mat & map8U,

18
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -226,23 +226,19 @@ private:
void exportPoses(int format);
QString captureScreen();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
const std::map<int, Transform> & poses,
float assembledVoxelSize,
bool regenerateClouds,
int regenerateDecimation,
float regenerateVoxelSize,
float regenerateMaxDepth) const;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > getClouds(
const std::map<int, Transform> & poses,
bool regenerateClouds,
int regenerateDecimation,
float regenerateVoxelSize,
float regenerateMaxDepth) const;
float regenerateMaxDepth,
int normalKSearch,
bool mls,
float mlsRadius) const;
bool getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans);
bool getExportedClouds(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool toSave);
void saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode = true);
bool getExportedClouds(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool toSave);
void saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
void saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds, bool binaryMode = true);

1
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -156,6 +156,7 @@ public:
int getMeshNormalKSearch() const;
double getMeshGP3Radius() const;
double getMeshGP3Mu() const;
bool getMeshSmoothing() const;
double getMeshSmoothingRadius() const;

69
guilib/src/CloudViewer.cpp
View File

@@ -318,6 +318,26 @@ bool CloudViewer::updateCloudPose(
return false;
}
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose,
const QColor & color)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
int index = _visualizer->getColorHandlerIndex(id);
this->removeCloud(id);
if(this->addCloud(id, cloud, pose, color))
{
_visualizer->updateColorHandlerIndex(id, index);
return true;
}
}
return false;
}
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -358,6 +378,19 @@ bool CloudViewer::updateCloud(
return false;
}
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose,
const QColor & color)
{
if(!updateCloud(id, cloud, pose, color))
{
return addCloud(id, cloud, pose, color);
}
return true;
}
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -389,6 +422,7 @@ bool CloudViewer::addCloud(
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb,
bool haveNormals,
const QColor & color)
{
if(!_addedClouds.contains(id))
@@ -422,7 +456,21 @@ bool CloudViewer::addCloud(
//rgb
colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
}
if(haveNormals)
{
//normals
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
}
if(rgb)
{
_visualizer->updateColorHandlerIndex(id, 5);
}
else if(color.isValid())
@@ -437,6 +485,23 @@ bool CloudViewer::addCloud(
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & pose,
const QColor & color)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, true, true, color);
}
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -449,7 +514,7 @@ bool CloudViewer::addCloud(
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, true, color);
return addCloud(id, binaryCloud, pose, true, false, color);
}
return false;
}
@@ -466,7 +531,7 @@ bool CloudViewer::addCloud(
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, false, color);
return addCloud(id, binaryCloud, pose, false, false, color);
}
return false;
}

13
guilib/src/ExportCloudsDialog.cpp
View File

@@ -52,6 +52,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->groupBox_gp3, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
}
ExportCloudsDialog::~ExportCloudsDialog()
@@ -77,6 +78,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("mesh", this->getMesh());
settings.setValue("mesh_k", this->getMeshNormalKSearch());
settings.setValue("mesh_radius", this->getMeshGp3Radius());
settings.setValue("mesh_mu", this->getMeshGp3Mu());
if(!group.isEmpty())
{
settings.endGroup();
@@ -101,6 +103,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
this->setMesh(settings.value("mesh", this->getMesh()).toBool());
this->setMeshNormalKSearch(settings.value("mesh_k", this->getMeshNormalKSearch()).toInt());
this->setMeshGp3Radius(settings.value("mesh_radius", this->getMeshGp3Radius()).toDouble());
this->setMeshGp3Mu(settings.value("mesh_mu", this->getMeshGp3Mu()).toDouble());
if(!group.isEmpty())
{
settings.endGroup();
@@ -124,6 +127,7 @@ void ExportCloudsDialog::restoreDefaults()
setMesh(false);
setMeshNormalKSearch(20);
setMeshGp3Radius(0.04);
setMeshGp3Radius(2.5);
}
void ExportCloudsDialog::setSaveButton()
@@ -209,6 +213,10 @@ double ExportCloudsDialog::getMeshGp3Radius() const
{
return _ui->doubleSpinBox_gp3Radius->value();
}
double ExportCloudsDialog::getMeshGp3Mu() const
{
return _ui->doubleSpinBox_gp3Mu->value();
}
//setters
void ExportCloudsDialog::setAssemble(bool on)
@@ -259,6 +267,9 @@ void ExportCloudsDialog::setMeshGp3Radius(double radius)
{
_ui->doubleSpinBox_gp3Radius->setValue(radius);
}
void ExportCloudsDialog::setMeshGp3Mu(double mu)
{
_ui->doubleSpinBox_gp3Mu->setValue(mu);
}
}

2
guilib/src/ExportCloudsDialog.h
View File

@@ -65,6 +65,7 @@ public:
bool getMesh() const;
int getMeshNormalKSearch() const;
double getMeshGp3Radius() const;
double getMeshGp3Mu() const;
//setters
void setAssemble(bool on);
@@ -79,6 +80,7 @@ public:
void setMesh(bool on);
void setMeshNormalKSearch(int k);
void setMeshGp3Radius(double radius);
void setMeshGp3Mu(double mu);
signals:
void configChanged();

333
guilib/src/MainWindow.cpp
View File

@@ -1751,7 +1751,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
_preferencesDialog->getCloudVoxelSize(0),
_preferencesDialog->getSubstractFilteringMinPts());
UDEBUG("Filtering %d from %d -> %d", (int)previousCloud->size(), (int)cloud->size(), (int)cloudFiltered->size());
_createdClouds.at(link.from()) = cloudFiltered;
}
}
}
@@ -1764,13 +1764,21 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing())
{
cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
cloudWithNormals = util3d::computeNormalsSmoothed(
cloudFiltered,
(float)_preferencesDialog->getMeshSmoothingRadius(),
false,
(float)_preferencesDialog->getCloudVoxelSize(0));
//if(_preferencesDialog->getCloudVoxelSize(0))
//{
// cloudWithNormals = util3d::voxelize(cloudWithNormals, _preferencesDialog->getCloudVoxelSize(0));
//}
}
else
{
cloudWithNormals = util3d::computeNormals(cloudFiltered, _preferencesDialog->getMeshNormalKSearch());
}
mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius());
mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(), _preferencesDialog->getMeshGP3Mu());
}
if(mesh->polygons.size())
@@ -1788,7 +1796,11 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
if(_preferencesDialog->getMeshSmoothing())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
cloudWithNormals = util3d::computeNormalsSmoothed(
cloudFiltered,
(float)_preferencesDialog->getMeshSmoothingRadius(),
false,
(float)_preferencesDialog->getCloudVoxelSize(0));
cloudFiltered.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloudWithNormals, *cloudFiltered);
}
@@ -4386,7 +4398,7 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
void MainWindow::exportClouds()
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(getExportedClouds(clouds, meshes, true))
@@ -4405,7 +4417,7 @@ void MainWindow::exportClouds()
void MainWindow::viewClouds()
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(getExportedClouds(clouds, meshes, false))
@@ -4449,7 +4461,7 @@ void MainWindow::viewClouds()
}
else if(clouds.size())
{
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
{
_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
_initProgressDialog->incrementStep();
@@ -4470,7 +4482,7 @@ void MainWindow::viewClouds()
}
bool MainWindow::getExportedClouds(
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
std::map<int, pcl::PolygonMesh::Ptr> & meshes,
bool toSave)
{
@@ -4493,68 +4505,140 @@ bool MainWindow::getExportedClouds(
_initProgressDialog->resetProgress();
_initProgressDialog->show();
int mul = _exportDialog->getMesh()&&!_exportDialog->getGenerate()?3:_exportDialog->getMLS()&&!_exportDialog->getGenerate()?2:1;
int mul = 1;
if(_exportDialog->getMesh())
{
mul+=1;
}
if(_exportDialog->getAssemble())
{
mul+=1;
}
_initProgressDialog->setMaximumSteps(int(poses.size())*mul+1);
if(_exportDialog->getMLS())
{
_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... "
"[search radius=%1m voxel=%2m]").arg(_exportDialog->getMLSRadius()).arg(_exportDialog->getGenerateVoxel()));
}
_initProgressDialog->appendText(tr("Computing surface normals... "
"[K neighbors=%1]").arg(_exportDialog->getMeshNormalKSearch()));
clouds = this->getClouds(
poses,
_exportDialog->getGenerate(),
_exportDialog->getGenerateDecimation(),
_exportDialog->getGenerateVoxel(),
_exportDialog->getGenerateMaxDepth(),
_exportDialog->getMeshNormalKSearch(),
_exportDialog->getAssemble()?false:_exportDialog->getMLS(),
(float)_exportDialog->getMLSRadius());
if(_exportDialog->getAssemble())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->getAssembledCloud(
poses,
_exportDialog->getAssembleVoxel(),
_exportDialog->getGenerate(),
_exportDialog->getGenerateDecimation(),
_exportDialog->getGenerateVoxel(),
_exportDialog->getGenerateMaxDepth());
_initProgressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size()));
QApplication::processEvents();
clouds.insert(std::make_pair(0, cloud));
}
else
{
clouds = this->getClouds(
poses,
_exportDialog->getGenerate(),
_exportDialog->getGenerateDecimation(),
_exportDialog->getGenerateVoxel(),
_exportDialog->getGenerateMaxDepth());
}
if(_exportDialog->getMLS() || _exportDialog->getMesh())
{
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter=clouds.begin();
int i =0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter=clouds.begin();
iter!= clouds.end();
++iter)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_exportDialog->getMLS())
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first));
*assembledCloud += *transformed;
_initProgressDialog->appendText(tr("Assembled cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(clouds.size()));
_initProgressDialog->incrementStep();
if(i % 100 == 0)
{
_initProgressDialog->appendText(tr("Smoothing the surface of cloud %1 using Moving Least Squares (MLS) algorithm... "
"[search radius=%2m]").arg(iter->first).arg(_exportDialog->getMLSRadius()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormalsSmoothed(iter->second, (float)_exportDialog->getMLSRadius());
iter->second->clear();
pcl::copyPointCloud(*cloudWithNormals, *iter->second);
}
else if(_exportDialog->getMesh())
{
_initProgressDialog->appendText(tr("Computing surface normals of cloud %1 (without smoothing)... "
"[K neighbors=%2]").arg(iter->first).arg(_exportDialog->getMeshNormalKSearch()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
}
cloudWithNormals = util3d::computeNormals(iter->second, _exportDialog->getMeshNormalKSearch());
if(_exportDialog->getMLS())
{
_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...")
.arg(assembledCloud->size())
.arg(_exportDialog->getGenerateVoxel()));
QApplication::processEvents();
if(_exportDialog->getGenerateVoxel())
{
assembledCloud = util3d::voxelize(
assembledCloud,
_exportDialog->getGenerateVoxel());
}
if(_exportDialog->getMesh())
_initProgressDialog->appendText(tr("Smoothing (MLS) of the assembled cloud (%1 points)...").arg(assembledCloud->size()));
QApplication::processEvents();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloudXYZRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*assembledCloud, *assembledCloudXYZRGB);
assembledCloud = util3d::computeNormalsSmoothed(
assembledCloudXYZRGB,
(float)_exportDialog->getMLSRadius(),
true,
_exportDialog->getGenerateVoxel());
if(_exportDialog->getAssembleVoxel())
{
_initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius()));
_initProgressDialog->incrementStep();
_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...")
.arg(assembledCloud->size())
.arg(_exportDialog->getAssembleVoxel()));
QApplication::processEvents();
pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _exportDialog->getMeshGp3Radius());
meshes.insert(std::make_pair(iter->first, mesh));
assembledCloud = util3d::voxelize(
assembledCloud,
_exportDialog->getAssembleVoxel());
}
_initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(assembledCloud->size()).arg(poses.size()));
QApplication::processEvents();
pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoints(new pcl::PointCloud<pcl::PointXYZ>);
viewpoints->resize(poses.size());
int oi=0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
(*viewpoints)[oi].x = iter->second.x();
(*viewpoints)[oi].y = iter->second.y();
(*viewpoints)[oi++].z = iter->second.z();
}
util3d::adjustNormalsToViewPoints(viewpoints, *assembledCloud);
}
else if(_exportDialog->getAssembleVoxel())
{
_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points)...").arg(assembledCloud->size()));
QApplication::processEvents();
assembledCloud = util3d::voxelize(
assembledCloud,
_exportDialog->getAssembleVoxel());
_initProgressDialog->appendText(tr("Voxelized assembled cloud (%1 points)").arg(assembledCloud->size()));
}
clouds.clear();
clouds.insert(std::make_pair(0, assembledCloud));
}
if(_exportDialog->getMesh())
{
_initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius()));
QApplication::processEvents();
int i=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter=clouds.begin();
iter!= clouds.end();
++iter)
{
pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu());
meshes.insert(std::make_pair(iter->first, mesh));
_initProgressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size()));
_initProgressDialog->incrementStep();
if(i % 100 == 0)
{
QApplication::processEvents();
}
}
}
@@ -4746,7 +4830,7 @@ void MainWindow::dataRecorderDestroyed()
//END ACTIONS
void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode)
void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode)
{
if(clouds.size() == 1)
{
@@ -4811,11 +4895,11 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
if(ok)
{
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
{
if(iter->second->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud;
pcl::PointCloud<pcl::PointXYZRGBNormal>::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);
@@ -5082,119 +5166,17 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
const std::map<int, Transform> & poses,
float assembledVoxelSize,
bool regenerateClouds,
int regenerateDecimation,
float regenerateVoxelSize,
float regenerateMaxDepth) const
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
int i=0;
int count = 0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(!iter->second.isNull())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(regenerateClouds)
{
if(_cachedSignatures.contains(iter->first))
{
const Signature & s = _cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
cv::Mat image, depth;
d.uncompressData(&image, &depth, 0);
if(!image.empty() && !depth.empty())
{
UASSERT(iter->first == d.id());
cloud = util3d::cloudRGBFromSensorData(
d,
regenerateDecimation,
regenerateMaxDepth,
regenerateVoxelSize);
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, iter->second);
}
}
else if(s.getWords3().size())
{
cloud->resize(s.getWords3().size());
int oi=0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
{
(*cloud)[oi].x = jter->second.x;
(*cloud)[oi].y = jter->second.y;
(*cloud)[oi].z = jter->second.z;
(*cloud)[oi].r = 255;
(*cloud)[oi].g = 255;
(*cloud)[oi++].b = 255;
}
}
}
else
{
UWARN("Cloud %d not found in cache!", iter->first);
}
}
else if(uContains(_createdClouds, iter->first))
{
cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
}
if(cloud->size())
{
*assembledCloud += *cloud;
inserted = true;
++count;
}
}
else
{
UERROR("transform is null!?");
}
if(inserted)
{
_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
if(count % 100 == 0)
{
if(assembledCloud->size() && assembledVoxelSize)
{
assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
}
}
}
else
{
_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
_initProgressDialog->incrementStep();
QApplication::processEvents();
}
if(assembledCloud->size() && assembledVoxelSize)
{
assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
}
return assembledCloud;
}
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > MainWindow::getClouds(
const std::map<int, Transform> & poses,
bool regenerateClouds,
int regenerateDecimation,
float regenerateVoxelSize,
float regenerateMaxDepth) const
float regenerateMaxDepth,
int normalKSearch,
bool mls,
float mlsRadius) const
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
int i=0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -5246,7 +5228,28 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
if(cloud->size())
{
clouds.insert(std::make_pair(iter->first, cloud));
if(mls)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals = util3d::computeNormalsSmoothed(
cloud,
mlsRadius,
true,
regenerateVoxelSize);
if(regenerateVoxelSize)
{
cloudWithNormals = util3d::voxelize(
cloudWithNormals,
regenerateVoxelSize);
}
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals = util3d::computeNormals(cloud, _exportDialog->getMeshNormalKSearch());
clouds.insert(std::make_pair(iter->first, cloudWithNormals));
inserted = true;
}
}

8
guilib/src/PreferencesDialog.cpp
View File

@@ -297,6 +297,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_mls, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1029,6 +1030,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_meshing->setChecked(false);
_ui->doubleSpinBox_gp3Radius->setValue(0.04);
_ui->doubleSpinBox_gp3Mu->setValue(2.5);
_ui->spinBox_normalKSearch->setValue(20);
_ui->checkBox_mls->setChecked(false);
_ui->doubleSpinBox_mlsRadius->setValue(0.04);
@@ -1319,6 +1321,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->checkBox_meshing->setChecked(settings.value("meshing", _ui->checkBox_meshing->isChecked()).toBool());
_ui->doubleSpinBox_gp3Radius->setValue(settings.value("meshGP3Radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble());
_ui->doubleSpinBox_gp3Mu->setValue(settings.value("meshGP3Mu", _ui->doubleSpinBox_gp3Mu->value()).toDouble());
_ui->spinBox_normalKSearch->setValue(settings.value("meshNormalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
_ui->checkBox_mls->setChecked(settings.value("meshSmoothing", _ui->checkBox_mls->isChecked()).toBool());
_ui->doubleSpinBox_mlsRadius->setValue(settings.value("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value()).toDouble());
@@ -1630,6 +1633,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("meshing", _ui->checkBox_meshing->isChecked());
settings.setValue("meshGP3Radius", _ui->doubleSpinBox_gp3Radius->value());
settings.setValue("meshGP3Mu", _ui->doubleSpinBox_gp3Mu->value());
settings.setValue("meshNormalKSearch", _ui->spinBox_normalKSearch->value());
settings.setValue("meshSmoothing", _ui->checkBox_mls->isChecked());
settings.setValue("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value());
@@ -3237,6 +3241,10 @@ double PreferencesDialog::getMeshGP3Radius() const
{
return _ui->doubleSpinBox_gp3Radius->value();
}
double PreferencesDialog::getMeshGP3Mu() const
{
return _ui->doubleSpinBox_gp3Mu->value();
}
bool PreferencesDialog::getMeshSmoothing() const
{
return _ui->checkBox_mls->isChecked();

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

@@ -242,7 +242,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<item row="0" column="1">
<widget class="QLabel" name="label_71">
<property name="text">
<string>Set the number of k nearest neighbors to use for the normal estimation to create the mesh. Not used when mesh smoothing (MLS) above is used.</string>
<string>Set the number of k nearest neighbors to use for the normal estimation to create the mesh.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -279,6 +279,38 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_169">
<property name="text">
<string>Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point (this will make the algorithm adapt to different point densities in the cloud).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gp3Mu">
<property name="suffix">
<string>0</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>2.500000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>

45
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>755</width>
<height>1591</height>
<y>-394</y>
<width>759</width>
<height>938</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>4</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1087,7 +1087,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item row="16" column="2">
<widget class="QLabel" name="label_168">
<property name="text">
<string>Sphere radius that is to be used for determining the k-nearest neighbors used for triangulating (GP3). Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<string>(GP3) Sphere radius that is to be used for determining the k-nearest neighbors used for triangulating. Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -1250,6 +1250,41 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="17" column="2">
<widget class="QLabel" name="label_246">
<property name="text">
<string>(GP3) Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point (this will make the algorithm adapt to different point densities in the cloud).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="17" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gp3Mu">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>2.500000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>