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