Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
      3 // http://code.google.com/p/ceres-solver/
      4 //
      5 // Redistribution and use in source and binary forms, with or without
      6 // modification, are permitted provided that the following conditions are met:
      7 //
      8 // * Redistributions of source code must retain the above copyright notice,
      9 //   this list of conditions and the following disclaimer.
     10 // * Redistributions in binary form must reproduce the above copyright notice,
     11 //   this list of conditions and the following disclaimer in the documentation
     12 //   and/or other materials provided with the distribution.
     13 // * Neither the name of Google Inc. nor the names of its contributors may be
     14 //   used to endorse or promote products derived from this software without
     15 //   specific prior written permission.
     16 //
     17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
     21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     27 // POSSIBILITY OF SUCH DAMAGE.
     28 //
     29 // Author: sameeragarwal (at) google.com (Sameer Agarwal)
     30 
     31 #include "ceres/triplet_sparse_matrix.h"
     32 
     33 #include <algorithm>
     34 #include <cstddef>
     35 #include "ceres/internal/eigen.h"
     36 #include "ceres/internal/port.h"
     37 #include "ceres/internal/scoped_ptr.h"
     38 #include "ceres/matrix_proto.h"
     39 #include "ceres/types.h"
     40 #include "glog/logging.h"
     41 
     42 namespace ceres {
     43 namespace internal {
     44 
     45 TripletSparseMatrix::TripletSparseMatrix()
     46     : num_rows_(0),
     47       num_cols_(0),
     48       max_num_nonzeros_(0),
     49       num_nonzeros_(0),
     50       rows_(NULL),
     51       cols_(NULL),
     52       values_(NULL) {}
     53 
     54 TripletSparseMatrix::~TripletSparseMatrix() {}
     55 
     56 TripletSparseMatrix::TripletSparseMatrix(int num_rows,
     57                                          int num_cols,
     58                                          int max_num_nonzeros)
     59     : num_rows_(num_rows),
     60       num_cols_(num_cols),
     61       max_num_nonzeros_(max_num_nonzeros),
     62       num_nonzeros_(0),
     63       rows_(NULL),
     64       cols_(NULL),
     65       values_(NULL) {
     66   // All the sizes should at least be zero
     67   CHECK_GE(num_rows, 0);
     68   CHECK_GE(num_cols, 0);
     69   CHECK_GE(max_num_nonzeros, 0);
     70   AllocateMemory();
     71 }
     72 
     73 TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig)
     74     : SparseMatrix(),
     75       num_rows_(orig.num_rows_),
     76       num_cols_(orig.num_cols_),
     77       max_num_nonzeros_(orig.max_num_nonzeros_),
     78       num_nonzeros_(orig.num_nonzeros_),
     79       rows_(NULL),
     80       cols_(NULL),
     81       values_(NULL) {
     82   AllocateMemory();
     83   CopyData(orig);
     84 }
     85 
     86 #ifndef CERES_NO_PROTOCOL_BUFFERS
     87 TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) {
     88   CHECK(outer_proto.has_triplet_matrix());
     89 
     90   const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix();
     91   CHECK(proto.has_num_rows());
     92   CHECK(proto.has_num_cols());
     93   CHECK_EQ(proto.rows_size(), proto.cols_size());
     94   CHECK_EQ(proto.cols_size(), proto.values_size());
     95 
     96   // Initialize the matrix with the appropriate size and capacity.
     97   max_num_nonzeros_ = 0;
     98   set_num_nonzeros(0);
     99   Reserve(proto.num_nonzeros());
    100   Resize(proto.num_rows(), proto.num_cols());
    101   set_num_nonzeros(proto.num_nonzeros());
    102 
    103   // Copy the entries in.
    104   for (int i = 0; i < proto.num_nonzeros(); ++i) {
    105     rows_[i] = proto.rows(i);
    106     cols_[i] = proto.cols(i);
    107     values_[i] = proto.values(i);
    108   }
    109 }
    110 #endif
    111 
    112 TripletSparseMatrix& TripletSparseMatrix::operator=(
    113     const TripletSparseMatrix& rhs) {
    114   num_rows_ = rhs.num_rows_;
    115   num_cols_ = rhs.num_cols_;
    116   num_nonzeros_ = rhs.num_nonzeros_;
    117   max_num_nonzeros_ = rhs.max_num_nonzeros_;
    118   AllocateMemory();
    119   CopyData(rhs);
    120   return *this;
    121 }
    122 
    123 bool TripletSparseMatrix::AllTripletsWithinBounds() const {
    124   for (int i = 0; i < num_nonzeros_; ++i) {
    125     if ((rows_[i] < 0) || (rows_[i] >= num_rows_) ||
    126         (cols_[i] < 0) || (cols_[i] >= num_cols_))
    127       return false;
    128   }
    129   return true;
    130 }
    131 
    132 void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) {
    133   CHECK_LE(num_nonzeros_, new_max_num_nonzeros)
    134       << "Reallocation will cause data loss";
    135 
    136   // Nothing to do if we have enough space already.
    137   if (new_max_num_nonzeros <= max_num_nonzeros_)
    138     return;
    139 
    140   int* new_rows = new int[new_max_num_nonzeros];
    141   int* new_cols = new int[new_max_num_nonzeros];
    142   double* new_values = new double[new_max_num_nonzeros];
    143 
    144   for (int i = 0; i < num_nonzeros_; ++i) {
    145     new_rows[i] = rows_[i];
    146     new_cols[i] = cols_[i];
    147     new_values[i] = values_[i];
    148   }
    149 
    150   rows_.reset(new_rows);
    151   cols_.reset(new_cols);
    152   values_.reset(new_values);
    153 
    154   max_num_nonzeros_ = new_max_num_nonzeros;
    155 }
    156 
    157 void TripletSparseMatrix::SetZero() {
    158   fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0);
    159   num_nonzeros_ = 0;
    160 }
    161 
    162 void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) {
    163   CHECK_GE(num_nonzeros, 0);
    164   CHECK_LE(num_nonzeros, max_num_nonzeros_);
    165   num_nonzeros_ = num_nonzeros;
    166 };
    167 
    168 void TripletSparseMatrix::AllocateMemory() {
    169   rows_.reset(new int[max_num_nonzeros_]);
    170   cols_.reset(new int[max_num_nonzeros_]);
    171   values_.reset(new double[max_num_nonzeros_]);
    172 }
    173 
    174 void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) {
    175   for (int i = 0; i < num_nonzeros_; ++i) {
    176     rows_[i] = orig.rows_[i];
    177     cols_[i] = orig.cols_[i];
    178     values_[i] = orig.values_[i];
    179   }
    180 }
    181 
    182 void TripletSparseMatrix::RightMultiply(const double* x,  double* y) const {
    183   for (int i = 0; i < num_nonzeros_; ++i) {
    184     y[rows_[i]] += values_[i]*x[cols_[i]];
    185   }
    186 }
    187 
    188 void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const {
    189   for (int i = 0; i < num_nonzeros_; ++i) {
    190     y[cols_[i]] += values_[i]*x[rows_[i]];
    191   }
    192 }
    193 
    194 void TripletSparseMatrix::SquaredColumnNorm(double* x) const {
    195   CHECK_NOTNULL(x);
    196   VectorRef(x, num_cols_).setZero();
    197   for (int i = 0; i < num_nonzeros_; ++i) {
    198     x[cols_[i]] += values_[i] * values_[i];
    199   }
    200 }
    201 
    202 void TripletSparseMatrix::ScaleColumns(const double* scale) {
    203   CHECK_NOTNULL(scale);
    204   for (int i = 0; i < num_nonzeros_; ++i) {
    205     values_[i] = values_[i] * scale[cols_[i]];
    206   }
    207 }
    208 
    209 void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
    210   dense_matrix->resize(num_rows_, num_cols_);
    211   dense_matrix->setZero();
    212   Matrix& m = *dense_matrix;
    213   for (int i = 0; i < num_nonzeros_; ++i) {
    214     m(rows_[i], cols_[i]) += values_[i];
    215   }
    216 }
    217 
    218 #ifndef CERES_NO_PROTOCOL_BUFFERS
    219 void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const {
    220   proto->Clear();
    221 
    222   TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix();
    223   tsm_proto->set_num_rows(num_rows_);
    224   tsm_proto->set_num_cols(num_cols_);
    225   tsm_proto->set_num_nonzeros(num_nonzeros_);
    226   for (int i = 0; i < num_nonzeros_; ++i) {
    227     tsm_proto->add_rows(rows_[i]);
    228     tsm_proto->add_cols(cols_[i]);
    229     tsm_proto->add_values(values_[i]);
    230   }
    231 }
    232 #endif
    233 
    234 void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) {
    235   CHECK_EQ(B.num_cols(), num_cols_);
    236   Reserve(num_nonzeros_ + B.num_nonzeros_);
    237   for (int i = 0; i < B.num_nonzeros_; ++i) {
    238     rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_;
    239     cols_.get()[num_nonzeros_] = B.cols()[i];
    240     values_.get()[num_nonzeros_++] = B.values()[i];
    241   }
    242   num_rows_ = num_rows_ + B.num_rows();
    243 }
    244 
    245 void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) {
    246   CHECK_EQ(B.num_rows(), num_rows_);
    247   Reserve(num_nonzeros_ + B.num_nonzeros_);
    248   for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) {
    249     rows_.get()[num_nonzeros_] = B.rows()[i];
    250     cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_;
    251     values_.get()[num_nonzeros_] = B.values()[i];
    252   }
    253   num_cols_ = num_cols_ + B.num_cols();
    254 }
    255 
    256 
    257 void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) {
    258   if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) {
    259     num_rows_  = new_num_rows;
    260     num_cols_ = new_num_cols;
    261     return;
    262   }
    263 
    264   num_rows_ = new_num_rows;
    265   num_cols_ = new_num_cols;
    266 
    267   int* r_ptr = rows_.get();
    268   int* c_ptr = cols_.get();
    269   double* v_ptr = values_.get();
    270 
    271   int dropped_terms = 0;
    272   for (int i = 0; i < num_nonzeros_; ++i) {
    273     if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) {
    274       if (dropped_terms) {
    275         r_ptr[i-dropped_terms] = r_ptr[i];
    276         c_ptr[i-dropped_terms] = c_ptr[i];
    277         v_ptr[i-dropped_terms] = v_ptr[i];
    278       }
    279     } else {
    280       ++dropped_terms;
    281     }
    282   }
    283   num_nonzeros_ -= dropped_terms;
    284 }
    285 
    286 TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix(
    287     const double* values, int num_rows) {
    288   TripletSparseMatrix* m =
    289       new TripletSparseMatrix(num_rows, num_rows, num_rows);
    290   for (int i = 0; i < num_rows; ++i) {
    291     m->mutable_rows()[i] = i;
    292     m->mutable_cols()[i] = i;
    293     m->mutable_values()[i] = values[i];
    294   }
    295   m->set_num_nonzeros(num_rows);
    296   return m;
    297 }
    298 
    299 void TripletSparseMatrix::ToTextFile(FILE* file) const {
    300   CHECK_NOTNULL(file);
    301   for (int i = 0; i < num_nonzeros_; ++i) {
    302     fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]);
    303   }
    304 }
    305 
    306 }  // namespace internal
    307 }  // namespace ceres
    308