Merge pull request #3986 from pleroy/GramSchmidt

Implement classical Gram-Schmidt for matrices

numerics/concepts.hpp

@@ -16,6 +16,8 @@ concept one_dimensional = requires(T& t, int index) {
 template<typename T>
 concept two_dimensional = requires(T& t, int row, int column) {
   { t(row, column) } -> std::same_as<typename T::Scalar&>;
+} || requires(T const& t, int row, int column) {
+  { t(row, column) } -> std::same_as<typename T::Scalar const&>;
 };
 
 template<typename T1, typename T2>

numerics/fixed_arrays.hpp

@@ -5,6 +5,7 @@
 #include <vector>
 
 #include "base/tags.hpp"
+#include "numerics/matrix_views.hpp"
 #include "numerics/transposed_view.hpp"
 #include "quantities/named_quantities.hpp"
 #include "quantities/si.hpp"
@@ -15,6 +16,7 @@ namespace _fixed_arrays {
 namespace internal {
 
 using namespace principia::base::_tags;
+using namespace principia::numerics::_matrix_views;
 using namespace principia::numerics::_transposed_view;
 using namespace principia::quantities::_named_quantities;
 using namespace principia::quantities::_si;
@@ -38,6 +40,12 @@ class FixedVector final {
   constexpr FixedVector(
       std::array<Scalar, size_>&& data);  // NOLINT(runtime/explicit)
 
+  // Constructs a fixed vector by copying data from the view.  Note that the
+  // result is fixed even if the matrix being viewed is an UnboundedMatrix.
+  template<typename T>
+    requires std::same_as<typename T::Scalar, Scalar_>
+  constexpr explicit FixedVector(ColumnView<T> const& view);
+
   // Convertible to an array.
   explicit constexpr operator std::array<Scalar, size_>() const;
 
@@ -173,6 +181,8 @@ class FixedStrictlyLowerTriangularMatrix final {
   constexpr FixedStrictlyLowerTriangularMatrix(
       std::array<Scalar, size_> const& data);
 
+  explicit constexpr operator FixedMatrix<Scalar, rows_, rows_>() const;
+
   friend bool operator==(FixedStrictlyLowerTriangularMatrix const& left,
                          FixedStrictlyLowerTriangularMatrix const& right) =
       default;
@@ -215,6 +225,8 @@ class FixedLowerTriangularMatrix final {
   explicit FixedLowerTriangularMatrix(
       TransposedView<FixedUpperTriangularMatrix<Scalar, rows_>> const& view);
 
+  explicit constexpr operator FixedMatrix<Scalar, rows_, rows_>() const;
+
   friend bool operator==(FixedLowerTriangularMatrix const& left,
                          FixedLowerTriangularMatrix const& right) = default;
   friend bool operator!=(FixedLowerTriangularMatrix const& left,
@@ -252,6 +264,8 @@ class FixedUpperTriangularMatrix final {
   explicit FixedUpperTriangularMatrix(
       TransposedView<FixedLowerTriangularMatrix<Scalar, columns_>> const& view);
 
+  explicit constexpr operator FixedMatrix<Scalar, columns_, columns_>() const;
+
   friend bool operator==(FixedUpperTriangularMatrix const& left,
                          FixedUpperTriangularMatrix const& right) = default;
   friend bool operator!=(FixedUpperTriangularMatrix const& left,

numerics/fixed_arrays_body.hpp

@@ -60,6 +60,17 @@ constexpr FixedVector<Scalar_, size_>::FixedVector(
     std::array<Scalar, size_>&& data)
     : data_(std::move(data)) {}
 
+template<typename Scalar_, int size_>
+template<typename T>
+  requires std::same_as<typename T::Scalar, Scalar_>
+constexpr FixedVector<Scalar_, size_>::FixedVector(
+    ColumnView<T> const& view) : FixedVector(uninitialized) {
+  CONSTEXPR_DCHECK(view.size() == size_);
+  for (int i = 0; i < size_; ++i) {
+    (*this)[i] = view[i];
+  }
+}
+
 template<typename Scalar_, int size_>
 constexpr FixedVector<Scalar_, size_>::operator std::array<Scalar_, size_>()
     const {
@@ -308,6 +319,18 @@ constexpr FixedStrictlyLowerTriangularMatrix<Scalar_, rows_>::
 FixedStrictlyLowerTriangularMatrix(std::array<Scalar, size_> const& data)
     : data_(data) {}
 
+template<typename Scalar_, int rows_>
+constexpr FixedStrictlyLowerTriangularMatrix<Scalar_, rows_>::
+operator FixedMatrix<Scalar_, rows_, rows_>() const {
+  FixedMatrix<Scalar, rows_, rows_> result;  // Initialized.
+  for (int i = 0; i < rows_; ++i) {
+    for (int j = 0; j < i; ++j) {
+      result(i, j) = (*this)(i, j);
+    }
+  }
+  return result;
+}
+
 template<typename Scalar_, int rows_>
 constexpr Scalar_& FixedStrictlyLowerTriangularMatrix<Scalar_, rows_>::
 operator()(int const row, int const column) {
@@ -366,6 +389,18 @@ FixedLowerTriangularMatrix<Scalar_, rows_>::FixedLowerTriangularMatrix(
   }
 }
 
+template<typename Scalar_, int rows_>
+constexpr FixedLowerTriangularMatrix<Scalar_, rows_>::
+operator FixedMatrix<Scalar_, rows_, rows_>() const {
+  FixedMatrix<Scalar, rows_, rows_> result;  // Initialized.
+  for (int i = 0; i < rows_; ++i) {
+    for (int j = 0; j <= i; ++j) {
+      result(i, j) = (*this)(i, j);
+    }
+  }
+  return result;
+}
+
 template<typename Scalar_, int rows_>
 constexpr Scalar_& FixedLowerTriangularMatrix<Scalar_, rows_>::
 operator()(int const row, int const column) {
@@ -417,6 +452,18 @@ FixedUpperTriangularMatrix<Scalar_, columns_>::FixedUpperTriangularMatrix(
   }
 }
 
+template<typename Scalar_, int columns_>
+constexpr FixedUpperTriangularMatrix<Scalar_, columns_>::
+operator FixedMatrix<Scalar_, columns_, columns_>() const {
+  FixedMatrix<Scalar, columns_, columns_> result;  // Initialized.
+  for (int j = 0; j < columns_; ++j) {
+    for (int i = 0; i <= j; ++i) {
+      result(i, j) = (*this)(i, j);
+    }
+  }
+  return result;
+}
+
 template<typename Scalar_, int columns_>
 constexpr Scalar_& FixedUpperTriangularMatrix<Scalar_, columns_>::
 operator()(int const row, int const column) {

numerics/fixed_arrays_test.cpp

@@ -261,6 +261,25 @@ TEST_F(FixedArraysTest, Row) {
   EXPECT_EQ(-24, r1 * v);
 }
 
+TEST_F(FixedArraysTest, Conversions) {
+  using M = FixedMatrix<double, 4, 4>;
+  EXPECT_EQ(M({0, 0,  0, 0,
+               1, 0,  0, 0,
+               2, 3,  0, 0,
+               5, 8, 13, 0}),
+            M(sl4_));
+  EXPECT_EQ(M({1,   0,  0,  0,
+               2,   3,  0,  0,
+               5,   8, 13,  0,
+               21, 34, 55, 89}),
+            M(l4_));
+  EXPECT_EQ(M({1, 2,  3,  5,
+               0, 8, 13, 21,
+               0, 0, 34, 55,
+               0, 0,  0, 89}),
+            M(u4_));
+}
+
 TEST_F(FixedArraysTest, Transpose) {
   EXPECT_EQ(
       (FixedMatrix<double, 4, 3>({-8,  -4,  6,

numerics/matrix_computations.hpp

@@ -29,6 +29,14 @@ struct ᵗRDRDecompositionGenerator;
 template<typename M, typename V>
 struct SubstitutionGenerator;
 
+// Declares:
+//   struct Result {
+//     ⟨upper triangular matrix⟩ R;
+//     ⟨matrix⟩ Q;
+//   };
+template<typename M>
+struct GramSchmidtGenerator;
+
 // Declares:
 //   struct Result {
 //     (matrix) H;
@@ -99,6 +107,10 @@ typename SubstitutionGenerator<LowerTriangularMatrix, Vector>::Result
 ForwardSubstitution(LowerTriangularMatrix const& L,
                     Vector const& b);
 
+template<typename Matrix>
+typename GramSchmidtGenerator<Matrix>::Result
+ClassicalGramSchmidt(Matrix const& L);
+
 // If A is a square matrix, returns U and H so that A = ᵗU H U, where H is an
 // upper Hessenberg matrix.
 // TODO(phl): Add support for returning U.
@@ -141,6 +153,7 @@ Solve(Matrix A, Vector b);
 
 using internal::BackSubstitution;
 using internal::CholeskyDecomposition;
+using internal::ClassicalGramSchmidt;
 using internal::ClassicalJacobi;
 using internal::ForwardSubstitution;
 using internal::HessenbergDecomposition;

numerics/matrix_computations_body.hpp

@@ -285,6 +285,29 @@ struct SubstitutionGenerator<TriangularMatrix<LScalar, dimension>,
   static Result Uninitialized(TriangularMatrix<LScalar, dimension> const& m);
 };
 
+template<typename Scalar>
+struct GramSchmidtGenerator<UnboundedMatrix<Scalar>> {
+  struct Result {
+    UnboundedMatrix<double> Q;
+    UnboundedUpperTriangularMatrix<Scalar> R;
+  };
+  using AVector = UnboundedVector<Scalar>;
+  using QVector = UnboundedVector<double>;
+  static Result Uninitialized(UnboundedMatrix<Scalar> const& m);
+};
+
+template<typename Scalar, int dimension>
+struct GramSchmidtGenerator<FixedMatrix<Scalar, dimension, dimension>> {
+  struct Result {
+    FixedMatrix<double, dimension, dimension> Q;
+    FixedUpperTriangularMatrix<Scalar, dimension> R;
+  };
+  using AVector = FixedVector<Scalar, dimension>;
+  using QVector = FixedVector<double, dimension>;
+  static Result Uninitialized(
+      FixedMatrix<Scalar, dimension, dimension> const& m);
+};
+
 template<typename Scalar>
 struct HessenbergDecompositionGenerator<UnboundedMatrix<Scalar>> {
   struct Result {
@@ -432,6 +455,22 @@ Uninitialized(TriangularMatrix<LScalar> const& m) -> Result {
   return Result(m.columns(), uninitialized);
 }
 
+template<typename Scalar>
+auto GramSchmidtGenerator<UnboundedMatrix<Scalar>>::
+Uninitialized(UnboundedMatrix<Scalar> const& m) -> Result {
+  return Result{
+      .Q = UnboundedMatrix<double>(m.rows(), m.columns(), uninitialized),
+      .R = UnboundedUpperTriangularMatrix<Scalar>(m.columns(), uninitialized)};
+}
+
+template<typename Scalar, int dimension>
+auto GramSchmidtGenerator<FixedMatrix<Scalar, dimension, dimension>>::
+Uninitialized(FixedMatrix<Scalar, dimension, dimension> const& m) -> Result {
+  return Result{
+      .Q = FixedMatrix<double, dimension, dimension>(uninitialized),
+      .R = FixedUpperTriangularMatrix<Scalar, dimension>(uninitialized)};
+}
+
 template<typename Scalar>
 auto ClassicalJacobiGenerator<UnboundedMatrix<Scalar>>::Identity(
     UnboundedMatrix<Scalar> const& m) -> Rotation {
@@ -603,6 +642,44 @@ ForwardSubstitution(LowerTriangularMatrix const& L,
   return x;
 }
 
+template<typename Matrix>
+typename GramSchmidtGenerator<Matrix>::Result ClassicalGramSchmidt(
+    Matrix const& A) {
+  using G = GramSchmidtGenerator<Matrix>;
+  auto result = G::Uninitialized(A);
+  auto& Q = result.Q;
+  auto& R = result.R;
+  int const n = A.rows();
+
+  // [Hig02], Algorithm 19.11.
+  for (int j = 0; j < n; ++j) {
+    auto const aⱼ = ColumnView<Matrix const>{.matrix = A,
+                                             .first_row = 0,
+                                             .last_row = n - 1,
+                                             .column = j};
+    for (int i = 0; i < j; ++i) {
+      auto const qᵢ = ColumnView{.matrix = Q,
+                                 .first_row = 0,
+                                 .last_row = n - 1,
+                                 .column = i};
+      R(i, j) = TransposedView{.transpose = qᵢ} * aⱼ;  // NOLINT
+    }
+    typename G::AVector qʹⱼ(aⱼ);
+    for (int k = 0; k < j; ++k) {
+      qʹⱼ -= R(k, j) * typename G::QVector(ColumnView{.matrix = Q,
+                                                      .first_row = 0,
+                                                      .last_row = n - 1,
+                                                      .column = k});
+    }
+    R(j, j) = qʹⱼ.Norm();
+    ColumnView{.matrix = Q,  // NOLINT
+               .first_row = 0,
+               .last_row = n - 1,
+               .column = j} = qʹⱼ / R(j, j);
+  }
+  return result;
+}
+
 template<typename Matrix>
 typename HessenbergDecompositionGenerator<Matrix>::Result
 HessenbergDecomposition(Matrix const& A) {

numerics/matrix_computations_test.cpp

@@ -5,6 +5,7 @@
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"
 #include "numerics/fixed_arrays.hpp"
+#include "numerics/transposed_view.hpp"
 #include "numerics/unbounded_arrays.hpp"
 #include "quantities/elementary_functions.hpp"
 #include "testing_utilities/almost_equals.hpp"
@@ -18,6 +19,7 @@ namespace numerics {
 
 using namespace principia::numerics::_fixed_arrays;
 using namespace principia::numerics::_matrix_computations;
+using namespace principia::numerics::_transposed_view;
 using namespace principia::numerics::_unbounded_arrays;
 using namespace principia::quantities::_elementary_functions;
 using namespace principia::testing_utilities::_almost_equals;
@@ -110,6 +112,31 @@ TYPED_TEST(MatrixComputationsTest, ForwardSubstitution) {
   EXPECT_THAT(x4_actual, AlmostEquals(x4_expected, 0));
 }
 
+TYPED_TEST(MatrixComputationsTest, ClassicalGramSchmidt) {
+  using Matrix = typename std::tuple_element<3, TypeParam>::type;
+  Matrix const m4({1, 2, 3, -4,
+                   5, 6, 7, 8,
+                   9, 8, -7, 6,
+                   5, 4, 3, 2});
+  auto const qr = ClassicalGramSchmidt(m4);
+
+  // Check that the decomposition is correct.
+  auto const near_m4 = qr.Q * Matrix(qr.R);
+  EXPECT_THAT(near_m4, AlmostEquals(m4, 1));
+
+  // Check that Q is nearly orthogonal.
+  auto const near_identity = Matrix(TransposedView{.transpose = qr.Q}) * qr.Q;
+  for (int i = 0; i < near_identity.rows(); ++i) {
+    for (int j = 0; j < near_identity.columns(); ++j) {
+      if (i == j) {
+        EXPECT_THAT(near_identity(i, j), AlmostEquals(1, 0, 2));
+      } else {
+        EXPECT_THAT(near_identity(i, j), VanishesBefore(1, 0, 3));
+      }
+    }
+  }
+}
+
 TYPED_TEST(MatrixComputationsTest, HessenbergForm) {
   using Matrix = typename std::tuple_element<3, TypeParam>::type;
   Matrix const m4({1, 2, 3, -4,

numerics/matrix_views.hpp

@@ -31,6 +31,13 @@ struct BlockView {
   constexpr Scalar& operator()(int row, int column);
   constexpr Scalar const& operator()(int row, int column) const;
 
+  template<typename T>
+    requires two_dimensional<T> && same_elements_as<T, Matrix>
+  BlockView& operator=(T const& right);
+  template<typename T>
+    requires two_dimensional<T> && same_elements_as<T, Matrix>
+  BlockView& operator=(T&& right);
+
   template<typename T>
     requires two_dimensional<T> && same_elements_as<T, Matrix>
   BlockView& operator+=(T const& right);
@@ -59,6 +66,13 @@ struct ColumnView {
   constexpr Scalar& operator[](int index);
   constexpr Scalar const& operator[](int index) const;
 
+  template<typename T>
+    requires one_dimensional<T> && same_elements_as<T, Matrix>
+  ColumnView& operator=(T const& right);
+  template<typename T>
+    requires one_dimensional<T> && same_elements_as<T, Matrix>
+  ColumnView& operator=(T&& right);
+
   template<typename T>
     requires one_dimensional<T> && same_elements_as<T, Matrix>
   ColumnView& operator+=(T const& right);
@@ -74,6 +88,11 @@ struct ColumnView {
   constexpr int size() const;
 };
 
+template<typename LMatrix, typename RMatrix>
+Product<typename LMatrix::Scalar, typename RMatrix::Scalar> operator*(
+    TransposedView<ColumnView<LMatrix>> const& left,
+    ColumnView<RMatrix> const& right);
+
 // The declarations below also declare the symmetric operator== and
 // operator!=.