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 The residual error to return the robust error for, with range (-infinity, infinity)
161 * @param sigma The standard deviation of the expected residual error, with range (0, infinity) if 'needSigma<tEstimator>() == true', 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 The residual error to return the robust error for, with range (-infinity, infinity)
172 * @param sigma The standard deviation of the expected residual error, with range (0, infinity) if 'needSigma(estimator) == true', otherwise 0
173 * @param estimator The 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 The squared residual error to return the robust error for, with range [0, infinity)
182 * @param sqrSigma The squared standard deviation (the variance) of the expected residual error, with range (0, infinity) if 'needSigma<tEstimator>() == true', 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 The squared residual error to return the robust error for, with range [0, infinity)
193 * @param sqrSigma The squared standard deviation (the variance) of the expected residual error, with range (0, infinity) if 'needSigma(estimator) == true', 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 The residual error to return the weight for
203 * @param sigma The 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 The residual error to return the weight for
213 * @param sigma The 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 The squared residual error to return the weight for
222 * @param sqrSigma The squared standard deviation (the variance) 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 The squared residual error to return the weight for
232 * @param sqrSigma The squared standard deviation (the variance) 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 sigmaTuningConstant();
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(Numeric::ratio(Scalar(1), absValue, maximalWeight()), 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(Numeric::ratio(sigma, absValue, maximalWeight()), 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 Numeric::ratio(Scalar(1), (Scalar(1) + Numeric::sqr(value / sigma)), maximalWeight());
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 return std::min(Numeric::ratio(Scalar(1), Numeric::sqrt(sqrValue), maximalWeight()), maximalWeight());
742}
743
744template <>
745inline Scalar Estimator::robustWeightSquare<Estimator::ET_HUBER>(const Scalar sqrValue, const Scalar sqrSigma)
746{
747 ocean_assert(sqrValue >= 0 && sqrSigma > 0);
748
749 if (sqrValue <= sqrSigma)
750 {
751 return Scalar(1);
752 }
753
754 return std::min(Numeric::sqrt(Numeric::ratio(sqrSigma, sqrValue, maximalWeight() * maximalWeight())), maximalWeight());
755}
756
757template <>
758inline Scalar Estimator::robustWeightSquare<Estimator::ET_TUKEY>(const Scalar sqrValue, const Scalar sqrSigma)
759{
760 ocean_assert(sqrSigma > 0);
761
762 if (sqrValue > sqrSigma)
763 {
764 return Scalar(0);
765 }
766
767 return std::min(Numeric::sqr(Scalar(1) - sqrValue / sqrSigma), maximalWeight());
768}
769
770template <>
771inline Scalar Estimator::robustWeightSquare<Estimator::ET_CAUCHY>(const Scalar sqrValue, const Scalar sqrSigma)
772{
773 ocean_assert(sqrSigma > 0);
774
775 return Numeric::ratio(Scalar(1), Scalar(1) + sqrValue / sqrSigma, maximalWeight());
776}
777
778inline Scalar Estimator::robustWeightSquare(const Scalar sqrValue, const Scalar sqrSigma, const EstimatorType estimator)
779{
780 switch (estimator)
781 {
782 case ET_SQUARE:
783 return robustWeightSquare<ET_SQUARE>(sqrValue);
784
785 case ET_LINEAR:
786 return robustWeightSquare<ET_LINEAR>(sqrValue);
787
788 case ET_HUBER:
789 return robustWeightSquare<ET_HUBER>(sqrValue, sqrSigma);
790
791 case ET_TUKEY:
792 return robustWeightSquare<ET_TUKEY>(sqrValue, sqrSigma);
793
794 case ET_CAUCHY:
795 return robustWeightSquare<ET_CAUCHY>(sqrValue, sqrSigma);
796
797 case ET_INVALID:
798 break;
799 }
800
801 ocean_assert(false && "Invalid estimator!");
802 return robustWeightSquare<ET_SQUARE>(sqrValue);
803}
804
805template <Estimator::EstimatorType tEstimator>
806inline Scalar Estimator::determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters)
807{
808 ocean_assert(number > 0);
809
810 const Scalar median = Median::constMedian(errors, number);
811
812 if (number <= modelParameters)
813 {
814 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * median);
815 }
816
817 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(number - modelParameters)) * median);
818}
819
820inline Scalar Estimator::determineSigma(const Scalar* errors, const size_t number, const size_t modelParameters, const EstimatorType estimator)
821{
822 switch (estimator)
823 {
824 case ET_SQUARE:
825 case ET_LINEAR:
826 break;
827
828 case ET_HUBER:
829 return determineSigma<ET_HUBER>(errors, number, modelParameters);
830
831 case ET_TUKEY:
832 return determineSigma<ET_TUKEY>(errors, number, modelParameters);
833
834 case ET_CAUCHY:
835 return determineSigma<ET_CAUCHY>(errors, number, modelParameters);
836
837 case ET_INVALID:
838 break;
839 }
840
841 ocean_assert(false && "Invalid estimator!");
842 return Scalar(1);
843}
844
845template <Estimator::EstimatorType tEstimator>
846inline Scalar Estimator::determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters)
847{
848 ocean_assert(errors != nullptr);
849 ocean_assert(indices != nullptr);
850 ocean_assert(numberIndices > 0);
851 ocean_assert(modelParameters >= 1);
852
853 // extract the subset of the errors
854 Scalars subsetErrors(numberIndices);
855 for (size_t n = 0; n < numberIndices; ++n)
856 {
857 subsetErrors[n] = errors[indices[n]];
858 }
859
860 const Scalar median = Median::median(subsetErrors.data(), numberIndices);
861
862 if (numberIndices <= modelParameters)
863 {
864 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * median);
865 }
866
867 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(numberIndices - modelParameters)) * median);
868}
869
870inline Scalar Estimator::determineSigma(const Scalar* errors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator)
871{
872 switch (estimator)
873 {
874 case ET_SQUARE:
875 case ET_LINEAR:
876 break;
877
878 case ET_HUBER:
879 return determineSigma<ET_HUBER>(errors, indices, numberIndices, modelParameters);
880
881 case ET_TUKEY:
882 return determineSigma<ET_TUKEY>(errors, indices, numberIndices, modelParameters);
883
884 case ET_CAUCHY:
885 return determineSigma<ET_CAUCHY>(errors, indices, numberIndices, modelParameters);
886
887 case ET_INVALID:
888 break;
889 }
890
891 ocean_assert(false && "Invalid estimator!");
892 return Scalar(1);
893}
894
895template <Estimator::EstimatorType tEstimator>
896inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters)
897{
898 ocean_assert(sqrErrors != nullptr);
899 ocean_assert(number > 0);
900
901 const Scalar sqrMedian = Median::constMedian(sqrErrors, number);
902
903 if (number <= modelParameters)
904 {
905 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * Numeric::sqrt(sqrMedian));
906 }
907
908 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(number - modelParameters)) * Numeric::sqrt(sqrMedian));
909}
910
911inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const size_t number, const size_t modelParameters, const EstimatorType estimator)
912{
913 switch (estimator)
914 {
915 case ET_SQUARE:
916 case ET_LINEAR:
917 break;
918
919 case ET_HUBER:
920 return determineSigmaSquare<ET_HUBER>(sqrErrors, number, modelParameters);
921
922 case ET_TUKEY:
923 return determineSigmaSquare<ET_TUKEY>(sqrErrors, number, modelParameters);
924
925 case ET_CAUCHY:
926 return determineSigmaSquare<ET_CAUCHY>(sqrErrors, number, modelParameters);
927
928 case ET_INVALID:
929 break;
930 }
931
932 ocean_assert(false && "Invalid estimator!");
933 return Scalar(1);
934}
935
936template <Estimator::EstimatorType tEstimator>
937inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters)
938{
939 ocean_assert(sqrErrors != nullptr);
940 ocean_assert(indices != nullptr);
941 ocean_assert(numberIndices > 0);
942
943 // extract the subset of the errors
944 Scalars subsetSqrErrors(numberIndices);
945 for (size_t n = 0; n < numberIndices; ++n)
946 {
947 subsetSqrErrors[n] = sqrErrors[indices[n]];
948 }
949
950 const Scalar sqrMedian = Median::median(subsetSqrErrors.data(), numberIndices);
951
952 if (numberIndices <= modelParameters)
953 {
954 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * Numeric::sqrt(sqrMedian));
955 }
956
957 return max(Numeric::eps(), sigmaTuningConstant<tEstimator>() * Scalar(1.4826) * (1 + Scalar(5) / Scalar(numberIndices - modelParameters)) * Numeric::sqrt(sqrMedian));
958}
959
960inline Scalar Estimator::determineSigmaSquare(const Scalar* sqrErrors, const unsigned int* indices, const size_t numberIndices, const size_t modelParameters, const EstimatorType estimator)
961{
962 switch (estimator)
963 {
964 case ET_SQUARE:
965 case ET_LINEAR:
966 break;
967
968 case ET_HUBER:
969 return determineSigmaSquare<ET_HUBER>(sqrErrors, indices, numberIndices, modelParameters);
970
971 case ET_TUKEY:
972 return determineSigmaSquare<ET_TUKEY>(sqrErrors, indices, numberIndices, modelParameters);
973
974 case ET_CAUCHY:
975 return determineSigmaSquare<ET_CAUCHY>(sqrErrors, indices, numberIndices, modelParameters);
976
977 case ET_INVALID:
978 break;
979 }
980
981 ocean_assert(false && "Invalid estimator!");
982 return Scalar(1);
983}
984
985template <Estimator::EstimatorType tEstimator>
986inline Scalar Estimator::determineRobustError(const Scalar* sqrErrors, const size_t number, const size_t modelParameters)
987{
988 ocean_assert(sqrErrors != nullptr && number >= 1);
989 ocean_assert(modelParameters >= 1);
990
991 // determine the sigma ideal for the square errors
992 const Scalar sqrSigma = needSigma<tEstimator>() ? Numeric::sqr(determineSigmaSquare<tEstimator>(sqrErrors, number, modelParameters)) : 0;
993
994 Scalar robustError = 0;
995
996 for (size_t n = 0; n < number; ++n)
997 {
998 robustError += sqrErrors[n] * robustWeightSquare<tEstimator>(sqrErrors[n], sqrSigma);
999 }
1000
1001 // return the averaged robust error
1002 return robustError / Scalar(number);
1003}
1004
1005constexpr Scalar Estimator::maximalWeight()
1006{
1007 return Scalar(10) / Numeric::weakEps();
1008}
1009
1010constexpr Scalar Estimator::invMaximalWeight()
1011{
1012 return Scalar(1) / maximalWeight();
1013}
1014
1015template <Estimator::EstimatorType tEstimator>
1016inline Scalar Estimator::sigmaTuningConstant()
1017{
1018 ocean_assert(false && "Invalid estimator type!");
1019 return Scalar(1);
1020}
1021
1022template <>
1023inline Scalar Estimator::sigmaTuningConstant<Estimator::ET_HUBER>()
1024{
1025 return Scalar(1.345);
1026}
1027
1028template <>
1029inline Scalar Estimator::sigmaTuningConstant<Estimator::ET_TUKEY>()
1030{
1031 return Scalar(4.6851);
1032}
1033
1034template <>
1035inline Scalar Estimator::sigmaTuningConstant<Estimator::ET_CAUCHY>()
1036{
1037 return Scalar(2.3849);
1038}
1039
1040}
1041
1042}
1043
1044#endif // META_OCEAN_GEOMETRY_ESTIMATOR_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:1010
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:896
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:986
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:1005
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::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::sigmaTuningConstant
static Scalar sigmaTuningConstant()
Returns the tuning constant allowing to determine a 95 percent efficiency on the standard normal dist...
Definition Estimator.h:1016
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:806
Ocean::Median::median
static T median(T *values, const size_t number)
Returns the median value in a given data array.
Definition Median.h:1175
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:1266
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:1659
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:1537
Ocean::NumericT::eps
static constexpr T eps()
Returns a small epsilon.
Ocean::NumericT::ratio
static constexpr T ratio(const T nominator, const T denominator, const T fallback=T(1))
Returns the ratio between two values if the denominator is not equal a small epsilon.
Definition Numeric.h:2085
Ocean::NumericT::sqr
static constexpr T sqr(const T value)
Returns the square of a given value.
Definition Numeric.h:1499
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