Add a single precision variant of EigenSparseCholesky.

Given a double precision linear system, solve it using
a single precision Cholesky factorization.

Change-Id: I8a6e8b7a451e961a8a23a62dd7b5d8159c4db8ce

internal/ceres/eigensparse.cc

diff --git a/internal/ceres/eigensparse.cc b/internal/ceres/eigensparse.cc
index 4945036..8ca1d17 100644
--- a/internal/ceres/eigensparse.cc
+++ b/internal/ceres/eigensparse.cc
@@ -42,7 +42,7 @@
 namespace internal {
 
 template <typename Solver>
-class EigenSparseCholeskyTemplate : public EigenSparseCholesky {
+class EigenSparseCholeskyTemplate : public SparseCholesky {
  public:
   EigenSparseCholeskyTemplate() : analyzed_(false) {}
   virtual ~EigenSparseCholeskyTemplate() {}
@@ -51,7 +51,8 @@
   }
 
   virtual LinearSolverTerminationType Factorize(
-      const Eigen::SparseMatrix<double>& lhs, std::string* message) {
+      const Eigen::SparseMatrix<typename Solver::Scalar>& lhs,
+      std::string* message) {
     if (!analyzed_) {
       solver_.analyzePattern(lhs);
 
@@ -77,13 +78,24 @@
     return LINEAR_SOLVER_SUCCESS;
   }
 
-  virtual LinearSolverTerminationType Solve(const double* rhs,
-                                            double* solution,
+  virtual LinearSolverTerminationType Solve(const double* rhs_ptr,
+                                            double* solution_ptr,
                                             std::string* message) {
     CHECK(analyzed_) << "Solve called without a call to Factorize first.";
 
-    VectorRef(solution, solver_.cols()) =
-        solver_.solve(ConstVectorRef(rhs, solver_.cols()));
+    ConstVectorRef rhs(rhs_ptr, solver_.cols());
+    VectorRef solution(solution_ptr, solver_.cols());
+
+    // The two casts are needed if the Scalar in this class is not
+    // double. For code simplicitly we are going to assume that Eigen
+    // is smart enough to figure out that casting a double Vector to a
+    // double Vector is a straight copy. If this turns into a
+    // performance bottleneck (unlikely), we can revisit this.
+    solution =
+        solver_
+        .solve(rhs.template cast<typename Solver::Scalar>())
+        .template cast<double>();
+
     if (solver_.info() != Eigen::Success) {
       *message = "Eigen failure. Unable to do triangular solve.";
       return LINEAR_SOLVER_FAILURE;
@@ -94,23 +106,37 @@
   virtual LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs,
                                                 std::string* message) {
     CHECK_EQ(lhs->storage_type(), StorageType());
-    Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs(
-        lhs->num_rows(),
-        lhs->num_rows(),
-        lhs->num_nonzeros(),
-        lhs->mutable_rows(),
-        lhs->mutable_cols(),
-        lhs->mutable_values());
+
+    typename Solver::Scalar* values_ptr = NULL;
+    if (std::is_same<typename Solver::Scalar, double>::value) {
+      values_ptr =
+          reinterpret_cast<typename Solver::Scalar*>(lhs->mutable_values());
+    } else {
+      // In the case where the scalar used in this class is not
+      // double. In that case, make a copy of the values array in the
+      // CompressedRowSparseMatrix and cast it to Scalar along the way.
+      values_ = ConstVectorRef(lhs->values(), lhs->num_nonzeros())
+                    .cast<typename Solver::Scalar>();
+      values_ptr = values_.data();
+    }
+
+    Eigen::MappedSparseMatrix<typename Solver::Scalar, Eigen::ColMajor>
+        eigen_lhs(lhs->num_rows(),
+                  lhs->num_rows(),
+                  lhs->num_nonzeros(),
+                  lhs->mutable_rows(),
+                  lhs->mutable_cols(),
+                  values_ptr);
     return Factorize(eigen_lhs, message);
   }
 
  private:
+  Eigen::Matrix<typename Solver::Scalar, Eigen::Dynamic, 1> values_;
   bool analyzed_;
   Solver solver_;
 };
 
-EigenSparseCholesky* EigenSparseCholesky::Create(
-    const OrderingType ordering_type) {
+SparseCholesky* EigenSparseCholesky::Create(const OrderingType ordering_type) {
   // The preprocessor gymnastics here are dealing with the fact that
   // before version 3.2.2, Eigen did not support a third template
   // parameter to specify the ordering and it always defaults to AMD.
@@ -137,6 +163,35 @@
 
 EigenSparseCholesky::~EigenSparseCholesky() {}
 
+SparseCholesky* FloatEigenSparseCholesky::Create(
+    const OrderingType ordering_type) {
+  // The preprocessor gymnastics here are dealing with the fact that
+  // before version 3.2.2, Eigen did not support a third template
+  // parameter to specify the ordering and it always defaults to AMD.
+#if EIGEN_VERSION_AT_LEAST(3, 2, 2)
+  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>,
+                                Eigen::Upper,
+                                Eigen::AMDOrdering<int> >
+      WithAMDOrdering;
+  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>,
+                                Eigen::Upper,
+                                Eigen::NaturalOrdering<int> >
+      WithNaturalOrdering;
+  if (ordering_type == AMD) {
+    LOG(FATAL) << "We should not be doing this";
+    return new EigenSparseCholeskyTemplate<WithAMDOrdering>();
+  } else {
+    return new EigenSparseCholeskyTemplate<WithNaturalOrdering>();
+  }
+#else
+  typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, Eigen::Upper>
+      WithAMDOrdering;
+  return new EigenSparseCholeskyTemplate<WithAMDOrdering>();
+#endif
+}
+
+FloatEigenSparseCholesky::~FloatEigenSparseCholesky() {}
+
 }  // namespace internal
 }  // namespace ceres
 

internal/ceres/eigensparse.h

diff --git a/internal/ceres/eigensparse.h b/internal/ceres/eigensparse.h
index c73a811..ce8ef34 100644
--- a/internal/ceres/eigensparse.h
+++ b/internal/ceres/eigensparse.h
@@ -49,10 +49,29 @@
 class EigenSparseCholesky : public SparseCholesky {
  public:
   // Factory
-  static EigenSparseCholesky* Create(const OrderingType ordering_type);
+  static SparseCholesky* Create(const OrderingType ordering_type);
 
   // SparseCholesky interface.
   virtual ~EigenSparseCholesky();
+  virtual LinearSolverTerminationType Factorize(
+      CompressedRowSparseMatrix* lhs, std::string* message) = 0;
+  virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0;
+  virtual LinearSolverTerminationType Solve(const double* rhs,
+                                            double* solution,
+                                            std::string* message) = 0;
+};
+
+// Even though the input is double precision linear system, this class
+// solves it by computing a single precision Cholesky factorization.
+class FloatEigenSparseCholesky : public SparseCholesky {
+ public:
+  // Factory
+  static SparseCholesky* Create(const OrderingType ordering_type);
+
+  // SparseCholesky interface.
+  virtual ~FloatEigenSparseCholesky();
+  virtual LinearSolverTerminationType Factorize(
+      CompressedRowSparseMatrix* lhs, std::string* message) = 0;
   virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0;
   virtual LinearSolverTerminationType Solve(const double* rhs,
                                             double* solution,

internal/ceres/sparse_cholesky_test.cc

diff --git a/internal/ceres/sparse_cholesky_test.cc b/internal/ceres/sparse_cholesky_test.cc
index 79db689..b75e3aa 100644
--- a/internal/ceres/sparse_cholesky_test.cc
+++ b/internal/ceres/sparse_cholesky_test.cc
@@ -205,6 +205,13 @@
                                            ::testing::Values(AMD, NATURAL),
                                            ::testing::Values(true, false)),
                         ParamInfoToString);
+
+INSTANTIATE_TEST_CASE_P(EigenSparseCholeskySingle,
+                        SparseCholeskyTest,
+                        ::testing::Combine(::testing::Values(EIGEN_SPARSE),
+                                           ::testing::Values(AMD, NATURAL),
+                                           ::testing::Values(true, false)),
+                        ParamInfoToString);
 #endif
 
 }  // namespace internal