Return type of GaussianFactorGraph::hessianBlockDiagonal

[Richard Roberts]

The return type of this has two potential performance issues:

std::map<Key,Matrix> hessianBlockDiagonal() const

• std::map can be slow - use FastMap instead (this is probably worth fixing for sure)
• Matrix will use unnesessary space, perhaps use Vector instead since you're returning diagonals. You can then use Eigen functions to do calculations with the Vector interpreted as a matrix diagonal. I think it would be vector.asDiagonal(). (This may or may not be worth fixing now... perhaps just keep in mind when profiling in the future).