Bootstrap sequence

• Created factors are inactive if bootstrapping == true --> in emplaceFactors()
• Implement virtual void ProcessorMotion::bootstrap() method, doing the following:
  • wait for the conditions to apply the bootstrap
  • compute the necessary elements <-- 3 strategies, see below. Get q0, eventually v0.
  • transform all the nodes in Problem with the new bootsrapped solution
  • re-compute keyframe states produced by IMU to account for new initial conditions
  • re-activate all factors that were declared inactive
  • clear the bootstrapping flag

We implement 3 different bootstrapping strategies. The option is controlled via YAML. The three methods are implemented in ProcessorImu::bootstrap().

1. Implement Lupton's TRO-11 method to determine v0 and g from three kfs.

   • Conditions: 3 KFs of a processor that we know it's good

   • Computation: The method in the paper is too complicated and it seems to have plenty of errors.

     The method is revised here (ask jsola@iri.upc.edu for permission) https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac

     It boils down to this:

     1. express p1 as a function of p0, R0, v0, Dt01, Dp01, g
     2. express p2 as a function of p0, R0, v0, R1, Dt01, Dp01, Dv01, Dt02, Dp02
     3. assume p0, R0, p1, R1, p2 are provided by some external method (e.g. visual odom or slam)
     4. assume Dtij, Dpij and Dvij are provided by IMU preintegration
     5. write a linear system as a function of v0 and g
     6. solve for v0 and g
     • This ^^^ is coded in imu_tools.h, getting v0_loc and g_loc
     • Call function in imu_tools.h
     • compute q0 so that q0 * g_loc = g_glob
     • Transform v0_glob = q0 * v0_loc
     • Reintegrate IMU buffers according to new v0

     Once this math is done (in imu_tools.h), integrate it in WOLF. <-- discussed in core issue 461: mobile_robotics/wolf_projects/wolf_lib/wolf#461 (closed)

2. IMU averaging to get only g

   • Conditions: buffer length >= N
   • Computation:
     • average N IMU samples of acceleration, get local g vector, g_loc
     • compute q0 so that q0*g_loc = g_glob

3. Static case of v=0, w=0 --> IMU averaging allows us to compute g and gyro bias.

   • Conditions: buffer length >= N
   • Computations:
     • average N IMU samples
     • Impose v=0, w=0
     • compute gyro bias
     • reintegrate IMU buffers with new gyro bias
     • compute local g_loc
     • compute q0 so that q0*g_loc=g_glob