1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
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);
}
|
