MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
ba_linear_solver.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <stdexcept>
11#include <iostream>
12
13#include "util/timer.h"
14#include "math/matrix.h"
15#include "math/matrix_tools.h"
17#include "sfm/ba_cholesky.h"
19
22
23namespace
24{
25 /*
26 * Inverts a symmetric, positive definite matrix with NxN bocks on its
27 * diagonal using Cholesky decomposition. All other entries must be zero.
28 */
29 void
30 invert_block_matrix_NxN_inplace (SparseMatrix<double>* A, int blocksize)
31 {
32 if (A->num_rows() != A->num_cols())
33 throw std::invalid_argument("Block matrix must be square");
34 if (A->num_non_zero() != A->num_rows() * blocksize)
35 throw std::invalid_argument("Invalid number of non-zeros");
36
37 int const bs2 = blocksize * blocksize;
38 std::vector<double> matrix_block(bs2);
39 for (double* iter = A->begin(); iter != A->end(); )
40 {
41 double* iter_backup = iter;
42 for (int i = 0; i < bs2; ++i)
43 matrix_block[i] = *(iter++);
44
45 cholesky_invert_inplace(matrix_block.data(), blocksize);
46
47 iter = iter_backup;
48 for (int i = 0; i < bs2; ++i)
49 if (std::isfinite(matrix_block[i]))
50 *(iter++) = matrix_block[i];
51 else
52 *(iter++) = 0.0;
53 }
54 }
55
56 /*
57 * Inverts a matrix with 3x3 bocks on its diagonal. All other entries
58 * must be zero. Reading blocks is thus very efficient.
59 */
60 void
61 invert_block_matrix_3x3_inplace (SparseMatrix<double>* A)
62 {
63 if (A->num_rows() != A->num_cols())
64 throw std::invalid_argument("Block matrix must be square");
65 if (A->num_non_zero() != A->num_rows() * 3)
66 throw std::invalid_argument("Invalid number of non-zeros");
67
68 for (double* iter = A->begin(); iter != A->end(); )
69 {
70 double* iter_backup = iter;
72 for (int i = 0; i < 9; ++i)
73 rot[i] = *(iter++);
74
75 double det = math::matrix_determinant(rot);
76 if (MATH_DOUBLE_EQ(det, 0.0))
77 continue;
78
79 rot = math::matrix_inverse(rot, det);
80 iter = iter_backup;
81 for (int i = 0; i < 9; ++i)
82 *(iter++) = rot[i];
83 }
84 }
85
86 /*
87 * Computes for a given matrix A the square matrix A^T * A for the
88 * case that block columns of A only need to be multiplied with itself.
89 * Becase the resulting matrix is symmetric, only about half the work
90 * needs to be performed.
91 */
92 void
93 matrix_block_column_multiply (SparseMatrix<double> const& A,
94 std::size_t block_size, SparseMatrix<double>* B)
95 {
96 SparseMatrix<double>::Triplets triplets;
97 triplets.reserve(A.num_cols() * block_size);
98 for (std::size_t block = 0; block < A.num_cols(); block += block_size)
99 {
100 std::vector<DenseVector<double>> columns(block_size);
101 for (std::size_t col = 0; col < block_size; ++col)
102 A.column_nonzeros(block + col, &columns[col]);
103 for (std::size_t col = 0; col < block_size; ++col)
104 {
105 double dot = columns[col].dot(columns[col]);
106 triplets.emplace_back(block + col, block + col, dot);
107 for (std::size_t row = col + 1; row < block_size; ++row)
108 {
109 dot = columns[col].dot(columns[row]);
110 triplets.emplace_back(block + row, block + col, dot);
111 triplets.emplace_back(block + col, block + row, dot);
112 }
113 }
114 }
115 B->allocate(A.num_cols(), A.num_cols());
116 B->set_from_triplets(triplets);
117 }
118}
119
120LinearSolver::Status
121LinearSolver::solve (SparseMatrixType const& jac_cams,
122 SparseMatrixType const& jac_points,
123 DenseVectorType const& vector_f,
124 DenseVectorType* delta_x)
125{
126 bool const has_jac_cams = jac_cams.num_rows() > 0;
127 bool const has_jac_points = jac_points.num_rows() > 0;
128
129 /* Select solver based on bundle adjustment mode. */
130 if (has_jac_cams && has_jac_points)
131 return this->solve_schur(jac_cams, jac_points, vector_f, delta_x);
132 else if (has_jac_cams && !has_jac_points)
133 return this->solve(jac_cams, vector_f, delta_x, 0);
134 else if (!has_jac_cams && has_jac_points)
135 return this->solve(jac_points, vector_f, delta_x, 3);
136 else
137 throw std::invalid_argument("No Jacobian given");
138}
139
141LinearSolver::solve_schur (SparseMatrixType const& jac_cams,
142 SparseMatrixType const& jac_points,
143 DenseVectorType const& values, DenseVectorType* delta_x)
144{
145 /*
146 * Jacobian J = [ Jc Jp ] with Jc camera block, Jp point block.
147 * Hessian H = [ B E; E^T C ] = J^T J = [ Jc^T; Jp^T ] * [ Jc Jp ]
148 * with B = Jc^T * Jc and E = Jc^T * Jp and C = Jp^T Jp
149 */
150 DenseVectorType const& F = values;
151 SparseMatrixType const& Jc = jac_cams;
152 SparseMatrixType const& Jp = jac_points;
153 SparseMatrixType JcT = Jc.transpose();
154 SparseMatrixType JpT = Jp.transpose();
155
156 /* Compute the blocks of the Hessian. */
157 SparseMatrixType B, C;
158 /* Jc^T * Jc */
159 matrix_block_column_multiply(Jc, this->opts.camera_block_dim, &B);
160 /* Jp^T * Jp */
161 matrix_block_column_multiply(Jp, 3, &C);
162 SparseMatrixType E = JcT.multiply(Jp);
163
164 /* Assemble two values vectors. */
165 DenseVectorType v = JcT.multiply(F);
166 DenseVectorType w = JpT.multiply(F);
167 v.negate_self();
168 w.negate_self();
169
170 /* Save diagonal for computing predicted error decrease */
171 SparseMatrixType B_diag = B.diagonal_matrix();
172 SparseMatrixType C_diag = C.diagonal_matrix();
173
174 /* Add regularization to C and B. */
175 C.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius);
176 B.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius);
177
178 /* Invert C matrix. */
179 invert_block_matrix_3x3_inplace(&C);
180
181 /* Compute the Schur complement matrix S. */
182 SparseMatrixType ET = E.transpose();
183 SparseMatrixType S = B.subtract(E.multiply(C).multiply(ET));
184 DenseVectorType rhs = v.subtract(E.multiply(C.multiply(w)));
185
186 /* Compute pre-conditioner for linear system. */
187 //SparseMatrixType precond = S.diagonal_matrix();
188 //precond.cwise_invert();
189 SparseMatrixType precond = B;
190 invert_block_matrix_NxN_inplace(&precond, this->opts.camera_block_dim);
191
192 /* Solve linear system. */
193 DenseVectorType delta_y(Jc.num_cols());
194 typedef sfm::ba::ConjugateGradient<double> CGSolver;
195 CGSolver::Options cg_opts;
196 cg_opts.max_iterations = this->opts.cg_max_iterations;
197 cg_opts.tolerance = 1e-20;
198 CGSolver solver(cg_opts);
199 CGSolver::Status cg_status;
200 cg_status = solver.solve(S, rhs, &delta_y, &precond);
201
202 Status status;
203 status.num_cg_iterations = cg_status.num_iterations;
204 switch (cg_status.info)
205 {
206 case CGSolver::CG_CONVERGENCE:
207 status.success = true;
208 break;
209 case CGSolver::CG_MAX_ITERATIONS:
210 status.success = true;
211 break;
212 case CGSolver::CG_INVALID_INPUT:
213 std::cout << "BA: CG failed (invalid input)" << std::endl;
214 status.success = false;
215 return status;
216 default:
217 break;
218 }
219
220 /* Substitute back to obtain delta z. */
221 DenseVectorType delta_z = C.multiply(w.subtract(ET.multiply(delta_y)));
222
223 /* Fill output vector. */
224 std::size_t const jac_cam_cols = Jc.num_cols();
225 std::size_t const jac_point_cols = Jp.num_cols();
226 std::size_t const jac_cols = jac_cam_cols + jac_point_cols;
227
228 if (delta_x->size() != jac_cols)
229 delta_x->resize(jac_cols, 0.0);
230 for (std::size_t i = 0; i < jac_cam_cols; ++i)
231 delta_x->at(i) = delta_y[i];
232 for (std::size_t i = 0; i < jac_point_cols; ++i)
233 delta_x->at(jac_cam_cols + i) = delta_z[i];
234
235 /* Compute predicted error decrease */
236 status.predicted_error_decrease = 0.0;
237 status.predicted_error_decrease += delta_y.dot(B_diag.multiply(
238 delta_y).multiply(1.0 / this->opts.trust_region_radius).add(v));
239 status.predicted_error_decrease += delta_z.dot(C_diag.multiply(
240 delta_z).multiply(1.0 / this->opts.trust_region_radius).add(w));
241
242 return status;
243}
244
245LinearSolver::Status
246LinearSolver::solve (SparseMatrixType const& J,
247 DenseVectorType const& vector_f,
248 DenseVectorType* delta_x,
249 std::size_t block_size)
250{
251 DenseVectorType const& F = vector_f;
252 SparseMatrixType Jt = J.transpose();
253 SparseMatrixType H = Jt.multiply(J);
254 SparseMatrixType H_diag = H.diagonal_matrix();
255
256 /* Compute RHS. */
257 DenseVectorType g = Jt.multiply(F);
258 g.negate_self();
259
260 /* Add regularization to H. */
261 H.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius);
262
263 Status status;
264 if (block_size == 0)
265 {
266 /* Use preconditioned CG using the diagonal of H. */
267 SparseMatrixType precond = H.diagonal_matrix();
268 precond.cwise_invert();
269
270 typedef sfm::ba::ConjugateGradient<double> CGSolver;
271 CGSolver::Options cg_opts;
272 cg_opts.max_iterations = this->opts.cg_max_iterations;
273 cg_opts.tolerance = 1e-20;
274 CGSolver solver(cg_opts);
275 CGSolver::Status cg_status;
276 cg_status = solver.solve(H, g, delta_x, &precond);
277 status.num_cg_iterations = cg_status.num_iterations;
278
279 switch (cg_status.info)
280 {
281 case CGSolver::CG_CONVERGENCE:
282 status.success = true;
283 break;
284 case CGSolver::CG_MAX_ITERATIONS:
285 status.success = true;
286 break;
287 case CGSolver::CG_INVALID_INPUT:
288 std::cout << "BA: CG failed (invalid input)" << std::endl;
289 status.success = false;
290 return status;
291 default:
292 break;
293 }
294 }
295 else if (block_size == 3)
296 {
297 /* Invert blocks of H directly */
298 invert_block_matrix_3x3_inplace(&H);
299 *delta_x = H.multiply(g);
300 status.success = true;
301 status.num_cg_iterations = 0;
302 }
303 else
304 {
305 status.success = false;
306 throw std::invalid_argument("Unsupported block_size in linear solver");
307 }
308
309 status.predicted_error_decrease = delta_x->dot(H_diag.multiply(
310 *delta_x).multiply(1.0 / this->opts.trust_region_radius).add(g));
311
312 return status;
313}
314
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Sparse matrix class in Yale format for column-major matrices.
std::size_t num_rows(void) const
#define MATH_DOUBLE_EQ(x, v)
Definition defines.h:99
Matrix< T, N, N > matrix_inverse(Matrix< T, N, N > const &mat)
Calculates the inverse of the given matrix.
T matrix_determinant(Matrix< T, N, N > const &mat)
Calculates the determinant of the given matrix.
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