testSymmetricBlockMatrix.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 
20 
21 using namespace std;
22 using namespace gtsam;
23 
25  std::vector<size_t>{3, 2, 1},
26  (Matrix(6, 6) <<
27  1, 2, 3, 4, 5, 6,
28  2, 8, 9, 10, 11, 12,
29  3, 9, 15, 16, 17, 18,
30  4, 10, 16, 22, 23, 24,
31  5, 11, 17, 23, 29, 30,
32  6, 12, 18, 24, 30, 36).finished());
33 
34 /* ************************************************************************* */
36 {
37  // On the diagonal
38  Matrix expected1 = (Matrix(2, 2) <<
39  22, 23,
40  23, 29).finished();
42  EXPECT(assert_equal(expected1, actual1));
43 
44  // Above the diagonal
45  Matrix expected2 = (Matrix(3, 2) <<
46  4, 5,
47  10, 11,
48  16, 17).finished();
50  EXPECT(assert_equal(expected2, actual2));
51 }
52 
53 /* ************************************************************************* */
55 {
56  // On the diagonal
57  Matrix expected1 = testBlockMatrix.diagonalBlock(1);
58  SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
59 
60  bm1.setDiagonalBlock(1, expected1);
61  Matrix actual1 = bm1.diagonalBlock(1);
62  EXPECT(assert_equal(expected1, actual1));
63 
64  // Above the diagonal
65  Matrix expected2 = testBlockMatrix.aboveDiagonalBlock(0, 1);
66  SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
67  bm2.setOffDiagonalBlock(0, 1, expected2);
68  Matrix actual2 = bm2.aboveDiagonalBlock(0, 1);
69  EXPECT(assert_equal(expected2, actual2));
70 
71  // Below the diagonal
72  Matrix expected3 = testBlockMatrix.aboveDiagonalBlock(0, 1).transpose();
73  SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
74  bm3.setOffDiagonalBlock(1, 0, expected3);
75  Matrix actual3 = bm3.aboveDiagonalBlock(0, 1).transpose();
76  EXPECT(assert_equal(expected3, actual3));
77 }
78 
79 /* ************************************************************************* */
81 {
82  // On the diagonal
83  Matrix expected1 = (Matrix(3, 3) <<
84  22, 23, 24,
85  23, 29, 30,
86  24, 30, 36).finished();
87  Matrix actual1 = testBlockMatrix.selfadjointView(1, 3);
88  EXPECT(assert_equal(expected1, actual1));
89 
90  // Above the diagonal
91  Matrix expected2 = (Matrix(3, 3) <<
92  4, 5, 6,
93  10, 11, 12,
94  16, 17, 18).finished();
95  Matrix actual2 = testBlockMatrix.aboveDiagonalRange(0, 1, 1, 3);
96  EXPECT(assert_equal(expected2, actual2));
97 }
98 
99 /* ************************************************************************* */
101 {
102  const std::vector<size_t> dimensions{2, 3, 1};
103  SymmetricBlockMatrix expected1(dimensions, (Matrix(6, 6) <<
104  0, 0, 0, 0, 0, 0,
105  0, 0, 0, 0, 0, 0,
106  0, 0, 4, 6, 8, 0,
107  0, 0, 0, 9, 12, 0,
108  0, 0, 0, 0, 16, 0,
109  0, 0, 0, 0, 0, 0).finished());
110 
111  SymmetricBlockMatrix expected2(dimensions, (Matrix(6, 6) <<
112  0, 0, 10, 15, 20, 0,
113  0, 0, 12, 18, 24, 0,
114  0, 0, 0, 0, 0, 0,
115  0, 0, 0, 0, 0, 0,
116  0, 0, 0, 0, 0, 0,
117  0, 0, 0, 0, 0, 0).finished());
118 
119  Matrix a = (Matrix(1, 3) << 2, 3, 4).finished();
120  Matrix b = (Matrix(1, 2) << 5, 6).finished();
121 
123  bm1.setZero();
124  bm1.diagonalBlock(1).rankUpdate(a.transpose());
126 
128  bm2.setZero();
129  bm2.updateOffDiagonalBlock(0, 1, b.transpose() * a);
131 
133  bm3.setZero();
134  bm3.updateOffDiagonalBlock(1, 0, a.transpose() * b);
136 
138  bm4.setZero();
139  bm4.updateDiagonalBlock(1, expected1.diagonalBlock(1));
141 
143  bm5.setZero();
144  bm5.updateOffDiagonalBlock(0, 1, expected2.aboveDiagonalBlock(0, 1));
146 
148  bm6.setZero();
149  bm6.updateOffDiagonalBlock(1, 0, expected2.aboveDiagonalBlock(0, 1).transpose());
151 }
152 
153 /* ************************************************************************* */
154 TEST(SymmetricBlockMatrix, inverseInPlace) {
155  // generate an invertible matrix
156  const Vector3 a(1.0, 0.2, 2.0), b(0.3, 0.8, -1.0), c(0.1, 0.2, 0.7);
157  Matrix inputMatrix(3, 3);
158  inputMatrix.setZero();
159  inputMatrix += a * a.transpose();
160  inputMatrix += b * b.transpose();
161  inputMatrix += c * c.transpose();
162  const Matrix expectedInverse = inputMatrix.inverse();
163 
164  const std::vector<size_t> dimensions{2, 1};
165  SymmetricBlockMatrix symmMatrix(dimensions, inputMatrix);
166  // invert in place
167  symmMatrix.invertInPlace();
168  EXPECT(assert_equal(expectedInverse, symmMatrix.selfadjointView()));
169 }
170 
171 /* ************************************************************************* */
172 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
173 /* ************************************************************************* */
174 
Scalar * b
Definition: benchVecAdd.cpp:17
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated. ...
constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
Get a range [i,j) from the matrix. Indices are in block units.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
TEST(SymmetricBlockMatrix, ReadBlocks)
Definition: BFloat16.h:88
void invertInPlace()
Invert the entire active matrix in place.
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
static SymmetricBlockMatrix testBlockMatrix(std::vector< size_t >{3, 2, 1},(Matrix(6, 6)<< 1, 2, 3, 4, 5, 6, 2, 8, 9, 10, 11, 12, 3, 9, 15, 16, 17, 18, 4, 10, 16, 22, 23, 24, 5, 11, 17, 23, 29, 30, 6, 12, 18, 24, 30, 36).finished())
#define EXPECT(condition)
Definition: Test.h:150
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr...
traits
Definition: chartTesting.h:28
void setZero()
Set the entire active matrix zero.
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J&#39;th diagonal block as a self adjoint view.
const std::vector< size_t > dimensions
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:43