commit
19315cc3f3
|
@ -629,7 +629,7 @@ namespace gtsam {
|
|||
/**
|
||||
* The mEstimator name space contains all robust error functions.
|
||||
* It mirrors the exposition at
|
||||
* http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
|
||||
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
|
||||
* which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice.
|
||||
*
|
||||
* To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
|
||||
|
@ -681,7 +681,7 @@ namespace gtsam {
|
|||
/*
|
||||
* This method is responsible for returning the weight function for a given amount of error.
|
||||
* The weight function is related to the analytic derivative of the residual function. See
|
||||
* http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
|
||||
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
|
||||
* for details. This method is required when optimizing cost functions with robust penalties
|
||||
* using iteratively re-weighted least squares.
|
||||
*/
|
||||
|
@ -776,7 +776,8 @@ namespace gtsam {
|
|||
|
||||
Huber(double k = 1.345, const ReweightScheme reweight = Block);
|
||||
double weight(double error) const {
|
||||
return (error < k_) ? (1.0) : (k_ / fabs(error));
|
||||
double absError = std::abs(error);
|
||||
return (absError < k_) ? (1.0) : (k_ / absError);
|
||||
}
|
||||
void print(const std::string &s) const;
|
||||
bool equals(const Base& expected, double tol=1e-8) const;
|
||||
|
|
|
@ -452,19 +452,20 @@ TEST(NoiseModel, WhitenInPlace)
|
|||
/*
|
||||
* These tests are responsible for testing the weight functions for the m-estimators in GTSAM.
|
||||
* The weight function is related to the analytic derivative of the residual function. See
|
||||
* http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
|
||||
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
|
||||
* for details. This weight function is required when optimizing cost functions with robust
|
||||
* penalties using iteratively re-weighted least squares.
|
||||
*/
|
||||
|
||||
TEST(NoiseModel, robustFunctionHuber)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0;
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
|
||||
const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
|
||||
const double weight1 = huber->weight(error1),
|
||||
weight2 = huber->weight(error2);
|
||||
DOUBLES_EQUAL(1.0, weight1, 1e-8);
|
||||
DOUBLES_EQUAL(0.5, weight2, 1e-8);
|
||||
DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8);
|
||||
DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8);
|
||||
// Test negative value to ensure we take absolute value of error.
|
||||
DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
|
||||
DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionGemanMcClure)
|
||||
|
|
Loading…
Reference in New Issue