Incorporating motion model in Graph Slam and Bundle adjustment

Question

When doing global bundle adjustment how does one incorporate a motion model or IMU information?

Does it simply impose a cost function in relation to how far away the expected location is of the robot or is there something else?

Answer 1

Would say you can generally do 2 different ways of incorporating the information.

1. Compute the odometry.

Simply compute how far your robot is supposed to move in between frames. Either using your motion model, or integrating you IMU measurements. You then as you said simply create a cost function(edge) that has a cost the further away the node is from the expected location. Essentially this equation

$$ e(T_{w1},T_{w2}) = T_{w1}^{-1}*T_{w2} - T_{odometry}$$

Where $T_{odometry}$ is the computed movement from your motion model or IMU integration.

You can find an actual example of this here in Ceres.

2. Use a IMU/ motion model equations directly

You can actually use the equations directly to form a constraint between 2 pose nodes.So your cost function may look something like this: $$ e(T_{w1},T_{w2}) = T_{w1}^{-1}*T_{w2} - f(encodervalues)$$ where $f(encodervalues)$ is some motion model function.

However, this is usually done in a slightly different manner typically called a preintegrated factor. The paper describing it can be found here, with a code example in GTSAM..

I am not going to go into full detail on why it is done, but essentially this allows you to sum multiple IMU measurements into 1 cost function(generally IMU is running at a much higher rate so you may have 20 measurements between 2 camera measurements and we only want 1 cost function not 20).