Merge pull request #1161 from borglab/fix/robust_loss_factor

FIX: Robust loss error calculation
release/4.3a0
Fan Jiang 2022-06-30 21:44:17 -07:00 committed by GitHub
commit caf659f68c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 62 additions and 2 deletions

View File

@ -697,6 +697,12 @@ namespace gtsam {
return robust_->loss(std::sqrt(squared_distance));
}
// NOTE: This is special because in whiten the base version will do the reweighting
// which is incorrect!
double squaredMahalanobisDistance(const Vector& v) const override {
return noise_->squaredMahalanobisDistance(v);
}
// These are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;

View File

@ -661,12 +661,13 @@ TEST(NoiseModel, robustNoiseDCS)
TEST(NoiseModel, robustNoiseL2WithDeadZone)
{
double dead_zone_size = 1.0;
SharedNoiseModel robust = noiseModel::Robust::Create(
auto robust = noiseModel::Robust::Create(
noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
Unit::Create(3));
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,
robust->squaredMahalanobisDistance(error), 1e-8);
}

53
tests/testRobust.cpp Normal file
View File

@ -0,0 +1,53 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testRobust.cpp
* @brief Unit tests for Robust loss functions
* @author Fan Jiang
* @author Yetong Zhang
* @date Apr 7, 2022
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace gtsam;
TEST(RobustNoise, loss) {
// Keys.
gtsam::Key x1_key = 1;
gtsam::Key x2_key = 2;
auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0);
auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1));
auto factor = PriorFactor<double>(x1_key, 0.0, noise);
auto between_factor = BetweenFactor<double>(x1_key, x2_key, 0.0, noise);
Values values;
values.insert(x1_key, 10.0);
values.insert(x2_key, 0.0);
EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5);
EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5);
EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5);
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}