30 invert_block_matrix_NxN_inplace (SparseMatrix<double>* A,
int blocksize)
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");
37 int const bs2 = blocksize * blocksize;
38 std::vector<double> matrix_block(bs2);
39 for (
double* iter = A->begin(); iter != A->end(); )
41 double* iter_backup = iter;
42 for (
int i = 0; i < bs2; ++i)
43 matrix_block[i] = *(iter++);
48 for (
int i = 0; i < bs2; ++i)
49 if (std::isfinite(matrix_block[i]))
50 *(iter++) = matrix_block[i];
61 invert_block_matrix_3x3_inplace (SparseMatrix<double>* A)
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");
68 for (
double* iter = A->begin(); iter != A->end(); )
70 double* iter_backup = iter;
72 for (
int i = 0; i < 9; ++i)
81 for (
int i = 0; i < 9; ++i)
93 matrix_block_column_multiply (SparseMatrix<double>
const& A,
94 std::size_t block_size, SparseMatrix<double>* B)
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)
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)
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)
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);
115 B->allocate(A.num_cols(), A.num_cols());
116 B->set_from_triplets(triplets);
126 bool const has_jac_cams = jac_cams.
num_rows() > 0;
127 bool const has_jac_points = jac_points.
num_rows() > 0;
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);
137 throw std::invalid_argument(
"No Jacobian given");
141LinearSolver::solve_schur (SparseMatrixType
const& jac_cams,
142 SparseMatrixType
const& jac_points,
143 DenseVectorType
const& values, DenseVectorType* delta_x)
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();
157 SparseMatrixType B, C;
159 matrix_block_column_multiply(Jc, this->opts.camera_block_dim, &B);
161 matrix_block_column_multiply(Jp, 3, &C);
162 SparseMatrixType E = JcT.multiply(Jp);
165 DenseVectorType v = JcT.multiply(F);
166 DenseVectorType w = JpT.multiply(F);
171 SparseMatrixType B_diag = B.diagonal_matrix();
172 SparseMatrixType C_diag = C.diagonal_matrix();
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);
179 invert_block_matrix_3x3_inplace(&C);
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)));
189 SparseMatrixType precond = B;
190 invert_block_matrix_NxN_inplace(&precond, this->opts.camera_block_dim);
193 DenseVectorType delta_y(Jc.num_cols());
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);
203 status.num_cg_iterations = cg_status.num_iterations;
204 switch (cg_status.info)
206 case CGSolver::CG_CONVERGENCE:
207 status.success =
true;
209 case CGSolver::CG_MAX_ITERATIONS:
210 status.success =
true;
212 case CGSolver::CG_INVALID_INPUT:
213 std::cout <<
"BA: CG failed (invalid input)" << std::endl;
214 status.success =
false;
221 DenseVectorType delta_z = C.multiply(w.subtract(ET.multiply(delta_y)));
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;
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];
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));
246LinearSolver::solve (SparseMatrixType
const& J,
247 DenseVectorType
const& vector_f,
248 DenseVectorType* delta_x,
249 std::size_t block_size)
251 DenseVectorType
const& F = vector_f;
252 SparseMatrixType Jt = J.transpose();
253 SparseMatrixType H = Jt.multiply(J);
254 SparseMatrixType H_diag = H.diagonal_matrix();
257 DenseVectorType g = Jt.multiply(F);
261 H.mult_diagonal(1.0 + 1.0 / this->opts.trust_region_radius);
267 SparseMatrixType precond = H.diagonal_matrix();
268 precond.cwise_invert();
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;
279 switch (cg_status.info)
281 case CGSolver::CG_CONVERGENCE:
282 status.success =
true;
284 case CGSolver::CG_MAX_ITERATIONS:
285 status.success =
true;
287 case CGSolver::CG_INVALID_INPUT:
288 std::cout <<
"BA: CG failed (invalid input)" << std::endl;
289 status.success =
false;
295 else if (block_size == 3)
298 invert_block_matrix_3x3_inplace(&H);
299 *delta_x = H.multiply(g);
300 status.success =
true;
301 status.num_cg_iterations = 0;
305 status.success =
false;
306 throw std::invalid_argument(
"Unsupported block_size in linear solver");
309 status.predicted_error_decrease = delta_x->dot(H_diag.multiply(
310 *delta_x).multiply(1.0 / this->opts.trust_region_radius).add(g));
Matrix class for arbitrary dimensions and types.
Sparse matrix class in Yale format for column-major matrices.
std::size_t num_rows(void) const
#define MATH_DOUBLE_EQ(x, v)
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.
#define SFM_BA_NAMESPACE_BEGIN
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN
#define SFM_BA_NAMESPACE_END