i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.

Browse Source
This commit is contained in:
matlabbe
2018-09-28 22:02:07 -04:00
parent dbb9cfa77a
commit 8b055752aa
4 changed files with 90 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Memory.cpp
View File

@@ -801,6 +801,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
!_signatures.at(*_stMem.rbegin())->getPose().isNull())
{
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
if(maxAngVar != 1.0 && maxAngVar > 0.1)
{
UWARN("Very large angular variance (%f) detected! Please fix odometry "
"twist covariance, otherwise poor graph optimizations are "
"expected and wrong loop closure detections creating a lot "
"of errors in the map could be accepted.", maxAngVar);
}
cv::Mat infMatrix;
if(_covOffDiagonalIgnored)
{

44
corelib/src/RegistrationIcp.cpp
View File

@@ -1046,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
icpT = guess * t.inverse() * guessInv;
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
if(fromScan.hasNormals() && toScan.hasNormals())
{
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
else
{
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
_maxCorrespondenceDistance,
variance,
correspondences);
}
}
else
{
@@ -1097,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
if(maxLaserScans)
{
correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1125,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
info.icpTranslation,
info.icpRotation);
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
if(correspondences == 0)
{
UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
}
else
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
}
info.icpInliersRatio = correspondencesRatio;
if(correspondencesRatio < _correspondenceRatio)
if(correspondencesRatio <= _correspondenceRatio)
{
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);

1
corelib/src/Rtabmap.cpp
View File

@@ -1203,6 +1203,7 @@ bool Rtabmap::process(
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
{
std::cout << info.covariance << std::endl;
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}
}