How to optimize multi-constraint function with g2o

Question

I am currently trying to optimize a multi constraints function for SLAM. Classic optimization function minimize reprojection errors with g2o like proposed in https://fzheng.me/2016/03/15/g2o-demo/.

My problem is that I do not know how to modify this g2o sample code to jointly optimize two constraints (for example : 1 constraint for reprojection error and 1 constraint for inertial error).

Regards,

Answer 1

To include a custom constraint, you must to implement a specification of one BaseEdge<> subclasses.

There are 3 subclasses of BaseEdge<> that are BaseUnaryEdge<> (for self-constraints), BaseBinaryEdge<> (between 2 nodes) and BaseMultiEdge<> (arbitrary number of nodes).

Inertial error is a self-constraint, so you must specify an implementation of BaseUnaryEdge.

It is mandatory to implement just the computeError() method in your custom class, but you can also implement linearizeOplus() to set the jacobian by hand.

Then you can follow the sample code you posted. Instantiate the optimizer, create vertices, add reprojection constraints and add your custom inertial constraints.