Akleeman/sparse structured

include/albatross/linalg/Block

@@ -13,9 +13,12 @@
 #ifndef ALBATROSS_LINALG_BLOCK_H
 #define ALBATROSS_LINALG_BLOCK_H
 
-#include "../Common"
+#include "../Indexing"
+#include "../src/linalg/operators.hpp"
+#include "../src/linalg/structured.hpp"
 #include "../src/linalg/block_utils.hpp"
 #include "../src/linalg/block_diagonal.hpp"
 #include "../src/linalg/block_symmetric.hpp"
+#include "../src/linalg/infer_structure.hpp"
 
 #endif

include/albatross/src/cereal/block_utils.hpp

@@ -24,6 +24,13 @@ inline void serialize(Archive &archive,
           cereal::make_nvp("S", block_sym.S));
 }
 
+template <typename Archive>
+inline void serialize(Archive &archive,
+                      albatross::BlockDiagonalLDLT &block_ldlt,
+                      const std::uint32_t) {
+  archive(cereal::make_nvp("blocks", block_ldlt.blocks));
+}
+
 } // namespace cereal
 
 #endif /* ALBATROSS_SRC_CEREAL_BLOCK_UTILS_HPP_ */

include/albatross/src/linalg/block_diagonal.hpp

@@ -37,10 +37,11 @@ struct BlockDiagonalLDLT {
   solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
         ThreadPool *pool) const;
 
-  template <class _Scalar, int _Rows, int _Cols>
-  Eigen::Matrix<_Scalar, _Rows, _Cols>
-  sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
-             ThreadPool *pool) const;
+  template <typename Derived>
+  Eigen::MatrixXd sqrt_solve(const Eigen::DenseBase<Derived> &rhs,
+                             ThreadPool *pool) const;
+
+  BlockDiagonal sqrt_transpose() const;
 
   std::map<size_t, Eigen::Index> block_to_row_map() const;
 
@@ -49,6 +50,8 @@ struct BlockDiagonalLDLT {
   Eigen::Index rows() const;
 
   Eigen::Index cols() const;
+
+  bool operator==(const BlockDiagonalLDLT &other) const;
 };
 
 struct BlockDiagonal {
@@ -68,7 +71,7 @@ struct BlockDiagonal {
 
   Eigen::Index cols() const;
 
-  Eigen::MatrixXd toDense() const;
+  Eigen::MatrixXd to_dense() const;
 };
 
 /*
@@ -102,6 +105,13 @@ inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::sqrt_solve(
   return output;
 }
 
+inline BlockDiagonal BlockDiagonalLDLT::sqrt_transpose() const {
+  auto sqrt_transpose_block = [](const auto &b) { return b.sqrt_transpose(); };
+  BlockDiagonal output;
+  output.blocks = apply(this->blocks, sqrt_transpose_block);
+  return output;
+}
+
 inline std::map<size_t, Eigen::Index>
 BlockDiagonalLDLT::block_to_row_map() const {
   Eigen::Index row = 0;
@@ -132,15 +142,16 @@ BlockDiagonalLDLT::solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
   return output;
 }
 
-template <class _Scalar, int _Rows, int _Cols>
-inline Eigen::Matrix<_Scalar, _Rows, _Cols>
-BlockDiagonalLDLT::sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
+template <typename Derived>
+inline Eigen::MatrixXd
+BlockDiagonalLDLT::sqrt_solve(const Eigen::DenseBase<Derived> &rhs,
                               ThreadPool *pool) const {
   ALBATROSS_ASSERT(cols() == rhs.rows());
-  Eigen::Matrix<_Scalar, _Rows, _Cols> output(rows(), rhs.cols());
+  Eigen::MatrixXd output(rows(), rhs.cols());
 
   auto solve_and_fill_one_block = [&](const size_t i, const Eigen::Index row) {
-    const auto rhs_chunk = rhs.block(row, 0, blocks[i].rows(), rhs.cols());
+    const auto rhs_chunk =
+        rhs.derived().block(row, 0, blocks[i].rows(), rhs.cols());
     output.block(row, 0, blocks[i].rows(), rhs.cols()) =
         blocks[i].sqrt_solve(rhs_chunk);
   };
@@ -173,6 +184,10 @@ inline Eigen::Index BlockDiagonalLDLT::cols() const {
   return n;
 }
 
+inline bool
+BlockDiagonalLDLT::operator==(const BlockDiagonalLDLT &other) const {
+  return blocks == other.blocks;
+}
 /*
  * Block Diagonal
  */
@@ -204,7 +219,7 @@ inline BlockDiagonal BlockDiagonal::operator-(const BlockDiagonal &rhs) const {
   return output;
 }
 
-inline Eigen::MatrixXd BlockDiagonal::toDense() const {
+inline Eigen::MatrixXd BlockDiagonal::to_dense() const {
   Eigen::MatrixXd output = Eigen::MatrixXd::Zero(rows(), cols());
 
   Eigen::Index i = 0;

include/albatross/src/linalg/infer_structure.hpp

@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2019 Swift Navigation Inc.
+ * Contact: Swift Navigation <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef ALBATROSS_SRC_LINALG_INFER_STRUCTURE_HPP
+#define ALBATROSS_SRC_LINALG_INFER_STRUCTURE_HPP
+
+namespace albatross {
+
+namespace linalg {
+
+namespace detail {
+
+using VectorXb = Eigen::Matrix<bool, Eigen::Dynamic, 1>;
+
+void inline connected_indices(const Eigen::MatrixXd &x, const Eigen::Index &i,
+                              VectorXb *visited,
+                              std::set<Eigen::Index> *active_set) {
+  assert(visited != nullptr);
+  assert(active_set != nullptr);
+
+  for (Eigen::Index j = 0; j < x.cols(); ++j) {
+    if (x(i, j) != 0. && !(*visited)[j]) {
+      active_set->insert(j);
+      (*visited)[j] = true;
+      connected_indices(x, j, visited, active_set);
+    }
+  }
+}
+
+} // namespace detail
+
+std::vector<std::set<Eigen::Index>> inline infer_diagonal_blocks(
+    const Eigen::MatrixXd &x) {
+  assert(x.rows() == x.cols());
+  if (x.rows() == 0) {
+    return {};
+  }
+  std::vector<std::set<Eigen::Index>> connected_sets;
+  detail::VectorXb visited(x.rows());
+  visited.fill(false);
+
+  for (Eigen::Index i = 0; i < x.rows(); ++i) {
+    if (!visited[i]) {
+      std::set<Eigen::Index> active_set = {i};
+      detail::connected_indices(x, i, &visited, &active_set);
+      connected_sets.emplace_back(active_set);
+    }
+  }
+
+  return connected_sets;
+}
+
+Eigen::PermutationMatrix<Eigen::Dynamic> inline to_permutation_matrix(
+    const std::vector<std::set<Eigen::Index>> &blocks) {
+  Eigen::Index n = 0;
+  for (const auto &block : blocks) {
+    n += cast::to_index(block.size());
+  }
+
+  Eigen::VectorXi permutation_inds(n);
+  int i = 0;
+  for (const auto &block : blocks) {
+    for (const auto &ind : block) {
+      permutation_inds[ind] = i;
+      ++i;
+    }
+  }
+  return Eigen::PermutationMatrix<Eigen::Dynamic>(permutation_inds);
+}
+
+Structured<BlockDiagonal> inline infer_block_diagonal_matrix(
+    const Eigen::MatrixXd &x) {
+  const auto block_inds = infer_diagonal_blocks(x);
+  const auto P = to_permutation_matrix(block_inds);
+
+  Structured<BlockDiagonal> output;
+  output.P_rows = P.transpose();
+  output.P_cols = P;
+
+  // D = P * x * P.T
+  // x = P.T * D * P
+  for (const auto &block : block_inds) {
+    std::vector<Eigen::Index> inds(block.begin(), block.end());
+    output.matrix.blocks.emplace_back(subset(x, inds, inds));
+  }
+  return output;
+}
+
+} // namespace linalg
+
+} // namespace albatross
+
+#endif // ALBATROSS_SRC_LINALG_INFER_STRUCTURE_HPP

include/albatross/src/linalg/operators.hpp

@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2019 Swift Navigation Inc.
+ * Contact: Swift Navigation <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef ALBATROSS_SRC_LINALG_OPERATORS_HPP
+#define ALBATROSS_SRC_LINALG_OPERATORS_HPP
+
+namespace albatross {
+
+template <typename MatrixType>
+inline Eigen::MatrixXd to_dense(const MatrixType &x) {
+  return x.to_dense();
+}
+
+template <typename Derived>
+inline Eigen::MatrixXd to_dense(const Eigen::DenseBase<Derived> &x) {
+  return x.derived();
+}
+
+template <int Size, typename X>
+inline auto operator*(const Eigen::PermutationMatrix<Size> &matrix,
+                      const std::vector<X> &features) {
+  std::vector<X> output(features.size());
+  const auto &indices = matrix.indices();
+  for (Eigen::Index i = 0; i < indices.size(); ++i) {
+    const std::size_t si = cast::to_size(i);
+    const std::size_t sj = cast::to_size(indices[i]);
+    output[sj] = features[si];
+  }
+  return output;
+}
+
+} // namespace albatross
+
+#endif // ALBATROSS_SRC_LINALG_OPERATORS_HPP

include/albatross/src/linalg/structured.hpp

@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2019 Swift Navigation Inc.
+ * Contact: Swift Navigation <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef ALBATROSS_SRC_LINALG_STRUCTURED_HPP
+#define ALBATROSS_SRC_LINALG_STRUCTURED_HPP
+
+namespace albatross {
+
+/*
+ * Holds a structured representation of a matrix which often requires
+ * permuting rows and columns;
+ */
+template <typename MatrixType> struct Structured {
+
+  Eigen::Index rows() const { return matrix.rows(); }
+
+  Eigen::Index cols() const { return matrix.cols(); }
+
+  Eigen::MatrixXd to_dense() const {
+    return P_rows * albatross::to_dense(matrix) * P_cols;
+  }
+
+  MatrixType matrix;
+  Eigen::PermutationMatrix<Eigen::Dynamic> P_rows;
+  Eigen::PermutationMatrix<Eigen::Dynamic> P_cols;
+};
+
+template <typename MatrixType, typename RhsType>
+auto operator*(const Structured<MatrixType> &lhs, const RhsType &rhs) {
+  return lhs.P_rows * (lhs.matrix * (lhs.P_cols * rhs));
+}
+
+template <typename MatrixType>
+auto make_structured(MatrixType &&x,
+                     const Eigen::PermutationMatrix<Eigen::Dynamic> &P_rows,
+                     const Eigen::PermutationMatrix<Eigen::Dynamic> &P_cols) {
+  return Structured<MatrixType>{x, P_rows, P_cols};
+}
+
+namespace structured {
+
+template <typename MatrixType> auto ldlt(const Structured<MatrixType> &x) {
+  return make_structured(x.matrix.ldlt(), x.P_rows, x.P_cols);
+}
+
+template <typename MatrixType, typename RhsType>
+auto sqrt_solve(const Structured<MatrixType> &x, const RhsType &rhs) {
+  assert(x.P_rows == x.P_cols);
+  return x.matrix.sqrt_solve(x.P_rows.transpose() * rhs);
+}
+
+} // namespace structured
+
+} // namespace albatross
+
+#endif // ALBATROSS_SRC_LINALG_STRUCTURED_HPP