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

Added Icp/DebugExportFormat parameter

Browse Source
This commit is contained in:
matlabbe
2021-06-26 16:06:25 -04:00
parent 1a64e05e2f
commit 2b9ae3a2ad
5 changed files with 509 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -669,6 +669,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
RTABMAP_PARAM(Icp, OutlierRatio, float, 0.85, uFormat("Outlier ratio used with %s>0. For libpointmatcher, this parameter set TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the \"finalOverlapRatio\". The value should be between 0 and 1.", kIcpStrategy().c_str()));
RTABMAP_PARAM_STR(Icp, DebugExportFormat, "", "Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are \"pcd\", \"ply\" or \"vtk\". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.");
// libpointmatcher
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());

1
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -82,6 +82,7 @@ private:
unsigned int _ccSamplingLimit;
bool _ccFilterOutFarthestPoints;
double _ccMaxFinalRMS;
std::string _debugExportFormat;
void * _libpointmatcherICP;
void * _libpointmatcherICPFilters;

61
corelib/src/RegistrationIcp.cpp
View File

@@ -40,6 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/conversions.h>
#include <pcl/common/pca.h>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_io.h>
#ifdef RTABMAP_CCCORELIB
#include "icp/cccorelib.h"
@@ -79,6 +82,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_ccSamplingLimit (Parameters::defaultIcpCCSamplingLimit()),
_ccFilterOutFarthestPoints (Parameters::defaultIcpCCFilterOutFarthestPoints()),
_ccMaxFinalRMS (Parameters::defaultIcpCCMaxFinalRMS()),
_debugExportFormat(Parameters::defaultIcpDebugExportFormat()),
_libpointmatcherICP(0),
_libpointmatcherICPFilters(0)
{
@@ -128,6 +132,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpCCFilterOutFarthestPoints(), _ccFilterOutFarthestPoints);
Parameters::parse(parameters, Parameters::kIcpCCMaxFinalRMS(), _ccMaxFinalRMS);
Parameters::parse(parameters, Parameters::kIcpDebugExportFormat(), _debugExportFormat);
bool pointToPlane = _pointToPlane;
#ifndef RTABMAP_POINTMATCHER
@@ -443,6 +449,41 @@ Transform RegistrationIcp::computeTransformationImpl(
if(fromScan.size() && toScan.size())
{
std::string toPrefix = "rtabmap_icp_scan";
double now = UTimer::now();
if(!_debugExportFormat.empty())
{
std::string fromPrefix = "rtabmap_icp_scan";
if(ULogger::level() == ULogger::kDebug)
{
fromPrefix+=uReplaceChar(uFormat("_%.3f_from_%d", fromSignature.id(), now), '.', '_');
toPrefix+=uReplaceChar(uFormat("_%.3f_to_%d", toSignature.id(), now), '.', '_');
}
else
{
fromPrefix+="_from";
toPrefix+="_to";
}
if(_debugExportFormat.compare("vtk")==0)
{
pcl::io::saveVTKFile(fromPrefix+".vtk", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()));
pcl::io::saveVTKFile(toPrefix+".vtk", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()));
UWARN("Saved %s.vtk and %s.vtk (%s=\"%s\")", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
else if(_debugExportFormat.compare("ply")==0)
{
pcl::io::savePLYFile(fromPrefix+".ply", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
pcl::io::savePLYFile(toPrefix+".ply", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
UWARN("Saved %s.ply and %s.ply (%s=\"%s\")", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
else //pcd
{
pcl::io::savePCDFile(fromPrefix+".pcd", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
pcl::io::savePCDFile(toPrefix+".pcd", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
UWARN("Saved %s.pcd and %s.pcd (%s=\"%s\")", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
}
bool tooLowComplexityForPlaneToPlane = false;
float secondEigenValue = 1.0f;
cv::Mat complexityVectors;
@@ -873,6 +914,26 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
transform = icpT.inverse()*guess;
if(!_debugExportFormat.empty())
{
toPrefix+="_registered";
if(_debugExportFormat.compare("vtk")==0)
{
pcl::io::saveVTKFile(toPrefix+".vtk", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()));
UWARN("Saved %s.vtk (%s=\"%s\")", toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
else if(_debugExportFormat.compare("ply")==0)
{
pcl::io::savePLYFile(toPrefix+".ply", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
UWARN("Saved %s.ply (%s=\"%s\")", toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
else //pcd
{
pcl::io::savePCDFile(toPrefix+".pcd", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
UWARN("Saved %s.pcd (%s=\"%s\")", toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
}
}
}
}
}