diff --git a/include/RegistrationTools.h b/include/RegistrationTools.h index beaa467..dc716f6 100644 --- a/include/RegistrationTools.h +++ b/include/RegistrationTools.h @@ -161,6 +161,8 @@ namespace CCCoreLib , minRMSDecrease(1.0e-5) , nbMaxIterations(20) , adjustScale(false) + , minScale(std::numeric_limits::quiet_NaN()) + , maxScale(std::numeric_limits::quiet_NaN()) , filterOutFarthestPoints(false) , samplingLimit(50000) , finalOverlapRatio(1.0) @@ -185,6 +187,12 @@ namespace CCCoreLib //! Whether to release the scale parameter during the registration procedure or not bool adjustScale; + //! Min scale (if adjustScale is true - NaN if no minimum boundary) + double minScale; + + //! Max scale (if adjustScale is true - NaN if no maximum boundary) + double maxScale; + //! If true, the algorithm will automatically ignore farthest points from the reference, for better convergence bool filterOutFarthestPoints; diff --git a/src/RegistrationTools.cpp b/src/RegistrationTools.cpp index 79ff8e5..b74c0e4 100644 --- a/src/RegistrationTools.cpp +++ b/src/RegistrationTools.cpp @@ -811,8 +811,18 @@ ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register( GenericIndexed if (params.adjustScale) { - transform.s *= currentTrans.s; - transform.T *= currentTrans.s; + double newScale = transform.s * currentTrans.s; + if (std::isfinite(params.minScale)) + { + newScale = std::max(newScale, params.minScale); + } + if (std::isfinite(params.maxScale)) + { + newScale = std::min(newScale, params.maxScale); + } + + transform.T *= (newScale / transform.s); + transform.s = newScale; } transform.T += currentTrans.T;