Merge pull request #279 from PhiBabin/robust_fcts

Add robust fonctions, such as M-estimators

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}")
 
 
 

examples/data/icp_data/RobustWelschOutlierFilterApproximation.yaml → 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

pointmatcher/DataPointsFilters/NormalSpace.cpp

@@ -98,7 +98,7 @@ void NormalSpaceDataPointsFilter<T>::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)); 

pointmatcher/Matcher.cpp

@@ -36,6 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "PointMatcher.h"
 #include "PointMatcherPrivate.h"
 
+#include <limits>
+
+using namespace std;
+
 //! Construct without parameter
 template<typename T>
 PointMatcher<T>::Matcher::Matcher():

pointmatcher/Matches.cpp

@@ -75,7 +75,7 @@ T PointMatcher<T>::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<T>::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<typename T>
+T PointMatcher<T>::Matches::getMedianAbsDeviation() const
+{
+	vector<T> 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<T>::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<typename T>
+T PointMatcher<T>::Matches::getStandardDeviation() const
+{
+	auto d = dists.array();
+	return std::sqrt((d - d.mean()).square().sum()/(d.size()-1));
+}
+
+
 template struct PointMatcher<float>::Matches;
 template struct PointMatcher<double>::Matches;

pointmatcher/OutlierFiltersImpl.cpp

@@ -373,33 +373,227 @@ typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::GenericDescripto
 template struct OutlierFiltersImpl<float>::GenericDescriptorOutlierFilter;
 template struct OutlierFiltersImpl<double>::GenericDescriptorOutlierFilter;
 
-// RobustWelschOutlierFilter
+// RobustOutlierFilter
 template<typename T>
-OutlierFiltersImpl<T>::RobustWelschOutlierFilter::RobustWelschOutlierFilter(const Parameters& params):
-	OutlierFilter("RobustWelschOutlierFilter", RobustWelschOutlierFilter::availableParameters(), params),
-	squaredScale(pow(Parametrizable::get<T>("scale"),2)), //Note: we use squared distance later on
-	squaredApproximation(pow(Parametrizable::get<T>("approximation"),2))
+typename OutlierFiltersImpl<T>::RobustOutlierFilter::RobustFctMap
+OutlierFiltersImpl<T>::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<typename T>
+OutlierFiltersImpl<T>::RobustOutlierFilter::RobustOutlierFilter(const std::string& className,
+		const ParametersDoc paramsDoc,
+		const Parameters& params):
+	OutlierFilter(className, paramsDoc, params),
+	robustFctName(Parametrizable::get<string>("robustFct")),
+	tuning(Parametrizable::get<T>("tuning")),
+	squaredApproximation(pow(Parametrizable::get<T>("approximation"), 2)),
+	scaleEstimator(Parametrizable::get<string>("scaleEstimator")),
+	nbIterationForScale(Parametrizable::get<int>("nbIterationForScale")),
+	distanceType(Parametrizable::get<string>("distanceType")),
+	robustFctId(-1),
+	iteration(1),
+	scale(0.0),
+	berg_target_scale(0)
 {
+	const set<string> validScaleEstimator = {"none", "mad", "berg", "std"};
+	if (validScaleEstimator.find(scaleEstimator) == validScaleEstimator.end()) {
+		throw InvalidParameter("Invalid scale estimator name.");
+	}
+	const set<string> 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 T>
-typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::RobustWelschOutlierFilter::compute(
-	const DataPoints& filteredReading,
-	const DataPoints& filteredReference,
-	const Matches& input)
+OutlierFiltersImpl<T>::RobustOutlierFilter::RobustOutlierFilter(const Parameters& params):
+	RobustOutlierFilter("RobustOutlierFilter", RobustOutlierFilter::availableParameters(), params)
+{
+}
+
+
+template<typename T>
+void OutlierFiltersImpl<T>::RobustOutlierFilter::resolveEstimatorName(){
+	if (robustFcts.find(robustFctName) == robustFcts.end())
+	{
+		throw InvalidParameter("Invalid robust function name.");
+	}
+	robustFctId = robustFcts[robustFctName];
+}
+
+	template<typename T>
+typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::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 T>
+typename PointMatcher<T>::Matrix
+OutlierFiltersImpl<T>::RobustOutlierFilter::computePointToPlaneDistance(
+		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 T>
+typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::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++;
+
+	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, belowThres;
+	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²)²
+			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());
+			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<T>::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<float>::RobustWelschOutlierFilter;
-template struct OutlierFiltersImpl<double>::RobustWelschOutlierFilter;
-
 
+template struct OutlierFiltersImpl<float>::RobustOutlierFilter;
+template struct OutlierFiltersImpl<double>::RobustOutlierFilter;