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

Added parameter "IcpPointToPlaneMaxComplexity". Added util3d::computeNormalsComplexity(). OdomInfo has now RegistrationInfo field to avoid duplicating members.

Browse Source
This commit is contained in:
matlabbe
2017-09-11 13:17:53 -04:00
parent 380fc2cbde
commit aca005c287
25 changed files with 581 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OdometryEvent.h
+11 -11
View File
@@ -41,7 +41,7 @@ class OdometryEvent : public UEvent
public:
OdometryEvent()
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
OdometryEvent(
const SensorData & data,
@@ -51,17 +51,17 @@ public:
_pose(pose),
_info(info)
{
if(_info.covariance.empty())
if(_info.reg.covariance.empty())
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
@@ -69,7 +69,7 @@ public:
SensorData & data() {return _data;}
const SensorData & data() const {return _data;}
const Transform & pose() const {return _pose;}
const cv::Mat & covariance() const {return _info.covariance;}
const cv::Mat & covariance() const {return _info.reg.covariance;}
std::vector<float> velocity() const {
if(_info.interval>0.0)
{
corelib/include/rtabmap/core/OdometryInfo.h
+4 -14
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
@@ -39,9 +40,6 @@ class OdometryInfo
public:
OdometryInfo() :
lost(true),
matches(0),
inliers(0),
icpInliersRatio(0.0f),
features(0),
localMapSize(0),
localScanMapSize(0),
@@ -62,10 +60,7 @@ public:
{
OdometryInfo output;
output.lost = lost;
output.matches = matches;
output.inliers = inliers;
output.icpInliersRatio = icpInliersRatio;
output.covariance = covariance.clone();
output.reg = reg.copyWithoutData();
output.features = features;
output.localMapSize = localMapSize;
output.localScanMapSize = localScanMapSize;
@@ -87,10 +82,7 @@ public:
}
bool lost;
int matches;
int inliers;
float icpInliersRatio;
cv::Mat covariance;
RegistrationInfo reg;
int features;
int localMapSize;
int localScanMapSize;
@@ -108,12 +100,10 @@ public:
Transform transformGroundTruth;
float distanceTravelled;
int type; // 0=F2M, 1=F2F
int type;
// F2M
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::map<int, cv::Point3f> localMap;
cv::Mat localScanMap;
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -513,6 +513,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.0, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
// libpointmatcher
RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation.");
corelib/include/rtabmap/core/RegistrationIcp.h
+1
View File
@@ -66,6 +66,7 @@ private:
bool _pointToPlane;
int _pointToPlaneK;
float _pointToPlaneRadius;
float _pointToPlaneMinComplexity;
bool _libpointmatcher;
std::string _libpointmatcherConfig;
float _libpointmatcherOutlierRatio;
corelib/include/rtabmap/core/RegistrationInfo.h
+18 -1
View File
@@ -39,10 +39,26 @@ public:
matches(0),
icpInliersRatio(0),
icpTranslation(0.0f),
icpRotation(0.0f)
icpRotation(0.0f),
icpStructuralComplexity(0.0f)
{
}
RegistrationInfo copyWithoutData() const
{
RegistrationInfo output;
output.covariance = covariance.clone();
output.rejectedMsg = rejectedMsg;
output.inliers = inliers;
output.matches = matches;
output.icpInliersRatio = icpInliersRatio;
output.icpTranslation = icpTranslation;
output.icpRotation = icpRotation;
output.icpStructuralComplexity = icpStructuralComplexity;
return output;
}
cv::Mat covariance;
std::string rejectedMsg;
@@ -56,6 +72,7 @@ public:
float icpInliersRatio;
float icpTranslation;
float icpRotation;
float icpStructuralComplexity;
};
}
corelib/include/rtabmap/core/Statistics.h
+4 -1
View File
@@ -77,7 +77,10 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(NeighborLinkRefining, Accepted,);
RTABMAP_STATS(NeighborLinkRefining, Inliers,);
RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
RTABMAP_STATS(NeighborLinkRefining, Variance,);
RTABMAP_STATS(NeighborLinkRefining, Pts,);
corelib/include/rtabmap/core/util3d_surface.h
+12
View File
@@ -262,6 +262,18 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
float RTABMAP_EXP computeNormalsComplexity(
const cv::Mat & scan);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::Normal> & normals,
bool is2d = false);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointNormal> & cloud,
bool is2d = false);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
bool is2d = false);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius = 0.0f,
corelib/src/OdometryDVO.cpp
+1 -1
View File
@@ -270,7 +270,7 @@ Transform OdometryDVO::computeTransform(
if(info)
{
info->type = (int)kTypeDVO;
info->covariance = covariance;
info->reg.covariance = covariance;
}
UINFO("Odom update time = %fs", timer.elapsed());
corelib/src/OdometryF2F.cpp
+9 -5
View File
@@ -246,13 +246,17 @@ Transform OdometryF2F::computeTransform(
if(info)
{
info->type = 1;
info->covariance = regInfo.covariance;
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
info->type = kTypeF2F;
info->features = newFrame.sensorData().keypoints().size();
info->keyFrameAdded = addKeyFrame;
if(this->isInfoDataFilled())
{
info->reg = regInfo;
}
else
{
info->reg = regInfo.copyWithoutData();
}
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
corelib/src/OdometryF2M.cpp
+5 -6
View File
@@ -894,10 +894,6 @@ Transform OdometryF2M::computeTransform(
if(info)
{
info->covariance = regInfo.covariance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->features = nFeatures;
info->localKeyFrames = (int)bundlePoses_.size();
info->keyFrameAdded = addKeyFrame;
@@ -907,8 +903,11 @@ Transform OdometryF2M::computeTransform(
if(this->isInfoDataFilled())
{
info->wordMatches = regInfo.matchesIDs;
info->wordInliers = regInfo.inliersIDs;
info->reg = regInfo;
}
else
{
info->reg = regInfo.copyWithoutData();
}
}
corelib/src/OdometryFovis.cpp
+3 -3
View File
@@ -381,9 +381,9 @@ Transform OdometryFovis::computeTransform(
info->type = (int)kTypeFovis;
info->keyFrameAdded = fovis_->getChangeReferenceFrames();
info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
info->matches = fovis_->getMotionEstimator()->getNumMatches();
info->inliers = fovis_->getMotionEstimator()->getNumInliers();
info->covariance = covariance;
info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
info->reg.covariance = covariance;
if(this->isInfoDataFilled())
{
corelib/src/OdometryMono.cpp
+6 -6
View File
@@ -352,7 +352,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
}
correspondences = (int)matches.size();
@@ -397,10 +397,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
info->reg.inliersIDs.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
}
}
@@ -976,7 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(info)
{
// a very high variance tells that the new pose is not linked with the previous one
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
}
// generate kpts
@@ -1013,8 +1013,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(this->isInfoDataFilled() && info)
{
//info->variance = variance;
info->inliers = inliers;
info->matches = correspondences;
info->reg.inliers = inliers;
info->reg.matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
info->localMap = localMap_;
corelib/src/OdometryORBSLAM2.cpp
+9 -8
View File
@@ -891,15 +891,15 @@ Transform OdometryORBSLAM2::computeTransform(
{
info->lost = t.isNull();
info->type = (int)kTypeORBSLAM2;
info->covariance = covariance;
info->reg.covariance = covariance;
info->localMapSize = totalMapPoints;
info->localKeyFrames = totalKfs;
if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
{
const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
info->wordMatches.resize(kpts.size());
info->wordInliers.resize(kpts.size());
info->reg.matchesIDs.resize(kpts.size());
info->reg.inliersIDs.resize(kpts.size());
int oi = 0;
for (unsigned int i = 0; i < kpts.size(); ++i)
{
@@ -915,14 +915,15 @@ Transform OdometryORBSLAM2::computeTransform(
info->words.insert(std::make_pair(wordId, kpts[i]));
if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
{
info->wordMatches[oi] = wordId;
info->wordInliers[oi] = wordId;
info->reg.matchesIDs[oi] = wordId;
info->reg.inliersIDs[oi] = wordId;
++oi;
}
}
info->wordMatches.resize(oi);
info->wordInliers.resize(oi);
info->inliers = oi;
info->reg.matchesIDs.resize(oi);
info->reg.inliersIDs.resize(oi);
info->reg.inliers = oi;
info->reg.matches = oi;
std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
for (unsigned int i = 0; i < mapPoints.size(); ++i)
corelib/src/OdometryViso2.cpp
+3 -3
View File
@@ -273,11 +273,11 @@ Transform OdometryViso2::computeTransform(
{
info->type = (int)kTypeViso2;
info->keyFrameAdded = !keep_reference_frame_;
info->matches = viso2_->getNumberOfMatches();
info->inliers = viso2_->getNumberOfInliers();
info->reg.matches = viso2_->getNumberOfMatches();
info->reg.inliers = viso2_->getNumberOfInliers();
if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
{
info->covariance = covariance;
info->reg.covariance = covariance;
}
if(this->isInfoDataFilled())
corelib/src/RegistrationIcp.cpp
+198 -161
View File
@@ -232,6 +232,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_pointToPlane(Parameters::defaultIcpPointToPlane()),
_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
_libpointmatcher(Parameters::defaultIcpPM()),
_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -265,6 +266,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -428,6 +431,8 @@ Transform RegistrationIcp::computeTransformationImpl(
float correspondencesRatio = 0.0f;
int correspondences = 0;
double variance = 1.0;
bool transformComputed = false;
bool tooLowComplexityForPlaneToPlane = false;
if( _pointToPlane &&
_voxelSize == 0.0f &&
@@ -436,72 +441,88 @@ Transform RegistrationIcp::computeTransformationImpl(
!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
{
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
double fromComplexity = util3d::computeNormalsComplexity(fromScan);
double toComplexity = util3d::computeNormalsComplexity(toScan);
UWARN("%d->%d %f %f", fromSignature.id(), toSignature.id(), fromComplexity, toComplexity);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;
try
{
UASSERT(_libpointmatcherICP != 0);
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
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());
}
tooLowComplexityForPlaneToPlane = true;
UWARN("ICP PointToPlane ignored as structural complexity is too low: %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
}
else
#endif
{
icpT = util3d::icpPointToPlane(
fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
}
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
if(!icpT.isNull() && hasConverged)
{
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;
try
{
UASSERT(_libpointmatcherICP != 0);
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
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(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
}
transformComputed = true;
}
}
else
if(!transformComputed)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
@@ -534,44 +555,43 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane && // ICP Point To Plane
!tooLowComplexityForPlaneToPlane && // if previously rejected above
!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
{
pcl::PointCloud<pcl::Normal>::Ptr normals;
Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
if(_voxelSize > 0.0f)
{
normalsFrom = util3d::computeNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
else
{
normalsFrom = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
}
else
{
normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
}
Transform toT = guess * toLocalTransform;
Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
if(_voxelSize > 0.0f)
{
normals = util3d::computeNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
viewpointFrom);
}
}
else
{
normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
}
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
if(toScan.channels() == 2 || toScan.channels() == 5)
{
if(_voxelSize > 0.0f)
{
normals = util3d::computeNormals2D(
normalsTo = util3d::computeNormals2D(
toCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
@@ -579,7 +599,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
normals = util3d::computeFastOrganizedNormals2D(
normalsTo = util3d::computeFastOrganizedNormals2D(
toCloudFiltered,
_pointToPlaneK,
_pointToPlaneRadius,
@@ -588,99 +608,116 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
}
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
if(fromScan.channels() == 2 || fromScan.channels() == 5)
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5);
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
tooLowComplexityForPlaneToPlane = true;
UWARN("ICP PointToPlane ignored as structural complexity is too low: %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;
try
if(_libpointmatcher)
{
UASSERT(_libpointmatcherICP != 0);
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
UDEBUG("libpointmatcher icp...done!");
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
UDEBUG("match ratio: %f", matchRatio);
if(!icpT.isNull())
// Compute the transformation to express data in ref
PM::TransformationParameters T;
try
{
fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
hasConverged = true;
UASSERT(_libpointmatcherICP != 0);
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
UDEBUG("libpointmatcher icp...done!");
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());
}
}
catch(const std::exception & e)
else
#endif
{
UWARN("libpointmatcher has failed: %s", e.what());
icpT = util3d::icpPointToPlane(
fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
}
if(!icpT.isNull() && hasConverged)
{
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
}
}
else
#endif
{
icpT = util3d::icpPointToPlane(
fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
}
if(!icpT.isNull() && hasConverged)
{
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
}
transformComputed = true;
}
}
else // ICP Point to Point
if(!transformComputed) // ICP Point to Point
{
if(_pointToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
{
UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
}
@@ -707,7 +744,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
if(_libpointmatcher && !_pointToPlane) // don't use libpointmatcher if it is configured for point to plane
{
// Load point clouds
DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
corelib/src/Rtabmap.cpp
+4 -1
View File
@@ -1123,7 +1123,10 @@ bool Rtabmap::process(
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
}
}
corelib/src/RtabmapThread.cpp
+10 -10
View File
@@ -336,7 +336,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
{
OdometryInfo infoCov;
infoCov.covariance = e->info().odomCovariance;
infoCov.reg.covariance = e->info().odomCovariance;
this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
}
else
@@ -347,7 +347,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
else
{
OdometryInfo infoCov;
infoCov.covariance = e->info().odomCovariance;
infoCov.reg.covariance = e->info().odomCovariance;
this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
}
@@ -570,7 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() ||
odomEvent.info().covariance.at<double>(0,0)>=9999))
odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
{
if(odomEvent.pose().isIdentity())
{
@@ -578,20 +578,20 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
else
{
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
}
pushNewState(kStateTriggeringMap);
covariance_ = cv::Mat();
}
if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
odomEvent.info().covariance.at<double>(0,0)>0.0)
if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
{
// Use largest covariance error (to be independent of the odometry frame rate)
if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
{
covariance_ = odomEvent.info().covariance;
covariance_ = odomEvent.info().reg.covariance;
}
}
@@ -614,7 +614,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
covariance_ = cv::Mat::eye(6,6,CV_64FC1);
}
OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
odomInfo.covariance = covariance_;
odomInfo.reg.covariance = covariance_;
if(ignoreFrame)
{
// set negative id so rtabmap will detect it as an intermediate node
corelib/src/util3d.cpp
+11 -32
View File
@@ -1808,7 +1808,7 @@ cv::Mat projectCloudToCamera(
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
float fx = cameraMatrixK.at<double>(0,0);
@@ -1819,46 +1819,25 @@ cv::Mat projectCloudToCamera(
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
Transform t = cameraTransform.inverse();
const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
const float* vec7Ptr = laserScan.ptr<float>();
int count = 0;
for(int i=0; i<laserScan.cols; ++i)
{
const float* ptr = laserScan.ptr<float>(0, i);
// Get 3D from laser scan
cv::Point3f ptScan;
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
{
ptScan.x = vec2Ptr[i][0];
ptScan.y = vec2Ptr[i][1];
// 2D scans
ptScan.x = ptr[0];
ptScan.y = ptr[1];
ptScan.z = 0;
}
else if(laserScan.type() == CV_32FC3)
else // 3D scans
{
ptScan.x = vec3Ptr[i][0];
ptScan.y = vec3Ptr[i][1];
ptScan.z = vec3Ptr[i][2];
}
else if(laserScan.type() == CV_32FC(4))
{
ptScan.x = vec4Ptr[i][0];
ptScan.y = vec4Ptr[i][1];
ptScan.z = vec4Ptr[i][2];
}
else if(laserScan.type() == CV_32FC(6))
{
ptScan.x = vec6Ptr[i][0];
ptScan.y = vec6Ptr[i][1];
ptScan.z = vec6Ptr[i][2];
}
else // 7f
{
ptScan.x = (vec7Ptr+i*7)[0];
ptScan.y = (vec7Ptr+i*7)[1];
ptScan.z = (vec7Ptr+i*7)[2];
ptScan.x = ptr[0];
ptScan.y = ptr[1];
ptScan.z = ptr[2];
}
ptScan = util3d::transformPoint(ptScan, t);
corelib/src/util3d_surface.cpp
+147
View File
@@ -2338,6 +2338,153 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
return normals;
}
float computeNormalsComplexity(const cv::Mat & scan)
{
if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(scan.cols*2);
bool is2d = scan.channels() == 5;
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
for (int i = 0; i < scan.cols; ++i)
{
const float * ptrScan = scan.ptr<float>(0, i);
if(scan.channels() == 5)
{
if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[2];
ptr[1] = ptrScan[3];
}
}
else if(scan.channels() == 6)
{
if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[3];
ptr[1] = ptrScan[4];
ptr[2] = ptrScan[5];
}
}
else
{
if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[4];
ptr[1] = ptrScan[5];
ptr[2] = ptrScan[6];
}
}
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
}
else if(!scan.empty())
{
UERROR("Scan doesn't have normals!");
}
return 0.0f;
}
float computeNormalsComplexity(const pcl::PointCloud<pcl::PointNormal> & cloud, bool is2d)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(cloud.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
for (unsigned int i = 0; i < cloud.size(); ++i)
{
const pcl::PointNormal & pt = cloud.at(i);
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
if(!is2d)
{
ptr[2] = pt.normal_z;
}
}
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
float computeNormalsComplexity(const pcl::PointCloud<pcl::Normal> & normals, bool is2d)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(normals.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
for (unsigned int i = 0; i < normals.size(); ++i)
{
const pcl::Normal & pt = normals.at(i);
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
if(!is2d)
{
ptr[2] = pt.normal_z;
}
}
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
float computeNormalsComplexity(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, bool is2d)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(cloud.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
for (unsigned int i = 0; i < cloud.size(); ++i)
{
const pcl::PointXYZRGBNormal & pt = cloud.at(i);
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
if(!is2d)
{
ptr[2] = pt.normal_z;
}
}
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius,
guilib/src/MainWindow.cpp
+25 -19
View File
@@ -560,6 +560,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -812,7 +815,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
{
OdometryInfo odomInfo;
odomInfo.covariance = cameraEvent->info().odomCovariance;
odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
if (!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
@@ -927,11 +930,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
pose = _lastOdomPose;
lost = true;
}
else if(odom.info().inliers>0 &&
else if(odom.info().reg.inliers>0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -1202,9 +1205,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(_preferencesDialog->isOdomOnlyInliersShown())
{
std::multimap<int, cv::KeyPoint> kpInliers;
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
}
_ui->imageView_odometry->setFeatures(
kpInliers,
@@ -1271,13 +1274,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
_ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
_ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
}
}
}
@@ -1323,15 +1326,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
//Process info
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().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().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _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/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _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/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
guilib/src/OdometryViewer.cpp
+8 -8
View File
@@ -179,7 +179,7 @@ void OdometryViewer::clear()
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = odom.info().inliers;
int quality = odom.info().reg.inliers;
bool lost = false;
bool lostStateChanged = false;
@@ -193,11 +193,11 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
lost = true;
}
else if(odom.info().inliers>0 &&
else if(odom.info().reg.inliers>0 &&
qualityWarningThr_ &&
odom.info().inliers < qualityWarningThr_)
odom.info().reg.inliers < qualityWarningThr_)
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkYellow);
cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -425,13 +425,13 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
}
}
}
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -863,6 +863,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());
guilib/src/ui/preferencesDialog.ui
+80 -54
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-531</y>
<width>673</width>
<height>2749</height>
<y>-215</y>
<width>678</width>
<height>2736</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>18</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -13497,6 +13497,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
@@ -13598,35 +13653,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpVoxelSize">
<property name="suffix">
@@ -13723,25 +13749,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
@@ -13803,8 +13810,27 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
<item row="11" column="1">
<widget class="QLabel" name="label_429">
<property name="text">
<string>Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
tools/KittiDataset/main.cpp
+5 -5
View File
@@ -400,8 +400,8 @@ int main(int argc, char * argv[])
if(odomInfo.interval>0.0)
speed = odomInfo.transform.x()/odomInfo.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
bool processData = true;
if(iteration % mapUpdate != 0)
@@ -413,11 +413,11 @@ int main(int argc, char * argv[])
}
if(covariance.empty())
{
covariance = odomInfo.covariance;
covariance = odomInfo.reg.covariance;
}
else
{
covariance += odomInfo.covariance;
covariance += odomInfo.reg.covariance;
}
timer.restart();
@@ -431,7 +431,7 @@ int main(int argc, char * argv[])
++iteration;
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
if(processData && rtabmap.getLoopClosureId()>0)
{
printf(" *");
tools/RgbdDataset/main.cpp
+5 -5
View File
@@ -226,8 +226,8 @@ int main(int argc, char * argv[])
Transform pose = odom.process(data, &odomInfo);
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
bool processData = true;
if(detectionRate>0.0f &&
@@ -251,11 +251,11 @@ int main(int argc, char * argv[])
}
if(covariance.empty())
{
covariance = odomInfo.covariance;
covariance = odomInfo.reg.covariance;
}
else
{
covariance += odomInfo.covariance;
covariance += odomInfo.reg.covariance;
}
timer.restart();
@@ -269,7 +269,7 @@ int main(int argc, char * argv[])
++iteration;
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
if(processData && rtabmap.getLoopClosureId()>0)
{
printf(" *");