testLinearEquality.cpp
Go to the documentation of this file.
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 
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 
24 using namespace std;
25 using namespace gtsam;
26 
27 GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
28 
29 namespace {
30 namespace simple {
31 // Terms we'll use
32 using Terms = vector<pair<Key, Matrix> >;
33 const Terms terms{make_pair(5, I_3x3), make_pair(10, 2 * I_3x3),
34  make_pair(15, 3 * I_3x3)};
35 
36 // RHS and sigmas
37 const Vector b = (Vector(3) << 1., 2., 3.).finished();
38 const SharedDiagonal noise = noiseModel::Constrained::All(3);
39 } // namespace simple
40 } // namespace
41 
42 /* ************************************************************************* */
43 TEST(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 /* ************************************************************************* */
86 TEST(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 /* ************************************************************************* */
103 TEST(LinearEquality, error) {
104  LinearEquality factor(simple::terms, simple::b, 0);
105 
106  VectorValues values;
107  values.insert(5, Vector::Constant(3, 1.0));
108  values.insert(10, Vector::Constant(3, 0.5));
109  values.insert(15, Vector::Constant(3, 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(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(values);
120  EXPECT(assert_equal(expected_whitened, actual_whitened));
121 
122  double expected_error = 0.0;
123  double actual_error = factor.error(values);
124  DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
125 }
126 
127 /* ************************************************************************* */
128 TEST(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 /* ************************************************************************* */
152 TEST(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 /* ************************************************************************* */
180 TEST(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(1, (Vector(2) << 10., 20.).finished());
187  c.insert(2, (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(1, (Vector(2) << -20., -40.).finished());
197  expectedX.insert(2, (Vector(2) << 20., 40.).finished());
198  VectorValues actualX = VectorValues::Zero(expectedX);
199  lf.transposeMultiplyAdd(1.0, actualE, 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(1, (Vector(2) << 0.2, -0.1).finished());
206  expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
207  VectorValues actualG = lf.gradientAtZero();
208  EXPECT(assert_equal(expectedG, actualG));
209 }
210 
211 /* ************************************************************************* */
212 TEST(LinearEquality, default_error) {
213  LinearEquality f;
214  double actual = f.error(VectorValues());
215  DOUBLES_EQUAL(0.0, actual, 1e-15);
216 }
217 
218 //* ************************************************************************* */
219 TEST(LinearEquality, empty) {
220  // create an empty factor
221  LinearEquality f;
222  EXPECT(f.empty());
223 }
224 
225 /* ************************************************************************* */
226 int main() {
227  TestResult tr;
228  return TestRegistry::runAllTests(tr);
229 }
230 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
LinearEquality.h
LinearEquality derived from Base with constrained noise model.
gtsam::JacobianFactor::unweighted_error
Vector unweighted_error(const VectorValues &c) const
Definition: JacobianFactor.cpp:471
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:88
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple_graph::b2
Vector2 b2(4, -5)
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::LinearEquality::error
double error(const VectorValues &c) const override
Definition: LinearEquality.h:127
gtsam::JacobianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
Definition: JacobianFactor.cpp:714
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
TestableAssertions.h
Provides additional testing facilities for common data structures.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::LinearEquality
Definition: LinearEquality.h:29
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::LinearEquality::error_vector
Vector error_vector(const VectorValues &c) const
Definition: LinearEquality.h:119
gtsam::VectorValues
Definition: VectorValues.h:74
I
#define I
Definition: main.h:112
gtsam::JacobianFactor::jacobianUnweighted
std::pair< Matrix, Vector > jacobianUnweighted() const
Returns (dense) A,b pair associated with factor, does not bake in weights.
Definition: JacobianFactor.cpp:723
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition: JacobianFactor.cpp:690
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::JacobianFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Definition: JacobianFactor.cpp:730
VectorValues.h
Factor Graph Values.
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:148
main
int main()
Definition: testLinearEquality.cpp:226
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
TEST
TEST(LinearEquality, constructors_and_accessors)
Definition: testLinearEquality.cpp:43
empty
Definition: test_copy_move.cpp:19
gtsam
traits
Definition: chartTesting.h:28
error
static double error
Definition: testRot3.cpp:37
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Definition: JacobianFactor.h:301
leaf::values
leaf::MyValues values
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:142
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::JacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, VectorValues &x) const
Definition: JacobianFactor.cpp:618
simple
Definition: testJacobianFactor.cpp:34
gtsam::JacobianFactor::augmentedJacobianUnweighted
Matrix augmentedJacobianUnweighted() const
Definition: JacobianFactor.cpp:738
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:06:11