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

fix testNoiseModel

release/4.3a0
yetongumich 2022-06-30 23:38:34 -04:00
parent 7a47815e0b
commit 0d501b6de1
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -661,14 +661,15 @@ TEST(NoiseModel, robustNoiseDCS)
TEST(NoiseModel, robustNoiseL2WithDeadZone) TEST(NoiseModel, robustNoiseL2WithDeadZone)
{ {
double dead_zone_size = 1.0; double dead_zone_size = 1.0;
SharedNoiseModel robust = noiseModel::Robust::Create( auto robust = noiseModel::Robust::Create(
noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
Unit::Create(3)); Unit::Create(3));
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
Vector3 error = Vector3(i, 0, 0); Vector error = Vector3(i, 0, 0);
robust->WhitenSystem(error);
DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8); robust->squaredMahalanobisDistance(error), 1e-8);
} }
} }