Merge pull request #1161 from borglab/fix/robust_loss_factor
FIX: Robust loss error calculationrelease/4.3a0
commit
caf659f68c
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
Loading…
Reference in New Issue