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

Fix and optimize DCS

release/4.3a0
Enrique Fernandez 2015-08-20 15:46:03 -04:00
parent 44ae7bfe01
commit b9c63ef5df
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/linear/NoiseModel.cpp
View File

@ -835,8 +835,14 @@ DCS::DCS(double c, const ReweightScheme reweight)
} }
double DCS::weight(double error) const { double DCS::weight(double error) const {
double scale = 2.0*c_/(c_ + error*error); const double e2 = error*error;
return std::min(scale, 1.0); if (e2 > c_)
{
const double w = 2.0*c_/(c_ + e2);
return w*w;
}
return 1.0;
} }
void DCS::print(const std::string &s="") const { void DCS::print(const std::string &s="") const {

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

@ -349,7 +349,7 @@ TEST(NoiseModel, robustFunctionDCS)
const double weight1 = dcs->weight(error1), const double weight1 = dcs->weight(error1),
weight2 = dcs->weight(error2); weight2 = dcs->weight(error2);
DOUBLES_EQUAL(1.0 , weight1, 1e-8); DOUBLES_EQUAL(1.0 , weight1, 1e-8);
DOUBLES_EQUAL(0.01980198, weight2, 1e-8); DOUBLES_EQUAL(0.00039211, weight2, 1e-8);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -413,7 +413,7 @@ TEST(NoiseModel, robustNoiseDCS)
robust->WhitenSystem(A, b); robust->WhitenSystem(A, b);
const double sqrt_weight = sqrt(2.0*k/(k + error2*error2)); const double sqrt_weight = 2.0*k/(k + error2*error2);
DOUBLES_EQUAL(error1, b(0), 1e-8); DOUBLES_EQUAL(error1, b(0), 1e-8);
DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8); DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);