Write a GTSAM example using BLF estimators and ICRA 2021 dataset

[prashanthr05]

We can quickly get acquainted to using GTSAM library using the following test examples.

• Write a GTSAM example using BLF filters (InvEKF/LeggedOdometry/DILIGENT) as odometry factors with iSAM2 Fixed lag smoothing (Start with InvEKF). Reference examples in,

  • GTSAM https://github.com/borglab/gtsam/blob/develop/gtsam_unstable/examples/FixedLagSmootherExample.cpp
  • BLF filter https://github.com/dic-iit/element_floating-base-estimation/blob/feature/KinDynFusion/code/KinDynFusion/src/estimation/test/DiligentOdomTest.cpp

• Write a GTSAM example using BLF filters as odometry factors and GTSAM's preintegrated IMU factor with iSAM2 Fixed lag smoothing.

[prashanthr05]

Blocked by https://github.com/dic-iit/kindyn-vio/issues/25.