qpmad
Eigen-based C++ QP solver.
cholesky.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4 
5  @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
6  Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7 
8  @brief
9 */
10 
11 
12 #pragma once
13 
14 namespace qpmad
15 {
16  /**
17  * @brief Cholesky factorization
18  *
19  * We need our own implementation of Cholesky factorization since inplace
20  * factorization is not supported by old versions of Eigen. Also, we may
21  * want to add regularization at this stage.
22  */
24  {
25  public:
26  template <class t_MatrixType>
27  inline static void compute(t_MatrixType &M)
28  {
29  const MatrixIndex size = M.rows();
30 
31  M(0, 0) = std::sqrt(M(0,0));
32 
33  for (MatrixIndex i = 1; i < size; ++i)
34  {
35  M.col(i-1).segment(i, size-i) /= M(i-1,i-1);
36 
37  M.col(i).segment(i, size-i).noalias() -= M.block(i, 0, size-i, i) * M.row(i).segment(0, i).transpose();
38 
39  M(i, i) = std::sqrt(M(i,i));
40  }
41  }
42 
43 
44  template < class t_OutputVectorType,
45  class t_InputMatrixType0,
46  class t_InputMatrixType1>
47  inline static void solve( t_OutputVectorType &x,
48  t_InputMatrixType0 &L,
49  t_InputMatrixType1 &v)
50  {
51  x = L.template triangularView<Eigen::Lower>().solve(v);
52  L.transpose().template triangularView<Eigen::Upper>().solveInPlace(x);
53  }
54  };
55 }
int MatrixIndex
Definition: common.h:32
static void compute(t_MatrixType &M)
Definition: cholesky.h:27
static void solve(t_OutputVectorType &x, t_InputMatrixType0 &L, t_InputMatrixType1 &v)
Definition: cholesky.h:47
Cholesky factorization.
Definition: cholesky.h:23