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 testLinearEquality.cpp
14 * @brief Unit tests for LinearEquality
15 * @author Duy-Nguyen Ta
16 **/
17
18#include <CppUnitLite/TestHarness.h>
19#include <gtsam/base/TestableAssertions.h>
20#include <gtsam/linear/HessianFactor.h>
21#include <gtsam/linear/VectorValues.h>
22#include <gtsam_unstable/linear/LinearEquality.h>
23
24using namespace std;
25using namespace gtsam;
26
27GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
28
29namespace {
30namespace simple {
31// Terms we'll use
32using Terms = vector<pair<Key, Matrix> >;
33const Terms terms{make_pair(x: 5, y: I_3x3), make_pair(x: 10, y: 2 * I_3x3),
34 make_pair(x: 15, y: 3 * I_3x3)};
35
36// RHS and sigmas
37const Vector b = (Vector(3) << 1., 2., 3.).finished();
38const SharedDiagonal noise = noiseModel::Constrained::All(dim: 3);
39} // namespace simple
40} // namespace
41
42/* ************************************************************************* */
43TEST(LinearEquality, constructors_and_accessors) {
44 using namespace simple;
45
46 // Test for using different numbers of terms
47 {
48 // One term constructor
49 LinearEquality expected(Terms(terms.begin(), terms.begin() + 1), b, 0);
50 LinearEquality actual(terms[0].first, terms[0].second, b, 0);
51 EXPECT(assert_equal(expected, actual));
52 LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
53 EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
54 EXPECT(assert_equal(b, expected.getb()));
55 EXPECT(assert_equal(b, actual.getb()));
56 EXPECT(assert_equal(*noise, *actual.get_model()));
57 }
58 {
59 // Two term constructor
60 LinearEquality expected(Terms(terms.begin(), terms.begin() + 2), b, 0);
61 LinearEquality actual(terms[0].first, terms[0].second, terms[1].first,
62 terms[1].second, b, 0);
63 EXPECT(assert_equal(expected, actual));
64 LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
65 EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
66 EXPECT(assert_equal(b, expected.getb()));
67 EXPECT(assert_equal(b, actual.getb()));
68 EXPECT(assert_equal(*noise, *actual.get_model()));
69 }
70 {
71 // Three term constructor
72 LinearEquality expected(Terms(terms.begin(), terms.begin() + 3), b, 0);
73 LinearEquality actual(terms[0].first, terms[0].second, terms[1].first,
74 terms[1].second, terms[2].first, terms[2].second, b,
75 0);
76 EXPECT(assert_equal(expected, actual));
77 LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
78 EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
79 EXPECT(assert_equal(b, expected.getb()));
80 EXPECT(assert_equal(b, actual.getb()));
81 EXPECT(assert_equal(*noise, *actual.get_model()));
82 }
83}
84
85/* ************************************************************************* */
86TEST(LinearEquality, Hessian_conversion) {
87 HessianFactor hessian(
88 0,
89 (Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225,
90 -1.1, -0.65, 1, 0.5, -2.35, -10.225, 0.5, 9.25)
91 .finished(),
92 (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725);
93
94 try {
95 LinearEquality actual(hessian);
96 EXPECT(false);
97 } catch (const std::runtime_error& exception) {
98 EXPECT(true);
99 }
100}
101
102/* ************************************************************************* */
103TEST(LinearEquality, error) {
104 LinearEquality factor(simple::terms, simple::b, 0);
105
106 VectorValues values;
107 values.insert(j: 5, value: Vector::Constant(size: 3, value: 1.0));
108 values.insert(j: 10, value: Vector::Constant(size: 3, value: 0.5));
109 values.insert(j: 15, value: Vector::Constant(size: 3, value: 1.0 / 3.0));
110
111 Vector expected_unwhitened(3);
112 expected_unwhitened << 2.0, 1.0, 0.0;
113 Vector actual_unwhitened = factor.unweighted_error(c: values);
114 EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
115
116 // whitened is meaningless in constraints
117 Vector expected_whitened(3);
118 expected_whitened = expected_unwhitened;
119 Vector actual_whitened = factor.error_vector(c: values);
120 EXPECT(assert_equal(expected_whitened, actual_whitened));
121
122 double expected_error = 0.0;
123 double actual_error = factor.error(c: values);
124 DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
125}
126
127/* ************************************************************************* */
128TEST(LinearEquality, matrices_NULL) {
129 // Make sure everything works with nullptr noise model
130 LinearEquality factor(simple::terms, simple::b, 0);
131
132 Matrix AExpected(3, 9);
133 AExpected << simple::terms[0].second, simple::terms[1].second,
134 simple::terms[2].second;
135 Vector rhsExpected = simple::b;
136 Matrix augmentedJacobianExpected(3, 10);
137 augmentedJacobianExpected << AExpected, rhsExpected;
138
139 // Whitened Jacobian
140 EXPECT(assert_equal(AExpected, factor.jacobian().first));
141 EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
142 EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
143
144 // Unwhitened Jacobian
145 EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first));
146 EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
147 EXPECT(assert_equal(augmentedJacobianExpected,
148 factor.augmentedJacobianUnweighted()));
149}
150
151/* ************************************************************************* */
152TEST(LinearEquality, matrices) {
153 // And now witgh a non-unit noise model
154 LinearEquality factor(simple::terms, simple::b, 0);
155
156 Matrix jacobianExpected(3, 9);
157 jacobianExpected << simple::terms[0].second, simple::terms[1].second,
158 simple::terms[2].second;
159 Vector rhsExpected = simple::b;
160 Matrix augmentedJacobianExpected(3, 10);
161 augmentedJacobianExpected << jacobianExpected, rhsExpected;
162
163 Matrix augmentedHessianExpected =
164 augmentedJacobianExpected.transpose() * simple::noise->R().transpose() *
165 simple::noise->R() * augmentedJacobianExpected;
166
167 // Whitened Jacobian
168 EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
169 EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
170 EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
171
172 // Unwhitened Jacobian
173 EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
174 EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
175 EXPECT(assert_equal(augmentedJacobianExpected,
176 factor.augmentedJacobianUnweighted()));
177}
178
179/* ************************************************************************* */
180TEST(LinearEquality, operators) {
181 Matrix I = I_2x2;
182 Vector b = (Vector(2) << 0.2, -0.1).finished();
183 LinearEquality lf(1, -I, 2, I, b, 0);
184
185 VectorValues c;
186 c.insert(j: 1, value: (Vector(2) << 10., 20.).finished());
187 c.insert(j: 2, value: (Vector(2) << 30., 60.).finished());
188
189 // test A*x
190 Vector expectedE = (Vector(2) << 20., 40.).finished();
191 Vector actualE = lf * c;
192 EXPECT(assert_equal(expectedE, actualE));
193
194 // test A^e
195 VectorValues expectedX;
196 expectedX.insert(j: 1, value: (Vector(2) << -20., -40.).finished());
197 expectedX.insert(j: 2, value: (Vector(2) << 20., 40.).finished());
198 VectorValues actualX = VectorValues::Zero(other: expectedX);
199 lf.transposeMultiplyAdd(alpha: 1.0, e: actualE, x&: actualX);
200 EXPECT(assert_equal(expectedX, actualX));
201
202 // test gradient at zero
203 const auto [A, b2] = lf.jacobian();
204 VectorValues expectedG;
205 expectedG.insert(j: 1, value: (Vector(2) << 0.2, -0.1).finished());
206 expectedG.insert(j: 2, value: (Vector(2) << -0.2, 0.1).finished());
207 VectorValues actualG = lf.gradientAtZero();
208 EXPECT(assert_equal(expectedG, actualG));
209}
210
211/* ************************************************************************* */
212TEST(LinearEquality, default_error) {
213 LinearEquality f;
214 double actual = f.error(c: VectorValues());
215 DOUBLES_EQUAL(0.0, actual, 1e-15);
216}
217
218//* ************************************************************************* */
219TEST(LinearEquality, empty) {
220 // create an empty factor
221 LinearEquality f;
222 EXPECT(f.empty());
223}
224
225/* ************************************************************************* */
226int main() {
227 TestResult tr;
228 return TestRegistry::runAllTests(result&: tr);
229}
230/* ************************************************************************* */
231