DbViewer: added menu actions to view/update/export optimized mesh saved in database. CloudViewer: cubes can be added for convenience, fixed double-click not always working. util3d: added conversion function from LaserScan to PointCloud2, added conversion functions between polygons format saved in database and PCL polygons format with vertices.

This commit is contained in:
matlabbe
2018-04-25 12:34:47 -04:00
parent f13e384a1b
commit b044bae304
11 changed files with 617 additions and 24 deletions

View File

@@ -82,6 +82,8 @@ class MyInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
public:
MyInteractorStyle(CloudViewer * viewer) :
pcl::visualization::PCLVisualizerInteractorStyle(),
NumberOfClicks(0),
ResetPixelDistance(0),
pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
viewer_(viewer)
{
@@ -239,7 +241,7 @@ protected:
this->NumberOfClicks = 1;
}
if(this->NumberOfClicks == 2)
if(this->NumberOfClicks >= 2)
{
this->NumberOfClicks = 0;
this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
@@ -352,8 +354,6 @@ private:
unsigned int NumberOfClicks;
int PreviousPosition[2];
int ResetPixelDistance;
vtkSmartPointer<vtkTextActor> textActor_;
vtkSmartPointer<vtkLine> lineActor_;
float PreviousMeasure[3];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
CloudViewer * viewer_;
@@ -1727,6 +1727,68 @@ void CloudViewer::removeAllSpheres()
UASSERT(_spheres.empty());
}
void CloudViewer::addOrUpdateCube(
const std::string & id,
const Transform & pose,
float width,
float height,
float depth,
const QColor & color,
bool wireframe,
bool foreground)
{
if(id.empty())
{
UERROR("id should not be empty!");
return;
}
removeCube(id);
if(!pose.isNull())
{
_cubes.insert(id);
QColor c = Qt::gray;
if(color.isValid())
{
c = color;
}
_visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?2:1);
if(wireframe)
{
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
}
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
}
}
void CloudViewer::removeCube(const std::string & id)
{
if(id.empty())
{
UERROR("id should not be empty!");
return;
}
if(_cubes.find(id) != _cubes.end())
{
_visualizer->removeShape(id);
_cubes.erase(id);
}
}
void CloudViewer::removeAllCubes()
{
std::set<std::string> cubes = _cubes;
for(std::set<std::string>::iterator iter = cubes.begin(); iter!=cubes.end(); ++iter)
{
this->removeCube(*iter);
}
UASSERT(_cubes.empty());
}
static const float frustum_vertices[] = {
0.0f, 0.0f, 0.0f,
1.0f, 1.0f, 1.0f,
@@ -2706,7 +2768,7 @@ void CloudViewer::setNormalsScale(float scale)
}
}
void CloudViewer::buildLocator(bool enable)
void CloudViewer::buildPickingLocator(bool enable)
{
_buildLocator = enable;
}

View File

@@ -82,6 +82,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/RecoveryState.h"
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
@@ -248,6 +249,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -276,6 +280,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->menuExport_GPS->setEnabled(false);
ui_->actionPoses_KML->setEnabled(false);
ui_->actionExport_saved_2D_map->setEnabled(false);
ui_->actionView_optimized_mesh->setEnabled(false);
ui_->actionExport_optimized_mesh->setEnabled(false);
ui_->actionUpdate_optimized_mesh->setEnabled(false);
ui_->horizontalSlider_A->setTracking(false);
ui_->horizontalSlider_B->setTracking(false);
@@ -938,6 +945,9 @@ bool DatabaseViewer::closeDatabase()
ui_->actionPoses_KML->setEnabled(false);
ui_->actionExport_saved_2D_map->setEnabled(false);
ui_->actionImport_2D_map->setEnabled(false);
ui_->actionView_optimized_mesh->setEnabled(false);
ui_->actionExport_optimized_mesh->setEnabled(false);
ui_->actionUpdate_optimized_mesh->setEnabled(false);
ui_->checkBox_showOptimized->setEnabled(false);
ui_->toolBox_statistics->clear();
databaseFileName_.clear();
@@ -1478,6 +1488,9 @@ void DatabaseViewer::updateIds()
ui_->actionPoses_KML->setEnabled(false);
ui_->actionExport_saved_2D_map->setEnabled(false);
ui_->actionImport_2D_map->setEnabled(false);
ui_->actionView_optimized_mesh->setEnabled(false);
ui_->actionExport_optimized_mesh->setEnabled(false);
ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
links_.clear();
linksAdded_.clear();
linksRefined_.clear();
@@ -1643,6 +1656,12 @@ void DatabaseViewer::updateIds()
ui_->actionExport_saved_2D_map->setEnabled(hasMap);
ui_->actionImport_2D_map->setEnabled(hasMap);
if(!dbDriver_->loadOptimizedMesh().empty())
{
ui_->actionView_optimized_mesh->setEnabled(true);
ui_->actionExport_optimized_mesh->setEnabled(true);
}
UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
if(ids_.size() && ui_->toolBox_statistics->isVisible())
@@ -2418,6 +2437,300 @@ void DatabaseViewer::import2DMap()
}
}
void DatabaseViewer::viewOptimizedMesh()
{
if(!dbDriver_)
{
QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
return;
}
std::vector<std::vector<std::vector<unsigned int> > > polygons;
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
#else
std::vector<std::vector<Eigen::Vector2f> > texCoords;
#endif
cv::Mat textures;
cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
if(cloudMat.empty())
{
QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
}
else
{
CloudViewer * viewer = new CloudViewer(this);
viewer->setWindowFlags(Qt::Window);
viewer->setAttribute(Qt::WA_DeleteOnClose);
viewer->buildPickingLocator(true);
if(!textures.empty())
{
pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
util3d::fixTextureMeshForVisualization(*mesh);
viewer->setWindowTitle("Optimized Textured Mesh");
viewer->setPolygonPicking(true);
viewer->addCloudTextureMesh("mesh", mesh, textures);
}
else if(polygons.size() == 1)
{
pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
viewer->setWindowTitle("Optimized Mesh");
viewer->setPolygonPicking(true);
viewer->addCloudMesh("mesh", mesh);
}
else
{
LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
viewer->setWindowTitle("Optimized Point Cloud");
viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
}
viewer->show();
}
}
void DatabaseViewer::exportOptimizedMesh()
{
if(!dbDriver_)
{
QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
return;
}
std::vector<std::vector<std::vector<unsigned int> > > polygons;
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
#else
std::vector<std::vector<Eigen::Vector2f> > texCoords;
#endif
cv::Mat textures;
cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
if(cloudMat.empty())
{
QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
}
else
{
QString name = QFileInfo(databaseFileName_.c_str()).baseName();
if(!textures.empty())
{
pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
pathDatabase_+"/" + name + ".obj",
tr("Mesh (*.obj)"));
if(!path.isEmpty())
{
if(QFileInfo(path).suffix() == "")
{
path += ".obj";
}
QString baseName = QFileInfo(path).baseName();
if(mesh->tex_materials.size() == 1)
{
mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
}
else
{
for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
{
mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
}
}
pcl::io::saveOBJFile(path.toStdString(), *mesh);
QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
}
}
else if(polygons.size() == 1)
{
pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
pathDatabase_+"/" + name + ".ply",
tr("Mesh (*.ply)"));
if(!path.isEmpty())
{
if(QFileInfo(path).suffix() == "")
{
path += ".ply";
}
pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
}
}
else
{
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
pathDatabase_+"/" + name + ".ply",
tr("Point cloud data (*.ply *.pcd)"));
if(!path.isEmpty())
{
if(QFileInfo(path).suffix() == "")
{
path += ".ply";
}
bool success = false;
pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
if(QFileInfo(path).suffix() == "pcd")
{
success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
}
else
{
success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
}
if(success)
{
QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
}
else
{
QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
}
}
}
}
}
void DatabaseViewer::updateOptimizedMesh()
{
if(!ids_.size() || !dbDriver_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
if(graphes_.empty())
{
this->updateGraphView();
if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
return;
}
}
std::map<int, Transform> optimizedPoses;
if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
{
optimizedPoses = groundTruthPoses_;
}
else
{
optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
}
if(ui_->groupBox_posefiltering->isChecked())
{
optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(optimizedPoses.size() > 0)
{
exportDialog_->setDBDriver(dbDriver_);
exportDialog_->forceAssembling(true);
exportDialog_->setOkButton();
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
std::map<int, pcl::PolygonMesh::Ptr> meshes;
std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
if(exportDialog_->getExportedClouds(
optimizedPoses,
updateLinksWithModifications(links_),
mapIds_,
QMap<int, Signature>(),
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
std::map<int, LaserScan>(),
pathDatabase_,
ui_->parameters_toolbox->getParameters(),
clouds,
meshes,
textureMeshes,
textureVertexToPixels))
{
if(textureMeshes.size())
{
dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
cv::Mat globalTextures;
pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
if(textureMesh->tex_materials.size()>1)
{
globalTextures = util3d::mergeTextures(
*textureMesh,
std::map<int, cv::Mat>(),
std::map<int, std::vector<CameraModel> >(),
0,
dbDriver_,
exportDialog_->getTextureSize(),
exportDialog_->getMaxTextures(),
textureVertexToPixels,
exportDialog_->isGainCompensation(),
exportDialog_->getGainBeta(),
exportDialog_->isGainRGB(),
exportDialog_->isBlending(),
exportDialog_->getBlendingDecimation(),
exportDialog_->getTextureBrightnessConstrastRatioLow(),
exportDialog_->getTextureBrightnessConstrastRatioHigh(),
exportDialog_->isExposeFusion());
}
dbDriver_->saveOptimizedMesh(
util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
textureMesh->tex_coordinates,
globalTextures);
QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
ui_->actionView_optimized_mesh->setEnabled(true);
ui_->actionExport_optimized_mesh->setEnabled(true);
this->viewOptimizedMesh();
}
else if(meshes.size())
{
dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
ui_->actionView_optimized_mesh->setEnabled(true);
ui_->actionExport_optimized_mesh->setEnabled(true);
this->viewOptimizedMesh();
}
else if(clouds.size())
{
dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
ui_->actionView_optimized_mesh->setEnabled(true);
ui_->actionExport_optimized_mesh->setEnabled(true);
this->viewOptimizedMesh();
}
else
{
QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
}
}
exportDialog_->setProgressDialogToMax();
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
void DatabaseViewer::generateGraph()
{
if(!dbDriver_)

View File

@@ -273,6 +273,24 @@ void ExportCloudsDialog::cancel()
_progressDialog->appendText(tr("Canceled!"));
}
void ExportCloudsDialog::forceAssembling(bool enabled)
{
if(enabled)
{
_ui->checkBox_assemble->setChecked(true);
_ui->checkBox_assemble->setEnabled(false);
}
else
{
_ui->checkBox_assemble->setEnabled(true);
}
}
void ExportCloudsDialog::setProgressDialogToMax()
{
_progressDialog->setValue(_progressDialog->maximumSteps());
}
void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & group) const
{
if(!group.isEmpty())
@@ -433,7 +451,10 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_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());
if(_ui->checkBox_assemble->isEnabled())
{
_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());
_ui->comboBox_frame->setCurrentIndex(settings.value("frame", _ui->comboBox_frame->currentIndex()).toInt());
@@ -956,7 +977,7 @@ void ExportCloudsDialog::viewClouds(
}
viewer->setLighting(true);
viewer->setDefaultBackgroundColor(QColor(40, 40, 40, 255));
viewer->buildLocator(true);
viewer->buildPickingLocator(true);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
@@ -1187,6 +1208,60 @@ void ExportCloudsDialog::viewClouds(
_progressDialog->setValue(_progressDialog->maximumSteps());
}
int ExportCloudsDialog::getTextureSize() const
{
int textureSize = 1024;
if(_ui->comboBox_meshingTextureSize->currentIndex() > 0)
{
textureSize = 128 << _ui->comboBox_meshingTextureSize->currentIndex(); // start at 256
}
return textureSize;
}
int ExportCloudsDialog::getMaxTextures() const
{
return _ui->spinBox_mesh_maxTextures->value();
}
bool ExportCloudsDialog::isGainCompensation() const
{
return _ui->checkBox_gainCompensation->isChecked();
}
double ExportCloudsDialog::getGainBeta() const
{
return _ui->doubleSpinBox_gainBeta->value();
}
bool ExportCloudsDialog::isGainRGB() const
{
return _ui->checkBox_gainRGB->isChecked();
}
bool ExportCloudsDialog::isBlending() const
{
return _ui->checkBox_blending->isChecked();
}
int ExportCloudsDialog::getBlendingDecimation() const
{
int blendingDecimation = 0;
if(_ui->checkBox_blending->isChecked())
{
if(_ui->comboBox_blendingDecimation->currentIndex() > 0)
{
blendingDecimation = 1 << (_ui->comboBox_blendingDecimation->currentIndex()-1);
}
}
return blendingDecimation;
}
int ExportCloudsDialog::getTextureBrightnessConstrastRatioLow() const
{
return _ui->spinBox_textureBrightnessContrastRatioLow->value();
}
int ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh() const
{
return _ui->spinBox_textureBrightnessContrastRatioHigh->value();
}
bool ExportCloudsDialog::isExposeFusion() const
{
return _ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked();
}
bool ExportCloudsDialog::removeDirRecursively(const QString & dirName)
{
bool result = true;
@@ -2196,7 +2271,7 @@ bool ExportCloudsDialog::getExportedClouds(
poisson.reconstruct(*mesh);
}
_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(cloudsAdded).arg(clouds.size()));
_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(cloudsAdded).arg(cloudsWithNormals.size()));
QApplication::processEvents();
if(mesh->polygons.size()>0)

View File

@@ -621,6 +621,10 @@
<addaction name="actionExport_saved_2D_map"/>
<addaction name="actionImport_2D_map"/>
<addaction name="separator"/>
<addaction name="actionView_optimized_mesh"/>
<addaction name="actionUpdate_optimized_mesh"/>
<addaction name="actionExport_optimized_mesh"/>
<addaction name="separator"/>
<addaction name="actionQuit"/>
</widget>
<widget class="QMenu" name="menuEdit">
@@ -2640,6 +2644,21 @@
<string>Import 2D map...</string>
</property>
</action>
<action name="actionView_optimized_mesh">
<property name="text">
<string>View optimized mesh</string>
</property>
</action>
<action name="actionExport_optimized_mesh">
<property name="text">
<string>Export optimized mesh...</string>
</property>
</action>
<action name="actionUpdate_optimized_mesh">
<property name="text">
<string>Update optimized mesh...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>