Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2014 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: richie.stebbing (at) gmail.com (Richard Stebbing)
     30 
     31 #include "ceres/compressed_row_jacobian_writer.h"
     32 #include "ceres/dynamic_compressed_row_jacobian_writer.h"
     33 #include "ceres/casts.h"
     34 #include "ceres/dynamic_compressed_row_sparse_matrix.h"
     35 #include "ceres/parameter_block.h"
     36 #include "ceres/program.h"
     37 #include "ceres/residual_block.h"
     38 
     39 namespace ceres {
     40 namespace internal {
     41 
     42 ScratchEvaluatePreparer*
     43 DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) {
     44   return ScratchEvaluatePreparer::Create(*program_, num_threads);
     45 }
     46 
     47 SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const {
     48   // Initialize `jacobian` with zero number of `max_num_nonzeros`.
     49   const int num_residuals = program_->NumResiduals();
     50   const int num_effective_parameters = program_->NumEffectiveParameters();
     51 
     52   DynamicCompressedRowSparseMatrix* jacobian =
     53       new DynamicCompressedRowSparseMatrix(num_residuals,
     54                                            num_effective_parameters,
     55                                            0);
     56 
     57   CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors(
     58       program_, jacobian);
     59 
     60   return jacobian;
     61 }
     62 
     63 void DynamicCompressedRowJacobianWriter::Write(int residual_id,
     64                                                int residual_offset,
     65                                                double **jacobians,
     66                                                SparseMatrix* base_jacobian) {
     67   DynamicCompressedRowSparseMatrix* jacobian =
     68     down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian);
     69 
     70   // Get the `residual_block` of interest.
     71   const ResidualBlock* residual_block =
     72       program_->residual_blocks()[residual_id];
     73   const int num_residuals = residual_block->NumResiduals();
     74 
     75   vector<pair<int, int> > evaluated_jacobian_blocks;
     76   CompressedRowJacobianWriter::GetOrderedParameterBlocks(
     77     program_, residual_id, &evaluated_jacobian_blocks);
     78 
     79   // `residual_offset` is the residual row in the global jacobian.
     80   // Empty the jacobian rows.
     81   jacobian->ClearRows(residual_offset, num_residuals);
     82 
     83   // Iterate over each parameter block.
     84   for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) {
     85     const ParameterBlock* parameter_block =
     86         program_->parameter_blocks()[evaluated_jacobian_blocks[i].first];
     87     const int parameter_block_jacobian_index =
     88         evaluated_jacobian_blocks[i].second;
     89     const int parameter_block_size = parameter_block->LocalSize();
     90 
     91     // For each parameter block only insert its non-zero entries.
     92     for (int r = 0; r < num_residuals; ++r) {
     93       for (int c = 0; c < parameter_block_size; ++c) {
     94         const double& v = jacobians[parameter_block_jacobian_index][
     95             r * parameter_block_size + c];
     96         // Only insert non-zero entries.
     97         if (v != 0.0) {
     98           jacobian->InsertEntry(
     99             residual_offset + r, parameter_block->delta_offset() + c, v);
    100         }
    101       }
    102     }
    103   }
    104 }
    105 
    106 }  // namespace internal
    107 }  // namespace ceres
    108