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 (at) 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 42 namespace ceres { 43 namespace internal { 44 namespace { 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. 56 void 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 123 BlockJacobianWriter::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. 137 BlockEvaluatePreparer* 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 149 SparseMatrix* 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
