1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testRobust.cpp
14 * @brief Unit tests for Robust loss functions
15 * @author Fan Jiang
16 * @author Yetong Zhang
17 * @date Apr 7, 2022
18 **/
19
20#include <CppUnitLite/TestHarness.h>
21#include <gtsam/base/Testable.h>
22#include <gtsam/base/TestableAssertions.h>
23#include <gtsam/base/numericalDerivative.h>
24#include <gtsam/nonlinear/PriorFactor.h>
25#include <gtsam/slam/BetweenFactor.h>
26
27using namespace gtsam;
28
29TEST(RobustNoise, loss) {
30 // Keys.
31 gtsam::Key x1_key = 1;
32 gtsam::Key x2_key = 2;
33
34 auto gm = noiseModel::mEstimator::GemanMcClure::Create(k: 1.0);
35 auto noise = noiseModel::Robust::Create(robust: gm, noise: noiseModel::Unit::Create(dim: 1));
36
37 auto factor = PriorFactor<double>(x1_key, 0.0, noise);
38 auto between_factor = BetweenFactor<double>(x1_key, x2_key, 0.0, noise);
39
40 Values values;
41 values.insert(j: x1_key, val: 10.0);
42 values.insert(j: x2_key, val: 0.0);
43
44 EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5);
45 EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5);
46 EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5);
47}
48
49int main() {
50 TestResult tr;
51
52 return TestRegistry::runAllTests(result&: tr);
53}
54