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
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);
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};
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
A3
static const double A3[]
Definition: expn.h:8
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
TestHarness.h
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::RegularHessianFactor
Definition: RegularHessianFactor.h:28
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
RegularHessianFactor.h
HessianFactor class with constant sized blocks.
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::VectorValues
Definition: VectorValues.h:74
g2
Pose3 g2(g1.expmap(h *V1_g1))
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
Vector2
Definition: test_operator_overloading.cpp:18
VectorValues.h
Factor Graph Values.
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
TEST
TEST(RegularHessianFactor, Constructors)
Definition: testRegularHessianFactor.cpp:28
gtsam
traits
Definition: SFMdata.h:40
i1
double i1(double x)
Definition: i1.c:150
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
main
int main()
Definition: testRegularHessianFactor.cpp:128
std
Definition: BFloat16.h:88
A1
static const double A1[]
Definition: expn.h:6
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:17:10