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?


1 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.

  1. 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).

  • $\begingroup$ Thanks, after reading more about this, how do you know what your initial velocity is to integrate the IMU information? $\endgroup$ May 18, 2022 at 4:57
  • $\begingroup$ Good question. I don't know how you would actually start it from scratch. Normally if you are running an offline large graph slam problem with IMU data, your initial guesses come from an online algorithm. So that's where you would grab your initial values. For online SLAM you would run either an initilization scheme or assume your robot starts stationary(velocity is 0). $\endgroup$
    – edwinem
    May 20, 2022 at 18:29
  • $\begingroup$ Would velocity be approximated using finite distance between two position states or something else? $\endgroup$ May 20, 2022 at 20:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.