1 files changed, 75 insertions, 0 deletions
diff --git a/internal/ceres/compressed_col_sparse_matrix_utils.h b/internal/ceres/compressed_col_sparse_matrix_utils.h
index afabf1c..c8de2a1 100644
--- a/internal/ceres/compressed_col_sparse_matrix_utils.h
+++ b/internal/ceres/compressed_col_sparse_matrix_utils.h
@@ -61,6 +61,81 @@ void BlockOrderingToScalarOrdering(const vector<int>& blocks,
                                    const vector<int>& block_ordering,
                                    vector<int>* scalar_ordering);
 
+// Solve the linear system
+//
+//   R * solution = rhs
+//
+// Where R is an upper triangular compressed column sparse matrix.
+template <typename IntegerType>
+void SolveUpperTriangularInPlace(IntegerType num_cols,
+                                 const IntegerType* rows,
+                                 const IntegerType* cols,
+                                 const double* values,
+                                 double* rhs_and_solution) {
+  for (IntegerType c = num_cols - 1; c >= 0; --c) {
+    rhs_and_solution[c] /= values[cols[c + 1] - 1];
+    for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
+      const IntegerType r = rows[idx];
+      const double v = values[idx];
+      rhs_and_solution[r] -= v * rhs_and_solution[c];
+    }
+  }
+}
+
+// Solve the linear system
+//
+//   R' * solution = rhs
+//
+// Where R is an upper triangular compressed column sparse matrix.
+template <typename IntegerType>
+void SolveUpperTriangularTransposeInPlace(IntegerType num_cols,
+                                          const IntegerType* rows,
+                                          const IntegerType* cols,
+                                          const double* values,
+                                          double* rhs_and_solution) {
+  for (IntegerType c = 0; c < num_cols; ++c) {
+    for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
+      const IntegerType r = rows[idx];
+      const double v = values[idx];
+      rhs_and_solution[c] -= v * rhs_and_solution[r];
+    }
+    rhs_and_solution[c] =  rhs_and_solution[c] / values[cols[c + 1] - 1];
+  }
+}
+
+// Given a upper triangular matrix R in compressed column form, solve
+// the linear system,
+//
+//  R'R x = b
+//
+// Where b is all zeros except for rhs_nonzero_index, where it is
+// equal to one.
+//
+// The function exploits this knowledge to reduce the number of
+// floating point operations.
+template <typename IntegerType>
+void SolveRTRWithSparseRHS(IntegerType num_cols,
+                           const IntegerType* rows,
+                           const IntegerType* cols,
+                           const double* values,
+                           const int rhs_nonzero_index,
+                           double* solution) {
+  fill(solution, solution + num_cols, 0.0);
+  solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1];
+
+  for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) {
+    for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
+      const IntegerType r = rows[idx];
+      if (r < rhs_nonzero_index) continue;
+      const double v = values[idx];
+      solution[c] -= v * solution[r];
+    }
+    solution[c] =  solution[c] / values[cols[c + 1] - 1];
+  }
+
+  SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution);
+}
+
 }  // namespace internal
 }  // namespace ceres