triplet_sparse_matrix.cc revision 0ae28bd5885b5daa526898fcf7c323dc2c3e1963
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/matrix_proto.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix() 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : num_rows_(0), 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(0), 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(0), 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(0), 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) {} 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::~TripletSparseMatrix() {} 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix(int num_rows, 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols, 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_num_nonzeros) 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : num_rows_(num_rows), 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(num_cols), 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(max_num_nonzeros), 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(0), 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) { 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // All the sizes should at least be zero 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_rows, 0); 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_cols, 0); 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(max_num_nonzeros, 0); 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : SparseMatrix(), 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_(orig.num_rows_), 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_(orig.num_cols_), 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_(orig.max_num_nonzeros_), 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_(orig.num_nonzeros_), 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_(NULL), 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_(NULL), 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_(NULL) { 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CopyData(orig); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_PROTOCOL_BUFFERS 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(outer_proto.has_triplet_matrix()); 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(proto.has_num_rows()); 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(proto.has_num_cols()); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(proto.rows_size(), proto.cols_size()); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(proto.cols_size(), proto.values_size()); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Initialize the matrix with the appropriate size and capacity. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_ = 0; 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_num_nonzeros(0); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Reserve(proto.num_nonzeros()); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Resize(proto.num_rows(), proto.num_cols()); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_num_nonzeros(proto.num_nonzeros()); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Copy the entries in. 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < proto.num_nonzeros(); ++i) { 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[i] = proto.rows(i); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_[i] = proto.cols(i); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = proto.values(i); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix& TripletSparseMatrix::operator=( 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const TripletSparseMatrix& rhs) { 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = rhs.num_rows_; 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = rhs.num_cols_; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = rhs.num_nonzeros_; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_ = rhs.max_num_nonzeros_; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AllocateMemory(); 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CopyData(rhs); 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool TripletSparseMatrix::AllTripletsWithinBounds() const { 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (cols_[i] < 0) || (cols_[i] >= num_cols_)) 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(num_nonzeros_, new_max_num_nonzeros) 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Reallocation will cause data loss"; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Nothing to do if we have enough space already. 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (new_max_num_nonzeros <= max_num_nonzeros_) 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* new_rows = new int[new_max_num_nonzeros]; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* new_cols = new int[new_max_num_nonzeros]; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* new_values = new double[new_max_num_nonzeros]; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_rows[i] = rows_[i]; 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_cols[i] = cols_[i]; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_values[i] = values_[i]; 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.reset(new_rows); 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.reset(new_cols); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.reset(new_values); 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_num_nonzeros_ = new_max_num_nonzeros; 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::SetZero() { 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = 0; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(num_nonzeros, 0); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(num_nonzeros, max_num_nonzeros_); 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ = num_nonzeros; 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AllocateMemory() { 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.reset(new int[max_num_nonzeros_]); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.reset(new int[max_num_nonzeros_]); 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.reset(new double[max_num_nonzeros_]); 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[i] = orig.rows_[i]; 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_[i] = orig.cols_[i]; 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = orig.values_[i]; 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::RightMultiply(const double* x, double* y) const { 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[rows_[i]] += values_[i]*x[cols_[i]]; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[cols_[i]] += values_[i]*x[rows_[i]]; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::SquaredColumnNorm(double* x) const { 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(x, num_cols_).setZero(); 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x[cols_[i]] += values_[i] * values_[i]; 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ScaleColumns(const double* scale) { 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(scale); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = values_[i] * scale[cols_[i]]; 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->resize(num_rows_, num_cols_); 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->setZero(); 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix& m = *dense_matrix; 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m(rows_[i], cols_[i]) += values_[i]; 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_PROTOCOL_BUFFERS 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong proto->Clear(); 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->set_num_rows(num_rows_); 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->set_num_cols(num_cols_); 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->set_num_nonzeros(num_nonzeros_); 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->add_rows(rows_[i]); 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->add_cols(cols_[i]); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm_proto->add_values(values_[i]); 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(B.num_cols(), num_cols_); 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Reserve(num_nonzeros_ + B.num_nonzeros_); 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < B.num_nonzeros_; ++i) { 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.get()[num_nonzeros_] = B.cols()[i]; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.get()[num_nonzeros_++] = B.values()[i]; 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = num_rows_ + B.num_rows(); 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(B.num_rows(), num_rows_); 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Reserve(num_nonzeros_ + B.num_nonzeros_); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_.get()[num_nonzeros_] = B.rows()[i]; 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_.get()[num_nonzeros_] = B.values()[i]; 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = num_cols_ + B.num_cols(); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = new_num_rows; 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = new_num_cols; 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = new_num_rows; 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = new_num_cols; 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* r_ptr = rows_.get(); 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* c_ptr = cols_.get(); 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* v_ptr = values_.get(); 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int dropped_terms = 0; 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (dropped_terms) { 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong r_ptr[i-dropped_terms] = r_ptr[i]; 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong c_ptr[i-dropped_terms] = c_ptr[i]; 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v_ptr[i-dropped_terms] = v_ptr[i]; 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++dropped_terms; 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_nonzeros_ -= dropped_terms; 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* values, int num_rows) { 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* m = 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new TripletSparseMatrix(num_rows, num_rows, num_rows); 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_rows()[i] = i; 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_cols()[i] = i; 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->mutable_values()[i] = values[i]; 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->set_num_nonzeros(num_rows); 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return m; 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid TripletSparseMatrix::ToTextFile(FILE* file) const { 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(file); 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_nonzeros_; ++i) { 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 308