diff options
Diffstat (limited to 'internal/ceres/compressed_col_sparse_matrix_utils.cc')
| -rw-r--r-- | internal/ceres/compressed_col_sparse_matrix_utils.cc | 118 |
1 files changed, 118 insertions, 0 deletions
diff --git a/internal/ceres/compressed_col_sparse_matrix_utils.cc b/internal/ceres/compressed_col_sparse_matrix_utils.cc new file mode 100644 index 0000000..b62a6ed --- /dev/null +++ b/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -0,0 +1,118 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_col_sparse_matrix_utils.h" + +#include <vector> +#include <algorithm> +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +void CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, + const int* scalar_cols, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols) { + CHECK_NOTNULL(block_rows)->clear(); + CHECK_NOTNULL(block_cols)->clear(); + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + vector<int> row_block_starts(num_row_blocks); + for (int i = 0, cursor = 0; i < num_row_blocks; ++i) { + row_block_starts[i] = cursor; + cursor += row_blocks[i]; + } + + // This loop extracts the block sparsity of the scalar sparse matrix + // It does so by iterating over the columns, but only considering + // the columns corresponding to the first element of each column + // block. Within each column, the inner loop iterates over the rows, + // and detects the presence of a row block by checking for the + // presence of a non-zero entry corresponding to its first element. + block_cols->push_back(0); + int c = 0; + for (int col_block = 0; col_block < num_col_blocks; ++col_block) { + int column_size = 0; + for (int idx = scalar_cols[c]; idx < scalar_cols[c + 1]; ++idx) { + vector<int>::const_iterator it = lower_bound(row_block_starts.begin(), + row_block_starts.end(), + scalar_rows[idx]); + // Since we are using lower_bound, it will return the row id + // where the row block starts. For everything but the first row + // of the block, where these values will be the same, we can + // skip, as we only need the first row to detect the presence of + // the block. + // + // For rows all but the first row in the last row block, + // lower_bound will return row_block_starts.end(), but those can + // be skipped like the rows in other row blocks too. + if (it == row_block_starts.end() || *it != scalar_rows[idx]) { + continue; + } + + block_rows->push_back(it - row_block_starts.begin()); + ++column_size; + } + block_cols->push_back(block_cols->back() + column_size); + c += col_blocks[col_block]; + } +} + +void BlockOrderingToScalarOrdering(const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering) { + CHECK_EQ(blocks.size(), block_ordering.size()); + const int num_blocks = blocks.size(); + + // block_starts = [0, block1, block1 + block2 ..] + vector<int> block_starts(num_blocks); + for (int i = 0, cursor = 0; i < num_blocks ; ++i) { + block_starts[i] = cursor; + cursor += blocks[i]; + } + + scalar_ordering->resize(block_starts.back() + blocks.back()); + int cursor = 0; + for (int i = 0; i < num_blocks; ++i) { + const int block_id = block_ordering[i]; + const int block_size = blocks[block_id]; + int block_position = block_starts[block_id]; + for (int j = 0; j < block_size; ++j) { + (*scalar_ordering)[cursor++] = block_position++; + } + } +} +} // namespace internal +} // namespace ceres |