Ocean
Loading...
Searching...
No Matches
Estimator.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_GEOMETRY_ESTIMATOR_H
9#define META_OCEAN_GEOMETRY_ESTIMATOR_H
10
11#include "ocean/geometry/Geometry.h"
12
13#include "ocean/base/Median.h"
14
15namespace Ocean
16{
17
18namespace Geometry
19{
20
21/**
22 * This class implements robust estimator functions.
23 * See 'Parameter Estimation Techniques: A Tutorial with Application to Conic Fitting', Zhengyou Zhang, 1997 for detailed information.
24 * @ingroup geometry
25 */
26class OCEAN_GEOMETRY_EXPORT Estimator
27{
28 public:
29
30 /**
31 * Definition of individual robust estimator types.
32 */
33 enum EstimatorType : uint32_t
34 {
35 /**
36 * An invalid estimator type.
37 */
38 ET_INVALID = 0u,
39
40 /**
41 * The standard square error estimator (L2).
42 * The estimation function is defined by:
43 * <pre>
44 * p(x) = x^2 / 2
45 * </pre>
46 *
47 * The weighting function is given by:
48 * <pre>
49 * w(x) = 1
50 * </pre>
51 */
52 ET_SQUARE,
53
54 /**
55 * The linear estimator (L1).
56 * The estimation function is defined by:
57 * <pre>
58 * p(x) = |x|
59 * </pre>
60 *
61 * The weighting function is given by:
62 * <pre>
63 * w(x) = 1 / |x|
64 * </pre>
65 */
66 ET_LINEAR,
67
68 /**
69 * The Huber estimator type.
70 * The estimation function is defined by:
71 * <pre>
72 * p(x, s) = x^2 / 2, if |x| <= s
73 * = s * (|x| - s / 2), else
74 * </pre>
75 *
76 * The weighting function is given by:
77 * <pre>
78 * w(x, s) = 1, if |x| <= s
79 * = s / |x|, else
80 * </pre>
81 *
82 * The sigma tuning constant is given as: 1.345
83 */
84 ET_HUBER,
85
86 /**
87 * The Tukey estimator.
88 * The estimation function is defined by:
89 * <pre>
90 * p(x, s) = s^2 / 6 * (1 - (1 - (x / s)^2)^3), if |x| <= s
91 * = s^2 / 6, else
92 * </pre>
93 *
94 * The weighting function is given by:
95 * <pre>
96 * w(x, s) = (1 - (x / s)^2)^2, if |x| <= s
97 * = 0, else
98 * </pre>
99 *
100 * The sigma tuning constant is given as: 4.6851
101 */
102 ET_TUKEY,
103
104 /**
105 * The Cauchy estimator.
106 * The estimation function is defined by:
107 * <pre>
108 * p(x, s) = s^2 / 2 * log(1 + (x / s)^2)
109 * </pre>
110 *
111 * The weighting function is defined by:
112 * <pre>
113 * w(x, s) = 1 / (1 + (x / s)^2)
114 * </pre>
115 *
116 * The sigma tuning constant is given as: 2.3849
117 */
118 ET_CAUCHY
119 };
120
121 /**
122 * Definition of a vector holding estimator types.
123 */
124 using EstimatorTypes = std::vector<EstimatorType>;
125
126 public:
127
128 /**
129 * Returns whether an estimator needs a standard deviation for computation.
130 * @tparam tEstimator Estimator for that the dependency is requested
131 * @return True, if so
132 */
133 template <EstimatorType tEstimator>
134 static constexpr bool needSigma();
135
136 /**
137 * Returns whether an estimator is the standard square error estimator.
138 * @return True, if so
139 * @tparam tEstimator Estimator to check
140 */
141 template <EstimatorType tEstimator>
142 static constexpr bool isStandardEstimator();
143
144 /**
145 * Returns whether an estimator is the standard square error estimator.
146 * @param estimator The estimator to check
147 * @return True, if so
148 */
149 static inline bool isStandardEstimator(const EstimatorType estimator);
150
151 /**
152 * Returns whether a given estimator needs a standard deviation for computation.
153 * @param estimator The estimator for that the dependency is requested
154 * @return True, if so
155 */
156 static inline bool needSigma(const EstimatorType estimator);
157
158 /**
159 * Returns the robust error of a residual error for a specified estimator.
160 * @param value Residual error to return the robust error for, with range (-infinity, infinity)
161 * @param sigma Standard deviation of the expected residual error, with range (0, infinity) if 'needSigma<tEstimator>() == false', otherwise 0
162 * @return The resulting robust error, with range [0, infinity)
163 * @tparam tEstimator Type of the estimator to use
164 * @see needSigma().
165 */
166 template <EstimatorType tEstimator>
167 static inline Scalar robustError(const Scalar value, const Scalar sigma = 0);
168
169 /**
170 * Returns the robust error of a given residual error for a specified estimator.
171 * @param value Residual error to return the robust error for, with range (-infinity, infinity)
172 * @param sigma Standard deviation of the expected residual error, with range (0, infinity) if 'needSigma(estimator) == false', otherwise 0
173 * @param estimator Type of the estimator to use
174 * @return The resulting robust error, with range [0, infinity)
175 * @see needSigma().
176 */
177 static inline Scalar robustError(const Scalar value, const Scalar sigma, const EstimatorType estimator);
178
179 /**
180 * Returns the robust error of a squared residual error for a specified estimator.
181 * @param sqrValue Squared residual error to return the robust error for, with range [0, infinity)
182 * @param sqrSigma Standard deviation of the expected residual error, with range (0, infinity) if 'needSigma<tEstimator>() == false', otherwise 0
183 * @return The resulting robust error, with range [0, infinity)
184 * @tparam tEstimator Type of the estimator to use
185 * @see needSigma().
186 */
187 template <EstimatorType tEstimator>
188 static inline Scalar robustErrorSquare(const Scalar sqrValue, const Scalar sqrSigma = 0);
189
190 /**
191 * Returns the robust error of a given squared residual error for a specified estimator.
192 * @param sqrValue Residual error to return the robust error for, with range [0, infinity)
193 * @param sqrSigma Standard deviation of the expected residual error, with range (0, infinity) if 'needSigma(estimator) == false', otherwise 0
194 * @param estimator Type of the estimator to use
195 * @return The resulting robust error, with range [0, infinity)
196 * @see needSigma().
197 */
198 static inline Scalar robustErrorSquare(const Scalar sqrValue, const Scalar sqrSigma, const EstimatorType estimator);
199
200 /**
201 * Returns the weight in relation to a error for a given residual error and a specified estimator.
202 * @param value Residual error to return the weight for
203 * @param sigma Standard deviation of the expected residual error, beware: provide a valid standard deviation if necessary with range (0, infinity)
204 * @return Resulting weight
205 * @tparam tEstimator Type of the estimator to use
206 */
207 template <EstimatorType tEstimator>
208 static inline Scalar robustWeight(const Scalar value, const Scalar sigma = 0);
209
210 /**
211 * Returns the weight in relation to a error for a given residual error and a specified estimator.
212 * @param value Residual error to return the weight for
213 * @param sigma Standard deviation of the expected residual error
214 * @param estimator Type of the estimator to use
215 * @return Resulting weight
216 */
217 static inline Scalar robustWeight(const Scalar value, const Scalar sigma, const EstimatorType estimator);
218
219 /**
220 * Returns the weight in relation to a squared error for a given residual error and a specified estimator.
221 * @param sqrValue Squared residual error to return the weight for
222 * @param sqrSigma Squared standard deviation of the expected residual error, beware: provide a valid standard deviation if necessary with range (0, infinity)
223 * @return Resulting weight (not the squared result), with range [0, infinity)
224 * @tparam tEstimator Type of the estimator to use
225 */
226 template <EstimatorType tEstimator>
227 static inline Scalar robustWeightSquare(const Scalar sqrValue, const Scalar sqrSigma = 0);
228
229 /**
230 * Returns the weight in relation to a squared error for a given residual error and a specified estimator.
231 * @param sqrValue Squared residual error to return the weight for
232 * @param sqrSigma Squared standard deviation of the expected residual error
233 * @param estimator Type of the estimator to use
234 * @return Resulting weight (not the squared result), with range [0, infinity)
235 */
236 static inline Scalar robustWeightSquare(const Scalar sqrValue, const Scalar sqrSigma, const EstimatorType estimator);
237
238 /**
239 * Determines the sigma for a specific set of residual errors and a specified estimator.
240 * @param errors Residual errors for that the corresponding sigma has to be determined
241 * @param number The number of provided residual errors, with range [1, infinity)
242 * @param modelParameters Number of the parameter that define the model
243 * @return Resulting sigma
244 * @tparam tEstimator Type of the estimator to use
245 */
246 template <EstimatorType tEstimator>
247 static inline Scalar determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters);
248
249 /**
250 * Determines the sigma for a specific set of residual errors and a specified estimator.
251 * @param errors Residual errors for that the corresponding sigma has to be determined
252 * @param number The number of provided residual errors, with range [1, infinity)
253 * @param modelParameters Number of the parameter that define the model
254 * @param estimator Type of the estimator to use
255 * @return Resulting sigma
256 */
257 static inline Scalar determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters, const EstimatorType estimator);
258
259 /**
260 * Determines the sigma for a specific subset of residual errors and a specified estimator.
261 * @param errors Residual errors for that the corresponding sigma has to be determined
262 * @param indices Indices of the subset of the residual errors, beware: no range check is applied
263 * @param numberIndices Number of provided indices, with range [1, infinity)
264 * @param modelParameters Number of the parameter that define the model, with range [1, infinity)
265 * @return Resulting sigma
266 * @tparam tEstimator Type of the estimator to use
267 */
268 template <EstimatorType tEstimator>
269 static inline Scalar determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters);
270
271 /**
272 * Determines the sigma for a specific subset of residual errors and a specified estimator.
273 * @param errors Residual errors for that the corresponding sigma has to be determined
274 * @param indices Indices of the subset of the residual errors, beware: no range check is applied
275 * @param numberIndices Number of provided indices, with range [1, infinity)
276 * @param modelParameters Number of the parameter that define the model
277 * @param estimator Type of the estimator to use
278 * @return Resulting sigma
279 */
280 static inline Scalar determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator);
281
282 /**
283 * Determines the squared sigma for a specific set of squared residual errors and a specified estimator.
284 * @param sqrErrors Squared residual errors for that the corresponding sigma has to be determined
285 * @param number The number of provided residual errors, with range [1, infinity)
286 * @param modelParameters Number of the parameter that define the model
287 * @return Resulting sigma (not the squared sigma)
288 * @tparam tEstimator Type of the estimator to use
289 */
290 template <EstimatorType tEstimator>
291 static inline Scalar determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters);
292
293 /**
294 * Determines the squared sigma for a specific set of squared residual errors and a specified estimator.
295 * @param sqrErrors Squared residual errors for that the corresponding sigma has to be determined
296 * @param number The number of provided residual errors, with range [1, infinity)
297 * @param modelParameters Number of the parameter that define the model
298 * @param estimator Type of the estimator to use
299 * @return Resulting sigma (not the squared sigma)
300 */
301 static inline Scalar determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters, const EstimatorType estimator);
302
303 /**
304 * Determines the squared sigma for a specific set of squared residual errors and a specified estimator.
305 * @param sqrErrors Squared residual errors for that the corresponding sigma has to be determined
306 * @param indices Indices of the subset of the residual errors, beware: no range check is applied
307 * @param numberIndices Number of provided indices, with range [1, infinity)
308 * @param modelParameters Number of the parameter that define the model
309 * @return Resulting sigma (not the squared sigma)
310 * @tparam tEstimator Type of the estimator to use
311 */
312 template <EstimatorType tEstimator>
313 static inline Scalar determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters);
314
315 /**
316 * Determines the squared sigma for a specific set of squared residual errors and a specified estimator.
317 * @param sqrErrors Squared residual errors for that the corresponding sigma has to be determined
318 * @param indices Indices of the subset of the residual errors, beware: no range check is applied
319 * @param numberIndices Number of provided indices, with range [1, infinity)
320 * @param modelParameters Number of the parameter that define the model
321 * @param estimator Type of the estimator to use
322 * @return Resulting sigma (not the squared sigma)
323 */
324 static inline Scalar determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator);
325
326 /**
327 * Determines the overall robust error for set of given squared errors, a specified estimator and the dimension of the model.
328 * @param sqrErrors The squared error values for which the overall robust error will be determined
329 * @param number The number of provided squared error values, with range [1, infinity)
330 * @param modelParameters Number of parameters that define the model that has to be optimized (the dimension of the model), with range [1, infinity)
331 * @return Resulting overall robust error for the given set of squared errors
332 * @tparam tEstimator Robust error estimator to be used
333 */
334 template <Estimator::EstimatorType tEstimator>
335 static inline Scalar determineRobustError(const Scalar* sqrErrors, const size_t number, const size_t modelParameters);
336
337 /**
338 * Returns the maximal weight for any estimator which is used to clamp extremely high weights (for tiny errors).
339 * @return The maximal weight to be applied
340 */
341 static constexpr Scalar maximalWeight();
342
343 /**
344 * Returns the inverse maximal weight for any estimator which is used to clamp extremely high weights (for tiny errors).
345 * @return Returns 1 / maximalWeight()
346 */
347 static constexpr Scalar invMaximalWeight();
348
349 /**
350 * Translates a given estimator type into a readable string.
351 * @param estimatorType The type of the estimator to translate
352 * @return The readable string, 'Invalid' if unknown
353 */
354 static std::string translateEstimatorType(const EstimatorType estimatorType);
355
356 /**
357 * Translates a readable name of an estimator type to it's value.
358 * @param estimatorType The name of the estimator type for which the value will be returned
359 * @return The estimator type, ET_INVALID if invalid
360 */
361 static EstimatorType translateEstimatorType(const std::string& estimatorType);
362
363 /**
364 * Returns all existing valid estimator types.
365 * @return The valid estimator types
366 */
367 static const EstimatorTypes& estimatorTypes();
368
369 protected:
370
371 /**
372 * Returns the tuning constant allowing to determine a 95 percent efficiency on the standard normal distribution for individual estimators.
373 * @return Tuning constant
374 */
375 template <EstimatorType tEstimator>
376 static inline Scalar sigmaTuningContstant();
377};
378
379template <Estimator::EstimatorType tEstimator>
380constexpr bool Estimator::needSigma()
381{
382 return tEstimator == Estimator::ET_HUBER || tEstimator == Estimator::ET_TUKEY || tEstimator == Estimator::ET_CAUCHY;
383}
384
385inline bool Estimator::needSigma(const EstimatorType estimator)
386{
387 switch (estimator)
388 {
389 case ET_SQUARE:
390 return needSigma<ET_SQUARE>();
391
392 case ET_LINEAR:
393 return needSigma<ET_LINEAR>();
394
395 case ET_HUBER:
396 return needSigma<ET_HUBER>();
397
398 case ET_TUKEY:
399 return needSigma<ET_TUKEY>();
400
401 case ET_CAUCHY:
402 return needSigma<ET_CAUCHY>();
403
404 case ET_INVALID:
405 break;
406 }
407
408 ocean_assert(false && "Invalid estimator!");
409 return false;
410}
411
412template <Estimator::EstimatorType tEstimator>
413constexpr bool Estimator::isStandardEstimator()
414{
415 static_assert(tEstimator != Estimator::ET_INVALID, "Invalid estimator!");
416
417 return tEstimator == Estimator::ET_SQUARE;
418}
419
420inline bool Estimator::isStandardEstimator(const EstimatorType estimator)
421{
422 ocean_assert(estimator != ET_INVALID);
423
424 return estimator == ET_SQUARE;
425}
426
427template <Estimator::EstimatorType tEstimator>
428inline Scalar Estimator::robustError(const Scalar value, const Scalar /*sigma*/)
429{
430 ocean_assert(false && "Invalid estimator type!");
431
432 return value;
433}
434
435template <>
436inline Scalar Estimator::robustError<Estimator::ET_SQUARE>(const Scalar value, const Scalar sigma)
437{
438 ocean_assert_and_suppress_unused(sigma == Scalar(0), sigma);
439
440 // value^2 / 2
441
442 return value * value * Scalar(0.5);
443}
444
445template <>
446inline Scalar Estimator::robustError<Estimator::ET_LINEAR>(const Scalar value, const Scalar sigma)
447{
448 ocean_assert_and_suppress_unused(sigma == Scalar(0), sigma);
449
450 // |value|
451
452 return Numeric::abs(value);
453}
454
455template <>
456inline Scalar Estimator::robustError<Estimator::ET_HUBER>(const Scalar value, const Scalar sigma)
457{
458 // |value| <= sigma : value * value / 2
459 // else : sigma * (|value| - sigma / 2)
460
461 ocean_assert(sigma > 0);
462
463 const Scalar absValue = Numeric::abs(value);
464
465 if (absValue <= sigma)
466 {
467 return value * value * Scalar(0.5);
468 }
469
470 return sigma * (absValue - sigma * Scalar(0.5));
471}
472
473template <>
474inline Scalar Estimator::robustError<Estimator::ET_TUKEY>(const Scalar value, const Scalar sigma)
475{
476 // |value| <= sigma : sigma^2 / 6 * (1 - (1 - (value / sigma)^2 )^3 )
477 // else : sigma^2 / 6
478
479 ocean_assert(sigma > 0);
480
481 const Scalar sqrSigma6 = sigma * sigma * Scalar(1.0 / 6.0);
482
483 if (Numeric::abs(value) <= sigma)
484 {
485 const Scalar tmp(Scalar(1) - Numeric::sqr(value / sigma));
486
487 return sqrSigma6 * (Scalar(1) - tmp * tmp * tmp);
488 }
489
490 return sqrSigma6;
491}
492
493template <>
494inline Scalar Estimator::robustError<Estimator::ET_CAUCHY>(const Scalar value, const Scalar sigma)
495{
496 // sigma^2 / 2 * log(1 + (value / sigma)^2)
497
498 ocean_assert(sigma > 0);
499
500 return Numeric::log(Scalar(1) + Numeric::sqr(value / sigma)) * Numeric::sqr(sigma) * Scalar(0.5);
501}
502
503inline Scalar Estimator::robustError(const Scalar value, const Scalar sigma, const EstimatorType estimator)
504{
505 switch (estimator)
506 {
507 case ET_SQUARE:
508 return robustError<ET_SQUARE>(value);
509
510 case ET_LINEAR:
511 return robustError<ET_LINEAR>(value);
512
513 case ET_HUBER:
514 return robustError<ET_HUBER>(value, sigma);
515
516 case ET_TUKEY:
517 return robustError<ET_TUKEY>(value, sigma);
518
519 case ET_CAUCHY:
520 return robustError<ET_CAUCHY>(value, sigma);
521
522 case ET_INVALID:
523 break;
524 }
525
526 ocean_assert(false && "Invalid estimator!");
527 return robustError<ET_SQUARE>(value);
528}
529
530template <Estimator::EstimatorType tEstimator>
531inline Scalar Estimator::robustErrorSquare(const Scalar sqrValue, const Scalar /*sqrSigma*/)
532{
533 ocean_assert(false && "Invalid estimator type!");
534
535 return sqrValue;
536}
537
538template <>
539inline Scalar Estimator::robustErrorSquare<Estimator::ET_SQUARE>(const Scalar sqrValue, const Scalar sqrSigma)
540{
541 ocean_assert_and_suppress_unused(sqrSigma == Scalar(0), sqrSigma);
542
543 // value^2 / 2
544
545 return sqrValue * Scalar(0.5);
546}
547
548template <>
549inline Scalar Estimator::robustErrorSquare<Estimator::ET_LINEAR>(const Scalar sqrValue, const Scalar sqrSigma)
550{
551 ocean_assert_and_suppress_unused(sqrSigma == Scalar(0), sqrSigma);
552
553 // |value|
554
555 return Numeric::sqrt(sqrValue);
556}
557
558template <>
559inline Scalar Estimator::robustErrorSquare<Estimator::ET_HUBER>(const Scalar sqrValue, const Scalar sqrSigma)
560{
561 // |value| <= sigma : value * value / 2
562 // else : sigma * (|value| - sigma / 2) = sigma * |value| - sigma^2 / 2
563
564 ocean_assert(sqrSigma > 0);
565
566 if (sqrValue <= sqrSigma)
567 {
568 return sqrValue * Scalar(0.5);
569 }
570
571 return Numeric::sqrt(sqrValue) * Numeric::sqrt(sqrSigma) - sqrSigma * Scalar(0.5);
572}
573
574template <>
575inline Scalar Estimator::robustErrorSquare<Estimator::ET_TUKEY>(const Scalar sqrValue, const Scalar sqrSigma)
576{
577 // |value| <= sigma : sigma^2 / 6 * (1 - (1 - (value / sigma)^2 )^3 )
578 // else : sigma^2 / 6
579
580 ocean_assert(sqrSigma > 0);
581
582 if (sqrValue <= sqrSigma)
583 {
584 const Scalar tmp(Scalar(1) - sqrValue / sqrSigma);
585
586 return sqrSigma * Scalar(1.0 / 6.0) * (Scalar(1) - tmp * tmp * tmp);
587 }
588
589 return sqrSigma * Scalar(1.0 / 6.0);
590}
591
592template <>
593inline Scalar Estimator::robustErrorSquare<Estimator::ET_CAUCHY>(const Scalar sqrValue, const Scalar sqrSigma)
594{
595 // sigma^2 / 2 * log(1 + (value / sigma)^2)
596
597 ocean_assert(sqrSigma > 0);
598
599 return Numeric::log(Scalar(1) + sqrValue / sqrSigma) * sqrSigma * Scalar(0.5);
600}
601
602inline Scalar Estimator::robustErrorSquare(const Scalar sqrValue, const Scalar sqrSigma, const EstimatorType estimator)
603{
604 switch (estimator)
605 {
606 case ET_SQUARE:
607 return robustErrorSquare<ET_SQUARE>(sqrValue);
608
609 case ET_LINEAR:
610 return robustErrorSquare<ET_LINEAR>(sqrValue);
611
612 case ET_HUBER:
613 return robustErrorSquare<ET_HUBER>(sqrValue, sqrSigma);
614
615 case ET_TUKEY:
616 return robustErrorSquare<ET_TUKEY>(sqrValue, sqrSigma);
617
618 case ET_CAUCHY:
619 return robustErrorSquare<ET_CAUCHY>(sqrValue, sqrSigma);
620
621 case ET_INVALID:
622 break;
623 }
624
625 ocean_assert(false && "Invalid estimator!");
626 return robustErrorSquare<ET_SQUARE>(sqrValue);
627}
628
629template <Estimator::EstimatorType tEstimator>
630inline Scalar Estimator::robustWeight(const Scalar value, const Scalar /*sigma*/)
631{
632 ocean_assert(false && "Invalid estimator!");
633
634 return value;
635}
636
637template <>
638inline Scalar Estimator::robustWeight<Estimator::ET_SQUARE>(const Scalar /*value*/, const Scalar sigma)
639{
640 ocean_assert_and_suppress_unused(sigma == Scalar(0), sigma);
641
642 return Scalar(1);
643}
644
645template <>
646inline Scalar Estimator::robustWeight<Estimator::ET_LINEAR>(const Scalar value, const Scalar sigma)
647{
648 ocean_assert_and_suppress_unused(sigma == Scalar(0), sigma);
649
650 const Scalar absValue = Numeric::abs(value);
651
652 return std::min(Scalar(1) / absValue, maximalWeight());
653}
654
655template <>
656inline Scalar Estimator::robustWeight<Estimator::ET_HUBER>(const Scalar value, const Scalar sigma)
657{
658 ocean_assert(sigma > 0);
659
660 const Scalar absValue = Numeric::abs(value);
661
662 if (absValue <= sigma)
663 {
664 return Scalar(1);
665 }
666
667 return std::min(sigma / absValue, maximalWeight());
668}
669
670template <>
671inline Scalar Estimator::robustWeight<Estimator::ET_TUKEY>(const Scalar value, const Scalar sigma)
672{
673 ocean_assert(sigma > 0);
674
675 const Scalar absValue = Numeric::abs(value);
676
677 if (absValue > sigma)
678 {
679 return Scalar(0);
680 }
681
682 return std::min(Numeric::sqr(Scalar(1) - Numeric::sqr(value / sigma)), maximalWeight());
683}
684
685template <>
686inline Scalar Estimator::robustWeight<Estimator::ET_CAUCHY>(const Scalar value, const Scalar sigma)
687{
688 ocean_assert(sigma > 0);
689
690 return Scalar(1) / (Scalar(1) + Numeric::sqr(value / sigma));
691}
692
693inline Scalar Estimator::robustWeight(const Scalar value, const Scalar sigma, const EstimatorType estimator)
694{
695 switch (estimator)
696 {
697 case ET_SQUARE:
698 return robustWeight<ET_SQUARE>(value);
699
700 case ET_LINEAR:
701 return robustWeight<ET_LINEAR>(value);
702
703 case ET_HUBER:
704 return robustWeight<ET_HUBER>(value, sigma);
705
706 case ET_TUKEY:
707 return robustWeight<ET_TUKEY>(value, sigma);
708
709 case ET_CAUCHY:
710 return robustWeight<ET_CAUCHY>(value, sigma);
711
712 case ET_INVALID:
713 break;
714 }
715
716 ocean_assert(false && "Invalid estimator!");
717 return robustWeight<ET_SQUARE>(value);
718}
719
720template <Estimator::EstimatorType tEstimator>
721inline Scalar Estimator::robustWeightSquare(const Scalar sqrValue, const Scalar /*sqrSigma*/)
722{
723 ocean_assert(false && "Invalid estimator!");
724 return sqrValue;
725}
726
727template <>
728inline Scalar Estimator::robustWeightSquare<Estimator::ET_SQUARE>(const Scalar /*sqrValue*/, const Scalar sqrSigma)
729{
730 ocean_assert_and_suppress_unused(sqrSigma == Scalar(0), sqrSigma);
731
732 return Scalar(1);
733}
734
735template <>
736inline Scalar Estimator::robustWeightSquare<Estimator::ET_LINEAR>(const Scalar sqrValue, const Scalar sqrSigma)
737{
738 ocean_assert(sqrValue >= 0);
739 ocean_assert_and_suppress_unused(sqrSigma == Scalar(0), sqrSigma);
740
741 if (sqrValue < Numeric::sqr(Numeric::weakEps()))
742 {
743 return Scalar(1) / Numeric::weakEps();
744 }
745
746 return Scalar(1) / Numeric::sqrt(sqrValue);
747}
748
749template <>
750inline Scalar Estimator::robustWeightSquare<Estimator::ET_HUBER>(const Scalar sqrValue, const Scalar sqrSigma)
751{
752 ocean_assert(sqrValue >= 0 && sqrSigma > 0);
753
754 if (sqrValue <= sqrSigma)
755 {
756 return Scalar(1);
757 }
758
759 return std::min(Numeric::sqrt(sqrSigma / sqrValue), maximalWeight());
760}
761
762template <>
763inline Scalar Estimator::robustWeightSquare<Estimator::ET_TUKEY>(const Scalar sqrValue, const Scalar sqrSigma)
764{
765 ocean_assert(sqrSigma > 0);
766
767 if (sqrValue > sqrSigma)
768 {
769 return Scalar(0);
770 }
771
772 return std::min(Numeric::sqr(Scalar(1) - sqrValue / sqrSigma), maximalWeight());
773}
774
775template <>
776inline Scalar Estimator::robustWeightSquare<Estimator::ET_CAUCHY>(const Scalar sqrValue, const Scalar sqrSigma)
777{
778 ocean_assert(sqrSigma > 0);
779
780 return Scalar(1) / Scalar(1 + sqrValue / sqrSigma);
781}
782
783inline Scalar Estimator::robustWeightSquare(const Scalar sqrValue, const Scalar sqrSigma, const EstimatorType estimator)
784{
785 switch (estimator)
786 {
787 case ET_SQUARE:
788 return robustWeightSquare<ET_SQUARE>(sqrValue);
789
790 case ET_LINEAR:
791 return robustWeightSquare<ET_LINEAR>(sqrValue);
792
793 case ET_HUBER:
794 return robustWeightSquare<ET_HUBER>(sqrValue, sqrSigma);
795
796 case ET_TUKEY:
797 return robustWeightSquare<ET_TUKEY>(sqrValue, sqrSigma);
798
799 case ET_CAUCHY:
800 return robustWeightSquare<ET_CAUCHY>(sqrValue, sqrSigma);
801
802 case ET_INVALID:
803 break;
804 }
805
806 ocean_assert(false && "Invalid estimator!");
807 return robustWeightSquare<ET_SQUARE>(sqrValue);
808}
809
810template <Estimator::EstimatorType tEstimator>
811inline Scalar Estimator::determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters)
812{
813 ocean_assert(number > 0);
814
815 const Scalar median = Median::constMedian(errors, number);
816
817 if (number <= modelParameters)
818 {
819 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * median);
820 }
821
822 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(number - modelParameters)) * median);
823}
824
825inline Scalar Estimator::determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters, const EstimatorType estimator)
826{
827 switch (estimator)
828 {
829 case ET_SQUARE:
830 case ET_LINEAR:
831 break;
832
833 case ET_HUBER:
834 return determineSigma<ET_HUBER>(errors, number, modelParameters);
835
836 case ET_TUKEY:
837 return determineSigma<ET_TUKEY>(errors, number, modelParameters);
838
839 case ET_CAUCHY:
840 return determineSigma<ET_CAUCHY>(errors, number, modelParameters);
841
842 case ET_INVALID:
843 break;
844 }
845
846 ocean_assert(false && "Invalid estimator!");
847 return Scalar(1);
848}
849
850template <Estimator::EstimatorType tEstimator>
851inline Scalar Estimator::determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters)
852{
853 ocean_assert(errors != nullptr);
854 ocean_assert(indices != nullptr);
855 ocean_assert(numberIndices > 0);
856 ocean_assert(modelParameters >= 1);
857
858 // extract the subset of the errors
859 Scalars subsetErrors(numberIndices);
860 for (size_t n = 0; n < numberIndices; ++n)
861 {
862 subsetErrors[n] = errors[indices[n]];
863 }
864
865 const Scalar median = Median::median(subsetErrors.data(), numberIndices);
866
867 if (numberIndices <= modelParameters)
868 {
869 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * median);
870 }
871
872 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(numberIndices - modelParameters)) * median);
873}
874
875inline Scalar Estimator::determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator)
876{
877 switch (estimator)
878 {
879 case ET_SQUARE:
880 case ET_LINEAR:
881 break;
882
883 case ET_HUBER:
884 return determineSigma<ET_HUBER>(errors, indices, numberIndices, modelParameters);
885
886 case ET_TUKEY:
887 return determineSigma<ET_TUKEY>(errors, indices, numberIndices, modelParameters);
888
889 case ET_CAUCHY:
890 return determineSigma<ET_CAUCHY>(errors, indices, numberIndices, modelParameters);
891
892 case ET_INVALID:
893 break;
894 }
895
896 ocean_assert(false && "Invalid estimator!");
897 return Scalar(1);
898}
899
900template <Estimator::EstimatorType tEstimator>
901inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters)
902{
903 ocean_assert(sqrErrors != nullptr);
904 ocean_assert(number > 0);
905
906 const Scalar sqrMedian = Median::constMedian(sqrErrors, number);
907
908 if (number <= modelParameters)
909 {
910 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * Numeric::sqrt(sqrMedian));
911 }
912
913 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(number - modelParameters)) * Numeric::sqrt(sqrMedian));
914}
915
916inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters, const EstimatorType estimator)
917{
918 switch (estimator)
919 {
920 case ET_SQUARE:
921 case ET_LINEAR:
922 break;
923
924 case ET_HUBER:
925 return determineSigmaSquare<ET_HUBER>(sqrErrors, number, modelParameters);
926
927 case ET_TUKEY:
928 return determineSigmaSquare<ET_TUKEY>(sqrErrors, number, modelParameters);
929
930 case ET_CAUCHY:
931 return determineSigmaSquare<ET_CAUCHY>(sqrErrors, number, modelParameters);
932
933 case ET_INVALID:
934 break;
935 }
936
937 ocean_assert(false && "Invalid estimator!");
938 return Scalar(1);
939}
940
941template <Estimator::EstimatorType tEstimator>
942inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters)
943{
944 ocean_assert(sqrErrors != nullptr);
945 ocean_assert(indices != nullptr);
946 ocean_assert(numberIndices > 0);
947
948 // extract the subset of the errors
949 Scalars subsetSqrErrors(numberIndices);
950 for (size_t n = 0; n < numberIndices; ++n)
951 {
952 subsetSqrErrors[n] = sqrErrors[indices[n]];
953 }
954
955 const Scalar sqrMedian = Median::median(subsetSqrErrors.data(), numberIndices);
956
957 if (numberIndices <= modelParameters)
958 {
959 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * Numeric::sqrt(sqrMedian));
960 }
961
962 return max(Numeric::eps(), sigmaTuningContstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(numberIndices - modelParameters)) * Numeric::sqrt(sqrMedian));
963}
964
965inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator)
966{
967 switch (estimator)
968 {
969 case ET_SQUARE:
970 case ET_LINEAR:
971 break;
972
973 case ET_HUBER:
974 return determineSigmaSquare<ET_HUBER>(sqrErrors, indices, numberIndices, modelParameters);
975
976 case ET_TUKEY:
977 return determineSigmaSquare<ET_TUKEY>(sqrErrors, indices, numberIndices, modelParameters);
978
979 case ET_CAUCHY:
980 return determineSigmaSquare<ET_CAUCHY>(sqrErrors, indices, numberIndices, modelParameters);
981
982 case ET_INVALID:
983 break;
984 }
985
986 ocean_assert(false && "Invalid estimator!");
987 return Scalar(1);
988}
989
990template <Estimator::EstimatorType tEstimator>
991inline Scalar Estimator::determineRobustError(const Scalar* sqrErrors, const size_t number, const size_t modelParameters)
992{
993 ocean_assert(sqrErrors != nullptr && number >= 1);
994 ocean_assert(modelParameters >= 1);
995
996 // determine the sigma ideal for the square errors
997 const Scalar sqrSigma = needSigma<tEstimator>() ? Numeric::sqr(determineSigmaSquare<tEstimator>(sqrErrors, number, modelParameters)) : 0;
998
999 Scalar robustError = 0;
1000
1001 for (size_t n = 0; n < number; ++n)
1002 {
1003 robustError += sqrErrors[n] * robustWeightSquare<tEstimator>(sqrErrors[n], sqrSigma);
1004 }
1005
1006 // return the averaged robust error
1007 return robustError / Scalar(number);
1008}
1009
1010constexpr Scalar Estimator::maximalWeight()
1011{
1012 return Scalar(10) / Numeric::weakEps();
1013}
1014
1015constexpr Scalar Estimator::invMaximalWeight()
1016{
1017 return Scalar(1) / maximalWeight();
1018}
1019
1020template <Estimator::EstimatorType tEstimator>
1021inline Scalar Estimator::sigmaTuningContstant()
1022{
1023 ocean_assert(false && "Invalid estimator type!");
1024 return Scalar(1);
1025}
1026
1027template <>
1028inline Scalar Estimator::sigmaTuningContstant<Estimator::ET_HUBER>()
1029{
1030 return Scalar(1.345);
1031}
1032
1033template <>
1034inline Scalar Estimator::sigmaTuningContstant<Estimator::ET_TUKEY>()
1035{
1036 return Scalar(4.6851);
1037}
1038
1039template <>
1040inline Scalar Estimator::sigmaTuningContstant<Estimator::ET_CAUCHY>()
1041{
1042 return Scalar(2.3849);
1043}
1044
1045}
1046
1047}
1048
1049#endif // META_OCEAN_GEOMETRY_ESTIMATOR_2_H
Ocean::Geometry::Estimator
This class implements robust estimator functions.
Definition Estimator.h:27
Ocean::Geometry::Estimator::robustWeight
static Scalar robustWeight(const Scalar value, const Scalar sigma=0)
Returns the weight in relation to a error for a given residual error and a specified estimator.
Definition Estimator.h:630
Ocean::Geometry::Estimator::robustWeightSquare
static Scalar robustWeightSquare(const Scalar sqrValue, const Scalar sqrSigma=0)
Returns the weight in relation to a squared error for a given residual error and a specified estimato...
Definition Estimator.h:721
Ocean::Geometry::Estimator::translateEstimatorType
static std::string translateEstimatorType(const EstimatorType estimatorType)
Translates a given estimator type into a readable string.
Ocean::Geometry::Estimator::invMaximalWeight
static constexpr Scalar invMaximalWeight()
Returns the inverse maximal weight for any estimator which is used to clamp extremely high weights (f...
Definition Estimator.h:1015
Ocean::Geometry::Estimator::determineSigmaSquare
static Scalar determineSigmaSquare(const Scalar *sqrErrors, const size_t number, const size_t modelParameters)
Determines the squared sigma for a specific set of squared residual errors and a specified estimator.
Definition Estimator.h:901
Ocean::Geometry::Estimator::determineRobustError
static Scalar determineRobustError(const Scalar *sqrErrors, const size_t number, const size_t modelParameters)
Determines the overall robust error for set of given squared errors, a specified estimator and the di...
Definition Estimator.h:991
Ocean::Geometry::Estimator::maximalWeight
static constexpr Scalar maximalWeight()
Returns the maximal weight for any estimator which is used to clamp extremely high weights (for tiny ...
Definition Estimator.h:1010
Ocean::Geometry::Estimator::translateEstimatorType
static EstimatorType translateEstimatorType(const std::string &estimatorType)
Translates a readable name of an estimator type to it's value.
Ocean::Geometry::Estimator::sigmaTuningContstant
static Scalar sigmaTuningContstant()
Returns the tuning constant allowing to determine a 95 percent efficiency on the standard normal dist...
Definition Estimator.h:1021
Ocean::Geometry::Estimator::needSigma
static constexpr bool needSigma()
Returns whether an estimator needs a standard deviation for computation.
Definition Estimator.h:380
Ocean::Geometry::Estimator::estimatorTypes
static const EstimatorTypes & estimatorTypes()
Returns all existing valid estimator types.
Ocean::Geometry::Estimator::EstimatorTypes
std::vector< EstimatorType > EstimatorTypes
Definition of a vector holding estimator types.
Definition Estimator.h:124
Ocean::Geometry::Estimator::EstimatorType
EstimatorType
Definition of individual robust estimator types.
Definition Estimator.h:34
Ocean::Geometry::Estimator::ET_HUBER
@ ET_HUBER
The Huber estimator type.
Definition Estimator.h:84
Ocean::Geometry::Estimator::ET_TUKEY
@ ET_TUKEY
The Tukey estimator.
Definition Estimator.h:102
Ocean::Geometry::Estimator::ET_SQUARE
@ ET_SQUARE
The standard square error estimator (L2).
Definition Estimator.h:52
Ocean::Geometry::Estimator::ET_LINEAR
@ ET_LINEAR
The linear estimator (L1).
Definition Estimator.h:66
Ocean::Geometry::Estimator::ET_INVALID
@ ET_INVALID
An invalid estimator type.
Definition Estimator.h:38
Ocean::Geometry::Estimator::ET_CAUCHY
@ ET_CAUCHY
The Cauchy estimator.
Definition Estimator.h:118
Ocean::Geometry::Estimator::isStandardEstimator
static constexpr bool isStandardEstimator()
Returns whether an estimator is the standard square error estimator.
Definition Estimator.h:413
Ocean::Geometry::Estimator::robustError
static Scalar robustError(const Scalar value, const Scalar sigma=0)
Returns the robust error of a residual error for a specified estimator.
Definition Estimator.h:428
Ocean::Geometry::Estimator::robustErrorSquare
static Scalar robustErrorSquare(const Scalar sqrValue, const Scalar sqrSigma=0)
Returns the robust error of a squared residual error for a specified estimator.
Definition Estimator.h:531
Ocean::Geometry::Estimator::determineSigma
static Scalar determineSigma(const Scalar *errors, const size_t number, const size_t modelParameters)
Determines the sigma for a specific set of residual errors and a specified estimator.
Definition Estimator.h:811
Ocean::Median::median
static T median(T *values, const size_t number)
Returns the median value in a given data array.
Definition Median.h:1144
Ocean::Median::constMedian
static T constMedian(const T *values, const size_t number)
Returns the median value in a given data array.
Definition Median.h:1208
Ocean::NumericT::weakEps
static constexpr T weakEps()
Returns a weak epsilon.
Ocean::NumericT::log
static T log(const T value)
Returns the natural logarithm of a given value (the logarithm to the base e).
Definition Numeric.h:1655
Ocean::NumericT::abs
static T abs(const T value)
Returns the absolute value of a given value.
Definition Numeric.h:1220
Ocean::NumericT::sqrt
static T sqrt(const T value)
Returns the square root of a given value.
Definition Numeric.h:1533
Ocean::NumericT::eps
static constexpr T eps()
Returns a small epsilon.
Ocean::NumericT::sqr
static constexpr T sqr(const T value)
Returns the square of a given value.
Definition Numeric.h:1495
Ocean::Scalar
float Scalar
Definition of a scalar type.
Definition Math.h:129
Ocean::Scalars
std::vector< Scalar > Scalars
Definition of a vector holding Scalar objects.
Definition Math.h:145
Ocean
The namespace covering the entire Ocean framework.
Definition Accessor.h:15