58 return OutlierWeights::Constant(input.
ids.rows(), input.
ids.cols(), 1);
123 const T limit = factor * median;
172 const T tunedRatio = optimizeInlierRatio(input);
183 typedef typename Eigen::Array<T, Eigen::Dynamic, 1> LineArray;
185 const int points_nbr =
matches.dists.rows() *
matches.dists.cols();
188 std::vector<T> tmpSortedDist;
189 tmpSortedDist.reserve(points_nbr);
190 for (
int x = 0;
x <
matches.dists.cols(); ++
x)
191 for (
int y = 0; y <
matches.dists.rows(); ++y)
192 if ((
matches.dists(y,
x) != numeric_limits<T>::infinity()) && (
matches.dists(y,
x) > 0))
193 tmpSortedDist.push_back(
matches.dists(y,
x));
194 if (tmpSortedDist.empty())
195 throw ConvergenceError(
"Inlier ratio optimization failed due to absence of matches");
197 std::sort(tmpSortedDist.begin(), tmpSortedDist.end());
198 std::vector<T> tmpCumSumSortedDist;
199 tmpCumSumSortedDist.reserve(points_nbr);
200 std::partial_sum(tmpSortedDist.begin(), tmpSortedDist.end(), tmpCumSumSortedDist.begin());
202 const int minEl = floor(this->minRatio*points_nbr);
203 const int maxEl = floor(this->maxRatio*points_nbr);
206 Eigen::Map<LineArray> sortedDist(&tmpCumSumSortedDist[0], points_nbr);
208 const LineArray trunkSortedDist = sortedDist.segment(minEl, maxEl-minEl);
210 const LineArray ids = LineArray::LinSpaced(trunkSortedDist.rows(), minEl+1, maxEl);
211 const LineArray ratio = ids / points_nbr;
212 const LineArray deno = ratio.pow(this->lambda);
214 const LineArray FRMS = trunkSortedDist * ids.inverse() * deno.inverse().square() ;
216 FRMS.minCoeff(&minIndex);
217 const T optRatio = (float)(minIndex + minEl)/ (float)points_nbr;
230 warningPrinted(false)
247 if(normalsReading.cols() != 0 && normalsReference.cols() != 0)
249 for (
int x = 0;
x < w.cols(); ++
x)
251 const Vector normalRead = normalsReading.col(
x).normalized();
253 for (
int y = 0; y < w.rows(); ++y)
255 const int idRef = input.
ids(y,
x);
262 const Vector normalRef = normalsReference.col(idRef).normalized();
264 const T value = normalRead.dot(normalRef);
275 if(warningPrinted ==
false)
277 LOG_INFO_STREAM(
"SurfaceNormalOutlierFilter: surface normals not available. Skipping filtering");
278 warningPrinted =
true;
281 w = Matrix::Ones(input.
dists.rows(), input.
dists.cols());
303 (boost::format(
"GenericDescriptorOutlierFilter: Error, the parameter named 'source' can only be set to 'reference' or 'reading' but was set to %1%") %
source).str());
316 const int readPtsCount = input.
dists.cols();
322 if(source ==
"reference")
323 cloud = &filteredReference;
325 cloud = &filteredReading;
332 (boost::format(
"GenericDescriptorOutlierFilter: Error, the parameter named 'descName' must be a 1D descriptor but the field %1% is %2%D") % descName % desc.rows()).str());
335 for(
int k=0; k <
knn; k++)
337 for(
int i=0; i < readPtsCount; i++)
340 if (source ==
"reference") {
341 point_id = input.
ids(k, i);
343 LOG_INFO_STREAM(
"Invalid Index in GenericOutlierFilter, setting weight to 0.");
352 if(useSoftThreshold ==
false)
354 if(useLargerThan ==
true)
356 if (desc(0, point_id) > threshold)
363 if (desc(0, point_id) < threshold)
372 w(k,i) = desc(0, point_id);
391 {
"cauchy", RobustFctId::Cauchy},
392 {
"welsch", RobustFctId::Welsch},
393 {
"sc", RobustFctId::SwitchableConstraint},
394 {
"gm", RobustFctId::GM},
395 {
"tukey", RobustFctId::Tukey},
396 {
"huber", RobustFctId::Huber},
397 {
"L1", RobustFctId::L1},
398 {
"student", RobustFctId::Student}
417 const set<string> validScaleEstimator = {
"none",
"mad",
"berg",
"std"};
418 if (validScaleEstimator.find(
scaleEstimator) == validScaleEstimator.end()) {
421 const set<string> validDistanceType = {
"point2point",
"point2plane"};
422 if (validDistanceType.find(
distanceType) == validDistanceType.end()) {
454 if (robustFcts.find(robustFctName) == robustFcts.end())
458 robustFctId = robustFcts[robustFctName];
467 return this->robustFiltering(filteredReading, filteredReference, input);
479 int nbr_read_point = input.
dists.cols();
480 int nbr_match = input.
dists.rows();
484 Vector reading_point(Vector::Zero(3));
485 Vector reference_point(Vector::Zero(3));
488 Matrix dists(Matrix::Zero(nbr_match, nbr_read_point));
490 for(
int i = 0; i < nbr_read_point; ++i)
492 reading_point =
reading.features.block(0, i, 3, 1);
493 for(
int j = 0; j < nbr_match; ++j)
495 const int reference_idx = input.
ids(j, i);
497 reference_point =
reference.features.block(0, reference_idx, 3, 1);
499 normal = normals.col(reference_idx).normalized();
501 dists(j, i) =
pow(normal.dot(reading_point-reference_point), 2);
515 if (scaleEstimator ==
"mad")
517 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
521 }
else if (scaleEstimator ==
"std")
523 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
527 }
else if (scaleEstimator ==
"berg")
529 if (iteration <= nbIterationForScale or nbIterationForScale == 0)
539 const T CONVERGENCE_RATE = 0.85;
540 scale = CONVERGENCE_RATE * (scale - berg_target_scale) + berg_target_scale;
550 Matrix dists = distanceType ==
"point2point" ? input.
dists : computePointToPlaneDistance(filteredReading, filteredReference, input);
553 Array e2 = dists.array() / (scale * scale);
557 Array w, aboveThres, belowThres;
558 switch (robustFctId) {
559 case RobustFctId::Cauchy:
560 w = (1 + e2 / k2).inverse();
562 case RobustFctId::Welsch:
563 w = (-e2 / k2).exp();
565 case RobustFctId::SwitchableConstraint:
566 aboveThres = 4.0 * k2 * ((k + e2).square()).inverse();
567 w = (e2 >= k).select(aboveThres, 1.0);
569 case RobustFctId::GM:
570 w = k2*((k + e2).square()).inverse();
572 case RobustFctId::Tukey:
573 belowThres = (1 - e2 / k2).square();
574 w = (e2 >= k2).select(0.0, belowThres);
576 case RobustFctId::Huber:
577 aboveThres = k * (e2.sqrt().inverse());
578 w = (e2 >= k2).select(aboveThres, 1.0);
580 case RobustFctId::L1:
581 w = e2.sqrt().inverse();
583 case RobustFctId::Student: {
585 auto p = (1 + e2 / k).
pow(-(k + d) / 2);
586 w = p * (k + d) * (k + e2).inverse();
595 const double ARBITRARY_SMALL_VALUE = 1e-50;
596 w = (w.array() <= ARBITRARY_SMALL_VALUE).select(ARBITRARY_SMALL_VALUE, w);
599 if(squaredApproximation != std::numeric_limits<T>::infinity())
602 w = (e2 >= squaredApproximation).select(0.0, w);