| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2022 Google Inc. All rights reserved. |
| // http://ceres-solver.org/ |
| // |
| // Redistribution and use in source and binary forms, with or without |
| // modification, are permitted provided that the following conditions are met: |
| // |
| // * Redistributions of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // * Redistributions in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // * Neither the name of Google Inc. nor the names of its contributors may be |
| // used to endorse or promote products derived from this software without |
| // specific prior written permission. |
| // |
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // POSSIBILITY OF SUCH DAMAGE. |
| // |
| // Author: keir@google.com (Keir Mierle) |
| |
| #include "ceres/block_jacobian_writer.h" |
| |
| #include <algorithm> |
| #include <memory> |
| |
| #include "ceres/block_evaluate_preparer.h" |
| #include "ceres/block_sparse_matrix.h" |
| #include "ceres/internal/eigen.h" |
| #include "ceres/internal/export.h" |
| #include "ceres/parameter_block.h" |
| #include "ceres/program.h" |
| #include "ceres/residual_block.h" |
| |
| namespace ceres::internal { |
| |
| using std::vector; |
| |
| namespace { |
| |
| // Given the residual block ordering, build a lookup table to determine which |
| // per-parameter jacobian goes where in the overall program jacobian. |
| // |
| // Since we expect to use a Schur type linear solver to solve the LM step, take |
| // extra care to place the E blocks and the F blocks contiguously. E blocks are |
| // the first num_eliminate_blocks parameter blocks as indicated by the parameter |
| // block ordering. The remaining parameter blocks are the F blocks. |
| // |
| // TODO(keir): Consider if we should use a boolean for each parameter block |
| // instead of num_eliminate_blocks. |
| void BuildJacobianLayout(const Program& program, |
| int num_eliminate_blocks, |
| vector<int*>* jacobian_layout, |
| vector<int>* jacobian_layout_storage) { |
| const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); |
| |
| // Iterate over all the active residual blocks and determine how many E blocks |
| // are there. This will determine where the F blocks start in the jacobian |
| // matrix. Also compute the number of jacobian blocks. |
| int f_block_pos = 0; |
| int num_jacobian_blocks = 0; |
| for (auto* residual_block : residual_blocks) { |
| const int num_residuals = residual_block->NumResiduals(); |
| const int num_parameter_blocks = residual_block->NumParameterBlocks(); |
| |
| // Advance f_block_pos over each E block for this residual. |
| for (int j = 0; j < num_parameter_blocks; ++j) { |
| ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; |
| if (!parameter_block->IsConstant()) { |
| // Only count blocks for active parameters. |
| num_jacobian_blocks++; |
| if (parameter_block->index() < num_eliminate_blocks) { |
| f_block_pos += num_residuals * parameter_block->TangentSize(); |
| } |
| } |
| } |
| } |
| |
| // We now know that the E blocks are laid out starting at zero, and the F |
| // blocks are laid out starting at f_block_pos. Iterate over the residual |
| // blocks again, and this time fill the jacobian_layout array with the |
| // position information. |
| |
| jacobian_layout->resize(program.NumResidualBlocks()); |
| jacobian_layout_storage->resize(num_jacobian_blocks); |
| |
| int e_block_pos = 0; |
| int* jacobian_pos = jacobian_layout_storage->data(); |
| for (int i = 0; i < residual_blocks.size(); ++i) { |
| const ResidualBlock* residual_block = residual_blocks[i]; |
| const int num_residuals = residual_block->NumResiduals(); |
| const int num_parameter_blocks = residual_block->NumParameterBlocks(); |
| |
| (*jacobian_layout)[i] = jacobian_pos; |
| for (int j = 0; j < num_parameter_blocks; ++j) { |
| ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; |
| const int parameter_block_index = parameter_block->index(); |
| if (parameter_block->IsConstant()) { |
| continue; |
| } |
| const int jacobian_block_size = |
| num_residuals * parameter_block->TangentSize(); |
| if (parameter_block_index < num_eliminate_blocks) { |
| *jacobian_pos = e_block_pos; |
| e_block_pos += jacobian_block_size; |
| } else { |
| *jacobian_pos = f_block_pos; |
| f_block_pos += jacobian_block_size; |
| } |
| jacobian_pos++; |
| } |
| } |
| } |
| |
| } // namespace |
| |
| BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, |
| Program* program) |
| : program_(program) { |
| CHECK_GE(options.num_eliminate_blocks, 0) |
| << "num_eliminate_blocks must be greater than 0."; |
| |
| BuildJacobianLayout(*program, |
| options.num_eliminate_blocks, |
| &jacobian_layout_, |
| &jacobian_layout_storage_); |
| } |
| |
| // Create evaluate prepareres that point directly into the final jacobian. This |
| // makes the final Write() a nop. |
| std::unique_ptr<BlockEvaluatePreparer[]> |
| BlockJacobianWriter::CreateEvaluatePreparers(unsigned num_threads) { |
| int max_derivatives_per_residual_block = |
| program_->MaxDerivativesPerResidualBlock(); |
| |
| auto preparers = std::make_unique<BlockEvaluatePreparer[]>(num_threads); |
| for (unsigned i = 0; i < num_threads; i++) { |
| preparers[i].Init(jacobian_layout_.data(), |
| max_derivatives_per_residual_block); |
| } |
| return preparers; |
| } |
| |
| std::unique_ptr<SparseMatrix> BlockJacobianWriter::CreateJacobian() const { |
| auto* bs = new CompressedRowBlockStructure; |
| |
| const vector<ParameterBlock*>& parameter_blocks = |
| program_->parameter_blocks(); |
| |
| // Construct the column blocks. |
| bs->cols.resize(parameter_blocks.size()); |
| for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { |
| CHECK_NE(parameter_blocks[i]->index(), -1); |
| CHECK(!parameter_blocks[i]->IsConstant()); |
| bs->cols[i].size = parameter_blocks[i]->TangentSize(); |
| bs->cols[i].position = cursor; |
| cursor += bs->cols[i].size; |
| } |
| |
| // Construct the cells in each row. |
| const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); |
| int row_block_position = 0; |
| bs->rows.resize(residual_blocks.size()); |
| for (int i = 0; i < residual_blocks.size(); ++i) { |
| const ResidualBlock* residual_block = residual_blocks[i]; |
| CompressedRow* row = &bs->rows[i]; |
| |
| row->block.size = residual_block->NumResiduals(); |
| row->block.position = row_block_position; |
| row_block_position += row->block.size; |
| |
| // Size the row by the number of active parameters in this residual. |
| const int num_parameter_blocks = residual_block->NumParameterBlocks(); |
| int num_active_parameter_blocks = 0; |
| for (int j = 0; j < num_parameter_blocks; ++j) { |
| if (residual_block->parameter_blocks()[j]->index() != -1) { |
| num_active_parameter_blocks++; |
| } |
| } |
| row->cells.resize(num_active_parameter_blocks); |
| |
| // Add layout information for the active parameters in this row. |
| for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { |
| const ParameterBlock* parameter_block = |
| residual_block->parameter_blocks()[j]; |
| if (!parameter_block->IsConstant()) { |
| Cell& cell = row->cells[k]; |
| cell.block_id = parameter_block->index(); |
| cell.position = jacobian_layout_[i][k]; |
| |
| // Only increment k for active parameters, since there is only layout |
| // information for active parameters. |
| k++; |
| } |
| } |
| |
| std::sort(row->cells.begin(), row->cells.end(), CellLessThan); |
| } |
| |
| return std::make_unique<BlockSparseMatrix>(bs); |
| } |
| |
| } // namespace ceres::internal |