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

Fix generalized Geman-McClure

release/4.3a0
Enrique Fernandez 2015-08-20 15:45:34 -04:00
parent 6cdc1de268
commit 44ae7bfe01
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/linear/NoiseModel.cpp
View File

@ -807,7 +807,10 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
} }
double GemanMcClure::weight(double error) const { double GemanMcClure::weight(double error) const {
return c_/(c_ + error*error); const double c2 = c_*c_;
const double c4 = c2*c2;
const double c2error = c2 + error*error;
return c4/(c2error*c2error);
} }
void GemanMcClure::print(const std::string &s="") const { void GemanMcClure::print(const std::string &s="") const {

3
gtsam/linear/NoiseModel.h
View File

@ -827,7 +827,8 @@ namespace gtsam {
/// (Zhang97ivc). /// (Zhang97ivc).
/// ///
/// Note that Geman-McClure weight function uses the parameter c == 1.0, /// Note that Geman-McClure weight function uses the parameter c == 1.0,
/// but here it's allowed to use different values. /// but here it's allowed to use different values, so we actually have
/// the generalized Geman-McClure from (Agarwal15phd).
class GTSAM_EXPORT GemanMcClure : public Base { class GTSAM_EXPORT GemanMcClure : public Base {
public: public:
typedef boost::shared_ptr<GemanMcClure> shared_ptr; typedef boost::shared_ptr<GemanMcClure> shared_ptr;

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

@ -338,8 +338,8 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
const double weight1 = gmc->weight(error1), const double weight1 = gmc->weight(error1),
weight2 = gmc->weight(error2); weight2 = gmc->weight(error2);
DOUBLES_EQUAL(0.5 , weight1, 1e-8); DOUBLES_EQUAL(0.25 , weight1, 1e-8);
DOUBLES_EQUAL(0.00990099, weight2, 1e-8); DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
} }
TEST(NoiseModel, robustFunctionDCS) TEST(NoiseModel, robustFunctionDCS)
@ -385,8 +385,12 @@ TEST(NoiseModel, robustNoiseGemanMcClure)
robust->WhitenSystem(A, b); robust->WhitenSystem(A, b);
const double sqrt_weight_error1 = sqrt(0.5); const double k2 = k*k;
const double sqrt_weight_error2 = sqrt(k/(k + error2*error2)); const double k4 = k2*k2;
const double k2error = k2 + error2*error2;
const double sqrt_weight_error1 = sqrt(0.25);
const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error));
DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8); DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);