12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working and tested implementation of hessianBlockDiagonal

release/4.3a0
dellaert 2014-02-14 13:22:13 -05:00
parent 6e4433f589
commit 7abcd81150
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/HessianFactor.cpp
View File

@ -366,6 +366,12 @@ VectorValues HessianFactor::hessianDiagonal() const {
/* ************************************************************************* */ /* ************************************************************************* */
map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const { map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
map<Key,Matrix> blocks; 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; return blocks;
} }