testRegularHessianFactor.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 
21 
23 
24 using namespace std;
25 using namespace gtsam;
26 
27 /* ************************************************************************* */
28 TEST(RegularHessianFactor, Constructors)
29 {
30  // First construct a regular JacobianFactor
31  // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I]
32  Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
33  Vector2 b(1,2);
34  const vector<pair<Key, Matrix> > terms {{0, A1}, {1, A2}, {3, A3}};
35  RegularJacobianFactor<2> jf(terms, b);
36 
37  // Test conversion from JacobianFactor
38  RegularHessianFactor<2> factor(jf);
39 
40  // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b
41  // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f
42  // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5
43  Matrix G11 = I_2x2;
44  Matrix G12 = I_2x2;
45  Matrix G13 = I_2x2;
46 
47  Matrix G22 = I_2x2;
48  Matrix G23 = I_2x2;
49 
50  Matrix G33 = I_2x2;
51 
52  Vector2 g1 = b, g2 = b, g3 = b;
53 
54  double f = 5;
55 
56  // Test ternary constructor
57  RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
58  EXPECT(assert_equal(factor,factor2));
59 
60  // Test n-way constructor
61  KeyVector keys {0, 1, 3};
62  vector<Matrix> Gs {G11, G12, G13, G22, G23, G33};
63  vector<Vector> gs {g1, g2, g3};
65  EXPECT(assert_equal(factor, factor3));
66 
67  // Test constructor from Gaussian Factor Graph
69  gfg.push_back(jf);
70  RegularHessianFactor<2> factor4(gfg);
71  EXPECT(assert_equal(factor, factor4));
73  gfg2.push_back(factor);
74  RegularHessianFactor<2> factor5(gfg);
75  EXPECT(assert_equal(factor, factor5));
76 
77  // Test constructor from Information matrix
78  Matrix info = factor.augmentedInformation();
79  vector<size_t> dims {2, 2, 2};
80  SymmetricBlockMatrix sym(dims, info, true);
81  RegularHessianFactor<2> factor6(keys, sym);
82  EXPECT(assert_equal(factor, factor6));
83 
84  // multiplyHessianAdd:
85  {
86  // brute force
87  Matrix AtA = factor.information();
88  HessianFactor::const_iterator i1 = factor.begin();
89  HessianFactor::const_iterator i2 = i1 + 1;
90  Vector X(6); X << 1,2,3,4,5,6;
91  Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
92  EXPECT(assert_equal(Y,AtA*X));
93 
94  VectorValues x{{0, Vector2(1, 2)}, {1, Vector2(3, 4)}, {3, Vector2(5, 6)}};
95 
97  expected.insert(0, Y.segment<2>(0));
98  expected.insert(1, Y.segment<2>(2));
99  expected.insert(3, Y.segment<2>(4));
100 
101  // VectorValues version
102  double alpha = 1.0;
103  VectorValues actualVV;
104  actualVV.insert(0, Vector2::Zero());
105  actualVV.insert(1, Vector2::Zero());
106  actualVV.insert(3, Vector2::Zero());
107  factor.multiplyHessianAdd(alpha, x, actualVV);
108  EXPECT(assert_equal(expected, actualVV));
109 
110  // RAW ACCESS
111  Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
112  Vector fast_y = Vector8::Zero();
113  double xvalues[8] = {1,2,3,4,0,0,5,6};
114  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
115  EXPECT(assert_equal(expected_y, fast_y));
116 
117  // now, do it with non-zero y
118  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
119  EXPECT(assert_equal(2*expected_y, fast_y));
120 
121  // check some expressions
122  EXPECT(assert_equal(G12,factor.info().aboveDiagonalBlock(i1 - factor.begin(), i2 - factor.begin())));
123  EXPECT(assert_equal(G22,factor.info().diagonalBlock(i2 - factor.begin())));
124  }
125 }
126 
127 /* ************************************************************************* */
128 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
129 /* ************************************************************************* */
static Matrix A1
const char Y
Scalar * b
Definition: benchVecAdd.cpp:17
static int runAllTests(TestResult &result)
Matrix expected
Definition: testMatrix.cpp:971
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Definition: BFloat16.h:88
iterator insert(const std::pair< Key, Vector > &key_value)
Matrix augmentedInformation() const override
else if n * info
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
Matrix information() const override
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar alpha
HessianFactor class with constant sized blocks.
Linear Factor Graph where all factors are Gaussians.
TEST(RegularHessianFactor, Constructors)
traits
Definition: chartTesting.h:28
Eigen::Vector2d Vector2
Definition: Vector.h:42
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J&#39;th diagonal block as a self adjoint view.
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
const_iterator begin() const
Definition: Factor.h:145
#define X
Definition: icosphere.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Pose3 g2(g1.expmap(h *V1_g1))
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:15