11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2013 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/incomplete_lq_factorization.h" 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <utility> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cmath> 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/compressed_row_sparse_matrix.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/eigen.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/port.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Normalize a row and return it's norm. 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double NormalizeRow(const int row, CompressedRowSparseMatrix* matrix) { 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_begin = matrix->rows()[row]; 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_end = matrix->rows()[row + 1]; 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* values = matrix->mutable_values(); 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double norm = 0.0; 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = row_begin; i < row_end; ++i) { 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling norm += values[i] * values[i]; 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling norm = sqrt(norm); 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double inverse_norm = 1.0 / norm; 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = row_begin; i < row_end; ++i) { 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values[i] *= inverse_norm; 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return norm; 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Compute a(row_a,:) * b(row_b, :)' 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double RowDotProduct(const CompressedRowSparseMatrix& a, 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_a, 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const CompressedRowSparseMatrix& b, 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_b) { 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* a_rows = a.rows(); 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* a_cols = a.cols(); 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* a_values = a.values(); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* b_rows = b.rows(); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* b_cols = b.cols(); 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* b_values = b.values(); 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_a_end = a_rows[row_a + 1]; 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_b_end = b_rows[row_b + 1]; 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int idx_a = a_rows[row_a]; 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int idx_b = b_rows[row_b]; 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double dot_product = 0.0; 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling while (idx_a < row_a_end && idx_b < row_b_end) { 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (a_cols[idx_a] == b_cols[idx_b]) { 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling dot_product += a_values[idx_a++] * b_values[idx_b++]; 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling while (a_cols[idx_a] < b_cols[idx_b] && idx_a < row_a_end) { 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++idx_a; 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling while (a_cols[idx_a] > b_cols[idx_b] && idx_b < row_b_end) { 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++idx_b; 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return dot_product; 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct SecondGreaterThan { 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const pair<int, double>& lhs, 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const pair<int, double>& rhs) const { 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return (fabs(lhs.second) > fabs(rhs.second)); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// In the row vector dense_row(0:num_cols), drop values smaller than 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the max_value * drop_tolerance. Of the remaining non-zero values, 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// choose at most level_of_fill values and then add the resulting row 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector to matrix. 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid DropEntriesAndAddRow(const Vector& dense_row, 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_entries, 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int level_of_fill, 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double drop_tolerance, 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<pair<int, double> >* scratch, 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix* matrix) { 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* rows = matrix->mutable_rows(); 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* cols = matrix->mutable_cols(); 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* values = matrix->mutable_values(); 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_nonzeros = rows[matrix->num_rows()]; 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (num_entries == 0) { 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->set_num_rows(matrix->num_rows() + 1); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows[matrix->num_rows()] = num_nonzeros; 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double max_value = dense_row.head(num_entries).cwiseAbs().maxCoeff(); 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double threshold = drop_tolerance * max_value; 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int scratch_count = 0; 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_entries; ++i) { 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (fabs(dense_row[i]) > threshold) { 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling pair<int, double>& entry = (*scratch)[scratch_count]; 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling entry.first = i; 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling entry.second = dense_row[i]; 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++scratch_count; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (scratch_count > level_of_fill) { 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling nth_element(scratch->begin(), 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scratch->begin() + level_of_fill, 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scratch->begin() + scratch_count, 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling SecondGreaterThan()); 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scratch_count = level_of_fill; 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sort(scratch->begin(), scratch->begin() + scratch_count); 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < scratch_count; ++i) { 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const pair<int, double>& entry = (*scratch)[i]; 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols[num_nonzeros] = entry.first; 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values[num_nonzeros] = entry.second; 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++num_nonzeros; 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->set_num_rows(matrix->num_rows() + 1); 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows[matrix->num_rows()] = num_nonzeros; 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Saad's Incomplete LQ factorization algorithm. 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingCompressedRowSparseMatrix* IncompleteLQFactorization( 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const CompressedRowSparseMatrix& matrix, 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int l_level_of_fill, 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double l_drop_tolerance, 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int q_level_of_fill, 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double q_drop_tolerance) { 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_rows = matrix.num_rows(); 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_cols = matrix.num_cols(); 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* rows = matrix.rows(); 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* cols = matrix.cols(); 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix.values(); 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix* l = 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new CompressedRowSparseMatrix(num_rows, 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_rows, 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_level_of_fill * num_rows); 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l->set_num_rows(0); 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix q(num_rows, num_cols, q_level_of_fill * num_rows); 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q.set_num_rows(0); 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* l_rows = l->mutable_rows(); 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* l_cols = l->mutable_cols(); 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* l_values = l->mutable_values(); 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* q_rows = q.mutable_rows(); 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* q_cols = q.mutable_cols(); 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* q_values = q.mutable_values(); 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector l_i(num_rows); 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector q_i(num_cols); 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<pair<int, double> > scratch(num_cols); 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_rows; ++i) { 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // l_i = q * matrix(i,:)'); 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_i.setZero(); 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < i; ++j) { 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_i(j) = RowDotProduct(matrix, i, q, j); 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DropEntriesAndAddRow(l_i, 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling i, 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_level_of_fill, 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_drop_tolerance, 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &scratch, 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l); 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // q_i = matrix(i,:) - q(0:i-1,:) * l_i); 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_i.setZero(); 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = rows[i]; idx < rows[i + 1]; ++idx) { 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_i(cols[idx]) = values[idx]; 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = l_rows[i]; j < l_rows[i + 1]; ++j) { 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int r = l_cols[j]; 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double lij = l_values[j]; 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = q_rows[r]; idx < q_rows[r + 1]; ++idx) { 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_i(q_cols[idx]) -= lij * q_values[idx]; 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DropEntriesAndAddRow(q_i, 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_cols, 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_level_of_fill, 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling q_drop_tolerance, 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &scratch, 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &q); 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // lii = |qi| 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_cols[l->num_nonzeros()] = i; 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_values[l->num_nonzeros()] = NormalizeRow(i, &q); 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling l_rows[l->num_rows()] += 1; 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return l; 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 240