Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2013 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: sameeragarwal (at) google.com (Sameer Agarwal)
     30 
     31 #include "ceres/incomplete_lq_factorization.h"
     32 
     33 #include <vector>
     34 #include <utility>
     35 #include <cmath>
     36 #include "ceres/compressed_row_sparse_matrix.h"
     37 #include "ceres/internal/eigen.h"
     38 #include "ceres/internal/port.h"
     39 #include "glog/logging.h"
     40 
     41 namespace ceres {
     42 namespace internal {
     43 
     44 // Normalize a row and return it's norm.
     45 inline double NormalizeRow(const int row, CompressedRowSparseMatrix* matrix) {
     46   const int row_begin =  matrix->rows()[row];
     47   const int row_end = matrix->rows()[row + 1];
     48 
     49   double* values = matrix->mutable_values();
     50   double norm = 0.0;
     51   for (int i =  row_begin; i < row_end; ++i) {
     52     norm += values[i] * values[i];
     53   }
     54 
     55   norm = sqrt(norm);
     56   const double inverse_norm = 1.0 / norm;
     57   for (int i = row_begin; i < row_end; ++i) {
     58     values[i] *= inverse_norm;
     59   }
     60 
     61   return norm;
     62 }
     63 
     64 // Compute a(row_a,:) * b(row_b, :)'
     65 inline double RowDotProduct(const CompressedRowSparseMatrix& a,
     66                             const int row_a,
     67                             const CompressedRowSparseMatrix& b,
     68                             const int row_b) {
     69   const int* a_rows = a.rows();
     70   const int* a_cols = a.cols();
     71   const double* a_values = a.values();
     72 
     73   const int* b_rows = b.rows();
     74   const int* b_cols = b.cols();
     75   const double* b_values = b.values();
     76 
     77   const int row_a_end = a_rows[row_a + 1];
     78   const int row_b_end = b_rows[row_b + 1];
     79 
     80   int idx_a = a_rows[row_a];
     81   int idx_b = b_rows[row_b];
     82   double dot_product = 0.0;
     83   while (idx_a < row_a_end && idx_b < row_b_end) {
     84     if (a_cols[idx_a] == b_cols[idx_b]) {
     85       dot_product += a_values[idx_a++] * b_values[idx_b++];
     86     }
     87 
     88     while (a_cols[idx_a] < b_cols[idx_b] && idx_a < row_a_end) {
     89       ++idx_a;
     90     }
     91 
     92     while (a_cols[idx_a] > b_cols[idx_b] && idx_b < row_b_end) {
     93       ++idx_b;
     94     }
     95   }
     96 
     97   return dot_product;
     98 }
     99 
    100 struct SecondGreaterThan {
    101  public:
    102   bool operator()(const pair<int, double>& lhs,
    103                   const pair<int, double>& rhs) const {
    104     return (fabs(lhs.second) > fabs(rhs.second));
    105   }
    106 };
    107 
    108 // In the row vector dense_row(0:num_cols), drop values smaller than
    109 // the max_value * drop_tolerance. Of the remaining non-zero values,
    110 // choose at most level_of_fill values and then add the resulting row
    111 // vector to matrix.
    112 
    113 void DropEntriesAndAddRow(const Vector& dense_row,
    114                           const int num_entries,
    115                           const int level_of_fill,
    116                           const double drop_tolerance,
    117                           vector<pair<int, double> >* scratch,
    118                           CompressedRowSparseMatrix* matrix) {
    119   int* rows = matrix->mutable_rows();
    120   int* cols = matrix->mutable_cols();
    121   double* values = matrix->mutable_values();
    122   int num_nonzeros = rows[matrix->num_rows()];
    123 
    124   if (num_entries == 0) {
    125     matrix->set_num_rows(matrix->num_rows() + 1);
    126     rows[matrix->num_rows()] = num_nonzeros;
    127     return;
    128   }
    129 
    130   const double max_value = dense_row.head(num_entries).cwiseAbs().maxCoeff();
    131   const double threshold = drop_tolerance * max_value;
    132 
    133   int scratch_count = 0;
    134   for (int i = 0; i < num_entries; ++i) {
    135     if (fabs(dense_row[i]) > threshold) {
    136       pair<int, double>& entry = (*scratch)[scratch_count];
    137       entry.first = i;
    138       entry.second = dense_row[i];
    139       ++scratch_count;
    140     }
    141   }
    142 
    143   if (scratch_count > level_of_fill) {
    144     nth_element(scratch->begin(),
    145                 scratch->begin() + level_of_fill,
    146                 scratch->begin() + scratch_count,
    147                 SecondGreaterThan());
    148     scratch_count = level_of_fill;
    149     sort(scratch->begin(), scratch->begin() + scratch_count);
    150   }
    151 
    152   for (int i = 0; i < scratch_count; ++i) {
    153     const pair<int, double>& entry = (*scratch)[i];
    154     cols[num_nonzeros] = entry.first;
    155     values[num_nonzeros] = entry.second;
    156     ++num_nonzeros;
    157   }
    158 
    159   matrix->set_num_rows(matrix->num_rows() + 1);
    160   rows[matrix->num_rows()] = num_nonzeros;
    161 }
    162 
    163 // Saad's Incomplete LQ factorization algorithm.
    164 CompressedRowSparseMatrix* IncompleteLQFactorization(
    165     const CompressedRowSparseMatrix& matrix,
    166     const int l_level_of_fill,
    167     const double l_drop_tolerance,
    168     const int q_level_of_fill,
    169     const double q_drop_tolerance) {
    170   const int num_rows = matrix.num_rows();
    171   const int num_cols = matrix.num_cols();
    172   const int* rows = matrix.rows();
    173   const int* cols = matrix.cols();
    174   const double* values = matrix.values();
    175 
    176   CompressedRowSparseMatrix* l =
    177       new CompressedRowSparseMatrix(num_rows,
    178                                     num_rows,
    179                                     l_level_of_fill * num_rows);
    180   l->set_num_rows(0);
    181 
    182   CompressedRowSparseMatrix q(num_rows, num_cols, q_level_of_fill * num_rows);
    183   q.set_num_rows(0);
    184 
    185   int* l_rows = l->mutable_rows();
    186   int* l_cols = l->mutable_cols();
    187   double* l_values = l->mutable_values();
    188 
    189   int* q_rows = q.mutable_rows();
    190   int* q_cols = q.mutable_cols();
    191   double* q_values = q.mutable_values();
    192 
    193   Vector l_i(num_rows);
    194   Vector q_i(num_cols);
    195   vector<pair<int, double> > scratch(num_cols);
    196   for (int i = 0; i < num_rows; ++i) {
    197     // l_i = q * matrix(i,:)');
    198     l_i.setZero();
    199     for (int j = 0; j < i; ++j) {
    200       l_i(j) = RowDotProduct(matrix, i, q, j);
    201     }
    202     DropEntriesAndAddRow(l_i,
    203                          i,
    204                          l_level_of_fill,
    205                          l_drop_tolerance,
    206                          &scratch,
    207                          l);
    208 
    209     // q_i = matrix(i,:) - q(0:i-1,:) * l_i);
    210     q_i.setZero();
    211     for (int idx = rows[i]; idx < rows[i + 1]; ++idx) {
    212       q_i(cols[idx]) = values[idx];
    213     }
    214 
    215     for (int j = l_rows[i]; j < l_rows[i + 1]; ++j) {
    216       const int r = l_cols[j];
    217       const double lij = l_values[j];
    218       for (int idx = q_rows[r]; idx < q_rows[r + 1]; ++idx) {
    219         q_i(q_cols[idx]) -= lij * q_values[idx];
    220       }
    221     }
    222     DropEntriesAndAddRow(q_i,
    223                          num_cols,
    224                          q_level_of_fill,
    225                          q_drop_tolerance,
    226                          &scratch,
    227                          &q);
    228 
    229     // lii = |qi|
    230     l_cols[l->num_nonzeros()] = i;
    231     l_values[l->num_nonzeros()] = NormalizeRow(i, &q);
    232     l_rows[l->num_rows()] += 1;
    233   }
    234 
    235   return l;
    236 }
    237 
    238 }  // namespace internal
    239 }  // namespace ceres
    240