internal/ceres/dynamic_compressed_row_sparse_matrix.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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@gmail.com (Richard Stebbing)
  30
  31 #include <cstring>
  32 #include "ceres/dynamic_compressed_row_sparse_matrix.h"
  33
  34 namespace ceres {
  35 namespace internal {
  36
  37 DynamicCompressedRowSparseMatrix::DynamicCompressedRowSparseMatrix(
  38   int num_rows,
  39   int num_cols,
  40   int initial_max_num_nonzeros)
  41     : CompressedRowSparseMatrix(num_rows,
  42                                 num_cols,
  43                                 initial_max_num_nonzeros) {
  44     dynamic_cols_.resize(num_rows);
  45     dynamic_values_.resize(num_rows);
  46   }
  47
  48 void DynamicCompressedRowSparseMatrix::InsertEntry(int row,
  49                                                    int col,
  50                                                    const double& value) {
  51   CHECK_GE(row, 0);
  52   CHECK_LT(row, num_rows());
  53   CHECK_GE(col, 0);
  54   CHECK_LT(col, num_cols());
  55   dynamic_cols_[row].push_back(col);
  56   dynamic_values_[row].push_back(value);
  57 }
  58
  59 void DynamicCompressedRowSparseMatrix::ClearRows(int row_start,
  60                                                  int num_rows) {
  61   for (int r = 0; r < num_rows; ++r) {
  62     const int i = row_start + r;
  63     CHECK_GE(i, 0);
  64     CHECK_LT(i, this->num_rows());
  65     dynamic_cols_[i].resize(0);
  66     dynamic_values_[i].resize(0);
  67   }
  68 }
  69
  70 void DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) {
  71   // `num_additional_elements` is provided as an argument so that additional
  72   // storage can be reserved when it is known by the finalizer.
  73   CHECK_GE(num_additional_elements, 0);
  74
  75   // Count the number of non-zeros and resize `cols_` and `values_`.
  76   int num_jacobian_nonzeros = 0;
  77   for (int i = 0; i < dynamic_cols_.size(); ++i) {
  78     num_jacobian_nonzeros += dynamic_cols_[i].size();
  79   }
  80
  81   SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements);
  82
  83   // Flatten `dynamic_cols_` into `cols_` and `dynamic_values_`
  84   // into `values_`.
  85   int index_into_values_and_cols = 0;
  86   for (int i = 0; i < num_rows(); ++i) {
  87     mutable_rows()[i] = index_into_values_and_cols;
  88     const int num_nonzero_columns = dynamic_cols_[i].size();
  89     if (num_nonzero_columns > 0) {
  90       memcpy(mutable_cols() + index_into_values_and_cols,
  91              &dynamic_cols_[i][0],
  92              dynamic_cols_[i].size() * sizeof(dynamic_cols_[0][0]));
  93       memcpy(mutable_values() + index_into_values_and_cols,
  94              &dynamic_values_[i][0],
  95              dynamic_values_[i].size() * sizeof(dynamic_values_[0][0]));
  96       index_into_values_and_cols += dynamic_cols_[i].size();
  97     }
  98   }
  99   mutable_rows()[num_rows()] = index_into_values_and_cols;
 100
 101   CHECK_EQ(index_into_values_and_cols, num_jacobian_nonzeros)
 102     << "Ceres bug: final index into values_ and cols_ should be equal to "
 103     << "the number of jacobian nonzeros. Please contact the developers!";
 104 }
 105
 106 }  // namespace internal
 107 }  // namespace ceres