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

RegInfo: added icpStructuralDistribution info

Browse Source
This commit is contained in:
matlabbe
2020-04-14 17:06:53 -04:00
parent 607dc67135
commit de354b901d
3 changed files with 39 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/RegistrationInfo.h
View File

@@ -44,6 +44,7 @@ public:
icpTranslation(0.0f),
icpRotation(0.0f),
icpStructuralComplexity(0.0f),
icpStructuralDistribution(0.0f),
icpCorrespondences(0)
{
@@ -63,6 +64,7 @@ public:
output.icpTranslation = icpTranslation;
output.icpRotation = icpRotation;
output.icpStructuralComplexity = icpStructuralComplexity;
output.icpStructuralDistribution = icpStructuralDistribution;
output.icpCorrespondences = icpCorrespondences;
return output;
}
@@ -85,6 +87,7 @@ public:
float icpTranslation;
float icpRotation;
float icpStructuralComplexity;
float icpStructuralDistribution;
int icpCorrespondences;
};

34
corelib/src/RegistrationIcp.cpp
View File

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/conversions.h>
#include <pcl/common/pca.h>
#ifdef RTABMAP_POINTMATCHER
#include <fstream>
@@ -637,6 +638,22 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
if(fromCloudNormals->size() > 2 && toCloudNormals->size() > 2)
{
pcl::PCA<pcl::PointNormal> pca;
pca.setInputCloud(fromCloudNormals);
Eigen::Vector3f valuesFrom = pca.getEigenValues();
pca.setInputCloud(toCloudNormals);
Eigen::Vector3f valuesTo = pca.getEigenValues();
if(valuesFrom[0]/fromCloudNormals->size() < valuesTo[0]/toCloudNormals->size())
{
info.icpStructuralDistribution = sqrt(valuesFrom[0]/fromCloudNormals->size());
}
else
{
info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloudNormals->size());
}
}
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -708,6 +725,23 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
UDEBUG("Conversion time = %f s", timer.ticks());
if(fromCloud->size() > 2 && toCloud->size() > 2)
{
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(fromCloud);
Eigen::Vector3f valuesFrom = pca.getEigenValues();
pca.setInputCloud(toCloud);
Eigen::Vector3f valuesTo = pca.getEigenValues();
if(valuesFrom[0]/fromCloud->size() < valuesTo[0]/toCloud->size())
{
info.icpStructuralDistribution = sqrt(valuesFrom[0]/fromCloud->size());
}
else
{
info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloud->size());
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
if(_voxelSize > 0.0f)