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@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
41namespace ceres {
42namespace internal {
43
44// Normalize a row and return it's norm.
45inline 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, :)'
65inline 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
100struct 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
113void 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.
164CompressedRowSparseMatrix* 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