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

libpointmatcher integration

Browse Source
This commit is contained in:
matlabbe
2017-08-22 16:20:49 -04:00
parent d0d387a42f
commit f7872d346b
11 changed files with 625 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+19
View File
@@ -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)")
Version.h.in
+1
View File
@@ -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
corelib/include/rtabmap/core/Parameters.h
+3
View File
@@ -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.");
corelib/include/rtabmap/core/RegistrationIcp.h
+4 -1
View File
@@ -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;
};
}
corelib/src/CMakeLists.txt
+11
View File
@@ -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}
corelib/src/RegistrationIcp.cpp
+394 -29
View File
@@ -37,6 +37,176 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/conversions.h>
#ifdef RTABMAP_POINTMATCHER
#include "pointmatcher/PointMatcher.h"
typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;
DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::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<bool> 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; i<pclCloud->size(); ++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<pcl::PointNormal>::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<bool> 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; i<pclCloud->size(); ++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<pcl::PointXYZ> & 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.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
}
}
void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & 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<pclCloud.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
pclCloud.at(i).normal_x = viewNormalX(0, i);
pclCloud.at(i).normal_y = viewNormalY(0, i);
pclCloud.at(i).normal_z = viewNormalZ(0, i);
}
}
template<typename T>
typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
{
typedef typename PointMatcher<T>::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<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
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<double>(T.template cast<double>(), 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<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
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<double>(T.template cast<double>(), 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<double>(T.template cast<double>(), 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)
{
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -306,6 +306,7 @@ private slots:
void changeWorkingDirectory();
void changeDictionaryPath();
void changeOdometryORBSLAM2Vocabulary();
void changeIcpPMConfigPath();
void readSettingsEnd();
void setupTreeView();
void updateBasicParameter();
guilib/src/AboutDialog.cpp
+8
View File
@@ -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);
guilib/src/PreferencesDialog.cpp
+24
View File
@@ -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);
guilib/src/ui/aboutDialog.ui
+78 -45
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>831</width>
<height>832</height>
<width>861</width>
<height>902</height>
</rect>
</property>
<property name="sizePolicy">
@@ -161,6 +161,19 @@ p, li { white-space: pre-wrap; }
</item>
<item>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
<item row="17" column="1">
<widget class="QLabel" name="label_octomap">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_pcl_license">
<property name="text">
@@ -230,20 +243,27 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="16" column="1">
<widget class="QLabel" name="label_octomap">
<item row="4" column="0">
<widget class="QLabel" name="label_79">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
<string>Qt version :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="17" column="0">
<item row="1" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>OpenCV version :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="18" column="0">
<widget class="QLabel" name="label_24">
<property name="text">
<string>With CPU-TSDF :</string>
@@ -253,7 +273,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="17" column="1">
<item row="18" column="1">
<widget class="QLabel" name="label_cputsdf">
<property name="text">
<string/>
@@ -266,7 +286,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="18" column="0">
<item row="19" column="0">
<widget class="QLabel" name="label_25">
<property name="text">
<string>With FOVIS :</string>
@@ -286,7 +306,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="16" column="0">
<item row="17" column="0">
<widget class="QLabel" name="label_20">
<property name="text">
<string>With Octomap :</string>
@@ -296,7 +316,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="20" column="1">
<item row="21" column="1">
<widget class="QLabel" name="label_dvo">
<property name="text">
<string/>
@@ -309,7 +329,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="21" column="0">
<item row="22" column="0">
<widget class="QLabel" name="label_28">
<property name="text">
<string>With ORB SLAM 2 :</string>
@@ -319,7 +339,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="21" column="1">
<item row="22" column="1">
<widget class="QLabel" name="label_orbslam2">
<property name="text">
<string/>
@@ -365,7 +385,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="18" column="1">
<item row="19" column="1">
<widget class="QLabel" name="label_fovis">
<property name="text">
<string/>
@@ -378,7 +398,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="19" column="0">
<item row="20" column="0">
<widget class="QLabel" name="label_26">
<property name="text">
<string>With Viso2 :</string>
@@ -457,16 +477,6 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>OpenCV version :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
@@ -658,7 +668,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="16" column="2">
<item row="17" column="2">
<widget class="QLabel" name="label_octomap_license">
<property name="text">
<string>BSD</string>
@@ -668,7 +678,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="17" column="2">
<item row="18" column="2">
<widget class="QLabel" name="label_cputsdf_license">
<property name="text">
<string>BSD</string>
@@ -678,7 +688,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="18" column="2">
<item row="19" column="2">
<widget class="QLabel" name="label_fovis_license">
<property name="text">
<string>GPLv2</string>
@@ -688,7 +698,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="19" column="2">
<item row="20" column="2">
<widget class="QLabel" name="label_viso2_license">
<property name="text">
<string>GPLv3</string>
@@ -698,7 +708,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="20" column="2">
<item row="21" column="2">
<widget class="QLabel" name="label_dvo_license">
<property name="text">
<string>GPLv3</string>
@@ -761,7 +771,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="19" column="1">
<item row="20" column="1">
<widget class="QLabel" name="label_viso2">
<property name="text">
<string/>
@@ -774,7 +784,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="20" column="0">
<item row="21" column="0">
<widget class="QLabel" name="label_27">
<property name="text">
<string>With DVO :</string>
@@ -784,16 +794,6 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_79">
<property name="text">
<string>Qt version :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_qt_license">
<property name="text">
@@ -847,7 +847,7 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="21" column="2">
<item row="22" column="2">
<widget class="QLabel" name="label_orbslam2_license">
<property name="text">
<string>GPLv3</string>
@@ -857,6 +857,39 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="16" column="2">
<widget class="QLabel" name="label_libpointmatcher_license">
<property name="text">
<string>BSD</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="16" column="0">
<widget class="QLabel" name="label_29">
<property name="text">
<string>With libpointmatcher :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="16" column="1">
<widget class="QLabel" name="label_libpointmatcher">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
guilib/src/ui/preferencesDialog.ui
+82 -90
View File
@@ -63,25 +63,16 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-310</y>
<width>678</width>
<height>2739</height>
<height>2736</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -95,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>20</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4530,16 +4521,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_93">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -11561,22 +11543,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_415">
<property name="text">
<string>Path to ORB vocabulary (*.txt).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_OdomORBSLAM2VocPath"/>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_414">
<property name="text">
@@ -11625,6 +11591,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_OdomORBSLAM2VocPath"/>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_415">
<property name="text">
<string>Path to ORB vocabulary (*.txt).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QToolButton" name="toolButton_OdomORBSLAM2VocPath">
<property name="text">
@@ -11835,7 +11817,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item>
<widget class="QStackedWidget" name="stackedWidget_odometryFiltering">
<property name="currentIndex">
<number>2</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_31">
<layout class="QVBoxLayout" name="verticalLayout_84"/>
@@ -12491,16 +12473,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
<widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -12640,16 +12613,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
<widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -12807,16 +12771,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -12896,16 +12851,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -13017,16 +12963,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -13763,6 +13700,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_libpointmatcher">
<property name="title">
<string>libpointmatcher</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_126">
<item>
<widget class="QLabel" name="label_422">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;libpointmatcher: &lt;a href=&quot;https://github.com/ethz-asl/libpointmatcher&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/ethz-asl/libpointmatcher&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;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.&lt;/p&gt;&lt;p&gt;Below we can set a &lt;a href=&quot;https://github.com/ethz-asl/libpointmatcher/blob/master/doc/Configuration.md&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;configuration file&lt;/span&gt;&lt;/a&gt; (*.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.&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="openExternalLinks">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<widget class="QToolButton" name="toolButton_IcpConfigPath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_IcpPMConfigPath"/>
</item>
<item>
<widget class="QLabel" name="label_423">
<property name="text">
<string>Configuration file (*.yaml).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>