Equality constrained g2o, which allows to add equality edge factors - View it on GitHub