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 testSmartRangeFactor.cpp
14 * @brief Unit tests for SmartRangeFactor Class
15 * @author Frank Dellaert
16 * @date Nov 2013
17 */
18
19#include <gtsam_unstable/slam/SmartRangeFactor.h>
20#include <gtsam/nonlinear/NonlinearFactorGraph.h>
21#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22#include <CppUnitLite/TestHarness.h>
23
24using namespace std;
25using namespace gtsam;
26
27static const double sigma = 2.0;
28
29// Test situation:
30static const Point2 p(0, 10);
31static const Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
32static const double r1 = pose1.range(point: p), r2 = pose2.range(point: p), r3 = pose3.range(
33 point: p);
34
35/* ************************************************************************* */
36
37TEST( SmartRangeFactor, constructor ) {
38 SmartRangeFactor f1;
39 LONGS_EQUAL(0, f1.size())
40 SmartRangeFactor f2(sigma);
41 LONGS_EQUAL(0, f2.size())
42}
43/* ************************************************************************* */
44
45TEST( SmartRangeFactor, addRange ) {
46 SmartRangeFactor f(sigma);
47 f.addRange(key: 1, measuredRange: 10);
48 f.addRange(key: 2, measuredRange: 12);
49 LONGS_EQUAL(2, f.size())
50}
51/* ************************************************************************* */
52
53TEST( SmartRangeFactor, scenario ) {
54 DOUBLES_EQUAL(10, r1, 1e-9);
55 DOUBLES_EQUAL(sqrt(100.0+25.0), r2, 1e-9);
56 DOUBLES_EQUAL(sqrt(50.0), r3, 1e-9);
57}
58/* ************************************************************************* */
59
60TEST( SmartRangeFactor, unwhitenedError ) {
61 Values values; // all correct
62 values.insert(j: 1, val: pose1);
63 values.insert(j: 2, val: pose2);
64 values.insert(j: 3, val: pose3);
65
66 SmartRangeFactor f(sigma);
67 f.addRange(key: 1, measuredRange: r1);
68
69 // Check Jacobian for n==1
70 vector<Matrix> H1(1);
71 f.unwhitenedError(x: values, H&: H1); // with H now !
72 CHECK(assert_equal(Matrix::Zero(3,1), H1.front()));
73
74 // Whenever there are two ranges or less, error should be zero
75 Vector actual1 = f.unwhitenedError(x: values);
76 EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual1));
77 f.addRange(key: 2, measuredRange: r2);
78 Vector actual2 = f.unwhitenedError(x: values);
79 EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual2));
80
81 f.addRange(key: 3, measuredRange: r3);
82 vector<Matrix> H(3);
83 Vector actual3 = f.unwhitenedError(x: values);
84 EXPECT_LONGS_EQUAL(3, f.keys().size());
85 EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual3));
86
87 // Check keys and Jacobian
88 Vector actual4 = f.unwhitenedError(x: values, H); // with H now !
89 EXPECT(assert_equal((Vector(1) << 0.0).finished(), actual4));
90 CHECK(assert_equal((Matrix(1, 3) << 0.0,-1.0,0.0).finished(), H.front()));
91 CHECK(assert_equal((Matrix(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0).finished(), H.back()));
92
93 // Test clone
94 NonlinearFactor::shared_ptr clone = f.clone();
95 EXPECT_LONGS_EQUAL(3, clone->keys().size());
96}
97/* ************************************************************************* */
98
99TEST( SmartRangeFactor, optimization ) {
100 SmartRangeFactor f(sigma);
101 f.addRange(key: 1, measuredRange: r1);
102 f.addRange(key: 2, measuredRange: r2);
103 f.addRange(key: 3, measuredRange: r3);
104
105 // Create initial value for optimization
106 Values initial;
107 initial.insert(j: 1, val: pose1);
108 initial.insert(j: 2, val: pose2);
109 initial.insert(j: 3, val: Pose2(5, 6, 0)); // does not satisfy range measurement
110 Vector actual5 = f.unwhitenedError(x: initial);
111 EXPECT(assert_equal((Vector(1) << sqrt(25.0+16.0)-sqrt(50.0)).finished(), actual5));
112
113 // Create Factor graph
114 NonlinearFactorGraph graph;
115 graph.push_back(factor: f);
116 const noiseModel::Base::shared_ptr //
117 priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(1, 1, M_PI));
118 graph.addPrior(key: 1, prior: pose1, model: priorNoise);
119 graph.addPrior(key: 2, prior: pose2, model: priorNoise);
120
121 // Try optimizing
122 LevenbergMarquardtParams params;
123 // params.setVerbosity("ERROR");
124 LevenbergMarquardtOptimizer optimizer(graph, initial, params);
125 Values result = optimizer.optimize();
126 EXPECT(assert_equal(initial.at<Pose2>(1), result.at<Pose2>(1)));
127 EXPECT(assert_equal(initial.at<Pose2>(2), result.at<Pose2>(2)));
128 // only the third pose will be changed, converges on following:
129 EXPECT(assert_equal(Pose2(5.52159, 5.582727, 0), result.at<Pose2>(3),1e-5));
130}
131
132/* ************************************************************************* */
133int main() {
134 TestResult tr;
135 return TestRegistry::runAllTests(result&: tr);
136}
137/* ************************************************************************* */
138
139