1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 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: keir@google.com (Keir Mierle) 30 31#include "ceres/block_jacobian_writer.h" 32 33#include "ceres/block_evaluate_preparer.h" 34#include "ceres/block_sparse_matrix.h" 35#include "ceres/parameter_block.h" 36#include "ceres/program.h" 37#include "ceres/residual_block.h" 38#include "ceres/internal/eigen.h" 39#include "ceres/internal/port.h" 40#include "ceres/internal/scoped_ptr.h" 41 42namespace ceres { 43namespace internal { 44namespace { 45 46// Given the residual block ordering, build a lookup table to determine which 47// per-parameter jacobian goes where in the overall program jacobian. 48// 49// Since we expect to use a Schur type linear solver to solve the LM step, take 50// extra care to place the E blocks and the F blocks contiguously. E blocks are 51// the first num_eliminate_blocks parameter blocks as indicated by the parameter 52// block ordering. The remaining parameter blocks are the F blocks. 53// 54// TODO(keir): Consider if we should use a boolean for each parameter block 55// instead of num_eliminate_blocks. 56void BuildJacobianLayout(const Program& program, 57 int num_eliminate_blocks, 58 vector<int*>* jacobian_layout, 59 vector<int>* jacobian_layout_storage) { 60 const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); 61 62 // Iterate over all the active residual blocks and determine how many E blocks 63 // are there. This will determine where the F blocks start in the jacobian 64 // matrix. Also compute the number of jacobian blocks. 65 int f_block_pos = 0; 66 int num_jacobian_blocks = 0; 67 for (int i = 0; i < residual_blocks.size(); ++i) { 68 ResidualBlock* residual_block = residual_blocks[i]; 69 const int num_residuals = residual_block->NumResiduals(); 70 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 71 72 // Advance f_block_pos over each E block for this residual. 73 for (int j = 0; j < num_parameter_blocks; ++j) { 74 ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; 75 if (!parameter_block->IsConstant()) { 76 // Only count blocks for active parameters. 77 num_jacobian_blocks++; 78 if (parameter_block->index() < num_eliminate_blocks) { 79 f_block_pos += num_residuals * parameter_block->LocalSize(); 80 } 81 } 82 } 83 } 84 85 // We now know that the E blocks are laid out starting at zero, and the F 86 // blocks are laid out starting at f_block_pos. Iterate over the residual 87 // blocks again, and this time fill the jacobian_layout array with the 88 // position information. 89 90 jacobian_layout->resize(program.NumResidualBlocks()); 91 jacobian_layout_storage->resize(num_jacobian_blocks); 92 93 int e_block_pos = 0; 94 int* jacobian_pos = &(*jacobian_layout_storage)[0]; 95 for (int i = 0; i < residual_blocks.size(); ++i) { 96 const ResidualBlock* residual_block = residual_blocks[i]; 97 const int num_residuals = residual_block->NumResiduals(); 98 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 99 100 (*jacobian_layout)[i] = jacobian_pos; 101 for (int j = 0; j < num_parameter_blocks; ++j) { 102 ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; 103 const int parameter_block_index = parameter_block->index(); 104 if (parameter_block->IsConstant()) { 105 continue; 106 } 107 const int jacobian_block_size = 108 num_residuals * parameter_block->LocalSize(); 109 if (parameter_block_index < num_eliminate_blocks) { 110 *jacobian_pos = e_block_pos; 111 e_block_pos += jacobian_block_size; 112 } else { 113 *jacobian_pos = f_block_pos; 114 f_block_pos += jacobian_block_size; 115 } 116 jacobian_pos++; 117 } 118 } 119} 120 121} // namespace 122 123BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, 124 Program* program) 125 : program_(program) { 126 CHECK_GE(options.num_eliminate_blocks, 0) 127 << "num_eliminate_blocks must be greater than 0."; 128 129 BuildJacobianLayout(*program, 130 options.num_eliminate_blocks, 131 &jacobian_layout_, 132 &jacobian_layout_storage_); 133} 134 135// Create evaluate prepareres that point directly into the final jacobian. This 136// makes the final Write() a nop. 137BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( 138 int num_threads) { 139 int max_derivatives_per_residual_block = 140 program_->MaxDerivativesPerResidualBlock(); 141 142 BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; 143 for (int i = 0; i < num_threads; i++) { 144 preparers[i].Init(&jacobian_layout_[0], max_derivatives_per_residual_block); 145 } 146 return preparers; 147} 148 149SparseMatrix* BlockJacobianWriter::CreateJacobian() const { 150 CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; 151 152 const vector<ParameterBlock*>& parameter_blocks = 153 program_->parameter_blocks(); 154 155 // Construct the column blocks. 156 bs->cols.resize(parameter_blocks.size()); 157 for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { 158 CHECK_NE(parameter_blocks[i]->index(), -1); 159 CHECK(!parameter_blocks[i]->IsConstant()); 160 bs->cols[i].size = parameter_blocks[i]->LocalSize(); 161 bs->cols[i].position = cursor; 162 cursor += bs->cols[i].size; 163 } 164 165 // Construct the cells in each row. 166 const vector<ResidualBlock*>& residual_blocks = 167 program_->residual_blocks(); 168 int row_block_position = 0; 169 bs->rows.resize(residual_blocks.size()); 170 for (int i = 0; i < residual_blocks.size(); ++i) { 171 const ResidualBlock* residual_block = residual_blocks[i]; 172 CompressedRow* row = &bs->rows[i]; 173 174 row->block.size = residual_block->NumResiduals(); 175 row->block.position = row_block_position; 176 row_block_position += row->block.size; 177 178 // Size the row by the number of active parameters in this residual. 179 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 180 int num_active_parameter_blocks = 0; 181 for (int j = 0; j < num_parameter_blocks; ++j) { 182 if (residual_block->parameter_blocks()[j]->index() != -1) { 183 num_active_parameter_blocks++; 184 } 185 } 186 row->cells.resize(num_active_parameter_blocks); 187 188 // Add layout information for the active parameters in this row. 189 for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { 190 const ParameterBlock* parameter_block = 191 residual_block->parameter_blocks()[j]; 192 if (!parameter_block->IsConstant()) { 193 Cell& cell = row->cells[k]; 194 cell.block_id = parameter_block->index(); 195 cell.position = jacobian_layout_[i][k]; 196 197 // Only increment k for active parameters, since there is only layout 198 // information for active parameters. 199 k++; 200 } 201 } 202 203 sort(row->cells.begin(), row->cells.end(), CellLessThan); 204 } 205 206 BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); 207 CHECK_NOTNULL(jacobian); 208 return jacobian; 209} 210 211} // namespace internal 212} // namespace ceres 213