Read existing code base to summarize a good software architecture

[prashanthr05]

• https://github.com/HKUST-Aerial-Robotics/VINS-Mono
• https://github.com/uzh-rpg/vimo/tree/master/vimo_estimator
• Using Ceres-solver for optimization
• GTSAM examples https://github.com/borglab/gtsam/tree/develop/gtsam_unstable/examples
• Sliding window MAP example using Ceres
• ICP code https://github.com/arntanguy/icp Theory: Appendix B, Visual SLAM for humanoid robot localization and
  closed-loop control
• manif example on Smoothing and Mapping https://github.com/artivis/manif/blob/devel/examples/se3_sam.cpp

[prashanthr05]

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,

KinDynVIOManager members

    AdvanceableRunner<ImageProcessor> imgProcessorThread;
    std::shared_ptr<ImageProcessor> imgProcessor;
    std::shared_ptr< SharedResource <TimeStampedImg > > imgDataShared;

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,

IMUPreintegratorInput

struct IMUPreintegratorInput
{
    std::deque<double> ts;
    std::deque<Eigen::Vector3d> linAcc; // m per second per second
    std::deque<Eigen::Vector3d> gyro; // radians per second
    std::deque<Eigen::Vector3d> orientRPY; // RPY in radians

    gtsam::Key posei, posej; // IMU poses at ith and jth timestep
    gtsam::Key vi, vj; // IMU linear velocities at ith and jth timestep
    gtsam::Key bi, bj; // IMU biases at ith and jth timestep
};

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,

ForsterIMUPreintegrator

class ForsterIMUPreintegrator : public IMUPreintegrator<gtsam::CombinedImuFactor>
{
public:
    ForsterIMUPreintegrator();
    virtual ~ForsterIMUPreintegrator();

    bool initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler) final;
    bool setInput(const IMUPreintegratorInput& input) final;

    virtual bool advance() final;
    const gtsam::CombinedImuFactor& getOutput() const final;
    bool isOutputValid() const final;

    virtual bool getPredictedState(const IMUState& currentState,
                                   IMUState& predictedState) final;
    virtual void resetIMUIntegration() final;
    virtual void resetIMUIntegration(const gtsam::imuBias::ConstantBias& bias);
private:
    class Impl;
    std::unique_ptr<Impl> m_pimpl;
};

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,

Templatized IMUPreintegrator

template <typename PreintegratedFactor>
class IMUPreintegrator : public BipedalLocomotion::System::Advanceable<IMUPreintegratorInput, PreintegratedFactor>
{
public:
    virtual bool initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
    virtual bool setInput(const IMUPreintegratorInput& input) = 0;

    virtual bool advance() = 0;
    virtual const PreintegratedFactor& getOutput() const = 0;
    virtual bool isOutputValid() const = 0;

    virtual bool getPredictedState(const IMUState& currentState,
                                   IMUState& predictedState) = 0;
    virtual void resetIMUIntegration() = 0;
};

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.

[prashanthr05]

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