From a88e8dd013893661a31e41ecf1e4e0b038d004ae Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 5 Aug 2015 20:16:27 -0400 Subject: [PATCH 001/110] ICP correspondence ratio set to 0 when maximum laser scan count is not set --- corelib/include/rtabmap/core/Link.h | 2 +- corelib/src/Memory.cpp | 2 +- corelib/src/Rtabmap.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 59816069..ae67c46d 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -130,7 +130,7 @@ public: from_, link.to(), type_, - transform_ * link.transform(), + transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2) infMatrix_ + link.infMatrix()); } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 178f02bd..a0174e7b 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2495,7 +2495,7 @@ Transform Memory::computeIcpTransform( { Transform icpT; bool hasConverged = false; - float correspondencesRatio = -1.0f; + float correspondencesRatio = 0.0f; int correspondences = 0; double variance = 1; pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud()); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 68d1347e..4b19e677 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1107,7 +1107,7 @@ bool Rtabmap::process( if(!transform.isNull() && _globalLoopClosureIcpType > 0) { transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance); - variance = 1.0f; // ICP, set variance to 1 + variance = 1.0f; // ICP, set variance to 1 // FIXME why? all other links based on visual keep the variance } if(!transform.isNull()) { From 0350e475b2e47e9329d5a484ca8719e354494963 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 5 Aug 2015 20:32:33 -0400 Subject: [PATCH 002/110] ICP: when max laser scan max pts is not set, use relative correspondence ratio (backward compatibility) --- corelib/src/Memory.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index a0174e7b..943cc9a1 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2346,7 +2346,7 @@ Transform Memory::computeIcpTransform( bool hasConverged = false; Transform icpT; int correspondences = 0; - float correspondencesRatio = -1.0f; + float correspondencesRatio = 0.0f; double variance = 1; if(_icpPointToPlane) { @@ -2561,8 +2561,9 @@ Transform Memory::computeIcpTransform( } else { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS.id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()); } UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", @@ -2756,8 +2757,9 @@ Transform Memory::computeScanMatchingTransform( } else { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS->id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()); } UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", From e12b10b860a85872568180e11f5babd78bf266b0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 6 Aug 2015 10:01:55 -0400 Subject: [PATCH 003/110] :lipstick: --- corelib/include/rtabmap/core/OdometryInfo.h | 1 + 1 file changed, 1 insertion(+) diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index ab775e77..7da1c8c0 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -43,6 +43,7 @@ public: features(-1), localMapSize(-1), timeEstimation(-1), + timeParticleFiltering(-1), stamp(0), interval(0), distanceTravelled(0), From e4d5282e6ffcc383267855e63847a60ade506049 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 7 Aug 2015 22:49:54 -0400 Subject: [PATCH 004/110] updated when "Export clouds/scans" actions are enabled --- guilib/src/MainWindow.cpp | 32 +++++++------------------------- 1 file changed, 7 insertions(+), 25 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index b6274538..3503b663 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1344,31 +1344,6 @@ void MainWindow::updateMapCloud( _currentLinksMap = constraints; _currentMapIds = mapIdsIn; _curentLabels = labels; - if(_currentPosesMap.size()) - { - if(!_ui->actionSave_point_cloud->isEnabled() && - _cachedSignatures.size() && - (!(--_cachedSignatures.end())->sensorData().depthOrRightCompressed().empty() || - !(--_cachedSignatures.end())->getWords3().empty())) - { - //enable save cloud action - _ui->actionSave_point_cloud->setEnabled(true); - _ui->actionView_high_res_point_cloud->setEnabled(true); - } - - if(!_ui->actionView_scans->isEnabled() && - _cachedSignatures.size() && - !(--_cachedSignatures.end())->sensorData().laserScanCompressed().empty()) - { - _ui->actionExport_2D_scans_ply_pcd->setEnabled(true); - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true); - _ui->actionView_scans->setEnabled(true); - } - else if(_preferencesDialog->isGridMapFrom3DCloud() && _projectionLocalMaps.size()) - { - _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true); - } - } if(_state != kMonitoring && _state != kDetecting) { _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1); @@ -1517,6 +1492,13 @@ void MainWindow::updateMapCloud( ++i; } + // activate actions + _ui->actionSave_point_cloud->setEnabled(!_createdClouds.empty()); + _ui->actionView_high_res_point_cloud->setEnabled(!_createdClouds.empty()); + _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); + _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); + _ui->actionView_scans->setEnabled(!_createdScans.empty()); + //remove not used clouds for(QMap::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter) { From 25e7b4a5c69bfa985edde5c6bf592601a131614c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 9 Aug 2015 13:31:05 -0400 Subject: [PATCH 005/110] Increased version 0.10.5: Added unknownSpaceFilled parameter to occupancy2DFromLaserScan() --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/util3d_mapping.h | 3 ++- corelib/src/toro3d/transformation2.hh | 6 ++++-- corelib/src/util3d_mapping.cpp | 5 +++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d2ced370..1b468954 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 10) -SET(RTABMAP_PATCH_VERSION 4) +SET(RTABMAP_PATCH_VERSION 5) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index fceb7e7a..88fd1ce3 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -47,7 +47,8 @@ void RTABMAP_EXP occupancy2DFromLaserScan( const cv::Mat & scan, cv::Mat & ground, cv::Mat & obstacles, - float cellSize); + float cellSize, + bool unknownSpaceFilled = false); cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( const std::map & poses, diff --git a/corelib/src/toro3d/transformation2.hh b/corelib/src/toro3d/transformation2.hh index dcf0ba5e..03d77ba2 100644 --- a/corelib/src/toro3d/transformation2.hh +++ b/corelib/src/toro3d/transformation2.hh @@ -187,8 +187,10 @@ struct Transformation2{ } /** Constructor that sets the translation and rotation **/ - Transformation2 (const T& _theta, const Vector2& trans): - Transformation2(trans.x(), trans.y(), _theta){} + Transformation2 (const T& _theta, const Vector2& trans){ + setRotation(_theta); + setTranslation(trans.x(), trans.y()); + } /** Copy constructor **/ diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 9cb312e8..a0cf6fb4 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -49,7 +49,8 @@ void occupancy2DFromLaserScan( const cv::Mat & scan, cv::Mat & ground, cv::Mat & obstacles, - float cellSize) + float cellSize, + bool unknownSpaceFilled) { if(scan.empty()) { @@ -66,7 +67,7 @@ void occupancy2DFromLaserScan( scans.insert(std::make_pair(1, obstaclesCloud)); float xMin, yMin; - cv::Mat map8S = create2DMap(poses, scans, cellSize, false, xMin, yMin); + cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin); // find ground cells std::list groundIndices; From 2d0177f40b9afe902bff198ba7bfd6cb08b0776f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 10 Aug 2015 11:39:40 -0400 Subject: [PATCH 006/110] Added "File->Export cameras in Bundler format (*.out)" action --- guilib/include/rtabmap/gui/MainWindow.h | 1 + guilib/src/MainWindow.cpp | 121 +++++++++++++++++++++++- guilib/src/ui/mainWindow.ui | 6 ++ 3 files changed, 124 insertions(+), 4 deletions(-) diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index d7ff45e6..0d1f42a2 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -180,6 +180,7 @@ private slots: void exportGridMap(); void exportScans(); void exportClouds(); + void exportBundlerFormat(); void viewScans(); void viewClouds(); void resetOdometry(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 3503b663..593376fd 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -62,6 +62,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -322,6 +324,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds())); connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans())); connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap())); + connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat())); connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans())); connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds())); connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry())); @@ -334,6 +337,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->actionSave_point_cloud->setEnabled(false); _ui->actionExport_2D_scans_ply_pcd->setEnabled(false); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false); + _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionView_scans->setEnabled(false); _ui->actionView_high_res_point_cloud->setEnabled(false); _ui->actionReset_Odometry->setEnabled(false); @@ -1498,6 +1502,7 @@ void MainWindow::updateMapCloud( _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); + _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_createdClouds.empty()); //remove not used clouds for(QMap::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter) @@ -3948,6 +3953,7 @@ void MainWindow::clearTheCache() _ui->actionExport_2D_scans_ply_pcd->setEnabled(false); _ui->actionPost_processing->setEnabled(false); _ui->actionSave_point_cloud->setEnabled(false); + _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionView_scans->setEnabled(false); _ui->actionView_high_res_point_cloud->setEnabled(false); _likelihoodCurve->clear(); @@ -4558,6 +4564,113 @@ bool MainWindow::getExportedClouds( return false; } +void MainWindow::exportBundlerFormat() +{ + std::map posesIn = _ui->widget_mapVisibility->getVisiblePoses(); + + std::map poses; + for(std::map::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter) + { + if(_cachedSignatures.contains(iter->first)) + { + if(_cachedSignatures[iter->first].sensorData().imageRaw().empty() && + _cachedSignatures[iter->first].sensorData().imageCompressed().empty()) + { + UWARN("Missing image in cache for node %d", iter->first); + } + else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValid()) || + _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValid()) + { + poses.insert(*iter); + } + else + { + UWARN("Missing calibration for node %d", iter->first); + } + } + else + { + UWARN("Did not find node %d in cache", iter->first); + } + } + + if(poses.size()) + { + QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _preferencesDialog->getWorkingDirectory()); + if(!path.isEmpty()) + { + // export cameras and images + QFile fileOut(path+QDir::separator()+"cameras.out"); + QFile fileList(path+QDir::separator()+"list.txt"); + QDir(path).mkdir("images"); + if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text)) + { + if(fileList.open(QIODevice::WriteOnly | QIODevice::Text)) + { + QTextStream out(&fileOut); + QTextStream list(&fileList); + out << "# Bundle file v0.3\n"; + out << poses.size() << " 0\n"; + + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first); + list << p << "\n"; + p = path+QDir::separator()+p; + cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw(); + if(image.empty()) + { + _cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0); + } + + if(cv::imwrite(p.toStdString(), image)) + { + UINFO("saved image %s", p.toStdString().c_str()); + } + else + { + UERROR("Failed to save image %s", p.toStdString().c_str()); + } + + if(_cachedSignatures[iter->first].sensorData().cameraModels().size()) + { + out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n"; + } + else + { + out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n"; + } + + out << iter->second.r11() << " " << iter->second.r12() << " " << iter->second.r13() << "\n"; + out << iter->second.r21() << " " << iter->second.r22() << " " << iter->second.r23() << "\n"; + out << iter->second.r31() << " " << iter->second.r32() << " " << iter->second.r33() << "\n"; + out << iter->second.x() << " " << iter->second.y() << " " << iter->second.z() << "\n"; + + //double t[3]; + //matrix_product(3, 3, 3, 1, cameras[idx].R, cameras[idx].t, t); + //matrix_scale(3, 1, t, -1.0, t); + //fprintf(f, "%0.10e %0.10e %0.10e\n", t[0], t[1], t[2]); + } + + QMessageBox::Button b = QMessageBox::question(this, + tr("Exporting cameras in Bundler format..."), + tr("%1 cameras/images exported to directory \"%2\".\nDo you want to export the cloud/mesh (PLY)?").arg(poses.size()).arg(path)); + if(b == QMessageBox::Ok) + { + this->exportClouds(); + } + fileList.close(); + } + fileOut.close(); + } + } + } + else + { + QMessageBox::warning(this, tr("Exporting cameras..."), tr("No poses exported...")); + } +} + void MainWindow::resetOdometry() { UINFO("reset odometry"); @@ -5189,7 +5302,7 @@ void MainWindow::changeState(MainWindow::State newState) } } actions = _ui->menuFile->actions(); - if(actions.size()>=9) + if(actions.size()>=10) { if(actions.at(2)->isSeparator()) { @@ -5199,9 +5312,9 @@ void MainWindow::changeState(MainWindow::State newState) { UWARN("Menu File separators have not the same order."); } - if(actions.at(8)->isSeparator()) + if(actions.at(9)->isSeparator()) { - actions.at(8)->setVisible(!monitoring); + actions.at(9)->setVisible(!monitoring); } else { @@ -5210,7 +5323,7 @@ void MainWindow::changeState(MainWindow::State newState) } else { - UWARN("Menu File separators have not the same order."); + UWARN("Menu File actions size has changed (%d)", actions.size()); } actions = _ui->menuProcess->actions(); if(actions.size()>=2) diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index dd4c1572..f6ec2d8f 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -42,6 +42,7 @@ + @@ -1247,6 +1248,11 @@ TORO (*.graph) + + + Export cameras in Bundler format (*.out)... + + From 3ac9d71480ad220b6df8b4a9c9e401c0e231f208 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Mon, 10 Aug 2015 13:55:08 -0400 Subject: [PATCH 007/110] fixed Windows build --- corelib/src/util3d_motion_estimation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp index b359a7e9..7f3f939a 100644 --- a/corelib/src/util3d_motion_estimation.cpp +++ b/corelib/src/util3d_motion_estimation.cpp @@ -134,7 +134,7 @@ Transform estimateMotion3DTo2D( oi = 0; for(unsigned int i=0; i::const_iterator iter = words3B.find(matches[inliers[i]]); + std::map::const_iterator iter = words3B.find(matches[inliers[i]]); if(iter != words3B.end() && pcl::isFinite(iter->second)) { const cv::Point3f & objPt = objectPoints[inliers[i]]; From 9c653655bd5a6c36f63f841871627d6a6948e792 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Mon, 10 Aug 2015 17:12:55 -0400 Subject: [PATCH 008/110] functional bundler export (camera orientations are correct in MeshLab) --- guilib/src/MainWindow.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 593376fd..f5e180e0 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4632,30 +4632,40 @@ void MainWindow::exportBundlerFormat() UERROR("Failed to save image %s", p.toStdString().c_str()); } + Transform localTransform; if(_cachedSignatures[iter->first].sensorData().cameraModels().size()) { out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n"; + localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform(); } else { out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n"; + localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform(); } - out << iter->second.r11() << " " << iter->second.r12() << " " << iter->second.r13() << "\n"; - out << iter->second.r21() << " " << iter->second.r22() << " " << iter->second.r23() << "\n"; - out << iter->second.r31() << " " << iter->second.r32() << " " << iter->second.r33() << "\n"; - out << iter->second.x() << " " << iter->second.y() << " " << iter->second.z() << "\n"; + Transform rotation(0,-1,0,0, + 0,0,1,0, + -1,0,0,0); - //double t[3]; - //matrix_product(3, 3, 3, 1, cameras[idx].R, cameras[idx].t, t); - //matrix_scale(3, 1, t, -1.0, t); - //fprintf(f, "%0.10e %0.10e %0.10e\n", t[0], t[1], t[2]); + Transform R = rotation*iter->second.rotation().inverse(); + + out << R.r11() << " " << R.r12() << " " << R.r13() << "\n"; + out << R.r21() << " " << R.r22() << " " << R.r23() << "\n"; + out << R.r31() << " " << R.r32() << " " << R.r33() << "\n"; + + Transform t = R * iter->second.translation(); + t.x() *= -1.0f; + t.y() *= -1.0f; + t.z() *= -1.0f; + out << t.x() << " " << t.y() << " " << t.z() << "\n"; } QMessageBox::Button b = QMessageBox::question(this, tr("Exporting cameras in Bundler format..."), - tr("%1 cameras/images exported to directory \"%2\".\nDo you want to export the cloud/mesh (PLY)?").arg(poses.size()).arg(path)); - if(b == QMessageBox::Ok) + tr("%1 cameras/images exported to directory \"%2\".\nDo you want to export the cloud/mesh (PLY)?").arg(poses.size()).arg(path), + QMessageBox::Yes | QMessageBox::No); + if(b == QMessageBox::Yes) { this->exportClouds(); } From abdd3de7737fac17f1eb08b73877bc1665406f6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Tue, 11 Aug 2015 17:19:12 -0400 Subject: [PATCH 009/110] Mesh reconstruction update: fixed normals computation (with and without MLS) --- .../include/rtabmap/core/util3d_filtering.h | 29 ++ corelib/include/rtabmap/core/util3d_surface.h | 40 ++- .../include/rtabmap/core/util3d_transforms.h | 3 + corelib/src/util3d_filtering.cpp | 88 +++++ corelib/src/util3d_surface.cpp | 44 ++- corelib/src/util3d_transforms.cpp | 8 + guilib/include/rtabmap/gui/CloudViewer.h | 49 ++- guilib/include/rtabmap/gui/MainWindow.h | 18 +- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/CloudViewer.cpp | 69 +++- guilib/src/ExportCloudsDialog.cpp | 13 +- guilib/src/ExportCloudsDialog.h | 2 + guilib/src/MainWindow.cpp | 333 +++++++++--------- guilib/src/PreferencesDialog.cpp | 8 + guilib/src/ui/exportCloudsDialog.ui | 34 +- guilib/src/ui/preferencesDialog.ui | 45 ++- 16 files changed, 580 insertions(+), 204 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index e9c2ac4e..90316dc3 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -46,6 +46,9 @@ pcl::PointCloud::Ptr RTABMAP_EXP voxelize( pcl::PointCloud::Ptr RTABMAP_EXP voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); +pcl::PointCloud::Ptr RTABMAP_EXP voxelize( + const pcl::PointCloud::Ptr & cloud, + float voxelSize); pcl::PointCloud::Ptr RTABMAP_EXP sampling( @@ -141,6 +144,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( float radiusSearch, int minNeighborsInRadius = 0); +/** + * For convenience. + */ +pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::PointCloud::Ptr & substractCloud, + float radiusSearch, + int minNeighborsInRadius = 0); + +/** + * Subtract a cloud from another one using radius filtering. + * @param cloud the input cloud. + * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked. + * @param cloud the input cloud to subtract. + * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked. + * @param radiusSearch the radius in meter. + * @return the indices of the points satisfying the parameters. + */ +pcl::IndicesPtr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const pcl::PointCloud::Ptr & substractCloud, + const pcl::IndicesPtr & substractIndices, + float radiusSearch, + int minNeighborsInRadius = 0); + /** * For convenience. diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index d6ecfec2..1576ccc1 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - #include #include #include @@ -49,7 +48,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh( float gp3MaximumSurfaceAngle = M_PI/4, float gp3MinimumAngle = M_PI/18, float gp3MaximumAngle = 2*M_PI/3, - bool gp3NormalConsistency = false); + bool gp3NormalConsistency = true); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, @@ -62,7 +61,42 @@ pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( pcl::PointCloud::Ptr RTABMAP_EXP computeNormalsSmoothed( const pcl::PointCloud::Ptr & cloud, float smoothingSearchRadius = 0.025, - bool smoothingPolynomialFit = true); + bool smoothingPolynomialFit = true, + float voxelSize = 0.0f); + +void RTABMAP_EXP adjustNormalsToViewPoints( + const pcl::PointCloud::Ptr & viewpoints, + pcl::PointCloud & cloud); + +template +std::vector normalizePolygonsSide( + const pcl::PointCloud & cloud, + const std::vector & polygons, + const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0)) +{ + std::vector output(polygons.size()); + for(unsigned int i=0; i::Ptr RTABMAP_EXP transformPointCloud( pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); +pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform); pcl::PointXYZ RTABMAP_EXP transformPoint( const pcl::PointXYZ & pt, diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 61f5a35a..5623885a 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -72,6 +72,18 @@ pcl::PointCloud::Ptr voxelize( filter.filter(*output); return output; } +pcl::PointCloud::Ptr voxelize( + const pcl::PointCloud::Ptr & cloud, + float voxelSize) +{ + UASSERT(voxelSize > 0.0f); + pcl::PointCloud::Ptr output(new pcl::PointCloud); + pcl::VoxelGrid filter; + filter.setLeafSize(voxelSize, voxelSize, voxelSize); + filter.setInputCloud(cloud); + filter.filter(*output); + return output; +} pcl::PointCloud::Ptr sampling( @@ -357,6 +369,82 @@ pcl::IndicesPtr subtractFiltering( } } +pcl::PointCloud::Ptr subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::PointCloud::Ptr & substractCloud, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::IndicesPtr indices(new std::vector); + pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius); + pcl::PointCloud::Ptr out(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *indicesOut, *out); + return out; +} + + +pcl::IndicesPtr subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const pcl::PointCloud::Ptr & substractCloud, + const pcl::IndicesPtr & substractIndices, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree(false)); + + if(indices->size()) + { + pcl::IndicesPtr output(new std::vector(indices->size())); + int oi = 0; // output iterator + if(substractIndices->size()) + { + tree->setInputCloud(substractCloud, substractIndices); + } + else + { + tree->setInputCloud(substractCloud); + } + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector 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(cloud->size())); + int oi = 0; // output iterator + if(substractIndices->size()) + { + tree->setInputCloud(substractCloud, substractIndices); + } + else + { + tree->setInputCloud(substractCloud); + } + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector 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 normalFiltering( const pcl::PointCloud::Ptr & cloud, diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 20d6a87e..38578f7d 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/utilite/ULogger.h" #include #include #include @@ -68,12 +69,16 @@ pcl::PolygonMesh::Ptr createMesh( gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees gp3.setNormalConsistency(gp3NormalConsistency); + gp3.setConsistentVertexOrdering(gp3NormalConsistency); // Get result gp3.setInputCloud (cloudWithNormalsNoNaN); gp3.setSearchMethod (tree2); gp3.reconstruct (*mesh); + //UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size()); + //mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons); + return mesh; } @@ -128,7 +133,8 @@ pcl::PointCloud::Ptr computeNormals( pcl::PointCloud::Ptr computeNormalsSmoothed( const pcl::PointCloud::Ptr & cloud, float smoothingSearchRadius, - bool smoothingPolynomialFit) + bool smoothingPolynomialFit, + float voxelSize) { pcl::PointCloud::Ptr cloud_with_normals(new pcl::PointCloud); pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); @@ -144,6 +150,11 @@ pcl::PointCloud::Ptr computeNormalsSmoothed( mls.setPolynomialFit (smoothingPolynomialFit); mls.setSearchMethod (tree); mls.setSearchRadius (smoothingSearchRadius); + if(voxelSize > 0.0f) + { + mls.setUpsamplingMethod(pcl::MovingLeastSquares::VOXEL_GRID_DILATION); + mls.setDilationVoxelSize(voxelSize); + } // Reconstruct mls.process (*cloud_with_normals); @@ -151,6 +162,37 @@ pcl::PointCloud::Ptr computeNormalsSmoothed( return cloud_with_normals; } +void adjustNormalsToViewPoints( + const pcl::PointCloud::Ptr & viewpoints, + pcl::PointCloud & cloud) +{ + if(viewpoints->size() && cloud.size()) + { + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (viewpoints); + + for(unsigned int i=0; i indices; + std::vector dist; + tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist); + UASSERT(indices.size() == 1); + + Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap(); + Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].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; + } + } + } +} + } } diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 659c1e0c..1c93bf38 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -51,6 +51,14 @@ pcl::PointCloud::Ptr transformPointCloud( pcl::transformPointCloud(*cloud, *output, transform.toEigen4f()); return output; } +pcl::PointCloud::Ptr transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform) +{ + pcl::PointCloud::Ptr output(new pcl::PointCloud); + pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f()); + return output; +} pcl::PointXYZ transformPoint( const pcl::PointXYZ & pt, diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 49c75b5f..94b7075b 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -72,6 +72,12 @@ public: const std::string & id, const Transform & pose); //including mesh + bool updateCloud( + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const Transform & pose = Transform::getIdentity(), + const QColor & color = QColor()); + bool updateCloud( const std::string & id, const pcl::PointCloud::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::Ptr & cloud, + const Transform & pose = Transform::getIdentity(), + const QColor & color = QColor()); + bool addOrUpdateCloud( const std::string & id, const pcl::PointCloud::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::Ptr & cloud, - const Transform & pose = Transform::getIdentity(), - const QColor & color = QColor()); + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const Transform & pose = Transform::getIdentity(), + const QColor & color = QColor()); bool addCloud( - const std::string & id, - const pcl::PointCloud::Ptr & cloud, - const Transform & pose = Transform::getIdentity(), - const QColor & color = QColor()); + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const Transform & pose = Transform::getIdentity(), + const QColor & color = QColor()); + + bool addCloud( + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const Transform & pose = Transform::getIdentity(), + const QColor & color = QColor()); bool addCloudMesh( - const std::string & id, - const pcl::PointCloud::Ptr & cloud, - const std::vector & polygons, - const Transform & pose = Transform::getIdentity()); + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const std::vector & 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, diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 0d1f42a2..5292ffbd 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -226,23 +226,19 @@ private: void exportPoses(int format); QString captureScreen(); - pcl::PointCloud::Ptr getAssembledCloud( - const std::map & poses, - float assembledVoxelSize, - bool regenerateClouds, - int regenerateDecimation, - float regenerateVoxelSize, - float regenerateMaxDepth) const; - std::map::Ptr > getClouds( + std::map::Ptr > getClouds( const std::map & poses, bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const; + float regenerateMaxDepth, + int normalKSearch, + bool mls, + float mlsRadius) const; bool getExportedScans(std::map::Ptr > & scans); - bool getExportedClouds(std::map::Ptr> & clouds, std::map & meshes, bool toSave); - void saveClouds(const std::map::Ptr> & clouds, bool binaryMode = true); + bool getExportedClouds(std::map::Ptr> & clouds, std::map & meshes, bool toSave); + void saveClouds(const std::map::Ptr> & clouds, bool binaryMode = true); void saveMeshes(const std::map & meshes, bool binaryMode = true); void saveScans(const std::map::Ptr> & clouds, bool binaryMode = true); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index fa5dcc4f..9d4bcc45 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -156,6 +156,7 @@ public: int getMeshNormalKSearch() const; double getMeshGP3Radius() const; + double getMeshGP3Mu() const; bool getMeshSmoothing() const; double getMeshSmoothingRadius() const; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 5c030b37..eac37919 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -318,6 +318,26 @@ bool CloudViewer::updateCloudPose( return false; } +bool CloudViewer::updateCloud( + const std::string & id, + const pcl::PointCloud::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::Ptr & cloud, @@ -358,6 +378,19 @@ bool CloudViewer::updateCloud( return false; } +bool CloudViewer::addOrUpdateCloud( + const std::string & id, + const pcl::PointCloud::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::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(binaryCloud)); _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id); + } + if(haveNormals) + { + //normals + colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField (binaryCloud, "normal_x")); + _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id); + colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField (binaryCloud, "normal_y")); + _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id); + colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField (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::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::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; } diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index d41992d7..1d5dbb4b 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -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); +} } diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index ad9b0298..046ec3eb 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -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(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index f5e180e0..51a5d16a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -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::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::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::copyPointCloud(*cloudWithNormals, *cloudFiltered); } @@ -4386,7 +4398,7 @@ bool MainWindow::getExportedScans(std::map:: void MainWindow::exportClouds() { - std::map::Ptr> clouds; + std::map::Ptr> clouds; std::map meshes; if(getExportedClouds(clouds, meshes, true)) @@ -4405,7 +4417,7 @@ void MainWindow::exportClouds() void MainWindow::viewClouds() { - std::map::Ptr> clouds; + std::map::Ptr> clouds; std::map meshes; if(getExportedClouds(clouds, meshes, false)) @@ -4449,7 +4461,7 @@ void MainWindow::viewClouds() } else if(clouds.size()) { - for(std::map::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) + for(std::map::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::Ptr> & clouds, + std::map::Ptr> & clouds, std::map & 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::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::Ptr>::iterator iter=clouds.begin(); + int i =0; + pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + for(std::map::Ptr>::iterator iter=clouds.begin(); iter!= clouds.end(); ++iter) { - pcl::PointCloud::Ptr cloudWithNormals; - if(_exportDialog->getMLS()) + pcl::PointCloud::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::Ptr assembledCloudXYZRGB(new pcl::PointCloud); + 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::Ptr viewpoints(new pcl::PointCloud); + viewpoints->resize(poses.size()); + int oi=0; + for(std::map::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::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::Ptr> & clouds, bool binaryMode) +void MainWindow::saveClouds(const std::map::Ptr> & clouds, bool binaryMode) { if(clouds.size() == 1) { @@ -4811,11 +4895,11 @@ void MainWindow::saveClouds(const std::map::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) + for(std::map::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) { if(iter->second->size()) { - pcl::PointCloud::Ptr transformedCloud; + pcl::PointCloud::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::P } } -pcl::PointCloud::Ptr MainWindow::getAssembledCloud( - const std::map & poses, - float assembledVoxelSize, - bool regenerateClouds, - int regenerateDecimation, - float regenerateVoxelSize, - float regenerateMaxDepth) const -{ - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int i=0; - int count = 0; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - bool inserted = false; - if(!iter->second.isNull()) - { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - 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::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::Ptr > MainWindow::getClouds( +std::map::Ptr > MainWindow::getClouds( const std::map & poses, bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const + float regenerateMaxDepth, + int normalKSearch, + bool mls, + float mlsRadius) const { - std::map::Ptr> clouds; + std::map::Ptr> clouds; int i=0; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { @@ -5246,7 +5228,28 @@ std::map::Ptr > MainWindow::getClouds( if(cloud->size()) { - clouds.insert(std::make_pair(iter->first, cloud)); + if(mls) + { + pcl::PointCloud::Ptr cloudWithNormals = util3d::computeNormalsSmoothed( + cloud, + mlsRadius, + true, + regenerateVoxelSize); + + if(regenerateVoxelSize) + { + cloudWithNormals = util3d::voxelize( + cloudWithNormals, + regenerateVoxelSize); + } + + cloud->clear(); + pcl::copyPointCloud(*cloudWithNormals, *cloud); + } + + pcl::PointCloud::Ptr cloudWithNormals = util3d::computeNormals(cloud, _exportDialog->getMeshNormalKSearch()); + + clouds.insert(std::make_pair(iter->first, cloudWithNormals)); inserted = true; } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index ab542b74..54891a57 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -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(); diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 141d1ab8..ebfe1297 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -242,7 +242,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - 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. + Set the number of k nearest neighbors to use for the normal estimation to create the mesh. true @@ -279,6 +279,38 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. + + + + 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). + + + true + + + + + + + 0 + + + 1 + + + 0.100000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 2.500000000000000 + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 8b8f85d7..feeefa96 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 - 755 - 1591 + -394 + 759 + 938 @@ -86,7 +86,7 @@ QFrame::Raised - 4 + 1 @@ -1087,7 +1087,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - 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. + (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. true @@ -1250,6 +1250,41 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + (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). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 1 + + + 0.100000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 2.500000000000000 + + + From a8a3566fed332fa4361e0d4704e5cf09d9f2816f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 12 Aug 2015 10:49:11 -0400 Subject: [PATCH 010/110] DatabaseViewer: fixed ICP refined clouds superposition error --- guilib/src/DatabaseViewer.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 0e48c342..e5621911 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2249,12 +2249,12 @@ void DatabaseViewer::updateConstraintView( if(cloudFrom->size()) { - ui_->constraintsViewer->addOrUpdateCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red); + ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red); } if(cloudTo->size()) { cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); - ui_->constraintsViewer->addOrUpdateCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan); + ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan); } } if(ui_->checkBox_show3DWords->isChecked()) @@ -2297,28 +2297,28 @@ void DatabaseViewer::updateConstraintView( if(cloudFrom->size()) { - ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red); + ui_->constraintsViewer->addOrUpdateCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red); } else { UWARN("Empty 3D words for node %d", link.from()); - ui_->constraintsViewer->removeCloud("cloud0"); + ui_->constraintsViewer->removeCloud("words0"); } if(cloudTo->size()) { - ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan); + ui_->constraintsViewer->addOrUpdateCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan); } else { UWARN("Empty 3D words for node %d", link.to()); - ui_->constraintsViewer->removeCloud("cloud1"); + ui_->constraintsViewer->removeCloud("words1"); } } else { UERROR("Not found signature %d or %d in RAM", link.from(), link.to()); - ui_->constraintsViewer->removeCloud("cloud0"); - ui_->constraintsViewer->removeCloud("cloud1"); + ui_->constraintsViewer->removeCloud("words0"); + ui_->constraintsViewer->removeCloud("words1"); } } } From 31d374e1419070ee1830dfc6bbd6afe7526dec2e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 12 Aug 2015 20:59:21 -0400 Subject: [PATCH 011/110] Added cvsba support for post-processing bundle adjustment (SBA) --- CMakeLists.txt | 7 + cmake_modules/FindFlyCapture2.cmake | 3 - corelib/include/rtabmap/core/Graph.h | 44 ++++- corelib/src/CMakeLists.txt | 12 ++ corelib/src/Graph.cpp | 227 +++++++++++++++++++++++++ guilib/src/AboutDialog.cpp | 7 + guilib/src/MainWindow.cpp | 35 +++- guilib/src/PostProcessingDialog.cpp | 62 ++++++- guilib/src/PostProcessingDialog.h | 8 + guilib/src/ui/aboutDialog.ui | 33 +++- guilib/src/ui/mainWindow.ui | 2 +- guilib/src/ui/postProcessingDialog.ui | 101 ++++++++++- utilite/include/rtabmap/utilite/UStl.h | 20 +++ 13 files changed, 543 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b468954..a10a9e36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ FIND_PACKAGE(OpenNI2) FIND_PACKAGE(DC1394) FIND_PACKAGE(G2O) FIND_PACKAGE(FlyCapture2) +FIND_PACKAGE(cvsba) # If Qt is here, the GUI will be built IF("${RTABMAP_QT_VERSION}" STREQUAL "4") @@ -380,6 +381,12 @@ ELSE() MESSAGE(STATUS " With g2o = NO (g2o not found)") ENDIF() +IF(cvsba_FOUND) +MESSAGE(STATUS " With cvsba = YES") +ELSE() +MESSAGE(STATUS " With cvsba = NO (cvsba not found)") +ENDIF() + IF(QT4_FOUND) MESSAGE(STATUS " With Qt = YES (version 4)") ELSEIF(Qt5_FOUND) diff --git a/cmake_modules/FindFlyCapture2.cmake b/cmake_modules/FindFlyCapture2.cmake index 0bbe8430..d96bc156 100644 --- a/cmake_modules/FindFlyCapture2.cmake +++ b/cmake_modules/FindFlyCapture2.cmake @@ -34,9 +34,6 @@ IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY}) ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY) -MESSAGE(STATUS "FlyCapture2_INCLUDE_DIRS={FlyCapture2_INCLUDE_DIRS}") -MESSAGE(STATUS "FlyCapture2_LIBRARIES={FlyCapture2_LIBRARIES}") - IF (FlyCapture2_FOUND) # show which FlyCapture2 was found only if not quiet IF (NOT FlyCapture2_FIND_QUIETLY) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index dda25071..b74831a8 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { class Memory; @@ -49,7 +50,8 @@ public: enum Type { kTypeUndef = -1, kTypeTORO = 0, - kTypeG2O = 1 + kTypeG2O = 1, + kTypeCVSBA = 2 }; static Optimizer * create(const ParametersMap & parameters); static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap()); @@ -73,11 +75,17 @@ public: bool isCovarianceIgnored() const {return covarianceIgnored_;} double epsilon() const {return epsilon_;} + // inherited classes should implement one of these methods virtual std::map optimize( int rootId, const std::map & poses, const std::multimap & constraints, - std::list > * intermediateGraphes = 0) = 0; + std::list > * intermediateGraphes = 0); + virtual std::map optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures); virtual void parseParameters(const ParametersMap & parameters); @@ -145,6 +153,38 @@ public: std::list > * intermediateGraphes = 0); }; +class CVSBAOptimizer : public Optimizer +{ +public: + static bool available(); + +public: + CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : + Optimizer(iterations, slam2d, covarianceIgnored), + inlierDistance_(0.02), + minInliers_(10){} + CVSBAOptimizer(const ParametersMap & parameters) : + Optimizer(parameters), + inlierDistance_(0.02), + minInliers_(10){} + virtual ~CVSBAOptimizer() {} + + virtual Type type() const {return kTypeCVSBA;} + + void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;} + void setMinInliers(int minInliers) {minInliers_ = minInliers;} + + virtual std::map optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures); + +private: + float inlierDistance_; + float minInliers_; +}; + //////////////////////////////////////////// // Graph utilities //////////////////////////////////////////// diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 6af33c18..22a425d5 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -151,6 +151,18 @@ IF(G2O_FOUND) ) ENDIF(G2O_FOUND) +IF(cvsba_FOUND) + ADD_DEFINITIONS("-DWITH_CVSBA") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${cvsba_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${cvsba_LIBS} + ) +ENDIF(cvsba_FOUND) + #################################### # Generate resources files #################################### diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 34391c39..f3728b8f 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -54,6 +54,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "g2o/types/slam2d/edge_se2.h" #endif +#ifdef WITH_CVSBA +#include +#include "rtabmap/core/util3d_motion_estimation.h" +#include "rtabmap/core/util3d_transforms.h" +#endif + namespace rtabmap { namespace graph { @@ -137,6 +143,26 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_); } +std::map Optimizer::optimize( + int rootId, + const std::map & poses, + const std::multimap & constraints, + std::list > * intermediateGraphes) +{ + UERROR("Optimizer %d doesn't implement optimize() method. See optimizeBA().", (int)this->type()); + return std::map(); +} + +std::map Optimizer::optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures) +{ + UERROR("Optimizer %d doesn't implement optimizeBA() method. See optimize().", (int)this->type()); + return std::map(); +} + void Optimizer::getConnectedGraph( int fromId, const std::map & posesIn, @@ -889,6 +915,207 @@ std::map G2OOptimizer::optimize( return optimizedPoses; } +////////////////////// +// cvsba +////////////////////// +bool CVSBAOptimizer::available() +{ +#ifdef WITH_CVSBA + return true; +#else + return false; +#endif +} + +std::map CVSBAOptimizer::optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures) +{ +#ifdef WITH_CVSBA + // run sba optimization + cvsba::Sba sba; + + // change params if desired + cvsba::Sba::Params params ; + params.type = cvsba::Sba::MOTIONSTRUCTURE; + params.iterations = this->iterations(); + params.minError = 1e-10; + params.fixedIntrinsics = 5; + params.fixedDistortion = 5; + params.verbose=ULogger::level() <= ULogger::kInfo; + sba.setParams(params); + + std::map frames = poses; + + std::vector cameraMatrix(frames.size()); //nframes + std::vector R(frames.size()); //nframes + std::vector T(frames.size()); //nframes + std::vector distCoeffs(frames.size()); //nframes + std::map frameIdToIndex; + std::map models; + int oi=0; + for(std::map::iterator iter=frames.begin(); iter!=frames.end(); ) + { + CameraModel model; + if(uContains(signatures, iter->first)) + { + if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValid()) + { + model = signatures.at(iter->first).sensorData().cameraModels()[0]; + } + else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValid()) + { + model = signatures.at(iter->first).sensorData().stereoCameraModel().left(); + } + else + { + UERROR("Missing calibration for node %d", iter->first); + } + } + else + { + UERROR("Did not find node %d in cache", iter->first); + } + + if(model.isValid()) + { + frameIdToIndex.insert(std::make_pair(iter->first, oi)); + + cameraMatrix[oi] = model.K(); + distCoeffs[oi] = model.D(); + + Transform t = (iter->second * model.localTransform()).inverse(); + + R[oi] = (cv::Mat_(3,3) << + (double)t.r11(), (double)t.r12(), (double)t.r13(), + (double)t.r21(), (double)t.r22(), (double)t.r23(), + (double)t.r31(), (double)t.r32(), (double)t.r33()); + T[oi] = (cv::Mat_(1,3) << (double)t.x(), (double)t.y(), (double)t.z()); + ++oi; + + models.insert(std::make_pair(iter->first, model)); + + UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str()); + + ++iter; + } + else + { + frames.erase(iter++); + } + } + cameraMatrix.resize(oi); + R.resize(oi); + T.resize(oi); + distCoeffs.resize(oi); + + std::map points3DMap; + std::multimap > wordReferences; // + int genWordId = 1; + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(uContains(signatures, iter->second.from()) && + uContains(signatures, iter->second.to()) && + uContains(frames, iter->second.from())) + { + const Signature & sFrom = signatures.at(iter->second.from()); + const Signature & sTo = signatures.at(iter->second.to()); + + std::vector inliers; + Transform t = util3d::estimateMotion3DTo3D( + uMultimapToMapUnique(sFrom.getWords3()), + uMultimapToMapUnique(sTo.getWords3()), + 10, + inlierDistance_, + 100, + 10, + 0, + 0, + &inliers); + if(!t.isNull()) + { + Transform pose = frames.at(sFrom.id()); + for(unsigned int i=0; isecond, pose); + points3DMap.insert(std::make_pair(genWordId, p)); + wordReferences.insert(std::make_pair(genWordId, std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); + wordReferences.insert(std::make_pair(genWordId, std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); + ++genWordId; + } + } + else + { + UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id()); + } + } + } + + std::list wordReferencesKeys = uUniqueKeys(wordReferences); + UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size()); + std::vector points(wordReferencesKeys.size()); //npoints + std::vector > imagePoints(frames.size()); //nframes -> npoints + std::vector > visibility(frames.size()); //nframes -> npoints + for(unsigned int i=0; i::quiet_NaN(), std::numeric_limits::quiet_NaN())); + visibility[i].resize(wordReferencesKeys.size(), 0); + } + int i=0; + for(std::list::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter) + { + pcl::PointXYZ & p = points3DMap.at(*iter); + points[i].x = p.x; + points[i].y = p.y; + points[i].z = p.z; + + std::multimap >::iterator jter = wordReferences.lower_bound(*iter); + while(jter->first == *iter && jter != wordReferences.end()) + { + imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second; + visibility[frameIdToIndex.at(jter->second.first)][i] = 1; + ++jter; + } + + ++i; + } + + // SBA + try + { + sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs); + } + catch(cv::Exception & e) + { + UERROR("Running SBA... error! %s", e.what()); + return std::map(); + } + + //update poses + i=0; + for(std::map::iterator iter=frames.begin(); iter!=frames.end(); ++iter) + { + Transform t(R[i].at(0,0), R[i].at(0,1), R[i].at(0,2), T[i].at(0), + R[i].at(1,0), R[i].at(1,1), R[i].at(1,2), T[i].at(1), + R[i].at(2,0), R[i].at(2,1), R[i].at(2,2), T[i].at(2)); + + UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str()); + + iter->second = (models.at(iter->first).localTransform() * t).inverse(); + + ++i; + } + + return frames; + +#else + UERROR("RTAB-Map is not built with cvsba!"); + return std::map(); +#endif +} + //////////////////////////////////////////// // Graph utilities //////////////////////////////////////////// diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 65e97fde..53149d62 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -61,6 +61,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No"); + +#ifdef WITH_CVSBA + _ui->label_cvsba->setText("Yes"); +#else + _ui->label_cvsba->setText("No"); +#endif + } AboutDialog::~AboutDialog() diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 51a5d16a..978758cf 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3250,8 +3250,12 @@ void MainWindow::postProcessing() double clusterRadius = _postProcessingDialog->clusterRadius(); double clusterAngle = _postProcessingDialog->clusterAngle(); int detectLoopClosureIterations = _postProcessingDialog->iterations(); + bool sba = _postProcessingDialog->isSBA(); + int sbaIterations = _postProcessingDialog->sbaIterations(); + double sbaInlierDistance = _postProcessingDialog->sbaInlierDistance(); + int sbaMinInliers = _postProcessingDialog->sbaMinInliers(); - if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks) + if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba) { UWARN("No post-processing selection..."); return; @@ -3331,6 +3335,10 @@ void MainWindow::postProcessing() { totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size(); } + if(sba) + { + totalSteps+=1; + } _initProgressDialog->setMaximumSteps(totalSteps); _initProgressDialog->show(); @@ -3676,6 +3684,31 @@ void MainWindow::postProcessing() _initProgressDialog->appendText(tr("Optimizing graph with updated links... done!")); _initProgressDialog->incrementStep(); + if(sba) + { + _initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints)...") + .arg(optimizedPoses.size()).arg(linksOut.size())); + QApplication::processEvents(); + QApplication::processEvents(); + + ParametersMap parametersSBA = _preferencesDialog->getAllParameters(); + uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations))); + graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parameters); + cvsba.setInlierDistance(sbaInlierDistance); + cvsba.setMinInliers(sbaMinInliers); + std::map newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap()); + if(newPoses.size()) + { + optimizedPoses = newPoses; + _initProgressDialog->appendText(tr("SBA... done!")); + } + else + { + _initProgressDialog->appendText(tr("SBA... failed!")); + } + _initProgressDialog->incrementStep(); + } + _initProgressDialog->appendText(tr("Updating map...")); this->updateMapCloud( optimizedPoses, Transform(), diff --git a/guilib/src/PostProcessingDialog.cpp b/guilib/src/PostProcessingDialog.cpp index d6e62f90..2967b593 100644 --- a/guilib/src/PostProcessingDialog.cpp +++ b/guilib/src/PostProcessingDialog.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ui_postProcessingDialog.h" #include +#include namespace rtabmap { @@ -38,6 +39,13 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) : _ui = new Ui_PostProcessingDialog(); _ui->setupUi(this); + if(!graph::CVSBAOptimizer::available()) + { + _ui->sba->setEnabled(false); + } + + restoreDefaults(); + connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox())); connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox())); connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox())); @@ -50,6 +58,11 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) : connect(_ui->reextractFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + + connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->sba_minInlierDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); } PostProcessingDialog::~PostProcessingDialog() @@ -70,6 +83,10 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr settings.setValue("reextract_features", this->isReextractFeatures()); settings.setValue("refine_neigbors", this->isRefineNeighborLinks()); settings.setValue("refine_lc", this->isRefineLoopClosureLinks()); + settings.setValue("sba", this->isSBA()); + settings.setValue("sba_iterations", this->sbaIterations()); + settings.setValue("sba_inlier_distance", this->sbaInlierDistance()); + settings.setValue("sba_min_inliers", this->sbaMinInliers()); if(!group.isEmpty()) { settings.endGroup(); @@ -89,6 +106,10 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr this->setReextractFeatures(settings.value("reextract_features", this->isReextractFeatures()).toBool()); this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool()); this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool()); + this->setSBA(settings.value("sba", this->isSBA()).toBool()); + this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt()); + this->setSBAInlierDistance(settings.value("sba_inlier_distance", this->sbaInlierDistance()).toDouble()); + this->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt()); if(!group.isEmpty()) { settings.endGroup(); @@ -104,12 +125,16 @@ void PostProcessingDialog::restoreDefaults() setReextractFeatures(false); setRefineNeighborLinks(false); setRefineLoopClosureLinks(false); + setSBA(false); + setSBAIterations(150); + setSBAInlierDistance(0.02); + setSBAMinInliers(10); } void PostProcessingDialog::updateButtonBox() { _ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled( - isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks()); + isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks() || isSBA()); } bool PostProcessingDialog::isDetectMoreLoopClosures() const @@ -147,6 +172,24 @@ bool PostProcessingDialog::isRefineLoopClosureLinks() const return _ui->refineLoopClosureLinks->isChecked(); } +bool PostProcessingDialog::isSBA() const +{ + return _ui->sba->isChecked(); +} + +int PostProcessingDialog::sbaIterations() const +{ + return _ui->sba_iterations->value(); +} +double PostProcessingDialog::sbaInlierDistance() const +{ + return _ui->sba_minInlierDistance->value(); +} +int PostProcessingDialog::sbaMinInliers() const +{ + return _ui->sba_minInliers->value(); +} + //setters void PostProcessingDialog::setDetectMoreLoopClosures(bool on) { @@ -176,5 +219,22 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on) { _ui->refineLoopClosureLinks->setChecked(on); } +void PostProcessingDialog::setSBA(bool on) +{ + _ui->sba->setChecked(graph::CVSBAOptimizer::available() && on); +} +void PostProcessingDialog::setSBAIterations(int iterations) +{ + _ui->sba_iterations->setValue(iterations); +} +void PostProcessingDialog::setSBAInlierDistance(double inlierDistance) +{ + _ui->sba_minInlierDistance->setValue(inlierDistance); +} +void PostProcessingDialog::setSBAMinInliers(int minInliers) +{ + _ui->sba_minInliers->setValue(minInliers); +} + } diff --git a/guilib/src/PostProcessingDialog.h b/guilib/src/PostProcessingDialog.h index 096b272b..2c20b79b 100644 --- a/guilib/src/PostProcessingDialog.h +++ b/guilib/src/PostProcessingDialog.h @@ -56,6 +56,10 @@ public: bool isReextractFeatures() const; bool isRefineNeighborLinks() const; bool isRefineLoopClosureLinks() const; + bool isSBA() const; + int sbaIterations() const; + double sbaInlierDistance() const; + int sbaMinInliers() const; //setters void setDetectMoreLoopClosures(bool on); @@ -65,6 +69,10 @@ public: void setReextractFeatures(bool on); void setRefineNeighborLinks(bool on); void setRefineLoopClosureLinks(bool on); + void setSBA(bool on); + void setSBAIterations(int iterations); + void setSBAInlierDistance(double inlierDistance); + void setSBAMinInliers(int minInliers); signals: void configChanged(); diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 373bf653..7fe1f390 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -7,7 +7,7 @@ 0 0 824 - 507 + 581 @@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; } + + + + With OpenNI2 : + + + @@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; } - - - - With OpenNI2 : - - - @@ -293,6 +293,23 @@ p, li { white-space: pre-wrap; } + + + + With cvsba : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index f6ec2d8f..8e93e2fb 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 22 + 25 diff --git a/guilib/src/ui/postProcessingDialog.ui b/guilib/src/ui/postProcessingDialog.ui index b67a731c..d512d9eb 100644 --- a/guilib/src/ui/postProcessingDialog.ui +++ b/guilib/src/ui/postProcessingDialog.ui @@ -6,8 +6,8 @@ 0 0 - 486 - 425 + 552 + 540 @@ -187,6 +187,103 @@ + + + + Sparse Bundle Adjustment (SBA) + + + true + + + false + + + + + + + + Iterations + + + true + + + + + + + 1 + + + 1000 + + + 100 + + + + + + + Inlier distance of corresponding 3D words + + + true + + + + + + + m + + + 3 + + + 0.001000000000000 + + + 10.000000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + Minimum inliers + + + true + + + + + + + 1 + + + 1000 + + + 10 + + + + + + + + diff --git a/utilite/include/rtabmap/utilite/UStl.h b/utilite/include/rtabmap/utilite/UStl.h index 91e7b232..a498fcea 100644 --- a/utilite/include/rtabmap/utilite/UStl.h +++ b/utilite/include/rtabmap/utilite/UStl.h @@ -472,6 +472,7 @@ inline std::list uVectorToList(const std::vector & v) /** * Convert a std::multimap to a std::map + * @see uMultimapToMapUnique to keep only unique keys */ template inline std::map uMultimapToMap(const std::multimap & m) @@ -479,6 +480,25 @@ inline std::map uMultimapToMap(const std::multimap & m) return std::map(m.begin(), m.end()); } +/** + * Convert a std::multimap to a std::map, keeping only unique keys! + */ +template +inline std::map uMultimapToMapUnique(const std::multimap & m) +{ + std::map mapOut; + std::list uniqueKeys = uUniqueKeys(m); + for(typename std::list::const_iterator iter = uniqueKeys.begin(); iter!=uniqueKeys.end(); ++iter) + { + if(m.count(*iter) == 1) + { + typename std::multimap::const_iterator jter=m.find(*iter); + mapOut.insert(std::pair(jter->first, jter->second)); + } + } + return mapOut; +} + /** * Append a list to another list. * @param list the list on which the other list will be appended From 15e75d17cc8bc28557ef271f4e86f74fe964a0b0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 13 Aug 2015 11:31:19 -0400 Subject: [PATCH 012/110] SBA: improved number of duplicated word correspondences (under inlier distance error) --- corelib/src/Graph.cpp | 37 +++++++++++++++++++++++++++---------- guilib/src/MainWindow.cpp | 7 ++++--- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index f3728b8f..762afa15 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1013,15 +1013,19 @@ std::map CVSBAOptimizer::optimizeBA( std::map points3DMap; std::multimap > wordReferences; // - int genWordId = 1; for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - if(uContains(signatures, iter->second.from()) && - uContains(signatures, iter->second.to()) && - uContains(frames, iter->second.from())) + Link link = iter->second; + if(link.to() < link.from()) { - const Signature & sFrom = signatures.at(iter->second.from()); - const Signature & sTo = signatures.at(iter->second.to()); + link = link.inverse(); + } + if(uContains(signatures, link.from()) && + uContains(signatures, link.to()) && + uContains(frames, link.from())) + { + const Signature & sFrom = signatures.at(link.from()); + const Signature & sTo = signatures.at(link.to()); std::vector inliers; Transform t = util3d::estimateMotion3DTo3D( @@ -1040,10 +1044,23 @@ std::map CVSBAOptimizer::optimizeBA( for(unsigned int i=0; isecond, pose); - points3DMap.insert(std::make_pair(genWordId, p)); - wordReferences.insert(std::make_pair(genWordId, std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); - wordReferences.insert(std::make_pair(genWordId, std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); - ++genWordId; + std::map::iterator jter = points3DMap.find(inliers[i]); + if(jter == points3DMap.end()) + { + points3DMap.insert(std::make_pair(inliers[i], p)); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); + } + else + { + float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z); + if(dist <= inlierDistance_) + { + // in case of loop closure links + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); + } + } } } else diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 978758cf..a50bb1f5 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3686,14 +3686,15 @@ void MainWindow::postProcessing() if(sba) { - _initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints)...") - .arg(optimizedPoses.size()).arg(linksOut.size())); + _initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...") + .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations)); QApplication::processEvents(); + uSleep(100); QApplication::processEvents(); ParametersMap parametersSBA = _preferencesDialog->getAllParameters(); uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations))); - graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parameters); + graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parametersSBA); cvsba.setInlierDistance(sbaInlierDistance); cvsba.setMinInliers(sbaMinInliers); std::map newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap()); From 633019c0177f17478f04a7b7150990652c582e68 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 13 Aug 2015 15:12:43 -0400 Subject: [PATCH 013/110] CMake: added build options WITH_*** for all optional dependencies --- CMakeLists.txt | 79 +++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 62 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b468954..10cd4696 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,26 +121,57 @@ IF(APPLE) ENDIF(APPLE) ####### DEPENDENCIES ####### + +option(WITH_QT "Include Qt support" ON) +option(WITH_FREENECT "Include Freenect support" ON) +option(WITH_FREENECT2 "Include Freenect2 support" ON) +option(WITH_OPENNI2 "Include OpenNI2 support" ON) +option(WITH_DC1394 "Include dc1394 support" ON) +option(WITH_G2O "Include g2o support" ON) +option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) + FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(PCL 1.7 REQUIRED) -FIND_PACKAGE(VTK REQUIRED) -IF("${VTK_MAJOR_VERSION}" EQUAL 5) - FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 -ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5) FIND_PACKAGE(ZLIB REQUIRED) -FIND_PACKAGE(Freenect) -FIND_PACKAGE(freenect2 QUIET) -FIND_PACKAGE(OpenNI2) -FIND_PACKAGE(DC1394) -FIND_PACKAGE(G2O) -FIND_PACKAGE(FlyCapture2) +IF(WITH_QT) # If Qt is here, the GUI will be built -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) -ELSE() - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) -ENDIF() + IF("${RTABMAP_QT_VERSION}" STREQUAL "4") + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) + ELSE() + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) + ENDIF() + IF(QT4_FOUND OR Qt5_FOUND) + FIND_PACKAGE(VTK REQUIRED) + IF("${VTK_MAJOR_VERSION}" EQUAL 5) + FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 + ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5) + ENDIF(QT4_FOUND OR Qt5_FOUND) +ENDIF(WITH_QT) + +IF(WITH_FREENECT) + FIND_PACKAGE(Freenect) +ENDIF(WITH_FREENECT) + +IF(WITH_FREENECT2) + FIND_PACKAGE(freenect2 QUIET) +ENDIF(WITH_FREENECT2) + +IF(WITH_OPENNI2) + FIND_PACKAGE(OpenNI2) +ENDIF(WITH_OPENNI2) + +IF(WITH_DC1394) + FIND_PACKAGE(DC1394) +ENDIF(WITH_DC1394) + +IF(WITH_G2O) + FIND_PACKAGE(G2O) +ENDIF(WITH_G2O) + +IF(WITH_FLYCAPTURE2) + FIND_PACKAGE(FlyCapture2) +ENDIF(WITH_FLYCAPTURE2) ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) @@ -183,8 +214,8 @@ ADD_SUBDIRECTORY( corelib ) IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) ADD_SUBDIRECTORY( guilib ) ADD_SUBDIRECTORY( app ) -ELSE() - MESSAGE(STATUS "[WARNING] Qt not found, the GUI lib and the stand-alone application will not be compiled...") +ELSEIF(WITH_QT) + MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...") ENDIF() ADD_SUBDIRECTORY( tools ) @@ -346,36 +377,48 @@ ENDIF(OpenCV_FOUND) IF(Freenect_FOUND) MESSAGE(STATUS " With Freenect = YES") +ELSEIF(NOT WITH_FREENECT) +MESSAGE(STATUS " With Freenect = NO (WITH_FREENECT=OFF)") ELSE() MESSAGE(STATUS " With Freenect = NO (libfreenect not found)") ENDIF() IF(OpenNI2_FOUND) MESSAGE(STATUS " With OpenNI2 = YES") +ELSEIF(NOT WITH_OPENNI2) +MESSAGE(STATUS " With OpenNI2 = NO (WITH_OPENNI2=OFF)") ELSE() MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)") ENDIF() IF(freenect2_FOUND) MESSAGE(STATUS " With Freenect2 = YES") +ELSEIF(NOT WITH_FREENECT2) +MESSAGE(STATUS " With Freenect2 = NO (WITH_FREENECT2=OFF)") ELSE() MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") ENDIF() IF(DC1394_FOUND) MESSAGE(STATUS " With dc1394 = YES") +ELSEIF(NOT WITH_DC1394) +MESSAGE(STATUS " With dc1394 = NO (WITH_DC1394=OFF)") ELSE() MESSAGE(STATUS " With dc1394 = NO (dc1394 not found)") ENDIF() IF(FlyCapture2_FOUND) MESSAGE(STATUS " With FlyCapture2/Triclops = YES") +ELSEIF(NOT WITH_FLYCAPTURE2) +MESSAGE(STATUS " With FlyCapture2/Triclops = NO (WITH_FLYCAPTURE2=OFF)") ELSE() MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)") ENDIF() IF(G2O_FOUND) MESSAGE(STATUS " With g2o = YES") +ELSEIF(NOT WITH_G2O) +MESSAGE(STATUS " With g2o = NO (WITH_G2O=OFF)") ELSE() MESSAGE(STATUS " With g2o = NO (g2o not found)") ENDIF() @@ -384,6 +427,8 @@ IF(QT4_FOUND) MESSAGE(STATUS " With Qt = YES (version 4)") ELSEIF(Qt5_FOUND) MESSAGE(STATUS " With Qt = YES (version 5)") +ELSEIF(NOT WITH_QT) +MESSAGE(STATUS " With Qt = NO (WITH_QT=OFF)") ELSE() MESSAGE(STATUS " With Qt = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)") ENDIF() From 508c30205333afc92c88bb46195c8f21d025ec11 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 13 Aug 2015 15:34:59 -0400 Subject: [PATCH 014/110] Updated RTABMapConfig.cmake with split Debug/Release libraries lookup --- CMakeLists.txt | 1 + RTABMapConfig.cmake.in | 68 +++++++++++++++++++++++++++++++++++++++--- 2 files changed, 65 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 10cd4696..f00821ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -244,6 +244,7 @@ set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" "${PROJECT_SOURCE_DIR}/guilib/include" "${PROJECT_SOURCE_DIR}/utilite/include") set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}") +set(CONF_WITH_GUI ${WITH_QT}) configure_file(RTABMapConfig.cmake.in "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY) diff --git a/RTABMapConfig.cmake.in b/RTABMapConfig.cmake.in index 90a138f2..18284c90 100644 --- a/RTABMapConfig.cmake.in +++ b/RTABMapConfig.cmake.in @@ -2,12 +2,72 @@ # It defines the following variables # RTABMap_INCLUDE_DIRS - include directories for RTABMap # RTABMap_LIBRARIES - libraries to link against +# RTABMap_CORE - core library +# RTABMap_UTILITE - utilite library +# RTABMap_GUI - gui library (set if RTABMap is built with Qt) # Compute paths get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") -find_library(RTABMAP_CORE NAMES rtabmap_core rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMAP_GUI NAMES rtabmap_gui rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMAP_UTILITE NAMES rtabmap_utilite rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -set(RTABMap_LIBRARIES ${RTABMAP_CORE} ${RTABMAP_GUI} ${RTABMAP_UTILITE}) \ No newline at end of file +#core lib +find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + +IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE) + SET(RTABMap_CORE + debug ${RTABMap_CORE_DEBUG} + optimized ${RTABMap_CORE_RELEASE} + ) +ELSEIF(RTABMap_CORE_RELEASE) + SET(RTABMap_CORE ${RTABMap_CORE_RELEASE}) +ELSEIF(RTABMap_CORE_DEBUG) + SET(RTABMap_CORE ${RTABMap_CORE_DEBUG}) +ENDIF() + +#utilite lib +find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + +IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE) + SET(RTABMap_UTILITE + debug ${RTABMap_UTILITE_DEBUG} + optimized ${RTABMap_UTILITE_RELEASE} + ) +ELSEIF(RTABMap_UTILITE_RELEASE) + SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE}) +ELSEIF(RTABMap_UTILITE_DEBUG) + SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG}) +ENDIF() + +set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE}) + +#gui lib (OFF if RTAB-Map is not built with Qt) +if(@CONF_WITH_GUI@) + find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + + IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE) + SET(RTABMap_GUI + debug ${RTABMap_GUI_DEBUG} + optimized ${RTABMap_GUI_RELEASE} + ) + ELSEIF(RTABMap_GUI_RELEASE) + SET(RTABMap_GUI ${RTABMap_GUI_RELEASE}) + ELSEIF(RTABMap_GUI_DEBUG) + SET(RTABMap_GUI ${RTABMap_GUI_DEBUG}) + ENDIF() + + set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI}) +endif(@CONF_WITH_GUI@) + +#backward compatibilities +if(RTABMap_CORE) + set(RTABMAP_CORE ${RTABMap_CORE}) +endif(RTABMap_CORE) +if(RTABMap_UTILITE) + set(RTABMAP_UTILITE ${RTABMap_UTILITE}) +endif(RTABMap_UTILITE) +if(RTABMap_GUI) + set(RTABMAP_GUI ${RTABMap_GUI}) +endif(RTABMap_GUI) \ No newline at end of file From 25822bca30dd0a224ac1320f9e2c481b6fc975bf Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Aug 2015 13:08:04 -0400 Subject: [PATCH 015/110] Fixed About dialog for cvsba --- guilib/src/AboutDialog.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 53149d62..a3ccf431 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -61,12 +61,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No"); - -#ifdef WITH_CVSBA - _ui->label_cvsba->setText("Yes"); -#else - _ui->label_cvsba->setText("No"); -#endif + _ui->label_cvsba->setText(graph::CVSBAOptimizer::available()?"Yes":"No"); } From ea8a188661a495896abfda631238a4b84b35324c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Aug 2015 13:19:36 -0400 Subject: [PATCH 016/110] Activated Auto-close progress dialog on post-processing --- guilib/src/MainWindow.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 62bbd4f8..16f71efe 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3321,7 +3321,7 @@ void MainWindow::postProcessing() return; } - _initProgressDialog->setAutoClose(false, 1); + _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->clear(); _initProgressDialog->show(); @@ -3708,6 +3708,7 @@ void MainWindow::postProcessing() else { _initProgressDialog->appendText(tr("SBA... failed!")); + _initProgressDialog->setAutoClose(false, 1); } _initProgressDialog->incrementStep(); } From cdfa23f97dc74fa0c27c0e1a0f7b6a0a333f523d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Sun, 16 Aug 2015 09:26:46 -0400 Subject: [PATCH 017/110] fixed CVSBAOptimizer link error on Windows --- corelib/include/rtabmap/core/Graph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index b74831a8..0c92fb08 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -153,7 +153,7 @@ public: std::list > * intermediateGraphes = 0); }; -class CVSBAOptimizer : public Optimizer +class RTABMAP_EXP CVSBAOptimizer : public Optimizer { public: static bool available(); From b2cb29adcad47b2f85635480b99212cca143aa41 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 17 Aug 2015 13:33:09 -0400 Subject: [PATCH 018/110] Set default GFTT/QualityLevel to 0.001 --- corelib/include/rtabmap/core/Parameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index a23358dd..7dd3005b 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -56,7 +56,7 @@ typedef std::pair ParametersPair; * DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));} * }; * DummyVideoImageWidth dummyVideoImageWidth; - * @endcode + * @endcode */ #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \ public: \ @@ -156,7 +156,7 @@ typedef std::pair ParametersPair; * std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640" * @endcode * @see getDefaultParameters() - * TODO Add a detailed example with simple classes + * TODO Add a detailed example with simple classes */ class RTABMAP_EXP Parameters { @@ -248,7 +248,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected."); RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU."); - RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, ""); + RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, ""); RTABMAP_PARAM(GFTT, MinDistance, double, 5, ""); RTABMAP_PARAM(GFTT, BlockSize, int, 3, ""); RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, ""); From 7fe064ca8214f9115573bcd9e0823da4d985b47f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 17 Aug 2015 18:30:16 -0400 Subject: [PATCH 019/110] added a fixme --- corelib/src/RtabmapThread.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 84d4a0bb..7b34d67e 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -510,6 +510,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) lastPose_ = odomEvent.pose(); double maxRotVar = odomEvent.rotVariance(); double maxTransVar = odomEvent.transVariance(); + // FIXME: should merge the transformations/variances like Link::merge(); if(maxRotVar > _rotVariance) { _rotVariance = maxRotVar; From 698ae0eedab093163fbf5bad8164756222572f3e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 11:59:26 -0400 Subject: [PATCH 020/110] GUI: Added export mesh with textures, refactored Export dialog (more MLS options) --- corelib/include/rtabmap/core/CameraModel.h | 2 +- corelib/include/rtabmap/core/util3d_surface.h | 26 +- .../include/rtabmap/core/util3d_transforms.h | 3 + corelib/src/util3d_surface.cpp | 248 ++++- corelib/src/util3d_transforms.cpp | 8 + guilib/include/rtabmap/gui/CloudViewer.h | 6 + guilib/include/rtabmap/gui/MainWindow.h | 15 +- guilib/src/CloudViewer.cpp | 18 + guilib/src/DetailedProgressDialog.cpp | 8 +- guilib/src/DetailedProgressDialog.h | 2 +- guilib/src/ExportCloudsDialog.cpp | 249 +++-- guilib/src/ExportCloudsDialog.h | 38 +- guilib/src/MainWindow.cpp | 463 ++++++--- guilib/src/ui/exportCloudsDialog.ui | 983 ++++++++++++------ guilib/src/ui/mainWindow.ui | 4 +- 15 files changed, 1471 insertions(+), 602 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 0db92a2f..fc89db14 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -98,7 +98,7 @@ public: const cv::Size & imageSize() const {return imageSize_;} int imageWidth() const {return imageSize_.width;} - int imageWeight() const {return imageSize_.height;} + int imageHeight() const {return imageSize_.height;} bool load(const std::string & directory, const std::string & cameraName); bool save(const std::string & directory) const; diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 1576ccc1..89bb593d 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -33,6 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include +#include namespace rtabmap { @@ -50,6 +53,13 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh( float gp3MaximumAngle = 2*M_PI/3, bool gp3NormalConsistency = true); +pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( + const pcl::PolygonMesh::Ptr & mesh, + const std::map & poses, + const std::map & cameraModels, + const std::map & images, + const std::string & tmpDirectory = "."); + pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, int normalKSearch = 20); @@ -58,15 +68,21 @@ pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, int normalKSearch = 20); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormalsSmoothed( +pcl::PointCloud::Ptr RTABMAP_EXP mls( const pcl::PointCloud::Ptr & cloud, - float smoothingSearchRadius = 0.025, - bool smoothingPolynomialFit = true, - float voxelSize = 0.0f); + float searchRadius = 0.0f, + int polygonialOrder = 2, + int upsamplingMethod = 0, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION + float upsamplingRadius = 0.0f, // SAMPLE_LOCAL_PLANE + float upsamplingStep = 0.0f, // SAMPLE_LOCAL_PLANE + int pointDensity = 0, // RANDOM_UNIFORM_DENSITY + float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION + int dilationIterations = 0); // VOXEL_GRID_DILATION void RTABMAP_EXP adjustNormalsToViewPoints( const pcl::PointCloud::Ptr & viewpoints, - pcl::PointCloud & cloud); + pcl::PointCloud::Ptr & cloud, + int k = 0); // optional: recompute normal with k neighbors (min k=3) template std::vector normalizePolygonsSide( diff --git a/corelib/include/rtabmap/core/util3d_transforms.h b/corelib/include/rtabmap/core/util3d_transforms.h index 51658e84..ab50dc7a 100644 --- a/corelib/include/rtabmap/core/util3d_transforms.h +++ b/corelib/include/rtabmap/core/util3d_transforms.h @@ -46,6 +46,9 @@ pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); +pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform); pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 38578f7d..65a620a5 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -28,10 +28,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UConversion.h" #include #include #include +#include #include +#include + +#include namespace rtabmap { @@ -82,6 +88,147 @@ pcl::PolygonMesh::Ptr createMesh( return mesh; } +pcl::texture_mapping::CameraVector createTextureCameras( + const std::map & poses, + const std::map & cameraModels, + const std::map & images, + const std::string & tmpDirectory) +{ + UASSERT(poses.size() == cameraModels.size() && poses.size() == images.size()); + UASSERT(UDirectory::exists(tmpDirectory)); + pcl::texture_mapping::CameraVector cameras(poses.size()); + std::map::const_iterator poseIter=poses.begin(); + std::map::const_iterator modelIter=cameraModels.begin(); + std::map::const_iterator imageIter=images.begin(); + int oi=0; + for(; poseIter!=poses.end(); ++poseIter, ++modelIter, ++imageIter) + { + UASSERT(poseIter->first == modelIter->first); + UASSERT(poseIter->first == imageIter->first); + pcl::TextureMapping::Camera cam; + + // transform into optical referential + Transform rotation(0,-1,0,0, + 0,0,-1,0, + 1,0,0,0); + + Transform t = poseIter->second*rotation.inverse(); + + cam.pose = t.toEigen3f(); + + UASSERT(modelIter->second.fx()>0 && imageIter->second.rows>0 && imageIter->second.cols>0); + cam.focal_length=modelIter->second.fx(); + cam.height=imageIter->second.rows; + cam.width=imageIter->second.cols; + + + std::string fileName = uFormat("%s/%s%d.png", tmpDirectory.c_str(), "texture_", poseIter->first); + if(!cv::imwrite(fileName, imageIter->second)) + { + UERROR("Cannot save texture of image %d", poseIter->first); + } + else + { + UINFO("Saved temporary texture: \"%s\"", fileName.c_str()); + } + cam.texture_file = fileName; + cameras[oi++] = cam; + } + return cameras; +} + +pcl::TextureMesh::Ptr createTextureMesh( + const pcl::PolygonMesh::Ptr & mesh, + const std::map & poses, + const std::map & cameraModels, + const std::map & images, + const std::string & tmpDirectory) +{ + // Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp: + // Author: Raphael Favier, Technical University Eindhoven, (r.mysurname tue.nl) + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *cloud); + + // Create the texturemesh object that will contain our UV-mapped mesh + pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); + textureMesh->cloud = mesh->cloud; + std::vector< pcl::Vertices> polygons; + + // push faces into the texturemesh object + polygons.resize (mesh->polygons.size ()); + for(size_t i =0; i < mesh->polygons.size (); ++i) + { + polygons[i] = mesh->polygons[i]; + } + textureMesh->tex_polygons.push_back(polygons); + + // create cameras + pcl::texture_mapping::CameraVector cameras = createTextureCameras( + poses, + cameraModels, + images, + tmpDirectory); + + // Create materials for each texture (and one extra for occluded faces) + textureMesh->tex_materials.resize (cameras.size () + 1); + for(int i = 0 ; i <= cameras.size() ; ++i) + { + pcl::TexMaterial mesh_material; + mesh_material.tex_Ka.r = 0.2f; + mesh_material.tex_Ka.g = 0.2f; + mesh_material.tex_Ka.b = 0.2f; + + mesh_material.tex_Kd.r = 0.8f; + mesh_material.tex_Kd.g = 0.8f; + mesh_material.tex_Kd.b = 0.8f; + + mesh_material.tex_Ks.r = 1.0f; + mesh_material.tex_Ks.g = 1.0f; + mesh_material.tex_Ks.b = 1.0f; + + mesh_material.tex_d = 1.0f; + mesh_material.tex_Ns = 75.0f; + mesh_material.tex_illum = 2; + + std::stringstream tex_name; + tex_name << "material_" << i; + tex_name >> mesh_material.tex_name; + + if(i < cameras.size ()) + { + mesh_material.tex_file = cameras[i].texture_file; + } + else + { + mesh_material.tex_file = tmpDirectory+UDirectory::separator()+"occluded.png"; + cv::Mat emptyImage; + if(i>0) + { + emptyImage = cv::Mat::zeros(cameras[i-1].height,cameras[i-1].width, CV_8UC1); + } + else + { + emptyImage = cv::Mat::zeros(480, 640, CV_8UC1); + } + cv::imwrite(mesh_material.tex_file, emptyImage); + } + + textureMesh->tex_materials[i] = mesh_material; + } + + // Sort faces + pcl::TextureMapping tm; // TextureMapping object that will perform the sort + tm.textureMeshwithMultipleCameras(*textureMesh, cameras); + + // compute normals for the mesh + pcl::PointCloud::Ptr cloudWithNormals = computeNormals(cloud, 20); + + pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud); + + return textureMesh; +} + pcl::PointCloud::Ptr computeNormals( const pcl::PointCloud::Ptr & cloud, int normalKSearch) @@ -130,33 +277,48 @@ pcl::PointCloud::Ptr computeNormals( return cloud_with_normals; } -pcl::PointCloud::Ptr computeNormalsSmoothed( +pcl::PointCloud::Ptr mls( const pcl::PointCloud::Ptr & cloud, - float smoothingSearchRadius, - bool smoothingPolynomialFit, - float voxelSize) + float searchRadius, + int polygonialOrder, + int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION + float upsamplingRadius, // SAMPLE_LOCAL_PLANE + float upsamplingStep, // SAMPLE_LOCAL_PLANE + int pointDensity, // RANDOM_UNIFORM_DENSITY + float dilationVoxelSize, // VOXEL_GRID_DILATION + int dilationIterations) // VOXEL_GRID_DILATION { pcl::PointCloud::Ptr cloud_with_normals(new pcl::PointCloud); pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (cloud); - // Init object (second point type is for the normals, even if unused) + // Init object (second point type is for the normals) pcl::MovingLeastSquares mls; - mls.setComputeNormals (true); - // Set parameters - mls.setInputCloud (cloud); - mls.setPolynomialFit (smoothingPolynomialFit); - mls.setSearchMethod (tree); - mls.setSearchRadius (smoothingSearchRadius); - if(voxelSize > 0.0f) + mls.setComputeNormals (true); + if(polygonialOrder > 0) { - mls.setUpsamplingMethod(pcl::MovingLeastSquares::VOXEL_GRID_DILATION); - mls.setDilationVoxelSize(voxelSize); + mls.setPolynomialFit (true); + mls.setPolynomialOrder(polygonialOrder); } + else + { + mls.setPolynomialFit (false); + } + UASSERT(upsamplingMethod >= mls.NONE && + upsamplingMethod <= mls.VOXEL_GRID_DILATION); + mls.setUpsamplingMethod((pcl::MovingLeastSquares::UpsamplingMethod)upsamplingMethod); + mls.setSearchRadius(searchRadius); + mls.setUpsamplingRadius(upsamplingRadius); + mls.setUpsamplingStepSize(upsamplingStep); + mls.setPointDensity(pointDensity); + mls.setDilationVoxelSize(dilationVoxelSize); + mls.setDilationIterations(dilationIterations); // Reconstruct + mls.setInputCloud (cloud); + mls.setSearchMethod (tree); mls.process (*cloud_with_normals); return cloud_with_normals; @@ -164,30 +326,56 @@ pcl::PointCloud::Ptr computeNormalsSmoothed( void adjustNormalsToViewPoints( const pcl::PointCloud::Ptr & viewpoints, - pcl::PointCloud & cloud) + pcl::PointCloud::Ptr & cloud, + int k) { - if(viewpoints->size() && cloud.size()) + // FIXME: maybe better to project points in camera planes to know if they are visible from a specified viewpoint + if(viewpoints->size() && cloud->size()) { - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (viewpoints); + pcl::search::KdTree::Ptr viewpointsTree (new pcl::search::KdTree); + viewpointsTree->setInputCloud (viewpoints); - for(unsigned int i=0; i::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + for(unsigned int i=0; isize(); ++i) { std::vector indices; std::vector dist; - tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist); + viewpointsTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist); UASSERT(indices.size() == 1); - - Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap(); - Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z); - - float result = v.dot(n); - if(result < 0) + if(indices.size() && indices[0]>=0) { - //reverse normal - cloud.points[i].normal_x *= -1.0f; - cloud.points[i].normal_y *= -1.0f; - cloud.points[i].normal_z *= -1.0f; + + Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud->points[i].getVector3fMap(); + + //compute point normal + if(k >= 3) + { + tree->nearestKSearch(cloud->points[i], k, indices, dist); + if(indices.size() >= 3) + { + Eigen::Vector4f planeParameters; + float curvature; + pcl::computePointNormal(*cloud, indices, planeParameters, curvature); + + //update normal + cloud->points[i].normal_x = planeParameters[0]; + cloud->points[i].normal_y = planeParameters[1]; + cloud->points[i].normal_z = planeParameters[2]; + } + } + + Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].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; + } } } } diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 1c93bf38..01b97087 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -51,6 +51,14 @@ pcl::PointCloud::Ptr transformPointCloud( pcl::transformPointCloud(*cloud, *output, transform.toEigen4f()); return output; } +pcl::PointCloud::Ptr transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform) +{ + pcl::PointCloud::Ptr output(new pcl::PointCloud); + pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f()); + return output; +} pcl::PointCloud::Ptr transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform) diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 94b7075b..2728cd9c 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "rtabmap/core/Transform.h" #include #include @@ -145,6 +146,11 @@ public: const pcl::PolygonMesh::Ptr & mesh, const Transform & pose = Transform::getIdentity()); + bool addCloudTextureMesh( + const std::string & id, + const pcl::TextureMesh::Ptr & textureMesh, + const Transform & pose = Transform::getIdentity()); + bool addOccupancyGridMap( const cv::Mat & map8U, float resolution, // cell size diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 5292ffbd..110218be 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { class CameraThread; @@ -226,20 +227,22 @@ private: void exportPoses(int format); QString captureScreen(); - std::map::Ptr > getClouds( + std::map::Ptr > getClouds( const std::map & poses, bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth, - int normalKSearch, - bool mls, - float mlsRadius) const; + float regenerateMaxDepth) const; bool getExportedScans(std::map::Ptr > & scans); - bool getExportedClouds(std::map::Ptr> & clouds, std::map & meshes, bool toSave); + bool getExportedClouds( + std::map::Ptr> & clouds, + std::map & meshes, + std::map & textureMeshes, + bool toSave); void saveClouds(const std::map::Ptr> & clouds, bool binaryMode = true); void saveMeshes(const std::map & meshes, bool binaryMode = true); + void saveTextureMeshes(const std::map & meshes); void saveScans(const std::map::Ptr> & clouds, bool binaryMode = true); private: diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index eac37919..a6db87bd 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -573,6 +573,24 @@ bool CloudViewer::addCloudMesh( return false; } +bool CloudViewer::addCloudTextureMesh( + const std::string & id, + const pcl::TextureMesh::Ptr & textureMesh, + const Transform & pose) +{ + if(!_addedClouds.contains(id)) + { + UDEBUG("Adding %s", id.c_str()); + if(_visualizer->addTextureMesh(*textureMesh, id)) + { + _visualizer->updatePointCloudPose(id, pose.toEigen3f()); + _addedClouds.insert(id, pose); + return true; + } + } + return false; +} + bool CloudViewer::addOccupancyGridMap( const cv::Mat & map8U, float resolution, // cell size diff --git a/guilib/src/DetailedProgressDialog.cpp b/guilib/src/DetailedProgressDialog.cpp index 454cf9c1..ec47af17 100644 --- a/guilib/src/DetailedProgressDialog.cpp +++ b/guilib/src/DetailedProgressDialog.cpp @@ -69,6 +69,8 @@ DetailedProgressDialog::DetailedProgressDialog(QWidget *parent, Qt::WindowFlags hLayout->addWidget(_closeWhenDoneCheckBox); hLayout->addWidget(_closeButton); this->setLayout(layout); + + this->setModal(true); } DetailedProgressDialog::~DetailedProgressDialog() @@ -78,7 +80,10 @@ DetailedProgressDialog::~DetailedProgressDialog() void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec) { - _delayedClosingTime = delayedClosingTimeSec; + if(delayedClosingTimeSec >= 0) + { + _delayedClosingTime = delayedClosingTimeSec; + } _closeWhenDoneCheckBox->setChecked(on); } @@ -146,7 +151,6 @@ void DetailedProgressDialog::closeEvent(QCloseEvent *event) if(_progressBar->value() == _progressBar->maximum()) { event->accept(); - _closeWhenDoneCheckBox->setChecked(true); } else { diff --git a/guilib/src/DetailedProgressDialog.h b/guilib/src/DetailedProgressDialog.h index f7fb79f7..85a0eba3 100644 --- a/guilib/src/DetailedProgressDialog.h +++ b/guilib/src/DetailedProgressDialog.h @@ -50,7 +50,7 @@ public: void setValue(int value); int maximumSteps() const; void setMaximumSteps(int steps); - void setAutoClose(bool on, int delayedClosingTimeMsec = 0); + void setAutoClose(bool on, int delayedClosingTimeMsec = -1); protected: virtual void closeEvent(QCloseEvent * event); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 1d5dbb4b..210db906 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -40,19 +40,37 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults())); - connect(_ui->groupBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); - connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + restoreDefaults(); + _ui->comboBox_upsamplingMethod->setItemData(1, 0, Qt::UserRole - 1); // disable DISTINCT_CLOUD + + 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->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); - connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + + connect(_ui->groupBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->groupBox_mls, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_polygonialOrder, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_sampleStep, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_randomPoints, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_dilationVoxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_dilationSteps, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + _ui->stackedWidget_upsampling->setCurrentIndex(_ui->comboBox_upsamplingMethod->currentIndex()); + connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_upsampling, SLOT(setCurrentIndex(int))); + connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(updateMLSGrpVisibility())); + updateMLSGrpVisibility(); + 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())); + connect(_ui->checkBox_textureMapping, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); } ExportCloudsDialog::~ExportCloudsDialog() @@ -60,25 +78,48 @@ ExportCloudsDialog::~ExportCloudsDialog() delete _ui; } +void ExportCloudsDialog::updateMLSGrpVisibility() +{ + _ui->groupBox->setVisible(_ui->comboBox_upsamplingMethod->currentIndex() == 0); + _ui->groupBox_2->setVisible(_ui->comboBox_upsamplingMethod->currentIndex() == 1); + _ui->groupBox_3->setVisible(_ui->comboBox_upsamplingMethod->currentIndex() == 2); + _ui->groupBox_4->setVisible(_ui->comboBox_upsamplingMethod->currentIndex() == 3); + _ui->groupBox_5->setVisible(_ui->comboBox_upsamplingMethod->currentIndex() == 4); +} + void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & group) const { if(!group.isEmpty()) { settings.beginGroup(group); } - settings.setValue("assemble", this->getAssemble()); - settings.setValue("assemble_voxel", this->getAssembleVoxel()); + settings.setValue("binary", this->getBinaryFile()); + settings.setValue("normals_k", this->getNormalKSearch()); + settings.setValue("regenerate", this->getGenerate()); settings.setValue("regenerate_decimation", this->getGenerateDecimation()); settings.setValue("regenerate_voxel", this->getGenerateVoxel()); settings.setValue("regenerate_max_depth", this->getGenerateMaxDepth()); - settings.setValue("binary", this->getBinaryFile()); + + settings.setValue("assemble", this->getAssemble()); + settings.setValue("assemble_voxel", this->getAssembleVoxel()); + settings.setValue("mls", this->getMLS()); settings.setValue("mls_radius", this->getMLSRadius()); + settings.setValue("mls_polygonial_order", this->getMLSPolygonialOrder()); + settings.setValue("mls_upsampling_method", this->getMLSUpsamplingMethod()); + settings.setValue("mls_upsampling_radius", this->getMLSUpsamplingRadius()); + settings.setValue("mls_upsampling_step", this->getMLSUpsamplingStep()); + settings.setValue("mls_point_density", this->getMLSPointDensity()); + settings.setValue("mls_dilation_voxel_size", this->getMLSDilationVoxelSize()); + settings.setValue("mls_dilation_iterations", this->getMLSDilationIterations()); + settings.setValue("mesh", this->getMesh()); - settings.setValue("mesh_k", this->getMeshNormalKSearch()); settings.setValue("mesh_radius", this->getMeshGp3Radius()); settings.setValue("mesh_mu", this->getMeshGp3Mu()); + + settings.setValue("mesh_texture", this->getMeshTexture()); + if(!group.isEmpty()) { settings.endGroup(); @@ -91,19 +132,34 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou { settings.beginGroup(group); } - this->setAssemble(settings.value("assemble", this->getAssemble()).toBool()); - this->setAssembleVoxel(settings.value("assemble_voxel", this->getAssembleVoxel()).toDouble()); - this->setGenerate(settings.value("regenerate", this->getGenerate()).toBool()); - this->setGenerateDecimation(settings.value("regenerate_decimation", this->getGenerateDecimation()).toInt()); - this->setGenerateVoxel(settings.value("regenerate_voxel", this->getGenerateVoxel()).toDouble()); - this->setGenerateMaxDepth(settings.value("regenerate_max_depth", this->getGenerateMaxDepth()).toDouble()); - this->setBinaryFile(settings.value("binary", this->getBinaryFile()).toBool()); - this->setMLS(settings.value("mls", this->getMLS()).toBool()); - this->setMLSRadius(settings.value("mls_radius", this->getMLSRadius()).toDouble()); - 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()); + + _ui->checkBox_binary->setChecked(settings.value("binary", this->getBinaryFile()).toBool()); + _ui->spinBox_normalKSearch->setValue(settings.value("normals_k", this->getNormalKSearch()).toInt()); + + _ui->groupBox_regenerate->setChecked(settings.value("regenerate", this->getGenerate()).toBool()); + _ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", this->getGenerateDecimation()).toInt()); + _ui->doubleSpinBox_voxelSize->setValue(settings.value("regenerate_voxel", this->getGenerateVoxel()).toDouble()); + _ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", this->getGenerateMaxDepth()).toDouble()); + + _ui->groupBox_assemble->setChecked(settings.value("assemble", this->getAssemble()).toBool()); + _ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", this->getAssembleVoxel()).toDouble()); + + _ui->groupBox_mls->setChecked(settings.value("mls", this->getMLS()).toBool()); + _ui->doubleSpinBox_mlsRadius->setValue(settings.value("mls_radius", this->getMLSRadius()).toDouble()); + _ui->spinBox_polygonialOrder->setValue(settings.value("mls_polygonial_order", this->getMLSPolygonialOrder()).toInt()); + _ui->comboBox_upsamplingMethod->setCurrentIndex(settings.value("mls_upsampling_method", this->getMLSUpsamplingMethod()).toInt()); + _ui->doubleSpinBox_sampleRadius->setValue(settings.value("mls_upsampling_radius", this->getMLSRadius()).toDouble()); + _ui->doubleSpinBox_sampleStep->setValue(settings.value("mls_upsampling_step", this->getMLSUpsamplingStep()).toDouble()); + _ui->spinBox_randomPoints->setValue(settings.value("mls_point_density", this->getMLSPointDensity()).toInt()); + _ui->doubleSpinBox_dilationVoxelSize->setValue(settings.value("mls_dilation_voxel_size", this->getMLSDilationVoxelSize()).toDouble()); + _ui->spinBox_dilationSteps->setValue(settings.value("mls_dilation_iterations", this->getMLSDilationIterations()).toInt()); + + _ui->groupBox_gp3->setChecked(settings.value("mesh", this->getMesh()).toBool()); + _ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", this->getMeshGp3Radius()).toDouble()); + _ui->doubleSpinBox_gp3Mu->setValue(settings.value("mesh_mu", this->getMeshGp3Mu()).toDouble()); + + _ui->checkBox_textureMapping->setChecked(settings.value("mesh_texture", this->getGenerate()).toBool()); + if(!group.isEmpty()) { settings.endGroup(); @@ -112,22 +168,34 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou void ExportCloudsDialog::restoreDefaults() { - setAssemble(true); - setAssembleVoxel(0.005); + _ui->checkBox_binary->setChecked(true); + _ui->spinBox_normalKSearch->setValue(20); + if(_ui->groupBox_regenerate->isEnabled()) { - setGenerate(true); + _ui->groupBox_regenerate->setChecked(true); } - setGenerateDecimation(1); - setGenerateVoxel(0.005); - setGenerateMaxDepth(4); - setBinaryFile(true); - setMLS(false); - setMLSRadius(0.04); - setMesh(false); - setMeshNormalKSearch(20); - setMeshGp3Radius(0.04); - setMeshGp3Radius(2.5); + _ui->spinBox_decimation->setValue(1); + _ui->doubleSpinBox_voxelSize->setValue(0.01); + _ui->doubleSpinBox_maxDepth->setValue(4); + + _ui->groupBox_assemble->setChecked(true); + _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01); + + _ui->groupBox_mls->setChecked(false); + _ui->doubleSpinBox_mlsRadius->setValue(0.04); + _ui->spinBox_polygonialOrder->setValue(2); + _ui->comboBox_upsamplingMethod->setCurrentIndex(0); + _ui->doubleSpinBox_sampleRadius->setValue(0.01); + _ui->doubleSpinBox_sampleStep->setValue(0.0); + _ui->spinBox_randomPoints->setValue(0); + _ui->doubleSpinBox_dilationVoxelSize->setValue(0.01); + _ui->spinBox_dilationSteps->setValue(0); + + _ui->groupBox_gp3->setChecked(false); + _ui->doubleSpinBox_gp3Radius->setValue(0.04); + _ui->doubleSpinBox_gp3Mu->setValue(2.5); + _ui->checkBox_textureMapping->setChecked(false); } void ExportCloudsDialog::setSaveButton() @@ -135,6 +203,9 @@ void ExportCloudsDialog::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); + _ui->checkBox_textureMapping->setVisible(true); + _ui->label_textureMapping->setVisible(true); } void ExportCloudsDialog::setOkButton() @@ -142,6 +213,9 @@ void ExportCloudsDialog::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); + _ui->checkBox_textureMapping->setVisible(false); + _ui->label_textureMapping->setVisible(false); } void ExportCloudsDialog::enableRegeneration(bool enabled) @@ -154,14 +228,13 @@ void ExportCloudsDialog::enableRegeneration(bool enabled) } //getters -bool ExportCloudsDialog::getAssemble() const +bool ExportCloudsDialog::getBinaryFile() const { - return _ui->groupBox_assemble->isChecked(); + return _ui->checkBox_binary->isChecked(); } - -double ExportCloudsDialog::getAssembleVoxel() const +int ExportCloudsDialog::getNormalKSearch() const { - return _ui->doubleSpinBox_voxelSize_assembled->value(); + return _ui->spinBox_normalKSearch->value(); } bool ExportCloudsDialog::getGenerate() const @@ -184,31 +257,57 @@ double ExportCloudsDialog::getGenerateMaxDepth() const return _ui->doubleSpinBox_maxDepth->value(); } -bool ExportCloudsDialog::getBinaryFile() const +bool ExportCloudsDialog::getAssemble() const { - return _ui->checkBox_binary->isChecked(); + return _ui->groupBox_assemble->isChecked(); +} + +double ExportCloudsDialog::getAssembleVoxel() const +{ + return _ui->doubleSpinBox_voxelSize_assembled->value(); } bool ExportCloudsDialog::getMLS() const { return _ui->groupBox_mls->isChecked(); } - double ExportCloudsDialog::getMLSRadius() const { return _ui->doubleSpinBox_mlsRadius->value(); } +int ExportCloudsDialog::getMLSPolygonialOrder() const +{ + return _ui->spinBox_polygonialOrder->value(); +} +int ExportCloudsDialog::getMLSUpsamplingMethod() const +{ + return _ui->comboBox_upsamplingMethod->currentIndex(); +} +double ExportCloudsDialog::getMLSUpsamplingRadius() const +{ + return _ui->doubleSpinBox_sampleRadius->value(); +} +double ExportCloudsDialog::getMLSUpsamplingStep() const +{ + return _ui->doubleSpinBox_sampleStep->value(); +} +int ExportCloudsDialog::getMLSPointDensity() const +{ + return _ui->spinBox_randomPoints->value(); +} +double ExportCloudsDialog::getMLSDilationVoxelSize() const +{ + return _ui->doubleSpinBox_dilationVoxelSize->value(); +} +int ExportCloudsDialog::getMLSDilationIterations() const +{ + return _ui->spinBox_dilationSteps->value(); +} bool ExportCloudsDialog::getMesh() const { return _ui->groupBox_gp3->isChecked(); } - -int ExportCloudsDialog::getMeshNormalKSearch() const -{ - return _ui->spinBox_normalKSearch->value(); -} - double ExportCloudsDialog::getMeshGp3Radius() const { return _ui->doubleSpinBox_gp3Radius->value(); @@ -217,59 +316,9 @@ double ExportCloudsDialog::getMeshGp3Mu() const { return _ui->doubleSpinBox_gp3Mu->value(); } - -//setters -void ExportCloudsDialog::setAssemble(bool on) +bool ExportCloudsDialog::getMeshTexture() const { - _ui->groupBox_assemble->setChecked(on); -} -void ExportCloudsDialog::setAssembleVoxel(double voxel) -{ - _ui->doubleSpinBox_voxelSize_assembled->setValue(voxel); -} -void ExportCloudsDialog::setGenerate(bool on) -{ - _ui->groupBox_regenerate->setChecked(on); -} -void ExportCloudsDialog::setGenerateDecimation(int decimation) -{ - _ui->spinBox_decimation->setValue(decimation); -} -void ExportCloudsDialog::setGenerateVoxel(double voxel) -{ - _ui->doubleSpinBox_voxelSize->setValue(voxel); -} -void ExportCloudsDialog::setGenerateMaxDepth(double maxDepth) -{ - _ui->doubleSpinBox_maxDepth->setValue(maxDepth); -} -void ExportCloudsDialog::setBinaryFile(bool on) -{ - _ui->checkBox_binary->setChecked(on); -} -void ExportCloudsDialog::setMLS(bool on) -{ - _ui->groupBox_mls->setChecked(on); -} -void ExportCloudsDialog::setMLSRadius(double radius) -{ - _ui->doubleSpinBox_mlsRadius->setValue(radius); -} -void ExportCloudsDialog::setMesh(bool on) -{ - _ui->groupBox_gp3->setChecked(on); -} -void ExportCloudsDialog::setMeshNormalKSearch(int k) -{ - _ui->spinBox_normalKSearch->setValue(k); -} -void ExportCloudsDialog::setMeshGp3Radius(double radius) -{ - _ui->doubleSpinBox_gp3Radius->setValue(radius); -} -void ExportCloudsDialog::setMeshGp3Mu(double mu) -{ - _ui->doubleSpinBox_gp3Mu->setValue(mu); + return _ui->checkBox_textureMapping->isChecked(); } } diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index 046ec3eb..29b9138d 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -53,34 +53,31 @@ public: void enableRegeneration(bool enabled); //getters - bool getAssemble() const; - double getAssembleVoxel() const; + bool getBinaryFile() const; + int getNormalKSearch() const; + bool getGenerate() const; int getGenerateDecimation() const; double getGenerateVoxel() const; double getGenerateMaxDepth() const; - bool getBinaryFile() const; + + bool getAssemble() const; + double getAssembleVoxel() const; + bool getMLS() const; double getMLSRadius() const; + int getMLSPolygonialOrder() const; + int getMLSUpsamplingMethod() const; + double getMLSUpsamplingRadius() const; + double getMLSUpsamplingStep() const; + int getMLSPointDensity() const; + double getMLSDilationVoxelSize() const; + int getMLSDilationIterations() const; + bool getMesh() const; - int getMeshNormalKSearch() const; double getMeshGp3Radius() const; double getMeshGp3Mu() const; - - //setters - void setAssemble(bool on); - void setAssembleVoxel(double voxel); - void setGenerate(bool on); - void setGenerateDecimation(int decimation); - void setGenerateVoxel(double voxel); - void setGenerateMaxDepth(double maxDepth); - void setBinaryFile(bool on); - void setMLS(bool on); - void setMLSRadius(double radius); - void setMesh(bool on); - void setMeshNormalKSearch(int k); - void setMeshGp3Radius(double radius); - void setMeshGp3Mu(double mu); + bool getMeshTexture() const; signals: void configChanged(); @@ -88,6 +85,9 @@ signals: public slots: void restoreDefaults(); +private slots: + void updateMLSGrpVisibility(); + private: Ui_ExportCloudsDialog * _ui; }; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 16f71efe..1e4bc018 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -99,6 +99,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -1764,7 +1765,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int pcl::PointCloud::Ptr cloudWithNormals; if(_preferencesDialog->getMeshSmoothing()) { - cloudWithNormals = util3d::computeNormalsSmoothed( + cloudWithNormals = util3d::mls( cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius(), false, @@ -1796,7 +1797,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int if(_preferencesDialog->getMeshSmoothing()) { pcl::PointCloud::Ptr cloudWithNormals; - cloudWithNormals = util3d::computeNormalsSmoothed( + cloudWithNormals = util3d::mls( cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius(), false, @@ -3321,7 +3322,6 @@ void MainWindow::postProcessing() return; } - _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->clear(); _initProgressDialog->show(); @@ -3708,7 +3708,7 @@ void MainWindow::postProcessing() else { _initProgressDialog->appendText(tr("SBA... failed!")); - _initProgressDialog->setAutoClose(false, 1); + _initProgressDialog->setAutoClose(false); } _initProgressDialog->incrementStep(); } @@ -4437,10 +4437,15 @@ void MainWindow::exportClouds() { std::map::Ptr> clouds; std::map meshes; + std::map textureMeshes; - if(getExportedClouds(clouds, meshes, true)) + if(getExportedClouds(clouds, meshes, textureMeshes, true)) { - if(meshes.size()) + if(textureMeshes.size()) + { + saveTextureMeshes(textureMeshes); + } + else if(meshes.size()) { saveMeshes(meshes, _exportDialog->getBinaryFile()); } @@ -4456,8 +4461,9 @@ void MainWindow::viewClouds() { std::map::Ptr> clouds; std::map meshes; + std::map textureMeshes; - if(getExportedClouds(clouds, meshes, false)) + if(getExportedClouds(clouds, meshes, textureMeshes, false)) { QDialog * window = new QDialog(this, Qt::Window); if(meshes.size()) @@ -4519,8 +4525,9 @@ void MainWindow::viewClouds() } bool MainWindow::getExportedClouds( - std::map::Ptr> & clouds, + std::map::Ptr> & cloudsWithNormals, std::map & meshes, + std::map & textureMeshes, bool toSave) { if(_exportDialog->isVisible()) @@ -4551,6 +4558,11 @@ bool MainWindow::getExportedClouds( { mul+=1; } + mul+=1; // normals + if(_exportDialog->getMeshTexture()) + { + mul+=1; + } _initProgressDialog->setMaximumSteps(int(poses.size())*mul+1); if(_exportDialog->getMLS()) @@ -4559,17 +4571,14 @@ bool MainWindow::getExportedClouds( "[search radius=%1m voxel=%2m]").arg(_exportDialog->getMLSRadius()).arg(_exportDialog->getGenerateVoxel())); } _initProgressDialog->appendText(tr("Computing surface normals... " - "[K neighbors=%1]").arg(_exportDialog->getMeshNormalKSearch())); + "[K neighbors=%1]").arg(_exportDialog->getNormalKSearch())); - clouds = this->getClouds( + std::map::Ptr> clouds = this->getClouds( poses, _exportDialog->getGenerate(), _exportDialog->getGenerateDecimation(), _exportDialog->getGenerateVoxel(), - _exportDialog->getGenerateMaxDepth(), - _exportDialog->getMeshNormalKSearch(), - _exportDialog->getAssemble()?false:_exportDialog->getMLS(), - (float)_exportDialog->getMLSRadius()); + _exportDialog->getGenerateMaxDepth()); if(_exportDialog->getAssemble()) { @@ -4577,62 +4586,75 @@ bool MainWindow::getExportedClouds( QApplication::processEvents(); int i =0; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(std::map::Ptr>::iterator iter=clouds.begin(); + pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + for(std::map::Ptr>::iterator iter=clouds.begin(); iter!= clouds.end(); ++iter) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first)); + pcl::PointCloud::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) - { - QApplication::processEvents(); - } + QApplication::processEvents(); } + _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()); + } + clouds.clear(); + clouds.insert(std::make_pair(0, assembledCloud)); + } + + // normals + for(std::map::Ptr>::iterator iter=clouds.begin(); + iter!= clouds.end(); + ++iter) + { + pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); 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()); - } - - _initProgressDialog->appendText(tr("Smoothing (MLS) of the assembled cloud (%1 points)...").arg(assembledCloud->size())); + _initProgressDialog->appendText(tr("Smoothing (MLS) of the assembled cloud (%1 points)...").arg(iter->second->size())); QApplication::processEvents(); - pcl::PointCloud::Ptr assembledCloudXYZRGB(new pcl::PointCloud); - pcl::copyPointCloud(*assembledCloud, *assembledCloudXYZRGB); - assembledCloud = util3d::computeNormalsSmoothed( - assembledCloudXYZRGB, + cloudWithNormals = util3d::mls( + iter->second, (float)_exportDialog->getMLSRadius(), - true, - _exportDialog->getGenerateVoxel()); + _exportDialog->getMLSPolygonialOrder(), + _exportDialog->getMLSUpsamplingMethod(), + (float)_exportDialog->getMLSUpsamplingRadius(), + (float)_exportDialog->getMLSUpsamplingStep(), + _exportDialog->getMLSPointDensity(), + (float)_exportDialog->getMLSDilationVoxelSize(), + _exportDialog->getMLSDilationIterations()); - if(_exportDialog->getAssembleVoxel()) - { - _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") - .arg(assembledCloud->size()) - .arg(_exportDialog->getAssembleVoxel())); - QApplication::processEvents(); - - assembledCloud = util3d::voxelize( - assembledCloud, - _exportDialog->getAssembleVoxel()); - } - - _initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(assembledCloud->size()).arg(poses.size())); + // Re-voxelize to make sure to have uniform density + _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") + .arg(cloudWithNormals->size()) + .arg(_exportDialog->getAssemble()?_exportDialog->getAssembleVoxel():_exportDialog->getGenerateVoxel())); QApplication::processEvents(); + cloudWithNormals = util3d::voxelize( + cloudWithNormals, + _exportDialog->getAssemble()?_exportDialog->getAssembleVoxel():_exportDialog->getGenerateVoxel()); + + } + else + { + //compute normals + cloudWithNormals = util3d::computeNormals(iter->second, _exportDialog->getNormalKSearch()); + } + + if(_exportDialog->getAssemble()) + { + _initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size())); pcl::PointCloud::Ptr viewpoints(new pcl::PointCloud); viewpoints->resize(poses.size()); int oi=0; @@ -4642,30 +4664,23 @@ bool MainWindow::getExportedClouds( (*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())); + util3d::adjustNormalsToViewPoints(viewpoints, cloudWithNormals, _exportDialog->getNormalKSearch()); } + cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals)); - clouds.clear(); - clouds.insert(std::make_pair(0, assembledCloud)); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); } + //mesh if(_exportDialog->getMesh()) { _initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius())); QApplication::processEvents(); int i=0; - for(std::map::Ptr>::iterator iter=clouds.begin(); - iter!= clouds.end(); + for(std::map::Ptr>::iterator iter=cloudsWithNormals.begin(); + iter!= cloudsWithNormals.end(); ++iter) { pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu()); @@ -4673,13 +4688,78 @@ bool MainWindow::getExportedClouds( _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(); - } + QApplication::processEvents(); } } + if(toSave && _exportDialog->getMeshTexture()) + { + int i=0; + for(std::map::iterator iter=meshes.begin(); + iter!= meshes.end(); + ++iter) + { + std::map cameras; + if(iter->first == 0) + { + cameras = poses; + } + else + { + UASSERT(uContains(poses, iter->first)); + cameras.insert(std::make_pair(iter->first, Transform::getIdentity())); + } + std::map cameraPoses; + std::map cameraModels; + std::map images; + for(std::map::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter) + { + if(_cachedSignatures.contains(iter->first)) + { + const Signature & s = _cachedSignatures.value(iter->first); + CameraModel model; + if(s.sensorData().stereoCameraModel().isValid()) + { + model = s.sensorData().stereoCameraModel().left(); + } + else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValid()) + { + model = s.sensorData().cameraModels()[0]; + } + cv::Mat image = s.sensorData().imageRaw(); + if(image.empty() && !s.sensorData().imageCompressed().empty()) + { + s.sensorData().uncompressDataConst(&image, 0, 0, 0); + } + if(!iter->second.isNull() && model.isValid() && !image.empty()) + { + cameraPoses.insert(std::make_pair(iter->first, iter->second)); + cameraModels.insert(std::make_pair(iter->first, model)); + images.insert(std::make_pair(iter->first, image)); + } + } + } + if(cameraPoses.size()) + { + QDir dir(_preferencesDialog->getWorkingDirectory()); + dir.mkdir("tmp_textures"); + pcl::TextureMesh::Ptr textureMesh = util3d::createTextureMesh( + iter->second, + cameraPoses, + cameraModels, + images, + dir.filePath("tmp_textures").toStdString()); + + textureMeshes.insert(std::make_pair(iter->first, textureMesh)); + } + + _initProgressDialog->appendText(tr("TextureMesh %1 created [cameras=%2] (%3/%4).").arg(iter->first).arg(cameraPoses.size()).arg(++i).arg(clouds.size())); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + } + + } + return true; } return false; @@ -4782,14 +4862,9 @@ void MainWindow::exportBundlerFormat() out << t.x() << " " << t.y() << " " << t.z() << "\n"; } - QMessageBox::Button b = QMessageBox::question(this, + QMessageBox::question(this, tr("Exporting cameras in Bundler format..."), - tr("%1 cameras/images exported to directory \"%2\".\nDo you want to export the cloud/mesh (PLY)?").arg(poses.size()).arg(path), - QMessageBox::Yes | QMessageBox::No); - if(b == QMessageBox::Yes) - { - this->exportClouds(); - } + tr("%1 cameras/images exported to directory \"%2\".").arg(poses.size()).arg(path)); fileList.close(); } fileOut.close(); @@ -4987,7 +5062,11 @@ void MainWindow::saveMeshes(const std::map & meshes, _initProgressDialog->appendText(tr("Saving the mesh (%1 polygons)...").arg(meshes.begin()->second->polygons.size())); bool success =false; - if(QFileInfo(path).suffix() == "ply") + if(QFileInfo(path).suffix() == "") + { + path += ".ply"; + } + else if(QFileInfo(path).suffix() == "ply") { if(binaryMode) { @@ -4998,18 +5077,9 @@ void MainWindow::saveMeshes(const std::map & meshes, success = pcl::io::savePLYFile(path.toStdString(), *meshes.begin()->second) == 0; } } - else if(QFileInfo(path).suffix() == "") + else if(QFileInfo(path).suffix() == "obj") { - //default ply - path += ".ply"; - if(binaryMode) - { - success = pcl::io::savePLYFileBinary(path.toStdString(), *meshes.begin()->second) == 0; - } - else - { - success = pcl::io::savePLYFile(path.toStdString(), *meshes.begin()->second) == 0; - } + success = pcl::io::saveOBJFile(path.toStdString(), *meshes.begin()->second) == 0; } else { @@ -5035,38 +5105,177 @@ void MainWindow::saveMeshes(const std::map & meshes, } else if(meshes.size()) { - QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply)..."), _preferencesDialog->getWorkingDirectory(), 0); + QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply *.obj)..."), _preferencesDialog->getWorkingDirectory(), 0); + if(!path.isEmpty()) + { + bool ok = false; + QStringList items; + items.push_back("ply"); + items.push_back("obj"); + 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, "mesh", &ok); + + if(ok) + { + for(std::map::const_iterator iter=meshes.begin(); iter!=meshes.end(); ++iter) + { + if(iter->second->polygons.size()) + { + pcl::PolygonMesh mesh; + mesh.polygons = iter->second->polygons; + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); + tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first)); + pcl::toPCLPointCloud2(*tmp, mesh.cloud); + + QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); + bool success =false; + if(suffix == "ply") + { + if(binaryMode) + { + success = pcl::io::savePLYFileBinary(pathFile.toStdString(), mesh) == 0; + } + else + { + success = pcl::io::savePLYFile(pathFile.toStdString(), mesh) == 0; + } + } + else if(suffix == "obj") + { + success = pcl::io::saveOBJFile(pathFile.toStdString(), mesh) == 0; + } + else + { + UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str()); + } + if(success) + { + _initProgressDialog->appendText(tr("Saved mesh %1 (%2 polygons) to %3.") + .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile)); + } + else + { + _initProgressDialog->appendText(tr("Failed saving mesh %1 (%2 polygons) to %3.") + .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile)); + } + } + else + { + _initProgressDialog->appendText(tr("Mesh %1 is empty!").arg(iter->first)); + } + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + } + } + } + } + } +} + +void MainWindow::saveTextureMeshes(const std::map & meshes) +{ + if(meshes.size() == 1) + { + QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+QDir::separator()+"mesh.obj", tr("Mesh (*.obj)")); + if(!path.isEmpty()) + { + if(meshes.begin()->second->tex_materials.size()) + { + _initProgressDialog->appendText(tr("Saving the mesh (with %1 textures)...").arg(meshes.begin()->second->tex_materials.size())); + + bool success =false; + if(QFileInfo(path).suffix() == "") + { + path += ".obj"; + } + + pcl::TextureMesh mesh; + mesh.tex_coordinates = meshes.begin()->second->tex_coordinates; + mesh.tex_materials = meshes.begin()->second->tex_materials; + QDir(QFileInfo(path).absoluteDir().absolutePath()).mkdir(QFileInfo(path).baseName()); + for(unsigned int i=0;isecond->tex_materials.size(); ++i) + { + QFileInfo info(mesh.tex_materials[i].tex_file.c_str()); + QString fullPath = QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+QFileInfo(path).baseName()+QDir::separator()+info.fileName(); + // relative path + mesh.tex_materials[i].tex_file=(QFileInfo(path).baseName()+QDir::separator()+info.fileName()).toStdString(); + if(!QFile::copy(meshes.begin()->second->tex_materials[i].tex_file.c_str(), fullPath)) + { + _initProgressDialog->appendText(tr("Failed copying texture \"%1\" to \"%2\".") + .arg(meshes.begin()->second->tex_materials[i].tex_file.c_str()).arg(fullPath), Qt::darkRed); + _initProgressDialog->setAutoClose(false); + } + } + mesh.tex_polygons = meshes.begin()->second->tex_polygons; + mesh.cloud = meshes.begin()->second->cloud; + + success = pcl::io::saveOBJFile(path.toStdString(), mesh) == 0; + if(success) + { + _initProgressDialog->incrementStep(); + _initProgressDialog->appendText(tr("Saving the mesh (with %1 textures)... done.").arg(mesh.tex_materials.size())); + + QMessageBox::information(this, tr("Save successful!"), tr("Mesh 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("No textures...")); + } + } + } + else if(meshes.size()) + { + QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.obj)..."), _preferencesDialog->getWorkingDirectory(), 0); if(!path.isEmpty()) { bool ok = false; QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "mesh", &ok); - QString suffix = "ply"; + QString suffix = "obj"; if(ok) { - for(std::map::const_iterator iter=meshes.begin(); iter!=meshes.end(); ++iter) + for(std::map::const_iterator iter=meshes.begin(); iter!=meshes.end(); ++iter) { - if(iter->second->polygons.size()) + QString currentPrefix=prefix+QString::number(iter->first); + if(iter->second->tex_materials.size()) { - pcl::PolygonMesh mesh; - mesh.polygons = iter->second->polygons; - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::TextureMesh mesh; + mesh.tex_coordinates = iter->second->tex_coordinates; + mesh.tex_materials = iter->second->tex_materials; + QDir(path).mkdir(currentPrefix); + for(unsigned int i=0;isecond->tex_materials.size(); ++i) + { + QFileInfo info(mesh.tex_materials[i].tex_file.c_str()); + QString fullPath = path+QDir::separator()+currentPrefix+QDir::separator()+info.fileName(); + // relative path + mesh.tex_materials[i].tex_file=(currentPrefix+QDir::separator()+info.fileName()).toStdString(); + if(!QFile::copy(iter->second->tex_materials[i].tex_file.c_str(), fullPath)) + { + _initProgressDialog->appendText(tr("Failed copying texture \"%1\" to \"%2\".") + .arg(iter->second->tex_materials[i].tex_file.c_str()).arg(fullPath), Qt::darkRed); + _initProgressDialog->setAutoClose(false); + } + } + mesh.tex_polygons = iter->second->tex_polygons; + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first)); pcl::toPCLPointCloud2(*tmp, mesh.cloud); - QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); + QString pathFile = path+QDir::separator()+QString("%1.%3").arg(currentPrefix).arg(suffix); bool success =false; - if(suffix == "ply") + if(suffix == "obj") { - if(binaryMode) - { - success = pcl::io::savePLYFileBinary(pathFile.toStdString(), mesh) == 0; - } - else - { - success = pcl::io::savePLYFile(pathFile.toStdString(), mesh) == 0; - } + success = pcl::io::saveOBJFile(pathFile.toStdString(), mesh) == 0; } else { @@ -5074,13 +5283,13 @@ void MainWindow::saveMeshes(const std::map & meshes, } if(success) { - _initProgressDialog->appendText(tr("Saved mesh %1 (%2 polygons) to %3.") - .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile)); + _initProgressDialog->appendText(tr("Saved mesh %1 (%2 textures) to %3.") + .arg(iter->first).arg(iter->second->tex_materials.size()-1).arg(pathFile)); } else { - _initProgressDialog->appendText(tr("Failed saving mesh %1 (%2 polygons) to %3.") - .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile)); + _initProgressDialog->appendText(tr("Failed saving mesh %1 (%2 textures) to %3.") + .arg(iter->first).arg(iter->second->tex_materials.size()-1).arg(pathFile), Qt::darkRed); } } else @@ -5203,17 +5412,14 @@ void MainWindow::saveScans(const std::map::P } } -std::map::Ptr > MainWindow::getClouds( +std::map::Ptr > MainWindow::getClouds( const std::map & poses, bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth, - int normalKSearch, - bool mls, - float mlsRadius) const + float regenerateMaxDepth) const { - std::map::Ptr> clouds; + std::map::Ptr> clouds; int i=0; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { @@ -5265,28 +5471,7 @@ std::map::Ptr > MainWindow::getClou if(cloud->size()) { - if(mls) - { - pcl::PointCloud::Ptr cloudWithNormals = util3d::computeNormalsSmoothed( - cloud, - mlsRadius, - true, - regenerateVoxelSize); - - if(regenerateVoxelSize) - { - cloudWithNormals = util3d::voxelize( - cloudWithNormals, - regenerateVoxelSize); - } - - cloud->clear(); - pcl::copyPointCloud(*cloudWithNormals, *cloud); - } - - pcl::PointCloud::Ptr cloudWithNormals = util3d::computeNormals(cloud, _exportDialog->getMeshNormalKSearch()); - - clouds.insert(std::make_pair(iter->first, cloudWithNormals)); + clouds.insert(std::make_pair(iter->first, cloud)); inserted = true; } } diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index ebfe1297..ffc19f5d 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -6,312 +6,701 @@ 0 0 - 678 - 579 + 807 + 847 Export 3D clouds - + - - - Regenerate clouds - - + + true - - - - - m - - - 3 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.005000000000000 - - - - - - - 3D cloud voxel size. - - - true - - - - - - - 1 - - - 32 - - - 1 - - - - - - - 3D cloud decimation (1-2-4-8-...). - - - true - - - - - - - m - - - 1 - - - 100.000000000000000 - - - 0.100000000000000 - - - 4.000000000000000 - - - - - - - 3D cloud maximum depth (0 means no limit). - - - true - - - - - - - - - - Assemble clouds to a single output cloud - - - true - - - - - - m - - - 3 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.005000000000000 - - - - - - - Voxel size. - - - true - - - - - - - - - - Binary file - - - true - - - - - - - Mesh smoothing using Moving Least Squares algorithm (MLS) - - - true - - - false - - - - - - WARNING: This adds significative time to process, though the clouds will be more smooth. - - - true - - - - - - - - - m - - - 3 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.040000000000000 - - - - - - - MLS search radius: Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting. + + + + 0 + 0 + 766 + 915 + + + + + + + + + Binary file. + + + true + + + + + + + + + + true + + + + + + + Set the number of k nearest neighbors to use for the normal estimation. + + + true + + + + + + + 0 + + + 20 + + + + + + + + + Regenerate clouds + + + true + + + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + 1 + + + 32 + + + 1 + + + + + + + 3D cloud decimation (1-2-4-8-...). + + + true + + + + + + + 3D cloud voxel size. + + + true + + + + + + + m + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 4.000000000000000 + + + + + + + 3D cloud maximum depth (0 means no limit). + + + true + + + + + + + + + + Assemble clouds to a single output cloud + + + true + + + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + Voxel size. + + + true + + + + + + + + + + Cloud smoothing using Moving Least Squares algorithm (MLS) + + + true + + + true + + + + + + WARNING: This adds significative time to process, though the clouds will be more smooth. + + + true + + + + + + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.040000000000000 + + + + + + + + NONE + + + + + DISTINCT_CLOUD + + + + + SAMPLE_LOCAL_PLANE + + + + + RANDOM_UNIFORM_DENSITY + + + + + VOXEL_GRID_DILATION + + + + + + + + MLS search radius: Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting. Guidelines: 4 times the voxel size, 0.025 for voxel=0. - - - true - - - - - - - - - - - - Meshing using Greedy Projection Triangulation algorithm (GP3) - - - true - - - false - - - - - - 0 - - - 20 - - - - - - - Set the number of k nearest neighbors to use for the normal estimation to create the mesh. - - - true - - - - - - - m - - - 3 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.040000000000000 - - - - - - - Sphere radius that is to be used for determining the k-nearest neighbors used for triangulating (GP3). + + + true + + + + + + + Upsampling method. + + + true + + + + + + + Polygonial order. Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation (value=0). + + + true + + + + + + + 0 + + + 4 + + + 2 + + + + + + + + + 0 + + + + + + + NONE + + + + + + No upsampling will be done, only the input points will be projected to their own MLS surfaces. + + + true + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + DISTINCT_CLOUD + + + + + + Will project the points of the distinct cloud to the closest point on the MLS surface. + + + true + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + SAMPLE_LOCAL_PLANE + + + + + + The local plane of each input point will be sampled in a circular fashion. + + + true + + + + + + + + + Set the step size for the local plane sampling. + + + true + + + + + + + + + + Set the radius of the circle in the local point plane that will be sampled. + + + true + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + RANDOM_UNIFORM_DENSITY + + + + + + The local plane of each input point will be sampled using an uniform random distribution such that the density of points is constant throughout the cloud. + + + true + + + + + + + + + + + + Set the parameter that specifies the desired number of points within the search radius. + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + VOXEL_GRID_DILATION + + + + + + The input cloud will be inserted into a voxel grid. This voxel grid will be dilated and the resulting points will be projected to the MLS surface of the closest point in the input cloud. The result is a point cloud with filled holes and a constant point density. + + + true + + + + + + + + + Set the voxel size for the voxel grid. + + + true + + + + + + + + + + Set the number of dilation steps of the voxel grid. + + + true + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + + + + Meshing using Greedy Projection Triangulation algorithm (GP3) + + + true + + + true + + + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.040000000000000 + + + + + + + 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. - - - true - - - - - - - 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). - - - true - - - - - - - 0 - - - 1 - - - 0.100000000000000 - - - 100.000000000000000 - - - 1.000000000000000 - - - 2.500000000000000 - - - - + + + true + + + + + + + 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). + + + true + + + + + + + 0 + + + 1 + + + 0.100000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 2.500000000000000 + + + + + + + + + + + + + + Texture mapping. Images of the cameras will be projected on the mesh(es). Output is a *.obj format. + + + true + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + @@ -322,7 +711,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. 20 - 40 + 0 diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 8e93e2fb..9b7bd423 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 25 + 22 @@ -987,7 +987,7 @@ - Export 3D clouds (*.ply *.pcd)... + Export 3D clouds (*.ply *.pcd *.obj)... From 45877e7486d1ac2c94701d8c1e4386da413c3049 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 12:49:45 -0400 Subject: [PATCH 021/110] GUI export: added mesh quadric decimation factor option --- corelib/include/rtabmap/core/util3d_surface.h | 2 + corelib/src/util3d_surface.cpp | 13 +++- guilib/include/rtabmap/gui/CloudViewer.h | 6 ++ guilib/src/CloudViewer.cpp | 19 ++++++ guilib/src/ExportCloudsDialog.cpp | 10 ++++ guilib/src/ExportCloudsDialog.h | 1 + guilib/src/MainWindow.cpp | 60 ++++++++++++++++--- guilib/src/ui/exportCloudsDialog.ui | 60 +++++++++++++------ 8 files changed, 144 insertions(+), 27 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 89bb593d..21fd776f 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -84,6 +84,8 @@ void RTABMAP_EXP adjustNormalsToViewPoints( pcl::PointCloud::Ptr & cloud, int k = 0); // optional: recompute normal with k neighbors (min k=3) +pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor); + template std::vector normalizePolygonsSide( const pcl::PointCloud & cloud, diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 65a620a5..66ef2540 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -36,8 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include - -#include +#include namespace rtabmap { @@ -381,6 +380,16 @@ void adjustNormalsToViewPoints( } } +pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor) +{ + pcl::MeshQuadricDecimationVTK mqd; + mqd.setTargetReductionFactor(factor); + mqd.setInputMesh(mesh); + pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh); + mqd.process (*output); + return output; +} + } } diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 2728cd9c..2927428b 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -135,6 +135,12 @@ public: const Transform & pose = Transform::getIdentity(), const QColor & color = QColor()); + bool addCloudMesh( + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const std::vector & polygons, + const Transform & pose = Transform::getIdentity()); + bool addCloudMesh( const std::string & id, const pcl::PointCloud::Ptr & cloud, diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index a6db87bd..29d329b9 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -536,6 +536,25 @@ bool CloudViewer::addCloud( return false; } +bool CloudViewer::addCloudMesh( + const std::string & id, + const pcl::PointCloud::Ptr & cloud, + const std::vector & polygons, + const Transform & pose) +{ + if(!_addedClouds.contains(id)) + { + UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size()); + if(_visualizer->addPolygonMesh(cloud, polygons, id)) + { + _visualizer->updatePointCloudPose(id, pose.toEigen3f()); + _addedClouds.insert(id, pose); + return true; + } + } + return false; +} + bool CloudViewer::addCloudMesh( const std::string & id, const pcl::PointCloud::Ptr & cloud, diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 210db906..ee4ebb30 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -70,6 +70,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->groupBox_gp3, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_meshDecimationFactor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->checkBox_textureMapping, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); } @@ -117,6 +118,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("mesh", this->getMesh()); settings.setValue("mesh_radius", this->getMeshGp3Radius()); settings.setValue("mesh_mu", this->getMeshGp3Mu()); + settings.setValue("mesh_decimation_factor", this->getMeshDecimationFactor()); settings.setValue("mesh_texture", this->getMeshTexture()); @@ -157,6 +159,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->groupBox_gp3->setChecked(settings.value("mesh", this->getMesh()).toBool()); _ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", this->getMeshGp3Radius()).toDouble()); _ui->doubleSpinBox_gp3Mu->setValue(settings.value("mesh_mu", this->getMeshGp3Mu()).toDouble()); + _ui->doubleSpinBox_meshDecimationFactor->setValue(settings.value("mesh_decimation_factor", this->getMeshDecimationFactor()).toDouble()); _ui->checkBox_textureMapping->setChecked(settings.value("mesh_texture", this->getGenerate()).toBool()); @@ -195,7 +198,10 @@ void ExportCloudsDialog::restoreDefaults() _ui->groupBox_gp3->setChecked(false); _ui->doubleSpinBox_gp3Radius->setValue(0.04); _ui->doubleSpinBox_gp3Mu->setValue(2.5); + _ui->doubleSpinBox_meshDecimationFactor->setValue(1.0); _ui->checkBox_textureMapping->setChecked(false); + + this->update(); } void ExportCloudsDialog::setSaveButton() @@ -316,6 +322,10 @@ double ExportCloudsDialog::getMeshGp3Mu() const { return _ui->doubleSpinBox_gp3Mu->value(); } +double ExportCloudsDialog::getMeshDecimationFactor() const +{ + return _ui->doubleSpinBox_meshDecimationFactor->value(); +} bool ExportCloudsDialog::getMeshTexture() const { return _ui->checkBox_textureMapping->isChecked(); diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index 29b9138d..c27806ed 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -77,6 +77,7 @@ public: bool getMesh() const; double getMeshGp3Radius() const; double getMeshGp3Mu() const; + double getMeshDecimationFactor() const; bool getMeshTexture() const; signals: diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 1e4bc018..08fb2fdd 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4495,9 +4495,27 @@ void MainWindow::viewClouds() { _initProgressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->polygons.size())); _initProgressDialog->incrementStep(); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(iter->second->cloud, *cloud); - viewer->addCloudMesh(uFormat("mesh%d",iter->first), cloud, iter->second->polygons, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity()); + bool isRGB = false; + for(unsigned int i=0; isecond->cloud.fields.size(); ++i) + { + if(iter->second->cloud.fields[i].name.compare("rgb") == 0) + { + isRGB=true; + break; + } + } + if(isRGB) + { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(iter->second->cloud, *cloud); + viewer->addCloudMesh(uFormat("mesh%d",iter->first), cloud, iter->second->polygons, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity()); + } + else + { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(iter->second->cloud, *cloud); + viewer->addCloudMesh(uFormat("mesh%d",iter->first), cloud, iter->second->polygons, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity()); + } _initProgressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)... done.").arg(iter->first).arg(iter->second->polygons.size())); QApplication::processEvents(); } @@ -4684,9 +4702,16 @@ bool MainWindow::getExportedClouds( ++iter) { pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu()); + _initProgressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size())); + + if(_exportDialog->getMeshDecimationFactor() < 1.0) + { + mesh = util3d::meshDecimation(mesh, (float)_exportDialog->getMeshDecimationFactor()); + _initProgressDialog->appendText(tr("Mesh %1 decimation (factor=%2) to %3 polygons").arg(iter->first).arg(_exportDialog->getMeshDecimationFactor()).arg(mesh->polygons.size())); + } + 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(); QApplication::processEvents(); } @@ -5126,10 +5151,29 @@ void MainWindow::saveMeshes(const std::map & meshes, { pcl::PolygonMesh mesh; mesh.polygons = iter->second->polygons; - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); - tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first)); - pcl::toPCLPointCloud2(*tmp, mesh.cloud); + bool isRGB = false; + for(unsigned int i=0; isecond->cloud.fields.size(); ++i) + { + if(iter->second->cloud.fields[i].name.compare("rgb") == 0) + { + isRGB=true; + break; + } + } + if(isRGB) + { + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); + tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first)); + pcl::toPCLPointCloud2(*tmp, mesh.cloud); + } + else + { + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::fromPCLPointCloud2(iter->second->cloud, *tmp); + tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first)); + pcl::toPCLPointCloud2(*tmp, mesh.cloud); + } QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); bool success =false; diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index ffc19f5d..96986185 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - 0 + -178 766 - 915 + 949 @@ -604,6 +604,16 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. true + + + + 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). + + + true + + + @@ -634,21 +644,8 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - - - - 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). - - - true - - - - - 0 - 1 @@ -666,14 +663,14 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - + - + Texture mapping. Images of the cameras will be projected on the mesh(es). Output is a *.obj format. @@ -683,6 +680,35 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. + + + + Mesh quadric decimation factor (1=no decimation). Used to reduce the number of polygons. + + + true + + + + + + + 2 + + + 0.010000000000000 + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + From d57e12023b7c9f611f675664bae5b4b1f60f1d75 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 16:06:08 -0400 Subject: [PATCH 022/110] Remove recursively tmp textures folder --- guilib/src/MainWindow.cpp | 35 ++++++++++++++++++++++++++---- guilib/src/PreferencesDialog.cpp | 14 ++++++------ guilib/src/ui/preferencesDialog.ui | 14 ++++++++---- 3 files changed, 48 insertions(+), 15 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 08fb2fdd..0afe18ac 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4542,6 +4542,29 @@ void MainWindow::viewClouds() } } +bool removeDir(const QString & dirName) +{ + bool result = true; + QDir dir(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()); + } + else { + result = QFile::remove(info.absoluteFilePath()); + } + + if (!result) { + return result; + } + } + result = dir.rmdir(dirName); + } + return result; +} + bool MainWindow::getExportedClouds( std::map::Ptr> & cloudsWithNormals, std::map & meshes, @@ -4639,7 +4662,7 @@ bool MainWindow::getExportedClouds( pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); if(_exportDialog->getMLS()) { - _initProgressDialog->appendText(tr("Smoothing (MLS) of the assembled cloud (%1 points)...").arg(iter->second->size())); + _initProgressDialog->appendText(tr("Smoothing (MLS) the cloud (%1 points)...").arg(iter->second->size())); QApplication::processEvents(); cloudWithNormals = util3d::mls( @@ -4654,7 +4677,7 @@ bool MainWindow::getExportedClouds( _exportDialog->getMLSDilationIterations()); // Re-voxelize to make sure to have uniform density - _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") + _initProgressDialog->appendText(tr("Voxelize cloud (%1 points, voxel size = %2 m)...") .arg(cloudWithNormals->size()) .arg(_exportDialog->getAssemble()?_exportDialog->getAssembleVoxel():_exportDialog->getGenerateVoxel())); QApplication::processEvents(); @@ -4667,6 +4690,7 @@ bool MainWindow::getExportedClouds( else { //compute normals + _initProgressDialog->appendText(tr("Computing normals (%1 points)...").arg(iter->second->size())); cloudWithNormals = util3d::computeNormals(iter->second, _exportDialog->getNormalKSearch()); } @@ -4719,6 +4743,9 @@ bool MainWindow::getExportedClouds( if(toSave && _exportDialog->getMeshTexture()) { + QDir dir(_preferencesDialog->getWorkingDirectory()); + removeDir(_preferencesDialog->getWorkingDirectory()+QDir::separator()+"tmp_textures"); + dir.mkdir("tmp_textures"); int i=0; for(std::map::iterator iter=meshes.begin(); iter!= meshes.end(); @@ -4766,8 +4793,6 @@ bool MainWindow::getExportedClouds( } if(cameraPoses.size()) { - QDir dir(_preferencesDialog->getWorkingDirectory()); - dir.mkdir("tmp_textures"); pcl::TextureMesh::Ptr textureMesh = util3d::createTextureMesh( iter->second, cameraPoses, @@ -5240,6 +5265,7 @@ void MainWindow::saveTextureMeshes(const std::map & 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()); QDir(QFileInfo(path).absoluteDir().absolutePath()).mkdir(QFileInfo(path).baseName()); for(unsigned int i=0;isecond->tex_materials.size(); ++i) { @@ -5295,6 +5321,7 @@ void MainWindow::saveTextureMeshes(const std::map & pcl::TextureMesh mesh; mesh.tex_coordinates = iter->second->tex_coordinates; mesh.tex_materials = iter->second->tex_materials; + QDir(path).rmdir(currentPrefix); QDir(path).mkdir(currentPrefix); for(unsigned int i=0;isecond->tex_materials.size(); ++i) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 54891a57..812e99c1 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1014,15 +1014,15 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) for(int i=0; i<2; ++i) { _3dRenderingShowClouds[i]->setChecked(true); - _3dRenderingVoxelSize[i]->setValue(i==2?0.005:0.00); - _3dRenderingDecimation[i]->setValue(i==0?4:i==1?2:1); + _3dRenderingVoxelSize[i]->setValue(0.00); + _3dRenderingDecimation[i]->setValue(i==0?4:2); _3dRenderingMaxDepth[i]->setValue(i==1?0.0:4.0); _3dRenderingShowScans[i]->setChecked(true); - _3dRenderingOpacity[i]->setValue(1.0); - _3dRenderingPtSize[i]->setValue(i==0?1:2); - _3dRenderingOpacityScan[i]->setValue(1.0); - _3dRenderingPtSizeScan[i]->setValue(1); + _3dRenderingOpacity[i]->setValue(i==0?1.0:0.5); + _3dRenderingPtSize[i]->setValue(2); + _3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5); + _3dRenderingPtSizeScan[i]->setValue(2); } _ui->checkBox_showGraphs->setChecked(true); @@ -1035,7 +1035,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_mls->setChecked(false); _ui->doubleSpinBox_mlsRadius->setValue(0.04); - _ui->checkBox_nodeFiltering->setChecked(true); + _ui->checkBox_nodeFiltering->setChecked(false); _ui->checkBox_subtractFiltering->setChecked(false); _ui->doubleSpinBox_cloudFilterRadius->setValue(0.1); _ui->doubleSpinBox_cloudFilterAngle->setValue(30); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index feeefa96..e1dcb675 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -394 - 759 - 938 + -542 + 755 + 1591 @@ -468,7 +468,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - true + false @@ -955,6 +955,9 @@ Show a yellow background when the number of odometry inliers goes under this thr 64 + + 2 + @@ -965,6 +968,9 @@ Show a yellow background when the number of odometry inliers goes under this thr 64 + + 2 + From 58e3da18c273e20c508a428c0f763d8915e812d5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 17:12:39 -0400 Subject: [PATCH 023/110] GUI export: Added cloud filtering option --- guilib/include/rtabmap/gui/MainWindow.h | 4 +- guilib/src/ExportCloudsDialog.cpp | 30 +++++++++- guilib/src/ExportCloudsDialog.h | 4 ++ guilib/src/MainWindow.cpp | 37 ++++++++++-- guilib/src/ui/exportCloudsDialog.ui | 77 +++++++++++++++++++++++-- 5 files changed, 140 insertions(+), 12 deletions(-) diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 110218be..9594457f 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -232,7 +232,9 @@ private: bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const; + float regenerateMaxDepth, + float filteringRadius, + float filteringMinNeighbors) const; bool getExportedScans(std::map::Ptr > & scans); bool getExportedClouds( diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index ee4ebb30..eceb181e 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -51,6 +51,10 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), 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->groupBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -102,6 +106,10 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("regenerate_voxel", this->getGenerateVoxel()); settings.setValue("regenerate_max_depth", this->getGenerateMaxDepth()); + settings.setValue("filtering", this->getFiltering()); + settings.setValue("filtering_radius", this->getFilteringRadius()); + settings.setValue("filtering_min_neighbors", this->getFilteringMinNeighbors()); + settings.setValue("assemble", this->getAssemble()); settings.setValue("assemble_voxel", this->getAssembleVoxel()); @@ -143,6 +151,10 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->doubleSpinBox_voxelSize->setValue(settings.value("regenerate_voxel", this->getGenerateVoxel()).toDouble()); _ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", this->getGenerateMaxDepth()).toDouble()); + _ui->groupBox_filtering->setChecked(settings.value("filtering", this->getFiltering()).toBool()); + _ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", this->getFilteringRadius()).toDouble()); + _ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", this->getFilteringMinNeighbors()).toInt()); + _ui->groupBox_assemble->setChecked(settings.value("assemble", this->getAssemble()).toBool()); _ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", this->getAssembleVoxel()).toDouble()); @@ -182,6 +194,10 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_voxelSize->setValue(0.01); _ui->doubleSpinBox_maxDepth->setValue(4); + _ui->groupBox_filtering->setChecked(false); + _ui->doubleSpinBox_filteringRadius->setValue(0.02); + _ui->spinBox_filteringMinNeighbors->setValue(2); + _ui->groupBox_assemble->setChecked(true); _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01); @@ -263,11 +279,23 @@ double ExportCloudsDialog::getGenerateMaxDepth() const return _ui->doubleSpinBox_maxDepth->value(); } +bool ExportCloudsDialog::getFiltering() const +{ + return _ui->groupBox_filtering->isChecked(); +} +double ExportCloudsDialog::getFilteringRadius() const +{ + return _ui->doubleSpinBox_filteringRadius->value(); +} +int ExportCloudsDialog::getFilteringMinNeighbors() const +{ + return _ui->spinBox_filteringMinNeighbors->value(); +} + bool ExportCloudsDialog::getAssemble() const { return _ui->groupBox_assemble->isChecked(); } - double ExportCloudsDialog::getAssembleVoxel() const { return _ui->doubleSpinBox_voxelSize_assembled->value(); diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index c27806ed..f74dfdae 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -61,6 +61,10 @@ public: double getGenerateVoxel() const; double getGenerateMaxDepth() const; + bool getFiltering() const; + double getFilteringRadius() const; + int getFilteringMinNeighbors() const; + bool getAssemble() const; double getAssembleVoxel() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 0afe18ac..33be7822 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4619,7 +4619,9 @@ bool MainWindow::getExportedClouds( _exportDialog->getGenerate(), _exportDialog->getGenerateDecimation(), _exportDialog->getGenerateVoxel(), - _exportDialog->getGenerateMaxDepth()); + _exportDialog->getGenerateMaxDepth(), + _exportDialog->getFiltering()?_exportDialog->getFilteringRadius():0.0f, + _exportDialog->getFiltering()?_exportDialog->getFilteringMinNeighbors():0.0f); if(_exportDialog->getAssemble()) { @@ -4642,14 +4644,29 @@ bool MainWindow::getExportedClouds( _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") .arg(assembledCloud->size()) - .arg(_exportDialog->getGenerateVoxel())); + .arg(_exportDialog->getAssembleVoxel())); QApplication::processEvents(); - if(_exportDialog->getGenerateVoxel()) + if(_exportDialog->getAssembleVoxel()) { assembledCloud = util3d::voxelize( assembledCloud, - _exportDialog->getGenerateVoxel()); + _exportDialog->getAssembleVoxel()); } + + _initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...") + .arg(assembledCloud->size()) + .arg(_exportDialog->getFilteringRadius()) + .arg(_exportDialog->getFilteringMinNeighbors())); + if(_exportDialog->getFiltering() && + _exportDialog->getFilteringRadius() > 0.0 && + _exportDialog->getFilteringMinNeighbors() > 0) + { + pcl::IndicesPtr indices = util3d::radiusFiltering(assembledCloud, (float)_exportDialog->getFilteringRadius(), _exportDialog->getFilteringMinNeighbors()); + pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud); + pcl::copyPointCloud(*assembledCloud, *indices, *cloudFiltered); + assembledCloud = cloudFiltered; + } + clouds.clear(); clouds.insert(std::make_pair(0, assembledCloud)); } @@ -5488,7 +5505,9 @@ std::map::Ptr > MainWindow::getClouds( bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const + float regenerateMaxDepth, + float filteringRadius, + float filteringMinNeighbors) const { std::map::Ptr> clouds; int i=0; @@ -5542,6 +5561,14 @@ std::map::Ptr > MainWindow::getClouds( if(cloud->size()) { + if(filteringRadius > 0.0f && filteringMinNeighbors > 0) + { + pcl::IndicesPtr indices = util3d::radiusFiltering(cloud, filteringRadius, filteringMinNeighbors); + pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *indices, *cloudFiltered); + cloud = cloudFiltered; + } + clouds.insert(std::make_pair(iter->first, cloud)); inserted = true; } diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 96986185..ad778d1e 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -6,8 +6,8 @@ 0 0 - 807 - 847 + 814 + 898 @@ -23,9 +23,9 @@ 0 - -178 - 766 - 949 + 0 + 773 + 1064 @@ -166,6 +166,73 @@ + + + + Cloud filtering (remove noisy points) + + + true + + + + + + Radius search. + + + true + + + + + + + m + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + 1 + + + 100 + + + 2 + + + + + + + Minimum neighbors in the search radius. + + + true + + + + + + From 8ea1fa06efff3855c6f093e30e8f6d3a6512100e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 18:35:24 -0400 Subject: [PATCH 024/110] Fixed normals update with corresponding cameras --- corelib/include/rtabmap/core/util3d_surface.h | 4 +++- corelib/src/util3d_surface.cpp | 18 ++++++++------- guilib/src/ExportCloudsDialog.cpp | 2 +- guilib/src/MainWindow.cpp | 23 ++++++++++++++----- guilib/src/ui/exportCloudsDialog.ui | 10 ++++---- 5 files changed, 36 insertions(+), 21 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 21fd776f..a1f73be4 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -80,7 +80,9 @@ pcl::PointCloud::Ptr RTABMAP_EXP mls( int dilationIterations = 0); // VOXEL_GRID_DILATION void RTABMAP_EXP adjustNormalsToViewPoints( - const pcl::PointCloud::Ptr & viewpoints, + const std::map & poses, + const pcl::PointCloud::Ptr & rawCloud, + const std::vector & rawCameraIndices, pcl::PointCloud::Ptr & cloud, int k = 0); // optional: recompute normal with k neighbors (min k=3) diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 66ef2540..4e00e275 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -324,15 +324,16 @@ pcl::PointCloud::Ptr mls( } void adjustNormalsToViewPoints( - const pcl::PointCloud::Ptr & viewpoints, + const std::map & poses, + const pcl::PointCloud::Ptr & rawCloud, + const std::vector & rawCameraIndices, pcl::PointCloud::Ptr & cloud, int k) { - // FIXME: maybe better to project points in camera planes to know if they are visible from a specified viewpoint - if(viewpoints->size() && cloud->size()) + if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size()) { - pcl::search::KdTree::Ptr viewpointsTree (new pcl::search::KdTree); - viewpointsTree->setInputCloud (viewpoints); + pcl::search::KdTree::Ptr rawTree (new pcl::search::KdTree); + rawTree->setInputCloud (rawCloud); pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (cloud); @@ -341,12 +342,13 @@ void adjustNormalsToViewPoints( { std::vector indices; std::vector dist; - viewpointsTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, 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) { - - Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud->points[i].getVector3fMap(); + 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(); //compute point normal if(k >= 3) diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index eceb181e..035e97e1 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -214,7 +214,7 @@ void ExportCloudsDialog::restoreDefaults() _ui->groupBox_gp3->setChecked(false); _ui->doubleSpinBox_gp3Radius->setValue(0.04); _ui->doubleSpinBox_gp3Mu->setValue(2.5); - _ui->doubleSpinBox_meshDecimationFactor->setValue(1.0); + _ui->doubleSpinBox_meshDecimationFactor->setValue(0.0); _ui->checkBox_textureMapping->setChecked(false); this->update(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 33be7822..2f558b35 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4623,6 +4623,8 @@ bool MainWindow::getExportedClouds( _exportDialog->getFiltering()?_exportDialog->getFilteringRadius():0.0f, _exportDialog->getFiltering()?_exportDialog->getFilteringMinNeighbors():0.0f); + pcl::PointCloud::Ptr rawAssembledCloud(new pcl::PointCloud); + std::vector rawCameraIndices; if(_exportDialog->getAssemble()) { _initProgressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size())); @@ -4636,12 +4638,15 @@ bool MainWindow::getExportedClouds( { pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first)); *assembledCloud += *transformed; + rawCameraIndices.resize(assembledCloud->size(), iter->first); _initProgressDialog->appendText(tr("Assembled cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(clouds.size())); _initProgressDialog->incrementStep(); QApplication::processEvents(); } + pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); + _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") .arg(assembledCloud->size()) .arg(_exportDialog->getAssembleVoxel())); @@ -4653,14 +4658,15 @@ bool MainWindow::getExportedClouds( _exportDialog->getAssembleVoxel()); } - _initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...") - .arg(assembledCloud->size()) - .arg(_exportDialog->getFilteringRadius()) - .arg(_exportDialog->getFilteringMinNeighbors())); if(_exportDialog->getFiltering() && _exportDialog->getFilteringRadius() > 0.0 && _exportDialog->getFilteringMinNeighbors() > 0) { + _initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...") + .arg(assembledCloud->size()) + .arg(_exportDialog->getFilteringRadius()) + .arg(_exportDialog->getFilteringMinNeighbors())); + pcl::IndicesPtr indices = util3d::radiusFiltering(assembledCloud, (float)_exportDialog->getFilteringRadius(), _exportDialog->getFilteringMinNeighbors()); pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud); pcl::copyPointCloud(*assembledCloud, *indices, *cloudFiltered); @@ -4723,7 +4729,12 @@ bool MainWindow::getExportedClouds( (*viewpoints)[oi].y = iter->second.y(); (*viewpoints)[oi++].z = iter->second.z(); } - util3d::adjustNormalsToViewPoints(viewpoints, cloudWithNormals, _exportDialog->getNormalKSearch()); + util3d::adjustNormalsToViewPoints( + poses, + rawAssembledCloud, + rawCameraIndices, + cloudWithNormals, + _exportDialog->getNormalKSearch()); } cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals)); @@ -4745,7 +4756,7 @@ bool MainWindow::getExportedClouds( pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu()); _initProgressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size())); - if(_exportDialog->getMeshDecimationFactor() < 1.0) + if(_exportDialog->getMeshDecimationFactor() > 0.0) { mesh = util3d::meshDecimation(mesh, (float)_exportDialog->getMeshDecimationFactor()); _initProgressDialog->appendText(tr("Mesh %1 decimation (factor=%2) to %3 polygons").arg(iter->first).arg(_exportDialog->getMeshDecimationFactor()).arg(mesh->polygons.size())); diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index ad778d1e..042f2afa 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,7 +23,7 @@ 0 - 0 + -242 773 1064 @@ -750,7 +750,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - Mesh quadric decimation factor (1=no decimation). Used to reduce the number of polygons. + Mesh quadric decimation factor (0=no decimation). Used to reduce the number of polygons. true @@ -763,16 +763,16 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. 2 - 0.010000000000000 + 0.000000000000000 - 1.000000000000000 + 0.990000000000000 0.100000000000000 - 1.000000000000000 + 0.000000000000000 From fcdc82daae563928f30d07bea739b3aa6db88763 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 19:08:58 -0400 Subject: [PATCH 025/110] Post-Processing dialog: Updated default parameters --- guilib/src/PostProcessingDialog.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guilib/src/PostProcessingDialog.cpp b/guilib/src/PostProcessingDialog.cpp index 737701b3..60cd3c3f 100644 --- a/guilib/src/PostProcessingDialog.cpp +++ b/guilib/src/PostProcessingDialog.cpp @@ -123,7 +123,7 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr void PostProcessingDialog::restoreDefaults() { - setDetectMoreLoopClosures(false); + setDetectMoreLoopClosures(true); setClusterRadius(0.3); setClusterAngle(30); setIterations(1); From 0ac79d0e8fbd00bf5f779d3b52cc294fa5d415aa Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Aug 2015 21:02:32 -0400 Subject: [PATCH 026/110] :lipstick: --- corelib/include/rtabmap/core/util3d_surface.h | 3 +-- corelib/src/util3d_surface.cpp | 27 ++++--------------- guilib/src/MainWindow.cpp | 12 +-------- 3 files changed, 7 insertions(+), 35 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index a1f73be4..b794beb1 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -83,8 +83,7 @@ void RTABMAP_EXP adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, const std::vector & rawCameraIndices, - pcl::PointCloud::Ptr & cloud, - int k = 0); // optional: recompute normal with k neighbors (min k=3) + pcl::PointCloud::Ptr & cloud); pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor); diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 4e00e275..c121ada2 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -327,17 +327,13 @@ void adjustNormalsToViewPoints( const std::map & poses, const pcl::PointCloud::Ptr & rawCloud, const std::vector & rawCameraIndices, - pcl::PointCloud::Ptr & cloud, - int k) + pcl::PointCloud::Ptr & cloud) { if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size()) { pcl::search::KdTree::Ptr rawTree (new pcl::search::KdTree); rawTree->setInputCloud (rawCloud); - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (cloud); - for(unsigned int i=0; isize(); ++i) { std::vector indices; @@ -350,23 +346,6 @@ void adjustNormalsToViewPoints( pcl::PointXYZ viewpoint(p.x(), p.y(), p.z()); Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap(); - //compute point normal - if(k >= 3) - { - tree->nearestKSearch(cloud->points[i], k, indices, dist); - if(indices.size() >= 3) - { - Eigen::Vector4f planeParameters; - float curvature; - pcl::computePointNormal(*cloud, indices, planeParameters, curvature); - - //update normal - cloud->points[i].normal_x = planeParameters[0]; - cloud->points[i].normal_y = planeParameters[1]; - cloud->points[i].normal_z = planeParameters[2]; - } - } - Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z); float result = v.dot(n); @@ -378,6 +357,10 @@ void adjustNormalsToViewPoints( cloud->points[i].normal_z *= -1.0f; } } + else + { + UWARN("Not found camera viewpoint for point %d", i); + } } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2f558b35..3c2ea3e6 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4720,21 +4720,11 @@ bool MainWindow::getExportedClouds( if(_exportDialog->getAssemble()) { _initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size())); - pcl::PointCloud::Ptr viewpoints(new pcl::PointCloud); - viewpoints->resize(poses.size()); - int oi=0; - for(std::map::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( poses, rawAssembledCloud, rawCameraIndices, - cloudWithNormals, - _exportDialog->getNormalKSearch()); + cloudWithNormals); } cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals)); From 0cf1ea8777fcae16655ac5fe20868c1f143274ff Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 10:32:27 -0400 Subject: [PATCH 027/110] Moved some common include files to public headers folder --- .../rtabmap/core}/BayesFilter.h | 0 .../rtabmap/core}/ParticleFilter.h | 0 .../rtabmap/core}/VisualWord.h | 0 corelib/src/BayesFilter.cpp | 2 +- corelib/src/DBDriver.cpp | 2 +- corelib/src/DBDriverSqlite3.cpp | 2 +- corelib/src/Memory.cpp | 2 +- corelib/src/Odometry.cpp | 2 +- corelib/src/Rtabmap.cpp | 2 +- corelib/src/VWDictionary.cpp | 2 +- corelib/src/VisualWord.cpp | 2 +- .../rtabmap/gui}/ConsoleWidget.h | 4 +++- .../rtabmap/gui}/GraphViewer.h | 4 +++- guilib/include/rtabmap/gui/MainWindow.h | 4 ++-- guilib/{src => include/rtabmap/gui}/PdfPlot.h | 8 ++++--- .../rtabmap/gui/ProgressDialog.h} | 14 ++++++----- .../rtabmap/gui}/StatsToolBox.h | 6 +++-- .../include/rtabmap/{gui => utilite}/UCv2Qt.h | 0 .../rtabmap}/utilite/UImageView.h | 4 +++- .../{src => include/rtabmap}/utilite/UPlot.h | 18 +++++++------- guilib/src/CMakeLists.txt | 16 ++++++------- guilib/src/CalibrationDialog.cpp | 2 +- guilib/src/CameraViewer.cpp | 2 +- guilib/src/ConsoleWidget.cpp | 2 +- guilib/src/DataRecorder.cpp | 2 +- guilib/src/DatabaseViewer.cpp | 18 +++++++------- guilib/src/GraphViewer.cpp | 4 ++-- guilib/src/MainWindow.cpp | 12 +++++----- guilib/src/OdometryViewer.cpp | 2 +- guilib/src/PdfPlot.cpp | 4 ++-- guilib/src/PreferencesDialog.cpp | 4 ++-- ...dProgressDialog.cpp => ProgressDialog.cpp} | 24 +++++++++---------- guilib/src/StatsToolBox.cpp | 4 ++-- guilib/src/ui/DatabaseViewer.ui | 2 +- guilib/src/ui/calibrationDialog.ui | 2 +- guilib/src/ui/mainWindow.ui | 8 +++---- guilib/src/ui/preferencesDialog.ui | 2 +- guilib/src/utilite/UPlot.cpp | 2 +- tools/EpipolarGeometry/main.cpp | 2 +- 39 files changed, 103 insertions(+), 89 deletions(-) rename corelib/{src => include/rtabmap/core}/BayesFilter.h (100%) rename corelib/{src => include/rtabmap/core}/ParticleFilter.h (100%) rename corelib/{src => include/rtabmap/core}/VisualWord.h (100%) rename guilib/{src => include/rtabmap/gui}/ConsoleWidget.h (94%) rename guilib/{src => include/rtabmap/gui}/GraphViewer.h (97%) rename guilib/{src => include/rtabmap/gui}/PdfPlot.h (90%) rename guilib/{src/DetailedProgressDialog.h => include/rtabmap/gui/ProgressDialog.h} (89%) rename guilib/{src => include/rtabmap/gui}/StatsToolBox.h (92%) rename guilib/include/rtabmap/{gui => utilite}/UCv2Qt.h (100%) rename guilib/{src => include/rtabmap}/utilite/UImageView.h (93%) rename guilib/{src => include/rtabmap}/utilite/UPlot.h (93%) rename guilib/src/{DetailedProgressDialog.cpp => ProgressDialog.cpp} (86%) diff --git a/corelib/src/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h similarity index 100% rename from corelib/src/BayesFilter.h rename to corelib/include/rtabmap/core/BayesFilter.h diff --git a/corelib/src/ParticleFilter.h b/corelib/include/rtabmap/core/ParticleFilter.h similarity index 100% rename from corelib/src/ParticleFilter.h rename to corelib/include/rtabmap/core/ParticleFilter.h diff --git a/corelib/src/VisualWord.h b/corelib/include/rtabmap/core/VisualWord.h similarity index 100% rename from corelib/src/VisualWord.h rename to corelib/include/rtabmap/core/VisualWord.h diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index e80f142f..02c51208 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "BayesFilter.h" +#include "rtabmap/core/BayesFilter.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/Parameters.h" diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 6ec9ae91..062a4a52 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/DBDriver.h" #include "rtabmap/core/Signature.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UMath.h" #include "rtabmap/utilite/ULogger.h" diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 771ffdf1..3f4a84e3 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "DBDriverSqlite3.h" #include "rtabmap/core/Signature.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/Compression.h" diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 943cc9a1..284244df 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/VWDictionary.h" #include -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/Features2d.h" #include "DBDriverSqlite3.h" #include "rtabmap/core/util3d_features.h" diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 51ae1bf9..1f6d7504 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UConversion.h" -#include "ParticleFilter.h" +#include "rtabmap/core/ParticleFilter.h" namespace rtabmap { diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 4b19e677..5997b620 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" -#include "BayesFilter.h" +#include "rtabmap/core/BayesFilter.h" #include #include diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index fd9fee37..fe18fa61 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/VWDictionary.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/DBDriver.h" diff --git a/corelib/src/VisualWord.cpp b/corelib/src/VisualWord.cpp index b80cd6cc..9b54d4c4 100644 --- a/corelib/src/VisualWord.cpp +++ b/corelib/src/VisualWord.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UStl.h" diff --git a/guilib/src/ConsoleWidget.h b/guilib/include/rtabmap/gui/ConsoleWidget.h similarity index 94% rename from guilib/src/ConsoleWidget.h rename to guilib/include/rtabmap/gui/ConsoleWidget.h index ee59e4f2..3c9734fb 100644 --- a/guilib/src/ConsoleWidget.h +++ b/guilib/include/rtabmap/gui/ConsoleWidget.h @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CONSOLEWIDGET_H_ #define CONSOLEWIDGET_H_ +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + #include #include #include @@ -40,7 +42,7 @@ class QTextCursor; namespace rtabmap { -class ConsoleWidget : public QWidget, public UEventsHandler +class RTABMAPGUI_EXP ConsoleWidget : public QWidget, public UEventsHandler { Q_OBJECT; diff --git a/guilib/src/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h similarity index 97% rename from guilib/src/GraphViewer.h rename to guilib/include/rtabmap/gui/GraphViewer.h index 5639dd53..24c5aba1 100644 --- a/guilib/src/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef GRAPHVIEWER_H_ #define GRAPHVIEWER_H_ +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + #include #include #include @@ -44,7 +46,7 @@ namespace rtabmap { class NodeItem; class LinkItem; -class GraphViewer : public QGraphicsView { +class RTABMAPGUI_EXP GraphViewer : public QGraphicsView { Q_OBJECT; diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 9594457f..2c24ec7c 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -63,7 +63,7 @@ class AboutDialog; class Plot; class PdfPlotCurve; class StatsToolBox; -class DetailedProgressDialog; +class ProgressDialog; class TwistGridWidget; class ExportCloudsDialog; class PostProcessingDialog; @@ -297,7 +297,7 @@ private: PdfPlotCurve * _likelihoodCurve; PdfPlotCurve * _rawLikelihoodCurve; - DetailedProgressDialog * _initProgressDialog; + ProgressDialog * _initProgressDialog; QString _graphSavingFileName; QMap _exportPosesFileName; diff --git a/guilib/src/PdfPlot.h b/guilib/include/rtabmap/gui/PdfPlot.h similarity index 90% rename from guilib/src/PdfPlot.h rename to guilib/include/rtabmap/gui/PdfPlot.h index 35abac8b..7eb4e55e 100644 --- a/guilib/src/PdfPlot.h +++ b/guilib/include/rtabmap/gui/PdfPlot.h @@ -28,13 +28,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef PDFPLOT_H_ #define PDFPLOT_H_ -#include +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + +#include #include "opencv2/opencv.hpp" #include "rtabmap/core/Signature.h" namespace rtabmap { -class PdfPlotItem : public UPlotItem +class RTABMAPGUI_EXP PdfPlotItem : public UPlotItem { public: PdfPlotItem(float dataX, float dataY, float width, int childCount = -1); @@ -57,7 +59,7 @@ private: }; -class PdfPlotCurve : public UPlotCurve +class RTABMAPGUI_EXP PdfPlotCurve : public UPlotCurve { Q_OBJECT diff --git a/guilib/src/DetailedProgressDialog.h b/guilib/include/rtabmap/gui/ProgressDialog.h similarity index 89% rename from guilib/src/DetailedProgressDialog.h rename to guilib/include/rtabmap/gui/ProgressDialog.h index 85a0eba3..d2abf7bf 100644 --- a/guilib/src/DetailedProgressDialog.h +++ b/guilib/include/rtabmap/gui/ProgressDialog.h @@ -25,8 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DETAILEDPROGRESSDIALOG_H_ -#define DETAILEDPROGRESSDIALOG_H_ +#ifndef PROGRESSDIALOG_H_ +#define PROGRESSDIALOG_H_ + +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines #include @@ -38,13 +40,13 @@ class QCheckBox; namespace rtabmap { -class DetailedProgressDialog : public QDialog +class RTABMAPGUI_EXP ProgressDialog : public QDialog { Q_OBJECT public: - DetailedProgressDialog(QWidget *parent = 0, Qt::WindowFlags flags = 0); - virtual ~DetailedProgressDialog(); + ProgressDialog(QWidget *parent = 0, Qt::WindowFlags flags = 0); + virtual ~ProgressDialog(); void setEndMessage(const QString & message) {_endMessage = message;} // Message shown when the progress is finished void setValue(int value); @@ -73,4 +75,4 @@ private: } -#endif /* DETAILEDPROGRESSDIALOG_H_ */ +#endif /* PROGRESSDIALOG_H_ */ diff --git a/guilib/src/StatsToolBox.h b/guilib/include/rtabmap/gui/StatsToolBox.h similarity index 92% rename from guilib/src/StatsToolBox.h rename to guilib/include/rtabmap/gui/StatsToolBox.h index 53bac8f2..eb4202b0 100644 --- a/guilib/src/StatsToolBox.h +++ b/guilib/include/rtabmap/gui/StatsToolBox.h @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef STATSTOOLBOX_H_ #define STATSTOOLBOX_H_ +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + #include #include @@ -39,7 +41,7 @@ class QToolBox; namespace rtabmap { -class StatItem : public QWidget +class RTABMAPGUI_EXP StatItem : public QWidget { Q_OBJECT; @@ -75,7 +77,7 @@ private: -class StatsToolBox : public QWidget +class RTABMAPGUI_EXP StatsToolBox : public QWidget { Q_OBJECT; diff --git a/guilib/include/rtabmap/gui/UCv2Qt.h b/guilib/include/rtabmap/utilite/UCv2Qt.h similarity index 100% rename from guilib/include/rtabmap/gui/UCv2Qt.h rename to guilib/include/rtabmap/utilite/UCv2Qt.h diff --git a/guilib/src/utilite/UImageView.h b/guilib/include/rtabmap/utilite/UImageView.h similarity index 93% rename from guilib/src/utilite/UImageView.h rename to guilib/include/rtabmap/utilite/UImageView.h index 936e2bb6..1091b7a3 100644 --- a/guilib/src/utilite/UImageView.h +++ b/guilib/include/rtabmap/utilite/UImageView.h @@ -8,10 +8,12 @@ #ifndef IMAGEVIEW_H_ #define IMAGEVIEW_H_ +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + #include #include -class UImageView : public QWidget +class RTABMAPGUI_EXP UImageView : public QWidget { Q_OBJECT; public: diff --git a/guilib/src/utilite/UPlot.h b/guilib/include/rtabmap/utilite/UPlot.h similarity index 93% rename from guilib/src/utilite/UPlot.h rename to guilib/include/rtabmap/utilite/UPlot.h index b9fb80f5..b40bc6aa 100644 --- a/guilib/src/utilite/UPlot.h +++ b/guilib/include/rtabmap/utilite/UPlot.h @@ -20,6 +20,8 @@ #ifndef UPLOT_H_ #define UPLOT_H_ +#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines + #include #include #include @@ -40,7 +42,7 @@ class QFormLayout; * UPlotItem is a QGraphicsEllipseItem and can be inherited to do custom behaviors * on an hoverEnterEvent() for example. */ -class UPlotItem : public QGraphicsEllipseItem +class RTABMAPGUI_EXP UPlotItem : public QGraphicsEllipseItem { public: /** @@ -86,7 +88,7 @@ class UPlot; /** * UPlotCurve is a curve used to hold data shown in a UPlot. */ -class UPlotCurve : public QObject +class RTABMAPGUI_EXP UPlotCurve : public QObject { Q_OBJECT @@ -255,7 +257,7 @@ private: /** * A special UPlotCurve that shows as a line at the specified value, spanning all the UPlot. */ -class UPlotCurveThreshold : public UPlotCurve +class RTABMAPGUI_EXP UPlotCurveThreshold : public UPlotCurve { Q_OBJECT @@ -288,7 +290,7 @@ private: /** * The UPlot axis object. */ -class UPlotAxis : public QWidget +class RTABMAPGUI_EXP UPlotAxis : public QWidget { public: /** @@ -338,7 +340,7 @@ private: /** * The UPlot legend item. Used internally by UPlot. */ -class UPlotLegendItem : public QPushButton +class RTABMAPGUI_EXP UPlotLegendItem : public QPushButton { Q_OBJECT @@ -378,7 +380,7 @@ private: /** * The UPlot legend. Used internally by UPlot. */ -class UPlotLegend : public QWidget +class RTABMAPGUI_EXP UPlotLegend : public QWidget { Q_OBJECT @@ -420,7 +422,7 @@ private: /** * Orientable QLabel. Inherit QLabel and let you to specify the orientation. */ -class UOrientableLabel : public QLabel +class RTABMAPGUI_EXP UOrientableLabel : public QLabel { Q_OBJECT @@ -476,7 +478,7 @@ private: * * */ -class UPlot : public QWidget +class RTABMAPGUI_EXP UPlot : public QWidget { Q_OBJECT diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index da3ec599..9ebb2d42 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -6,13 +6,13 @@ SET(headers_ui ../include/${PROJECT_PREFIX}/gui/PreferencesDialog.h ../include/${PROJECT_PREFIX}/gui/DatabaseViewer.h ./AboutDialog.h - ./ConsoleWidget.h + ../include/${PROJECT_PREFIX}/gui/ConsoleWidget.h ../include/${PROJECT_PREFIX}/gui/ImageView.h - ./PdfPlot.h - ./StatsToolBox.h - ./DetailedProgressDialog.h - ./utilite/UPlot.h - ./utilite/UImageView.h + ../include/${PROJECT_PREFIX}/gui/PdfPlot.h + ../include/${PROJECT_PREFIX}/gui/StatsToolBox.h + ../include/${PROJECT_PREFIX}/gui/ProgressDialog.h + ../include/${PROJECT_PREFIX}/utilite/UPlot.h + ../include/${PROJECT_PREFIX}/utilite/UImageView.h ../include/${PROJECT_PREFIX}/gui/CloudViewer.h ../include/${PROJECT_PREFIX}/gui/OdometryViewer.h ../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h @@ -23,7 +23,7 @@ SET(headers_ui ./PostProcessingDialog.h ./ExportCloudsDialog.h ./MapVisibilityWidget.h - ./GraphViewer.h + ../include/${PROJECT_PREFIX}/gui/GraphViewer.h ./CreateSimpleCalibrationDialog.h ) @@ -69,7 +69,7 @@ SET(SRC_FILES ./ImageView.cpp ./PdfPlot.cpp ./StatsToolBox.cpp - ./DetailedProgressDialog.cpp + ./ProgressDialog.cpp ./AboutDialog.cpp ./ConsoleWidget.cpp ./DatabaseViewer.cpp diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 6ada1792..a30aa114 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 42162c31..c65f0a31 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include #include #include diff --git a/guilib/src/ConsoleWidget.cpp b/guilib/src/ConsoleWidget.cpp index a525acf5..97531e5f 100644 --- a/guilib/src/ConsoleWidget.cpp +++ b/guilib/src/ConsoleWidget.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "ConsoleWidget.h" +#include "rtabmap/gui/ConsoleWidget.h" #include "ui_consoleWidget.h" #include #include diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index c7909096..1cabf92e 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include #include #include diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index e5621911..55e73d57 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -48,7 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Memory.h" #include "rtabmap/core/DBDriver.h" #include "rtabmap/gui/KeypointItem.h" -#include "rtabmap/gui/UCv2Qt.h" +#include "rtabmap/utilite/UCv2Qt.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_filtering.h" @@ -63,7 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/DataRecorder.h" #include "rtabmap/core/SensorData.h" #include "ExportDialog.h" -#include "DetailedProgressDialog.h" +#include "rtabmap/gui/ProgressDialog.h" #include #include @@ -797,7 +797,7 @@ void DatabaseViewer::exportDatabase() if(recorder.init(path, false)) { - rtabmap::DetailedProgressDialog * progressDialog = new rtabmap::DetailedProgressDialog(this); + rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this); progressDialog->setAttribute(Qt::WA_DeleteOnClose); progressDialog->setMaximumSteps(ids.size()); progressDialog->show(); @@ -1237,7 +1237,7 @@ void DatabaseViewer::view3DMap() } if(optimizedPoses.size() > 0) { - rtabmap::DetailedProgressDialog progressDialog(this); + rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps((int)optimizedPoses.size()); progressDialog.show(); @@ -1338,7 +1338,7 @@ void DatabaseViewer::generate3DMap() } if(optimizedPoses.size() > 0) { - rtabmap::DetailedProgressDialog progressDialog; + rtabmap::ProgressDialog progressDialog; progressDialog.setMaximumSteps((int)optimizedPoses.size()); progressDialog.show(); @@ -1435,7 +1435,7 @@ void DatabaseViewer::refineAllNeighborLinks() { if(neighborLinks_.size()) { - rtabmap::DetailedProgressDialog progressDialog(this); + rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(neighborLinks_.size()); progressDialog.show(); @@ -1460,7 +1460,7 @@ void DatabaseViewer::refineAllLoopClosureLinks() { if(loopLinks_.size()) { - rtabmap::DetailedProgressDialog progressDialog(this); + rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(loopLinks_.size()); progressDialog.show(); @@ -1485,7 +1485,7 @@ void DatabaseViewer::refineVisuallyAllNeighborLinks() { if(neighborLinks_.size()) { - rtabmap::DetailedProgressDialog progressDialog(this); + rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(neighborLinks_.size()); progressDialog.show(); @@ -1510,7 +1510,7 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks() { if(loopLinks_.size()) { - rtabmap::DetailedProgressDialog progressDialog(this); + rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(loopLinks_.size()); progressDialog.show(); diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index b46eb2ef..4b5aba7e 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "GraphViewer.h" +#include "rtabmap/gui/GraphViewer.h" #include #include @@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 3c2ea3e6..32910b3d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -43,20 +43,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/KeypointItem.h" #include "rtabmap/gui/DataRecorder.h" #include "rtabmap/gui/DatabaseViewer.h" +#include "rtabmap/gui/PdfPlot.h" +#include "rtabmap/gui/StatsToolBox.h" +#include "rtabmap/gui/ProgressDialog.h" #include #include #include #include #include -#include "utilite/UPlot.h" -#include "rtabmap/gui/UCv2Qt.h" +#include "rtabmap/utilite/UPlot.h" +#include "rtabmap/utilite/UCv2Qt.h" #include "ExportCloudsDialog.h" #include "AboutDialog.h" -#include "PdfPlot.h" -#include "StatsToolBox.h" -#include "DetailedProgressDialog.h" #include "PostProcessingDialog.h" #include @@ -250,7 +250,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate()); _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit()); - _initProgressDialog = new DetailedProgressDialog(this); + _initProgressDialog = new ProgressDialog(this); _initProgressDialog->setWindowTitle(tr("Progress dialog")); _initProgressDialog->setMinimumWidth(800); diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index e4af0cca..3f62d0c6 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UConversion.h" -#include "rtabmap/gui/UCv2Qt.h" +#include "rtabmap/utilite/UCv2Qt.h" #include "rtabmap/gui/ImageView.h" #include "rtabmap/gui/CloudViewer.h" diff --git a/guilib/src/PdfPlot.cpp b/guilib/src/PdfPlot.cpp index b52225a0..e9d4b4f6 100644 --- a/guilib/src/PdfPlot.cpp +++ b/guilib/src/PdfPlot.cpp @@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "PdfPlot.h" +#include "rtabmap/gui/PdfPlot.h" #include -#include "rtabmap/gui/UCv2Qt.h" +#include "rtabmap/utilite/UCv2Qt.h" #include "rtabmap/core/util3d.h" namespace rtabmap { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 812e99c1..71582d1b 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -62,7 +62,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/CameraViewer.h" #include "rtabmap/gui/CloudViewer.h" #include "rtabmap/gui/ImageView.h" -#include "GraphViewer.h" +#include "rtabmap/gui/GraphViewer.h" #include "ExportCloudsDialog.h" #include "PostProcessingDialog.h" #include "CreateSimpleCalibrationDialog.h" @@ -71,7 +71,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include "utilite/UPlot.h" +#include "rtabmap/utilite/UPlot.h" #include #if CV_MAJOR_VERSION < 3 diff --git a/guilib/src/DetailedProgressDialog.cpp b/guilib/src/ProgressDialog.cpp similarity index 86% rename from guilib/src/DetailedProgressDialog.cpp rename to guilib/src/ProgressDialog.cpp index ec47af17..8a21ce2d 100644 --- a/guilib/src/DetailedProgressDialog.cpp +++ b/guilib/src/ProgressDialog.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "DetailedProgressDialog.h" +#include "rtabmap/gui/ProgressDialog.h" #include #include #include @@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -DetailedProgressDialog::DetailedProgressDialog(QWidget *parent, Qt::WindowFlags flags) : +ProgressDialog::ProgressDialog(QWidget *parent, Qt::WindowFlags flags) : QDialog(parent, flags), _delayedClosingTime(1) { @@ -73,12 +73,12 @@ DetailedProgressDialog::DetailedProgressDialog(QWidget *parent, Qt::WindowFlags this->setModal(true); } -DetailedProgressDialog::~DetailedProgressDialog() +ProgressDialog::~ProgressDialog() { } -void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec) +void ProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec) { if(delayedClosingTimeSec >= 0) { @@ -87,7 +87,7 @@ void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec) _closeWhenDoneCheckBox->setChecked(on); } -void DetailedProgressDialog::appendText(const QString & text, const QColor & color) +void ProgressDialog::appendText(const QString & text, const QColor & color) { _text->setText(text); QString html = tr("%1 %3").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text); @@ -96,7 +96,7 @@ void DetailedProgressDialog::appendText(const QString & text, const QColor & col _detailedText->horizontalScrollBar()->setSliderPosition(0); _detailedText->verticalScrollBar()->setSliderPosition(_detailedText->verticalScrollBar()->maximum()); } -void DetailedProgressDialog::setValue(int value) +void ProgressDialog::setValue(int value) { _progressBar->setValue(value); if(value == _progressBar->maximum()) @@ -113,16 +113,16 @@ void DetailedProgressDialog::setValue(int value) } } } -int DetailedProgressDialog::maximumSteps() const +int ProgressDialog::maximumSteps() const { return _progressBar->maximum(); } -void DetailedProgressDialog::setMaximumSteps(int steps) +void ProgressDialog::setMaximumSteps(int steps) { _progressBar->setMaximum(steps); } -void DetailedProgressDialog::incrementStep() +void ProgressDialog::incrementStep() { //incremental progress bar (if we don't know how many items will be added) if(_progressBar->value() == _progressBar->maximum()-1) @@ -132,7 +132,7 @@ void DetailedProgressDialog::incrementStep() _progressBar->setValue(_progressBar->value()+1); } -void DetailedProgressDialog::clear() +void ProgressDialog::clear() { _text->clear(); _progressBar->reset(); @@ -140,13 +140,13 @@ void DetailedProgressDialog::clear() _closeButton->setEnabled(false); } -void DetailedProgressDialog::resetProgress() +void ProgressDialog::resetProgress() { _progressBar->reset(); _closeButton->setEnabled(false); } -void DetailedProgressDialog::closeEvent(QCloseEvent *event) +void ProgressDialog::closeEvent(QCloseEvent *event) { if(_progressBar->value() == _progressBar->maximum()) { diff --git a/guilib/src/StatsToolBox.cpp b/guilib/src/StatsToolBox.cpp index 0bbf4d02..8a09f929 100644 --- a/guilib/src/StatsToolBox.cpp +++ b/guilib/src/StatsToolBox.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "StatsToolBox.h" +#include "rtabmap/gui/StatsToolBox.h" #include #include @@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include "utilite/UPlot.h" +#include "rtabmap/utilite/UPlot.h" #include namespace rtabmap { diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 9df1291f..64a22a90 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -2317,7 +2317,7 @@ rtabmap::GraphViewer QGraphicsView -
GraphViewer.h
+
rtabmap/gui/GraphViewer.h
 diff --git a/guilib/src/ui/calibrationDialog.ui b/guilib/src/ui/calibrationDialog.ui index b19b0666..dda9a8e1 100644 --- a/guilib/src/ui/calibrationDialog.ui +++ b/guilib/src/ui/calibrationDialog.ui @@ -712,7 +712,7 @@ UImageView QWidget -
utilite/UImageView.h
+
rtabmap/utilite/UImageView.h
1 diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 9b7bd423..db5c625b 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -1258,7 +1258,7 @@ UPlot QWidget -
utilite/UPlot.h
+
../include/rtabmap/utilite/UPlot.h
1 @@ -1269,13 +1269,13 @@ rtabmap::StatsToolBox QWidget -
StatsToolBox.h
+
../include/rtabmap/gui/StatsToolBox.h
1 rtabmap::ConsoleWidget QWidget -
ConsoleWidget.h
+
../include/rtabmap/gui/ConsoleWidget.h
1 @@ -1299,7 +1299,7 @@ rtabmap::GraphViewer QGraphicsView -
GraphViewer.h
+
../include/rtabmap/gui/GraphViewer.h
 diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index e1dcb675..fd5d99bb 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -9797,7 +9797,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare UPlot QWidget -
utilite/UPlot.h
+
rtabmap/utilite/UPlot.h
1 diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 98b11f33..974c97f0 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -17,7 +17,7 @@ * along with this program. If not, see . */ -#include "UPlot.h" +#include "rtabmap/utilite/UPlot.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UMath.h" diff --git a/tools/EpipolarGeometry/main.cpp b/tools/EpipolarGeometry/main.cpp index 1693bb78..9cbacdac 100644 --- a/tools/EpipolarGeometry/main.cpp +++ b/tools/EpipolarGeometry/main.cpp @@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/EpipolarGeometry.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/Odometry.h" -#include "rtabmap/gui/UCv2Qt.h" +#include "rtabmap/utilite/UCv2Qt.h" #include "rtabmap/gui/ImageView.h" #include "rtabmap/gui/KeypointItem.h" From cd7748970485a50ec91114fe5ed6f47108eac1a0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 10:33:26 -0400 Subject: [PATCH 028/110] Increased version to 0.10.6 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ff0b117..bf2614d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 10) -SET(RTABMAP_PATCH_VERSION 5) +SET(RTABMAP_PATCH_VERSION 6) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) From b4efdec9fc773d2d0719edf2ed354ee1270b93b3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 11:10:49 -0400 Subject: [PATCH 029/110] Fixed build with PCL < 1.7.2, fixed some warnings --- corelib/src/util3d_surface.cpp | 2 +- guilib/src/CloudViewer.cpp | 3 +++ guilib/src/MainWindow.cpp | 14 +++++++------- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index c121ada2..723f5697 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -171,7 +171,7 @@ pcl::TextureMesh::Ptr createTextureMesh( // Create materials for each texture (and one extra for occluded faces) textureMesh->tex_materials.resize (cameras.size () + 1); - for(int i = 0 ; i <= cameras.size() ; ++i) + for(unsigned int i = 0 ; i <= cameras.size() ; ++i) { pcl::TexMaterial mesh_material; mesh_material.tex_Ka.r = 0.2f; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 29d329b9..d471dbc7 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -597,6 +597,7 @@ bool CloudViewer::addCloudTextureMesh( const pcl::TextureMesh::Ptr & textureMesh, const Transform & pose) { +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) if(!_addedClouds.contains(id)) { UDEBUG("Adding %s", id.c_str()); @@ -607,6 +608,8 @@ bool CloudViewer::addCloudTextureMesh( return true; } } +#endif + // not implemented on lower version of PCL return false; } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 32910b3d..07445f6c 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4782,11 +4782,11 @@ bool MainWindow::getExportedClouds( std::map cameraPoses; std::map cameraModels; std::map images; - for(std::map::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter) + for(std::map::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter) { - if(_cachedSignatures.contains(iter->first)) + if(_cachedSignatures.contains(jter->first)) { - const Signature & s = _cachedSignatures.value(iter->first); + const Signature & s = _cachedSignatures.value(jter->first); CameraModel model; if(s.sensorData().stereoCameraModel().isValid()) { @@ -4801,11 +4801,11 @@ bool MainWindow::getExportedClouds( { s.sensorData().uncompressDataConst(&image, 0, 0, 0); } - if(!iter->second.isNull() && model.isValid() && !image.empty()) + if(!jter->second.isNull() && model.isValid() && !image.empty()) { - cameraPoses.insert(std::make_pair(iter->first, iter->second)); - cameraModels.insert(std::make_pair(iter->first, model)); - images.insert(std::make_pair(iter->first, image)); + cameraPoses.insert(std::make_pair(jter->first, jter->second)); + cameraModels.insert(std::make_pair(jter->first, model)); + images.insert(std::make_pair(jter->first, image)); } } } From 537a473482d9a3dda5b116996719f33b953f4fb5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 16:00:14 -0400 Subject: [PATCH 030/110] Set cloud substraction filtering by default --- guilib/src/MainWindow.cpp | 1 - guilib/src/PreferencesDialog.cpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 07445f6c..1696ae59 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1752,7 +1752,6 @@ 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; } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 71582d1b..c479405a 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1014,7 +1014,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) for(int i=0; i<2; ++i) { _3dRenderingShowClouds[i]->setChecked(true); - _3dRenderingVoxelSize[i]->setValue(0.00); + _3dRenderingVoxelSize[i]->setValue(i==0?0.01:0.0); // voxel of 1 cm for cloud substracting _3dRenderingDecimation[i]->setValue(i==0?4:2); _3dRenderingMaxDepth[i]->setValue(i==1?0.0:4.0); _3dRenderingShowScans[i]->setChecked(true); @@ -1036,10 +1036,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_mlsRadius->setValue(0.04); _ui->checkBox_nodeFiltering->setChecked(false); - _ui->checkBox_subtractFiltering->setChecked(false); + _ui->checkBox_subtractFiltering->setChecked(true); _ui->doubleSpinBox_cloudFilterRadius->setValue(0.1); _ui->doubleSpinBox_cloudFilterAngle->setValue(30); - _ui->spinBox_substractFilteringMinPts->setValue(0); + _ui->spinBox_substractFilteringMinPts->setValue(1); _ui->checkBox_map_shown->setChecked(false); _ui->doubleSpinBox_map_resolution->setValue(0.05); From dbc03024ef8b51871bec744f3e56372beefb38ab Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 16:29:27 -0400 Subject: [PATCH 031/110] Added warning on invalid decimation value when creating a RGBD cloud from SensorData --- corelib/src/util3d.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index d479aeaa..eef8a4ff 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -287,8 +288,8 @@ pcl::PointCloud::Ptr cloudFromDepthRGB( { UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); - UASSERT(imageDepth.rows % decimation == 0); - UASSERT(imageDepth.cols % decimation == 0); + UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str()); + UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepth.rows, decimation).c_str()); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); if(decimation < 1) @@ -641,6 +642,13 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( { if(sensorData.cameraModels()[i].isValid()) { + if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0) + { + UWARN("Image size (%d,%d) modulus decimation (%d) is not null " + "for the cloud creation! Setting decimation to 1...", + subImageWidth, sensorData.depthRaw().rows, decimation); + decimation = 1; + } pcl::PointCloud::Ptr tmp = util3d::cloudFromDepthRGB( cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)), cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), From 12bd8a1720138422f020d627cf677dea6e09c9bc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 19:36:48 -0400 Subject: [PATCH 032/110] Added special export poses for the KITTI dataset --- corelib/include/rtabmap/core/RtabmapEvent.h | 2 +- corelib/src/Rtabmap.cpp | 17 ++++++++++++++--- guilib/include/rtabmap/gui/MainWindow.h | 3 ++- guilib/src/MainWindow.cpp | 17 +++++++++++------ guilib/src/ui/mainWindow.ui | 8 +++++++- 5 files changed, 35 insertions(+), 12 deletions(-) diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index 295772cf..b21f8921 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -65,7 +65,7 @@ public: kCmdDumpMemory, kCmdDumpPrediction, kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin - kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO) + kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO) kCmdCleanDataBuffer, kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly kCmdTriggerNewMap, diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 5997b620..d997b980 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -735,7 +735,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - if(type==2) // TORO + if(type==3) // TORO { graph::TOROOptimizer::saveGraph(path, poses, constraints); } @@ -785,11 +785,22 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, } else // default / KITTI format { + Transform pose = iter->second; + if(type == 2) + { + // for KITTI, we need to remove optical rotation + // z pointing front, x left, y down + Transform t( 0, 0, 1, 0, + -1, 0, 0, 0, + 0,-1, 0, 0); + pose = t.inverse() * pose * t; + } + // Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz - const float * p = (const float *)(*iter).second.data(); + const float * p = (const float *)pose.data(); fprintf(fout, "%f", p[0]); - for(int i=1; i<(*iter).second.size(); i++) + for(int i=1; iactionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec())); connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds())); connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT())); - connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI())); + connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw())); connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM())); + connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI())); connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO())); connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory())); connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); @@ -3155,7 +3156,7 @@ void MainWindow::generateGraphDOT() } } -void MainWindow::exportPosesKITTI() +void MainWindow::exportPosesRaw() { exportPoses(0); } @@ -3163,10 +3164,14 @@ void MainWindow::exportPosesRGBDSLAM() { exportPoses(1); } -void MainWindow::exportPosesTORO() +void MainWindow::exportPosesKITTI() { exportPoses(2); } +void MainWindow::exportPosesTORO() +{ + exportPoses(3); +} void MainWindow::exportPoses(int format) { @@ -3204,14 +3209,14 @@ void MainWindow::exportPoses(int format) if(_exportPosesFileName[format].isEmpty()) { - _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==2?"toro.graph":"poses.txt"); + _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":"poses.txt"); } QString path = QFileDialog::getSaveFileName( this, tr("Save File"), _exportPosesFileName[format], - format == 2?tr("TORO file (*.graph)"):tr("Text file (*.txt)")); + format == 3?tr("TORO file (*.graph)"):tr("Text file (*.txt)")); if(!path.isEmpty()) { @@ -3222,7 +3227,7 @@ void MainWindow::exportPoses(int format) _ui->dockWidget_console->show(); _ui->widget_console->appendMsg( QString("%1 saved (global=%2, optimized=%3)... %4") - .arg(format == 2?"TORO graph":"Poses") + .arg(format == 3?"TORO graph":"Poses") .arg(global?"true":"false") .arg(optimized?"true":"false") .arg(_exportPosesFileName[format])); diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index db5c625b..a3cd3a9a 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -60,8 +60,9 @@ Export poses... - + + @@ -1253,6 +1254,11 @@ Export cameras in Bundler format (*.out)... + + + Raw format (*.txt) + + From 1b35b8cb99a2b4e1325846e0bd91f43ccc3a813d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 19:39:32 -0400 Subject: [PATCH 033/110] Updated default value for parameter RGBD/OptimizeEpsilon=0.0001 --- corelib/include/rtabmap/core/Parameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 7dd3005b..83b84fd3 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -308,7 +308,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations."); RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses."); RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); - RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); From 0651d5dfbd43b7a07c220f19a72216a477bfdb09 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Aug 2015 19:41:55 -0400 Subject: [PATCH 034/110] Updated default values for Icp3 parameters --- corelib/include/rtabmap/core/Parameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 83b84fd3..02be0981 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -379,9 +379,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(LccReextract, MaxWords, int, 1000, "0 no limits."); RTABMAP_PARAM(LccReextract, MaxDepth, float, 0.0, "Max depth of the words (0 means no limit)."); - RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation."); - RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth."); - RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.01, "Voxel size to be used for ICP computation."); + RTABMAP_PARAM(LccIcp3, Decimation, int, 4, "Depth image decimation."); + RTABMAP_PARAM(LccIcp3, MaxDepth, float, 3.0, "Max cloud depth."); + RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation."); RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set."); RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences."); RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations."); From ce2bbd8febd286537a1125aada17232abf0c8376 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 27 Aug 2015 17:16:12 -0400 Subject: [PATCH 035/110] CameraThread: Added stereo to depth option. Added parameter "Mem/SaveDepth16Format". --- corelib/include/rtabmap/core/CameraThread.h | 2 + corelib/include/rtabmap/core/Memory.h | 1 + corelib/include/rtabmap/core/Parameters.h | 1 + corelib/include/rtabmap/core/util2d.h | 10 +- corelib/include/rtabmap/core/util3d.h | 7 - corelib/src/CameraThread.cpp | 15 +- corelib/src/Compression.cpp | 19 +- corelib/src/Memory.cpp | 9 +- corelib/src/util2d.cpp | 117 +++++++- corelib/src/util3d.cpp | 259 +++++++----------- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/MainWindow.cpp | 1 + guilib/src/PreferencesDialog.cpp | 11 + guilib/src/ui/preferencesDialog.ui | 116 +++++--- tools/CameraRGBD/main.cpp | 3 +- tools/OdometryViewer/main.cpp | 1 - 16 files changed, 343 insertions(+), 230 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 2662606a..752f0087 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -52,6 +52,7 @@ public: void setMirroringEnabled(bool enabled) {_mirroring = enabled;} void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} + void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} //getters bool isPaused() const {return !this->isRunning();} @@ -68,6 +69,7 @@ private: Camera * _camera; bool _mirroring; bool _colorOnly; + bool _stereoToDepth; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 3015d458..123f9976 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -232,6 +232,7 @@ private: float _similarityThreshold; bool _rawDataKept; bool _binDataKept; + bool _saveDepth16Format; bool _notLinkedNodesKeptInDb; bool _incrementalMemory; int _maxStMemSize; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 02be0981..d0ba7423 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity."); RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM."); RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db."); + RTABMAP_PARAM(Mem, SaveDepth16Format, bool, true, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters)."); RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes)."); RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size."); RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode."); diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index 53576bc2..fd45266d 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/corelib/include/rtabmap/core/util2d.h @@ -41,7 +41,8 @@ namespace util2d cv::Mat RTABMAP_EXP disparityFromStereoImages( const cv::Mat & leftImage, - const cv::Mat & rightImage); + const cv::Mat & rightImage, + int type = CV_32FC1); // CV_32FC1 or CV_16SC1 cv::Mat RTABMAP_EXP disparityFromStereoImages( const cv::Mat & leftImage, @@ -53,6 +54,10 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages( double flowEps = 0.02, float maxCorrespondencesSlope = 0.1f); +cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, + float fx, float baseline, + int type = CV_32FC1); // CV_32FC1 or CV_16UC1 + cv::Mat RTABMAP_EXP depthFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, @@ -78,6 +83,9 @@ cv::Mat RTABMAP_EXP depthFromStereoCorrespondences( const std::vector & mask, float fx, float baseline); +cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); +cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); + float RTABMAP_EXP getDepth( const cv::Mat & depthImage, float x, float y, diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 4d841d94..10fda15a 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -127,9 +127,6 @@ pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( float maxDepth = 0, const Transform & localTransform = Transform::getIdentity()); -cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); -cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); - cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud); pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); @@ -147,10 +144,6 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D( const cv::Mat & disparity, float cx, float cy, float fx, float baseline); -cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, - float fx, float baseline, - int type = CV_32FC1); - pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( const std::list::Ptr> & clouds); pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 7610fb0d..bf4dcac8 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Camera.h" #include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" #include #include @@ -40,7 +42,8 @@ namespace rtabmap CameraThread::CameraThread(Camera * camera) : _camera(camera), _mirroring(false), - _colorOnly(false) + _colorOnly(false), + _stereoToDepth(false) { UASSERT(_camera != 0); } @@ -96,6 +99,16 @@ void CameraThread::mainLoop() data.setDepthOrRightRaw(tmpDepth); } } + if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty()) + { + cv::Mat depth = util2d::depthFromDisparity( + util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); + data.setCameraModel(data.stereoCameraModel().left()); + data.setDepthOrRightRaw(depth); + data.setStereoCameraModel(StereoCameraModel()); + } this->post(new CameraEvent(data, _camera->getSerial())); } diff --git a/corelib/src/Compression.cpp b/corelib/src/Compression.cpp index 3d15a7ee..c549052f 100644 --- a/corelib/src/Compression.cpp +++ b/corelib/src/Compression.cpp @@ -88,7 +88,16 @@ std::vector compressImage(const cv::Mat & image, const std::strin std::vector bytes; if(!image.empty()) { - cv::imencode(format, image, bytes); + if(image.type() == CV_32FC1) + { + //save in 8bits-4channel + cv::Mat bgra(image.size(), CV_8UC4, image.data); + cv::imencode(format, bgra, bytes); + } + else + { + cv::imencode(format, image, bytes); + } } return bytes; } @@ -114,6 +123,10 @@ cv::Mat uncompressImage(const cv::Mat & bytes) #else image = cv::imdecode(bytes, -1); #endif + if(image.type() == CV_8UC4) + { + image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + } } return image; } @@ -128,6 +141,10 @@ cv::Mat uncompressImage(const std::vector & bytes) #else image = cv::imdecode(bytes, -1); #endif + if(image.type() == CV_8UC4) + { + image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + } } return image; } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 284244df..85c868cb 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -68,6 +68,7 @@ Memory::Memory(const ParametersMap & parameters) : _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()), _rawDataKept(Parameters::defaultMemImageKept()), _binDataKept(Parameters::defaultMemBinDataKept()), + _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()), _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()), _incrementalMemory(Parameters::defaultMemIncrementalMemory()), _maxStMemSize(Parameters::defaultMemSTMSize()), @@ -399,6 +400,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept); Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept); + Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format); Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb); Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal); Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds); @@ -4257,10 +4259,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p std::vector imageBytes; std::vector depthBytes; - if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) + if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) { - UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion."); - depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage); + UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion."); + depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage); } rtabmap::CompressionThread ctImage(image, std::string(".jpg")); @@ -4347,7 +4349,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p s->sensorData().setUserDataRaw(data.userDataRaw()); } - t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); UDEBUG("time compressing data (id=%d) %fs", id, t); diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index 28560b97..b2c1400f 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -42,12 +42,13 @@ namespace util2d cv::Mat disparityFromStereoImages( const cv::Mat & leftImage, - const cv::Mat & rightImage) + const cv::Mat & rightImage, + int type) { - UASSERT(!leftImage.empty() && !rightImage.empty() && - (leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 && - leftImage.cols == rightImage.cols && - leftImage.rows == rightImage.rows); + UASSERT(!leftImage.empty() && !rightImage.empty()); + UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows); + UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1); + UASSERT(type == CV_32FC1 || type == CV_16SC1); cv::Mat leftMono; if(leftImage.channels() == 3) @@ -70,7 +71,7 @@ cv::Mat disparityFromStereoImages( stereo.state->textureThreshold = 10; stereo.state->speckleWindowSize = 100; stereo.state->speckleRange = 4; - stereo(leftMono, rightImage, disparity, CV_16SC1); + stereo(leftMono, rightImage, disparity, type); #else cv::Ptr stereo = cv::StereoBM::create(); stereo->setBlockSize(15); @@ -97,10 +98,9 @@ cv::Mat disparityFromStereoImages( double flowEps, float maxCorrespondencesSlope) { - UASSERT(!leftImage.empty() && !rightImage.empty() && - leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 && - leftImage.cols == rightImage.cols && - leftImage.rows == rightImage.rows); + UASSERT(!leftImage.empty() && !rightImage.empty()); + UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1); + UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows); // Find features in the new left image std::vector status; @@ -122,6 +122,51 @@ cv::Mat disparityFromStereoImages( return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope); } +cv::Mat depthFromDisparity(const cv::Mat & disparity, + float fx, float baseline, + int type) +{ + UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1)); + UASSERT(type == CV_32FC1 || type == CV_16UC1); + cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type); + int countOverMax = 0; + for (int i = 0; i < disparity.rows; i++) + { + for (int j = 0; j < disparity.cols; j++) + { + float disparity_value = disparity.type() == CV_16SC1?float(disparity.at(i,j))/16.0f:disparity.at(i,j); + if (disparity_value > 0.0f) + { + // baseline * focal / disparity + float d = baseline * fx / disparity_value; + if(d>0) + { + if(depth.type() == CV_32FC1) + { + depth.at(i,j) = d; + } + else + { + if(d*1000.0f <= (float)USHRT_MAX) + { + depth.at(i,j) = (unsigned short)(d*1000.0f); + } + else + { + ++countOverMax; + } + } + } + } + } + } + if(countOverMax) + { + UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax); + } + return depth; +} + cv::Mat depthFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, @@ -209,6 +254,58 @@ cv::Mat depthFromStereoCorrespondences( return depth; } +cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F) +{ + UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1); + cv::Mat depth16U; + if(!depth32F.empty()) + { + depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1); + int countOverMax = 0; + for(int i=0; i(i,j)*1000.0f); + unsigned short depthMM = 0; + if(depth > 0 && depth <= (float)USHRT_MAX) + { + depthMM = (unsigned short)depth; + } + else if(depth > (float)USHRT_MAX) + { + ++countOverMax; + } + depth16U.at(i, j) = depthMM; + } + } + if(countOverMax) + { + UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax); + } + } + return depth16U; +} + +cv::Mat cvtDepthToFloat(const cv::Mat & depth16U) +{ + UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1); + cv::Mat depth32F; + if(!depth16U.empty()) + { + depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1); + for(int i=0; i(i,j))/1000.0f; + depth32F.at(i, j) = depth; + } + } + } + return depth32F; +} + float getDepth( const cv::Mat & depthImage, float x, float y, diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index eef8a4ff..a9220376 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -286,6 +286,7 @@ pcl::PointCloud::Ptr cloudFromDepthRGB( float fx, float fy, int decimation) { + UDEBUG(""); UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str()); @@ -504,14 +505,13 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( float voxelSize, int samples) { - pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) { //depth UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols); int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); - cloud.reset(new pcl::PointCloud); for(unsigned int i=0; i::Ptr RTABMAP_EXP cloudRGBFromSensorData( float voxelSize, int samples) { - pcl::PointCloud::Ptr cloud; + UASSERT(!sensorData.imageRaw().empty()); + UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) || + (!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - if(!sensorData.imageRaw().empty()) + if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) { - if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) + //depth + UDEBUG(""); + UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols); + UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size()); + int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size(); + for(unsigned int i=0; i); - for(unsigned int i=0; i::Ptr tmp = util3d::cloudFromDepthRGB( + cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)), + cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), + sensorData.cameraModels()[i].cx(), + sensorData.cameraModels()[i].cy(), + sensorData.cameraModels()[i].fx(), + sensorData.cameraModels()[i].fy(), + decimation); + + if(tmp->size()) + { + bool filtered = false; + if(tmp->size() && maxDepth) { - UWARN("Image size (%d,%d) modulus decimation (%d) is not null " - "for the cloud creation! Setting decimation to 1...", - subImageWidth, sensorData.depthRaw().rows, decimation); - decimation = 1; + tmp = util3d::passThrough(tmp, "z", 0, maxDepth); + filtered = true; + } + + if(tmp->size() && voxelSize) + { + tmp = util3d::voxelize(tmp, voxelSize); + filtered = true; + } + + if(tmp->size() && samples) + { + tmp = util3d::sampling(tmp, samples); + filtered = true; + } + + if(tmp->size() && !filtered) + { + tmp = util3d::removeNaNFromPointCloud(tmp); } - pcl::PointCloud::Ptr tmp = util3d::cloudFromDepthRGB( - cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)), - cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), - sensorData.cameraModels()[i].cx(), - sensorData.cameraModels()[i].cy(), - sensorData.cameraModels()[i].fx(), - sensorData.cameraModels()[i].fy(), - decimation); if(tmp->size()) { - bool filtered = false; - if(tmp->size() && maxDepth) - { - tmp = util3d::passThrough(tmp, "z", 0, maxDepth); - filtered = true; - } - - if(tmp->size() && voxelSize) - { - tmp = util3d::voxelize(tmp, voxelSize); - filtered = true; - } - - if(tmp->size() && samples) - { - tmp = util3d::sampling(tmp, samples); - filtered = true; - } - - if(tmp->size() && !filtered) - { - tmp = util3d::removeNaNFromPointCloud(tmp); - } - - if(tmp->size()) - { - tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform()); - } - - *cloud += *tmp; + tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform()); } + + *cloud += *tmp; } - else - { - UERROR("Camera model %d is invalid", i); - } + } + else + { + UERROR("Camera model %d is invalid", i); + } + } + + if(cloud->size() && voxelSize) + { + cloud = util3d::voxelize(cloud, voxelSize); + } + } + else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) + { + //stereo + UDEBUG(""); + cloud = cloudFromStereoImages(sensorData.imageRaw(), + sensorData.rightRaw(), + sensorData.stereoCameraModel().left().cx(), + sensorData.stereoCameraModel().left().cy(), + sensorData.stereoCameraModel().left().fx(), + sensorData.stereoCameraModel().baseline(), + decimation); + + if(cloud->size()) + { + bool filtered = false; + if(cloud->size() && maxDepth) + { + cloud = util3d::passThrough(cloud, "z", 0, maxDepth); + filtered = true; } if(cloud->size() && voxelSize) { cloud = util3d::voxelize(cloud, voxelSize); + filtered = true; + } + + if(cloud->size() && !filtered) + { + cloud = util3d::removeNaNFromPointCloud(cloud); } - } - else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) - { - //stereo - cloud = cloudFromStereoImages(sensorData.imageRaw(), - sensorData.rightRaw(), - sensorData.stereoCameraModel().left().cx(), - sensorData.stereoCameraModel().left().cy(), - sensorData.stereoCameraModel().left().fx(), - sensorData.stereoCameraModel().baseline(), - decimation); if(cloud->size()) { - bool filtered = false; - if(cloud->size() && maxDepth) - { - cloud = util3d::passThrough(cloud, "z", 0, maxDepth); - filtered = true; - } - - if(cloud->size() && voxelSize) - { - cloud = util3d::voxelize(cloud, voxelSize); - filtered = true; - } - - if(cloud->size() && !filtered) - { - cloud = util3d::removeNaNFromPointCloud(cloud); - } - - if(cloud->size()) - { - cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); - } + cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); } } } @@ -779,50 +780,6 @@ pcl::PointCloud laserScanFromDepthImage( return scan; } - -cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F) -{ - UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1); - cv::Mat depth16U; - if(!depth32F.empty()) - { - depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1); - for(int i=0; i(i,j)*1000.0f); - unsigned short depthMM = 0; - if(depth <= (float)USHRT_MAX) - { - depthMM = (unsigned short)depth; - } - depth16U.at(i, j) = depthMM; - } - } - } - return depth16U; -} - -cv::Mat cvtDepthToFloat(const cv::Mat & depth16U) -{ - UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1); - cv::Mat depth32F; - if(!depth16U.empty()) - { - depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1); - for(int i=0; i(i,j))/1000.0f; - depth32F.at(i, j) = depth; - } - } - } - return depth32F; -} - cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud) { cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); @@ -914,36 +871,6 @@ pcl::PointXYZ projectDisparityTo3D( return pcl::PointXYZ(bad_point, bad_point, bad_point); } -cv::Mat depthFromDisparity(const cv::Mat & disparity, - float fx, float baseline, - int type) -{ - UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1)); - UASSERT(type == CV_32FC1 || type == CV_16U); - cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type); - for (int i = 0; i < disparity.rows; i++) - { - for (int j = 0; j < disparity.cols; j++) - { - float disparity_value = disparity.type() == CV_16SC1?float(disparity.at(i,j))/16.0f:disparity.at(i,j); - if (disparity_value > 0.0f) - { - // baseline * focal / disparity - float d = baseline * fx / disparity_value; - if(depth.type() == CV_32FC1) - { - depth.at(i,j) = d; - } - else - { - depth.at(i,j) = (unsigned short)(d*1000.0f); - } - } - } - } - return depth; -} - pcl::PointCloud::Ptr concatenateClouds(const std::list::Ptr> & clouds) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 9d4bcc45..ae0addca 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -189,6 +189,7 @@ public: int getSourceDatabaseStartPos() const; //Database group bool getSourceDatabaseStampsUsed() const;//Database group bool isSourceRGBDColorOnly() const; + bool isSourceStereoDepthGenerated() const; Transform getSourceLocalTransform() const; //Openni group Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2d866d87..5f9e9465 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2859,6 +2859,7 @@ void MainWindow::startDetection() _camera = new CameraThread(camera); _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); + _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); //Create odometry thread if rgbd slam if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str())) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index c479405a..a2d58f18 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -392,6 +392,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple())); connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); @@ -444,6 +445,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Memory _ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str()); _ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str()); + _ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str()); _ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str()); _ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str()); _ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str()); @@ -1105,6 +1107,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) } _ui->checkbox_rgbd_colorOnly->setChecked(false); + _ui->checkbox_stereo_depthGenerated->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); _ui->openni2_autoExposure->setChecked(true); _ui->openni2_exposure->setValue(0); @@ -1367,6 +1370,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("stereo"); _ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt()); + _ui->checkbox_stereo_depthGenerated->setChecked(settings.value("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()).toBool()); settings.endGroup(); // stereo settings.beginGroup("rgb"); @@ -1678,6 +1682,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.beginGroup("stereo"); settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex()); + settings.setValue("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()); settings.endGroup(); // stereo settings.beginGroup("rgb"); @@ -3447,6 +3452,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const { return _ui->checkbox_rgbd_colorOnly->isChecked(); } +bool PreferencesDialog::isSourceStereoDepthGenerated() const +{ + return _ui->checkbox_stereo_depthGenerated->isChecked(); +} Camera * PreferencesDialog::createCamera(bool useRawImages) { @@ -3826,6 +3835,7 @@ void PreferencesDialog::testOdometry(int type) CameraThread cameraThread(camera); // take ownership of camera cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); + cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent"); UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent"); @@ -3892,6 +3902,7 @@ void PreferencesDialog::testCamera() CameraThread cameraThread(camera); cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); + cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); UEventsManager::createPipe(&cameraThread, window, "CameraEvent"); cameraThread.start(); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index fd5d99bb..652e1aba 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -542 + -392 755 1591 @@ -86,7 +86,7 @@ QFrame::Raised - 1 + 3 @@ -1769,7 +1769,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 1 @@ -2387,6 +2387,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + + + + + Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. + + + true + + + @@ -3880,7 +3897,45 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + True=Generate location Ids, False=use input image ids. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 50 + 0 + + + + m + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.000000000000000 + + + + 1 @@ -3997,19 +4052,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - True=Generate location Ids, False=use input image ids. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -4066,7 +4108,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Image decimation. This feature can be used to save images in lower resolution (size/decimation). @@ -4102,7 +4144,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + If > 0.0, voxelize laser scans when creating a location. This feature can be used to save laser scans already voxelized. @@ -4115,28 +4157,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - 50 - 0 - + + + + Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters). - - m + + true - - 3 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - 1.000000000000000 + + + + + + - - 0.010000000000000 - - - 0.000000000000000 + + false @@ -6298,7 +6338,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph. RGB-D images must be sent to work (see Source->RGB-D Camera). + Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph. true diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 24b2048a..2e375162 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" +#include "rtabmap/core/util2d.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" @@ -322,7 +323,7 @@ int main(int argc, char * argv[]) cv::Mat depth = data.depthRaw(); if(depth.type() == CV_32FC1) { - depth = rtabmap::util3d::cvtDepthFromFloat(depth); + depth = rtabmap::util2d::cvtDepthFromFloat(depth); } if(rgb.cols == depth.cols && rgb.rows == depth.rows && diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 060013ca..1381d764 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -807,7 +807,6 @@ int main (int argc, char * argv[]) if(camera->isCalibrated()) { rtabmap::CameraThread cameraThread(camera); - cameraThread.setColorOnly(true); odomThread.start(); cameraThread.start(); From a2b9c9f9a049ab84a8308abb8ec05f482e779148 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 28 Aug 2015 23:05:45 -0400 Subject: [PATCH 036/110] Updated rehearsal behavior when there are intermediate nodes and RGBD/LinearUpdate,RGBD/AngularUpdate are set. --- corelib/include/rtabmap/core/Link.h | 20 +- corelib/include/rtabmap/core/Parameters.h | 4 +- corelib/include/rtabmap/core/Statistics.h | 1 + corelib/src/Memory.cpp | 216 ++++++++++-------- corelib/src/Rtabmap.cpp | 42 ++-- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/MainWindow.cpp | 11 +- guilib/src/PreferencesDialog.cpp | 4 + 8 files changed, 168 insertions(+), 131 deletions(-) diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index ae67c46d..be6c2bd3 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -118,25 +118,29 @@ public: infMatrix_.at(5,5) = 1.0/rotVariance; } - Link merge(const Link & link) const + Link merge(const Link & link, Type outputType) const { UASSERT(to_ == link.from()); - UASSERT(type_ == link.type()); - UASSERT(!transform_.isNull()); - UASSERT(!link.transform().isNull()); + UASSERT(outputType != Link::kUndef); + UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull())); UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1); return Link( from_, link.to(), - type_, - transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2) - infMatrix_ + link.infMatrix()); + outputType, + transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2) + transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix()); } Link inverse() const { - return Link(to_, from_, type_, transform_.inverse(), infMatrix_); + return Link( + to_, + from_, + type_, + transform_.isNull()?Transform():transform_.inverse(), + transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_); } private: diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index d0ba7423..e14cb953 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -286,8 +286,8 @@ class RTABMAP_EXP Parameters // RGB-D SLAM RTABMAP_PARAM(RGBD, Enabled, bool, true, ""); RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required)."); - RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index cc2dcf56..4644c244 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -83,6 +83,7 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Memory, Signatures_retrieved,); RTABMAP_STATS(Memory, Images_buffered,); RTABMAP_STATS(Memory, Rehearsal_sim,); + RTABMAP_STATS(Memory, Rehearsal_id,); RTABMAP_STATS(Memory, Rehearsal_merged,); RTABMAP_STATS(Memory, Local_graph_size,); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 85c868cb..8799b503 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -420,6 +420,8 @@ void Memory::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str()); UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str()); UASSERT(_imageDecimation >= 1); + UASSERT(_rehearsalMaxDistance >= 0.0f); + UASSERT(_rehearsalMaxAngle >= 0.0f); // SLAM mode vs Localization mode iter = parameters.find(Parameters::kMemIncrementalMemory()); @@ -3091,7 +3093,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) } //============================================================ - // Compare with the last (not null) + // Compare with the last (not intermediate node) //============================================================ Signature * sB = 0; for(std::set::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter) @@ -3116,55 +3118,9 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) { if(_incrementalMemory) { - if(signature->hasLink(id)) + if(this->rehearsalMerge(id, signature->id())) { - if(signature->getLinks().begin()->second.transform().isNull()) - { - if(this->rehearsalMerge(id, signature->id())) - { - merged = id; - } - } - else - { - float x,y,z, roll,pitch,yaw; - signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if((_rehearsalMaxDistance>0.0f && ( - fabs(x) > _rehearsalMaxDistance || - fabs(y) > _rehearsalMaxDistance || - fabs(z) > _rehearsalMaxDistance)) || - (_rehearsalMaxAngle>0.0f && ( - fabs(roll) > _rehearsalMaxAngle || - fabs(pitch) > _rehearsalMaxAngle || - fabs(yaw) > _rehearsalMaxAngle))) - { - if(_rehearsalWeightIgnoredWhileMoving) - { - UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad", - _rehearsalMaxDistance, _rehearsalMaxAngle); - } - else - { - // if the robot has moved, increase only weight of the new one - signature->setWeight(sB->getWeight() + signature->getWeight() + 1); - sB->setWeight(0); - UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)", - signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle); - } - } - else if(this->rehearsalMerge(id, signature->id())) - { - merged = id; - } - } - } - else - { - // cannot merge not neighbor signatures, just update weight - signature->setWeight(sB->getWeight() + signature->getWeight() + 1); - sB->setWeight(0); - UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.", - signature->getWeight(), signature->id(), sB->id()); + merged = id; } } else @@ -3175,6 +3131,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged); if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim); + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0); UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks()); } else @@ -3202,65 +3159,122 @@ bool Memory::rehearsalMerge(int oldId, int newId) UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id()); - //remove mutual links - oldS->removeLink(newId); - newS->removeLink(oldId); - - if(_idUpdatedToNewOneRehearsal) + bool fullMerge; + bool intermediateMerge = false; + if(!newS->getLinks().begin()->second.transform().isNull()) { - // redirect neighbor links - const std::map & links = oldS->getLinks(); - for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + // we are in metric SLAM mode: + // 1) Normal merge if not moving AND has direct link + // 2) Transform to intermediate node (weight = -1) if not moving AND hasn't direct link. + float x,y,z, roll,pitch,yaw; + newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + bool isMoving = fabs(x) > _rehearsalMaxDistance || + fabs(y) > _rehearsalMaxDistance || + fabs(z) > _rehearsalMaxDistance || + fabs(roll) > _rehearsalMaxAngle || + fabs(pitch) > _rehearsalMaxAngle || + fabs(yaw) > _rehearsalMaxAngle; + if(isMoving && _rehearsalWeightIgnoredWhileMoving) { - Link link = iter->second; - link.setFrom(newS->id()); - - Signature * s = this->_getSignature(link.to()); - if(s) - { - // modify neighbor "from" - s->changeLinkIds(oldS->id(), newS->id()); - - newS->addLink(link); - } - else - { - UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); - } - } - newS->setLabel(oldS->getLabel()); - oldS->setLabel(""); - oldS->removeLinks(); // remove all links - oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location - - // Set old image to new signature - this->copyData(oldS, newS); - - // update weight - newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - - if(_lastGlobalLoopClosureId == oldS->id()) - { - _lastGlobalLoopClosureId = newS->id(); + UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)", + _rehearsalMaxDistance, _rehearsalMaxAngle); + return false; } + fullMerge = !isMoving && newS->hasLink(oldS->id()); + intermediateMerge = !isMoving && !newS->hasLink(oldS->id()); } else { - newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location - - // update weight - oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - - if(_lastSignature == newS) - { - _lastSignature = oldS; - } + fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull(); } - // remove location - moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); + if(fullMerge) + { + //remove mutual links + Link newToOldLink = newS->getLinks().at(oldS->id()); + oldS->removeLink(newId); + newS->removeLink(oldId); - return true; + if(_idUpdatedToNewOneRehearsal) + { + // redirect neighbor links + const std::map & links = oldS->getLinks(); + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + { + Link link = iter->second; + Link mergedLink = newToOldLink.merge(link, link.type()); + UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to()); + + Signature * s = this->_getSignature(link.to()); + if(s) + { + // modify neighbor "from" + s->removeLink(oldS->id()); + s->addLink(mergedLink.inverse()); + + newS->addLink(mergedLink); + } + else + { + UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); + } + } + newS->setLabel(oldS->getLabel()); + oldS->setLabel(""); + oldS->removeLinks(); // remove all links + oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location + + // Set old image to new signature + this->copyData(oldS, newS); + + // update weight + newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); + + if(_lastGlobalLoopClosureId == oldS->id()) + { + _lastGlobalLoopClosureId = newS->id(); + } + } + else + { + newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location + + // update weight + oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); + + if(_lastSignature == newS) + { + _lastSignature = oldS; + } + } + + // remove location + moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); + + return true; + } + else + { + // update only weights + if(_idUpdatedToNewOneRehearsal) + { + // just update weight + int w = oldS->getWeight()>=0?oldS->getWeight():0; + newS->setWeight(w + newS->getWeight() + 1); + oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + + if(_lastGlobalLoopClosureId == oldS->id()) + { + _lastGlobalLoopClosureId = newS->id(); + } + } + else // !_idUpdatedToNewOneRehearsal + { + int w = newS->getWeight()>=0?newS->getWeight():0; + oldS->setWeight(w + oldS->getWeight() + 1); + newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + } + } } else { @@ -3273,7 +3287,7 @@ bool Memory::rehearsalMerge(int oldId, int newId) UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId); } } - return false; + return false; // means that the newS can be removed without problem } Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const @@ -4599,7 +4613,7 @@ void Memory::getMetricConstraints( const Signature * s2 = this->getSignature(uter->first); if(s2) { - link = link.merge(uter->second); + link = link.merge(uter->second, uter->second.type()); poses.erase(s->id()); s = s2; } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index d997b980..b310d319 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -409,6 +409,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks); Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData); + UASSERT(_rgbdLinearUpdate >= 0.0f); + UASSERT(_rgbdAngularUpdate >= 0.0f); + // RGB-D SLAM stuff if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end()) { @@ -986,7 +989,7 @@ bool Rtabmap::process( UFATAL("Not supposed to be here...last signature is null?!?"); } - ULOGGER_INFO("Processing signature %d", signature->id()); + ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight()); timeMemoryUpdate = timer.ticks(); ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate); @@ -1002,7 +1005,7 @@ bool Rtabmap::process( { _optimizedPoses.erase(rehearsedId); } - else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f) + else if(signature->getWeight() >= 0 && _rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f) { //============================================================ // Minimum displacement required to add to Memory @@ -1010,20 +1013,25 @@ bool Rtabmap::process( const std::map & links = signature->getLinks(); if(links.size() == 1) { - float x,y,z, roll,pitch,yaw; - links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if((_rgbdLinearUpdate==0.0f || ( - fabs(x) < _rgbdLinearUpdate && - fabs(y) < _rgbdLinearUpdate && - fabs(z) < _rgbdLinearUpdate)) && - (_rgbdAngularUpdate==0.0f || ( - fabs(roll) < _rgbdAngularUpdate && - fabs(pitch) < _rgbdAngularUpdate && - fabs(yaw) < _rgbdAngularUpdate))) + // don't do this if there are intermediate nodes + const Signature * s = _memory->getSignature(links.begin()->second.to()); + UASSERT(s!=0); + if(s->getWeight() >= 0) { - // This will disable global loop closure detection, only retrieval will be done. - // The location will also be deleted at the end. - smallDisplacement = true; + float x,y,z, roll,pitch,yaw; + links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + bool isMoving = fabs(x) > _rgbdLinearUpdate || + fabs(y) > _rgbdLinearUpdate || + fabs(z) > _rgbdLinearUpdate || + fabs(roll) > _rgbdAngularUpdate || + fabs(pitch) > _rgbdAngularUpdate || + fabs(yaw) > _rgbdAngularUpdate; + if(!isMoving) + { + // This will disable global loop closure detection, only retrieval will be done. + // The location will also be deleted at the end. + smallDisplacement = true; + } } } } @@ -1088,7 +1096,7 @@ bool Rtabmap::process( UASSERT(s!=0); if(s->getWeight() == -1) { - tmp = _constraints.rbegin()->second.merge(tmp); + tmp = _constraints.rbegin()->second.merge(tmp, tmp.type()); _optimizedPoses.erase(s->id()); _constraints.erase(--_constraints.end()); } @@ -1156,7 +1164,7 @@ bool Rtabmap::process( // Bayes filter update //============================================================ int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0; - // Not a bad signature, not a small displacemnt unless the previous signature didn't have a loop closure + // Not a bad signature, not a small displacement unless the previous signature didn't have a loop closure if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)) { // If the working memory is empty, don't do the detection. It happens when it diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index ae0addca..18f270a2 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -205,6 +205,7 @@ public: bool isStatisticsPublished() const; double getLoopThr() const; double getVpThr() const; + double getSimThr() const; int getOdomStrategy() const; int getOdomBufferSize() const; QString getCameraInfoDir() const; // "workinfDir/camera_info" diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 5f9e9465..d5e953db 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1075,23 +1075,28 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->label_matchId->clear(); } - int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); + int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); + bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f; int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f); bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f); _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId)); - if(rehearsed > 0) + if(rehearsalMerged > 0) { _ui->imageView_source->setBackgroundColor(Qt::blue); } else if(localTimeClosures > 0) { - _ui->imageView_source->setBackgroundColor(Qt::darkCyan); + _ui->imageView_source->setBackgroundColor(Qt::darkYellow); } else if(scanMatchingSuccess) { _ui->imageView_source->setBackgroundColor(Qt::gray); } + else if(rehearsedSimilarity) + { + _ui->imageView_source->setBackgroundColor(Qt::darkBlue); + } UDEBUG("time= %d ms", time.restart()); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index a2d58f18..5d211b04 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -3670,6 +3670,10 @@ double PreferencesDialog::getVpThr() const { return _ui->general_doubleSpinBox_vp->value(); } +double PreferencesDialog::getSimThr() const +{ + return _ui->doubleSpinBox_similarityThreshold->value(); +} int PreferencesDialog::getOdomStrategy() const { return _ui->odom_strategy->currentIndex(); From 7a721105aec49c66001fcfe20926f031980303fd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 2 Sep 2015 17:51:43 -0400 Subject: [PATCH 037/110] Added parameter "RGBD/OptimizeRobust" (default true) to use Vertigo robust graph optimization (only for g2o and GTSAM optimization strategies). Added GTSAM support. --- CMakeLists.txt | 13 + app/src/main.cpp | 2 +- cmake_modules/FindG2O.cmake | 6 +- corelib/include/rtabmap/core/Graph.h | 58 +- corelib/include/rtabmap/core/Parameters.h | 5 +- corelib/include/rtabmap/core/RtabmapEvent.h | 2 +- corelib/include/rtabmap/core/Transform.h | 2 + corelib/src/CMakeLists.txt | 26 + corelib/src/Graph.cpp | 606 ++++++++++++++++-- corelib/src/Rtabmap.cpp | 32 +- corelib/src/Transform.cpp | 6 + corelib/src/sqlite3/readme.txt | 3 + corelib/src/toro3d/readme.txt | 3 + .../src/vertigo/g2o/edge_se2MaxMixture.cpp | 122 ++++ corelib/src/vertigo/g2o/edge_se2MaxMixture.h | 46 ++ .../src/vertigo/g2o/edge_se2Switchable.cpp | 131 ++++ corelib/src/vertigo/g2o/edge_se2Switchable.h | 50 ++ .../src/vertigo/g2o/edge_se3Switchable.cpp | 120 ++++ corelib/src/vertigo/g2o/edge_se3Switchable.h | 48 ++ corelib/src/vertigo/g2o/edge_switchPrior.cpp | 44 ++ corelib/src/vertigo/g2o/edge_switchPrior.h | 22 + corelib/src/vertigo/g2o/types_g2o_robust.cpp | 22 + .../src/vertigo/g2o/vertex_switchLinear.cpp | 59 ++ corelib/src/vertigo/g2o/vertex_switchLinear.h | 43 ++ .../src/vertigo/gtsam/betweenFactorMaxMix.h | 67 ++ .../vertigo/gtsam/betweenFactorSwitchable.h | 97 +++ .../src/vertigo/gtsam/switchVariableLinear.h | 130 ++++ .../src/vertigo/gtsam/switchVariableSigmoid.h | 129 ++++ corelib/src/vertigo/readme.txt | 8 + guilib/include/rtabmap/gui/DatabaseViewer.h | 1 + guilib/include/rtabmap/gui/MainWindow.h | 1 + .../include/rtabmap/gui/PreferencesDialog.h | 2 +- guilib/src/AboutDialog.cpp | 1 + guilib/src/DatabaseViewer.cpp | 103 ++- guilib/src/MainWindow.cpp | 12 +- guilib/src/PreferencesDialog.cpp | 63 +- guilib/src/ui/DatabaseViewer.ui | 114 ++-- guilib/src/ui/aboutDialog.ui | 35 +- guilib/src/ui/mainWindow.ui | 8 +- guilib/src/ui/preferencesDialog.ui | 41 +- 40 files changed, 2125 insertions(+), 158 deletions(-) create mode 100644 corelib/src/sqlite3/readme.txt create mode 100644 corelib/src/toro3d/readme.txt create mode 100644 corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp create mode 100644 corelib/src/vertigo/g2o/edge_se2MaxMixture.h create mode 100644 corelib/src/vertigo/g2o/edge_se2Switchable.cpp create mode 100644 corelib/src/vertigo/g2o/edge_se2Switchable.h create mode 100644 corelib/src/vertigo/g2o/edge_se3Switchable.cpp create mode 100644 corelib/src/vertigo/g2o/edge_se3Switchable.h create mode 100644 corelib/src/vertigo/g2o/edge_switchPrior.cpp create mode 100644 corelib/src/vertigo/g2o/edge_switchPrior.h create mode 100644 corelib/src/vertigo/g2o/types_g2o_robust.cpp create mode 100644 corelib/src/vertigo/g2o/vertex_switchLinear.cpp create mode 100644 corelib/src/vertigo/g2o/vertex_switchLinear.h create mode 100644 corelib/src/vertigo/gtsam/betweenFactorMaxMix.h create mode 100644 corelib/src/vertigo/gtsam/betweenFactorSwitchable.h create mode 100644 corelib/src/vertigo/gtsam/switchVariableLinear.h create mode 100644 corelib/src/vertigo/gtsam/switchVariableSigmoid.h create mode 100644 corelib/src/vertigo/readme.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index bf2614d9..10654dd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,6 +128,7 @@ option(WITH_FREENECT2 "Include Freenect2 support" ON) option(WITH_OPENNI2 "Include OpenNI2 support" ON) option(WITH_DC1394 "Include dc1394 support" ON) option(WITH_G2O "Include g2o support" ON) +option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) @@ -170,6 +171,10 @@ IF(WITH_G2O) FIND_PACKAGE(G2O) ENDIF(WITH_G2O) +IF(WITH_GTSAM) + FIND_PACKAGE(GTSAM) +ENDIF(WITH_GTSAM) + IF(WITH_FLYCAPTURE2) FIND_PACKAGE(FlyCapture2) ENDIF(WITH_FLYCAPTURE2) @@ -429,6 +434,14 @@ ELSE() MESSAGE(STATUS " With g2o = NO (g2o not found)") ENDIF() +IF(GTSAM_FOUND) +MESSAGE(STATUS " With GTSAM = YES") +ELSEIF(NOT WITH_GTSAM) +MESSAGE(STATUS " With GTSAM = NO (WITH_GTSAM=OFF)") +ELSE() +MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)") +ENDIF() + IF(cvsba_FOUND) MESSAGE(STATUS " With cvsba = YES") ELSEIF(NOT WITH_CVSBA) diff --git a/app/src/main.cpp b/app/src/main.cpp index 9305a1b7..7bd30cef 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -41,7 +41,7 @@ int main(int argc, char* argv[]) { /* Set logger type */ ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kInfo); + ULogger::setLevel(ULogger::kDebug); /* Create tasks */ QApplication * app = new QApplication(argc, argv); diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index 343ed530..892c0933 100644 --- a/cmake_modules/FindG2O.cmake +++ b/cmake_modules/FindG2O.cmake @@ -70,11 +70,13 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR}) SET(G2O_LIBRARIES ${G2O_STUFF_LIBRARY} - ${G2O_CORE_LIBRARY} + ${G2O_CORE_LIBRARY} + ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SLAM2D} ${G2O_TYPES_SLAM3D} - ${CSPARSE_LIBRARY}) + ${CSPARSE_LIBRARY} + cholmod) SET(G2O_FOUND "YES") ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 0c92fb08..b6c6d508 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -51,7 +51,8 @@ public: kTypeUndef = -1, kTypeTORO = 0, kTypeG2O = 1, - kTypeCVSBA = 2 + kTypeGTSAM = 2, + kTypeCVSBA = 3 }; static Optimizer * create(const ParametersMap & parameters); static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap()); @@ -74,6 +75,7 @@ public: bool isSlam2d() const {return slam2d_;} bool isCovarianceIgnored() const {return covarianceIgnored_;} double epsilon() const {return epsilon_;} + bool isRobust() const {return robust_;} // inherited classes should implement one of these methods virtual std::map optimize( @@ -94,7 +96,8 @@ protected: int iterations = Parameters::defaultRGBDOptimizeIterations(), bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), - double epsilon = Parameters::defaultRGBDOptimizeEpsilon()); + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()); Optimizer(const ParametersMap & parameters); private: @@ -102,6 +105,7 @@ private: bool slam2d_; bool covarianceIgnored_; double epsilon_; + bool robust_; }; class RTABMAP_EXP TOROOptimizer : public Optimizer @@ -117,8 +121,12 @@ public: std::multimap & edgeConstraints); public: - TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + TOROOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {} TOROOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~TOROOptimizer() {} @@ -136,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer { public: static bool available(); + static bool saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints = false); public: - G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + G2OOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + G2OOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~G2OOptimizer() {} @@ -153,6 +172,33 @@ public: std::list > * intermediateGraphes = 0); }; +class RTABMAP_EXP GTSAMOptimizer : public Optimizer +{ +public: + static bool available(); + +public: + GTSAMOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + + GTSAMOptimizer(const ParametersMap & parameters) : + Optimizer(parameters) {} + virtual ~GTSAMOptimizer() {} + + virtual Type type() const {return kTypeGTSAM;} + + virtual std::map optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes = 0); +}; + class RTABMAP_EXP CVSBAOptimizer : public Optimizer { public: diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index e14cb953..498dfc81 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -306,10 +306,11 @@ class RTABMAP_EXP Parameters // Graph optimization RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o."); - RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations."); + RTABMAP_PARAM(RGBD, OptimizeIterations, int, 10, "Optimization iterations."); RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses."); RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); - RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeRobust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies)."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index b21f8921..5eac0b2e 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -65,7 +65,7 @@ public: kCmdDumpMemory, kCmdDumpPrediction, kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin - kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO) + kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o) kCmdCleanDataBuffer, kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly kCmdTriggerNewMap, diff --git a/corelib/include/rtabmap/core/Transform.h b/corelib/include/rtabmap/core/Transform.h index 2ed7ead8..682d45a5 100644 --- a/corelib/include/rtabmap/core/Transform.h +++ b/corelib/include/rtabmap/core/Transform.h @@ -51,6 +51,8 @@ public: Transform(const cv::Mat & transformationMatrix); // x,y,z, roll,pitch,yaw Transform(float x, float y, float z, float roll, float pitch, float yaw); + // x,y, theta + Transform(float x, float y, float theta); float r11() const {return data()[0];} float r12() const {return data()[1];} diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 22a425d5..64b2a4cf 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -149,8 +149,34 @@ IF(G2O_FOUND) ${LIBRARIES} ${G2O_LIBRARIES} ) + #Newest versions require std11 + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + + IF(WITH_VERTIGO) + SET(SRC_FILES + ${SRC_FILES} + vertigo/g2o/edge_se2MaxMixture.cpp + vertigo/g2o/edge_se2Switchable.cpp + vertigo/g2o/edge_se3Switchable.cpp + vertigo/g2o/edge_switchPrior.cpp + vertigo/g2o/types_g2o_robust.cpp + vertigo/g2o/vertex_switchLinear.cpp + ) + ENDIF(WITH_VERTIGO) ENDIF(G2O_FOUND) +IF(GTSAM_FOUND) + ADD_DEFINITIONS("-DWITH_GTSAM") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + gtsam + ) +ENDIF(GTSAM_FOUND) + IF(cvsba_FOUND) ADD_DEFINITIONS("-DWITH_CVSBA") SET(INCLUDE_DIRS diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 233f3b8b..4df24f10 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -48,11 +48,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/solvers/csparse/linear_solver_csparse.h" +#include "g2o/solvers/cholmod/linear_solver_cholmod.h" +#include "g2o/solvers/pcg/linear_solver_pcg.h" #include "g2o/types/slam3d/vertex_se3.h" #include "g2o/types/slam3d/edge_se3.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o/types/slam2d/edge_se2.h" -#endif + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 3> > Slam2dBlockSolver; +typedef g2o::LinearSolverCSparse Slam2dLinearCSparseSolver; +typedef g2o::LinearSolverCholmod Slam2dLinearCholmodSolver; +typedef g2o::LinearSolverPCG Slam2dLinearPCGSolver; + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Slam3dBlockSolver; +typedef g2o::LinearSolverCSparse Slam3dLinearCSparseSolver; +typedef g2o::LinearSolverCholmod Slam3dLinearCholmodSolver; +typedef g2o::LinearSolverPCG Slam3dLinearPCGSolver; + +#include "vertigo/g2o/edge_switchPrior.h" +#include "vertigo/g2o/edge_se2Switchable.h" +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" + +#endif // end WITH_G2O + +#ifdef WITH_GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "vertigo/gtsam/betweenFactorMaxMix.h" +#include "vertigo/gtsam/betweenFactorSwitchable.h" +#include "vertigo/gtsam/switchVariableLinear.h" +#include "vertigo/gtsam/switchVariableSigmoid.h" +#endif // end WITH_GTSAM #ifdef WITH_CVSBA #include @@ -80,16 +119,23 @@ Optimizer * Optimizer::create(const ParametersMap & parameters) UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; case Optimizer::kTypeTORO: default: optimizer = new TOROOptimizer(parameters); - type = Optimizer::kTypeTORO; break; } @@ -103,9 +149,17 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; @@ -119,11 +173,12 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para return optimizer; } -Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) : +Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) : iterations_(iterations), slam2d_(slam2d), covarianceIgnored_(covarianceIgnored), - epsilon_(epsilon) + epsilon_(epsilon), + robust_(robust) { } @@ -131,7 +186,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) : iterations_(Parameters::defaultRGBDOptimizeIterations()), slam2d_(Parameters::defaultRGBDOptimizeSlam2D()), covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()), - epsilon_(Parameters::defaultRGBDOptimizeEpsilon()) + epsilon_(Parameters::defaultRGBDOptimizeEpsilon()), + robust_(Parameters::defaultRGBDOptimizeRobust()) { parseParameters(parameters); } @@ -142,6 +198,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_); Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_); Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_); + Parameters::parse(parameters, Parameters::kRGBDOptimizeRobust(), robust_); } std::map Optimizer::optimize( @@ -373,9 +430,12 @@ std::map TOROOptimizer::optimize( pg3.initializeOptimization(); } - UINFO("TORO iterate begin (iterations=%d)", iterations()); + UINFO("TORO optimizing begin (iterations=%d)", iterations()); double lasterror = 0; - for (int i=0; i0) { @@ -429,7 +489,7 @@ std::map TOROOptimizer::optimize( } // early stop condition - double errorDelta = lasterror - error; + errorDelta = lasterror - error; if(i>0 && errorDelta < this->epsilon()) { UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); @@ -437,7 +497,7 @@ std::map TOROOptimizer::optimize( } lasterror = error; } - UINFO("TORO iterate end"); + UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, errorDelta, timer.ticks()); if(isSlam2d()) { @@ -670,20 +730,77 @@ std::map G2OOptimizer::optimize( { // Apply g2o optimization - // create the linear solver - g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCSparse(); - - // create the block solver on top of the linear solver - g2o::BlockSolverX* blockSolver = new g2o::BlockSolverX(linearSolver); - - // create the algorithm to carry out the optimization - //g2o::OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(blockSolver); - g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver); - - // create the optimizer to load the data and carry out the optimization g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); - optimizer.setAlgorithm(optimizationAlgorithm); + int solverApproach = 0; + int optimizationApproach = 0; + if(isSlam2d()) + { + Slam2dBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + Slam2dLinearPCGSolver * linearSolver = new Slam2dLinearPCGSolver(); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + Slam2dLinearCSparseSolver* linearSolver = new Slam2dLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + else + { + //chmold + Slam2dLinearCholmodSolver * linearSolver = new Slam2dLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } + } + else + { + Slam3dBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + Slam3dLinearPCGSolver * linearSolver = new Slam3dLinearPCGSolver(); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + Slam3dLinearCSparseSolver* linearSolver = new Slam3dLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + else + { + //chmold + Slam3dLinearCholmodSolver * linearSolver = new Slam3dLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } + } + UDEBUG("fill poses to g2o..."); for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) @@ -694,16 +811,25 @@ std::map G2OOptimizer::optimize( { g2o::VertexSE2 * v2 = new g2o::VertexSE2(); v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta())); + if(iter->first == rootId) + { + v2->setFixed(true); + } vertex = v2; } else { g2o::VertexSE3 * v3 = new g2o::VertexSE3(); - Eigen::Isometry3d pose; + Eigen::Affine3d a = iter->second.toEigen3d(); + Eigen::Isometry3d pose; + pose = a.rotation(); pose.translation() = a.translation(); - pose.linear() = a.rotation(); v3->setEstimate(pose); + if(iter->first == rootId) + { + v3->setFixed(true); + } vertex = v3; } vertex->setId(iter->first); @@ -711,6 +837,7 @@ std::map G2OOptimizer::optimize( } UDEBUG("fill edges to g2o..."); + int vertigoVertexId = poses.rbegin()->first+1; for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { int id1 = iter->first; @@ -720,6 +847,32 @@ std::map G2OOptimizer::optimize( g2o::HyperGraph::Edge * edge = 0; + VertexSwitchLinear * v = 0; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + // For loop closure links, add switchable edges + + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + v = new VertexSwitchLinear(); + v->setEstimate(1.0); + v->setId(vertigoVertexId++); + UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str()); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + EdgeSwitchPrior * prior = new EdgeSwitchPrior(); + prior->setMeasurement(1.0); + prior->setVertex(0, v); + UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str()); + } + if(isSlam2d()) { Eigen::Matrix information = Eigen::Matrix::Identity(); @@ -736,16 +889,33 @@ std::map G2OOptimizer::optimize( information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } - g2o::EdgeSE2 * e = new g2o::EdgeSE2(); - g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); - g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE2Switchable * e = new EdgeSE2Switchable(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE2 * e = new g2o::EdgeSE2(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } } else { @@ -757,19 +927,36 @@ std::map G2OOptimizer::optimize( Eigen::Affine3d a = iter->second.transform().toEigen3d(); Eigen::Isometry3d constraint; + constraint = a.rotation(); constraint.translation() = a.translation(); - constraint.linear() = a.rotation(); - g2o::EdgeSE3 * e = new g2o::EdgeSE3(); - g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); - g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(constraint); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE3Switchable * e = new EdgeSE3Switchable(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE3 * e = new g2o::EdgeSE3(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } } if (!optimizer.addEdge(edge)) @@ -780,25 +967,13 @@ std::map G2OOptimizer::optimize( } UDEBUG("Initial optimization..."); - UASSERT(uContains(poses, rootId)); - if(isSlam2d()) - { - g2o::VertexSE2* firstRobotPose = (g2o::VertexSE2*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } - else - { - g2o::VertexSE3* firstRobotPose = (g2o::VertexSE3*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } + optimizer.initializeOptimization(); - UINFO("g2o iterate begin (max iterations=%d)", iterations()); + UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); int it = 0; + UTimer timer; if(intermediateGraphes) { - optimizer.initializeOptimization(); for(int i=0; i 0) @@ -847,18 +1022,17 @@ std::map G2OOptimizer::optimize( if(ULogger::level() == ULogger::kDebug) { optimizer.computeActiveErrors(); - UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } } } else { - optimizer.initializeOptimization(); it = optimizer.optimize(iterations()); optimizer.computeActiveErrors(); - UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } - UINFO("g2o iterate end (%d iterations done)", it); + UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks()); if(isSlam2d()) { @@ -916,6 +1090,312 @@ std::map G2OOptimizer::optimize( return optimizedPoses; } +bool G2OOptimizer::saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints) +{ + FILE * file = 0; + +#ifdef _MSC_VER + fopen_s(&file, fileName.c_str(), "w"); +#else + file = fopen(fileName.c_str(), "w"); +#endif + + if(file) + { + // VERTEX_SE3 id x y z qw qx qy qz + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + Eigen::Quaternionf q = iter->second.getQuaternionf(); + fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n", + iter->first, + iter->second.x(), + iter->second.y(), + iter->second.z(), + q.x(), + q.y(), + q.z(), + q.w()); + } + + //EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 + int virtualVertexId = poses.size()?poses.rbegin()->first+1:0; + for(std::multimap::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + std::string prefix = "EDGE_SE3:QUAT"; + std::string suffix = ""; + + if(useRobustConstraints && iter->second.type() != Link::kNeighbor) + { + prefix = "EDGE_SE3_SWITCHABLE"; + fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId); + fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId); + suffix = uFormat(" %d", virtualVertexId++); + } + + Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); + fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", + prefix.c_str(), + iter->first, + iter->second.to(), + suffix.c_str(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().z(), + q.x(), + q.y(), + q.z(), + q.w(), + iter->second.infMatrix().at(0,0), + iter->second.infMatrix().at(0,1), + iter->second.infMatrix().at(0,2), + iter->second.infMatrix().at(0,3), + iter->second.infMatrix().at(0,4), + iter->second.infMatrix().at(0,5), + iter->second.infMatrix().at(1,1), + iter->second.infMatrix().at(1,2), + iter->second.infMatrix().at(1,3), + iter->second.infMatrix().at(1,4), + iter->second.infMatrix().at(1,5), + iter->second.infMatrix().at(2,2), + iter->second.infMatrix().at(2,3), + iter->second.infMatrix().at(2,4), + iter->second.infMatrix().at(2,5), + iter->second.infMatrix().at(3,3), + iter->second.infMatrix().at(3,4), + iter->second.infMatrix().at(3,5), + iter->second.infMatrix().at(4,4), + iter->second.infMatrix().at(4,5), + iter->second.infMatrix().at(5,5)); + } + UINFO("Graph saved to %s", fileName.c_str()); + fclose(file); + } + else + { + UERROR("Cannot save to file %s", fileName.c_str()); + return false; + } + return true; +} + +////////////////////// +// GTSAM +////////////////////// +bool GTSAMOptimizer::available() +{ +#ifdef WITH_GTSAM + return true; +#else + return false; +#endif +} + +std::map GTSAMOptimizer::optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes) +{ + std::map optimizedPoses; +#ifdef WITH_GTSAM + UDEBUG("Optimizing graph..."); + if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0) + { + gtsam::NonlinearFactorGraph graph; + + //prior first pose + UASSERT(uContains(poses, rootId)); + const Transform & initialPose = poses.at(rootId); + if(isSlam2d()) + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01)); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + } + else + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + } + + UDEBUG("fill poses to gtsam..."); + gtsam::Values initialEstimate; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + UASSERT(!iter->second.isNull()); + if(isSlam2d()) + { + initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); + } + else + { + initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); + } + } + + UDEBUG("fill edges to gtsam..."); + int switchCounter = poses.rbegin()->first+1; + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + int id1 = iter->first; + int id2 = iter->second.to(); + + UASSERT(!iter->second.transform().isNull()); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + double prior = 1.0; + initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + } + + if(isSlam2d()) + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x + information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y + information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta + information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x + information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y + information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta + information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x + information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y + information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta + } + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information = information / 1000.0; + } + + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + } + } + + UDEBUG("create optimizer"); + gtsam::GaussNewtonParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + //gtsam::LevenbergMarquardtParams parametersLev; + //parametersLev.relativeErrorTol = epsilon(); + //parametersLev.maxIterations = iterations(); + //gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev); + //gtsam::DoglegParams parametersDogleg; + //parametersDogleg.relativeErrorTol = epsilon(); + //parametersDogleg.maxIterations = iterations(); + //gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg); + + UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); + UTimer timer; + for(int i=0; i 0) + { + std::map tmpPoses; + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + intermediateGraphes->push_back(tmpPoses); + } + try + { + optimizer.iterate(); + } + catch(gtsam::IndeterminantLinearSystemException & e) + { + UERROR("GTSAM exception catched: %s", e.what()); + return optimizedPoses; + } + UDEBUG("iteration %d error =%f", i+1, optimizer.error()); + if(optimizer.error() < epsilon()) + { + break; + } + } + UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks()); + + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + } + UDEBUG("Optimizing graph...end!"); +#else + UERROR("Not built with GTSAM support!"); +#endif + return optimizedPoses; +} + ////////////////////// // cvsba ////////////////////// diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index b310d319..b3ec5bd1 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -742,6 +742,14 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, { graph::TOROOptimizer::saveGraph(path, poses, constraints); } + else if(type == 4) // g2o + { +#ifdef WITH_G2O + graph::G2OOptimizer::saveGraph(path, poses, constraints); +#else + UERROR("Cannot export in g2o format because RTAB-Map is not built with g2o support!"); +#endif + } else { //get timestamps @@ -2680,7 +2688,6 @@ void Rtabmap::optimizeCurrentMap( std::multimap * constraints) const { //Optimize the map - optimizedPoses.clear(); UINFO("Optimize map: around location %d", id); if(_memory && id > 0) { @@ -2692,14 +2699,23 @@ void Rtabmap::optimizeCurrentMap( } UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks()); - optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); - - if(_memory->getSignature(id) && uContains(optimizedPoses, id)) - { - Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); - UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); - } + std::map poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); UINFO("optimize time %f s", timer.ticks()); + + if(poses.size()) + { + optimizedPoses = poses; + + if(_memory->getSignature(id) && uContains(optimizedPoses, id)) + { + Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); + UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); + } + } + else + { + UERROR("Failed to optimize the graph! Keeping the graph without optimization..."); + } } } diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index cb025873..7678e69b 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -66,6 +66,12 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y *this = fromEigen3f(t); } +Transform::Transform(float x, float y, float theta) +{ + Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta); + *this = fromEigen3f(t); +} + bool Transform::isNull() const { return (data()[0] == 0.0f && diff --git a/corelib/src/sqlite3/readme.txt b/corelib/src/sqlite3/readme.txt new file mode 100644 index 00000000..5f1dcc60 --- /dev/null +++ b/corelib/src/sqlite3/readme.txt @@ -0,0 +1,3 @@ + +Info: https://www.sqlite.org/ +License: Public domain (https://www.sqlite.org/copyright.html) \ No newline at end of file diff --git a/corelib/src/toro3d/readme.txt b/corelib/src/toro3d/readme.txt new file mode 100644 index 00000000..8de8c6e8 --- /dev/null +++ b/corelib/src/toro3d/readme.txt @@ -0,0 +1,3 @@ + +Info: https://www.openslam.org/toro.html +License: Creative Commons (Attribution-NonCommercial-ShareAlike) \ No newline at end of file diff --git a/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp new file mode 100644 index 00000000..41407b83 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp @@ -0,0 +1,122 @@ +/* + * edge_se2MaxMixture.cpp + * + * Created on: 12.06.2012 + * Author: niko + */ + + +#include "edge_se2MaxMixture.h" + +#include +using namespace std; +using namespace Eigen; + +// ================================================ +EdgeSE2MaxMixture::EdgeSE2MaxMixture() : g2o::EdgeSE2::EdgeSE2() +{ + nullHypothesisMoreLikely = false; +} + +// ================================================ +bool EdgeSE2MaxMixture::read(std::istream& is) + { + Vector3d p; + is >> weight >> p[0] >> p[1] >> p[2]; + setMeasurement(g2o::SE2(p)); + _inverseMeasurement = measurement().inverse(); + //measurement().fromVector(p); + //inverseMeasurement() = measurement().inverse(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + + information_constraint = _information; + nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant()); + information_nullHypothesis = information_constraint*weight; + nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant()); + + return true; + } +// ================================================ +bool EdgeSE2MaxMixture::write(std::ostream& os) const +{ + Vector3d p = measurement().toVector(); + os << p.x() << " " << p.y() << " " << p.z(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE2MaxMixture::linearizeOplus() +{ + g2o::EdgeSE2::linearizeOplus(); + if (nullHypothesisMoreLikely) { + _jacobianOplusXi *= weight; + _jacobianOplusXj *= weight; + } +} + +// ================================================ +void EdgeSE2MaxMixture::computeError() +{ + // calculate the error for this constraint + g2o::EdgeSE2::computeError(); + + // determine the likelihood for constraint and null hypothesis + double mahal_constraint = _error.transpose() * information_constraint * _error; + double likelihood_constraint = nu_constraint * exp(-mahal_constraint); + + double mahal_nullHypothesis = _error.transpose() * (information_nullHypothesis) * _error; + double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis); + + // if the nullHypothesis is more likely ... + if (likelihood_nullHypothesis > likelihood_constraint) { + _information = information_nullHypothesis; + nullHypothesisMoreLikely = true; + } + else { + _information = information_constraint; + nullHypothesisMoreLikely = false; + } + + +} + + +// ================================================ +#ifdef G2O_HAVE_OPENGL + EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){} + + g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE2MaxMixture* e = static_cast(element); + + + g2o::VertexSE2* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE2* toEdge = static_cast(e->vertices()[1]); + + + if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0); + else glColor3f(1.0,0.5,0.2); + + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); + glEnd(); + glPopAttrib(); + return this; + } +#endif + + + diff --git a/corelib/src/vertigo/g2o/edge_se2MaxMixture.h b/corelib/src/vertigo/g2o/edge_se2MaxMixture.h new file mode 100644 index 00000000..48d134fe --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2MaxMixture.h @@ -0,0 +1,46 @@ +/* + * edge_se2MaxMixture.h + * + * Created on: 12.06.2012 + * Author: niko + */ + +#ifndef EDGE_SE2MAXMIXTURE_H_ +#define EDGE_SE2MAXMIXTURE_H_ + +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/types/slam2d/edge_se2.h" + + +class EdgeSE2MaxMixture : public g2o::EdgeSE2 +{ + public: + EdgeSE2MaxMixture(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + double weight; + + bool nullHypothesisMoreLikely; + + InformationType information_nullHypothesis; + double nu_nullHypothesis; + InformationType information_constraint; + double nu_constraint ; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE2MaxMixtureDrawAction: public g2o::DrawAction{ + public: + EdgeSE2MaxMixtureDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + +#endif /* EDGE_SE2MAXMIXTURE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp new file mode 100644 index 00000000..1e6deffd --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp @@ -0,0 +1,131 @@ +/* + * edge_se2Switchable.cpp + * + * Created on: 13.07.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/edge_se2Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" +#include + +using namespace std; +using namespace Eigen; + + +// ================================================ +EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>() +{ + resize(3); + _jacobianOplus[0].resize(3,3); + _jacobianOplus[1].resize(3,3); + _jacobianOplus[2].resize(3,1); + +} +// ================================================ +bool EdgeSE2Switchable::read(std::istream& is) + { + Vector3d p; + is >> p[0] >> p[1] >> p[2]; + setMeasurement(g2o::SE2(p)); + _inverseMeasurement = measurement().inverse(); + + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } +// ================================================ +bool EdgeSE2Switchable::write(std::ostream& os) const +{ + Vector3d p = measurement().toVector(); + os << p.x() << " " << p.y() << " " << p.z(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE2Switchable::linearizeOplus() +{ + + const g2o::VertexSE2* vi = static_cast(_vertices[0]); + const g2o::VertexSE2* vj = static_cast(_vertices[1]); + const VertexSwitchLinear* vSwitch = static_cast(_vertices[2]); + + double thetai = vi->estimate().rotation().angle(); + + Vector2d dt = vj->estimate().translation() - vi->estimate().translation(); + double si=sin(thetai), ci=cos(thetai); + + _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y(); + _jacobianOplus[0](1, 0) = si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y(); + _jacobianOplus[0](2, 0) = 0; _jacobianOplus[0](2, 1) = 0; _jacobianOplus[0](2, 2) = -1; + + _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0; + _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0; + _jacobianOplus[1](2, 0) = 0; _jacobianOplus[1](2, 1)= 0; _jacobianOplus[1](2, 2)= 1; + + const g2o::SE2& rmean = _inverseMeasurement; + Matrix3d z = Matrix3d::Zero(); + z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix(); + z(2, 2) = 1.; + _jacobianOplus[0] = z * _jacobianOplus[0]; + _jacobianOplus[1] = z * _jacobianOplus[1]; + + + _jacobianOplus[0]*=vSwitch->estimate(); + _jacobianOplus[1]*=vSwitch->estimate(); + + + // derivative w.r.t switch vertex + _jacobianOplus[2].setZero(); + g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate()); + _jacobianOplus[2] = delta.toVector() * vSwitch->gradient(); +} + +// ================================================ +void EdgeSE2Switchable::computeError() +{ + const g2o::VertexSE2* v1 = static_cast(_vertices[0]); + const g2o::VertexSE2* v2 = static_cast(_vertices[1]); + const VertexSwitchLinear* v3 = static_cast(_vertices[2]); + + g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); + _error = delta.toVector() * v3->estimate(); +} + + +#ifdef G2O_HAVE_OPENGL + EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){} + + g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE2Switchable* e = static_cast(element); + + + g2o::VertexSE2* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE2* toEdge = static_cast(e->vertices()[1]); + VertexSwitchLinear* s = static_cast(e->vertices()[2]); + + glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1); + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); + glEnd(); + glPopAttrib(); + return this; + } +#endif + diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.h b/corelib/src/vertigo/g2o/edge_se2Switchable.h new file mode 100644 index 00000000..a4004566 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2Switchable.h @@ -0,0 +1,50 @@ +/* + * edge_se2Switchable.h + * + * Created on: 13.07.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#ifndef EDGE_SE2SWITCHABLE_H_ +#define EDGE_SE2SWITCHABLE_H_ + +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/core/base_multi_edge.h" +#include "g2o/core/hyper_graph_action.h" + +class EdgeSE2Switchable : public g2o::BaseMultiEdge<3, g2o::SE2> +{ + public: + EdgeSE2Switchable(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + + virtual void setMeasurement(const g2o::SE2& m){ + _measurement = m; + _inverseMeasurement = m.inverse(); + } + + protected: + g2o::SE2 _inverseMeasurement; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE2SwitchableDrawAction: public g2o::DrawAction{ + public: + EdgeSE2SwitchableDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + + +#endif /* EDGE_SE2SWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp new file mode 100644 index 00000000..d5b8a4ed --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp @@ -0,0 +1,120 @@ +/* + * edge_se3Switchable.cpp + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" +#include +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d/isometry3d_gradients.h" + + +using namespace std; +using namespace Eigen; + + +// ================================================ +EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>() +{ + resize(3); + _jacobianOplus[0].resize(6,6); + _jacobianOplus[1].resize(6,6); + _jacobianOplus[2].resize(6,1); + +} +// ================================================ +bool EdgeSE3Switchable::read(std::istream& is) + { + g2o::Vector7d meas; + for (int i=0; i<7; i++) + is >> meas[i]; + // normalize the quaternion to recover numerical precision lost by storing as human readable text + Vector4d::MapType(meas.data()+3).normalize(); + setMeasurement(g2o::internal::fromVectorQT(meas)); + + for (int i=0; i<6; i++) + for (int j=i; j<6; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i) = information()(i,j); + } + return true; + + } +// ================================================ +bool EdgeSE3Switchable::write(std::ostream& os) const +{ + g2o::Vector7d meas = g2o::internal::toVectorQT(measurement()); + for (int i=0; i<7; i++) os << meas[i] << " "; + for (int i = 0; i < 6; ++i) + for (int j = i; j < 6; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE3Switchable::linearizeOplus() +{ + + g2o::VertexSE3* from = static_cast(_vertices[0]); + g2o::VertexSE3* to = static_cast(_vertices[1]); + const VertexSwitchLinear* vSwitch = static_cast(_vertices[2]); + + Eigen::Isometry3d E; + const Eigen::Isometry3d& Xi=from->estimate(); + const Eigen::Isometry3d& Xj=to->estimate(); + const Eigen::Isometry3d& Z=_measurement; + g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj); + + _jacobianOplus[0]*=vSwitch->estimate(); + _jacobianOplus[1]*=vSwitch->estimate(); + + // derivative w.r.t switch vertex + _jacobianOplus[2].setZero(); + _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient(); +} + + +// ================================================ +void EdgeSE3Switchable::computeError() +{ + const g2o::VertexSE3* v1 = dynamic_cast(_vertices[0]); + const g2o::VertexSE3* v2 = dynamic_cast(_vertices[1]); + const VertexSwitchLinear* v3 = static_cast(_vertices[2]); + + Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); + _error = g2o::internal::toVectorMQT(delta) * v3->estimate(); +} + + +#ifdef G2O_HAVE_OPENGL + EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){} + + g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE3Switchable* e = static_cast(element); + + + g2o::VertexSE3* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE3* toEdge = static_cast(e->vertices()[1]); + VertexSwitchLinear* s = static_cast(e->vertices()[2]); + + glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1); + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z()); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z()); + glEnd(); + glPopAttrib(); + return this; + } +#endif diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.h b/corelib/src/vertigo/g2o/edge_se3Switchable.h new file mode 100644 index 00000000..ee636edd --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se3Switchable.h @@ -0,0 +1,48 @@ +/* + * edge_se3Switchable.h + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#ifndef EDGE_SE3SWITCHABLE_H_ +#define EDGE_SE3SWITCHABLE_H_ + +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/core/base_multi_edge.h" +#include "g2o/core/hyper_graph_action.h" + +class EdgeSE3Switchable : public g2o::BaseMultiEdge<6, Eigen::Isometry3d> +{ + public: + EdgeSE3Switchable(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + virtual void setMeasurement(const Eigen::Isometry3d& m){ + _measurement = m; + _inverseMeasurement = m.inverse(); + } + + protected: + Eigen::Isometry3d _inverseMeasurement; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE3SwitchableDrawAction: public g2o::DrawAction{ + public: + EdgeSE3SwitchableDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + +#endif /* EDGE_SE3SWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_switchPrior.cpp b/corelib/src/vertigo/g2o/edge_switchPrior.cpp new file mode 100644 index 00000000..0af37b45 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_switchPrior.cpp @@ -0,0 +1,44 @@ +#include "vertigo/g2o/edge_switchPrior.h" +using namespace std; + + EdgeSwitchPrior::EdgeSwitchPrior() + { + setMeasurement(1.0); + } + + bool EdgeSwitchPrior::read(std::istream &is) + { + double new_measurement; + is >> new_measurement; + + setMeasurement(new_measurement); + + is >> information()(0,0); + return true; + } + + bool EdgeSwitchPrior::write(std::ostream &os) const + { + os << measurement() << " " << information()(0,0); + return true; + } + + void EdgeSwitchPrior::setMeasurement(const double & m) + { + g2o::BaseEdge<1, double>::setMeasurement(m); + information()(0,0) = 1.0; + } + + void EdgeSwitchPrior::linearizeOplus() + { + _jacobianOplusXi[0]=-1.0; + } + + void EdgeSwitchPrior::computeError() + { + const VertexSwitchLinear* s = static_cast(_vertices[0]); + + _error[0] = measurement() - s->x(); + + } + diff --git a/corelib/src/vertigo/g2o/edge_switchPrior.h b/corelib/src/vertigo/g2o/edge_switchPrior.h new file mode 100644 index 00000000..b4505ffb --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_switchPrior.h @@ -0,0 +1,22 @@ +#pragma once + + +#include "vertex_switchLinear.h" +#include "g2o/core/base_unary_edge.h" + + + +class EdgeSwitchPrior : public g2o::BaseUnaryEdge<1, double, VertexSwitchLinear> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSwitchPrior(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual void setMeasurement(const double & m); + + virtual void linearizeOplus(); + void computeError(); +}; diff --git a/corelib/src/vertigo/g2o/types_g2o_robust.cpp b/corelib/src/vertigo/g2o/types_g2o_robust.cpp new file mode 100644 index 00000000..f1553f5f --- /dev/null +++ b/corelib/src/vertigo/g2o/types_g2o_robust.cpp @@ -0,0 +1,22 @@ +#include "g2o/core/factory.h" +#include "g2o/stuff/macros.h" + +#include "vertigo/g2o/edge_switchPrior.h" +#include "vertigo/g2o/edge_se2Switchable.h" +//#include "vertigo/g2o/edge_se2MaxMixture.h" +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" + + +G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior); +G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable); +//G2O_REGISTER_TYPE(EDGE_SE2_MAXMIX, EdgeSE2MaxMixture); +G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable); +G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear); + +#ifdef G2O_HAVE_OPENGL + G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction); + //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction); + G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction); +#endif + diff --git a/corelib/src/vertigo/g2o/vertex_switchLinear.cpp b/corelib/src/vertigo/g2o/vertex_switchLinear.cpp new file mode 100644 index 00000000..de4c503f --- /dev/null +++ b/corelib/src/vertigo/g2o/vertex_switchLinear.cpp @@ -0,0 +1,59 @@ +/* + * vertex_switchLinear.cpp + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/vertex_switchLinear.h" +#include + +using namespace std; + + VertexSwitchLinear::VertexSwitchLinear() : + _x(0) + { + setToOrigin(); + setEstimate(1.0); + } + + bool VertexSwitchLinear:: read(std::istream& is) + { + is >> _x; + _estimate=_x; + + return true; + } + + bool VertexSwitchLinear::write(std::ostream& os) const + { + os << _x; + return os.good(); + } + + void VertexSwitchLinear::setToOriginImpl() + { + _x=0; + _estimate=_x; + } + + + void VertexSwitchLinear::setEstimate(const double &et) + { + _x=et; + _estimate=_x; + } + + + void VertexSwitchLinear::oplusImpl(const double* update) + { + _x += update[0]; + + if (_x<0) _x=0; + if (_x>1) _x=1; + + _estimate=_x; + } diff --git a/corelib/src/vertigo/g2o/vertex_switchLinear.h b/corelib/src/vertigo/g2o/vertex_switchLinear.h new file mode 100644 index 00000000..7fa7edeb --- /dev/null +++ b/corelib/src/vertigo/g2o/vertex_switchLinear.h @@ -0,0 +1,43 @@ +/* + * vertex_switchLinear.h + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + + +#pragma once + +#include "g2o/core/base_vertex.h" +#include + + + + class VertexSwitchLinear : public g2o::BaseVertex<1, double> + { + + public: + VertexSwitchLinear(); + + virtual void setToOriginImpl(); + + virtual void oplusImpl(const double* update); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + virtual void setEstimate(const double &et); + + + double x() const { return _x; }; + + + //! The gradient at the current estimate is always 1; + double gradient() const { return 1; } ; + + private: + double _x; + + }; diff --git a/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h new file mode 100644 index 00000000..789609d5 --- /dev/null +++ b/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h @@ -0,0 +1,67 @@ +/* + * betweenFactorMaxMix.h + * + * Created on: 14.08.2012 + * Author: niko + */ + +#ifndef BETWEENFACTORMAXMIX_H_ +#define BETWEENFACTORMAXMIX_H_ + +#include +#include + +namespace vertigo { + + template + class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2 + { + public: + BetweenFactorMaxMix() : weight(0.0) {}; + BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w) + : gtsam::NoiseModelFactor2(model, key1, key2), weight(w), nullHypothesisModel(model2), + betweenFactor(key1, key2, measured, model) { }; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + + + + // which hypothesis is more likely + double m1 = this->noiseModel_->distance(error); + gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_; + gtsam::Matrix info1(g1->R().transpose()*g1->R()); + double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); + double l1 = nu1 * exp(-0.5*m1); + + double m2 = nullHypothesisModel->distance(error); + gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel; + gtsam::Matrix info2(g2->R().transpose()*g2->R()); + double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant()); + double l2 = nu2 * exp(-0.5*m2); + + // if the null hypothesis is more likely, than proceed by applying the weight ... + if (l2>l1) { + if (H1) *H1 = *H1 * weight; + if (H2) *H2 = *H2 * weight; + error *= sqrt(weight); + } + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + gtsam::SharedNoiseModel nullHypothesisModel; + double weight; + + }; +} + + +#endif /* BETWEENFACTORMAXMIX_H_ */ diff --git a/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h new file mode 100644 index 00000000..ab5a4438 --- /dev/null +++ b/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h @@ -0,0 +1,97 @@ +/* + * betweenFactorSwitchable.h + * + * Created on: 02.08.2012 + * Author: niko + */ + +#ifndef BETWEENFACTORSWITCHABLE_H_ +#define BETWEENFACTORSWITCHABLE_H_ + +#include + +#include +using std::cout; +using std::endl; + +#include "switchVariableLinear.h" +#include "switchVariableSigmoid.h" + + +namespace vertigo { + + template + class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3 + { + public: + BetweenFactorSwitchableLinear() {}; + BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor3(model, key1, key2, key3), + betweenFactor(key1, key2, measured, model) {}; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + error *= s.value(); + + // handle derivatives + if (H1) *H1 = *H1 * s.value(); + if (H2) *H2 = *H2 * s.value(); + if (H3) *H3 = error; + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + + }; + + + + template + class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3 + { + public: + BetweenFactorSwitchableSigmoid() {}; + BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor3(model, key1, key2, key3), + betweenFactor(key1, key2, measured, model) {}; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + + + double w = sigmoid(s.value()); + error *= w; + + // handle derivatives + if (H1) *H1 = *H1 * w; + if (H2) *H2 = *H2 * w; + if (H3) *H3 = error /* (w*(1.0-w))*/; // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + + double sigmoid(double x) const { + return 1.0/(1.0+exp(-x)); + } + }; + +} + +#endif /* BETWEENFACTORSWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/gtsam/switchVariableLinear.h b/corelib/src/vertigo/gtsam/switchVariableLinear.h new file mode 100644 index 00000000..34213884 --- /dev/null +++ b/corelib/src/vertigo/gtsam/switchVariableLinear.h @@ -0,0 +1,130 @@ +/* + * switchVariableLinear.h + * + * Created on: 02.08.2012 + * Author: niko + */ + +#ifndef SWITCHVARIABLELINEAR_H_ +#define SWITCHVARIABLELINEAR_H_ + +#pragma once + +#include +#include + +namespace vertigo { + + /** + * SwitchVariableLinear is a wrapper around double to allow it to be a Lie type + */ + struct SwitchVariableLinear : public gtsam::DerivedValue { + + /** default constructor */ + SwitchVariableLinear() : d_(0.0) {}; + + /** wrap a double */ + SwitchVariableLinear(double d) : d_(d) { + // if (d_ < 0.0) d_=0.0; + // else if(d_>1.0) d_=1.0; + }; + + /** access the underlying value */ + double value() const { return d_; } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } + + /** equality up to tolerance */ + inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } + + // Manifold requirements + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } + + /** Update the SwitchVariableLinear with a tangent space update */ + inline SwitchVariableLinear retract(const gtsam::Vector& v) const { + double x = value() + v(0); + + if (x>1.0) x=1.0; + else if (x<0.0) x=0.0; + + return SwitchVariableLinear(x); + } + + /** @return the local coordinates of another object */ + inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); } + + // Group requirements + + /** identity */ + inline static SwitchVariableLinear identity() { + return SwitchVariableLinear(); + } + + /** compose with another object */ + inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const { + return SwitchVariableLinear(d_ + p.d_); + } + + /** between operation */ + inline SwitchVariableLinear between(const SwitchVariableLinear& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::eye(1); + if(H2) *H2 = gtsam::eye(1); + return SwitchVariableLinear(l2.value() - value()); + } + + /** invert the object and yield a new one */ + inline SwitchVariableLinear inverse() const { + return SwitchVariableLinear(-1.0 * value()); + } + + // Lie functions + + /** Expmap around identity */ + static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); } + + /** Logmap around identity - just returns with default cast back */ + static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); } + + private: + double d_; + }; +} + +namespace gtsam { +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") { + key.print(str); + } + static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) { + return key1.equals(key2, tol); + } + static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();} + + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef gtsam::Vector TangentVector; + static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other); + } + static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return g.retract(v); + } +}; +} + + + +#endif /* SWITCHVARIABLELINEAR_H_ */ diff --git a/corelib/src/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/vertigo/gtsam/switchVariableSigmoid.h new file mode 100644 index 00000000..3a86790b --- /dev/null +++ b/corelib/src/vertigo/gtsam/switchVariableSigmoid.h @@ -0,0 +1,129 @@ +/* + * switchVariableSigmoid.h + * + * Created on: 08.08.2012 + * Author: niko + */ + +#ifndef SWITCHVARIABLESIGMOID_H_ +#define SWITCHVARIABLESIGMOID_H_ + +#pragma once + +#include +#include + +namespace vertigo { + + /** + * SwitchVariableSigmoid is a wrapper around double to allow it to be a Lie type + */ + struct SwitchVariableSigmoid : public gtsam::DerivedValue { + + /** default constructor */ + SwitchVariableSigmoid() : d_(10.0) {}; + + /** wrap a double */ + SwitchVariableSigmoid(double d) : d_(d) { + if (d_ < -10.0) d_=-10.0; + else if(d_>10.0) d_=10.0; + }; + + /** access the underlying value */ + double value() const { return d_; } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } + + /** equality up to tolerance */ + inline bool equals(const SwitchVariableSigmoid& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } + + // Manifold requirements + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } + + /** Update the SwitchVariableSigmoid with a tangent space update */ + inline SwitchVariableSigmoid retract(const gtsam::Vector& v) const { + double x = value() + v(0); + + if (x>10.0) x=10.0; + else if (x<-10.0) x=-10.0; + + return SwitchVariableSigmoid(x); + } + + /** @return the local coordinates of another object */ + inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); } + + // Group requirements + + /** identity */ + inline static SwitchVariableSigmoid identity() { + return SwitchVariableSigmoid(); + } + + /** compose with another object */ + inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const { + return SwitchVariableSigmoid(d_ + p.d_); + } + + /** between operation */ + inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::eye(1); + if(H2) *H2 = gtsam::eye(1); + return SwitchVariableSigmoid(l2.value() - value()); + } + + /** invert the object and yield a new one */ + inline SwitchVariableSigmoid inverse() const { + return SwitchVariableSigmoid(-1.0 * value()); + } + + // Lie functions + + /** Expmap around identity */ + static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); } + + /** Logmap around identity - just returns with default cast back */ + static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); } + + private: + double d_; + }; +} + + +namespace gtsam { +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") { + key.print(str); + } + static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) { + return key1.equals(key2, tol); + } + static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();} + + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef gtsam::Vector TangentVector; + static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other); + } + static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return g.retract(v); + } +}; +} + +#endif /* SWITCHVARIABLESIGMOID_H_ */ diff --git a/corelib/src/vertigo/readme.txt b/corelib/src/vertigo/readme.txt new file mode 100644 index 00000000..774a679f --- /dev/null +++ b/corelib/src/vertigo/readme.txt @@ -0,0 +1,8 @@ + +Info: http://openslam.org/vertigo.html +Source: https://github.com/christiankerl/vertigo/tree/master/trunk +Commit: fbd438488a56cdba805fa2f75aa3e394e3eda7ff +License: GPL v3 + +Tested with g2o (ROS Indigo/2014.02.18) +Tested with GTSAM commit c73b835 \ No newline at end of file diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index b3aac23e..3708e832 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -82,6 +82,7 @@ private slots: void extractImages(); void generateLocalGraph(); void generateTOROGraph(); + void generateG2OGraph(); void view3DMap(); void generate3DMap(); void detectMoreLoopClosures(); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 516740eb..c6b0dc37 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -132,6 +132,7 @@ private slots: void exportPosesRGBDSLAM(); void exportPosesKITTI(); void exportPosesTORO(); + void exportPosesG2O(); void postProcessing(); void deleteMemory(); void openWorkingDirectory(); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 18f270a2..8a2bdedc 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -301,7 +301,7 @@ private: void addParameter(const QObject * object, double value); void addParameter(const QObject * object, const QString & value); void addParameters(const QObjectList & children); - void addParameters(const QStackedWidget * stackedWidget); + void addParameters(const QStackedWidget * stackedWidget, int panel = -1); void addParameters(const QGroupBox * box); QList getGroupBoxes(); void readSettingsBegin(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index a3ccf431..af2d2927 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -61,6 +61,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No"); + _ui->label_gtsam->setText(graph::GTSAMOptimizer::available()?"Yes":"No"); _ui->label_cvsba->setText(graph::CVSBAOptimizer::available()?"Yes":"No"); } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 55e73d57..e41ed67d 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -127,6 +127,19 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : ui_->comboBox_graphOptimizer->setCurrentIndex(0); } } + if(!graph::GTSAMOptimizer::available()) + { + ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1); + if(ui_->comboBox_graphOptimizer->currentIndex() == 2) + { + UWARN("GTSAM is not available, setting optimization default to TORO."); + ui_->comboBox_graphOptimizer->setCurrentIndex(0); + } + } + if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available()) + { + ui_->checkBox_robust->setEnabled(false); + } ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction()); ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction()); @@ -146,6 +159,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph())); connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph())); connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph())); + connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph())); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available()); 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())); @@ -168,6 +183,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : ui_->pushButton_reject->setEnabled(false); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); ui_->horizontalSlider_A->setTracking(false); ui_->horizontalSlider_B->setTracking(false); @@ -198,6 +214,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); + connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); @@ -239,6 +256,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : // Graph view connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified())); + connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified())); @@ -358,6 +376,7 @@ void DatabaseViewer::readSettings() settings.beginGroup("optimization"); ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt()); ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool()); + ui_->checkBox_robust->setChecked(settings.value("robust", ui_->checkBox_robust->isChecked()).toBool()); ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool()); ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool()); ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool()); @@ -451,6 +470,7 @@ void DatabaseViewer::writeSettings() settings.beginGroup("optimization"); settings.setValue("iterations", ui_->spinBox_iterations->value()); settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()); + settings.setValue("robust", ui_->checkBox_robust->isChecked()); settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()); settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()); settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()); @@ -556,7 +576,9 @@ bool DatabaseViewer::openDatabase(const QString & path) linksRefined_.clear(); linksRemoved_.clear(); localMaps_.clear(); + ui_->graphViewer->clearAll(); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); ui_->checkBox_showOptimized->setEnabled(false); databaseFileName_.clear(); } @@ -1057,6 +1079,7 @@ void DatabaseViewer::updateIds() } ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); graphes_.clear(); graphLinks_.clear(); neighborLinks_.clear(); @@ -1193,7 +1216,51 @@ void DatabaseViewer::generateTOROGraph() QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)")); if(!path.isEmpty()) { - graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_); + if(ui_->checkBox_ignoreCovariance->isChecked()) + { + std::multimap links = graphLinks_; + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1)); + } + graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links); + } + else + { + graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_); + } + } + } +} + +void DatabaseViewer::generateG2OGraph() +{ + if(!graphes_.size() || !graphLinks_.size()) + { + QMessageBox::warning(this, tr("Cannot generate a g2o graph"), tr("No poses or no links...")); + return; + } + bool ok = false; + int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, (int)graphes_.size()-1, 1, &ok); + + if(ok) + { + QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".g2o", tr("g2o file (*.g2o)")); + if(!path.isEmpty()) + { + if(ui_->checkBox_ignoreCovariance->isChecked()) + { + std::multimap links = graphLinks_; + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1)); + } + graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links, ui_->checkBox_robust->isChecked()); + } + else + { + graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_, ui_->checkBox_robust->isChecked()); + } } } } @@ -2604,6 +2671,7 @@ void DatabaseViewer::updateGraphView() graphes_.push_back(poses); ui_->actionGenerate_TORO_graph_graph->setEnabled(true); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(true); std::multimap links = links_; // filter current map if not spanning to all maps @@ -2697,13 +2765,32 @@ void DatabaseViewer::updateGraphView() ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4)").arg(totalGlobal).arg(totalLocalSpace).arg(totalLocalTime).arg(totalUser)); graph::Optimizer * optimizer = 0; - if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O) + if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM) { - optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked()); + optimizer = new graph::GTSAMOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0, + ui_->checkBox_robust->isChecked()); + } + else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O) + { + UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0); + optimizer = new graph::G2OOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0, + ui_->checkBox_robust->isChecked()); } else { - optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked()); + optimizer = new graph::TOROOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0); } std::map posesOut; std::multimap linksOut; @@ -2723,6 +2810,10 @@ void DatabaseViewer::updateGraphView() graphLinks_ = linksOut; ui_->label_nodes->setNum((int)finalPoses.size()); delete optimizer; + if(posesOut.size() && finalPoses.empty()) + { + QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors.")); + } } if(graphes_.size()) { @@ -3006,7 +3097,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update } if(!updated) { - linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + linksRefined_.insert(std::make_pair(newLink.from(), newLink)); if(updateGraph) { @@ -3129,7 +3220,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo } if(!updated) { - linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + linksRefined_.insert(std::make_pair(newLink.from(), newLink)); if(updateGraph) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index d5e953db..a57cf406 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -308,6 +308,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM())); connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI())); connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO())); + connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O())); + _ui->actionG2o_g2o->setVisible(graph::G2OOptimizer::available()); connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory())); connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph())); @@ -3178,6 +3180,10 @@ void MainWindow::exportPosesTORO() { exportPoses(3); } +void MainWindow::exportPosesG2O() +{ + exportPoses(4); +} void MainWindow::exportPoses(int format) { @@ -3215,14 +3221,14 @@ void MainWindow::exportPoses(int format) if(_exportPosesFileName[format].isEmpty()) { - _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":"poses.txt"); + _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt"); } QString path = QFileDialog::getSaveFileName( this, tr("Save File"), _exportPosesFileName[format], - format == 3?tr("TORO file (*.graph)"):tr("Text file (*.txt)")); + format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)")); if(!path.isEmpty()) { @@ -3233,7 +3239,7 @@ void MainWindow::exportPoses(int format) _ui->dockWidget_console->show(); _ui->widget_console->appendMsg( QString("%1 saved (global=%2, optimized=%3)... %4") - .arg(format == 3?"TORO graph":"Poses") + .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses") .arg(global?"true":"false") .arg(optimized?"true":"false") .arg(_exportPosesFileName[format])); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 5d211b04..83bb2e72 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -176,6 +176,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1); } + if(!graph::GTSAMOptimizer::available()) + { + _ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1); + } + if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available()) + { + _ui->graphOptimization_robust->setEnabled(false); + } if(!CameraOpenni::available()) { _ui->comboBox_cameraRGBD->setItemData(0, 0, Qt::UserRole - 1); @@ -571,6 +579,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str()); _ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str()); + _ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str()); @@ -1854,7 +1863,7 @@ bool PreferencesDialog::validateForm() // optimization strategy if(!graph::G2OOptimizer::available()) { - if(_ui->graphOptimization_type->currentIndex() > 0) + if(_ui->graphOptimization_type->currentIndex() == 1) { QMessageBox::warning(this, tr("Parameter warning"), tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built " @@ -1862,6 +1871,16 @@ bool PreferencesDialog::validateForm() _ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO); } } + if(!graph::GTSAMOptimizer::available()) + { + if(_ui->graphOptimization_type->currentIndex() == 2) + { + QMessageBox::warning(this, tr("Parameter warning"), + tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built " + "with GTSAM. TORO is set instead for graph optimization strategy.")); + _ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO); + } + } //verify binary features and nearest neighbor // BOW dictionary type @@ -2487,6 +2506,17 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string ok = false; } } + if(!graph::GTSAMOptimizer::available()) + { + if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0) + { + UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built " + "with GTSAM. Keeping default combo value: %s.", + combo->objectName().toStdString().c_str(), + combo->currentText().toStdString().c_str()); + ok = false; + } + } if(ok) { combo->setCurrentIndex(valueInt); @@ -2650,6 +2680,22 @@ void PreferencesDialog::addParameter(const QObject * object, int value) this->addParameters(_ui->groupBox_loopClosure_icp2); } } + else if(comboBox == _ui->loopClosure_estimationType) + { + this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex()); + } + else if(comboBox == _ui->odom_estimationType) + { + this->addParameters(_ui->stackedWidget_odomEstimation, _ui->stackedWidget_odomEstimation->currentIndex()); + } + else if(comboBox == _ui->graphOptimization_type) + { + this->addParameter(_ui->graphOptimization_iterations, _ui->graphOptimization_iterations->value()); + this->addParameter(_ui->graphOptimization_covarianceIgnored, _ui->graphOptimization_covarianceIgnored->isChecked()); + this->addParameter(_ui->graphOptimization_slam2d, _ui->graphOptimization_slam2d->isChecked()); + this->addParameter(_ui->graphOptimization_stopEpsilon, _ui->graphOptimization_stopEpsilon->value()); + this->addParameter(_ui->graphOptimization_robust, _ui->graphOptimization_robust->isChecked()); + } } // Add parameter _parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString())); @@ -2816,13 +2862,22 @@ void PreferencesDialog::addParameters(const QObjectList & children) } } -void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget) +void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget, int panel) { if(stackedWidget) { - for(int i=0; icount(); ++i) + if(panel == -1) { - const QObjectList & children = stackedWidget->widget(i)->children(); + for(int i=0; icount(); ++i) + { + const QObjectList & children = stackedWidget->widget(i)->children(); + addParameters(children); + } + } + else + { + UASSERT(panelcount()); + const QObjectList & children = stackedWidget->widget(panel)->children(); addParameters(children); } } diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 64a22a90..879260a4 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -50,8 +50,8 @@ 0 0 - 154 - 184 + 175 + 173 @@ -236,8 +236,8 @@ 0 0 - 154 - 184 + 174 + 173 @@ -418,7 +418,7 @@ 0 0 1285 - 22 + 25 @@ -441,6 +441,7 @@ + @@ -828,15 +829,15 @@ - 1 + 2 0 0 - 312 - 314 + 314 + 303 @@ -1055,8 +1056,8 @@ 0 0 - 366 - 361 + 351 + 347 @@ -1344,9 +1345,9 @@ 0 - 0 - 330 - 304 + -26 + 333 + 333 @@ -1363,14 +1364,14 @@ - + Ignore global loop closures - + @@ -1419,14 +1420,14 @@ - + Ignore pose correction - + @@ -1436,7 +1437,7 @@ - + @@ -1446,7 +1447,7 @@ - + @@ -1456,6 +1457,19 @@ + + + + Qt::Vertical + + + + 0 + 40 + + + + @@ -1471,6 +1485,11 @@ g2o + + + GTSAM + + @@ -1480,34 +1499,21 @@ - + Ignore user loop closures - + Ignore local loop closures (space) - - - - Qt::Vertical - - - - 0 - 40 - - - - - + @@ -1517,7 +1523,7 @@ - + Ignore local loop closures (time) @@ -1531,7 +1537,7 @@ - + @@ -1548,13 +1554,30 @@ - + Ignore covariance + + + + Robust optimization + + + + + + + + + + false + + + @@ -1562,8 +1585,8 @@ 0 0 - 248 - 319 + 243 + 284 @@ -1757,7 +1780,7 @@ 0 0 201 - 126 + 117 @@ -1856,8 +1879,8 @@ 0 0 - 283 - 322 + 285 + 309 @@ -2301,6 +2324,11 @@ Reset all changes + + + Generate g2o graph (*.g2o)... + + diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 7fe1f390..b9937a35 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; } + + + + With Freenect : + + + @@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; } - - - - With Freenect : - - - @@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; } - + With cvsba : - + @@ -310,6 +310,23 @@ p, li { white-space: pre-wrap; } + + + + With GTSAM : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index a3cd3a9a..0222ab10 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 22 + 25 @@ -64,6 +64,7 @@ + @@ -1259,6 +1260,11 @@ Raw format (*.txt) + + + g2o (*.g2o) + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 652e1aba..14972472 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,8 +64,8 @@ 0 -392 - 755 - 1591 + 760 + 1570 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 19 @@ -6553,6 +6553,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag g2o + + + GTSAM + + @@ -6575,14 +6580,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + Optimize graph from the newest node. @@ -6595,7 +6600,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Qt::Horizontal @@ -6632,7 +6637,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation). @@ -6645,7 +6650,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. @@ -6710,6 +6715,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + + Robust graph optimization using Vertigo (only for g2o and GTSAM optimization strategies). This approach can filter wrong loop closure detections. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + From 786bbaa2c2ad3f357aba3fc402ecede5522c8729 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Sep 2015 09:56:25 -0400 Subject: [PATCH 038/110] updated debug->info level at startup --- app/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/app/src/main.cpp b/app/src/main.cpp index 7bd30cef..9305a1b7 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -41,7 +41,7 @@ int main(int argc, char* argv[]) { /* Set logger type */ ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kDebug); + ULogger::setLevel(ULogger::kInfo); /* Create tasks */ QApplication * app = new QApplication(argc, argv); From 198899ba32ac6b36b3157b02f13133a135b11f9a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Sep 2015 12:37:14 -0400 Subject: [PATCH 039/110] Added parameter "RGBD/OptimizeMaxError" (default: 1 m) to reject loop closures after too large optimization errors --- corelib/include/rtabmap/core/Parameters.h | 23 +-- corelib/include/rtabmap/core/Rtabmap.h | 2 + corelib/include/rtabmap/core/Statistics.h | 5 +- corelib/src/Rtabmap.cpp | 181 +++++++++++++++++----- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 39 ++++- 6 files changed, 194 insertions(+), 57 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 498dfc81..0b3c0656 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -284,18 +284,19 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC."); // RGB-D SLAM - RTABMAP_PARAM(RGBD, Enabled, bool, true, ""); - RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required)."); - RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); - RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); - RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); - RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter."); + RTABMAP_PARAM(RGBD, Enabled, bool, true, ""); + RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required)."); + RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); + RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); + RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph."); + RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); + RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter."); RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\"."); - RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); - RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); - RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); + RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); + RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); + RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); // Local loop closure detection RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index a80b05bf..675f5242 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -191,6 +191,7 @@ private: bool _localPathOdomPosesUsed; std::string _databasePath; bool _optimizeFromGraphEnd; + float _optimizationMaxLinearError; bool _reextractLoopClosureFeatures; int _reextractNNType; float _reextractNNDR; @@ -205,6 +206,7 @@ private: std::pair _loopClosureHypothesis; std::pair _highestHypothesis; double _lastProcessTime; + bool _someNodesHaveBeenTransferred; // Abstract classes containing all loop closure // strategies for a type of signature or configuration. diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 4644c244..ca81b786 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -57,11 +57,12 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, Highest_hypothesis_id,); RTABMAP_STATS(Loop, Highest_hypothesis_value,); RTABMAP_STATS(Loop, Vp_hypothesis,); - RTABMAP_STATS(Loop, ReactivateId,); + RTABMAP_STATS(Loop, Reactivate_id,); RTABMAP_STATS(Loop, Hypothesis_ratio,); RTABMAP_STATS(Loop, Hypothesis_reactivated,); - RTABMAP_STATS(Loop, VisualInliers,); + RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Last_id,); + RTABMAP_STATS(Loop, Optimization_max_error, m); RTABMAP_STATS(LocalLoop, Time_closures,); RTABMAP_STATS(LocalLoop, Space_last_closure_id,); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index b3ec5bd1..90292cfc 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -100,6 +100,7 @@ Rtabmap::Rtabmap() : _localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()), _databasePath(""), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), + _optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()), _reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()), _reextractNNType(Parameters::defaultLccReextractNNType()), _reextractNNDR(Parameters::defaultLccReextractNNDR()), @@ -113,6 +114,7 @@ Rtabmap::Rtabmap() : _loopClosureHypothesis(0,0.0f), _highestHypothesis(0,0.0f), _lastProcessTime(0.0), + _someNodesHaveBeenTransferred(false), _epipolarGeometry(0), _bayesFilter(0), _graphOptimizer(0), @@ -319,6 +321,7 @@ void Rtabmap::close() _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; + _someNodesHaveBeenTransferred = false; _optimizedPoses.clear(); _constraints.clear(); _mapCorrection.setIdentity(); @@ -398,6 +401,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed); Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); + Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError); Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures); Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType); Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR); @@ -829,6 +833,7 @@ void Rtabmap::resetMemory() _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; + _someNodesHaveBeenTransferred = false; _optimizedPoses.clear(); _constraints.clear(); _mapCorrection.setIdentity(); @@ -902,7 +907,6 @@ bool Rtabmap::process( std::map childCount; std::set signaturesRetrieved; int localLoopClosuresInTimeFound = 0; - bool scanMatchingSuccess = false; const Signature * signature = 0; const Signature * sLoop = 0; @@ -1044,49 +1048,77 @@ bool Rtabmap::process( } } - Transform newPose = _mapCorrection * signature->getPose(); - _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); - _lastLocalizationPose = newPose; // used in localization mode only (path planning) - - //============================================================ - // Scan matching - //============================================================ - if(_poseScanMatching && - signature->getLinks().size() == 1 && - !signature->sensorData().laserScanCompressed().empty() && - rehearsedId == 0) // don't do it if rehearsal happened + // Update optimizedPoses with the newly added node + Transform newPose; + if(signature->getLinks().size() == 1) { - UINFO("Odometry correction by scan matching"); int oldId = signature->getLinks().begin()->first; const Signature * oldS = _memory->getSignature(oldId); UASSERT(oldS != 0); - std::string rejectedMsg; - Transform guess = signature->getLinks().begin()->second.transform(); - double variance = 1.0; - int inliers = 0; - float inliersRatio = 0; - Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio); - if(!t.isNull()) + + //============================================================ + // Scan matching + //============================================================ + if(_poseScanMatching && + !signature->sensorData().laserScanCompressed().empty() && + rehearsedId == 0) // don't do it if rehearsal happened { - scanMatchingSuccess = true; - UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s", - signature->id(), - oldId, - signature->getLinks().at(oldId).transform().prettyPrint().c_str(), - t.prettyPrint().c_str()); - _memory->updateLink(signature->id(), oldId, t, variance, variance); + UINFO("Odometry correction by scan matching"); + Transform guess = signature->getLinks().begin()->second.transform(); + double variance = 1.0; + int inliers = 0; + float inliersRatio = 0; + std::string rejectedMsg; + Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio); + if(!t.isNull()) + { + UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s", + signature->id(), + oldId, + signature->getLinks().at(oldId).transform().prettyPrint().c_str(), + t.prettyPrint().c_str()); + _memory->updateLink(signature->id(), oldId, t, variance, variance); + + if(_optimizeFromGraphEnd) + { + // update all previous nodes + // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd + // parameters just changed state, we should put back all poses without map correction. + Transform u = guess.inverse() * t; + Transform mapCorrectionInv = _mapCorrection.inverse(); + for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) + { + iter->second = mapCorrectionInv * iter->second * u; + } + } + } + else + { + UINFO("Scan matching rejected: %s", rejectedMsg.c_str()); + } + statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0); + statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers); + statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio); + statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance); } - else + timeScanMatching = timer.ticks(); + ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching); + + UASSERT(oldS->hasLink(signature->id())); + UASSERT(uContains(_optimizedPoses, oldId)); + newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform(); + _mapCorrection = newPose * signature->getPose().inverse(); + if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) { - UINFO("Scan matching rejected: %s", rejectedMsg.c_str()); + UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); } - statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0); - statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers); - statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio); - statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance); } - timeScanMatching = timer.ticks(); - ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching); + else + { + newPose = _mapCorrection * signature->getPose(); + } + _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); + _lastLocalizationPose = newPose; // used in localization mode only (path planning) if(signature->getLinks().size() == 1) { @@ -1543,7 +1575,7 @@ bool Rtabmap::process( if(immunizedLocally >= maxLocalLocationsImmunized) { // set 20 to avoid this warning when starting mapping - if(maxLocalLocationsImmunized > 20) + if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred) { UWARN("Could not immunize the whole local path (%d) between " "%d and %d (max location immunized=%d). You may want " @@ -1675,6 +1707,7 @@ bool Rtabmap::process( // Update loop closure links // (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM) //============================================================= + std::list > loopClosureLinksAdded; int loopClosureVisualInliers = 0; // for statistics if(_loopClosureHypothesis.first>0) { @@ -1758,6 +1791,10 @@ bool Rtabmap::process( { // Make the new one the parent of the old one rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance)); + if(!rejectedHypothesis) + { + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first)); + } } if(rejectedHypothesis) @@ -1911,6 +1948,7 @@ bool Rtabmap::process( nearestId, transform.prettyPrint().c_str()); _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); if(_loopClosureHypothesis.first == 0) { @@ -2000,6 +2038,7 @@ bool Rtabmap::process( transform.prettyPrint().c_str()); // set Identify covariance for laser scan matching only _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1)); + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); ++localSpaceClosuresAddedByICPOnly; @@ -2027,10 +2066,10 @@ bool Rtabmap::process( //============================================================ // Optimize map graph //============================================================ + float maxLinearError = 0.0f; if(_rgbdSlamMode && - (_loopClosureHypothesis.first>0 || // can be different map of the current one + (_loopClosureHypothesis.first>0 || // can be different map of the current one localLoopClosuresInTimeFound>0 || // only same map of the current one - scanMatchingSuccess || // only same map of the current one lastLocalSpaceClosureId>0 || // can be different map of the current one signaturesRetrieved.size())) // can be different map of the current one { @@ -2038,8 +2077,61 @@ bool Rtabmap::process( { UINFO("Update map correction: SLAM mode"); // SLAM mode! - optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints); - UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()); + std::map poses = _optimizedPoses; + std::multimap constraints; + optimizeCurrentMap(signature->id(), false, poses, &constraints); + UASSERT(poses.find(signature->id()) != poses.end()); + + // Check added loop closures have broken the graph + // (in case of wrong loop closures). + bool updateConstraints = true; + if(_optimizationMaxLinearError > 0.0f && loopClosureLinksAdded.size()) + { + const Link * maxLinearLink = 0; + for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) + { + Transform t1 = uValue(poses, iter->second.from(), Transform()); + Transform t2 = uValue(poses, iter->second.to(), Transform()); + Transform t = t1.inverse()*t2; + float linearError = uMax3( + fabs(iter->second.transform().x() - t.x()), + fabs(iter->second.transform().y() - t.y()), + fabs(iter->second.transform().z() - t.z())); + if(linearError > maxLinearError) + { + maxLinearError = linearError; + maxLinearLink = &iter->second; + } + } + + if(maxLinearError > _optimizationMaxLinearError) + { + UWARN("Rejecting all added loop closures (%d) in this " + "iteration because a wrong loop closure has been " + "detected after graph optimization, resulting in " + "a maximum graph error of %f m (edge %d->%d). The " + "maximum error parameter is %f m.", + (int)loopClosureLinksAdded.size(), + maxLinearError, + maxLinearLink->from(), + maxLinearLink->to(), + _optimizationMaxLinearError); + for(std::list >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter) + { + _memory->removeLink(iter->first, iter->second); + UWARN("Loop closure %d->%d rejected!", iter->first, iter->second); + } + updateConstraints = false; + _loopClosureHypothesis.first = 0; + lastLocalSpaceClosureId = 0; + rejectedHypothesis = true; + } + } + if(updateConstraints) + { + _optimizedPoses = poses; + _constraints = constraints; + } // Update map correction, it should be identify when optimizing from the last node _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); @@ -2138,10 +2230,11 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second); statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated); statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis); - statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId); + statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId); statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio); - statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers); + statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers); statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId()); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError); statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound); statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually); @@ -2273,6 +2366,10 @@ bool Rtabmap::process( ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed); std::list transferred = _memory->forget(immunizedLocations); signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end()); + if(!_someNodesHaveBeenTransferred && transferred.size()) + { + _someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization + } } _lastProcessTime = totalTime; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 83bb2e72..b2c01544 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -578,6 +578,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str()); _ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str()); _ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); + _ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str()); _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str()); _ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 14972472..6716e79b 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -392 + -509 760 - 1570 + 1598 @@ -6735,6 +6735,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 1.000000000000000 + + + From 1c327e7b6778875cb17d8cd67e5e2e9f2bcbed66 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Sep 2015 14:33:28 -0400 Subject: [PATCH 040/110] GraphViewer: added loop closure outlier color option --- guilib/include/rtabmap/gui/GraphViewer.h | 6 + guilib/src/GraphViewer.cpp | 177 ++++++++++++++--------- 2 files changed, 116 insertions(+), 67 deletions(-) diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index 24c5aba1..12f35062 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -83,6 +83,7 @@ public: const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;} const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;} const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;} + const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;} const QColor & getLocalPathColor() const {return _localPathColor;} const QColor & getGlobalPathColor() const {return _globalPathColor;} const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;} @@ -92,6 +93,7 @@ public: bool isOriginVisible() const; bool isReferentialVisible() const; bool isLocalRadiusVisible() const; + float getLoopClosureOutlierThr() const; // setters void setWorkingDirectory(const QString & path); @@ -104,6 +106,7 @@ public: void setLocalLoopClosureColor(const QColor & color); void setUserLoopClosureColor(const QColor & color); void setVirtualLoopClosureColor(const QColor & color); + void setRejectedLoopClosureColor(const QColor & color); void setLocalPathColor(const QColor & color); void setGlobalPathColor(const QColor & color); void setIntraSessionLoopColor(const QColor & color); @@ -113,6 +116,7 @@ public: void setOriginVisible(bool visible); void setReferentialVisible(bool visible); void setLocalRadiusVisible(bool visible); + void setLoopClosureOutlierThr(float value); signals: void configChanged(); @@ -133,6 +137,7 @@ private: QColor _loopClosureLocalColor; QColor _loopClosureUserColor; QColor _loopClosureVirtualColor; + QColor _loopClosureRejectedColor; QColor _localPathColor; QColor _globalPathColor; QColor _loopIntraSessionColor; @@ -150,6 +155,7 @@ private: QGraphicsItemGroup * _originReferential; float _gridCellSize; QGraphicsEllipseItem * _localRadius; + float _loopClosureOutlierThr; }; } /* namespace rtabmap */ diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 4b5aba7e..aea24dbf 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -170,6 +170,7 @@ GraphViewer::GraphViewer(QWidget * parent) : _loopClosureLocalColor(Qt::yellow), _loopClosureUserColor(Qt::red), _loopClosureVirtualColor(Qt::magenta), + _loopClosureRejectedColor(Qt::black), _localPathColor(Qt::cyan), _globalPathColor(Qt::darkMagenta), _loopIntraSessionColor(Qt::red), @@ -181,7 +182,8 @@ GraphViewer::GraphViewer(QWidget * parent) : _gridMap(0), _referential(0), _gridCellSize(0.0f), - _localRadius(0) + _localRadius(0), + _loopClosureOutlierThr(0) { this->setScene(new QGraphicsScene(this)); this->setDragMode(QGraphicsView::ScrollHandDrag); @@ -282,13 +284,13 @@ void GraphViewer::updateGraph(const std::map & poses, std::map::const_iterator jterA = poses.find(idFrom); std::map::const_iterator jterB = poses.find(idTo); + LinkItem * linkItem = 0; if(jterA != poses.end() && jterB != poses.end() && _nodeItems.contains(iter->first) && _nodeItems.contains(idTo)) { const Transform & poseA = jterA->second; const Transform & poseB = jterB->second; - bool added = false; if(_linkItems.contains(idFrom)) { QMultiMap::iterator itemIter = _linkItems.find(iter->first); @@ -298,86 +300,89 @@ void GraphViewer::updateGraph(const std::map & poses, { itemIter.value()->setPoses(poseA, poseB); itemIter.value()->show(); - added = true; - // reset color - if(iter->second.type() == Link::kNeighbor) - { - itemIter.value()->setColor(_neighborColor); - } - else if(iter->second.type() == Link::kVirtualClosure) - { - itemIter.value()->setColor(_loopClosureVirtualColor); - } - else if(iter->second.type() == Link::kUserClosure) - { - itemIter.value()->setColor(_loopClosureUserColor); - } - else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure) - { - itemIter.value()->setColor(_loopClosureLocalColor); - } - else - { - itemIter.value()->setColor(_loopClosureColor); - } + linkItem = itemIter.value(); break; } ++itemIter; } } - if(!added) + + bool interSessionClosure = false; + if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first)) + { + interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first); + } + + if(linkItem == 0) { //create a link item - bool interSessionClosure = false; - if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first)) - { - interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first); - } - LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure); - QPen p = item->pen(); + linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure); + QPen p = linkItem->pen(); p.setWidthF(_linkWidth); - item->setPen(p); - item->setZValue(10); - if(iter->second.type() == Link::kNeighbor) + linkItem->setPen(p); + linkItem->setZValue(10); + this->scene()->addItem(linkItem); + linkItem->setParentItem(_root); + _linkItems.insert(idFrom, linkItem); + } + + //update color + if(iter->second.type() == Link::kNeighbor) + { + linkItem->setColor(_neighborColor); + } + else if(iter->second.type() == Link::kVirtualClosure) + { + linkItem->setColor(_loopClosureVirtualColor); + } + else if(iter->second.type() == Link::kUserClosure) + { + linkItem->setColor(_loopClosureUserColor); + } + else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure) + { + if(_intraInterSessionColors) { - item->setColor(_neighborColor); - } - else if(iter->second.type() == Link::kVirtualClosure) - { - item->setColor(_loopClosureVirtualColor); - } - else if(iter->second.type() == Link::kUserClosure) - { - item->setColor(_loopClosureUserColor); - } - else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure) - { - if(_intraInterSessionColors) - { - item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); - item->setZValue(interSessionClosure?8:9); - } - else - { - item->setColor(_loopClosureLocalColor); - } + linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); + linkItem->setZValue(interSessionClosure?8:9); } else { - if(_intraInterSessionColors) + linkItem->setColor(_loopClosureLocalColor); + } + } + else + { + if(_intraInterSessionColors) + { + linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); + linkItem->setZValue(interSessionClosure?8:9); + } + else + { + linkItem->setColor(_loopClosureColor); + } + } + + //rejected loop closures + if(_loopClosureOutlierThr > 0.0f) + { + Transform t = poseA.inverse()*poseB; + if(iter->second.to() != idTo) + { + t = t.inverse(); + } + if(iter->second.type() != Link::kNeighbor) + { + float linearError = uMax3( + fabs(iter->second.transform().x() - t.x()), + fabs(iter->second.transform().y() - t.y()), + fabs(iter->second.transform().z() - t.z())); + if(linearError > _loopClosureOutlierThr) { - item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); - item->setZValue(interSessionClosure?8:9); - } - else - { - item->setColor(_loopClosureColor); + linkItem->setColor(_loopClosureRejectedColor); } } - - this->scene()->addItem(item); - item->setParentItem(_root); - _linkItems.insert(idFrom, item); } } } @@ -643,6 +648,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("local_color", this->getLocalLoopClosureColor()); settings.setValue("user_color", this->getUserLoopClosureColor()); settings.setValue("virtual_color", this->getVirtualLoopClosureColor()); + settings.setValue("rejected_color", this->getRejectedLoopClosureColor()); settings.setValue("local_path_color", this->getLocalPathColor()); settings.setValue("global_path_color", this->getGlobalPathColor()); settings.setValue("intra_session_color", this->getIntraSessionLoopColor()); @@ -652,6 +658,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("origin_visible", this->isOriginVisible()); settings.setValue("referential_visible", this->isReferentialVisible()); settings.setValue("local_radius_visible", this->isLocalRadiusVisible()); + settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()); if(!group.isEmpty()) { settings.endGroup(); @@ -673,6 +680,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value()); this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value()); this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value()); + this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value()); this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value()); this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value()); this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value()); @@ -682,6 +690,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool()); this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool()); this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool()); + this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble()); if(!group.isEmpty()) { settings.endGroup(); @@ -704,6 +713,10 @@ bool GraphViewer::isLocalRadiusVisible() const { return _localRadius->isVisible(); } +float GraphViewer::getLoopClosureOutlierThr() const +{ + return _loopClosureOutlierThr; +} void GraphViewer::setWorkingDirectory(const QString & path) { @@ -808,6 +821,10 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color) } } } +void GraphViewer::setRejectedLoopClosureColor(const QColor & color) +{ + _loopClosureRejectedColor = color; +} void GraphViewer::setLocalPathColor(const QColor & color) { _localPathColor = color; @@ -884,6 +901,10 @@ void GraphViewer::setLocalRadiusVisible(bool visible) { _localRadius->setVisible(visible); } +void GraphViewer::setLoopClosureOutlierThr(float value) +{ + _loopClosureOutlierThr = value; +} void GraphViewer::restoreDefaults() { @@ -937,6 +958,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure")); QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure")); QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure")); + QAction * aChangeRejectedLoopColor = menuLink->addAction(tr("Outlier loop closure")); + QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold...")); QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path")); QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path")); menuLink->addSeparator(); @@ -948,6 +971,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor)); aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor)); aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor)); + aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor)); aChangeLocalPathColor->setIcon(createIcon(_localPathColor)); aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor)); aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor)); @@ -957,6 +981,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeLocalLoopColor->setIconVisibleInMenu(true); aChangeUserLoopColor->setIconVisibleInMenu(true); aChangeVirtualLoopColor->setIconVisibleInMenu(true); + aChangeRejectedLoopColor->setIconVisibleInMenu(true); aChangeLocalPathColor->setIconVisibleInMenu(true); aChangeGlobalPathColor->setIconVisibleInMenu(true); aChangeIntraSessionLoopColor->setIconVisibleInMenu(true); @@ -1088,6 +1113,15 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked()); } + else if(r == aChangeRejectedLoopThr) + { + bool ok; + double value = QInputDialog::getDouble(this, tr("Loop closure outlier threshold"), tr("Value (m)"), _loopClosureOutlierThr, 0.0, 1000.0, 2, &ok); + if(ok) + { + setLoopClosureOutlierThr(value); + } + } else if(r == aChangeNodeColor || r == aChangeCurrentGoalColor || r == aChangeNeighborColor || @@ -1095,6 +1129,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) r == aChangeLocalLoopColor || r == aChangeUserLoopColor || r == aChangeVirtualLoopColor || + r == aChangeRejectedLoopColor || r == aChangeLocalPathColor || r == aChangeGlobalPathColor || r == aChangeIntraSessionLoopColor || @@ -1125,6 +1160,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { color = _loopClosureVirtualColor; } + else if(r == aChangeRejectedLoopColor) + { + color = _loopClosureRejectedColor; + } else if(r == aChangeLocalPathColor) { color = _localPathColor; @@ -1173,6 +1212,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { this->setVirtualLoopClosureColor(color); } + else if(r == aChangeRejectedLoopColor) + { + this->setRejectedLoopClosureColor(color); + } else if(r == aChangeLocalPathColor) { this->setLocalPathColor(color); From f731b30cf9a03aa4d181b0ddf1e09df5348b6ec2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Sep 2015 07:34:33 -0400 Subject: [PATCH 041/110] Fixed undefined vertigo (g2o) methods --- corelib/src/CMakeLists.txt | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 64b2a4cf..ee444e76 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -152,17 +152,15 @@ IF(G2O_FOUND) #Newest versions require std11 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - IF(WITH_VERTIGO) - SET(SRC_FILES - ${SRC_FILES} - vertigo/g2o/edge_se2MaxMixture.cpp - vertigo/g2o/edge_se2Switchable.cpp - vertigo/g2o/edge_se3Switchable.cpp - vertigo/g2o/edge_switchPrior.cpp - vertigo/g2o/types_g2o_robust.cpp - vertigo/g2o/vertex_switchLinear.cpp - ) - ENDIF(WITH_VERTIGO) + SET(SRC_FILES + ${SRC_FILES} + vertigo/g2o/edge_se2MaxMixture.cpp + vertigo/g2o/edge_se2Switchable.cpp + vertigo/g2o/edge_se3Switchable.cpp + vertigo/g2o/edge_switchPrior.cpp + vertigo/g2o/types_g2o_robust.cpp + vertigo/g2o/vertex_switchLinear.cpp + ) ENDIF(G2O_FOUND) IF(GTSAM_FOUND) From 3bd9dd6a53f62b4c8d4ce1d1556d6d13d6c1e92a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Sep 2015 08:29:06 -0400 Subject: [PATCH 042/110] ImagesJoiner tool: can now merge images from two folders --- tools/ImagesJoiner/main.cpp | 48 +++++++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/tools/ImagesJoiner/main.cpp b/tools/ImagesJoiner/main.cpp index 74ca1733..72e9319f 100644 --- a/tools/ImagesJoiner/main.cpp +++ b/tools/ImagesJoiner/main.cpp @@ -37,6 +37,7 @@ void showUsage() { printf("Usage:\n" "imagesJoiner.exe [option] path\n" + "imagesJoiner.exe path_left path_right\n" " Options:\n" " -inv option for copying odd images on the right\n\n"); exit(1); @@ -58,15 +59,33 @@ int main(int argc, char * argv[]) printf(" Inversing option activated...\n"); continue; } - showUsage(); - printf(" Not recognized option: \"%s\"\n", argv[i]); + if(argc > 3) + { + showUsage(); + printf(" Not recognized option: \"%s\"\n", argv[i]); + } } - std::string path = argv[argc-1]; - printf(" Path = %s\n", path.c_str()); + std::string path, pathRight; + + if(argc == 3 && !inv) + { + //two paths + path = argv[1]; + pathRight = argv[2]; + + printf(" Path left = %s\n", path.c_str()); + printf(" Path right = %s\n", pathRight.c_str()); + } + else + { + path = argv[argc-1]; + printf(" Path = %s\n", path.c_str()); + } UDirectory dir(path, "jpg bmp png tiff jpeg"); - if(!dir.isValid()) + UDirectory dirRight(pathRight, "jpg bmp png tiff jpeg"); + if(!dir.isValid() || (!pathRight.empty() && !dirRight.isValid())) { printf("Path invalid!\n"); exit(-1); @@ -78,7 +97,15 @@ int main(int argc, char * argv[]) std::string fileNameA = dir.getNextFilePath(); - std::string fileNameB = dir.getNextFilePath(); + std::string fileNameB; + if(dirRight.isValid()) + { + fileNameB = dirRight.getNextFilePath(); + } + else + { + fileNameB = dir.getNextFilePath(); + } int i=1; while(!fileNameA.empty() && !fileNameB.empty()) @@ -124,7 +151,14 @@ int main(int argc, char * argv[]) } fileNameA = dir.getNextFilePath(); - fileNameB = dir.getNextFilePath(); + if(dirRight.isValid()) + { + fileNameB = dirRight.getNextFilePath(); + } + else + { + fileNameB = dir.getNextFilePath(); + } } else { From f912023a3061f9665a645b6acf6317ed702ff47b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Sep 2015 13:46:37 -0400 Subject: [PATCH 043/110] GUI: custom aspect ratio, option to save auto screenshots in RAM, default views updated (reseting 3D map camera position and hiding the status bar) --- guilib/include/rtabmap/gui/CloudViewer.h | 1 + guilib/include/rtabmap/gui/MainWindow.h | 5 +- .../include/rtabmap/gui/PreferencesDialog.h | 2 +- guilib/src/CloudViewer.cpp | 59 ++++---- guilib/src/MainWindow.cpp | 137 +++++++++++++----- guilib/src/PreferencesDialog.cpp | 6 +- guilib/src/ui/mainWindow.ui | 7 + 7 files changed, 154 insertions(+), 63 deletions(-) diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 2927428b..535f06e1 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -196,6 +196,7 @@ public: void setTrajectoryShown(bool shown); void setTrajectorySize(unsigned int value); void clearTrajectory(); + void resetCamera(); void removeAllClouds(); //including meshes bool removeCloud(const std::string & id); //including mesh diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index c6b0dc37..9ac1f6de 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -180,6 +180,7 @@ private slots: void setAspectRatio480p(); void setAspectRatio720p(); void setAspectRatio1080p(); + void setAspectRatioCustom(); void exportGridMap(); void exportScans(); void exportClouds(); @@ -227,7 +228,7 @@ private: void saveFigures(); void loadFigures(); void exportPoses(int format); - QString captureScreen(); + QString captureScreen(bool cacheInRAM = false); std::map::Ptr > getClouds( const std::map & poses, @@ -304,6 +305,8 @@ private: QString _graphSavingFileName; QMap _exportPosesFileName; bool _autoScreenCaptureOdomSync; + bool _autoScreenCaptureRAM; + QMap _autoScreenCaptureCachedImages; QVector _refIds; QVector _loopClosureIds; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 8a2bdedc..c6e6159f 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -117,7 +117,7 @@ public: void saveWindowGeometry(const QWidget * window); void loadWindowGeometry(QWidget * window); void saveMainWindowState(const QMainWindow * mainWindow); - void loadMainWindowState(QMainWindow * mainWindow, bool & maximized); + void loadMainWindowState(QMainWindow * mainWindow, bool & maximized, bool & statusBarShown); void saveWidgetState(const QWidget * widget); void loadWidgetState(QWidget * widget); diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index d471dbc7..06d0b544 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -889,6 +889,37 @@ void CloudViewer::clearTrajectory() this->update(); } +void CloudViewer::resetCamera() +{ + _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0); + if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull()) + { + // reset relative to last current pose + if(_aLockViewZ->isChecked()) + { + _visualizer->setCameraPosition( + _lastPose.x()-1, _lastPose.y(), _lastPose.z(), + _lastPose.x(), _lastPose.y(), _lastPose.z(), + 0, 0, 1); + } + else + { + _visualizer->setCameraPosition( + _lastPose.x()-1, _lastPose.y(), _lastPose.z(), + _lastPose.x(), _lastPose.y(), _lastPose.z(), + _lastPose.r31(), _lastPose.r32(), _lastPose.r33()); + } + } + else + { + _visualizer->setCameraPosition( + -1, 0, 0, + 0, 0, 0, + 0, 0, 1); + } + this->update(); +} + void CloudViewer::removeAllClouds() { _addedClouds.clear(); @@ -1506,33 +1537,7 @@ void CloudViewer::handleAction(QAction * a) } else if(a == _aResetCamera) { - _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0); - if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull()) - { - // reset relative to last current pose - if(_aLockViewZ->isChecked()) - { - _visualizer->setCameraPosition( - _lastPose.x()-1, _lastPose.y(), _lastPose.z(), - _lastPose.x(), _lastPose.y(), _lastPose.z(), - 0, 0, 1); - } - else - { - _visualizer->setCameraPosition( - _lastPose.x()-1, _lastPose.y(), _lastPose.z(), - _lastPose.x(), _lastPose.y(), _lastPose.z(), - _lastPose.r31(), _lastPose.r32(), _lastPose.r33()); - } - } - else - { - _visualizer->setCameraPosition( - -1, 0, 0, - 0, 0, 0, - 0, 0, 1); - } - this->update(); + this->resetCamera(); } else if(a == _aShowGrid) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a57cf406..949e82af 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -145,6 +145,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _likelihoodCurve(0), _rawLikelihoodCurve(0), _autoScreenCaptureOdomSync(false), + _autoScreenCaptureRAM(false), _firstCall(true) { UDEBUG(""); @@ -174,20 +175,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : this->setObjectName("MainWindow"); //Setup dock widgets position if it is the first time the application is started. - //if(!QFile::exists(PreferencesDialog::getIniFilePath())) - { - _ui->dockWidget_posterior->setVisible(false); - _ui->dockWidget_likelihood->setVisible(false); - _ui->dockWidget_rawlikelihood->setVisible(false); - _ui->dockWidget_statsV2->setVisible(false); - _ui->dockWidget_console->setVisible(false); - _ui->dockWidget_loopClosureViewer->setVisible(false); - _ui->dockWidget_mapVisibility->setVisible(false); - _ui->dockWidget_graphViewer->setVisible(false); - //_ui->dockWidget_odometry->setVisible(false); - //_ui->dockWidget_cloudViewer->setVisible(false); - //_ui->dockWidget_imageView->setVisible(false); - } + setDefaultViews(); _ui->widget_mainWindow->setVisible(false); @@ -204,7 +192,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _preferencesDialog->init(); // Restore window geometry - _preferencesDialog->loadMainWindowState(this, _savedMaximized); + bool statusBarShown = false; + _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown); _preferencesDialog->loadWindowGeometry(_preferencesDialog); _preferencesDialog->loadWindowGeometry(_exportDialog); _preferencesDialog->loadWindowGeometry(_postProcessingDialog); @@ -285,6 +274,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : QAction * a = _ui->menuShow_view->addAction("Progress dialog"); a->setCheckable(false); connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show())); + QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar"); + statusBarAction->setCheckable(true); + statusBarAction->setChecked(statusBarShown); + connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool))); // connect actions with custom slots connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI())); @@ -325,6 +318,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p())); connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p())); connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p())); + connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom())); connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds())); connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans())); connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap())); @@ -416,6 +410,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : 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())); + connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified())); QList dockWidgets = this->findChildren(); for(int i=0; iactionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync) { - this->captureScreen(); + this->captureScreen(_autoScreenCaptureRAM); } } @@ -1330,7 +1325,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime); if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync) { - this->captureScreen(); + this->captureScreen(_autoScreenCaptureRAM); } if(!_preferencesDialog->isImagesKept()) @@ -2562,26 +2557,41 @@ void MainWindow::changeMappingMode() emit mappingModeChanged(_ui->actionSLAM_mode->isChecked()); } -QString MainWindow::captureScreen() +QString MainWindow::captureScreen(bool cacheInRAM) { - QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured"; - QDir dir; - if(!dir.exists(targetDir)) - { - dir.mkdir(targetDir); - } - targetDir += QDir::separator(); - targetDir += "Main_window"; - if(!dir.exists(targetDir)) - { - dir.mkdir(targetDir); - } - targetDir += QDir::separator(); QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png"); _ui->statusbar->clearMessage(); QPixmap figure = QPixmap::grabWidget(this); - figure.save(targetDir + name); - QString msg = tr("Screen captured \"%1\"").arg(targetDir + name); + + QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured"; + QString msg; + if(cacheInRAM) + { + msg = tr("Screen captured \"%1\"").arg(name); + QByteArray bytes; + QBuffer buffer(&bytes); + buffer.open(QIODevice::WriteOnly); + figure.save(&buffer, "PNG"); + _autoScreenCaptureCachedImages.insert(name, bytes); + } + else + { + QDir dir; + if(!dir.exists(targetDir)) + { + dir.mkdir(targetDir); + } + targetDir += QDir::separator(); + targetDir += "Main_window"; + if(!dir.exists(targetDir)) + { + dir.mkdir(targetDir); + } + targetDir += QDir::separator(); + + figure.save(targetDir + name); + msg = tr("Screen captured \"%1\"").arg(targetDir + name); + } _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500); _ui->widget_console->appendMsg(msg); @@ -4131,6 +4141,9 @@ void MainWindow::setDefaultViews() _ui->dockWidget_imageView->setVisible(true); _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused); _ui->toolBar_2->setVisible(true); + _ui->statusbar->setVisible(false); + this->setAspectRatio720p(); + _ui->widget_cloudViewer->resetCamera(); } void MainWindow::selectScreenCaptureFormat(bool checked) @@ -4151,12 +4164,56 @@ void MainWindow::selectScreenCaptureFormat(bool checked) { _autoScreenCaptureOdomSync = true; } + + if(_state != kMonitoring && _state != kMonitoringPaused) + { + int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM? Images will be saved on disk when clicking auto screen capture again."), QMessageBox::Yes | QMessageBox::No, QMessageBox::No); + if(r == QMessageBox::No || r == QMessageBox::Yes) + { + _autoScreenCaptureRAM = r == QMessageBox::Yes; + } + else + { + _ui->actionAuto_screen_capture->setChecked(false); + } + } } else { _ui->actionAuto_screen_capture->setChecked(false); } } + else if(_autoScreenCaptureCachedImages.size()) + { + QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured"; + QDir dir; + if(!dir.exists(targetDir)) + { + dir.mkdir(targetDir); + } + targetDir += QDir::separator(); + targetDir += "Main_window"; + if(!dir.exists(targetDir)) + { + dir.mkdir(targetDir); + } + targetDir += QDir::separator(); + + _initProgressDialog->resetProgress(); + _initProgressDialog->show(); + _initProgressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size()); + int i=0; + for(QMap::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end(); ++iter) + { + QPixmap figure; + figure.loadFromData(iter.value(), "PNG"); + figure.save(targetDir + iter.key(), "PNG"); + _initProgressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size())); + _initProgressDialog->incrementStep(); + } + _autoScreenCaptureCachedImages.clear(); + _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); + } } void MainWindow::takeScreenshot() @@ -4230,6 +4287,20 @@ void MainWindow::setAspectRatio1080p() this->setAspectRatio((1080*16)/9, 1080); } +void MainWindow::setAspectRatioCustom() +{ + bool ok; + int width = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Width (pixels):"), this->geometry().width(), 100, 10000, 100, &ok); + if(ok) + { + int height = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Height (pixels):"), this->geometry().height(), 100, 10000, 100, &ok); + if(ok) + { + this->setAspectRatio(width, height); + } + } +} + void MainWindow::exportGridMap() { double gridCellSize = 0.05; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b2c01544..be3e2d2c 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include "ui_preferencesDialog.h" @@ -2047,12 +2048,13 @@ void PreferencesDialog::saveMainWindowState(const QMainWindow * mainWindow) settings.beginGroup(mainWindow->objectName()); settings.setValue("state", mainWindow->saveState()); settings.setValue("maximized", mainWindow->isMaximized()); + settings.setValue("status_bar", mainWindow->statusBar()->isVisible()); settings.endGroup(); // "MainWindow" settings.endGroup(); // rtabmap } } -void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow, bool & maximized) +void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow, bool & maximized, bool & statusBarShown) { if(!mainWindow->objectName().isNull()) { @@ -2068,6 +2070,8 @@ void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow, bool & ma mainWindow->restoreState(bytes); } maximized = settings.value("maximized", false).toBool(); + statusBarShown = settings.value("status_bar", false).toBool(); + mainWindow->statusBar()->setVisible(statusBarShown); settings.endGroup(); // "MainWindow" settings.endGroup(); // rtabmap } diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 0222ab10..f87bf20d 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -238,6 +238,8 @@ + + @@ -1265,6 +1267,11 @@ g2o (*.g2o) + + + Custom... + + From f51754ad64607ae651276b44c1795f315dc3a22c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Sep 2015 15:30:33 -0400 Subject: [PATCH 044/110] :lipstick: --- corelib/src/Graph.cpp | 103 +++++++++++++----------------------------- 1 file changed, 31 insertions(+), 72 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 4df24f10..71a2daaf 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -55,15 +55,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "g2o/types/slam2d/vertex_se2.h" #include "g2o/types/slam2d/edge_se2.h" -typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 3> > Slam2dBlockSolver; -typedef g2o::LinearSolverCSparse Slam2dLinearCSparseSolver; -typedef g2o::LinearSolverCholmod Slam2dLinearCholmodSolver; -typedef g2o::LinearSolverPCG Slam2dLinearPCGSolver; - -typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Slam3dBlockSolver; -typedef g2o::LinearSolverCSparse Slam3dLinearCSparseSolver; -typedef g2o::LinearSolverCholmod Slam3dLinearCholmodSolver; -typedef g2o::LinearSolverPCG Slam3dLinearPCGSolver; +typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; +typedef g2o::LinearSolverCSparse SlamLinearCSparseSolver; +typedef g2o::LinearSolverCholmod SlamLinearCholmodSolver; +typedef g2o::LinearSolverPCG SlamLinearPCGSolver; #include "vertigo/g2o/edge_switchPrior.h" #include "vertigo/g2o/edge_se2Switchable.h" @@ -731,76 +726,40 @@ std::map G2OOptimizer::optimize( // Apply g2o optimization g2o::SparseOptimizer optimizer; - optimizer.setVerbose(false); + optimizer.setVerbose(ULogger::level()==ULogger::kDebug); int solverApproach = 0; - int optimizationApproach = 0; - if(isSlam2d()) - { - Slam2dBlockSolver * blockSolver; - if(solverApproach == 1) - { - //pcg - Slam2dLinearPCGSolver * linearSolver = new Slam2dLinearPCGSolver(); - blockSolver = new Slam2dBlockSolver(linearSolver); - } - else if(solverApproach == 2) - { - //csparse - Slam2dLinearCSparseSolver* linearSolver = new Slam2dLinearCSparseSolver(); - linearSolver->setBlockOrdering(false); - blockSolver = new Slam2dBlockSolver(linearSolver); - } - else - { - //chmold - Slam2dLinearCholmodSolver * linearSolver = new Slam2dLinearCholmodSolver(); - linearSolver->setBlockOrdering(false); - blockSolver = new Slam2dBlockSolver(linearSolver); - } + int optimizationApproach = 1; - if(optimizationApproach == 1) - { - optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); - } - else - { - optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); - } + SlamBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver(); + blockSolver = new SlamBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new SlamBlockSolver(linearSolver); } else { - Slam3dBlockSolver * blockSolver; - if(solverApproach == 1) - { - //pcg - Slam3dLinearPCGSolver * linearSolver = new Slam3dLinearPCGSolver(); - blockSolver = new Slam3dBlockSolver(linearSolver); - } - else if(solverApproach == 2) - { - //csparse - Slam3dLinearCSparseSolver* linearSolver = new Slam3dLinearCSparseSolver(); - linearSolver->setBlockOrdering(false); - blockSolver = new Slam3dBlockSolver(linearSolver); - } - else - { - //chmold - Slam3dLinearCholmodSolver * linearSolver = new Slam3dLinearCholmodSolver(); - linearSolver->setBlockOrdering(false); - blockSolver = new Slam3dBlockSolver(linearSolver); - } - - if(optimizationApproach == 1) - { - optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); - } - else - { - optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); - } + //chmold + SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new SlamBlockSolver(linearSolver); } + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } UDEBUG("fill poses to g2o..."); for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) From d425d84ce2c6c05b1ec4dab0fb03a1b84a14a6d6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 7 Sep 2015 23:31:03 -0400 Subject: [PATCH 045/110] Fixed Bayes Filter prediction generation bug --- corelib/src/BayesFilter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index 02c51208..a9142dc5 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -289,9 +289,9 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector for(std::list::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) { float sum = 0.0f; // sum values added - sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap); + sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap); idsDone.insert(*iter); - this->normalize(prediction, i, sum, ids[0]<0); + this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0); } } else From 5dcbb66a6d3125e54dfd9aa9d47c4928430aa238 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Sep 2015 23:18:12 -0400 Subject: [PATCH 046/110] Fixed FATAL error for link variance == 0 on loop closure of two identical images --- corelib/src/Rtabmap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 90292cfc..3631ce55 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1790,7 +1790,7 @@ bool Rtabmap::process( if(!rejectedHypothesis) { // Make the new one the parent of the old one - rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance)); + rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001)); if(!rejectedHypothesis) { loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first)); From c107436e853b69dec6349cbc2da4810dae5a81f2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 00:58:45 -0400 Subject: [PATCH 047/110] VWDictionary: changed miniflann to flann directly so version 1.8 can be used. Added parameter "Kp/IncrementalFlann" --- CMakeLists.txt | 5 +- cmake_modules/FindFLANN.cmake | 66 ++++ corelib/include/rtabmap/core/Parameters.h | 1 + corelib/include/rtabmap/core/VWDictionary.h | 5 +- corelib/src/CMakeLists.txt | 4 + corelib/src/VWDictionary.cpp | 354 ++++++++++++++++---- guilib/src/AboutDialog.cpp | 3 + guilib/src/CMakeLists.txt | 4 + guilib/src/PreferencesDialog.cpp | 5 + guilib/src/ui/aboutDialog.ui | 73 ++-- guilib/src/ui/preferencesDialog.ui | 27 +- utilite/include/rtabmap/utilite/UStl.h | 22 ++ 12 files changed, 471 insertions(+), 98 deletions(-) create mode 100644 cmake_modules/FindFLANN.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 10654dd5..211ce574 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,8 +132,11 @@ option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) +FIND_PACKAGE(FLANN 1.8) +SET(FLANN18_FOUND ${FLANN_FOUND}) + FIND_PACKAGE(OpenCV REQUIRED) -FIND_PACKAGE(PCL 1.7 REQUIRED) +FIND_PACKAGE(PCL 1.7 REQUIRED) # Will find FLANN too FIND_PACKAGE(ZLIB REQUIRED) IF(WITH_QT) diff --git a/cmake_modules/FindFLANN.cmake b/cmake_modules/FindFLANN.cmake new file mode 100644 index 00000000..2404dcba --- /dev/null +++ b/cmake_modules/FindFLANN.cmake @@ -0,0 +1,66 @@ +############################################################################### +# Find FLANN +# +# This sets the following variables: +# FLANN_FOUND - True if FLANN was found. +# FLANN_INCLUDE_DIRS - Directories containing the FLANN include files. +# FLANN_LIBRARIES - Libraries needed to use FLANN. +# FLANN_DEFINITIONS - Compiler flags for FLANN. +# If FLANN_USE_STATIC is specified and then look for static libraries ONLY else +# look for shared ones +# +# Original from https://github.com/PointCloudLibrary/pcl/blob/master/cmake/Modules/FindFLANN.cmake +# + +if(FLANN_USE_STATIC) + set(FLANN_RELEASE_NAME flann_cpp_s) + set(FLANN_DEBUG_NAME flann_cpp_s-gd) +else(FLANN_USE_STATIC) + set(FLANN_RELEASE_NAME flann_cpp) + set(FLANN_DEBUG_NAME flann_cpp-gd) +endif(FLANN_USE_STATIC) + +find_package(PkgConfig QUIET) +if (FLANN_FIND_VERSION) + pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION}) +else(FLANN_FIND_VERSION) + pkg_check_modules(PC_FLANN flann) +endif(FLANN_FIND_VERSION) + +set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER}) + +find_path(FLANN_INCLUDE_DIR flann/flann.hpp + HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES include) + +find_library(FLANN_LIBRARY + NAMES ${FLANN_RELEASE_NAME} + HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES lib) + +find_library(FLANN_LIBRARY_DEBUG + NAMES ${FLANN_DEBUG_NAME} ${FLANN_RELEASE_NAME} + HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES lib) + +if(NOT FLANN_LIBRARY_DEBUG) + set(FLANN_LIBRARY_DEBUG ${FLANN_LIBRARY}) +endif(NOT FLANN_LIBRARY_DEBUG) + +set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR}) +set(FLANN_LIBRARIES optimized ${FLANN_LIBRARY} debug ${FLANN_LIBRARY_DEBUG}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARY FLANN_INCLUDE_DIR) + +mark_as_advanced(FLANN_LIBRARY FLANN_LIBRARY_DEBUG FLANN_INCLUDE_DIR) + +if(FLANN_FOUND) + message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})") + if(FLANN_USE_STATIC) + add_definitions(-DFLANN_STATIC) + endif(FLANN_USE_STATIC) +endif(FLANN_FOUND) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 0b3c0656..3896987d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -205,6 +205,7 @@ class RTABMAP_EXP Parameters // KeypointMemory (Keypoint-based) RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, ""); + RTABMAP_PARAM(Kp, IncrementalFlann, bool, false, "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size)."); RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf)"); RTABMAP_PARAM(Kp, WordsPerImage, int, 400, ""); RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)."); diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index cfa2303b..d70ac4c4 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -41,6 +41,7 @@ namespace rtabmap class DBDriver; class VisualWord; +class FlannIndex; class RTABMAP_EXP VWDictionary { @@ -97,14 +98,16 @@ protected: private: bool _incrementalDictionary; + bool _incrementalFlann; float _nndrRatio; std::string _dictionaryPath; // a pre-computed dictionary (.txt) bool _newWordsComparedTogether; int _lastWordId; - cv::flann::Index * _flannIndex; + FlannIndex * _flannIndex; cv::Mat _dataTree; NNStrategy _strategy; std::map _mapIndexId; + std::map _mapIdIndex; std::map _unusedWords; //, note that these words stay in _visualWords std::set _notIndexedWords; // Words that are not indexed in the dictionary std::set _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index ee444e76..641bf422 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -76,6 +76,10 @@ SET(LIBRARIES ${ZLIB_LIBRARIES} ) +IF(FLANN18_FOUND) + ADD_DEFINITIONS("-DWITH_FLANN18") +ENDIF(FLANN18_FOUND) + IF(Freenect_FOUND) ADD_DEFINITIONS("-DWITH_FREENECT") IF(Freenect_DASH_INCLUDES) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index fe18fa61..7d89e0a3 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UtiLite.h" #include + #if CV_MAJOR_VERSION < 3 #include #else @@ -44,23 +45,160 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif +#include + #include #include namespace rtabmap { +class FlannIndex +{ +public: + FlannIndex(): + index_(0), + binaryType_(false) + { + } + virtual ~FlannIndex() + { + this->release(); + } + + void release() + { + if(index_) + { + if(binaryType_) + { + delete (flann::Index >*)index_; + + } + else + { + delete (flann::Index >*)index_; + } + index_ = 0; + } + } + + void build( + const cv::Mat & features, + const flann::IndexParams& params, + bool binaryType) + { + this->release(); + UASSERT(index_ == 0); + binaryType_ = binaryType; + + if(binaryType) + { + flann::Matrix dataset(features.data, features.rows, features.cols); + index_ = new flann::Index >(dataset, params); + ((flann::Index >*)index_)->buildIndex(); + } + else + { + flann::Matrix dataset((float*)features.data, features.rows, features.cols); + index_ = new flann::Index >(dataset, params); + ((flann::Index >*)index_)->buildIndex(); + } + } + + bool isIncremental() + { +#ifdef WITH_FLANN18 + return true; +#else + return false; +#endif + } + + void addPoints(const cv::Mat & features) + { +#ifdef WITH_FLANN18 + if(binaryType_) + { + flann::Matrix dataset(features.data, features.rows, features.cols); + ((flann::Index >*)index_)->addPoints(dataset); + } + else + { + flann::Matrix dataset((float*)features.data, features.rows, features.cols); + ((flann::Index >*)index_)->addPoints(dataset); + } +#else + UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method."); +#endif + } + + void removePoint(unsigned int index) + { +#ifdef WITH_FLANN18 + if(binaryType_) + { + ((flann::Index >*)index_)->removePoint(index); + } + else + { + ((flann::Index >*)index_)->removePoint(index); + } +#else + UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method."); +#endif + } + + void knnSearch( + const cv::Mat & query, + cv::Mat & indices, + cv::Mat & dists, + int knn, + const flann::SearchParams& params=flann::SearchParams()) + { + if(!index_) + { + UERROR("Flann index not yet created!"); + return; + } + indices.create(query.rows, knn, CV_32S); + dists.create(query.rows, knn, binaryType_?CV_32S:CV_32F); + + cv::flann::IndexParams i; + + flann::Matrix indicesF((int*)indices.data, indices.rows, indices.cols); + + if(binaryType_) + { + flann::Matrix distsF((unsigned int*)dists.data, dists.rows, dists.cols); + flann::Matrix queryF(query.data, query.rows, query.cols); + ((flann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } + else + { + flann::Matrix distsF((float*)dists.data, dists.rows, dists.cols); + flann::Matrix queryF((float*)query.data, query.rows, query.cols); + ((flann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } + } + +private: + void * index_; + bool binaryType_; +}; + const int VWDictionary::ID_START = 1; const int VWDictionary::ID_INVALID = 0; VWDictionary::VWDictionary(const ParametersMap & parameters) : _totalActiveReferences(0), _incrementalDictionary(Parameters::defaultKpIncrementalDictionary()), + _incrementalFlann(Parameters::defaultKpIncrementalFlann()), _nndrRatio(Parameters::defaultKpNndrRatio()), _dictionaryPath(Parameters::defaultKpDictionaryPath()), _newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()), _lastWordId(0), - _flannIndex(new cv::flann::Index()), + _flannIndex(new FlannIndex()), _strategy(kNNBruteForce) { this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy()); @@ -78,6 +216,13 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) ParametersMap::const_iterator iter; Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio); Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether); + Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann); + + if(_incrementalFlann && !_flannIndex->isIncremental()) + { + UERROR("TRying to set \"KpIncrementalFlann\"=true but RTAB-Map is not built with FLANN>=1.8. Setting to false."); + _incrementalFlann = false; + } UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str()); @@ -257,7 +402,15 @@ void VWDictionary::setNNStrategy(NNStrategy strategy) } else { + bool update = _strategy != strategy; _strategy = strategy; + if(update) + { + _dataTree = cv::Mat(); + _notIndexedWords = uKeysSet(_visualWords); + _removedIndexedWords.clear(); + this->update(); + } } } } @@ -287,55 +440,106 @@ void VWDictionary::update() if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) { - _mapIndexId.clear(); int oldSize = _dataTree.rows; - _dataTree = cv::Mat(); - _flannIndex->release(); - - if(_visualWords.size()) + if(_incrementalFlann && + _flannIndex->isIncremental() && + _strategy < kNNBruteForce && + (_notIndexedWords.size() || _removedIndexedWords.size()) && + oldSize) { - UTimer timer; - timer.start(); - - int type = _visualWords.begin()->second->getDescriptor().type(); - int dim = _visualWords.begin()->second->getDescriptor().cols; - - UASSERT(type == CV_32F || type == CV_8U); - UASSERT(dim > 0); - - // Create the data matrix - _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F - std::map::const_iterator iter = _visualWords.begin(); - for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { - UASSERT(iter->second->getDescriptor().cols == dim); - UASSERT(iter->second->getDescriptor().type() == type); - - iter->second->getDescriptor().copyTo(_dataTree.row(i)); - _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + UASSERT(w); + UASSERT(w->getDescriptor().cols == _dataTree.cols); + UASSERT(w->getDescriptor().type() == _dataTree.type()); + _dataTree.push_back(w->getDescriptor()); + _mapIndexId.insert(_mapIndexId.end(), std::pair(_dataTree.rows-1, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); + if(!inserted.second) + { + //update to new index + inserted.first->second = _dataTree.rows-1; + } + _flannIndex->addPoints(w->getDescriptor()); } - - ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim); - ULOGGER_DEBUG("copying data = %f s", timer.ticks()); - - switch(_strategy) + for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) { - case kNNFlannNaive: - _flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); - break; - case kNNFlannKdTree: - UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2); - break; - case kNNFlannLSH: - UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING); - break; - default: - break; + UASSERT(uContains(_mapIdIndex, *iter)); + _flannIndex->removePoint(_mapIdIndex.at(*iter)); } + } + else if(_strategy >= kNNBruteForce && + _notIndexedWords.size() && + _removedIndexedWords.size() == 0 && + oldSize) + { + //just add not indexed words + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + { + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + UASSERT(w); + UASSERT(w->getDescriptor().cols == _dataTree.cols); + UASSERT(w->getDescriptor().type() == _dataTree.type()); + _dataTree.push_back(w->getDescriptor()); + _mapIndexId.insert(_mapIndexId.end(), std::pair(_dataTree.rows-1, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); + UASSERT(inserted.second); + } + } + else + { + _mapIndexId.clear(); + _mapIdIndex.clear(); + _dataTree = cv::Mat(); + _flannIndex->release(); - ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); + if(_visualWords.size()) + { + UTimer timer; + timer.start(); + + int type = _visualWords.begin()->second->getDescriptor().type(); + int dim = _visualWords.begin()->second->getDescriptor().cols; + + UASSERT(type == CV_32F || type == CV_8U); + UASSERT(dim > 0); + + // Create the data matrix + _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F + std::map::const_iterator iter = _visualWords.begin(); + for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + { + UASSERT(iter->second->getDescriptor().cols == dim); + UASSERT(iter->second->getDescriptor().type() == type); + + iter->second->getDescriptor().copyTo(_dataTree.row(i)); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); + _mapIdIndex.insert(_mapIdIndex.end(), std::pair(iter->second->id(), i)); + } + + ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim); + ULOGGER_DEBUG("copying data = %f s", timer.ticks()); + + switch(_strategy) + { + case kNNFlannNaive: + _flannIndex->build(_dataTree, flann::LinearIndexParams(), type != CV_32F); + break; + case kNNFlannKdTree: + UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); + _flannIndex->build(_dataTree, flann::KDTreeIndexParams(), false); + break; + case kNNFlannLSH: + UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); + _flannIndex->build(_dataTree, flann::LshIndexParams(12, 20, 2), true); + break; + default: + break; + } + + ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); + } } UDEBUG("Dictionary updated! (size=%d->%d added=%d removed=%d)", oldSize, _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); @@ -370,6 +574,7 @@ void VWDictionary::clear() _lastWordId = 0; _dataTree = cv::Mat(); _mapIndexId.clear(); + _mapIdIndex.clear(); _unusedWords.clear(); _flannIndex->release(); } @@ -544,10 +749,15 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, { for(int j=0; j(i,j) >= 0) + float d = dists.at(i,j); + int id = uValue(_mapIndexId, results.at(i,j)); + if(d >= 0.0f && id > 0) { - float d = dists.at(i,j); - fullResults.insert(std::pair(d, uValue(_mapIndexId, results.at(i,j)))); + std::multimap::iterator iter = fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -555,10 +765,15 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, { for(unsigned int j=0; j= 0) + float d = matches.at(i).at(j).distance; + int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) { - float d = matches.at(i).at(j).distance; - fullResults.insert(std::pair(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx))); + std::multimap::iterator iter = fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -566,8 +781,8 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, // Check if this descriptor matches with a word from the last signature (a word not already added to the tree) if(_newWordsComparedTogether && newWords.rows) { - cv::flann::Index linearSeach; - linearSeach.build(newWords, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); + FlannIndex linearSeach; + linearSeach.build(newWords, flann::LinearIndexParams(), type != CV_32F); cv::Mat resultsLinear; cv::Mat distsLinear; linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1); @@ -582,10 +797,15 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, { for(int j=0; j(0,j) >= 0) + float d = distsLinear.at(0,j); + if(d >= 0.0f && resultsLinear.at(0,j) >= 0) { - float d = distsLinear.at(0,j); - fullResults.insert(std::pair(d, newWordsId[resultsLinear.at(0,j)])); + std::multimap::iterator iter = fullResults.insert(std::pair(d, newWordsId[resultsLinear.at(0,j)])); + UASSERT(iter->second > 0); + } + else + { + break; } } } @@ -637,7 +857,6 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, this->addWordRef(fullResults.begin()->second, signatureId); wordIds.push_back(fullResults.begin()->second); - UASSERT(fullResults.begin()->second>0); } } else if(fullResults.size()) @@ -786,8 +1005,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const // Find nearest neighbor ULOGGER_DEBUG("Searching in words not indexed..."); - cv::flann::Index linearSeach; - linearSeach.build(dataNotIndexed, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); + FlannIndex linearSeach; + linearSeach.build(dataNotIndexed, flann::LinearIndexParams(), type != CV_32F); linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1); // In case of binary descriptors if(distsNotIndexed.type() == CV_32S) @@ -806,10 +1025,11 @@ std::vector VWDictionary::findNN(const std::list & vws) const { for(int j=0; j(i,j) > 0) + float d = dists.at(i,j); + int id = uValue(_mapIndexId, results.at(i,j)); + if(d >= 0.0f && id > 0) { - float d = dists.at(i,j); - fullResults.insert(std::pair(d, uValue(_mapIndexId, results.at(i,j)))); + fullResults.insert(std::pair(d, id)); } } } @@ -817,10 +1037,11 @@ std::vector VWDictionary::findNN(const std::list & vws) const { for(unsigned int j=0; j 0) + float d = matches.at(i).at(j).distance; + int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) { - float d = matches.at(i).at(j).distance; - fullResults.insert(std::pair(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx))); + fullResults.insert(std::pair(d, id)); } } } @@ -828,10 +1049,11 @@ std::vector VWDictionary::findNN(const std::list & vws) const // not indexed.. for(int j=0; j(i,j) > 0) + float d = distsNotIndexed.at(i,j); + if(d >= 0.0f && resultsNotIndexed.at(i,j) > 0) { - float d = distsNotIndexed.at(i,j); - fullResults.insert(std::pair(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at(i,j)))); + std::multimap::iterator iter = fullResults.insert(std::pair(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at(i,j)))); + UASSERT(iter->second > 0); } } diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index af2d2927..cfeb8999 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + namespace rtabmap { AboutDialog::AboutDialog(QWidget * parent) : @@ -53,6 +55,7 @@ AboutDialog::AboutDialog(QWidget * parent) : #endif _ui->label_version->setText(version); _ui->label_opencv_version->setText(cv_version); + _ui->label_flann_version->setText(FLANN_VERSION_); _ui->label_pcl_version->setText(PCL_VERSION_PRETTY); _ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No"); _ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No"); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 9ebb2d42..fa609300 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -121,6 +121,10 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) add_definitions(${PCL_DEFINITIONS}) +IF(FLANN18_FOUND) + ADD_DEFINITIONS("-DWITH_FLANN18") +ENDIF(FLANN18_FOUND) + # create a library from the source files ADD_LIBRARY(rtabmap_gui ${SRC_FILES}) # Linking with Qt libraries diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index be3e2d2c..2f4c4a05 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -490,6 +490,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : //Keypoint-based _ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str()); _ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str()); + _ui->checkBox_kp_incrementalFlann->setObjectName(Parameters::kKpIncrementalFlann().c_str()); +#ifndef WITH_FLANN18 + _ui->checkBox_kp_incrementalFlann->setEnabled(false); + _ui->checkBox_kp_incrementalFlann->setChecked(false); +#endif _ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str()); _ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str()); _ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str()); diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index b9937a35..7e534623 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -6,8 +6,8 @@ 0 0 - 824 - 581 + 831 + 615 @@ -82,21 +82,31 @@ p, li { white-space: pre-wrap; } - + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + With Freenect : - + With OpenNI2 : - + With stereo dc1394 : @@ -202,24 +212,14 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - + With g2o : - + @@ -229,7 +229,7 @@ p, li { white-space: pre-wrap; } - + @@ -239,7 +239,7 @@ p, li { white-space: pre-wrap; } - + @@ -249,21 +249,21 @@ p, li { white-space: pre-wrap; } - + With Freenect2 : - + With stereo FlyCapture2 : - + @@ -273,7 +273,7 @@ p, li { white-space: pre-wrap; } - + @@ -283,7 +283,7 @@ p, li { white-space: pre-wrap; } - + @@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; } - + With cvsba : - + @@ -310,14 +310,14 @@ p, li { white-space: pre-wrap; } - + With GTSAM : - + @@ -327,6 +327,23 @@ p, li { white-space: pre-wrap; } + + + + FLANN version : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 6716e79b..fff68dba 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -509 + -349 760 1598 @@ -86,7 +86,7 @@ QFrame::Raised - 19 + 9 @@ -4973,6 +4973,29 @@ When set to false, no new words are added to dictionary, so no more updates are + + + + When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + diff --git a/utilite/include/rtabmap/utilite/UStl.h b/utilite/include/rtabmap/utilite/UStl.h index a498fcea..c343d939 100644 --- a/utilite/include/rtabmap/utilite/UStl.h +++ b/utilite/include/rtabmap/utilite/UStl.h @@ -736,4 +736,26 @@ inline bool uStrContains(const std::string & string, const std::string & substri return string.find(substring) != std::string::npos; } +inline int uCompareVersion(const std::string & version, int major, int minor=-1, int patch=-1) +{ + std::vector v = uListToVector(uSplit(version, '.')); + if(v.size() == 3) + { + int vMajor = atoi(v[0].c_str()); + int vMinor = atoi(v[1].c_str()); + int vPatch = atoi(v[2].c_str()); + if(vMajor > major || + (vMajor == major && minor!=-1 && vMinor > minor) || + (vMajor == major && minor!=-1 && vMinor == minor && patch!=-1 && vPatch > patch)) + { + return 1; + } + else if(vMajor == major && (minor == -1 || (vMinor == minor && (patch == -1 || vPatch == patch)))) + { + return 0; + } + } + return -1; +} + #endif /* USTL_H */ From 3be4b9a3f5927355987d10874e1bb8fece787b70 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 01:30:40 -0400 Subject: [PATCH 048/110] Mac OSX: fixed build error "atoi not defined" --- utilite/include/rtabmap/utilite/UStl.h | 1 + 1 file changed, 1 insertion(+) diff --git a/utilite/include/rtabmap/utilite/UStl.h b/utilite/include/rtabmap/utilite/UStl.h index c343d939..560be22c 100644 --- a/utilite/include/rtabmap/utilite/UStl.h +++ b/utilite/include/rtabmap/utilite/UStl.h @@ -26,6 +26,7 @@ #include #include #include +#include /** * \file UStl.h From beab2271f087e289c3844f31249272e8588f0fb5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 12:08:51 -0400 Subject: [PATCH 049/110] Added statistics for incremental flann index size --- corelib/include/rtabmap/core/Statistics.h | 2 + corelib/include/rtabmap/core/VWDictionary.h | 2 + corelib/src/Rtabmap.cpp | 10 ++- corelib/src/VWDictionary.cpp | 82 +++++++++++++++++---- tools/ConsoleApp/main.cpp | 16 +++- 5 files changed, 93 insertions(+), 19 deletions(-) diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index ca81b786..e40a49cf 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -120,6 +120,8 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(TimingMem, Compressing_data, ms); RTABMAP_STATS(Keypoint, Dictionary_size, words); + RTABMAP_STATS(Keypoint, Indexed_words, words); + RTABMAP_STATS(Keypoint, Index_memory_usage, KB); RTABMAP_STATS(Keypoint, Response_threshold,); public: diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index d70ac4c4..79bb4d85 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -75,6 +75,8 @@ public: unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();} int getLastIndexedWordId() const; int getTotalActiveReferences() const {return _totalActiveReferences;} + unsigned int getIndexedWordsCount() const; + unsigned int getIndexMemoryUsed() const; void setNNStrategy(NNStrategy strategy); bool isIncremental() const {return _incrementalDictionary;} void setIncrementalDictionary(); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3631ce55..ee0a2552 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -230,6 +230,8 @@ void Rtabmap::setupLogFiles(bool overwrite) fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n"); fprintf(_foutInt, " 18-Local graph size\n"); fprintf(_foutInt, " 19-Sensor data id\n"); + fprintf(_foutInt, " 20-Indexed words\n"); + fprintf(_foutInt, " 21-Index memory usage (KB)\n"); } ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str()); @@ -2270,6 +2272,8 @@ bool Rtabmap::process( // Surf specific parameters statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize); + statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount()); + statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed()); //Epipolar geometry constraint statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0); @@ -2514,7 +2518,7 @@ bool Rtabmap::process( timeLocalTimeDetection, timeLocalSpaceDetection, timeMapOptimization); - std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", + std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", _loopClosureHypothesis.first, _highestHypothesis.first, (int)signaturesRemoved.size(), @@ -2533,7 +2537,9 @@ bool Rtabmap::process( rehearsalMaxId, rehearsalMaxId>0?1:0, localGraphSize, - data.id()); + data.id(), + _memory->getVWDictionary()->getIndexedWordsCount(), + _memory->getVWDictionary()->getIndexMemoryUsed()); if(_statisticLogsBufferedInRAM) { _bufferedLogsF.push_back(logF); diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 7d89e0a3..d1ed18d2 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -83,6 +83,41 @@ public: } } + unsigned int indexedFeatures() const + { + if(!index_) + { + return 0; + } + if(binaryType_) + { + return ((const flann::Index >*)index_)->size(); + + } + else + { + return ((const flann::Index >*)index_)->size(); + } + } + + // return KB + unsigned int memoryUsed() const + { + if(!index_) + { + return 0; + } + if(binaryType_) + { + return ((const flann::Index >*)index_)->usedMemory()/1000; + + } + else + { + return ((const flann::Index >*)index_)->usedMemory()/1000; + } + } + void build( const cv::Mat & features, const flann::IndexParams& params, @@ -427,6 +462,16 @@ int VWDictionary::getLastIndexedWordId() const } } +unsigned int VWDictionary::getIndexedWordsCount() const +{ + return _flannIndex->indexedFeatures(); +} + +unsigned int VWDictionary::getIndexMemoryUsed() const +{ + return _flannIndex->memoryUsed(); +} + void VWDictionary::update() { ULOGGER_DEBUG(""); @@ -447,21 +492,27 @@ void VWDictionary::update() (_notIndexedWords.size() || _removedIndexedWords.size()) && oldSize) { - for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + if(_notIndexedWords.size()) { - VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); - UASSERT(w); - UASSERT(w->getDescriptor().cols == _dataTree.cols); - UASSERT(w->getDescriptor().type() == _dataTree.type()); - _dataTree.push_back(w->getDescriptor()); - _mapIndexId.insert(_mapIndexId.end(), std::pair(_dataTree.rows-1, w->id())); - std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); - if(!inserted.second) + int i = _dataTree.rows; + _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { - //update to new index - inserted.first->second = _dataTree.rows-1; + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + UASSERT(w); + UASSERT(w->getDescriptor().cols == _dataTree.cols); + UASSERT(w->getDescriptor().type() == _dataTree.type()); + _dataTree.push_back(w->getDescriptor()); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); + if(!inserted.second) + { + //update to new index + inserted.first->second = i; + } + _flannIndex->addPoints(w->getDescriptor()); + ++i; } - _flannIndex->addPoints(w->getDescriptor()); } for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) { @@ -475,6 +526,8 @@ void VWDictionary::update() oldSize) { //just add not indexed words + int i = _dataTree.rows; + _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); @@ -482,9 +535,10 @@ void VWDictionary::update() UASSERT(w->getDescriptor().cols == _dataTree.cols); UASSERT(w->getDescriptor().type() == _dataTree.type()); _dataTree.push_back(w->getDescriptor()); - _mapIndexId.insert(_mapIndexId.end(), std::pair(_dataTree.rows-1, w->id())); - std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); UASSERT(inserted.second); + ++i; } } else diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index 0557c26c..81087809 100644 --- a/tools/ConsoleApp/main.cpp +++ b/tools/ConsoleApp/main.cpp @@ -65,6 +65,7 @@ void showUsage() " -warn Set Log level to Warning (Default Error)\n" " -exit_warn Set exit level to Warning (Default Fatal)\n" " -exit_error Set exit level to Error (Default Fatal)\n" + " -log_console Log to console\n" " -v Get version of RTAB-Map\n" " -input \"path\" Load previous database if it exists.\n"); exit(1); @@ -118,6 +119,7 @@ int main(int argc, char * argv[]) ParametersMap pm; ULogger::Level logLevel = ULogger::kError; ULogger::Level exitLevel = ULogger::kFatal; + bool logConsole = false; for(int i=1; i0.0f?1.0f/rate:0.0f); } else { - camera = new CameraVideo(path, false, 1.0f/rate); + camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f); } if(!camera || !camera->init()) From d83b2606352783f359cc265077c9d228d1a14c11 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 15:50:26 -0400 Subject: [PATCH 050/110] Removed dataTree when using flann incremental index (to save some space) --- corelib/src/VWDictionary.cpp | 130 ++++++++++++++++++++++++----------- 1 file changed, 89 insertions(+), 41 deletions(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index d1ed18d2..c2d955ec 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -58,7 +58,9 @@ class FlannIndex public: FlannIndex(): index_(0), - binaryType_(false) + nextIndex_(0), + featuresType_(0), + featuresDim_(0) { } virtual ~FlannIndex() @@ -70,7 +72,7 @@ public: { if(index_) { - if(binaryType_) + if(featuresType_ == CV_8UC1) { delete (flann::Index >*)index_; @@ -81,6 +83,7 @@ public: } index_ = 0; } + nextIndex_ = 0; } unsigned int indexedFeatures() const @@ -89,7 +92,7 @@ public: { return 0; } - if(binaryType_) + if(featuresType_ == CV_8UC1) { return ((const flann::Index >*)index_)->size(); @@ -107,7 +110,7 @@ public: { return 0; } - if(binaryType_) + if(featuresType_ == CV_8UC1) { return ((const flann::Index >*)index_)->usedMemory()/1000; @@ -120,14 +123,15 @@ public: void build( const cv::Mat & features, - const flann::IndexParams& params, - bool binaryType) + const flann::IndexParams& params) { this->release(); UASSERT(index_ == 0); - binaryType_ = binaryType; + UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); + featuresType_ = features.type(); + featuresDim_ = features.cols; - if(binaryType) + if(featuresType_ == CV_8UC1) { flann::Matrix dataset(features.data, features.rows, features.cols); index_ = new flann::Index >(dataset, params); @@ -139,6 +143,7 @@ public: index_ = new flann::Index >(dataset, params); ((flann::Index >*)index_)->buildIndex(); } + nextIndex_ = features.rows; } bool isIncremental() @@ -150,28 +155,51 @@ public: #endif } - void addPoints(const cv::Mat & features) + bool isBuilt() + { + return index_!=0; + } + + int featuresType() const {return featuresType_;} + int featuresDim() const {return featuresDim_;} + + unsigned int addPoint(const cv::Mat & feature) { #ifdef WITH_FLANN18 - if(binaryType_) + if(!index_) { - flann::Matrix dataset(features.data, features.rows, features.cols); + UERROR("Flann index not yet created!"); + return 0; + } + UASSERT(feature.type() == featuresType_); + UASSERT(feature.cols == featuresDim_); + UASSERT(feature.rows == 1); + if(featuresType_ == CV_8UC1) + { + flann::Matrix dataset(feature.data, feature.rows, feature.cols); ((flann::Index >*)index_)->addPoints(dataset); } else { - flann::Matrix dataset((float*)features.data, features.rows, features.cols); + flann::Matrix dataset((float*)feature.data, feature.rows, feature.cols); ((flann::Index >*)index_)->addPoints(dataset); } + return nextIndex_++; #else UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method."); + return 0; #endif } void removePoint(unsigned int index) { #ifdef WITH_FLANN18 - if(binaryType_) + if(!index_) + { + UERROR("Flann index not yet created!"); + return; + } + if(featuresType_ == CV_8UC1) { ((flann::Index >*)index_)->removePoint(index); } @@ -197,13 +225,13 @@ public: return; } indices.create(query.rows, knn, CV_32S); - dists.create(query.rows, knn, binaryType_?CV_32S:CV_32F); + dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F); cv::flann::IndexParams i; flann::Matrix indicesF((int*)indices.data, indices.rows, indices.cols); - if(binaryType_) + if(featuresType_ == CV_8UC1) { flann::Matrix distsF((unsigned int*)dists.data, dists.rows, dists.cols); flann::Matrix queryF(query.data, query.rows, query.cols); @@ -219,7 +247,9 @@ public: private: void * index_; - bool binaryType_; + unsigned int nextIndex_; + int featuresType_; + int featuresDim_; }; const int VWDictionary::ID_START = 1; @@ -485,33 +515,51 @@ void VWDictionary::update() if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) { - int oldSize = _dataTree.rows; if(_incrementalFlann && _flannIndex->isIncremental() && _strategy < kNNBruteForce && - (_notIndexedWords.size() || _removedIndexedWords.size()) && - oldSize) + _visualWords.size()) { if(_notIndexedWords.size()) { - int i = _dataTree.rows; - _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); UASSERT(w); - UASSERT(w->getDescriptor().cols == _dataTree.cols); - UASSERT(w->getDescriptor().type() == _dataTree.type()); - _dataTree.push_back(w->getDescriptor()); - _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); - std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); + int index = 0; + if(!_flannIndex->isBuilt()) + { + switch(_strategy) + { + case kNNFlannNaive: + _flannIndex->build(w->getDescriptor(), flann::LinearIndexParams()); + break; + case kNNFlannKdTree: + UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); + _flannIndex->build(w->getDescriptor(), flann::KDTreeIndexParams()); + break; + case kNNFlannLSH: + UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); + _flannIndex->build(w->getDescriptor(), flann::LshIndexParams(12, 20, 2)); + break; + default: + UFATAL("Not supposed to be here!"); + break; + } + } + else + { + UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim()); + UASSERT(w->getDescriptor().type() == _flannIndex->featuresType()); + index = _flannIndex->addPoint(w->getDescriptor()); + } + _mapIndexId.insert(_mapIndexId.end(), std::pair(index, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), index)); if(!inserted.second) { //update to new index - inserted.first->second = i; + inserted.first->second = index; } - _flannIndex->addPoints(w->getDescriptor()); - ++i; } } for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) @@ -523,7 +571,7 @@ void VWDictionary::update() else if(_strategy >= kNNBruteForce && _notIndexedWords.size() && _removedIndexedWords.size() == 0 && - oldSize) + _visualWords.size()) { //just add not indexed words int i = _dataTree.rows; @@ -578,15 +626,15 @@ void VWDictionary::update() switch(_strategy) { case kNNFlannNaive: - _flannIndex->build(_dataTree, flann::LinearIndexParams(), type != CV_32F); + _flannIndex->build(_dataTree, flann::LinearIndexParams()); break; case kNNFlannKdTree: UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->build(_dataTree, flann::KDTreeIndexParams(), false); + _flannIndex->build(_dataTree, flann::KDTreeIndexParams()); break; case kNNFlannLSH: UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->build(_dataTree, flann::LshIndexParams(12, 20, 2), true); + _flannIndex->build(_dataTree, flann::LshIndexParams(12, 20, 2)); break; default: break; @@ -595,8 +643,8 @@ void VWDictionary::update() ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); } } - UDEBUG("Dictionary updated! (size=%d->%d added=%d removed=%d)", - oldSize, _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); + UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)", + _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); } else { @@ -730,7 +778,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, UTimer timerLocal; timerLocal.start(); - if(!_dataTree.empty() && _dataTree.rows >= (int)k) + if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k)) { //Find nearest neighbors UDEBUG("newPts.total()=%d ", descriptors.rows); @@ -807,7 +855,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, int id = uValue(_mapIndexId, results.at(i,j)); if(d >= 0.0f && id > 0) { - std::multimap::iterator iter = fullResults.insert(std::pair(d, id)); + fullResults.insert(std::pair(d, id)); } else { @@ -823,7 +871,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); if(d >= 0.0f && id > 0) { - std::multimap::iterator iter = fullResults.insert(std::pair(d, id)); + fullResults.insert(std::pair(d, id)); } else { @@ -836,7 +884,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, if(_newWordsComparedTogether && newWords.rows) { FlannIndex linearSeach; - linearSeach.build(newWords, flann::LinearIndexParams(), type != CV_32F); + linearSeach.build(newWords, flann::LinearIndexParams()); cv::Mat resultsLinear; cv::Mat distsLinear; linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1); @@ -977,7 +1025,7 @@ std::vector VWDictionary::findNN(const std::list & vws) const } ULOGGER_DEBUG("Preparation time = %fs", timer.ticks()); - if(!_dataTree.empty() && _dataTree.rows >= (int)k) + if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k)) { //Find nearest neighbors UDEBUG("newPts.total()=%d ", query.total()); @@ -1060,7 +1108,7 @@ std::vector VWDictionary::findNN(const std::list & vws) const // Find nearest neighbor ULOGGER_DEBUG("Searching in words not indexed..."); FlannIndex linearSeach; - linearSeach.build(dataNotIndexed, flann::LinearIndexParams(), type != CV_32F); + linearSeach.build(dataNotIndexed, flann::LinearIndexParams()); linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1); // In case of binary descriptors if(distsNotIndexed.type() == CV_32S) From e78071b42774c9993671173be57c3d06882f0608 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 15:58:41 -0400 Subject: [PATCH 051/110] fixed bruteforce nn initialization --- corelib/src/VWDictionary.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index c2d955ec..2321c847 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -571,7 +571,8 @@ void VWDictionary::update() else if(_strategy >= kNNBruteForce && _notIndexedWords.size() && _removedIndexedWords.size() == 0 && - _visualWords.size()) + _visualWords.size() && + _dataTree.rows) { //just add not indexed words int i = _dataTree.rows; From acc47fa9a991739b375fd9cdfd8979125dd433fd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 16:36:59 -0400 Subject: [PATCH 052/110] Fixed Windows build with flann<1.8 --- CMakeLists.txt | 7 +++- cmake_modules/FindFLANN.cmake | 4 +- corelib/src/VWDictionary.cpp | 75 ++++++++++++++++++++++++++++++++--- 3 files changed, 77 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 211ce574..babaec39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,8 +132,11 @@ option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) -FIND_PACKAGE(FLANN 1.8) -SET(FLANN18_FOUND ${FLANN_FOUND}) +find_package(PkgConfig QUIET) +FIND_PACKAGE(FLANN 1.8 QUIET) +IF(PkgConfig_FOUND AND FLANN_FOUND) + SET(FLANN18_FOUND TRUE) +ENDIF(PkgConfig_FOUND AND FLANN_FOUND) FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(PCL 1.7 REQUIRED) # Will find FLANN too diff --git a/cmake_modules/FindFLANN.cmake b/cmake_modules/FindFLANN.cmake index 2404dcba..83b356fe 100644 --- a/cmake_modules/FindFLANN.cmake +++ b/cmake_modules/FindFLANN.cmake @@ -59,7 +59,9 @@ find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARY FLANN_INCLUDE_ mark_as_advanced(FLANN_LIBRARY FLANN_LIBRARY_DEBUG FLANN_INCLUDE_DIR) if(FLANN_FOUND) - message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})") + IF (NOT FLANN_FIND_QUIETLY) + message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})") + ENDIF (NOT FLANN_FIND_QUIETLY) if(FLANN_USE_STATIC) add_definitions(-DFLANN_STATIC) endif(FLANN_USE_STATIC) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 2321c847..5eee86aa 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -60,7 +60,8 @@ public: index_(0), nextIndex_(0), featuresType_(0), - featuresDim_(0) + featuresDim_(0), + isLSH_(false) { } virtual ~FlannIndex() @@ -74,8 +75,20 @@ public: { if(featuresType_ == CV_8UC1) { - delete (flann::Index >*)index_; - + +#ifdef WITH_FLANN18 + delete (flann::Index >*)index_; +#else + // issue with 1.7.1: we should explicitly use the corresponding index + if(isLSH_) + { + delete (flann::LshIndex >*)index_; + } + else + { + delete (flann::LinearIndex >*)index_; + } +#endif } else { @@ -84,6 +97,7 @@ public: index_ = 0; } nextIndex_ = 0; + isLSH_ = false; } unsigned int indexedFeatures() const @@ -94,8 +108,19 @@ public: } if(featuresType_ == CV_8UC1) { +#ifdef WITH_FLANN18 return ((const flann::Index >*)index_)->size(); - +#else + // issue with 1.7.1: we should explicitly use the corresponding index + if(isLSH_) + { + return ((const flann::LshIndex >*)index_)->size(); + } + else + { + return ((const flann::LinearIndex >*)index_)->size(); + } +#endif } else { @@ -111,9 +136,20 @@ public: return 0; } if(featuresType_ == CV_8UC1) - { + { +#ifdef WITH_FLANN18 return ((const flann::Index >*)index_)->usedMemory()/1000; - +#else + // issue with 1.7.1: we should explicitly use the corresponding index + if(isLSH_) + { + return ((const flann::LshIndex >*)index_)->usedMemory()/1000; + } + else + { + return ((const flann::LinearIndex >*)index_)->usedMemory()/1000; + } +#endif } else { @@ -134,7 +170,21 @@ public: if(featuresType_ == CV_8UC1) { flann::Matrix dataset(features.data, features.rows, features.cols); +#ifdef WITH_FLANN18 index_ = new flann::Index >(dataset, params); +#else + // issue with 1.7.1: we should explicitly create the corresponding index + flann::flann_algorithm_t algo = params.at("algorithm").cast(); + if(algo == flann::FLANN_INDEX_LSH) + { + isLSH_ = true; + index_ = new flann::LshIndex >(dataset, params); + } + else + { + index_ = new flann::LinearIndex >(dataset, params); + } +#endif ((flann::Index >*)index_)->buildIndex(); } else @@ -235,7 +285,19 @@ public: { flann::Matrix distsF((unsigned int*)dists.data, dists.rows, dists.cols); flann::Matrix queryF(query.data, query.rows, query.cols); +#ifdef WITH_FLANN18 ((flann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); +#else + // issue with 1.7.1: we should explicitly use the corresponding index + if(isLSH_) + { + ((flann::LshIndex >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } + else + { + ((flann::LinearIndex >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } +#endif } else { @@ -250,6 +312,7 @@ private: unsigned int nextIndex_; int featuresType_; int featuresDim_; + bool isLSH_; }; const int VWDictionary::ID_START = 1; From 1b13857693373c08e2064e22817ec28b788430e3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 16:44:55 -0400 Subject: [PATCH 053/110] fixed how flann 1.8 is detected --- CMakeLists.txt | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index babaec39..f881096f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,9 +134,9 @@ option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) find_package(PkgConfig QUIET) FIND_PACKAGE(FLANN 1.8 QUIET) -IF(PkgConfig_FOUND AND FLANN_FOUND) +IF(PKG_CONFIG_FOUND AND FLANN_FOUND) SET(FLANN18_FOUND TRUE) -ENDIF(PkgConfig_FOUND AND FLANN_FOUND) +ENDIF(PKG_CONFIG_FOUND AND FLANN_FOUND) FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(PCL 1.7 REQUIRED) # Will find FLANN too @@ -392,6 +392,12 @@ IF(OpenCV_FOUND) ENDIF() ENDIF(OpenCV_FOUND) +IF(FLANN18_FOUND) +MESSAGE(STATUS " With FLANN >= 1.8 = YES") +ELSE() +MESSAGE(STATUS " With FLANN >= 1.8 = NO") +ENDIF() + IF(Freenect_FOUND) MESSAGE(STATUS " With Freenect = YES") ELSEIF(NOT WITH_FREENECT) From 9a037b33ff7c482b44fc08887905a6c7d1d98165 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 17:15:42 -0400 Subject: [PATCH 054/110] Changed LinearIndex to Bruteforce for nn with not yet indexed words --- corelib/src/VWDictionary.cpp | 66 ++++++++++++++---------------------- 1 file changed, 25 insertions(+), 41 deletions(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 5eee86aa..0859155a 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -947,32 +947,21 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, // Check if this descriptor matches with a word from the last signature (a word not already added to the tree) if(_newWordsComparedTogether && newWords.rows) { - FlannIndex linearSeach; - linearSeach.build(newWords, flann::LinearIndexParams()); - cv::Mat resultsLinear; - cv::Mat distsLinear; - linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1); - // In case of binary descriptors - if(distsLinear.type() == CV_32S) + std::vector > matchesNewWords; + cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1); + UASSERT(matchesNewWords.size() == 1); + for(unsigned int j=0; j= 0.0f && id > 0) { - float d = distsLinear.at(0,j); - if(d >= 0.0f && resultsLinear.at(0,j) >= 0) - { - std::multimap::iterator iter = fullResults.insert(std::pair(d, newWordsId[resultsLinear.at(0,j)])); - UASSERT(iter->second > 0); - } - else - { - break; - } + fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -1153,9 +1142,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const } ULOGGER_DEBUG("Search dictionary time = %fs", timer.ticks()); - cv::Mat resultsNotIndexed; - cv::Mat distsNotIndexed; std::map mapIndexIdNotIndexed; + std::vector > matchesNotIndexed; if(_notIndexedWords.size()) { cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type); @@ -1171,16 +1159,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const // Find nearest neighbor ULOGGER_DEBUG("Searching in words not indexed..."); - FlannIndex linearSeach; - linearSeach.build(dataNotIndexed, flann::LinearIndexParams()); - linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1); - // In case of binary descriptors - if(distsNotIndexed.type() == CV_32S) - { - cv::Mat temp; - distsNotIndexed.convertTo(temp, CV_32F); - distsNotIndexed = temp; - } + cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1); } ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks()); @@ -1213,13 +1193,17 @@ std::vector VWDictionary::findNN(const std::list & vws) const } // not indexed.. - for(int j=0; j(i,j); - if(d >= 0.0f && resultsNotIndexed.at(i,j) > 0) + float d = matchesNotIndexed.at(i).at(j).distance; + int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) { - std::multimap::iterator iter = fullResults.insert(std::pair(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at(i,j)))); - UASSERT(iter->second > 0); + fullResults.insert(std::pair(d, id)); + } + else + { + break; } } From e3161c3b04c84fd52982342670ecf604c1f0248d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 20:11:18 -0400 Subject: [PATCH 055/110] Fixed vector out of bound error (findNN() matchesNotIndexed vector). Fixed error "map correction should be identity" when small displacement detected before scan matching --- corelib/src/Rtabmap.cpp | 25 ++++++++++++++----------- corelib/src/VWDictionary.cpp | 21 ++++++++++++--------- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index ee0a2552..51c59c74 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1052,7 +1052,7 @@ bool Rtabmap::process( // Update optimizedPoses with the newly added node Transform newPose; - if(signature->getLinks().size() == 1) + if(signature->getLinks().size() == 1 && !smallDisplacement) { int oldId = signature->getLinks().begin()->first; const Signature * oldS = _memory->getSignature(oldId); @@ -1112,7 +1112,10 @@ bool Rtabmap::process( _mapCorrection = newPose * signature->getPose().inverse(); if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) { - UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); + UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s", + _mapCorrection.prettyPrint().c_str(), + newPose.prettyPrint().c_str(), + signature->getPose().prettyPrint().c_str()); } } else @@ -2348,9 +2351,9 @@ bool Rtabmap::process( // Pass this point signature should not be used, since it could have been transferred... signature = 0; - + timeMemoryCleanup = timer.ticks(); - ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); + ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); @@ -2518,7 +2521,7 @@ bool Rtabmap::process( timeLocalTimeDetection, timeLocalSpaceDetection, timeMapOptimization); - std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", + std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", _loopClosureHypothesis.first, _highestHypothesis.first, (int)signaturesRemoved.size(), @@ -3060,8 +3063,8 @@ void Rtabmap::getGraph( std::map & poses, std::multimap & constraints, bool optimized, - bool global, - std::map * signatures) + bool global, + std::map * signatures) { if(_memory && _memory->getLastWorkingSignature()) { @@ -3083,8 +3086,8 @@ void Rtabmap::getGraph( std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - - if(signatures) + + if(signatures) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { @@ -3100,7 +3103,7 @@ void Rtabmap::getGraph( weight, stamp, label, - odomPose))); + odomPose))); } } } @@ -3243,7 +3246,7 @@ bool Rtabmap::computePath(const Transform & targetPose) links.insert(std::make_pair(jter->second.from(), jter->second.to())); links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> } - } + } UINFO("Time getting links = %fs", timer.ticks()); int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 0859155a..eedbe454 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -1193,17 +1193,20 @@ std::vector VWDictionary::findNN(const std::list & vws) const } // not indexed.. - for(unsigned int j=0; j= 0.0f && id > 0) + for(unsigned int j=0; j(d, id)); - } - else - { - break; + float d = matchesNotIndexed.at(i).at(j).distance; + int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) + { + fullResults.insert(std::pair(d, id)); + } + else + { + break; + } } } From 7c85f11fbba19753cfbcbf693c8395dbf5240f14 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 21:09:51 -0400 Subject: [PATCH 056/110] fixed goal events sent by DBReader --- corelib/src/DBReader.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index e28b7839..7bd5ae2a 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -205,19 +205,19 @@ void DBReader::mainLoop() double delay = stamp - previousStamp; UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...", goalId, delay); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); uSleep(delay*1000); } else { UWARN("Goal %d detected, posting it!", goalId); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); } } else { UWARN("Goal %d detected, posting it!", goalId); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); } } From 91d87ec6a8ae86e860dabbf5a775b3d58e0b6e1a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 22:31:02 -0400 Subject: [PATCH 057/110] fixed null variance assert on local loop closures in Time/Space --- corelib/src/Rtabmap.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 51c59c74..00a36da4 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1079,7 +1079,7 @@ bool Rtabmap::process( oldId, signature->getLinks().at(oldId).transform().prettyPrint().c_str(), t.prettyPrint().c_str()); - _memory->updateLink(signature->id(), oldId, t, variance, variance); + _memory->updateLink(signature->id(), oldId, t, variance>0?variance:0.0001, variance>0?variance:0.0001); if(_optimizeFromGraphEnd) { @@ -1180,7 +1180,7 @@ bool Rtabmap::process( *iter, transform.prettyPrint().c_str()); // Add a loop constraint - if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance))) + if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001))) { ++localLoopClosuresInTimeFound; UINFO("Local loop closure found between %d and %d with t=%s", @@ -1952,7 +1952,7 @@ bool Rtabmap::process( signature->id(), nearestId, transform.prettyPrint().c_str()); - _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); + _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001)); loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); if(_loopClosureHypothesis.first == 0) From a8c3899f5270b8a3b725b4c87cd1d91a59286867 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 10 Sep 2015 12:34:52 -0400 Subject: [PATCH 058/110] Improved performance of Bayes prediction matrix update --- corelib/src/BayesFilter.cpp | 71 +++++++++++++++++++++++++++++++------ 1 file changed, 60 insertions(+), 11 deletions(-) diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index a9142dc5..5d932b33 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -451,6 +451,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, UDEBUG("time getting removed ids = %fs", timer.restart()); int added = 0; + float epsilon = 0.00001f; // get ids to update std::set idsToUpdate; for(unsigned int i=0; i epsilon && j!=i && removedIds.find(oldIds[j]) == removedIds.end()) { //UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]); idsToUpdate.insert(oldIds[j]); + ++count; } } + UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count); } } if(iaddNeighborProb(prediction, i, neighbors, newIdToIndexMap); this->normalize(prediction, i, sum, newIds[0]<0); ++added; + int count = 0; for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) { if(uContains(oldIdToIndexMap, iter->first) && removedIds.find(iter->first) == removedIds.end()) { idsToUpdate.insert(iter->first); + ++count; } } + UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count); } } - UDEBUG("time getting ids to update = %fs", timer.restart()); + UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart()); // update modified/added ids int modified = 0; + std::set idsDone; for(std::set::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); - int index = newIdToIndexMap.at(*iter); - float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); - this->normalize(prediction, index, sum, newIds[0]<0); - ++modified; + if(idsDone.find(*iter) == idsDone.end() && *iter > 0) + { + std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); + + std::list idsLoopMargin; + //filter neighbors in STM + for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end();) + { + if(memory->isInSTM(jter->first)) + { + neighbors.erase(jter++); + } + else + { + if(jter->second == 0) + { + idsLoopMargin.push_back(jter->first); + } + ++jter; + } + } + + // should at least have 1 id in idsMarginLoop + if(idsLoopMargin.size() == 0) + { + UFATAL("No 0 margin neighbor for signature %d !?!?", *iter); + } + + // same neighbor tree for loop signatures (margin = 0) + for(std::list::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) + { + int index = newIdToIndexMap.at(*iter); + float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); + idsDone.insert(*iter); + this->normalize(prediction, index, sum, newIds[0]<0); + ++modified; + } + } } - UDEBUG("time updating modified/added ids = %fs", timer.restart()); + UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart()); //UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows); //UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows); @@ -510,15 +551,22 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, { if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end()) { - for(int j=0; j epsilon) { //UDEBUG("i=%d, j=%d", i, j); //UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]); //UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i])); //UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j])); - ((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols]; + float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols]; + int ii = newIdToIndexMap.at(oldIds[i]); + int jj = newIdToIndexMap.at(oldIds[j]); + ((float *)prediction.data)[ii + jj*prediction.cols] = v; + if(ii != jj) + { + ((float *)prediction.data)[jj + ii*prediction.cols] = v; + } } } ++copied; @@ -536,6 +584,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, for(int j=1; j0) From 4f6eb5ce0b6f4d21f71c12c5aa633d01df50c552 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 10 Sep 2015 15:58:10 -0400 Subject: [PATCH 059/110] Added noEventsExample --- corelib/include/rtabmap/core/SensorData.h | 4 +- examples/CMakeLists.txt | 1 + examples/NoEventsExample/CMakeLists.txt | 33 +++ examples/NoEventsExample/MapBuilder.h | 241 ++++++++++++++++++++++ examples/NoEventsExample/main.cpp | 168 +++++++++++++++ 5 files changed, 445 insertions(+), 2 deletions(-) create mode 100644 examples/NoEventsExample/CMakeLists.txt create mode 100644 examples/NoEventsExample/MapBuilder.h create mode 100644 examples/NoEventsExample/main.cpp diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index e1fa028a..2b2533b5 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -136,8 +136,8 @@ public: _laserScanCompressed.empty() && _cameraModels.size() == 0 && !_stereoCameraModel.isValid() && - !_userDataRaw.empty() && - !_userDataCompressed.empty() && + _userDataRaw.empty() && + _userDataCompressed.empty() && _keypoints.size() == 0 && _descriptors.empty()); } diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f3a992e2..e619619c 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,6 +4,7 @@ ADD_SUBDIRECTORY( BOWMapping ) IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( RGBDMapping ) ADD_SUBDIRECTORY( WifiMapping ) + ADD_SUBDIRECTORY( NoEventsExample ) ELSE() MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...") ENDIF() diff --git a/examples/NoEventsExample/CMakeLists.txt b/examples/NoEventsExample/CMakeLists.txt new file mode 100644 index 00000000..b30e40eb --- /dev/null +++ b/examples/NoEventsExample/CMakeLists.txt @@ -0,0 +1,33 @@ + +SET(INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/utilite/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/guilib/include + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +IF("${RTABMAP_QT_VERSION}" STREQUAL "4") + INCLUDE(${QT_USE_FILE}) +ENDIF() + +SET(LIBRARIES + ${OpenCV_LIBRARIES} + ${QT_LIBRARIES} + ${PCL_LIBRARIES} +) + +INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) + +IF("${RTABMAP_QT_VERSION}" STREQUAL "4") + QT4_WRAP_CPP(moc_srcs MapBuilder.h) +ELSE() + QT5_WRAP_CPP(moc_srcs MapBuilder.h) +ENDIF() + +ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs}) + +TARGET_LINK_LIBRARIES(noEventsExample rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) + +SET_TARGET_PROPERTIES( noEventsExample + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample) diff --git a/examples/NoEventsExample/MapBuilder.h b/examples/NoEventsExample/MapBuilder.h new file mode 100644 index 00000000..6c85ff85 --- /dev/null +++ b/examples/NoEventsExample/MapBuilder.h @@ -0,0 +1,241 @@ +/* +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 MAPBUILDER_H_ +#define MAPBUILDER_H_ + +#include +#include +#include + +#ifndef Q_MOC_RUN // Mac OS X issue +#include "rtabmap/gui/CloudViewer.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/Statistics.h" +#include "rtabmap/core/Signature.h" +#endif +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/ULogger.h" + +using namespace rtabmap; + +// This class receives RtabmapEvent and construct/update a 3D Map +class MapBuilder : public QWidget +{ + Q_OBJECT +public: + //Camera ownership is not transferred! + MapBuilder() : + odometryCorrection_(Transform::getIdentity()), + paused_(false) + { + this->setWindowFlags(Qt::Dialog); + this->setWindowTitle(tr("3D Map")); + this->setMinimumWidth(800); + this->setMinimumHeight(600); + + cloudViewer_ = new CloudViewer(this); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(cloudViewer_); + this->setLayout(layout); + + QAction * pause = new QAction(this); + this->addAction(pause); + pause->setShortcut(Qt::Key_Space); + connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection())); + } + + virtual ~MapBuilder() + { + } + + bool isPaused() const {return paused_;} + + void processOdometry( + const SensorData & data, + Transform pose, + const rtabmap::OdometryInfo & odom) + { + if(!this->isVisible()) + { + return; + } + + if(pose.isNull()) + { + //Odometry lost + cloudViewer_->setBackgroundColor(Qt::darkRed); + + pose = lastOdomPose_; + } + else + { + cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor()); + } + if(!pose.isNull()) + { + lastOdomPose_ = pose; + + // 3d cloud + if(data.depthOrRightRaw().cols == data.imageRaw().cols && + data.depthOrRightRaw().rows == data.imageRaw().rows && + !data.depthOrRightRaw().empty() && + (data.stereoCameraModel().isValid() || data.cameraModels().size())) + { + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + data, + 4, // decimation + 0.0f); // max depth + if(cloud->size()) + { + if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdom", false); + UWARN("Empty cloudOdom!"); + } + } + + if(!pose.isNull()) + { + // update camera position + cloudViewer_->updateCameraTargetPosition(odometryCorrection_*pose); + } + } + cloudViewer_->update(); + } + + + void processStatistics(const rtabmap::Statistics & stats) + { + + //============================ + // Add RGB-D clouds + //============================ + const std::map & poses = stats.poses(); + QMap clouds = cloudViewer_->getAddedClouds(); + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(!iter->second.isNull()) + { + std::string cloudName = uFormat("cloud%d", iter->first); + + // 3d point cloud + if(clouds.contains(cloudName)) + { + // Update only if the pose has changed + Transform tCloud; + cloudViewer_->getPose(cloudName, tCloud); + if(tCloud.isNull() || iter->second != tCloud) + { + if(!cloudViewer_->updateCloudPose(cloudName, iter->second)) + { + UERROR("Updating pose cloud %d failed!", iter->first); + } + } + cloudViewer_->setCloudVisibility(cloudName, true); + } + else if(uContains(stats.getSignatures(), iter->first)) + { + Signature s = stats.getSignatures().at(iter->first); + s.sensorData().uncompressData(); // make sure data is uncompressed + // Add the new cloud + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + s.sensorData(), + 4, // decimation + 4.0f); // max depth + if(cloud->size()) + { + if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second)) + { + UERROR("Adding cloud %d to viewer failed!", iter->first); + } + } + else + { + UWARN("Empty cloud %d!", iter->first); + } + } + } + else + { + UWARN("Null pose for %d ?!?", iter->first); + } + } + + //============================ + // Add 3D graph (show all poses) + //============================ + cloudViewer_->removeAllGraphs(); + cloudViewer_->removeCloud("graph_nodes"); + if(poses.size()) + { + // Set graph + pcl::PointCloud::Ptr graph(new pcl::PointCloud); + pcl::PointCloud::Ptr graphNodes(new pcl::PointCloud); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); + } + *graphNodes = *graph; + + + // add graph + cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray); + cloudViewer_->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green); + cloudViewer_->setCloudPointSize("graph_nodes", 5); + } + + odometryCorrection_ = stats.mapCorrection(); + + cloudViewer_->update(); + } + +protected slots: + void pauseDetection() + { + paused_ = !paused_; + } + +protected: + CloudViewer * cloudViewer_; + Transform lastOdomPose_; + Transform odometryCorrection_; + bool paused_; +}; + + +#endif /* MAPBUILDER_H_ */ diff --git a/examples/NoEventsExample/main.cpp b/examples/NoEventsExample/main.cpp new file mode 100644 index 00000000..c51e50d6 --- /dev/null +++ b/examples/NoEventsExample/main.cpp @@ -0,0 +1,168 @@ +/* +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 +#include +#include +#include +#include "MapBuilder.h" +#include +#include +#include + +using namespace rtabmap; + +void showUsage() +{ + printf("\nUsage:\n" + "rtabmap-noEventsExample camera_rate odom_update map_update calibration_dir calibration_name path_left_images path_right_images\n" + "Description:\n" + " camera_rate Rate (Hz) of the camera.\n" + " odom_update Do odometry update each X camera frames.\n" + " map_update Do map update each X odometry frames.\n" + "\n" + "Example:\n" + " (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n" + " $ rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right\n" + " Camera rate = 20 Hz\n" + " Odometry update rate = 10 Hz\n" + " Map update rate = 1 Hz\n"); + exit(1); +} + +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + + if(argc < 8) + { + showUsage(); + } + + int argIndex = 1; + int cameraRate = atoi(argv[argIndex++]); + if(cameraRate <= 0) + { + printf("camera_rate should be > 0\n"); + showUsage(); + } + int odomUpdate = atoi(argv[argIndex++]); + if(odomUpdate <= 0) + { + printf("odom_update should be > 0\n"); + showUsage(); + } + int mapUpdate = atoi(argv[argIndex++]); + if(mapUpdate <= 0) + { + printf("map_update should be > 0\n"); + showUsage(); + } + + printf("Camera rate = %d Hz\n", cameraRate); + printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate); + printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate); + + std::string calibrationDir = argv[argIndex++]; + std::string calibrationName = argv[argIndex++]; + std::string pathLeftImages = argv[argIndex++]; + std::string pathRightImages = argv[argIndex++]; + + Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); + CameraStereoImages camera( + pathLeftImages, + pathRightImages, + false, // no timestamps in filenames + "", // no file with timestamps + false, // assume that images are already rectified + 0.0f, // inf rate + opticalRotation); + + if(camera.init(calibrationDir, calibrationName)) + { + OdometryBOW odom; + Rtabmap rtabmap; + rtabmap.init(); + + QApplication app(argc, argv); + MapBuilder mapBuilder; + mapBuilder.show(); + QApplication::processEvents(); + float sleepTime = 1.0f/float(cameraRate); + + SensorData data = camera.takeImage(); + int cameraIteration = 0; + int odometryIteration = 0; + printf("Press \"Space\" in the window to pause\n"); + while(data.isValid() && mapBuilder.isVisible()) + { + if(cameraIteration++ % odomUpdate == 0) + { + OdometryInfo info; + Transform pose = odom.process(data, &info); + + if(odometryIteration++ % mapUpdate == 0) + { + if(rtabmap.process(data, pose)) + { + mapBuilder.processStatistics(rtabmap.getStatistics()); + if(rtabmap.getLoopClosureId() > 0) + { + printf("Loop closure detected!\n"); + } + } + } + + mapBuilder.processOdometry(data, pose, info); + } + + QApplication::processEvents(); + + while(mapBuilder.isPaused() && mapBuilder.isVisible()) + { + uSleep(100); + QApplication::processEvents(); + } + + data = camera.takeImage(); + uSleep(sleepTime); + } + + if(mapBuilder.isVisible()) + { + printf("Processed all frames\n"); + app.exec(); + } + } + else + { + UERROR("Camera init failed!"); + } + + return 0; +} From 1836f0fe25ab441fbdb47774c1870a1f5aac37a9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 10 Sep 2015 16:22:21 -0400 Subject: [PATCH 060/110] NoEventsExample updated using camera rate --- examples/NoEventsExample/main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/NoEventsExample/main.cpp b/examples/NoEventsExample/main.cpp index c51e50d6..8043865b 100644 --- a/examples/NoEventsExample/main.cpp +++ b/examples/NoEventsExample/main.cpp @@ -57,7 +57,7 @@ void showUsage() int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kWarning); + ULogger::setLevel(ULogger::kError); if(argc < 8) { @@ -100,7 +100,7 @@ int main(int argc, char * argv[]) false, // no timestamps in filenames "", // no file with timestamps false, // assume that images are already rectified - 0.0f, // inf rate + (float)cameraRate, opticalRotation); if(camera.init(calibrationDir, calibrationName)) @@ -113,7 +113,6 @@ int main(int argc, char * argv[]) MapBuilder mapBuilder; mapBuilder.show(); QApplication::processEvents(); - float sleepTime = 1.0f/float(cameraRate); SensorData data = camera.takeImage(); int cameraIteration = 0; @@ -150,7 +149,6 @@ int main(int argc, char * argv[]) } data = camera.takeImage(); - uSleep(sleepTime); } if(mapBuilder.isVisible()) From b98bc5a948f64afc9dc4ccb26a4a11bbca14c852 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Sep 2015 15:59:01 -0400 Subject: [PATCH 061/110] Flann incremental: remove points before adding new ones --- corelib/src/VWDictionary.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index eedbe454..96a27442 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -583,6 +583,12 @@ void VWDictionary::update() _strategy < kNNBruteForce && _visualWords.size()) { + for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) + { + UASSERT(uContains(_mapIdIndex, *iter)); + _flannIndex->removePoint(_mapIdIndex.at(*iter)); + } + if(_notIndexedWords.size()) { for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) @@ -625,11 +631,6 @@ void VWDictionary::update() } } } - for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) - { - UASSERT(uContains(_mapIdIndex, *iter)); - _flannIndex->removePoint(_mapIdIndex.at(*iter)); - } } else if(_strategy >= kNNBruteForce && _notIndexedWords.size() && From cc5065eff0e8896663eaf25c3889774c5382f560 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Sep 2015 16:53:17 -0400 Subject: [PATCH 062/110] Memory::getNeighborId() load only neighbor links from db when loop links are not required --- corelib/src/Memory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 8799b503..7099c60d 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -990,7 +990,7 @@ std::map Memory::getNeighborsId(int signatureId, ids.insert(std::pair(*jter, m)); UTimer timer; - _dbDriver->loadLinks(*jter, tmpLinks); + _dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kNeighbor:Link::kUndef); if(dbAccessTime) { *dbAccessTime += timer.getElapsedTime(); From 567c6a8a518d420c5b214ca501aad967f59bf2ec Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Sep 2015 18:42:20 -0400 Subject: [PATCH 063/110] VWDictionary removePoint(): added comments about having latest flann version to have a fix avoiding a Segmentation fault on dictionary update (only for flann incremental dictionary) --- corelib/src/VWDictionary.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 96a27442..68a6ce23 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -249,6 +249,12 @@ public: UERROR("Flann index not yet created!"); return; } + + // If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h": + // 707 - if (ids_[id]==id) { + // 707 + if (id < ids_.size() && ids_[id]==id) { + // ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151 + if(featuresType_ == CV_8UC1) { ((flann::Index >*)index_)->removePoint(index); From 1f05cf3299c49dfa182c0ba1d78bf35e8d4b574b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Sep 2015 21:59:50 -0400 Subject: [PATCH 064/110] fixed a warning --- corelib/src/VWDictionary.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 96a27442..b8dcce2a 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -277,8 +277,6 @@ public: indices.create(query.rows, knn, CV_32S); dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F); - cv::flann::IndexParams i; - flann::Matrix indicesF((int*)indices.data, indices.rows, indices.cols); if(featuresType_ == CV_8UC1) From cf0d781020d97028bbde03f7a83bfa061032133d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Sep 2015 23:23:01 -0400 Subject: [PATCH 065/110] Included FLANN headers in rtabmap for convenience (to have commit https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3) --- CMakeLists.txt | 8 +- corelib/src/CMakeLists.txt | 7 +- corelib/src/VWDictionary.cpp | 92 +- corelib/src/flann/algorithms/all_indices.h | 197 +++ .../src/flann/algorithms/autotuned_index.h | 763 +++++++++ corelib/src/flann/algorithms/center_chooser.h | 385 +++++ .../src/flann/algorithms/composite_index.h | 239 +++ corelib/src/flann/algorithms/dist.h | 790 +++++++++ .../hierarchical_clustering_index.h | 724 ++++++++ .../flann/algorithms/kdtree_cuda_3d_index.cu | 842 +++++++++ .../flann/algorithms/kdtree_cuda_3d_index.h | 327 ++++ .../flann/algorithms/kdtree_cuda_builder.h | 729 ++++++++ corelib/src/flann/algorithms/kdtree_index.h | 765 +++++++++ .../flann/algorithms/kdtree_single_index.h | 698 ++++++++ corelib/src/flann/algorithms/kmeans_index.h | 1066 ++++++++++++ corelib/src/flann/algorithms/linear_index.h | 163 ++ corelib/src/flann/algorithms/lsh_index.h | 548 ++++++ corelib/src/flann/algorithms/nn_index.h | 907 ++++++++++ corelib/src/flann/config.h | 38 + corelib/src/flann/config.h.in | 38 + corelib/src/flann/defines.h | 156 ++ corelib/src/flann/ext/lz4.c | 1516 +++++++++++++++++ corelib/src/flann/ext/lz4.h | 360 ++++ corelib/src/flann/ext/lz4hc.c | 731 ++++++++ corelib/src/flann/ext/lz4hc.h | 189 ++ corelib/src/flann/flann.cpp | 1320 ++++++++++++++ corelib/src/flann/flann.h | 609 +++++++ corelib/src/flann/flann.hpp | 435 +++++ corelib/src/flann/flann_cpp.cpp | 30 + corelib/src/flann/general.h | 227 +++ corelib/src/flann/io/hdf5.h | 231 +++ corelib/src/flann/mpi/client.h | 89 + corelib/src/flann/mpi/flann_mpi_client.cpp | 85 + corelib/src/flann/mpi/flann_mpi_server.cpp | 26 + corelib/src/flann/mpi/index.h | 271 +++ corelib/src/flann/mpi/matrix.h | 54 + corelib/src/flann/mpi/queries.h | 103 ++ corelib/src/flann/mpi/server.h | 153 ++ corelib/src/flann/nn/ground_truth.h | 98 ++ corelib/src/flann/nn/index_testing.h | 323 ++++ corelib/src/flann/nn/simplex_downhill.h | 186 ++ corelib/src/flann/readme.txt | 8 + corelib/src/flann/util/allocator.h | 202 +++ corelib/src/flann/util/any.h | 294 ++++ corelib/src/flann/util/cuda/heap.h | 139 ++ corelib/src/flann/util/cuda/result_set.h | 536 ++++++ corelib/src/flann/util/cutil_math.h | 1328 +++++++++++++++ corelib/src/flann/util/dynamic_bitset.h | 163 ++ corelib/src/flann/util/heap.h | 456 +++++ corelib/src/flann/util/logger.h | 137 ++ corelib/src/flann/util/lsh_table.h | 506 ++++++ corelib/src/flann/util/matrix.h | 135 ++ corelib/src/flann/util/object_factory.h | 91 + corelib/src/flann/util/params.h | 139 ++ corelib/src/flann/util/random.h | 145 ++ corelib/src/flann/util/result_set.h | 934 ++++++++++ corelib/src/flann/util/sampling.h | 72 + corelib/src/flann/util/saving.h | 135 ++ corelib/src/flann/util/serialization.h | 813 +++++++++ corelib/src/flann/util/timer.h | 95 ++ guilib/src/AboutDialog.cpp | 3 - guilib/src/CMakeLists.txt | 4 - guilib/src/ui/aboutDialog.ui | 65 +- 63 files changed, 22769 insertions(+), 149 deletions(-) create mode 100644 corelib/src/flann/algorithms/all_indices.h create mode 100644 corelib/src/flann/algorithms/autotuned_index.h create mode 100644 corelib/src/flann/algorithms/center_chooser.h create mode 100644 corelib/src/flann/algorithms/composite_index.h create mode 100644 corelib/src/flann/algorithms/dist.h create mode 100644 corelib/src/flann/algorithms/hierarchical_clustering_index.h create mode 100644 corelib/src/flann/algorithms/kdtree_cuda_3d_index.cu create mode 100644 corelib/src/flann/algorithms/kdtree_cuda_3d_index.h create mode 100644 corelib/src/flann/algorithms/kdtree_cuda_builder.h create mode 100644 corelib/src/flann/algorithms/kdtree_index.h create mode 100644 corelib/src/flann/algorithms/kdtree_single_index.h create mode 100644 corelib/src/flann/algorithms/kmeans_index.h create mode 100644 corelib/src/flann/algorithms/linear_index.h create mode 100644 corelib/src/flann/algorithms/lsh_index.h create mode 100644 corelib/src/flann/algorithms/nn_index.h create mode 100644 corelib/src/flann/config.h create mode 100644 corelib/src/flann/config.h.in create mode 100644 corelib/src/flann/defines.h create mode 100644 corelib/src/flann/ext/lz4.c create mode 100644 corelib/src/flann/ext/lz4.h create mode 100644 corelib/src/flann/ext/lz4hc.c create mode 100644 corelib/src/flann/ext/lz4hc.h create mode 100644 corelib/src/flann/flann.cpp create mode 100644 corelib/src/flann/flann.h create mode 100644 corelib/src/flann/flann.hpp create mode 100644 corelib/src/flann/flann_cpp.cpp create mode 100644 corelib/src/flann/general.h create mode 100644 corelib/src/flann/io/hdf5.h create mode 100644 corelib/src/flann/mpi/client.h create mode 100644 corelib/src/flann/mpi/flann_mpi_client.cpp create mode 100644 corelib/src/flann/mpi/flann_mpi_server.cpp create mode 100644 corelib/src/flann/mpi/index.h create mode 100644 corelib/src/flann/mpi/matrix.h create mode 100644 corelib/src/flann/mpi/queries.h create mode 100644 corelib/src/flann/mpi/server.h create mode 100644 corelib/src/flann/nn/ground_truth.h create mode 100644 corelib/src/flann/nn/index_testing.h create mode 100644 corelib/src/flann/nn/simplex_downhill.h create mode 100644 corelib/src/flann/readme.txt create mode 100644 corelib/src/flann/util/allocator.h create mode 100644 corelib/src/flann/util/any.h create mode 100644 corelib/src/flann/util/cuda/heap.h create mode 100644 corelib/src/flann/util/cuda/result_set.h create mode 100644 corelib/src/flann/util/cutil_math.h create mode 100644 corelib/src/flann/util/dynamic_bitset.h create mode 100644 corelib/src/flann/util/heap.h create mode 100644 corelib/src/flann/util/logger.h create mode 100644 corelib/src/flann/util/lsh_table.h create mode 100644 corelib/src/flann/util/matrix.h create mode 100644 corelib/src/flann/util/object_factory.h create mode 100644 corelib/src/flann/util/params.h create mode 100644 corelib/src/flann/util/random.h create mode 100644 corelib/src/flann/util/result_set.h create mode 100644 corelib/src/flann/util/sampling.h create mode 100644 corelib/src/flann/util/saving.h create mode 100644 corelib/src/flann/util/serialization.h create mode 100644 corelib/src/flann/util/timer.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f881096f..cb41ef99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,14 +132,8 @@ option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) -find_package(PkgConfig QUIET) -FIND_PACKAGE(FLANN 1.8 QUIET) -IF(PKG_CONFIG_FOUND AND FLANN_FOUND) - SET(FLANN18_FOUND TRUE) -ENDIF(PKG_CONFIG_FOUND AND FLANN_FOUND) - FIND_PACKAGE(OpenCV REQUIRED) -FIND_PACKAGE(PCL 1.7 REQUIRED) # Will find FLANN too +FIND_PACKAGE(PCL 1.7 REQUIRED) FIND_PACKAGE(ZLIB REQUIRED) IF(WITH_QT) diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 641bf422..ad312dc2 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -57,6 +57,9 @@ SET(SRC_FILES toro3d/posegraph2.cpp toro3d/treeoptimizer2.cpp + flann/ext/lz4.c + flann/ext/lz4hc.c + sqlite3/sqlite3.c ) @@ -76,10 +79,6 @@ SET(LIBRARIES ${ZLIB_LIBRARIES} ) -IF(FLANN18_FOUND) - ADD_DEFINITIONS("-DWITH_FLANN18") -ENDIF(FLANN18_FOUND) - IF(Freenect_FOUND) ADD_DEFINITIONS("-DWITH_FREENECT") IF(Freenect_DASH_INCLUDES) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index c4bc8064..b351a5c0 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif -#include +#include "flann/flann.hpp" #include #include @@ -75,20 +75,7 @@ public: { if(featuresType_ == CV_8UC1) { - -#ifdef WITH_FLANN18 - delete (flann::Index >*)index_; -#else - // issue with 1.7.1: we should explicitly use the corresponding index - if(isLSH_) - { - delete (flann::LshIndex >*)index_; - } - else - { - delete (flann::LinearIndex >*)index_; - } -#endif + delete (flann::Index >*)index_; } else { @@ -108,19 +95,7 @@ public: } if(featuresType_ == CV_8UC1) { -#ifdef WITH_FLANN18 return ((const flann::Index >*)index_)->size(); -#else - // issue with 1.7.1: we should explicitly use the corresponding index - if(isLSH_) - { - return ((const flann::LshIndex >*)index_)->size(); - } - else - { - return ((const flann::LinearIndex >*)index_)->size(); - } -#endif } else { @@ -137,19 +112,7 @@ public: } if(featuresType_ == CV_8UC1) { -#ifdef WITH_FLANN18 return ((const flann::Index >*)index_)->usedMemory()/1000; -#else - // issue with 1.7.1: we should explicitly use the corresponding index - if(isLSH_) - { - return ((const flann::LshIndex >*)index_)->usedMemory()/1000; - } - else - { - return ((const flann::LinearIndex >*)index_)->usedMemory()/1000; - } -#endif } else { @@ -170,21 +133,7 @@ public: if(featuresType_ == CV_8UC1) { flann::Matrix dataset(features.data, features.rows, features.cols); -#ifdef WITH_FLANN18 index_ = new flann::Index >(dataset, params); -#else - // issue with 1.7.1: we should explicitly create the corresponding index - flann::flann_algorithm_t algo = params.at("algorithm").cast(); - if(algo == flann::FLANN_INDEX_LSH) - { - isLSH_ = true; - index_ = new flann::LshIndex >(dataset, params); - } - else - { - index_ = new flann::LinearIndex >(dataset, params); - } -#endif ((flann::Index >*)index_)->buildIndex(); } else @@ -196,15 +145,6 @@ public: nextIndex_ = features.rows; } - bool isIncremental() - { -#ifdef WITH_FLANN18 - return true; -#else - return false; -#endif - } - bool isBuilt() { return index_!=0; @@ -215,7 +155,6 @@ public: unsigned int addPoint(const cv::Mat & feature) { -#ifdef WITH_FLANN18 if(!index_) { UERROR("Flann index not yet created!"); @@ -235,15 +174,10 @@ public: ((flann::Index >*)index_)->addPoints(dataset); } return nextIndex_++; -#else - UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method."); - return 0; -#endif } void removePoint(unsigned int index) { -#ifdef WITH_FLANN18 if(!index_) { UERROR("Flann index not yet created!"); @@ -263,9 +197,6 @@ public: { ((flann::Index >*)index_)->removePoint(index); } -#else - UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method."); -#endif } void knnSearch( @@ -289,19 +220,7 @@ public: { flann::Matrix distsF((unsigned int*)dists.data, dists.rows, dists.cols); flann::Matrix queryF(query.data, query.rows, query.cols); -#ifdef WITH_FLANN18 ((flann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); -#else - // issue with 1.7.1: we should explicitly use the corresponding index - if(isLSH_) - { - ((flann::LshIndex >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); - } - else - { - ((flann::LinearIndex >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); - } -#endif } else { @@ -350,12 +269,6 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether); Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann); - if(_incrementalFlann && !_flannIndex->isIncremental()) - { - UERROR("TRying to set \"KpIncrementalFlann\"=true but RTAB-Map is not built with FLANN>=1.8. Setting to false."); - _incrementalFlann = false; - } - UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str()); std::string dictionaryPath = _dictionaryPath; @@ -583,7 +496,6 @@ void VWDictionary::update() if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) { if(_incrementalFlann && - _flannIndex->isIncremental() && _strategy < kNNBruteForce && _visualWords.size()) { diff --git a/corelib/src/flann/algorithms/all_indices.h b/corelib/src/flann/algorithms/all_indices.h new file mode 100644 index 00000000..d904cca5 --- /dev/null +++ b/corelib/src/flann/algorithms/all_indices.h @@ -0,0 +1,197 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 FLANN_ALL_INDICES_H_ +#define FLANN_ALL_INDICES_H_ + +#include "flann/general.h" + +#include "flann/algorithms/nn_index.h" +#include "flann/algorithms/kdtree_index.h" +#include "flann/algorithms/kdtree_single_index.h" +#include "flann/algorithms/kmeans_index.h" +#include "flann/algorithms/composite_index.h" +#include "flann/algorithms/linear_index.h" +#include "flann/algorithms/hierarchical_clustering_index.h" +#include "flann/algorithms/lsh_index.h" +#include "flann/algorithms/autotuned_index.h" +#ifdef FLANN_USE_CUDA +#include "flann/algorithms/kdtree_cuda_3d_index.h" +#endif + + +namespace flann +{ + +/** + * enable_if sfinae helper + */ +template struct enable_if{}; +template struct enable_if { typedef T type; }; + +/** + * disable_if sfinae helper + */ +template struct disable_if{ typedef T type; }; +template struct disable_if { }; + +/** + * Check if two type are the same + */ +template +struct same_type +{ + enum {value = false}; +}; + +template +struct same_type +{ + enum {value = true}; +}; + +#define HAS_MEMBER(member) \ + template \ + struct member { \ + typedef char No; \ + typedef long Yes; \ + template static Yes test( typename C::member* ); \ + template static No test( ... ); \ + enum { value = sizeof (test(0))==sizeof(Yes) }; \ + }; + +HAS_MEMBER(needs_kdtree_distance) +HAS_MEMBER(needs_vector_space_distance) +HAS_MEMBER(is_kdtree_distance) +HAS_MEMBER(is_vector_space_distance) + +struct DummyDistance +{ + typedef float ElementType; + typedef float ResultType; + + template + ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const + { + return ResultType(0); + } + + template + inline ResultType accum_dist(const U& a, const V& b, int) const + { + return ResultType(0); + } +}; + +/** + * Checks if an index and a distance can be used together + */ +template