From 0350e475b2e47e9329d5a484ca8719e354494963 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 5 Aug 2015 20:32:33 -0400 Subject: [PATCH] ICP: when max laser scan max pts is not set, use relative correspondence ratio (backward compatibility) --- corelib/src/Memory.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index a0174e7b1a..943cc9a1b3 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2346,7 +2346,7 @@ Transform Memory::computeIcpTransform( bool hasConverged = false; Transform icpT; int correspondences = 0; - float correspondencesRatio = -1.0f; + float correspondencesRatio = 0.0f; double variance = 1; if(_icpPointToPlane) { @@ -2561,8 +2561,9 @@ Transform Memory::computeIcpTransform( } else { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS.id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()); } UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", @@ -2756,8 +2757,9 @@ Transform Memory::computeScanMatchingTransform( } else { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS->id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()); } UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",