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

RegIcp: complexity, checking if second eigen value is also under Icp/PointToPlaneMinComplexity to limit to only one axis. DbViewer: fixed refine link with scans having RGB channel.

Browse Source
This commit is contained in:
matlabbe
2020-11-22 19:32:01 -05:00
parent bce7ae6acd
commit 80199f23b5
2 changed files with 37 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/RegistrationIcp.cpp
View File

@@ -591,6 +591,7 @@ Transform RegistrationIcp::computeTransformationImpl(
double variance = 1.0;
bool transformComputed = false;
bool tooLowComplexityForPlaneToPlane = false;
float secondEigenValue = 1.0f;
cv::Mat complexityVectors;
if( _pointToPlane &&
@@ -614,13 +615,17 @@ Transform RegistrationIcp::computeTransformationImpl(
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
(complexityVectors.rows == 3 && complexityVectors.cols == 3));
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
secondEigenValue = complexityValuesFrom.at<float>(1,0)<complexityValuesTo.at<float>(1,0)?complexityValuesFrom.at<float>(1,0):complexityValuesTo.at<float>(1,0);
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
"PointToPoint is done instead, orientation is still optimized but translation will be limited to "
"direction of normals (%s: %s).",
fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
secondEigenValue,
fromComplexity<toComplexity?"From":"To",
complexityVectors.rows==2?
uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
secondEigenValue<_pointToPlaneMinComplexity?
uFormat("n=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2)).c_str():
uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
if(ULogger::level() == ULogger::kDebug)
@@ -1140,7 +1145,10 @@ Transform RegistrationIcp::computeTransformationImpl(
float a = v.dot(n1);
float b = v.dot(n2);
Eigen::Vector3f vp = n1*a;
vp += n2*b;
if(secondEigenValue >= _pointToPlaneMinComplexity)
{
vp += n2*b;
}
UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
v[0], v[1], v[2], vp[0], vp[1], vp[2]);
v = vp;