MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
ba_cholesky.h
Go to the documentation of this file.
1#ifndef SFM_BA_CHOLESKY_HEADER
2#define SFM_BA_CHOLESKY_HEADER
3
4#include <stdexcept>
5#include <cmath>
6
7#include "math/defines.h"
8#include "math/matrix_tools.h"
9#include "sfm/defines.h"
10
13
18template <typename T>
19void
20cholesky_invert (T const* A, int const cols, T* A_inv);
21
25template <typename T>
26void
27cholesky_invert_inplace (T* A, int const cols);
28
34template <typename T>
35void
36cholesky_decomposition (T const* A, int const cols, T* L);
37
42template <typename T>
43void
44invert_lower_diagonal (T const* A, int const cols, T* A_inv);
45
46/* ------------------------ Implementation ------------------------ */
47
48template <typename T>
49void
50cholesky_invert (T const* A, int const cols, T* A_inv)
51{
52 cholesky_decomposition(A, cols, A_inv);
53 T* tmp = new T[cols * cols];
54 invert_lower_diagonal(A_inv, cols, tmp);
55 math::matrix_transpose_multiply(tmp, cols, cols, A_inv);
56 delete[] tmp;
57}
58
59template <typename T>
60void
61cholesky_invert_inplace (T* A, int const cols)
62{
63 cholesky_decomposition(A, cols, A);
64 T* tmp = new T[cols * cols];
65 invert_lower_diagonal(A, cols, tmp);
66 math::matrix_transpose_multiply(tmp, cols, cols, A);
67 delete[] tmp;
68}
69
70template <typename T>
71void
72cholesky_decomposition (T const* A, int const cols, T* L)
73{
74 T* out_ptr = L;
75 for (int r = 0; r < cols; ++r)
76 {
77 /* Compute left-of-diagonal entries. */
78 for (int c = 0; c < r; ++c)
79 {
80 T result = T(0);
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];
85 }
86
87 /* Compute diagonal entry. */
88 {
89 T* L_row_ptr = L + r * cols;
90 T result = T(0);
91 for (int c = 0; c < r; ++c)
92 result += MATH_POW2(L_row_ptr[c]);
93 result = std::max(T(0), A[r * cols + r] - result);
94 (*out_ptr++) = std::sqrt(result);
95 }
96
97 /* Set right-of-diagonal entries zero. */
98 for (int c = r + 1; c < cols; ++c)
99 (*out_ptr++) = T(0);
100 }
101}
102
103template <typename T>
104void
105invert_lower_diagonal (T const* A, int const cols, T* A_inv)
106{
107 if (A == A_inv)
108 throw std::invalid_argument("In-place inversion not possible");
109
110 for (int r = 0; r < cols; ++r)
111 {
112 T const* A_row_ptr = A + r * cols;
113 T* A_inv_row_ptr = A_inv + r * cols;
114
115 /* Compute left-of-diagonal entries. */
116 for (int c = 0; c < r; ++c)
117 {
118 T result = T(0);
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];
122 }
123
124 /* Compute diagonal entry. */
125 A_inv_row_ptr[r] = T(1) / A_row_ptr[r];
126
127 /* Set right-of-diagonal entries zero. */
128 for (int c = r + 1; c < cols; ++c)
129 A_inv_row_ptr[c] = T(0);
130 }
131}
132
135
136#endif // SFM_BA_CHOLESKY_HEADER
#define MATH_POW2(x)
Definition defines.h:68
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.
Definition ba_cholesky.h:72
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...
Definition ba_cholesky.h:50
void cholesky_invert_inplace(T *A, int const cols)
Invert symmetric, positive definite matrix A inplace using Cholesky.
Definition ba_cholesky.h:61
#define SFM_BA_NAMESPACE_BEGIN
Definition defines.h:22
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
#define SFM_BA_NAMESPACE_END
Definition defines.h:23