Oh, and, it is clear that the Identity matrix for Leaf only expressions is completely known at compile time: Eigen::Matrix<double, T::dimension, T::dimension>::Identity(). That should nicely speed up many a PriorFactor (replacement).