blob: b657266a55c15dc2f7e61eebb79740eccdc74001 [file] [log] [blame]
Sameer Agarwal0e2743e2013-10-23 14:51:07 -07001// Ceres Solver - A fast non-linear least squares minimizer
Sameer Agarwal5a30cae2023-09-19 15:29:34 -07002// Copyright 2023 Google Inc. All rights reserved.
Keir Mierle7492b0d2015-03-17 22:30:16 -07003// http://ceres-solver.org/
Sameer Agarwal0e2743e2013-10-23 14:51:07 -07004//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9// this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11// this list of conditions and the following disclaimer in the documentation
12// and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14// used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: sameeragarwal@google.com (Sameer Agarwal)
30
31#include "ceres/block_random_access_diagonal_matrix.h"
32
33#include <algorithm>
Sameer Agarwalae652192022-02-02 13:17:29 -080034#include <memory>
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070035#include <set>
36#include <utility>
37#include <vector>
Keir Mierle7c4e8a42018-03-30 16:16:59 -070038
Sameer Agarwale712ce12015-04-07 14:11:10 -070039#include "Eigen/Dense"
Sameer Agarwal0a53aa92024-07-07 10:24:18 -070040#include "absl/log/check.h"
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080041#include "ceres/compressed_row_sparse_matrix.h"
Sergiu Deitschf90833f2022-02-07 23:43:19 +010042#include "ceres/internal/export.h"
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080043#include "ceres/parallel_for.h"
Dmitriy Korchemkin54ad3dd2022-12-19 18:24:54 +030044#include "ceres/parallel_vector_ops.h"
Sameer Agarwal4ad91492014-09-24 23:54:18 -070045#include "ceres/stl_util.h"
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070046#include "ceres/types.h"
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070047
Sameer Agarwalcaf614a2022-04-21 17:41:10 -070048namespace ceres::internal {
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070049
50BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix(
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080051 const std::vector<Block>& blocks, ContextImpl* context, int num_threads)
52 : context_(context), num_threads_(num_threads) {
53 m_ = CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(nullptr, blocks);
54 double* values = m_->mutable_values();
Sameer Agarwal6a74af22024-05-22 09:57:06 -070055 layout_ = std::make_unique<CellInfo[]>(blocks.size());
56 for (int i = 0; i < blocks.size(); ++i) {
57 layout_[i].values = values;
58 values += blocks[i].size * blocks[i].size;
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070059 }
60}
61
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070062CellInfo* BlockRandomAccessDiagonalMatrix::GetCell(int row_block_id,
63 int col_block_id,
64 int* row,
65 int* col,
66 int* row_stride,
67 int* col_stride) {
68 if (row_block_id != col_block_id) {
Sameer Agarwalae652192022-02-02 13:17:29 -080069 return nullptr;
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070070 }
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080071
72 auto& blocks = m_->row_blocks();
73 const int stride = blocks[row_block_id].size;
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070074
75 // Each cell is stored contiguously as its own little dense matrix.
76 *row = 0;
77 *col = 0;
78 *row_stride = stride;
79 *col_stride = stride;
Sameer Agarwal6a74af22024-05-22 09:57:06 -070080 return &layout_[row_block_id];
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070081}
82
83// Assume that the user does not hold any locks on any cell blocks
84// when they are calling SetZero.
85void BlockRandomAccessDiagonalMatrix::SetZero() {
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080086 ParallelSetZero(
87 context_, num_threads_, m_->mutable_values(), m_->num_nonzeros());
Sameer Agarwal0e2743e2013-10-23 14:51:07 -070088}
89
Sameer Agarwal4ad91492014-09-24 23:54:18 -070090void BlockRandomAccessDiagonalMatrix::Invert() {
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080091 auto& blocks = m_->row_blocks();
92 const int num_blocks = blocks.size();
93 ParallelFor(context_, 0, num_blocks, num_threads_, [this, blocks](int i) {
Sameer Agarwal6a74af22024-05-22 09:57:06 -070094 auto& cell_info = layout_[i];
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080095 auto& block = blocks[i];
Sameer Agarwal6a74af22024-05-22 09:57:06 -070096 MatrixRef b(cell_info.values, block.size, block.size);
Sameer Agarwalf86a3bd2022-08-29 21:52:30 -070097 b = b.selfadjointView<Eigen::Upper>().llt().solve(
98 Matrix::Identity(block.size, block.size));
Sameer Agarwal19ab2c12022-12-21 16:01:36 -080099 });
Sameer Agarwal4ad91492014-09-24 23:54:18 -0700100}
101
Sameer Agarwal04899642022-08-10 09:55:43 -0700102void BlockRandomAccessDiagonalMatrix::RightMultiplyAndAccumulate(
103 const double* x, double* y) const {
Sameer Agarwal94712db2018-08-27 07:12:43 -0700104 CHECK(x != nullptr);
105 CHECK(y != nullptr);
Sameer Agarwal19ab2c12022-12-21 16:01:36 -0800106 auto& blocks = m_->row_blocks();
107 const int num_blocks = blocks.size();
108 ParallelFor(
109 context_, 0, num_blocks, num_threads_, [this, blocks, x, y](int i) {
Sameer Agarwal6a74af22024-05-22 09:57:06 -0700110 auto& cell_info = layout_[i];
Sameer Agarwal19ab2c12022-12-21 16:01:36 -0800111 auto& block = blocks[i];
Sameer Agarwal6a74af22024-05-22 09:57:06 -0700112 ConstMatrixRef b(cell_info.values, block.size, block.size);
Sameer Agarwal19ab2c12022-12-21 16:01:36 -0800113 VectorRef(y + block.position, block.size).noalias() +=
114 b * ConstVectorRef(x + block.position, block.size);
115 });
Sameer Agarwal4ad91492014-09-24 23:54:18 -0700116}
117
Sameer Agarwalcaf614a2022-04-21 17:41:10 -0700118} // namespace ceres::internal