54 lines
1.5 KiB
C++
54 lines
1.5 KiB
C++
|
/* ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
* 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);
|
||
|
|
}
|