From 98184c31dea6e4675d130aa45e0a3b8d03a3ff1e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 10 May 2017 15:53:02 -0400 Subject: [PATCH] Export clouds/poses: frame reference can be selected when clouds are not assembled --- guilib/include/rtabmap/gui/MainWindow.h | 1 + guilib/src/ExportCloudsDialog.cpp | 108 +++++---- guilib/src/MainWindow.cpp | 100 ++++++++- guilib/src/ui/exportCloudsDialog.ui | 285 +++++++++++++----------- 4 files changed, 324 insertions(+), 170 deletions(-) diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index bb0ea4b4..c5db7369 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -326,6 +326,7 @@ private: LoopClosureViewer * _loopClosureViewer; QString _graphSavingFileName; + bool _exportPosesFrame; QMap _exportPosesFileName; bool _autoScreenCaptureOdomSync; bool _autoScreenCaptureRAM; diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 19041b9c..822dbbb6 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -115,6 +115,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SLOT(updateReconstructionFlavor())); connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor())); connect(_ui->checkBox_subtraction, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_subtraction, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); @@ -263,6 +265,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("assemble", _ui->checkBox_assemble->isChecked()); settings.setValue("assemble_voxel",_ui->doubleSpinBox_voxelSize_assembled->value()); + settings.setValue("frame",_ui->comboBox_frame->currentIndex()); settings.setValue("subtract",_ui->checkBox_subtraction->isChecked()); settings.setValue("subtract_point_radius",_ui->doubleSpinBox_subtractPointFilteringRadius->value()); @@ -370,6 +373,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _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()); _ui->checkBox_subtraction->setChecked(settings.value("subtract",_ui->checkBox_subtraction->isChecked()).toBool()); _ui->doubleSpinBox_subtractPointFilteringRadius->setValue(settings.value("subtract_point_radius",_ui->doubleSpinBox_subtractPointFilteringRadius->value()).toDouble()); @@ -477,6 +481,7 @@ void ExportCloudsDialog::restoreDefaults() _ui->checkBox_assemble->setChecked(true); _ui->doubleSpinBox_voxelSize_assembled->setValue(0.0); + _ui->comboBox_frame->setCurrentIndex(0); _ui->checkBox_subtraction->setChecked(false); _ui->doubleSpinBox_subtractPointFilteringRadius->setValue(0.02); @@ -558,10 +563,17 @@ void ExportCloudsDialog::updateReconstructionFlavor() _ui->checkBox_smoothing->setVisible(_ui->comboBox_pipeline->currentIndex() == 1); _ui->checkBox_smoothing->setEnabled(_ui->comboBox_pipeline->currentIndex() == 1); + _ui->comboBox_frame->setEnabled(!_ui->checkBox_assemble->isChecked() && _ui->checkBox_binary->isEnabled()); + _ui->comboBox_frame->setVisible(_ui->comboBox_frame->isEnabled()); + _ui->label_frame->setVisible(_ui->comboBox_frame->isEnabled()); + _ui->checkBox_gainCompensation->setEnabled(!(_ui->comboBox_frame->isEnabled() && _ui->comboBox_frame->currentIndex() == 2)); + _ui->checkBox_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled()); + _ui->label_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled()); + _ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked()); _ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked()); _ui->groupBox_filtering->setVisible(_ui->checkBox_filtering->isChecked()); - _ui->groupBox_gain->setVisible(_ui->checkBox_gainCompensation->isChecked()); + _ui->groupBox_gain->setVisible(_ui->checkBox_gainCompensation->isEnabled() && _ui->checkBox_gainCompensation->isChecked()); _ui->groupBox_mls->setVisible(_ui->checkBox_smoothing->isEnabled() && _ui->checkBox_smoothing->isChecked()); _ui->groupBox_meshing->setVisible(_ui->checkBox_meshing->isChecked()); _ui->groupBox_subtraction->setVisible(_ui->checkBox_subtraction->isChecked()); @@ -1060,7 +1072,9 @@ bool ExportCloudsDialog::getExportedClouds( _ui->comboBox_pipeline->currentIndex()==1 && _ui->checkBox_assemble->isChecked() && _ui->comboBox_meshingTextureSize->isEnabled() && - _ui->comboBox_meshingTextureSize->currentIndex() > 0)) + _ui->comboBox_meshingTextureSize->currentIndex() > 0) && + // Don't do compensation if clouds are in camera frame + !(_ui->comboBox_frame->isEnabled() && _ui->comboBox_frame->currentIndex()==2)) { UASSERT(_compensator == 0); _compensator = new GainCompensator(_ui->doubleSpinBox_gainRadius->value(), _ui->doubleSpinBox_gainOverlap->value(), 0.01, _ui->doubleSpinBox_gainBeta->value()); @@ -1196,7 +1210,7 @@ bool ExportCloudsDialog::getExportedClouds( return false; } - std::map viewPoints = poses; + std::map mlsViewPoints = poses; if(_ui->checkBox_smoothing->isEnabled() && _ui->checkBox_smoothing->isChecked()) { _progressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... " @@ -1205,29 +1219,32 @@ bool ExportCloudsDialog::getExportedClouds( uSleep(100); QApplication::processEvents(); - // Adjust view points with local transforms - for(std::map::iterator iter= viewPoints.begin(); iter!=viewPoints.end(); ++iter) + if(_ui->checkBox_assemble->isChecked()) { - std::vector models; - StereoCameraModel stereoModel; - if(cachedSignatures.contains(iter->first)) + // Adjust view points with local transforms + for(std::map::iterator iter= mlsViewPoints.begin(); iter!=mlsViewPoints.end(); ++iter) { - const SensorData & data = cachedSignatures.find(iter->first)->sensorData(); - models = data.cameraModels(); - stereoModel = data.stereoCameraModel(); - } - else if(_dbDriver) - { - _dbDriver->getCalibration(iter->first, models, stereoModel); - } + std::vector models; + StereoCameraModel stereoModel; + if(cachedSignatures.contains(iter->first)) + { + const SensorData & data = cachedSignatures.find(iter->first)->sensorData(); + models = data.cameraModels(); + stereoModel = data.stereoCameraModel(); + } + else if(_dbDriver) + { + _dbDriver->getCalibration(iter->first, models, stereoModel); + } - if(models.size() && !models[0].localTransform().isNull()) - { - iter->second *= models[0].localTransform(); - } - else if(!stereoModel.localTransform().isNull()) - { - iter->second *= stereoModel.localTransform(); + if(models.size() && !models[0].localTransform().isNull()) + { + iter->second *= models[0].localTransform(); + } + else if(!stereoModel.localTransform().isNull()) + { + iter->second *= stereoModel.localTransform(); + } } } } @@ -1289,7 +1306,7 @@ bool ExportCloudsDialog::getExportedClouds( _progressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size())); util3d::adjustNormalsToViewPoints( - viewPoints, + mlsViewPoints, rawAssembledCloud, rawCameraIndices, cloudWithNormals); @@ -1422,17 +1439,21 @@ bool ExportCloudsDialog::getExportedClouds( else #endif { - if(models.size() && !models[0].localTransform().isNull()) + if((_ui->comboBox_frame->isEnabled() && _ui->comboBox_frame->currentIndex() != 2) || + !iter->second->isOrganized()) { - viewpoint[0] = models[0].localTransform().x(); - viewpoint[1] = models[0].localTransform().y(); - viewpoint[2] = models[0].localTransform().z(); - } - else if(!stereoModel.localTransform().isNull()) - { - viewpoint[0] = stereoModel.localTransform().x(); - viewpoint[1] = stereoModel.localTransform().y(); - viewpoint[2] = stereoModel.localTransform().z(); + if(models.size() && !models[0].localTransform().isNull()) + { + viewpoint[0] = models[0].localTransform().x(); + viewpoint[1] = models[0].localTransform().y(); + viewpoint[2] = models[0].localTransform().z(); + } + else if(!stereoModel.localTransform().isNull()) + { + viewpoint[0] = stereoModel.localTransform().x(); + viewpoint[1] = stereoModel.localTransform().y(); + viewpoint[2] = stereoModel.localTransform().z(); + } } std::vector polygons = util3d::organizedFastMesh( @@ -2184,6 +2205,7 @@ std::map::Ptr, pcl::Indic { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::IndicesPtr indices(new std::vector); + Transform localTransform = Transform::getIdentity(); if(_ui->checkBox_regenerate->isChecked()) { SensorData data; @@ -2267,12 +2289,14 @@ std::map::Ptr, pcl::Indic Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f); if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull()) { + localTransform = data.cameraModels()[0].localTransform(); viewPoint[0] = data.cameraModels()[0].localTransform().x(); viewPoint[1] = data.cameraModels()[0].localTransform().y(); viewPoint[2] = data.cameraModels()[0].localTransform().z(); } else if(!data.stereoCameraModel().localTransform().isNull()) { + localTransform = data.stereoCameraModel().localTransform(); viewPoint[0] = data.stereoCameraModel().localTransform().x(); viewPoint[1] = data.stereoCameraModel().localTransform().y(); viewPoint[2] = data.stereoCameraModel().localTransform().z(); @@ -2362,12 +2386,14 @@ std::map::Ptr, pcl::Indic if(models.size() && !models[0].localTransform().isNull()) { + localTransform = models[0].localTransform(); viewPoint[0] = models[0].localTransform().x(); viewPoint[1] = models[0].localTransform().y(); viewPoint[2] = models[0].localTransform().z(); } else if(!stereoModel.localTransform().isNull()) { + localTransform = stereoModel.localTransform(); viewPoint[0] = stereoModel.localTransform().x(); viewPoint[1] = stereoModel.localTransform().y(); viewPoint[2] = stereoModel.localTransform().z(); @@ -2393,6 +2419,12 @@ std::map::Ptr, pcl::Indic { indices = util3d::radiusFiltering(cloud, indices, _ui->doubleSpinBox_filteringRadius->value(), _ui->spinBox_filteringMinNeighbors->value()); } + + if((_ui->comboBox_frame->isEnabled() && _ui->comboBox_frame->currentIndex()==2) && cloud->isOrganized()) + { + cloud = util3d::transformPointCloud(cloud, localTransform.inverse()); // put back in camera frame + } + clouds.insert(std::make_pair(iter->first, std::make_pair(cloud, indices))); points = (int)cloud->size(); totalIndices = (int)indices->size(); @@ -2502,7 +2534,7 @@ void ExportCloudsDialog::saveClouds( if(iter->second->size()) { pcl::PointCloud::Ptr transformedCloud; - transformedCloud = util3d::transformPointCloud(iter->second, poses.at(iter->first)); + transformedCloud = util3d::transformPointCloud(iter->second, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity()); QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); bool success =false; @@ -2642,14 +2674,14 @@ void ExportCloudsDialog::saveMeshes( { pcl::PointCloud::Ptr tmp(new pcl::PointCloud); pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); - tmp = util3d::transformPointCloud(tmp, poses.at(iter->first)); + tmp = util3d::transformPointCloud(tmp, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity()); pcl::toPCLPointCloud2(*tmp, mesh.cloud); } else { pcl::PointCloud::Ptr tmp(new pcl::PointCloud); pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); - tmp = util3d::transformPointCloud(tmp, poses.at(iter->first)); + tmp = util3d::transformPointCloud(tmp, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity()); pcl::toPCLPointCloud2(*tmp, mesh.cloud); } @@ -3564,7 +3596,7 @@ void ExportCloudsDialog::saveTextureMeshes( } pcl::PointCloud::Ptr tmp(new pcl::PointCloud); pcl::fromPCLPointCloud2(mesh->cloud, *tmp); - tmp = util3d::transformPointCloud(tmp, poses.at(iter->first)); + tmp = util3d::transformPointCloud(tmp, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity()); pcl::toPCLPointCloud2(*tmp, mesh->cloud); QString pathFile = path+QDir::separator()+QString("%1.%3").arg(currentPrefix).arg(suffix); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index f92c2669..b06d8c9f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -163,6 +163,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _posteriorCurve(0), _likelihoodCurve(0), _rawLikelihoodCurve(0), + _exportPosesFrame(0), _autoScreenCaptureOdomSync(false), _autoScreenCaptureRAM(false), _firstCall(true), @@ -4737,20 +4738,94 @@ void MainWindow::exportPoses(int format) { if(_currentPosesMap.size()) { + std::map poses; + QStringList items; + items.push_back("Robot"); + items.push_back("Camera"); + items.push_back("Scan"); + QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false); + if(item.isEmpty()) + { + return; + } + if(item.compare("Robot") != 0) + { + bool cameraFrame = item.compare("Camera") == 0; + _exportPosesFrame = cameraFrame?1:2; + for(std::map::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter) + { + if(_cachedSignatures.contains(iter->first)) + { + Transform localTransform; + if(cameraFrame) + { + if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && + !_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull())) + { + localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform(); + } + else if(!_cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform().isNull()) + { + localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform(); + } + else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1) + { + UWARN("Multi-camera is not supported (node %d)", iter->first); + } + else + { + UWARN("Missing calibration for node %d", iter->first); + } + } + else + { + if(!_cachedSignatures[iter->first].sensorData().laserScanInfo().localTransform().isNull()) + { + localTransform = _cachedSignatures[iter->first].sensorData().laserScanInfo().localTransform(); + } + else + { + UWARN("Missing scan info for node %d", iter->first); + } + } + if(!localTransform.isNull()) + { + poses.insert(std::make_pair(iter->first, iter->second * localTransform)); + } + } + else + { + UWARN("Did not find node %d in cache", iter->first); + } + } + if(poses.empty()) + { + QMessageBox::warning(this, + tr("Export Poses"), + tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item)); + poses = _currentPosesMap; + } + } + else + { + _exportPosesFrame = 0; + poses = _currentPosesMap; + } + std::map stamps; if(format == 1) { - for(std::map::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter) + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { if(_cachedSignatures.contains(iter->first)) { stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp())); } } - if(stamps.size()!=_currentPosesMap.size()) + if(stamps.size()!=poses.size()) { QMessageBox::warning(this, tr("Export poses..."), tr("RGB-D SLAM format: Poses (%1) and stamps (%2) have not the same size! Try again after updating the cache.") - .arg(_currentPosesMap.size()).arg(stamps.size())); + .arg(poses.size()).arg(stamps.size())); return; } } @@ -4770,7 +4845,24 @@ void MainWindow::exportPoses(int format) { _exportPosesFileName[format] = path; - bool saved = graph::exportPoses(path.toStdString(), format, _currentPosesMap, _currentLinksMap, stamps); + std::multimap links; + if(poses.size() != _currentPosesMap.size()) + { + for(std::multimap::iterator iter=_currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter) + { + if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to())) + { + links.insert(*iter); + } + } + } + else + { + links = _currentLinksMap; + } + + + bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps); if(saved) { diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index b89b73db..a540a8d4 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,89 +23,25 @@ 0 - -3080 - 778 - 3701 + 0 + 773 + 3795 - - + + - - - - - - - - Assemble clouds/meshes to a single output cloud/mesh. + Meshing. true - - - - - - - true - - - - - - - Set the number of k nearest neighbors to use for the normal estimation. - - - true - - - - - - - - - - - - - Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization. - - - true - - - - - - - Gain compensation. Normalize brightness of images. - - - true - - - - - - - 3 - - - 20 - - - - Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged. @@ -115,38 +51,65 @@ - - + + + + 3 + + + 20 + + + + + - Binary file. + + + + + + + + Cloud filtering. Remove sparse points that are far from surfaces. true - - - - - Organized Point Cloud - - - - - Dense Point Cloud - - - - - - + + - Reconstruction flavor. + - + + + + + + + + + + + + + + + + + + Cloud smoothing using Moving Least Squares algorithm (MLS). + + + true + + + + m @@ -165,51 +128,41 @@ - - + + - Cloud smoothing using Moving Least Squares algorithm (MLS). + Reconstruction flavor. + + + + + + + + Organized Point Cloud + + + + + Dense Point Cloud + + + + + + + + Binary file. true - - - - - - - - - - - - - - - - - - - - - - + - Cloud filtering. Remove sparse points that are far from surfaces. - - - true - - - - - - - Meshing. + Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization. true @@ -217,12 +170,88 @@ - + + + + + Set the number of k nearest neighbors to use for the normal estimation. + + + true + + + + + + + + + + true + + + + + + + Assemble clouds/meshes to a single output cloud/mesh. + + + true + + + + + + + + + + + + + + Gain compensation. Normalize brightness of images. + + + true + + + + + + + Output frame. + + + true + + + + + + + + Map + + + + + Robot + + + + + Camera + + + +