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

RegIcp: don't assert if normals cannot be computed properly before doing complexity check, just reject the transform

Browse Source
This commit is contained in:
matlabbe
2020-12-12 16:53:07 -05:00
parent 51826c9a97
commit 1acf8ff193
2 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
corelib/src/RegistrationIcp.cpp
View File

@@ -732,23 +732,29 @@ Transform RegistrationIcp::computeTransformationImpl(
if(complexity < _pointToPlaneMinComplexity) if(complexity < _pointToPlaneMinComplexity)
{ {
tooLowComplexityForPlaneToPlane = true; tooLowComplexityForPlaneToPlane = true;
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo; if(complexity > 0.0f)
{
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)|| complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
(complexityVectors.rows == 3 && complexityVectors.cols == 3));
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());
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
(complexityVectors.rows == 3 && complexityVectors.cols == 3));
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());
}
else
{
UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
}
if(ULogger::level() == ULogger::kDebug) if(ULogger::level() == ULogger::kDebug)
{ {
std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl; std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
@@ -962,18 +968,24 @@ Transform RegistrationIcp::computeTransformationImpl(
if(complexity < _pointToPlaneMinComplexity) if(complexity < _pointToPlaneMinComplexity)
{ {
tooLowComplexityForPlaneToPlane = true; tooLowComplexityForPlaneToPlane = true;
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo; if(complexity > 0.0f)
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)|| {
(complexityVectors.rows == 3 && complexityVectors.cols == 3)); complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). " UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
"PointToPoint is done instead, orientation is still optimized but translation will be limited to " (complexityVectors.rows == 3 && complexityVectors.cols == 3));
"direction of normals (%s: %s).", UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(), "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
fromComplexity<toComplexity?"From":"To", "direction of normals (%s: %s).",
complexityVectors.rows==2? fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str(): fromComplexity<toComplexity?"From":"To",
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()); complexityVectors.rows==2?
uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).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());
}
else
{
UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
}
if(ULogger::level() == ULogger::kDebug) if(ULogger::level() == ULogger::kDebug)
{ {
std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl; std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
@@ -1261,7 +1273,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{ {
if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2) if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2)
{ {
if(_pointToPlaneLowComplexityStrategy == 0) if(complexityVectors.empty() || _pointToPlaneLowComplexityStrategy == 0)
{ {
msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str()); msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
icpT.setNull(); icpT.setNull();

2
corelib/src/util3d_surface.cpp
View File

@@ -3018,7 +3018,7 @@ float computeNormalsComplexity(
{ {
*pcaEigenValues = pca_analysis.eigenvalues; *pcaEigenValues = pca_analysis.eigenvalues;
} }
UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity // 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 pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
} }