1 files changed, 61 insertions, 16 deletions

diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc
index 8df405ca115..54b588ba466 100644
--- a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc
+++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc
@@ -32,9 +32,10 @@
 
 #include <algorithm>
 #include <cstddef>
+
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/port.h"
-#include "ceres/internal/scoped_ptr.h"
+#include "ceres/random.h"
 #include "ceres/types.h"
 #include "glog/logging.h"
 
@@ -45,10 +46,8 @@ TripletSparseMatrix::TripletSparseMatrix()
     : num_rows_(0),
       num_cols_(0),
       max_num_nonzeros_(0),
-      num_nonzeros_(0),
-      rows_(NULL),
-      cols_(NULL),
-      values_(NULL) {}
+      num_nonzeros_(0) {}
+
 
 TripletSparseMatrix::~TripletSparseMatrix() {}
 
@@ -58,10 +57,7 @@ TripletSparseMatrix::TripletSparseMatrix(int num_rows,
     : num_rows_(num_rows),
       num_cols_(num_cols),
       max_num_nonzeros_(max_num_nonzeros),
-      num_nonzeros_(0),
-      rows_(NULL),
-      cols_(NULL),
-      values_(NULL) {
+      num_nonzeros_(0) {
   // All the sizes should at least be zero
   CHECK_GE(num_rows, 0);
   CHECK_GE(num_cols, 0);
@@ -69,21 +65,41 @@ TripletSparseMatrix::TripletSparseMatrix(int num_rows,
   AllocateMemory();
 }
 
+TripletSparseMatrix::TripletSparseMatrix(const int num_rows,
+                                         const int num_cols,
+                                         const std::vector<int>& rows,
+                                         const std::vector<int>& cols,
+                                         const std::vector<double>& values)
+    : num_rows_(num_rows),
+      num_cols_(num_cols),
+      max_num_nonzeros_(values.size()),
+      num_nonzeros_(values.size()) {
+  // All the sizes should at least be zero
+  CHECK_GE(num_rows, 0);
+  CHECK_GE(num_cols, 0);
+  CHECK_EQ(rows.size(), cols.size());
+  CHECK_EQ(rows.size(), values.size());
+  AllocateMemory();
+  std::copy(rows.begin(), rows.end(), rows_.get());
+  std::copy(cols.begin(), cols.end(), cols_.get());
+  std::copy(values.begin(), values.end(), values_.get());
+}
+
 TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig)
     : SparseMatrix(),
       num_rows_(orig.num_rows_),
       num_cols_(orig.num_cols_),
       max_num_nonzeros_(orig.max_num_nonzeros_),
-      num_nonzeros_(orig.num_nonzeros_),
-      rows_(NULL),
-      cols_(NULL),
-      values_(NULL) {
+      num_nonzeros_(orig.num_nonzeros_) {
   AllocateMemory();
   CopyData(orig);
 }
 
 TripletSparseMatrix& TripletSparseMatrix::operator=(
     const TripletSparseMatrix& rhs) {
+  if (this == &rhs) {
+    return *this;
+  }
   num_rows_ = rhs.num_rows_;
   num_cols_ = rhs.num_cols_;
   num_nonzeros_ = rhs.num_nonzeros_;
@@ -165,7 +181,7 @@ void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const {
 }
 
 void TripletSparseMatrix::SquaredColumnNorm(double* x) const {
-  CHECK_NOTNULL(x);
+  CHECK(x != nullptr);
   VectorRef(x, num_cols_).setZero();
   for (int i = 0; i < num_nonzeros_; ++i) {
     x[cols_[i]] += values_[i] * values_[i];
@@ -173,7 +189,7 @@ void TripletSparseMatrix::SquaredColumnNorm(double* x) const {
 }
 
 void TripletSparseMatrix::ScaleColumns(const double* scale) {
-  CHECK_NOTNULL(scale);
+  CHECK(scale != nullptr);
   for (int i = 0; i < num_nonzeros_; ++i) {
     values_[i] = values_[i] * scale[cols_[i]];
   }
@@ -254,11 +270,40 @@ TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix(
 }
 
 void TripletSparseMatrix::ToTextFile(FILE* file) const {
-  CHECK_NOTNULL(file);
+  CHECK(file != nullptr);
   for (int i = 0; i < num_nonzeros_; ++i) {
     fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]);
   }
 }
 
+TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix(
+    const TripletSparseMatrix::RandomMatrixOptions& options) {
+  CHECK_GT(options.num_rows, 0);
+  CHECK_GT(options.num_cols, 0);
+  CHECK_GT(options.density, 0.0);
+  CHECK_LE(options.density, 1.0);
+
+  std::vector<int> rows;
+  std::vector<int> cols;
+  std::vector<double> values;
+  while (rows.empty()) {
+    rows.clear();
+    cols.clear();
+    values.clear();
+    for (int r = 0; r < options.num_rows; ++r) {
+      for (int c = 0; c < options.num_cols; ++c) {
+        if (RandDouble() <= options.density) {
+          rows.push_back(r);
+          cols.push_back(c);
+          values.push_back(RandNormal());
+        }
+      }
+    }
+  }
+
+  return new TripletSparseMatrix(
+      options.num_rows, options.num_cols, rows, cols, values);
+}
+
 }  // namespace internal
 }  // namespace ceres