Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 1 | // Ceres Solver - A fast non-linear least squares minimizer |
Sameer Agarwal | 5a30cae | 2023-09-19 15:29:34 -0700 | [diff] [blame] | 2 | // Copyright 2023 Google Inc. All rights reserved. |
Keir Mierle | 7492b0d | 2015-03-17 22:30:16 -0700 | [diff] [blame] | 3 | // http://ceres-solver.org/ |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 4 | // |
| 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 Agarwal | ae65219 | 2022-02-02 13:17:29 -0800 | [diff] [blame] | 34 | #include <memory> |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 35 | #include <set> |
| 36 | #include <utility> |
| 37 | #include <vector> |
Keir Mierle | 7c4e8a4 | 2018-03-30 16:16:59 -0700 | [diff] [blame] | 38 | |
Sameer Agarwal | e712ce1 | 2015-04-07 14:11:10 -0700 | [diff] [blame] | 39 | #include "Eigen/Dense" |
Sameer Agarwal | 0a53aa9 | 2024-07-07 10:24:18 -0700 | [diff] [blame] | 40 | #include "absl/log/check.h" |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 41 | #include "ceres/compressed_row_sparse_matrix.h" |
Sergiu Deitsch | f90833f | 2022-02-07 23:43:19 +0100 | [diff] [blame] | 42 | #include "ceres/internal/export.h" |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 43 | #include "ceres/parallel_for.h" |
Dmitriy Korchemkin | 54ad3dd | 2022-12-19 18:24:54 +0300 | [diff] [blame] | 44 | #include "ceres/parallel_vector_ops.h" |
Sameer Agarwal | 4ad9149 | 2014-09-24 23:54:18 -0700 | [diff] [blame] | 45 | #include "ceres/stl_util.h" |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 46 | #include "ceres/types.h" |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 47 | |
Sameer Agarwal | caf614a | 2022-04-21 17:41:10 -0700 | [diff] [blame] | 48 | namespace ceres::internal { |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 49 | |
| 50 | BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 51 | 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 Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 55 | 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 Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 59 | } |
| 60 | } |
| 61 | |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 62 | CellInfo* 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 Agarwal | ae65219 | 2022-02-02 13:17:29 -0800 | [diff] [blame] | 69 | return nullptr; |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 70 | } |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 71 | |
| 72 | auto& blocks = m_->row_blocks(); |
| 73 | const int stride = blocks[row_block_id].size; |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 74 | |
| 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 Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 80 | return &layout_[row_block_id]; |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 81 | } |
| 82 | |
| 83 | // Assume that the user does not hold any locks on any cell blocks |
| 84 | // when they are calling SetZero. |
| 85 | void BlockRandomAccessDiagonalMatrix::SetZero() { |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 86 | ParallelSetZero( |
| 87 | context_, num_threads_, m_->mutable_values(), m_->num_nonzeros()); |
Sameer Agarwal | 0e2743e | 2013-10-23 14:51:07 -0700 | [diff] [blame] | 88 | } |
| 89 | |
Sameer Agarwal | 4ad9149 | 2014-09-24 23:54:18 -0700 | [diff] [blame] | 90 | void BlockRandomAccessDiagonalMatrix::Invert() { |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 91 | 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 Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 94 | auto& cell_info = layout_[i]; |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 95 | auto& block = blocks[i]; |
Sameer Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 96 | MatrixRef b(cell_info.values, block.size, block.size); |
Sameer Agarwal | f86a3bd | 2022-08-29 21:52:30 -0700 | [diff] [blame] | 97 | b = b.selfadjointView<Eigen::Upper>().llt().solve( |
| 98 | Matrix::Identity(block.size, block.size)); |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 99 | }); |
Sameer Agarwal | 4ad9149 | 2014-09-24 23:54:18 -0700 | [diff] [blame] | 100 | } |
| 101 | |
Sameer Agarwal | 0489964 | 2022-08-10 09:55:43 -0700 | [diff] [blame] | 102 | void BlockRandomAccessDiagonalMatrix::RightMultiplyAndAccumulate( |
| 103 | const double* x, double* y) const { |
Sameer Agarwal | 94712db | 2018-08-27 07:12:43 -0700 | [diff] [blame] | 104 | CHECK(x != nullptr); |
| 105 | CHECK(y != nullptr); |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 106 | 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 Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 110 | auto& cell_info = layout_[i]; |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 111 | auto& block = blocks[i]; |
Sameer Agarwal | 6a74af2 | 2024-05-22 09:57:06 -0700 | [diff] [blame] | 112 | ConstMatrixRef b(cell_info.values, block.size, block.size); |
Sameer Agarwal | 19ab2c1 | 2022-12-21 16:01:36 -0800 | [diff] [blame] | 113 | VectorRef(y + block.position, block.size).noalias() += |
| 114 | b * ConstVectorRef(x + block.position, block.size); |
| 115 | }); |
Sameer Agarwal | 4ad9149 | 2014-09-24 23:54:18 -0700 | [diff] [blame] | 116 | } |
| 117 | |
Sameer Agarwal | caf614a | 2022-04-21 17:41:10 -0700 | [diff] [blame] | 118 | } // namespace ceres::internal |