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), icpTranslation(0.0f),
icpRotation(0.0f), icpRotation(0.0f),
icpStructuralComplexity(0.0f), icpStructuralComplexity(0.0f),
icpStructuralDistribution(0.0f),
icpCorrespondences(0) icpCorrespondences(0)
{ {
@@ -63,6 +64,7 @@ public:
output.icpTranslation = icpTranslation; output.icpTranslation = icpTranslation;
output.icpRotation = icpRotation; output.icpRotation = icpRotation;
output.icpStructuralComplexity = icpStructuralComplexity; output.icpStructuralComplexity = icpStructuralComplexity;
output.icpStructuralDistribution = icpStructuralDistribution;
output.icpCorrespondences = icpCorrespondences; output.icpCorrespondences = icpCorrespondences;
return output; return output;
} }
@@ -85,6 +87,7 @@ public:
float icpTranslation; float icpTranslation;
float icpRotation; float icpRotation;
float icpStructuralComplexity; float icpStructuralComplexity;
float icpStructuralDistribution;
int icpCorrespondences; 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/UMath.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <pcl/conversions.h> #include <pcl/conversions.h>
#include <pcl/common/pca.h>
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
#include <fstream> #include <fstream>
@@ -637,6 +638,22 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); 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()); UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>()); 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()); pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
UDEBUG("Conversion time = %f s", timer.ticks()); 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 fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
if(_voxelSize > 0.0f) if(_voxelSize > 0.0f)

2
guilib/src/MainWindow.cpp
View File

@@ -593,6 +593,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false); _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false); _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false); _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false); _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false); _ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false); _ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
@@ -1511,6 +1512,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());