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/compressed_col_sparse_matrix_utils.h" 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <algorithm> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/port.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* scalar_cols, 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& row_blocks, 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& col_blocks, 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* block_rows, 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* block_cols) { 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(block_rows)->clear(); 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(block_cols)->clear(); 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_row_blocks = row_blocks.size(); 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_col_blocks = col_blocks.size(); 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> row_block_starts(num_row_blocks); 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0, cursor = 0; i < num_row_blocks; ++i) { 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling row_block_starts[i] = cursor; 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cursor += row_blocks[i]; 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // This loop extracts the block sparsity of the scalar sparse matrix 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // It does so by iterating over the columns, but only considering 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the columns corresponding to the first element of each column 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // block. Within each column, the inner loop iterates over the rows, 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // and detects the presence of a row block by checking for the 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // presence of a non-zero entry corresponding to its first element. 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_cols->push_back(0); 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int c = 0; 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int col_block = 0; col_block < num_col_blocks; ++col_block) { 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int column_size = 0; 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = scalar_cols[c]; idx < scalar_cols[c + 1]; ++idx) { 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>::const_iterator it = lower_bound(row_block_starts.begin(), 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling row_block_starts.end(), 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scalar_rows[idx]); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Since we are using lower_bound, it will return the row id 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // where the row block starts. For everything but the first row 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // of the block, where these values will be the same, we can 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // skip, as we only need the first row to detect the presence of 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the block. 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // For rows all but the first row in the last row block, 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // lower_bound will return row_block_starts.end(), but those can 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // be skipped like the rows in other row blocks too. 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (it == row_block_starts.end() || *it != scalar_rows[idx]) { 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_rows->push_back(it - row_block_starts.begin()); 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++column_size; 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_cols->push_back(block_cols->back() + column_size); 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling c += col_blocks[col_block]; 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid BlockOrderingToScalarOrdering(const vector<int>& blocks, 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& block_ordering, 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* scalar_ordering) { 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(blocks.size(), block_ordering.size()); 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_blocks = blocks.size(); 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // block_starts = [0, block1, block1 + block2 ..] 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> block_starts(num_blocks); 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0, cursor = 0; i < num_blocks ; ++i) { 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_starts[i] = cursor; 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cursor += blocks[i]; 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scalar_ordering->resize(block_starts.back() + blocks.back()); 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int cursor = 0; 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_blocks; ++i) { 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int block_id = block_ordering[i]; 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int block_size = blocks[block_id]; 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int block_position = block_starts[block_id]; 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < block_size; ++j) { 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*scalar_ordering)[cursor++] = block_position++; 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 119