Open prashanthr05 opened 3 years ago
Repasting image from #2 for reference,
Overall architecture |
---|
A very preliminary idea is given below (subject to changes or even abandonment).
KinDynVIOEstimator
will be a high level manager.
It will maintain a few shared resources and a few advanceable runners.
For example,
Let's consider a visual inertial odoemtery using aruco markers and IMU measurements. In this case, we have images coming in at different frequencies than the IMU measurements. Every third image that has a detected aruco marker can be considered a keyframe.
KinDynVIOEstimator
which is the high level manager, by itself will also be a advanceable block and will collect measurements from the robot interface using the usual setXXX()
methods and add it to a std::deque
maintained as a shared resource.
For example, the IMU measurements can be maintained in a struct,
KinDynVIOEstimator
maintains a SharedResource<IMUPreIntegratorInput>
populating the std::deque
of measurements. Any thread-safe operations are done only at the KinDynVIOEstimator
level. The composed classes are assumed to be advanceable blocks. Similarly populating other relevant measurement sources.
KinDynVIOEstimator
dispatches the operation of multiple threads using the AdvanceableRunner
class, for example.
AdvanceableRunner<ImageProcessor>
AdvanceableRunner<BLFFilter>
AdvanceableRunner<Smoother>
Some other helper threads will be maintained for intermediate operations, for example multiple pre-integration threads between two image key frames, such that the all the pre-integration from different modules are carried out in parallel and once these computations finish, the threads are made to join the smoothing thread and destroyed. These minion threads include,
AdvanceableRunner<IMUPreintegrator>
AdvanceableRunner<ContactForecePreintegrator>
For example, given a IMU pre-integration block as follows,
one call of advance computes all the necessary pre-integration from the gathered queue of IMU measurements between two keyframes. In the parallel, some other pre-integration thread would be doing a similar computation for a different set of measurements.
For the future, where we might replace this IMU preintegration with other IMU preintegration factors, we templatized this class,
KinDynVIOManager
will maintain the overall nonlinear factor graph to which the factors from different modules will be added. Periodic updates (optimization step) will be called in a thread-safe manner.
one call of advance computes all the necessary pre-integration from the gathered queue of IMU measurements between two keyframes.
This might be a costly integration especially at the time of creation of the factor. It is preferable to integrate the measurement as soon as it is received, triggering the start of a certain preintegration and it stop using conditional variables, as recommended by @S-Dafarra. This is also recommended in GTSAM CombinedIMUFactor documentation.
In this way, we can avoid storing the IMU measurements in a std::deque
, and just store the latest received measurement.
Also, creation of threads during run-time is an expensive operation, so we can forego the idea of the intermediate dispatching of helper threads. Instead the preintegration thread might be continuously running depending on its periodicity with even-based triggers through conditional variables. (An alternate approach to periodic threads would be to use callbacks, similar to ROS subscriber mechanism, but at the moment, the periodic threads satisfy the need).
cc @traversaro @S-Dafarra