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:31:40 -05:00
parent 411381fd88
commit a2829fffad
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/JacobianFactor.cpp
View File

@ -458,6 +458,12 @@ namespace gtsam {
/* ************************************************************************* */
map<Key,Matrix> JacobianFactor::hessianBlockDiagonal() const {
map<Key,Matrix> blocks;
for(size_t pos=0; pos<size(); ++pos)
{
Key j = keys_[pos];
Matrix Aj = model_->Whiten(Ab_(pos));
blocks.insert(make_pair(j,Aj.transpose()*Aj));
}
return blocks;
}