i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

2
corelib/src/OdometryDVO.cpp
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());

14
corelib/src/OdometryF2F.cpp
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",

11
corelib/src/OdometryF2M.cpp
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();
}
}

6
corelib/src/OdometryFovis.cpp
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())
{

12
corelib/src/OdometryMono.cpp
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_;

17
corelib/src/OdometryORBSLAM2.cpp
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)

6
corelib/src/OdometryViso2.cpp
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())

359
corelib/src/RegistrationIcp.cpp
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);

5
corelib/src/Rtabmap.cpp
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);
}
}

20
corelib/src/RtabmapThread.cpp
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

43
corelib/src/util3d.cpp
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);

147
corelib/src/util3d_surface.cpp
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,