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

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());
}
}
}
}
}