10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstddef> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix() 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : num_rows_(0), 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(0), 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(0), 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(0), 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) {} 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::~TripletSparseMatrix() {} 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix(int num_rows, 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols, 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_num_nonzeros) 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : num_rows_(num_rows), 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(num_cols), 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(max_num_nonzeros), 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(0), 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) { 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // All the sizes should at least be zero 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_rows, 0); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_cols, 0); 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(max_num_nonzeros, 0); 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : SparseMatrix(), 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_(orig.num_rows_), 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(orig.num_cols_), 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(orig.max_num_nonzeros_), 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(orig.num_nonzeros_), 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) { 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CopyData(orig); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix& TripletSparseMatrix::operator=( 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const TripletSparseMatrix& rhs) { 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = rhs.num_rows_; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = rhs.num_cols_; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = rhs.num_nonzeros_; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_ = rhs.max_num_nonzeros_; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CopyData(rhs); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool TripletSparseMatrix::AllTripletsWithinBounds() const { 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (cols_[i] < 0) || (cols_[i] >= num_cols_)) 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(num_nonzeros_, new_max_num_nonzeros) 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Reallocation will cause data loss"; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Nothing to do if we have enough space already. 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (new_max_num_nonzeros <= max_num_nonzeros_) 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* new_rows = new int[new_max_num_nonzeros]; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* new_cols = new int[new_max_num_nonzeros]; 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* new_values = new double[new_max_num_nonzeros]; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_rows[i] = rows_[i]; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_cols[i] = cols_[i]; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_values[i] = values_[i]; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.reset(new_rows); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.reset(new_cols); 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.reset(new_values); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_ = new_max_num_nonzeros; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::SetZero() { 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = 0; 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_nonzeros, 0); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(num_nonzeros, max_num_nonzeros_); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = num_nonzeros; 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AllocateMemory() { 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.reset(new int[max_num_nonzeros_]); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.reset(new int[max_num_nonzeros_]); 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.reset(new double[max_num_nonzeros_]); 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[i] = orig.rows_[i]; 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_[i] = orig.cols_[i]; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = orig.values_[i]; 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::RightMultiply(const double* x, double* y) const { 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[rows_[i]] += values_[i]*x[cols_[i]]; 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[cols_[i]] += values_[i]*x[rows_[i]]; 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::SquaredColumnNorm(double* x) const { 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(x, num_cols_).setZero(); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x[cols_[i]] += values_[i] * values_[i]; 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ScaleColumns(const double* scale) { 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(scale); 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = values_[i] * scale[cols_[i]]; 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->resize(num_rows_, num_cols_); 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->setZero(); 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix& m = *dense_matrix; 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m(rows_[i], cols_[i]) += values_[i]; 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(B.num_cols(), num_cols_); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Reserve(num_nonzeros_ + B.num_nonzeros_); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < B.num_nonzeros_; ++i) { 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.get()[num_nonzeros_] = B.cols()[i]; 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.get()[num_nonzeros_++] = B.values()[i]; 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = num_rows_ + B.num_rows(); 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(B.num_rows(), num_rows_); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Reserve(num_nonzeros_ + B.num_nonzeros_); 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.get()[num_nonzeros_] = B.rows()[i]; 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.get()[num_nonzeros_] = B.values()[i]; 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = num_cols_ + B.num_cols(); 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = new_num_rows; 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = new_num_cols; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = new_num_rows; 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = new_num_cols; 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* r_ptr = rows_.get(); 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* c_ptr = cols_.get(); 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* v_ptr = values_.get(); 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int dropped_terms = 0; 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (dropped_terms) { 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r_ptr[i-dropped_terms] = r_ptr[i]; 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong c_ptr[i-dropped_terms] = c_ptr[i]; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v_ptr[i-dropped_terms] = v_ptr[i]; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++dropped_terms; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ -= dropped_terms; 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* values, int num_rows) { 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* m = 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new TripletSparseMatrix(num_rows, num_rows, num_rows); 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_rows()[i] = i; 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_cols()[i] = i; 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_values()[i] = values[i]; 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->set_num_nonzeros(num_rows); 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return m; 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ToTextFile(FILE* file) const { 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(file); 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 265