sqrt_transpose in BlockDiagonalLDLT

swift-nav · Jan 11, 2023 · 884903e · 884903e
1 parent 74b2d30
commit 884903e
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/include/albatross/src/linalg/block_diagonal.hpp b/include/albatross/src/linalg/block_diagonal.hpp
@@ -42,6 +42,8 @@ struct BlockDiagonalLDLT {
   sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
              ThreadPool *pool) const;
 
+  BlockDiagonal sqrt_transpose() const;
+
   std::map<size_t, Eigen::Index> block_to_row_map() const;
 
   double log_determinant() const;
@@ -102,6 +104,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;