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 testBoundingConstraint.cpp
14 * @brief test of nonlinear inequality constraints on scalar bounds
15 * @author Alex Cunningham
16 */
17
18#include <tests/simulated2DConstraints.h>
19#include <gtsam/inference/Symbol.h>
20#include <gtsam/nonlinear/NonlinearFactorGraph.h>
21#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22
23#include <CppUnitLite/TestHarness.h>
24
25namespace iq2D = simulated2D::inequality_constraints;
26using namespace std;
27using namespace gtsam;
28
29static const double tol = 1e-5;
30
31SharedDiagonal soft_model2 = noiseModel::Unit::Create(dim: 2);
32SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
33SharedDiagonal hard_model1 = noiseModel::Constrained::All(dim: 1);
34
35// some simple inequality constraints
36gtsam::Key key = 1;
37double mu = 10.0;
38// greater than
39iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
40iq2D::PoseYInequality constraint2(key, 2.0, true, mu);
41
42// less than
43iq2D::PoseXInequality constraint3(key, 1.0, false, mu);
44iq2D::PoseYInequality constraint4(key, 2.0, false, mu);
45
46/* ************************************************************************* */
47TEST( testBoundingConstraint, unary_basics_inactive1 ) {
48 Point2 pt1(2.0, 3.0);
49 Values config;
50 config.insert(j: key, val: pt1);
51 EXPECT(!constraint1.active(config));
52 EXPECT(!constraint2.active(config));
53 EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol);
54 EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
55 EXPECT(constraint1.isGreaterThan());
56 EXPECT(constraint2.isGreaterThan());
57 EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol));
58 EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol));
59 EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol));
60 EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol));
61 EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
62 EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
63}
64
65/* ************************************************************************* */
66TEST( testBoundingConstraint, unary_basics_inactive2 ) {
67 Point2 pt2(-2.0, -3.0);
68 Values config;
69 config.insert(j: key, val: pt2);
70 EXPECT(!constraint3.active(config));
71 EXPECT(!constraint4.active(config));
72 EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol);
73 EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol);
74 EXPECT(!constraint3.isGreaterThan());
75 EXPECT(!constraint4.isGreaterThan());
76 EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol));
77 EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol));
78 EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol));
79 EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol));
80 EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
81 EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol);
82}
83
84/* ************************************************************************* */
85TEST( testBoundingConstraint, unary_basics_active1 ) {
86 Point2 pt2(-2.0, -3.0);
87 Values config;
88 config.insert(j: key, val: pt2);
89 EXPECT(constraint1.active(config));
90 EXPECT(constraint2.active(config));
91 EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
92 EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
93 EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol));
94 EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol));
95 EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
96 EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
97}
98
99/* ************************************************************************* */
100TEST( testBoundingConstraint, unary_basics_active2 ) {
101 Point2 pt1(2.0, 3.0);
102 Values config;
103 config.insert(j: key, val: pt1);
104 EXPECT(constraint3.active(config));
105 EXPECT(constraint4.active(config));
106 // EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
107 // EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
108 EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
109 EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
110 EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
111 EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
112}
113
114/* ************************************************************************* */
115TEST( testBoundingConstraint, unary_linearization_inactive) {
116 Point2 pt1(2.0, 3.0);
117 Values config1;
118 config1.insert(j: key, val: pt1);
119 GaussianFactor::shared_ptr actual1 = constraint1.linearize(x: config1);
120 GaussianFactor::shared_ptr actual2 = constraint2.linearize(x: config1);
121 EXPECT(!actual1);
122 EXPECT(!actual2);
123}
124
125/* ************************************************************************* */
126TEST( testBoundingConstraint, unary_linearization_active) {
127 Point2 pt2(-2.0, -3.0);
128 Values config2;
129 config2.insert(j: key, val: pt2);
130 GaussianFactor::shared_ptr actual1 = constraint1.linearize(x: config2);
131 GaussianFactor::shared_ptr actual2 = constraint2.linearize(x: config2);
132 JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(size: 1, value: 3.0), hard_model1);
133 JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(size: 1, value: 5.0), hard_model1);
134 EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
135 EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
136}
137
138/* ************************************************************************* */
139TEST( testBoundingConstraint, unary_simple_optimization1) {
140 // create a single-node graph with a soft and hard constraint to
141 // ensure that the hard constraint overrides the soft constraint
142 Point2 goal_pt(1.0, 2.0);
143 Point2 start_pt(0.0, 1.0);
144
145 NonlinearFactorGraph graph;
146 Symbol x1('x',1);
147 graph.emplace_shared<iq2D::PoseXInequality>(args&: x1, args: 1.0, args: true);
148 graph.emplace_shared<iq2D::PoseYInequality>(args&: x1, args: 2.0, args: true);
149 graph.emplace_shared<simulated2D::Prior>(args&: start_pt, args&: soft_model2, args&: x1);
150
151 Values initValues;
152 initValues.insert(j: x1, val: start_pt);
153
154 Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
155 Values expected;
156 expected.insert(j: x1, val: goal_pt);
157 CHECK(assert_equal(expected, actual, tol));
158}
159
160/* ************************************************************************* */
161TEST( testBoundingConstraint, unary_simple_optimization2) {
162 // create a single-node graph with a soft and hard constraint to
163 // ensure that the hard constraint overrides the soft constraint
164 Point2 goal_pt(1.0, 2.0);
165 Point2 start_pt(2.0, 3.0);
166
167 NonlinearFactorGraph graph;
168 graph.emplace_shared<iq2D::PoseXInequality>(args&: key, args: 1.0, args: false);
169 graph.emplace_shared<iq2D::PoseYInequality>(args&: key, args: 2.0, args: false);
170 graph.emplace_shared<simulated2D::Prior>(args&: start_pt, args&: soft_model2, args&: key);
171
172 Values initValues;
173 initValues.insert(j: key, val: start_pt);
174
175 Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
176 Values expected;
177 expected.insert(j: key, val: goal_pt);
178 CHECK(assert_equal(expected, actual, tol));
179}
180
181/* ************************************************************************* */
182TEST( testBoundingConstraint, MaxDistance_basics) {
183 gtsam::Key key1 = 1, key2 = 2;
184 Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
185 iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
186 EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
187 EXPECT(!rangeBound.isGreaterThan());
188 EXPECT(rangeBound.dim() == 1);
189
190 EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1)));
191 EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2)));
192 EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3)));
193 EXPECT(assert_equal(-1.0*I_1x1, rangeBound.evaluateError(pt1, pt4)));
194
195 Values config1;
196 config1.insert(j: key1, val: pt1);
197 config1.insert(j: key2, val: pt1);
198 EXPECT(!rangeBound.active(config1));
199 EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
200 EXPECT(!rangeBound.linearize(config1));
201 EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
202
203 config1.update(j: key2, val: pt2);
204 EXPECT(!rangeBound.active(config1));
205 EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
206 EXPECT(!rangeBound.linearize(config1));
207 EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
208
209 config1.update(j: key2, val: pt3);
210 EXPECT(rangeBound.active(config1));
211 EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
212 EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
213
214 config1.update(j: key2, val: pt4);
215 EXPECT(rangeBound.active(config1));
216 EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1)));
217 EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
218}
219
220/* ************************************************************************* */
221TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
222
223 Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
224 Symbol x1('x',1), x2('x',2);
225
226 NonlinearFactorGraph graph;
227 graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(args&: pt1, args&: x1);
228 graph.emplace_shared<simulated2D::Prior>(args&: pt2_init, args&: soft_model2_alt, args&: x2);
229 graph.emplace_shared<iq2D::PoseMaxDistConstraint>(args&: x1, args&: x2, args: 2.0);
230
231 Values initial_state;
232 initial_state.insert(j: x1, val: pt1);
233 initial_state.insert(j: x2, val: pt2_init);
234
235 Values expected;
236 expected.insert(j: x1, val: pt1);
237 expected.insert(j: x2, val: pt2_goal);
238
239 // FAILS: VectorValues assertion failure
240// Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state);
241// EXPECT(assert_equal(expected, *actual, tol));
242}
243
244/* ************************************************************************* */
245TEST( testBoundingConstraint, avoid_demo) {
246
247 Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
248 double radius = 1.0;
249 Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
250 Point2 odo(2.0, 0.0);
251
252 NonlinearFactorGraph graph;
253 graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(args&: x1_pt, args&: x1);
254 graph.emplace_shared<simulated2D::Odometry>(args&: odo, args&: soft_model2_alt, args&: x1, args&: x2);
255 graph.emplace_shared<iq2D::LandmarkAvoid>(args&: x2, args&: l1, args&: radius);
256 graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityPointConstraint>(args&: l1_pt, args&: l1);
257 graph.emplace_shared<simulated2D::Odometry>(args&: odo, args&: soft_model2_alt, args&: x2, args&: x3);
258 graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(args&: x3_pt, args&: x3);
259
260 Values init, expected;
261 init.insert(j: x1, val: x1_pt);
262 init.insert(j: x3, val: x3_pt);
263 init.insert(j: l1, val: l1_pt);
264 expected = init;
265 init.insert(j: x2, val: x2_init);
266 expected.insert(j: x2, val: x2_goal);
267
268 // FAILS: segfaults on optimization
269// Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
270// EXPECT(assert_equal(expected, *actual, tol));
271}
272
273/* ************************************************************************* */
274int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
275/* ************************************************************************* */
276
277