00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "OutlierFiltersImpl.h"
00037 #include "PointMatcherPrivate.h"
00038 #include "Functions.h"
00039 #include "MatchersImpl.h"
00040
00041 #include <algorithm>
00042 #include <vector>
00043 #include <iostream>
00044 #include <limits>
00045 #include <numeric>
00046 #include <ciso646>
00047
00048 using namespace std;
00049 using namespace PointMatcherSupport;
00050
00051
00052 template<typename T>
00053 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::NullOutlierFilter::compute(
00054 const DataPoints& filteredReading,
00055 const DataPoints& filteredReference,
00056 const Matches& input)
00057 {
00058 return OutlierWeights::Constant(input.ids.rows(), input.ids.cols(), 1);
00059 }
00060
00061 template struct OutlierFiltersImpl<float>::NullOutlierFilter;
00062 template struct OutlierFiltersImpl<double>::NullOutlierFilter;
00063
00064
00065
00066 template<typename T>
00067 OutlierFiltersImpl<T>::MaxDistOutlierFilter::MaxDistOutlierFilter(const Parameters& params):
00068 OutlierFilter("MaxDistOutlierFilter", MaxDistOutlierFilter::availableParameters(), params),
00069 maxDist(pow(Parametrizable::get<T>("maxDist"),2))
00070 {
00071 }
00072
00073
00074 template<typename T>
00075 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MaxDistOutlierFilter::compute(
00076 const DataPoints& filteredReading,
00077 const DataPoints& filteredReference,
00078 const Matches& input)
00079 {
00080 return OutlierWeights((input.dists.array() <= maxDist).template cast<T>());
00081 }
00082
00083 template struct OutlierFiltersImpl<float>::MaxDistOutlierFilter;
00084 template struct OutlierFiltersImpl<double>::MaxDistOutlierFilter;
00085
00086
00087 template<typename T>
00088 OutlierFiltersImpl<T>::MinDistOutlierFilter::MinDistOutlierFilter(const Parameters& params):
00089 OutlierFilter("MinDistOutlierFilter", MinDistOutlierFilter::availableParameters(), params),
00090 minDist(pow(Parametrizable::get<T>("minDist"),2))
00091 {
00092 }
00093
00094 template<typename T>
00095 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MinDistOutlierFilter::compute(
00096 const DataPoints& filteredReading,
00097 const DataPoints& filteredReference,
00098 const Matches& input)
00099 {
00100 return OutlierWeights((input.dists.array() >= minDist).template cast<T>());
00101 }
00102
00103 template struct OutlierFiltersImpl<float>::MinDistOutlierFilter;
00104 template struct OutlierFiltersImpl<double>::MinDistOutlierFilter;
00105
00106
00107
00108
00109 template<typename T>
00110 OutlierFiltersImpl<T>::MedianDistOutlierFilter::MedianDistOutlierFilter(const Parameters& params):
00111 OutlierFilter("MedianDistOutlierFilter", MedianDistOutlierFilter::availableParameters(), params),
00112 factor(Parametrizable::get<T>("factor"))
00113 {
00114 }
00115
00116 template<typename T>
00117 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::MedianDistOutlierFilter::compute(
00118 const DataPoints& filteredReading,
00119 const DataPoints& filteredReference,
00120 const Matches& input)
00121 {
00122 const T median = input.getDistsQuantile(0.5);
00123 const T limit = factor * median;
00124 return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00125 }
00126
00127 template struct OutlierFiltersImpl<float>::MedianDistOutlierFilter;
00128 template struct OutlierFiltersImpl<double>::MedianDistOutlierFilter;
00129
00130
00131
00132 template<typename T>
00133 OutlierFiltersImpl<T>::TrimmedDistOutlierFilter::TrimmedDistOutlierFilter(const Parameters& params):
00134 OutlierFilter("TrimmedDistOutlierFilter", TrimmedDistOutlierFilter::availableParameters(), params),
00135 ratio(Parametrizable::get<T>("ratio"))
00136 {
00137 }
00138
00139 template<typename T>
00140 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::TrimmedDistOutlierFilter::compute(
00141 const DataPoints& filteredReading,
00142 const DataPoints& filteredReference,
00143 const Matches& input)
00144 {
00145 const T limit = input.getDistsQuantile(ratio);
00146 return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00147 }
00148
00149 template struct OutlierFiltersImpl<float>::TrimmedDistOutlierFilter;
00150 template struct OutlierFiltersImpl<double>::TrimmedDistOutlierFilter;
00151
00152
00153 template<typename T>
00154 OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::VarTrimmedDistOutlierFilter(const Parameters& params):
00155 OutlierFilter("VarTrimmedDistOutlierFilter", VarTrimmedDistOutlierFilter::availableParameters(), params),
00156 minRatio(Parametrizable::get<T>("minRatio")),
00157 maxRatio(Parametrizable::get<T>("maxRatio")),
00158 lambda(Parametrizable::get<T>("lambda"))
00159 {
00160 if (this->minRatio >= this->maxRatio)
00161 {
00162 throw InvalidParameter((boost::format("VarTrimmedDistOutlierFilter: minRatio (%1%) should be smaller than maxRatio (%2%)") % minRatio % maxRatio).str());
00163 }
00164 }
00165
00166 template<typename T>
00167 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::compute(
00168 const DataPoints& filteredReading,
00169 const DataPoints& filteredReference,
00170 const Matches& input)
00171 {
00172 const T tunedRatio = optimizeInlierRatio(input);
00173 LOG_INFO_STREAM("Optimized ratio: " << tunedRatio);
00174
00175 const T limit = input.getDistsQuantile(tunedRatio);
00176 return OutlierWeights((input.dists.array() <= limit).template cast<T>());
00177 }
00178
00179 template<typename T>
00180 T OutlierFiltersImpl<T>::VarTrimmedDistOutlierFilter::optimizeInlierRatio(const Matches& matches)
00181 {
00182 typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00183 typedef typename Eigen::Array<T, Eigen::Dynamic, 1> LineArray;
00184
00185 const int points_nbr = matches.dists.rows() * matches.dists.cols();
00186
00187
00188 std::vector<T> tmpSortedDist;
00189 tmpSortedDist.reserve(points_nbr);
00190 for (int x = 0; x < matches.dists.cols(); ++x)
00191 for (int y = 0; y < matches.dists.rows(); ++y)
00192 if ((matches.dists(y, x) != numeric_limits<T>::infinity()) && (matches.dists(y, x) > 0))
00193 tmpSortedDist.push_back(matches.dists(y, x));
00194 if (tmpSortedDist.size() == 0)
00195 throw ConvergenceError("no outlier to filter");
00196
00197 std::sort(tmpSortedDist.begin(), tmpSortedDist.end());
00198 std::vector<T> tmpCumSumSortedDist;
00199 tmpCumSumSortedDist.reserve(points_nbr);
00200 std::partial_sum(tmpSortedDist.begin(), tmpSortedDist.end(), tmpCumSumSortedDist.begin());
00201
00202 const int minEl = floor(this->minRatio*points_nbr);
00203 const int maxEl = floor(this->maxRatio*points_nbr);
00204
00205
00206 Eigen::Map<LineArray> sortedDist(&tmpCumSumSortedDist[0], points_nbr);
00207
00208 const LineArray trunkSortedDist = sortedDist.segment(minEl, maxEl-minEl);
00209
00210 const LineArray ids = LineArray::LinSpaced(trunkSortedDist.rows(), minEl+1, maxEl);
00211 const LineArray ratio = ids / points_nbr;
00212 const LineArray deno = ratio.pow(this->lambda);
00213
00214 const LineArray FRMS = trunkSortedDist * ids.inverse() * deno.inverse().square() ;
00215 int minIndex(0);
00216 FRMS.minCoeff(&minIndex);
00217 const T optRatio = (float)(minIndex + minEl)/ (float)points_nbr;
00218
00219 return optRatio;
00220 }
00221
00222 template struct OutlierFiltersImpl<float>::VarTrimmedDistOutlierFilter;
00223 template struct OutlierFiltersImpl<double>::VarTrimmedDistOutlierFilter;
00224
00225
00226 template<typename T>
00227 OutlierFiltersImpl<T>::SurfaceNormalOutlierFilter::SurfaceNormalOutlierFilter(const Parameters& params):
00228 OutlierFilter("SurfaceNormalOutlierFilter", SurfaceNormalOutlierFilter::availableParameters(), params),
00229 eps(cos(Parametrizable::get<T>("maxAngle"))),
00230 warningPrinted(false)
00231 {
00232
00233 }
00234
00235 template<typename T>
00236 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::SurfaceNormalOutlierFilter::compute(
00237 const DataPoints& filteredReading,
00238 const DataPoints& filteredReference,
00239 const Matches& input)
00240 {
00241 const BOOST_AUTO(normalsReading, filteredReading.getDescriptorViewByName("normals"));
00242 const BOOST_AUTO(normalsReference, filteredReference.getDescriptorViewByName("normals"));
00243
00244
00245 OutlierWeights w(input.dists.rows(), input.dists.cols());
00246
00247 if(normalsReading.cols() != 0 && normalsReference.cols() != 0)
00248 {
00249 for (int x = 0; x < w.cols(); ++x)
00250 {
00251 const Vector normalRead = normalsReading.col(x).normalized();
00252
00253 for (int y = 0; y < w.rows(); ++y)
00254 {
00255 const int idRef = input.ids(y, x);
00256
00257 if (idRef == MatchersImpl<T>::NNS::InvalidIndex) {
00258 w(y, x) = 0;
00259 continue;
00260 }
00261
00262 const Vector normalRef = normalsReference.col(idRef).normalized();
00263
00264 const T value = anyabs(normalRead.dot(normalRef));
00265
00266 if(value < eps)
00267 w(y, x) = 0;
00268 else
00269 w(y, x) = 1;
00270 }
00271 }
00272 }
00273 else
00274 {
00275 if(warningPrinted == false)
00276 {
00277 LOG_INFO_STREAM("SurfaceNormalOutlierFilter: surface normals not available. Skipping filtering");
00278 warningPrinted = true;
00279 }
00280
00281 w = Matrix::Ones(input.dists.rows(), input.dists.cols());
00282 }
00283
00284 return w;
00285 }
00286
00287 template struct OutlierFiltersImpl<float>::SurfaceNormalOutlierFilter;
00288 template struct OutlierFiltersImpl<double>::SurfaceNormalOutlierFilter;
00289
00290
00291 template<typename T>
00292 OutlierFiltersImpl<T>::GenericDescriptorOutlierFilter::GenericDescriptorOutlierFilter(const Parameters& params):
00293 OutlierFilter("GenericDescriptorOutlierFilter", GenericDescriptorOutlierFilter::availableParameters(), params),
00294 source(Parametrizable::getParamValueString("source")),
00295 descName(Parametrizable::getParamValueString("descName")),
00296 useSoftThreshold(Parametrizable::get<bool>("useSoftThreshold")),
00297 useLargerThan(Parametrizable::get<bool>("useLargerThan")),
00298 threshold(Parametrizable::get<T>("threshold"))
00299 {
00300 if(source != "reference" && source != "reading")
00301 {
00302 throw InvalidParameter(
00303 (boost::format("GenericDescriptorOutlierFilter: Error, the parameter named 'source' can only be set to 'reference' or 'reading' but was set to %1%") % source).str());
00304 }
00305 }
00306
00307 template<typename T>
00308 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::GenericDescriptorOutlierFilter::compute(
00309 const DataPoints& filteredReading,
00310 const DataPoints& filteredReference,
00311 const Matches& input)
00312 {
00313 typedef typename DataPoints::ConstView ConstView;
00314
00315 const int knn = input.dists.rows();
00316 const int readPtsCount = input.dists.cols();
00317
00318 OutlierWeights w(knn, readPtsCount);
00319
00320 const DataPoints *cloud;
00321
00322 if(source == "reference")
00323 cloud = &filteredReference;
00324 else
00325 cloud = &filteredReference;
00326
00327 ConstView desc(cloud->getDescriptorViewByName(descName));
00328
00329 if(desc.rows() != 1)
00330 {
00331 throw InvalidParameter(
00332 (boost::format("GenericDescriptorOutlierFilter: Error, the parameter named 'descName' must be a 1D descriptor but the field %1% is %2%D") % descName % desc.rows()).str());
00333 }
00334
00335 for(int k=0; k < knn; k++)
00336 {
00337 for(int i=0; i < readPtsCount; i++)
00338 {
00339 const int idRead = input.ids(k, i);
00340 if (idRead == MatchersImpl<T>::NNS::InvalidIndex){
00341 w(k,i) = 0;
00342 continue;
00343 }
00344 if(useSoftThreshold == false)
00345 {
00346 if(useLargerThan == true)
00347 {
00348 if (desc(0, idRead) > threshold)
00349 w(k,i) = 1;
00350 else
00351 w(k,i) = 0;
00352 }
00353 else
00354 {
00355 if (desc(0, idRead) < threshold)
00356 w(k,i) = 1;
00357 else
00358 w(k,i) = 0;
00359 }
00360 }
00361 else
00362 {
00363
00364 w(k,i) = desc(0, idRead);
00365 }
00366 }
00367 }
00368
00369
00370 if(useSoftThreshold)
00371 w = w/w.maxCoeff();
00372
00373 return w;
00374 }
00375
00376 template struct OutlierFiltersImpl<float>::GenericDescriptorOutlierFilter;
00377 template struct OutlierFiltersImpl<double>::GenericDescriptorOutlierFilter;
00378
00379
00380 template<typename T>
00381 typename OutlierFiltersImpl<T>::RobustOutlierFilter::RobustFctMap
00382 OutlierFiltersImpl<T>::RobustOutlierFilter::robustFcts = {
00383 {"cauchy", RobustFctId::Cauchy},
00384 {"welsch", RobustFctId::Welsch},
00385 {"sc", RobustFctId::SwitchableConstraint},
00386 {"gm", RobustFctId::GM},
00387 {"tukey", RobustFctId::Tukey},
00388 {"huber", RobustFctId::Huber},
00389 {"L1", RobustFctId::L1},
00390 {"student", RobustFctId::Student}
00391 };
00392
00393 template<typename T>
00394 OutlierFiltersImpl<T>::RobustOutlierFilter::RobustOutlierFilter(const std::string& className,
00395 const ParametersDoc paramsDoc,
00396 const Parameters& params):
00397 OutlierFilter(className, paramsDoc, params),
00398 robustFctName(Parametrizable::get<string>("robustFct")),
00399 tuning(Parametrizable::get<T>("tuning")),
00400 squaredApproximation(pow(Parametrizable::get<T>("approximation"), 2)),
00401 scaleEstimator(Parametrizable::get<string>("scaleEstimator")),
00402 nbIterationForScale(Parametrizable::get<int>("nbIterationForScale")),
00403 distanceType(Parametrizable::get<string>("distanceType")),
00404 robustFctId(-1),
00405 iteration(1),
00406 scale(0.0),
00407 berg_target_scale(0)
00408 {
00409 const set<string> validScaleEstimator = {"none", "mad", "berg", "std"};
00410 if (validScaleEstimator.find(scaleEstimator) == validScaleEstimator.end()) {
00411 throw InvalidParameter("Invalid scale estimator name.");
00412 }
00413 const set<string> validDistanceType = {"point2point", "point2plane"};
00414 if (validDistanceType.find(distanceType) == validDistanceType.end()) {
00415 throw InvalidParameter("Invalid distance type name.");
00416 }
00417
00418 resolveEstimatorName();
00419
00420 if (scaleEstimator == "berg") {
00421 berg_target_scale = tuning;
00422
00423
00424 if (robustFctId == RobustFctId::Cauchy)
00425 {
00426 tuning = 4.3040;
00427 } else if (robustFctId == RobustFctId::Tukey)
00428 {
00429 tuning = 7.0589;
00430 } else if (robustFctId == RobustFctId::Huber)
00431 {
00432 tuning = 2.0138;
00433 }
00434 }
00435 }
00436
00437 template<typename T>
00438 OutlierFiltersImpl<T>::RobustOutlierFilter::RobustOutlierFilter(const Parameters& params):
00439 RobustOutlierFilter("RobustOutlierFilter", RobustOutlierFilter::availableParameters(), params)
00440 {
00441 }
00442
00443
00444 template<typename T>
00445 void OutlierFiltersImpl<T>::RobustOutlierFilter::resolveEstimatorName(){
00446 if (robustFcts.find(robustFctName) == robustFcts.end())
00447 {
00448 throw InvalidParameter("Invalid robust function name.");
00449 }
00450 robustFctId = robustFcts[robustFctName];
00451 }
00452
00453 template<typename T>
00454 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::RobustOutlierFilter::compute(
00455 const DataPoints& filteredReading,
00456 const DataPoints& filteredReference,
00457 const Matches& input)
00458 {
00459 return this->robustFiltering(filteredReading, filteredReference, input);
00460 }
00461
00462
00463
00464 template<typename T>
00465 typename PointMatcher<T>::Matrix
00466 OutlierFiltersImpl<T>::RobustOutlierFilter::computePointToPlaneDistance(
00467 const DataPoints& reading,
00468 const DataPoints& reference,
00469 const Matches& input) {
00470
00471 int nbr_read_point = input.dists.cols();
00472 int nbr_match = input.dists.rows();
00473
00474 Matrix normals = reference.getDescriptorViewByName("normals");
00475
00476 Vector reading_point(Vector::Zero(3));
00477 Vector reference_point(Vector::Zero(3));
00478 Vector normal(3);
00479
00480 Matrix dists(Matrix::Zero(nbr_match, nbr_read_point));
00481
00482 for(int i = 0; i < nbr_read_point; ++i)
00483 {
00484 reading_point = reading.features.block(0, i, 3, 1);
00485 for(int j = 0; j < nbr_match; ++j)
00486 {
00487 const int reference_idx = input.ids(j, i);
00488 if (reference_idx != Matches::InvalidId) {
00489 reference_point = reference.features.block(0, reference_idx, 3, 1);
00490
00491 normal = normals.col(reference_idx).normalized();
00492
00493 dists(j, i) = pow(normal.dot(reading_point-reference_point), 2);
00494 }
00495 }
00496 }
00497
00498 return dists;
00499 }
00500
00501 template<typename T>
00502 typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::RobustOutlierFilter::robustFiltering(
00503 const DataPoints& filteredReading,
00504 const DataPoints& filteredReference,
00505 const Matches& input) {
00506
00507 if (scaleEstimator == "mad")
00508 {
00509 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
00510 {
00511 scale = sqrt(input.getMedianAbsDeviation());
00512 }
00513 } else if (scaleEstimator == "std")
00514 {
00515 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
00516 {
00517 scale = sqrt(input.getStandardDeviation());
00518 }
00519 } else if (scaleEstimator == "berg")
00520 {
00521 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
00522 {
00523
00524
00525 if (iteration == 1)
00526 {
00527 scale = 1.9 * sqrt(input.getDistsQuantile(0.5));
00528 }
00529 else
00530 {
00531 const T CONVERGENCE_RATE = 0.85;
00532 scale = CONVERGENCE_RATE * (scale - berg_target_scale) + berg_target_scale;
00533 }
00534 }
00535 }
00536 else
00537 {
00538 scale = 1.0;
00539 }
00540 iteration++;
00541
00542 Matrix dists = distanceType == "point2point" ? input.dists : computePointToPlaneDistance(filteredReading, filteredReference, input);
00543
00544
00545 Array e2 = dists.array() / (scale * scale);
00546
00547 T k = tuning;
00548 const T k2 = k * k;
00549 Array w, aboveThres, belowThres;
00550 switch (robustFctId) {
00551 case RobustFctId::Cauchy:
00552 w = (1 + e2 / k2).inverse();
00553 break;
00554 case RobustFctId::Welsch:
00555 w = (-e2 / k2).exp();
00556 break;
00557 case RobustFctId::SwitchableConstraint:
00558 aboveThres = 4.0 * k2 * ((k + e2).square()).inverse();
00559 w = (e2 >= k).select(aboveThres, 1.0);
00560 break;
00561 case RobustFctId::GM:
00562 w = k2*((k + e2).square()).inverse();
00563 break;
00564 case RobustFctId::Tukey:
00565 belowThres = (1 - e2 / k2).square();
00566 w = (e2 >= k2).select(0.0, belowThres);
00567 break;
00568 case RobustFctId::Huber:
00569 aboveThres = k * (e2.sqrt().inverse());
00570 w = (e2 >= k2).select(aboveThres, 1.0);
00571 break;
00572 case RobustFctId::L1:
00573 w = e2.sqrt().inverse();
00574 break;
00575 case RobustFctId::Student: {
00576 const T d = 3;
00577 auto p = (1 + e2 / k).pow(-(k + d) / 2);
00578 w = p * (k + d) * (k + e2).inverse();
00579 break;
00580 }
00581 default:
00582 break;
00583 }
00584
00585
00586
00587 const double ARBITRARY_SMALL_VALUE = 1e-50;
00588 w = (w.array() <= ARBITRARY_SMALL_VALUE).select(ARBITRARY_SMALL_VALUE, w);
00589
00590
00591 if(squaredApproximation != std::numeric_limits<T>::infinity())
00592 {
00593
00594 w = (e2 >= squaredApproximation).select(0.0, w);
00595 }
00596
00597 return w;
00598 }
00599
00600
00601 template struct OutlierFiltersImpl<float>::RobustOutlierFilter;
00602 template struct OutlierFiltersImpl<double>::RobustOutlierFilter;