179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2014 Google Inc. All rights reserved. 379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// http://code.google.com/p/ceres-solver/ 479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Redistribution and use in source and binary forms, with or without 679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// modification, are permitted provided that the following conditions are met: 779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions of source code must retain the above copyright notice, 979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer. 1079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions in binary form must reproduce the above copyright notice, 1179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer in the documentation 1279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// and/or other materials provided with the distribution. 1379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Neither the name of Google Inc. nor the names of its contributors may be 1479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// used to endorse or promote products derived from this software without 1579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// specific prior written permission. 1679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 1779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 1879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 2079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 2179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 2279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 2379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 2479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 2579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 2679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 2779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// POSSIBILITY OF SUCH DAMAGE. 2879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 2979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Author: richie.stebbing@gmail.com (Richard Stebbing) 3079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <cstring> 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/dynamic_compressed_row_sparse_matrix.h" 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace ceres { 3579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace internal { 3679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezDynamicCompressedRowSparseMatrix::DynamicCompressedRowSparseMatrix( 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_rows, 3979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_cols, 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int initial_max_num_nonzeros) 4179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : CompressedRowSparseMatrix(num_rows, 4279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_cols, 4379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez initial_max_num_nonzeros) { 4479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_cols_.resize(num_rows); 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_values_.resize(num_rows); 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid DynamicCompressedRowSparseMatrix::InsertEntry(int row, 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int col, 5079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double& value) { 5179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(row, 0); 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_LT(row, num_rows()); 5379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(col, 0); 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_LT(col, num_cols()); 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_cols_[row].push_back(col); 5679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_values_[row].push_back(value); 5779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 5879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid DynamicCompressedRowSparseMatrix::ClearRows(int row_start, 6079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_rows) { 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int r = 0; r < num_rows; ++r) { 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int i = row_start + r; 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(i, 0); 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_LT(i, this->num_rows()); 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_cols_[i].resize(0); 6679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_values_[i].resize(0); 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid DynamicCompressedRowSparseMatrix::Finalize(int num_additional_elements) { 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // `num_additional_elements` is provided as an argument so that additional 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // storage can be reserved when it is known by the finalizer. 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(num_additional_elements, 0); 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Count the number of non-zeros and resize `cols_` and `values_`. 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_jacobian_nonzeros = 0; 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < dynamic_cols_.size(); ++i) { 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_jacobian_nonzeros += dynamic_cols_[i].size(); 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez SetMaxNumNonZeros(num_jacobian_nonzeros + num_additional_elements); 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Flatten `dynamic_cols_` into `cols_` and `dynamic_values_` 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // into `values_`. 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int index_into_values_and_cols = 0; 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_rows(); ++i) { 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez mutable_rows()[i] = index_into_values_and_cols; 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_nonzero_columns = dynamic_cols_[i].size(); 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (num_nonzero_columns > 0) { 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez memcpy(mutable_cols() + index_into_values_and_cols, 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &dynamic_cols_[i][0], 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_cols_[i].size() * sizeof(dynamic_cols_[0][0])); 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez memcpy(mutable_values() + index_into_values_and_cols, 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &dynamic_values_[i][0], 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez dynamic_values_[i].size() * sizeof(dynamic_values_[0][0])); 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez index_into_values_and_cols += dynamic_cols_[i].size(); 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez mutable_rows()[num_rows()] = index_into_values_and_cols; 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(index_into_values_and_cols, num_jacobian_nonzeros) 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Ceres bug: final index into values_ and cols_ should be equal to " 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "the number of jacobian nonzeros. Please contact the developers!"; 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace internal 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace ceres 108