Working and tested implementation of hessianBlockDiagonal

2014-02-14 13:22:13 -05:00 · 2014-02-14 13:22:13 -05:00 · 7abcd81150
parent 6e4433f589
commit 7abcd81150
1 changed files with 6 additions and 0 deletions
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@ -366,6 +366,12 @@ VectorValues HessianFactor::hessianDiagonal() const {
 /* ************************************************************************* */
 map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
  map<Key,Matrix> blocks;
+  // Loop over all variables
+  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    // Get the diagonal block, and insert it
+    Matrix B = info_(j, j).selfadjointView();
+    blocks.insert(make_pair(keys_[j],B));
+  }
  return blocks;
 }