12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

improved code for Huber robust noise model

release/4.3a0
Varun Agrawal 2019-09-18 20:01:24 -04:00
parent ba22688ca0
commit 813f169d4a
2 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/NoiseModel.h
View File

@ -776,7 +776,8 @@ namespace gtsam {
Huber(double k = 1.345, const ReweightScheme reweight = Block); Huber(double k = 1.345, const ReweightScheme reweight = Block);
double weight(double error) const { double weight(double error) const {
return (std::abs(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; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;

12
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -461,15 +461,11 @@ TEST(NoiseModel, robustFunctionHuber)
{ {
const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.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 mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
const double weight1 = huber->weight(error1), DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8);
weight2 = huber->weight(error2), DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8);
weight3 = huber->weight(error3),
weight4 = huber->weight(error4);
DOUBLES_EQUAL(1.0, weight1, 1e-8);
DOUBLES_EQUAL(0.5, weight2, 1e-8);
// Test negative value to ensure we take absolute value of error. // Test negative value to ensure we take absolute value of error.
DOUBLES_EQUAL(0.5, weight3, 1e-8); DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
DOUBLES_EQUAL(1.0, weight4, 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionGemanMcClure) TEST(NoiseModel, robustFunctionGemanMcClure)