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

added Doing opinions wanted labels

assigned to @jsola

created branch 29-bootstrap-sequence-to-determine-v0-and-g to address this issue

mentioned in issue mobile_robotics/wolf_projects/wolf_lib/wolf#461 (closed)

changed title from Bootstrap sequence to determine v0 and g to Bootstrap sequence

changed the description

changed the description

changed the description

changed the description

marked the checklist item transform all the nodes in Problem with the new bootsrapped solution as completed

marked the checklist item clear the bootstrapping flag as completed

changed the description

marked the checklist item This ^^^ is coded in imu_tools.h as completed

marked the checklist item Created factors are inactive if bootstrapping == true --> in emplaceFactors() as completed

changed the description

marked the checklist item Conditions: buffer length >= N as completed