All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
LinearSolveAndInverse-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
17 #define SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
18 
20 
21 namespace SurgSim
22 {
23 
24 namespace Math
25 {
26 
27 template <int BlockSize>
28 const Eigen::Block<const Matrix, BlockSize, BlockSize>
30 {
31  return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i - 1));
32 }
33 
34 template <int BlockSize>
35 const Eigen::Block<const Matrix, BlockSize, BlockSize>
37 {
38  return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i);
39 }
40 
41 template <int BlockSize>
42 const Eigen::Block<const Matrix, BlockSize, BlockSize>
44 {
45  return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i + 1));
46 }
47 
48 template <int BlockSize>
50  SurgSim::Math::Matrix* inverse,
51  bool isSymmetric)
52 {
53  SURGSIM_ASSERT(inverse != nullptr) << "Null inverse matrix pointer";
54 
55  SURGSIM_ASSERT(A.cols() == A.rows()) <<
56  "Cannot inverse a non square tri-diagonal block matrix ("<< A.rows() <<" x "<< A.cols() <<")";
57 
58  const size_t size = A.rows();
59  const size_t numBlocks = size / BlockSize;
60 
61  SURGSIM_ASSERT(numBlocks * BlockSize == size) <<
62  "Bad tri-diagonal block matrix structure, size = " << size << " block size = " << BlockSize <<
63  " and the number of blocks are " << numBlocks;
64 
65  // If the matrix size is less or equal to 4 (Eigen inverse use co-factor for those), or the matrix is
66  // composed of an unique block, simply call the normal Eigen inverse method.
67  if (size <= 4 || numBlocks == static_cast<size_t>(1))
68  {
69  *inverse = A.inverse();
70  return;
71  }
72 
73  if (inverse->rows() < 0 || static_cast<size_t>(inverse->rows()) != size
74  || inverse->cols() < 0 || static_cast<size_t>(inverse->cols()) != size)
75  {
76  inverse->resize(size, size);
77  }
78 
79  m_Bi_AiDiminus1_inv.resize(numBlocks);
80  m_Di.resize(numBlocks - 1);
81  m_Ei.resize(numBlocks); // Should be of size m_numBlocks - 1 too, but index 0 is undefined and we
82  // decided to not change the indexing to not introduce any confusion
83 
84  // Bi_AiDiminus1_inv[0] = (B0)^-1
85  // D [0] = (B0)^-1.C0
86  m_Bi_AiDiminus1_inv[0] = Bi(A, 0).inverse();
87  m_Di[0] = m_Bi_AiDiminus1_inv[0] * (-minusCi(A, 0));
88  // Bi_AiDiminus1_inv[i] = (Bi - Ai.D[i-1])^-1
89  // Di [i] = (Bi - Ai.D[i-1])^-1 . Ci
90  for(size_t i = 1; i < numBlocks - 1; ++i)
91  {
92  m_Bi_AiDiminus1_inv[i] = (Bi(A, i) - (-minusAi(A, i)) * m_Di[i - 1]).inverse();
93  m_Di[i] = m_Bi_AiDiminus1_inv[i] * (-minusCi(A, i));
94  }
95  // Bi_AiDiminus1_inv[nbBlock-1] = (B(nbBlock-1) - A(nbBlock-1).D(nbBlock-2))^-1
96  // D [nbBlock-1] = UNDEFINED because C(nbBlock-1) does not exist
97  m_Bi_AiDiminus1_inv[numBlocks - 1] =
98  (Bi(A, numBlocks - 1) - (-minusAi(A, numBlocks - 1)) * m_Di[numBlocks - 2]).inverse();
99 
100  // E[nbBlock-1] = (B(nbBlock-1))^-1 . A(nbBlock-1)
101  // Ei = (Bi - Ci.E(i+1))^-1 . Ai
102  // E0 = UNDEFINED because A0 does not exist
103  m_Ei[numBlocks - 1] = Bi(A, numBlocks - 1).inverse() * (-minusAi(A, numBlocks - 1));
104  for(size_t i = numBlocks - 2; i > 0; --i)
105  {
106  m_Ei[i] = (Bi(A, i) - (-minusCi(A, i)) * m_Ei[i + 1]).inverse() * (-minusAi(A, i));
107  }
108 
109  // Inverse diagonal blocks:
110  // inv(i,i) = (I - Di.E(i+1))^-1.Bi_AiDiminus1_inv[i]
111  for(size_t i = 0; i < numBlocks - 1; ++i)
112  {
113  inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i) =
114  (Block::Identity() - m_Di[i] * m_Ei[i + 1]).inverse() * m_Bi_AiDiminus1_inv[i];
115  }
116  // inv(nbBlock-1,nbBlock-1) = Bi_AiDiminus1_inv[nbBlock-1]
117  inverse->block<BlockSize, BlockSize>(BlockSize * (numBlocks - 1), BlockSize * (numBlocks - 1)) =
118  m_Bi_AiDiminus1_inv[numBlocks - 1];
119 
120  // Inverse off-diagonal blocks:
121  // inv(i,j) = Di.inv(i+1,j) for i<j
122  // inv(i,j) = Ei.inv(i-1,j) for i>j
123  if (isSymmetric)
124  {
125  for(size_t j = 1; j < numBlocks; ++j)
126  {
127  for(size_t i = j; i > 0; --i)
128  {
129  inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j) =
130  m_Di[i - 1] * inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j);
131  inverse->block<BlockSize, BlockSize>(BlockSize * j, BlockSize * (i - 1)) =
132  inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j).transpose();
133  }
134  }
135  }
136  else
137  {
138  for(int j = 0; j < static_cast<int>(numBlocks); ++j)
139  {
140  for(int i = j - 1; i >= 0; --i)
141  {
142  inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
143  m_Di[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i + 1), BlockSize * j);
144  }
145  for(int i = j + 1; i < static_cast<int>(numBlocks); ++i)
146  {
147  inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
148  m_Ei[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j);
149  }
150  }
151  }
152 }
153 
154 template <int BlockSize>
156  Matrix* Ainv)
157 {
158  SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
159 
160  if (Ainv != nullptr)
161  {
162  inverseTriDiagonalBlock(A, Ainv);
163  if (x != nullptr)
164  {
165  (*x) = (*Ainv) * b;
166  }
167  }
168  else if (x != nullptr)
169  {
170  inverseTriDiagonalBlock(A, &m_inverse);
171  (*x) = m_inverse * b;
172  }
173 }
174 
175 template <int BlockSize>
177  Vector* x,
178  Matrix* Ainv)
179 {
180  SURGSIM_ASSERT(A.cols() == A.rows()) << "Cannot inverse a non square matrix";
181 
182  if (Ainv != nullptr)
183  {
184  inverseTriDiagonalBlock(A, Ainv, true);
185  if (x != nullptr)
186  {
187  (*x) = (*Ainv) * b;
188  }
189  }
190  else if (x != nullptr)
191  {
192  inverseTriDiagonalBlock(A, &m_inverse, true);
193  (*x) = m_inverse * b;
194  }
195 }
196 
197 }; // namespace Math
198 
199 }; // namespace SurgSim
200 
201 #endif // SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H
const Eigen::Block< const Matrix, BlockSize, BlockSize > Bi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a diagonal block element (named Bi in the algorithm)
Definition: LinearSolveAndInverse-inl.h:36
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
virtual void operator()(const Matrix &A, const Vector &b, Vector *x=nullptr, Matrix *Ainv=nullptr) override
Solve a linear system A.x=b and compute the matrix A^-1.
Definition: LinearSolveAndInverse-inl.h:176
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
const Eigen::Block< const Matrix, BlockSize, BlockSize > minusAi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a lower-diagonal block element (named -Ai in the algorithm)
Definition: LinearSolveAndInverse-inl.h:29
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:67
virtual void operator()(const Matrix &A, const Vector &b, Vector *x=nullptr, Matrix *Ainv=nullptr) override
Solve a linear system A.x=b and compute the matrix A^-1.
Definition: LinearSolveAndInverse-inl.h:155
const Eigen::Block< const Matrix, BlockSize, BlockSize > minusCi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a upper-diagonal block element (named -Ci in the algorithm)
Definition: LinearSolveAndInverse-inl.h:43
The header that provides the assertion API.
void inverseTriDiagonalBlock(const SurgSim::Math::Matrix &A, SurgSim::Math::Matrix *inv, bool isSymmetric=false)
Computes the inverse matrix.
Definition: LinearSolveAndInverse-inl.h:49