From f7872d346b8b3ac758389362f40d67de50879d16 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 22 Aug 2017 16:20:49 -0400 Subject: [PATCH] libpointmatcher integration --- CMakeLists.txt | 19 + Version.h.in | 1 + corelib/include/rtabmap/core/Parameters.h | 3 + .../include/rtabmap/core/RegistrationIcp.h | 5 +- corelib/src/CMakeLists.txt | 11 + corelib/src/RegistrationIcp.cpp | 423 ++++++++++++++++-- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/AboutDialog.cpp | 8 + guilib/src/PreferencesDialog.cpp | 24 + guilib/src/ui/aboutDialog.ui | 123 +++-- guilib/src/ui/preferencesDialog.ui | 172 ++++--- 11 files changed, 625 insertions(+), 165 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2b86d2e..ae479856 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,7 @@ option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_TORO "Include TORO support" ON) option(WITH_VERTIGO "Include Vertigo support" ON) option(WITH_CVSBA "Include cvsba support" ON) +option(WITH_POINTMATCHER "Include libpointmatcher support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) option(WITH_ZED "Include ZED sdk support" ON) option(WITH_REALSENSE "Include RealSense support" ON) @@ -302,6 +303,13 @@ IF(WITH_CVSBA) ENDIF(cvsba_FOUND) ENDIF(WITH_CVSBA) +IF(WITH_POINTMATCHER) + find_package(libpointmatcher QUIET) + IF(libpointmatcher_FOUND) + MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}") + ENDIF(libpointmatcher_FOUND) +ENDIF(WITH_POINTMATCHER) + IF(WITH_ZED) IF(WIN32) # Windows SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS}) @@ -471,6 +479,9 @@ IF(NOT cvsba_FOUND) ELSE() SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES}) ENDIF() +IF(NOT libpointmatcher_FOUND) + SET(POINTMATCHER "//") +ENDIF(NOT libpointmatcher_FOUND) IF(NOT Freenect_FOUND) SET(FREENECT "//") ELSE() @@ -823,6 +834,14 @@ ELSE() MESSAGE(STATUS " With cvsba = NO (cvsba not found)") ENDIF() +IF(libpointmatcher_FOUND) +MESSAGE(STATUS " With libpointmatcher = YES (License: BSD)") +ELSEIF(NOT WITH_POINTMATCHER) +MESSAGE(STATUS " With libpointmatcher = NO (WITH_POINTMATCHER=OFF)") +ELSE() +MESSAGE(STATUS " With libpointmatcher = NO (libpointmatcher not found)") +ENDIF() + IF(ZED_FOUND) IF(CUDA_FOUND) MESSAGE(STATUS " With ZED = YES (With CUDA)") diff --git a/Version.h.in b/Version.h.in index 8046b86b..6b341230 100644 --- a/Version.h.in +++ b/Version.h.in @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @FREENECT@#define RTABMAP_FREENECT @FREENECT2@#define RTABMAP_FREENECT2 @CVSBA@#define RTABMAP_CVSBA +@POINTMATCHER@#define RTABMAP_POINTMATCHER @DC1394@#define RTABMAP_DC1394 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 @ZED@#define RTABMAP_ZED diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 10f894b2..d007fbe0 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -512,6 +512,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane."); + RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation."); + RTABMAP_PARAM_STR(Icp, PMConfig, "", uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str()); + // Stereo disparity RTABMAP_PARAM(Stereo, WinWidth, int, 15, "Window width."); RTABMAP_PARAM(Stereo, WinHeight, int, 3, "Window height."); diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 2ba4fc80..ecebb16d 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -41,7 +41,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration public: // take ownership of child RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0); - virtual ~RegistrationIcp() {} + virtual ~RegistrationIcp(); virtual void parseParameters(const ParametersMap & parameters); @@ -65,6 +65,9 @@ private: float _correspondenceRatio; bool _pointToPlane; int _pointToPlaneNormalNeighbors; + bool _libpointmatcher; + std::string _libpointmatcherConfig; + void * _libpointmatcherICP; }; } diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 93fcc840..6e951eac 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -243,6 +243,17 @@ IF(cvsba_FOUND) ) ENDIF(cvsba_FOUND) +IF(libpointmatcher_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${libpointmatcher_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${libpointmatcher_LIBRARIES} + ) +ENDIF(libpointmatcher_FOUND) + IF(ZED_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 8d1ba04d..7fd1f77c 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -37,6 +37,176 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include + +#ifdef RTABMAP_POINTMATCHER +#include "pointmatcher/PointMatcher.h" +typedef PointMatcher PM; +typedef PM::DataPoints DP; + +DP pclToDP(const pcl::PointCloud::Ptr & pclCloud) +{ + typedef DP::Label Label; + typedef DP::Labels Labels; + typedef DP::View View; + + if (pclCloud->empty()) + return DP(); + + // fill labels + // conversions of descriptor fields from pcl + // see http://www.ros.org/wiki/pcl/Overview + Labels featLabels; + Labels descLabels; + std::vector isFeature; + featLabels.push_back(Label("x", 1)); + isFeature.push_back(true); + featLabels.push_back(Label("y", 1)); + isFeature.push_back(true); + featLabels.push_back(Label("z", 1)); + isFeature.push_back(true); + featLabels.push_back(Label("pad", 1)); + + // create cloud + DP cloud(featLabels, descLabels, pclCloud->size()); + cloud.getFeatureViewByName("pad").setConstant(1); + + // fill cloud + View viewX(cloud.getFeatureViewByName("x")); + View viewY(cloud.getFeatureViewByName("y")); + View viewZ(cloud.getFeatureViewByName("z")); + for(unsigned int i=0; isize(); ++i) + { + viewX(0, i) = pclCloud->at(i).x; + viewY(0, i) = pclCloud->at(i).y; + viewZ(0, i) = pclCloud->at(i).z; + } + + return cloud; +} + +DP pclToDP(const pcl::PointCloud::Ptr & pclCloud) +{ + typedef DP::Label Label; + typedef DP::Labels Labels; + typedef DP::View View; + + if (pclCloud->empty()) + return DP(); + + // fill labels + // conversions of descriptor fields from pcl + // see http://www.ros.org/wiki/pcl/Overview + Labels featLabels; + Labels descLabels; + std::vector isFeature; + featLabels.push_back(Label("x", 1)); + isFeature.push_back(true); + featLabels.push_back(Label("y", 1)); + isFeature.push_back(true); + featLabels.push_back(Label("z", 1)); + isFeature.push_back(true); + + descLabels.push_back(Label("normals", 3)); + isFeature.push_back(false); + isFeature.push_back(false); + isFeature.push_back(false); + + featLabels.push_back(Label("pad", 1)); + + // create cloud + DP cloud(featLabels, descLabels, pclCloud->size()); + cloud.getFeatureViewByName("pad").setConstant(1); + + // fill cloud + View viewX(cloud.getFeatureViewByName("x")); + View viewY(cloud.getFeatureViewByName("y")); + View viewZ(cloud.getFeatureViewByName("z")); + View viewNormalX(cloud.getDescriptorRowViewByName("normals",0)); + View viewNormalY(cloud.getDescriptorRowViewByName("normals",1)); + View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2)); + for(unsigned int i=0; isize(); ++i) + { + viewX(0, i) = pclCloud->at(i).x; + viewY(0, i) = pclCloud->at(i).y; + viewZ(0, i) = pclCloud->at(i).z; + viewNormalX(0, i) = pclCloud->at(i).normal_x; + viewNormalY(0, i) = pclCloud->at(i).normal_y; + viewNormalZ(0, i) = pclCloud->at(i).normal_z; + } + + return cloud; +} + +void pclFromDP(const DP & cloud, pcl::PointCloud & pclCloud) +{ + typedef DP::ConstView ConstView; + + if (cloud.features.cols() == 0) + return; + + pclCloud.resize(cloud.features.cols()); + pclCloud.is_dense = true; + + // fill cloud + ConstView viewX(cloud.getFeatureViewByName("x")); + ConstView viewY(cloud.getFeatureViewByName("y")); + ConstView viewZ(cloud.getFeatureViewByName("z")); + for(unsigned int i=0; i & pclCloud) +{ + typedef DP::ConstView ConstView; + + if (cloud.features.cols() == 0) + return; + + pclCloud.resize(cloud.features.cols()); + pclCloud.is_dense = true; + + // fill cloud + ConstView viewX(cloud.getFeatureViewByName("x")); + ConstView viewY(cloud.getFeatureViewByName("y")); + ConstView viewZ(cloud.getFeatureViewByName("z")); + ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0)); + ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1)); + ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2)); + for(unsigned int i=0; i +typename PointMatcher::TransformationParameters eigenMatrixToDim(const typename PointMatcher::TransformationParameters& matrix, int dimp1) +{ + typedef typename PointMatcher::TransformationParameters M; + assert(matrix.rows() == matrix.cols()); + assert((matrix.rows() == 3) || (matrix.rows() == 4)); + assert((dimp1 == 3) || (dimp1 == 4)); + + if (matrix.rows() == dimp1) + return matrix; + + M out(M::Identity(dimp1,dimp1)); + out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2); + out.topRightCorner(2,1) = matrix.topRightCorner(2,1); + return out; +} + +#endif namespace rtabmap { @@ -51,11 +221,24 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _pointToPlane(Parameters::defaultIcpPointToPlane()), - _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()) + _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()), + _libpointmatcher(Parameters::defaultIcpPM()), + _libpointmatcherConfig(Parameters::defaultIcpPMConfig()), + _libpointmatcherICP(0) { this->parseParameters(parameters); } +RegistrationIcp::~RegistrationIcp() +{ +#ifdef RTABMAP_POINTMATCHER + if(_libpointmatcherICP) + { + delete (PM::ICP*)_libpointmatcherICP; + } +#endif +} + void RegistrationIcp::parseParameters(const ParametersMap & parameters) { Registration::parseParameters(parameters); @@ -71,6 +254,81 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors); + Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher); + Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig); +#ifndef RTABMAP_POINTMATCHER + if(_libpointmatcher) + { + UWARN("Parameter %s is set to true but RTAB-MAp has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str()); + _libpointmatcher = false; + } +#else + if(_libpointmatcher) + { + UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str()); + if(_libpointmatcherICP!=0) + { + delete (PM::ICP*)_libpointmatcherICP; + _libpointmatcherICP = 0; + } + + _libpointmatcherICP = new PM::ICP(); + + PM::ICP * icp = (PM::ICP*)_libpointmatcherICP; + + bool useDefaults = true; + if(!_libpointmatcherConfig.empty()) + { + // load YAML config + std::ifstream ifs(_libpointmatcherConfig.c_str()); + if (ifs.good()) + { + icp->loadFromYaml(ifs); + useDefaults = false; + } + else + { + UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str()); + } + } + if(useDefaults) + { + // Create the default ICP algorithm + // See the implementation of setDefault() to create a custom ICP algorithm + icp->setDefault(); + + icp->readingDataPointsFilters.clear(); + icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter")); + + icp->referenceDataPointsFilters.clear(); + icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter")); + + PM::Parameters params; + params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance); + icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params)); + params.clear(); + + params["ratio"] = uNumber2Str(0.65); // For kinect cloud, 0.65 is better than 0.85 + icp->outlierFilters.clear(); + icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params)); + params.clear(); + + icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer")); + + icp->transformationCheckers.clear(); + params["maxIterationCount"] = uNumber2Str(_maxIterations); + icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params)); + params.clear(); + + params["minDiffRotErr"] = uNumber2Str(_epsilon*_epsilon*100.0f); + params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon); + params["smoothLength"] = uNumber2Str(4); + icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params)); + params.clear(); + } + } +#endif + UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str()); UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str()); UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str()); @@ -90,12 +348,13 @@ Transform RegistrationIcp::computeTransformationImpl( UDEBUG("Voxel size=%f", _voxelSize); UDEBUG("PointToPlane=%d", _pointToPlane?1:0); UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors); - UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance); + UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance); UDEBUG("Max Iterations=%d", _maxIterations); UDEBUG("Correspondence Ratio=%f", _correspondenceRatio); UDEBUG("Max translation=%f", _maxTranslation); UDEBUG("Max rotation=%f", _maxRotation); UDEBUG("Downsampling step=%d", _downsamplingStep); + UDEBUG("libpointmatcher=%d", _libpointmatcher?1:0); UTimer timer; std::string msg; @@ -149,15 +408,50 @@ Transform RegistrationIcp::computeTransformationImpl( UDEBUG("Conversion time = %f s", timer.ticks()); pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); - icpT = util3d::icpPointToPlane( - fromCloudNormals, - toCloudNormals, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudNormalsRegistered, - _epsilon, - this->force3DoF()); +#ifdef RTABMAP_POINTMATCHER + if(_libpointmatcher) + { + // Load point clouds + DP data = pclToDP(fromCloudNormals); + DP ref = pclToDP(toCloudNormals); + + // Compute the transformation to express data in ref + PM::TransformationParameters T; + try + { + UASSERT(_libpointmatcherICP != 0); + PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); + T = icp(data, ref); + icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + + float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); + UDEBUG("match ratio: %f", matchRatio); + + if(!icpT.isNull()) + { + fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); + hasConverged = true; + } + } + catch(const std::exception & e) + { + UWARN("libpointmatcher has failed: %s", e.what()); + } + } + else + { +#endif + icpT = util3d::icpPointToPlane( + fromCloudNormals, + toCloudNormals, + _maxCorrespondenceDistance, + _maxIterations, + hasConverged, + *fromCloudNormalsRegistered, + _epsilon, + this->force3DoF()); + } + if(!icpT.isNull() && hasConverged) { util3d::computeVarianceAndCorrespondences( @@ -219,15 +513,52 @@ Transform RegistrationIcp::computeTransformationImpl( if(toCloudNormals->size() && fromCloudNormals->size()) { pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); - icpT = util3d::icpPointToPlane( - fromCloudNormals, - toCloudNormals, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudNormalsRegistered, - _epsilon, - this->force3DoF()); + +#ifdef RTABMAP_POINTMATCHER + if(_libpointmatcher) + { + // Load point clouds + DP data = pclToDP(fromCloudNormals); + DP ref = pclToDP(toCloudNormals); + + // Compute the transformation to express data in ref + PM::TransformationParameters T; + try + { + UASSERT(_libpointmatcherICP != 0); + PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); + T = icp(data, ref); + icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + + float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); + UDEBUG("match ratio: %f", matchRatio); + + if(!icpT.isNull()) + { + fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); + hasConverged = true; + } + } + catch(const std::exception & e) + { + UWARN("libpointmatcher has failed: %s", e.what()); + } + } + else +#endif + { + icpT = util3d::icpPointToPlane( + fromCloudNormals, + toCloudNormals, + _maxCorrespondenceDistance, + _maxIterations, + hasConverged, + *fromCloudNormalsRegistered, + _epsilon, + this->force3DoF()); + } + + if(!icpT.isNull() && hasConverged) { util3d::computeVarianceAndCorrespondences( @@ -248,15 +579,49 @@ Transform RegistrationIcp::computeTransformationImpl( toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); } - icpT = util3d::icp( - fromCloudFiltered, - toCloudFiltered, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudRegistered, - _epsilon, - this->force3DoF()); // icp2D +#ifdef RTABMAP_POINTMATCHER + if(_libpointmatcher) + { + // Load point clouds + DP data = pclToDP(fromCloudFiltered); + DP ref = pclToDP(toCloudFiltered); + + // Compute the transformation to express data in ref + PM::TransformationParameters T; + try + { + UASSERT(_libpointmatcherICP != 0); + PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); + T = icp(data, ref); + icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + + float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); + UDEBUG("match ratio: %f", matchRatio); + + if(!icpT.isNull()) + { + fromCloudRegistered = util3d::transformPointCloud(fromCloudFiltered, icpT); + hasConverged = true; + } + } + catch(const std::exception & e) + { + UWARN("libpointmatcher has failed: %s", e.what()); + } + } + else +#endif + { + icpT = util3d::icp( + fromCloudFiltered, + toCloudFiltered, + _maxCorrespondenceDistance, + _maxIterations, + hasConverged, + *fromCloudRegistered, + _epsilon, + this->force3DoF()); // icp2D + } if(!icpT.isNull() && hasConverged) { diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 9e186e73..78ad8f34 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -306,6 +306,7 @@ private slots: void changeWorkingDirectory(); void changeDictionaryPath(); void changeOdometryORBSLAM2Vocabulary(); + void changeIcpPMConfigPath(); void readSettingsEnd(); void setupTreeView(); void updateBasicParameter(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 7cb5d728..cc91ddfb 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -95,6 +95,14 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No"); _ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false); +#ifdef RTABMAP_POINTMATCHER + _ui->label_libpointmatcher->setText("Yes"); + _ui->label_libpointmatcher_license->setEnabled(true); +#else + _ui->label_libpointmatcher->setText("No"); + _ui->label_libpointmatcher_license->setEnabled(false); +#endif + #ifdef RTABMAP_FOVIS _ui->label_fovis->setText("Yes"); _ui->label_fovis_license->setEnabled(true); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 0b6219fc..f693eb17 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -238,6 +238,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->graphOptimization_robust->setEnabled(false); } +#ifndef RTABMAP_POINTMATCHER + _ui->groupBox_libpointmatcher->setEnabled(false); +#endif if(!CameraOpenni::available()) { _ui->comboBox_cameraRGBD->setItemData(0, 0, Qt::UserRole - 1); @@ -856,6 +859,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str()); _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str()); + _ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str()); + _ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str()); + connect(_ui->toolButton_IcpConfigPath, SIGNAL(clicked()), this, SLOT(changeIcpPMConfigPath())); + // Occupancy grid _ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str()); _ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGridGroundIsObstacle().c_str()); @@ -4051,6 +4058,23 @@ void PreferencesDialog::changeOdometryORBSLAM2Vocabulary() } } +void PreferencesDialog::changeIcpPMConfigPath() +{ + QString path; + if(_ui->lineEdit_IcpPMConfigPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("libpointmatcher (*.yaml)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_IcpPMConfigPath->text(), tr("libpointmatcher (*.yaml)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_IcpPMConfigPath->setText(path); + } +} + void PreferencesDialog::updateSourceGrpVisibility() { _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index a597356e..2d08317d 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -6,8 +6,8 @@ 0 0 - 831 - 832 + 861 + 902 @@ -161,6 +161,19 @@ p, li { white-space: pre-wrap; } + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + @@ -230,20 +243,27 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt version : true - + + + + OpenCV version : + + + true + + + + With CPU-TSDF : @@ -253,7 +273,7 @@ p, li { white-space: pre-wrap; } - + @@ -266,7 +286,7 @@ p, li { white-space: pre-wrap; } - + With FOVIS : @@ -286,7 +306,7 @@ p, li { white-space: pre-wrap; } - + With Octomap : @@ -296,7 +316,7 @@ p, li { white-space: pre-wrap; } - + @@ -309,7 +329,7 @@ p, li { white-space: pre-wrap; } - + With ORB SLAM 2 : @@ -319,7 +339,7 @@ p, li { white-space: pre-wrap; } - + @@ -365,7 +385,7 @@ p, li { white-space: pre-wrap; } - + @@ -378,7 +398,7 @@ p, li { white-space: pre-wrap; } - + With Viso2 : @@ -457,16 +477,6 @@ p, li { white-space: pre-wrap; } - - - - OpenCV version : - - - true - - - @@ -658,7 +668,7 @@ p, li { white-space: pre-wrap; } - + BSD @@ -668,7 +678,7 @@ p, li { white-space: pre-wrap; } - + BSD @@ -678,7 +688,7 @@ p, li { white-space: pre-wrap; } - + GPLv2 @@ -688,7 +698,7 @@ p, li { white-space: pre-wrap; } - + GPLv3 @@ -698,7 +708,7 @@ p, li { white-space: pre-wrap; } - + GPLv3 @@ -761,7 +771,7 @@ p, li { white-space: pre-wrap; } - + @@ -774,7 +784,7 @@ p, li { white-space: pre-wrap; } - + With DVO : @@ -784,16 +794,6 @@ p, li { white-space: pre-wrap; } - - - - Qt version : - - - true - - - @@ -847,7 +847,7 @@ p, li { white-space: pre-wrap; } - + GPLv3 @@ -857,6 +857,39 @@ p, li { white-space: pre-wrap; } + + + + BSD + + + true + + + + + + + With libpointmatcher : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index b6727ca1..945f21a3 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,25 +63,16 @@ 0 - 0 + -310 678 - 2739 + 2736 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -95,7 +86,7 @@ QFrame::Raised - 20 + 21 @@ -4530,16 +4521,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -11561,22 +11543,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Path to ORB vocabulary (*.txt). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - @@ -11625,6 +11591,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + Path to ORB vocabulary (*.txt). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -11835,7 +11817,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 2 + 1 @@ -12491,16 +12473,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - 0 - - - 0 - - - 0 - - + 0 @@ -12640,16 +12613,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - 0 - - - 0 - - - 0 - - + 0 @@ -12807,16 +12771,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -12896,16 +12851,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13017,16 +12963,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13763,6 +13700,61 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + libpointmatcher + + + true + + + + + + <html><head/><body><p>libpointmatcher: <a href="https://github.com/ethz-asl/libpointmatcher"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/ethz-asl/libpointmatcher</span></a></p><p>libpointmatcher is a modular library implementing the Iterative Closest Point (ICP) algorithm for aligning point clouds. It has applications in robotics and computer vision. When enabled, libpointmatcher is used for ICP registration instead of PCL's implementation.</p><p>Below we can set a <a href="https://github.com/ethz-asl/libpointmatcher/blob/master/doc/Configuration.md"><span style=" text-decoration: underline; color:#0000ff;">configuration file</span></a> (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., voxel filtering or downsampling above), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Maximum iterations, epsilon and max correspondence distance parameters are also ignored if configuration file is set.<br/></p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + Configuration file (*.yaml). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + +