1#ifndef SFM_BA_CHOLESKY_HEADER
2#define SFM_BA_CHOLESKY_HEADER
53 T* tmp =
new T[cols * cols];
64 T* tmp =
new T[cols * cols];
75 for (
int r = 0; r < cols; ++r)
78 for (
int c = 0; c < r; ++c)
81 for (
int ci = 0; ci < c; ++ci)
82 result += L[r * cols + ci] * L[c * cols + ci];
83 result = A[r * cols + c] - result;
84 (*out_ptr++) = result / L[c * cols + c];
89 T* L_row_ptr = L + r * cols;
91 for (
int c = 0; c < r; ++c)
93 result = std::max(T(0), A[r * cols + r] - result);
94 (*out_ptr++) = std::sqrt(result);
98 for (
int c = r + 1; c < cols; ++c)
108 throw std::invalid_argument(
"In-place inversion not possible");
110 for (
int r = 0; r < cols; ++r)
112 T
const* A_row_ptr = A + r * cols;
113 T* A_inv_row_ptr = A_inv + r * cols;
116 for (
int c = 0; c < r; ++c)
119 for (
int ci = 0; ci < r; ++ci)
120 result -= A_row_ptr[ci] * A_inv[ci * cols + c];
121 A_inv_row_ptr[c] = result / A_row_ptr[r];
125 A_inv_row_ptr[r] = T(1) / A_row_ptr[r];
128 for (
int c = r + 1; c < cols; ++c)
129 A_inv_row_ptr[c] = T(0);
void matrix_transpose_multiply(T const *mat_a, int rows, int cols, T *mat_res)
Matrix multiplication of the transposed with itself.
void cholesky_decomposition(T const *A, int const cols, T *L)
Cholesky decomposition of the symmetric, positive definite matrix A = L * L^T.
void invert_lower_diagonal(T const *A, int const cols, T *A_inv)
Invert a lower-triangular matrix (e.g.
void cholesky_invert(T const *A, int const cols, T *A_inv)
Invert symmetric, positive definite matrix A using Cholesky decomposition and inversion of the triang...
void cholesky_invert_inplace(T *A, int const cols)
Invert symmetric, positive definite matrix A inplace using Cholesky.
#define SFM_BA_NAMESPACE_BEGIN
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN
#define SFM_BA_NAMESPACE_END