From 701dbe4e1552f6a6c52eb66159c620249722898f Mon Sep 17 00:00:00 2001 From: Philippe Babin Date: Fri, 28 Sep 2018 15:28:40 -0400 Subject: [PATCH 1/4] Add robust fcts --- ...WelschOutlierFilterApproximation.ref_trans | 4 - ...s => defaultRobustOutlierFilter.ref_trans} | 0 ...n.yaml => defaultRobustOutlierFilter.yaml} | 7 +- .../defaultRobustWelschOutlierFilter.yaml | 34 --- .../DataPointsFilters/NormalSpace.cpp | 2 +- pointmatcher/Matcher.cpp | 4 + pointmatcher/Matches.cpp | 45 +++- pointmatcher/OutlierFiltersImpl.cpp | 223 ++++++++++++++++-- pointmatcher/OutlierFiltersImpl.h | 52 +++- pointmatcher/PointMatcher.h | 8 +- pointmatcher/Registry.cpp | 2 +- 11 files changed, 314 insertions(+), 67 deletions(-) delete mode 100644 examples/data/icp_data/RobustWelschOutlierFilterApproximation.ref_trans rename examples/data/icp_data/{defaultRobustWelschOutlierFilter.ref_trans => defaultRobustOutlierFilter.ref_trans} (100%) rename examples/data/icp_data/{RobustWelschOutlierFilterApproximation.yaml => defaultRobustOutlierFilter.yaml} (85%) delete mode 100644 examples/data/icp_data/defaultRobustWelschOutlierFilter.yaml diff --git a/examples/data/icp_data/RobustWelschOutlierFilterApproximation.ref_trans b/examples/data/icp_data/RobustWelschOutlierFilterApproximation.ref_trans deleted file mode 100644 index 2cbaa9b1..00000000 --- a/examples/data/icp_data/RobustWelschOutlierFilterApproximation.ref_trans +++ /dev/null @@ -1,4 +0,0 @@ -0.979910671710968 -0.1603235900402069 0.1186658293008804 -0.133875846862793 -0.1776747703552246 0.9719703793525696 -0.1539979726076126 -0.2382475137710571 --0.09064942598342896 0.1719872951507568 0.9809223413467407 -0.06081056594848633 - 0 0 0 1 \ No newline at end of file diff --git a/examples/data/icp_data/defaultRobustWelschOutlierFilter.ref_trans b/examples/data/icp_data/defaultRobustOutlierFilter.ref_trans similarity index 100% rename from examples/data/icp_data/defaultRobustWelschOutlierFilter.ref_trans rename to examples/data/icp_data/defaultRobustOutlierFilter.ref_trans diff --git a/examples/data/icp_data/RobustWelschOutlierFilterApproximation.yaml b/examples/data/icp_data/defaultRobustOutlierFilter.yaml similarity index 85% rename from examples/data/icp_data/RobustWelschOutlierFilterApproximation.yaml rename to examples/data/icp_data/defaultRobustOutlierFilter.yaml index 0b89b032..6622ddb3 100644 --- a/examples/data/icp_data/RobustWelschOutlierFilterApproximation.yaml +++ b/examples/data/icp_data/defaultRobustOutlierFilter.yaml @@ -8,9 +8,10 @@ matcher: epsilon: 0 outlierFilters: - - RobustWelschOutlierFilter: - scale: 1.5 - approximation: 1.5 + - RobustOutlierFilter: + robustFct: cauchy + scaleEstimator: mad + tuning: 1 errorMinimizer: PointToPointErrorMinimizer diff --git a/examples/data/icp_data/defaultRobustWelschOutlierFilter.yaml b/examples/data/icp_data/defaultRobustWelschOutlierFilter.yaml deleted file mode 100644 index 088406b8..00000000 --- a/examples/data/icp_data/defaultRobustWelschOutlierFilter.yaml +++ /dev/null @@ -1,34 +0,0 @@ -readingDataPointsFilters: - -referenceDataPointsFilters: - -matcher: - KDTreeMatcher: - knn: 10 - epsilon: 0 - -outlierFilters: - - RobustWelschOutlierFilter: - -errorMinimizer: - PointToPointErrorMinimizer - -transformationCheckers: - - CounterTransformationChecker: - maxIterationCount: 40 - - DifferentialTransformationChecker: - minDiffRotErr: 0.001 - minDiffTransErr: 0.01 - smoothLength: 4 - -inspector: - NullInspector -# VTKFileInspector: -# dumpDataLinks: 1 -# dumpReading: 1 -# dumpReference: 1 - -logger: - NullLogger -# FileLogger - diff --git a/pointmatcher/DataPointsFilters/NormalSpace.cpp b/pointmatcher/DataPointsFilters/NormalSpace.cpp index 4dd5d1bd..b4e33b79 100644 --- a/pointmatcher/DataPointsFilters/NormalSpace.cpp +++ b/pointmatcher/DataPointsFilters/NormalSpace.cpp @@ -98,7 +98,7 @@ void NormalSpaceDataPointsFilter::inPlaceFilter(DataPoints& cloud) ///(1) put all points of the data into buckets based on their normal direction for (int i = 0; i < nbPoints; ++i) { - assert(normals.col(i).head(3).norm() == 1); + assert(normals.col(i).head(3).norm() > 0.99999); //Theta = polar angle in [0 ; pi] const T theta = std::acos(normals(2, i)); diff --git a/pointmatcher/Matcher.cpp b/pointmatcher/Matcher.cpp index e9d2ca9b..7b99cad3 100644 --- a/pointmatcher/Matcher.cpp +++ b/pointmatcher/Matcher.cpp @@ -36,6 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "PointMatcher.h" #include "PointMatcherPrivate.h" +#include + +using namespace std; + //! Construct without parameter template PointMatcher::Matcher::Matcher(): diff --git a/pointmatcher/Matches.cpp b/pointmatcher/Matches.cpp index be3345ea..4df57c7c 100644 --- a/pointmatcher/Matches.cpp +++ b/pointmatcher/Matches.cpp @@ -75,7 +75,7 @@ T PointMatcher::Matches::getDistsQuantile(const T quantile) const } if (values.size() == 0) throw ConvergenceError("no outlier to filter"); - + if (quantile < 0.0 || quantile > 1.0) throw ConvergenceError("quantile must be between 0 and 1"); @@ -86,5 +86,48 @@ T PointMatcher::Matches::getDistsQuantile(const T quantile) const return values[values.size() * quantile]; } +//! Calculate the Median of Absolute Deviation(MAD), which is median(|x-median(x)|), a kind of robust standard deviation +template +T PointMatcher::Matches::getMedianAbsDeviation() const +{ + vector values; + values.reserve(dists.rows() * dists.cols()); + const long cols = dists.cols(); + const long rows = dists.rows(); + for (int x = 0; x < cols; ++x) + { + for (int y = 0; y < rows; ++y) + { + if (dists(y, x) != numeric_limits::infinity()) + { + values.push_back(dists(y, x)); + } + } + } + if (values.size() == 0) + throw ConvergenceError("no outlier to filter"); + + nth_element(values.begin(), values.begin() + (values.size() / 2), values.end()); + const T median = values[values.size() / 2]; + + // Compute absolute deviation + const unsigned size = values.size(); + for (unsigned i = 0; i < size; ++i) + { + values[i] = fabs(values[i] - median); + } + // Median of the absolute deviation + nth_element(values.begin(), values.begin() + (values.size() / 2), values.end()); + return values[values.size() / 2]; +} + +template +T PointMatcher::Matches::getStandardDeviation() const +{ + auto d = dists.array(); + return std::sqrt((d - d.mean()).square().sum()/(d.size()-1)); +} + + template struct PointMatcher::Matches; template struct PointMatcher::Matches; diff --git a/pointmatcher/OutlierFiltersImpl.cpp b/pointmatcher/OutlierFiltersImpl.cpp index a68c08d3..2efdd8cc 100644 --- a/pointmatcher/OutlierFiltersImpl.cpp +++ b/pointmatcher/OutlierFiltersImpl.cpp @@ -373,33 +373,228 @@ typename PointMatcher::OutlierWeights OutlierFiltersImpl::GenericDescripto template struct OutlierFiltersImpl::GenericDescriptorOutlierFilter; template struct OutlierFiltersImpl::GenericDescriptorOutlierFilter; -// RobustWelschOutlierFilter +// RobustOutlierFilter template -OutlierFiltersImpl::RobustWelschOutlierFilter::RobustWelschOutlierFilter(const Parameters& params): - OutlierFilter("RobustWelschOutlierFilter", RobustWelschOutlierFilter::availableParameters(), params), - squaredScale(pow(Parametrizable::get("scale"),2)), //Note: we use squared distance later on - squaredApproximation(pow(Parametrizable::get("approximation"),2)) +typename OutlierFiltersImpl::RobustOutlierFilter::RobustFctMap +OutlierFiltersImpl::RobustOutlierFilter::robustFcts = { + {"cauchy", RobustFctId::Cauchy}, + {"welsch", RobustFctId::Welsch}, + {"sc", RobustFctId::SwitchableConstraint}, + {"gm", RobustFctId::GM}, + {"tukey", RobustFctId::Tukey}, + {"huber", RobustFctId::Huber}, + {"L1", RobustFctId::L1}, + {"student", RobustFctId::Student} +}; + +template +OutlierFiltersImpl::RobustOutlierFilter::RobustOutlierFilter(const std::string& className, + const ParametersDoc paramsDoc, + const Parameters& params): + OutlierFilter(className, paramsDoc, params), + robustFctName(Parametrizable::get("robustFct")), + tuning(Parametrizable::get("tuning")), + squaredApproximation(pow(Parametrizable::get("approximation"), 2)), + scaleEstimator(Parametrizable::get("scaleEstimator")), + nbIterationForScale(Parametrizable::get("nbIterationForScale")), + distanceType(Parametrizable::get("distanceType")), + robustFctId(-1), + iteration(1), + scale(0.0), + berg_target_scale(0) { + const set validScaleEstimator = {"none", "mad", "berg", "std"}; + if (validScaleEstimator.find(scaleEstimator) == validScaleEstimator.end()) { + throw InvalidParameter("Invalid scale estimator name."); + } + const set validDistanceType = {"point2point", "point2plane"}; + if (validDistanceType.find(distanceType) == validDistanceType.end()) { + throw InvalidParameter("Invalid distance type name."); + } + + resolveEstimatorName(); + + if (scaleEstimator == "berg") { + berg_target_scale = tuning; + + // See \cite{Bergstrom2014} + if (robustFctId == RobustFctId::Cauchy) + { + tuning = 4.3040; + } else if (robustFctId == RobustFctId::Tukey) + { + tuning = 7.0589; + } else if (robustFctId == RobustFctId::Huber) + { + tuning = 2.0138; + } + } } template -typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustWelschOutlierFilter::compute( - const DataPoints& filteredReading, - const DataPoints& filteredReference, - const Matches& input) +OutlierFiltersImpl::RobustOutlierFilter::RobustOutlierFilter(const Parameters& params): + RobustOutlierFilter("RobustOutlierFilter", RobustOutlierFilter::availableParameters(), params) +{ +} + + +template +void OutlierFiltersImpl::RobustOutlierFilter::resolveEstimatorName(){ + if (robustFcts.find(robustFctName) == robustFcts.end()) + { + throw InvalidParameter("Invalid robust function name."); + } + robustFctId = robustFcts[robustFctName]; +} + + template +typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFilter::compute( + const DataPoints& filteredReading, + const DataPoints& filteredReference, + const Matches& input) { - OutlierWeights w = exp(- input.dists.array()/squaredScale); + return this->robustFiltering(filteredReading, filteredReference, input); +} + + + +template +typename PointMatcher::Matrix +OutlierFiltersImpl::RobustOutlierFilter::computePointToPlanDistance( + const DataPoints& reading, + const DataPoints& reference, + const Matches& input) { + + int nbr_read_point = input.dists.cols(); + int nbr_match = input.dists.rows(); + + Matrix normals = reference.getDescriptorViewByName("normals"); + + Vector reading_point(Vector::Zero(3)); + Vector reference_point(Vector::Zero(3)); + Vector normal(3); + + Matrix dists(Matrix::Zero(nbr_match, nbr_read_point)); + + for(int i = 0; i < nbr_read_point; ++i) + { + reading_point = reading.features.block(0, i, 3, 1); + for(int j = 0; j < nbr_match; ++j) + { + const int reference_idx = input.ids(j, i); + if (reference_idx != Matches::InvalidId) { + reference_point = reference.features.block(0, reference_idx, 3, 1); + + normal = normals.col(reference_idx).normalized(); + // distance_point_to_plan = dot(n, p-q)² + dists(j, i) = pow(normal.dot(reading_point-reference_point), 2); + } + } + } + + return dists; +} + +template +typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFilter::robustFiltering( + const DataPoints& filteredReading, + const DataPoints& filteredReference, + const Matches& input) { + + if (scaleEstimator == "mad") + { + if (iteration <= nbIterationForScale or nbIterationForScale == 0) + { + scale = sqrt(input.getMedianAbsDeviation()); + } + } else if (scaleEstimator == "std") + { + if (iteration <= nbIterationForScale or nbIterationForScale == 0) + { + scale = sqrt(input.getStandardDeviation()); + } + } else if (scaleEstimator == "berg") + { + if (iteration <= nbIterationForScale or nbIterationForScale == 0) + { + // The tuning constant is the target scale that we want to reach + // It's a bit confusing to use the tuning constant for scaling... + if (iteration == 1) + { + scale = 1.9 * sqrt(input.getDistsQuantile(0.5)); + } + else + { // TODO: maybe add it has another parameter or make him a function of the max iteration + const T CONVERGENCE_RATE = 0.85; + scale = CONVERGENCE_RATE * (scale - berg_target_scale) + berg_target_scale; + } + } + } + else + { + scale = 1.0; // We don't rescale + } + iteration++; + + auto dists = distanceType == "point2point" ? input.dists : computePointToPlanDistance(filteredReading, filteredReference, input); + + // e² = scaled squared distance + auto e2 = dists.array() / (scale * scale); + + T k = tuning; + const T k2 = k * k; + + OutlierWeights w, aboveThres, bellowThres; + switch (robustFctId) { + case RobustFctId::Cauchy: // 1/(1 + e²/k²) + w = (1 + e2 / k2).inverse(); + break; + case RobustFctId::Welsch: // exp(-e²/k²) + w = (-e2 / k2).exp(); + break; + case RobustFctId::SwitchableConstraint: // if e² > k² then 4 * k²/(k + e²)² + aboveThres = 4.0 * k2 * ((k + e2).square()).inverse(); + w = (e2 >= k).select(aboveThres, 1.0); + break; + case RobustFctId::GM: // k²/(k + e²)² + w = k2*((k + e2).square()).inverse(); + break; + case RobustFctId::Tukey: // if e² < k then (1-e²/k²)² + bellowThres = (1 - e2 / k2).square(); + w = (e2 >= k2).select(0.0, bellowThres); + break; + case RobustFctId::Huber: // if |e| >= k then k/|e| = k/sqrt(e²) + aboveThres = k * (e2.sqrt().inverse()); + w = (e2 >= k2).select(aboveThres, 1.0); + break; + case RobustFctId::L1: // 1/|e| = 1/sqrt(e²) + w = e2.sqrt().inverse(); + break; + case RobustFctId::Student: { // .... + const T d = 3; + auto p = (1 + e2 / k).pow(-(k + d) / 2); + w = p * (k + d) * (k + e2).inverse(); + break; + } + default: + break; + } + + // In the minimizer, zero weight are ignored, we want them to be notice by having the smallest value + // The value can not be a numeric limit, since they might cause a nan/inf. + const double ARBITRARY_SMALL_VALUE = 1e-50; + w = (w.array() <= ARBITRARY_SMALL_VALUE).select(ARBITRARY_SMALL_VALUE, w); + if(squaredApproximation != std::numeric_limits::infinity()) { //Note from Eigen documentation: (if statement).select(then matrix, else matrix) - w = (input.dists.array() >= squaredApproximation).select(0.0, w); + w = (e2 >= squaredApproximation).select(0.0, w); } return w; } -template struct OutlierFiltersImpl::RobustWelschOutlierFilter; -template struct OutlierFiltersImpl::RobustWelschOutlierFilter; - +template struct OutlierFiltersImpl::RobustOutlierFilter; +template struct OutlierFiltersImpl::RobustOutlierFilter; diff --git a/pointmatcher/OutlierFiltersImpl.h b/pointmatcher/OutlierFiltersImpl.h index 5bf9c653..2dd522a1 100644 --- a/pointmatcher/OutlierFiltersImpl.h +++ b/pointmatcher/OutlierFiltersImpl.h @@ -218,25 +218,61 @@ struct OutlierFiltersImpl virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); }; - struct RobustWelschOutlierFilter: public OutlierFilter + struct RobustOutlierFilter: public OutlierFilter { + inline static const std::string description() { - return "Robust weight function part of the M-Estimator familly. The Welsch weight uses an exponential decay reducing the influence of matched point farther away \\cite{RobustWeightFunctions}. More explicitly, the function is w = exp[- (matched distance)^2/scale^2]."; + return "Robust weight function. 8 robust functions to choose from (Cauchy, Welsch, Switchable Constraint, Geman-McClure, Tukey, Huber, L1 and Student). All the functions are M-Estimator (\\cite{RobustWeightFunctions}) except L1 and Student."; } inline static const ParametersDoc availableParameters() { return boost::assign::list_of - ( "scale", "Tuning parameter used to limit the influence of outliers. It could be interpreted as a standard deviation. The unit of this parameter is the same as the distance used, typically meters.", "5.0", "0.0000001", "inf", &P::Comp) + ( "robustFct", "Type of robust function used. Available fct: 'cauchy', 'welsch', 'sc'(aka Switchable-Constraint), 'gm' (aka Geman-McClure), 'tukey', 'huber' and 'L1'. (Default: cauchy)", "cauchy") + ( "tuning", "Tuning parameter used to limit the influence of outliers." + "If the 'scaleEstimator' is 'mad' or 'none', this parameter acts as the tuning parameter." + "If the 'scaleEstimator' is 'berg' this parameter acts as the target scale (σ*).", "1.0", "0.0000001", "inf", &P::Comp) + ( "scaleEstimator", "The scale estimator is used to convert the error distance into a Mahalanobis distance. 3 estimators are available: " + "'none': no estimator (scale = 1), " + "'mad': use the median of absolute deviation (a kind of robust standard deviation), " + "'berg': an iterative exponentially decreasing estimator", "mad") + ( "nbIterationForScale", "For how many iteration the 'scaleEstimator' is recalculated. After 'nbIterationForScale' iteration the previous scale is kept. A nbIterationForScale==0 means that the estiamtor is recalculated at each iteration.", "0", "0", "100", &P::Comp) + ( "distanceType", "Type of error distance used, either point to point ('point2point') or point to plane('point2plane'). Point to point gives better result normally.", "point2point") ( "approximation", "If the matched distance is larger than this threshold, its weight will be forced to zero. This can save computation as zero values are not minimized. If set to inf (default value), no approximation is done. The unit of this parameter is the same as the distance used, typically meters.", "inf", "0.0", "inf", &P::Comp) ; } - - const T squaredScale; - const T squaredApproximation; - - RobustWelschOutlierFilter(const Parameters& params = Parameters()); + + Matrix computePointToPlanDistance(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); + RobustOutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); + RobustOutlierFilter(const Parameters& params = Parameters()); + protected: + enum RobustFctId { + Cauchy=0, + Welsch=1, + SwitchableConstraint=2, + GM=3, + Tukey=4, + Huber=5, + L1=6, + Student=7 + }; + typedef std::map RobustFctMap; + static RobustFctMap robustFcts; + const std::string robustFctName; + T tuning; + const T squaredApproximation; + const std::string scaleEstimator; + const int nbIterationForScale; + const std::string distanceType; + int robustFctId; + int iteration; + T scale; + T berg_target_scale; + + + virtual void resolveEstimatorName(); + virtual OutlierWeights robustFiltering(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); }; }; // OutlierFiltersImpl diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h index 9129a94f..90e01f82 100644 --- a/pointmatcher/PointMatcher.h +++ b/pointmatcher/PointMatcher.h @@ -173,7 +173,10 @@ struct PointMatcher typedef typename Eigen::Matrix IntMatrix; //! A dense signed 64-bits matrix typedef typename Eigen::Matrix Int64Matrix; - + //! A dense array over ScalarType + typedef typename Eigen::Array Array; + + //! A matrix holding the parameters a transformation. /** The transformation lies in the special Euclidean group of dimension \f$n\f$, \f$SE(n)\f$, implemented as a dense matrix of size \f$n+1 \times n+1\f$ over ScalarType. @@ -384,6 +387,9 @@ struct PointMatcher Ids ids; //!< identifiers of closest points T getDistsQuantile(const T quantile) const; + T getMedianAbsDeviation() const; + T getStandardDeviation() const; + }; //! Weights of the associations between the points in Matches and the points in the reference. diff --git a/pointmatcher/Registry.cpp b/pointmatcher/Registry.cpp index 15cd1e63..37e45511 100644 --- a/pointmatcher/Registry.cpp +++ b/pointmatcher/Registry.cpp @@ -101,7 +101,7 @@ PointMatcher::PointMatcher() ADD_TO_REGISTRAR(OutlierFilter, VarTrimmedDistOutlierFilter, typename OutlierFiltersImpl::VarTrimmedDistOutlierFilter) ADD_TO_REGISTRAR(OutlierFilter, SurfaceNormalOutlierFilter, typename OutlierFiltersImpl::SurfaceNormalOutlierFilter) ADD_TO_REGISTRAR(OutlierFilter, GenericDescriptorOutlierFilter, typename OutlierFiltersImpl::GenericDescriptorOutlierFilter) - ADD_TO_REGISTRAR(OutlierFilter, RobustWelschOutlierFilter, typename OutlierFiltersImpl::RobustWelschOutlierFilter) + ADD_TO_REGISTRAR(OutlierFilter, RobustOutlierFilter, typename OutlierFiltersImpl::RobustOutlierFilter) ADD_TO_REGISTRAR_NO_PARAM(ErrorMinimizer, IdentityErrorMinimizer, typename ErrorMinimizersImpl::IdentityErrorMinimizer) ADD_TO_REGISTRAR_NO_PARAM(ErrorMinimizer, PointToPointErrorMinimizer, typename ErrorMinimizersImpl::PointToPointErrorMinimizer) From b2a97949bc6635735fa8f721ba62f23fe4bd9a0d Mon Sep 17 00:00:00 2001 From: Philippe Babin Date: Mon, 1 Oct 2018 11:55:02 -0400 Subject: [PATCH 2/4] Debugging falling test --- utest/utest.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/utest/utest.cpp b/utest/utest.cpp index 888bba25..5cf7fa6b 100644 --- a/utest/utest.cpp +++ b/utest/utest.cpp @@ -93,6 +93,7 @@ TEST(icpTest, icpTest) { if (!fs::is_regular_file(d->status()) ) continue; + std::cout << "Testing file " << d->path().string() << std::endl; // Load config file, and form ICP object PM::ICP icp; std::string config_file = d->path().string(); From c778f90add3727b05b12ae8b80fc6886ff1a1530 Mon Sep 17 00:00:00 2001 From: Philippe Babin Date: Wed, 3 Oct 2018 14:47:23 -0400 Subject: [PATCH 3/4] Fix segfault, cause by undefined behavior of an 'auto'. Also remove eigen warning. --- CMakeLists.txt | 5 ++++- pointmatcher/OutlierFiltersImpl.cpp | 7 +++---- pointmatcher/OutlierFiltersImpl.h | 3 ++- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fe85384c..dab40b0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,9 +131,12 @@ find_path(EIGEN_INCLUDE_DIR Eigen/Core include_directories(${EIGEN_INCLUDE_DIR}) #note: no library to add, eigen rely only on headers +# Suppress Eigen's warning by adding it to the system's library +include_directories(SYSTEM "${EIGEN_INCLUDE_DIR}") + #TODO: this should be a more standard way #find_package(Eigen3 REQUIRED) -#message("-- eigen3 found, version ${Eigen3_VERSION}") +#message("-- eigen3 , version ${EIGEN_VERSION}") diff --git a/pointmatcher/OutlierFiltersImpl.cpp b/pointmatcher/OutlierFiltersImpl.cpp index 2efdd8cc..41be8b65 100644 --- a/pointmatcher/OutlierFiltersImpl.cpp +++ b/pointmatcher/OutlierFiltersImpl.cpp @@ -536,15 +536,14 @@ typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFil } iteration++; - auto dists = distanceType == "point2point" ? input.dists : computePointToPlanDistance(filteredReading, filteredReference, input); + Matrix dists = distanceType == "point2point" ? input.dists : computePointToPlanDistance(filteredReading, filteredReference, input); // e² = scaled squared distance - auto e2 = dists.array() / (scale * scale); + Array e2 = dists.array() / (scale * scale); T k = tuning; const T k2 = k * k; - - OutlierWeights w, aboveThres, bellowThres; + Array w, aboveThres, bellowThres; switch (robustFctId) { case RobustFctId::Cauchy: // 1/(1 + e²/k²) w = (1 + e2 / k2).inverse(); diff --git a/pointmatcher/OutlierFiltersImpl.h b/pointmatcher/OutlierFiltersImpl.h index 2dd522a1..21f44ee4 100644 --- a/pointmatcher/OutlierFiltersImpl.h +++ b/pointmatcher/OutlierFiltersImpl.h @@ -52,7 +52,8 @@ struct OutlierFiltersImpl typedef typename PointMatcher::Matches Matches; typedef typename PointMatcher::OutlierFilter OutlierFilter; typedef typename PointMatcher::OutlierWeights OutlierWeights; - typedef typename PointMatcher::Matrix Matrix; + typedef typename PointMatcher::Matrix Matrix; + typedef typename PointMatcher::Array Array; typedef typename PointMatcher::Vector Vector; struct NullOutlierFilter: public OutlierFilter From 660a72b93886a5e4e20de87570c4a5e094b728fe Mon Sep 17 00:00:00 2001 From: Philippe Babin Date: Tue, 9 Oct 2018 14:12:35 -0400 Subject: [PATCH 4/4] Fix typo --- pointmatcher/OutlierFiltersImpl.cpp | 14 +++++++------- pointmatcher/OutlierFiltersImpl.h | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/pointmatcher/OutlierFiltersImpl.cpp b/pointmatcher/OutlierFiltersImpl.cpp index 41be8b65..a8e362f3 100644 --- a/pointmatcher/OutlierFiltersImpl.cpp +++ b/pointmatcher/OutlierFiltersImpl.cpp @@ -460,7 +460,7 @@ typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFil template typename PointMatcher::Matrix -OutlierFiltersImpl::RobustOutlierFilter::computePointToPlanDistance( +OutlierFiltersImpl::RobustOutlierFilter::computePointToPlaneDistance( const DataPoints& reading, const DataPoints& reference, const Matches& input) { @@ -536,14 +536,14 @@ typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFil } iteration++; - Matrix dists = distanceType == "point2point" ? input.dists : computePointToPlanDistance(filteredReading, filteredReference, input); + Matrix dists = distanceType == "point2point" ? input.dists : computePointToPlaneDistance(filteredReading, filteredReference, input); // e² = scaled squared distance Array e2 = dists.array() / (scale * scale); T k = tuning; const T k2 = k * k; - Array w, aboveThres, bellowThres; + Array w, aboveThres, belowThres; switch (robustFctId) { case RobustFctId::Cauchy: // 1/(1 + e²/k²) w = (1 + e2 / k2).inverse(); @@ -551,16 +551,16 @@ typename PointMatcher::OutlierWeights OutlierFiltersImpl::RobustOutlierFil case RobustFctId::Welsch: // exp(-e²/k²) w = (-e2 / k2).exp(); break; - case RobustFctId::SwitchableConstraint: // if e² > k² then 4 * k²/(k + e²)² + case RobustFctId::SwitchableConstraint: // if e² > k then 4 * k²/(k + e²)² aboveThres = 4.0 * k2 * ((k + e2).square()).inverse(); w = (e2 >= k).select(aboveThres, 1.0); break; case RobustFctId::GM: // k²/(k + e²)² w = k2*((k + e2).square()).inverse(); break; - case RobustFctId::Tukey: // if e² < k then (1-e²/k²)² - bellowThres = (1 - e2 / k2).square(); - w = (e2 >= k2).select(0.0, bellowThres); + case RobustFctId::Tukey: // if e² < k² then (1-e²/k²)² + belowThres = (1 - e2 / k2).square(); + w = (e2 >= k2).select(0.0, belowThres); break; case RobustFctId::Huber: // if |e| >= k then k/|e| = k/sqrt(e²) aboveThres = k * (e2.sqrt().inverse()); diff --git a/pointmatcher/OutlierFiltersImpl.h b/pointmatcher/OutlierFiltersImpl.h index 21f44ee4..1d8906be 100644 --- a/pointmatcher/OutlierFiltersImpl.h +++ b/pointmatcher/OutlierFiltersImpl.h @@ -243,7 +243,7 @@ struct OutlierFiltersImpl ; } - Matrix computePointToPlanDistance(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); + Matrix computePointToPlaneDistance(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); RobustOutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); RobustOutlierFilter(const Parameters& params = Parameters());