Review literature for understanding merging of visual and centroidal factors with kinematic-inertial robot estimation

[prashanthr05]

Recent works post this review

• VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots 2021
• Legged Robot State Estimation With Dynamic Contact Event Information, 2021

Base Paper readings

• [x] VID-Fusion: Robust Visual-Inertial-Dynamic Odometry for Accurate External Force Estimation
• [x] VIMO: Simultaneous Visual Inertial Model-based Odometry and Force Estimation Code: https://github.com/uzh-rpg/vimo
• [x] VINS-MONO: A Robust and Versatile Monocular Visual-Inertial State Estimator Code: https://github.com/HKUST-Aerial-Robotics/VINS-Mono
• [x] Contact forces pre-integration for the whole body estimation of legged robots
• [x] Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry
• [x] IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation
• [x] Factor graphs for Robot Perception (long read but quite an useful tool)
• [x] Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs
• [x] Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors
• [x] Robust Legged Robot State Estimation using Factor Graph Optimization
• [x] Absolute humanoid localization and mapping based on IMU Lie group and fiducial markers
• [x] Preintegrated Velocity Bias Estimation to Overcome Contact Nonlinearities in Legged Robot Odometry

iSAM2 Background

• iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree

Supplementary readings

• High Accuracy Preintegration for Visual-Inertial Navigation
• Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems
• Online Variational Bayesian Motion Averaging
• An Invariant-EKF VINS Algorithm for Improving Consistency
• A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
• Fusion of Optical Flow and Inertial Measurements for Robust Egomotion Estimation
• A Factor Graph Approach to Estimation and Model Predictive Control on Unmanned Aerial Vehicles
• An Analytical Solution to the IMU Initialization Problem for Visual-Inertial Systems Code: https://github.com/dzunigan/imu_initialization

[prashanthr05]

IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation

This paper holds the premise that optimization outperforms filtering since the former allows for relinearization considering the past states. Filtering marginalizes all the previous states, holding only the previous state, while a fixed-lag smoothing holds last N states. In any case, both filtering and fixed-lag smoothing uses marginalization that depends on choosing proper linearization points, errors in which might lead to drifts and inconsistencies.

This paper uses incremental smoothing and mapping based optimization in order to estimate the state along with the IMU biases. This smoothing updates only a small set of state variables affected by the new measurements, leading to fast computations of optimal MAP estimate.

Uses preintegrated IMU measurements. IMU preintegration involves summarizing multiple IMU measurements occuring between two image keyframes within a single motion constraint. Additionally uses so-called structureless vision factors (however relies on an existing front end for obtaining 3d landmarks in the scene). Camera describes appearance and geometry of 3d scene upto an unknown scale. IMU makes the metric scale of monocular image and the gravity observable. So they're complementary measurement set.

Implementation using GTSAM toolbox 4.0. Boasts an average computation time of 10ms. GTSAM 4.0 includes an implementation of Preintegrated IMU factor.

[prashanthr05]

S.No	Paper/Year	Measurement modalities	Factor representations	Implementation remarks	Other remarks
1.	IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation 2015 (Forster's method)	- Pixel measurements of 3D landmarks with keyframe selection (monocular vision) -IMU	- Prior Factor - Preintergrated IMU factor with bias model (SO(3)) -Structureless vision factors by linear elimination of landmarks	- GTSAM iSAM2 solver backend -SVO front end for feature tracking by minimizing photometric error and outlier resistant probabilistic landmark triangulation	-Back end solver CPU time average 10ms -Accelerated computation by avoiding to include landmark positions in optimization, instead consider only poses observing the landmarks using a null space projection trick (as used in MSCKF)
2.	VINS-MONO: A Robust and Versatile Monocular Visual-Inertial State Estimator 2018	- Monocular camera - IMU	- IMU preintegration feature - Visual landmark features based on unit sphere based projection	- Ceres solver NLOPT - Key frame selection using parallax threshold between tracked features and minimum number of tracked features - FAST+KLT+RANSAC based feature tracking enforcing a uniform distribution of features across the image	- robust initialization of the estimator starting from SFM based vision only initialization followed by aligning IMU with the visual structure to initialize gyro bias, monocular scale, gravity - marginalization based on spatially separated keyframes - marginalization of old states using Schur's complement (might be computationally expensive - Bayes tree shown to be faster) - Huber norm robust cost function for the factors
3.	Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors 2018	- stereo camera module providing direct pose measurements - IMU - Joint Encoders -Binary Contact Sensors	- Prior State Factors -Local loop closure factores using pose measurements -Preintegrated IMU factors with bias models -Unary Forward Kinematic factors between base and contact frame -Preintegrated rigid/point contact factors	- GTSAM iSAM2 solver for NLOPT - Analytical Jacobians provided in supplementary material	See http://web.eecs.umich.edu/faculty/grizzle/papers/ICRAsupplement2018MRH.pdf
4.	Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs 2018	Same as [3]	Same as [1] with some improvements - FK factor generalized to include prismatic joints -Contact factors generalized into a Hybrid preintegrated contact factor in order to reduce number variables in the factor graph -Validation experiment with a terrain factor using the contact frame pose	- GTSAM iSAM2 for NLOPT solver -GTSAM built in IMU factor -SVO 2.0 for camera pose -Time synchronized multi-frequency measurements	- Iterative update forms for hybrid preintegration of contact factors between contact switching
5.	VIMO: Simultaneous Visual Inertial Model-based Odometry and Force Estimation
6.	Robust Legged Robot State Estimation using Factor Graph Optimization 2019	- Pixel measurements of 3d landmarks from keyframes -IMU -Joint encoders	- State priors (initialized using IMU) - Landmark priors (initialized using DLT transfrom from features observed in last N frames) -Preintegrated IMU factor with bias model - Reprojection error factor of landmark positions -Legged Odometry factors (obtained from an external filter) -Camera's zero velocity motion factor	-iSAM2 solver -Separate threads for measurement handlers - Factor graphs updated at every new camera measurement -Overal great system architecture	-Feature tracking using Harris Corner detection + KLT tracker + RANSAC outlier rejection - DLT for landmark triangulation -Marginalization of states older than 10s and lost landmarks
7.	Absolute humanoid localization and mapping based on IMU Lie group and fiducial markers 2019	- IMU - Fiducial marker measurements from AprilTag markers	- Preintegrated IMU factor using a novel IMU deltas Lie group (improvement over Forster method) - Fiducial marker factor	- Non linear maximum likelihood optimization using Ceres solver - Hardware synchronized measurements of camera images and IMU	- Novel IMU deltas Lie group that may allow to generalize other motion integration methods - Replacing cumbersome computations using recursive Lie group operations - Effect of pixel noise on pinhole transformation of marker corners to associate noise to the fiducial marker measurements - Ambiguities in orientation estimates of planar markers solved using dynamic covariance scaling in case of detected ambiguities (flips in rotations) - Towards loop closure and localization with known landmarks
8.	VID-Fusion: Robust Visual-Inertial-Dynamic Odometry for Accurate External Force Estimation
9.	Preintegrated Velocity Bias Estimation to Overcome Contact Nonlinearities in Legged Robot Odometry, 2020	- Stereo vision point landmark features - IMU - EKF based Legged Odometry velocity measurements accounting for multiple legs in contact replacing position constraints with velocity constraints to reduce Legged Odometry based drifts	- State priors - Preintegrated IMU factors with bias - Stereo vision landmark factors -Preintegrated velocity factor considering legged odometry based base velocity bias	- GTSAM iSAM2 - Parallel threads for IMU preint, velocity preint, stereo feature tracking and optimization - Optimizer collects when new keyframe is processed	- Graph variables limited to 500 - FAST+KLT+RANSAC for feature tracking - Dynamic Covariance Scaling based robust cost functions for outlier handling
10.	Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry 2021	- Point landmark feature measurements from a camera overlapped with LIDAR depth information - Plane and line feature measurements from LIDAR - IMU	- State priors -Preintegrated IMU factors -Mono/Stereo visual point landmark factors - PCL based geoemtric primitives: planes and lines factors - Zero velocity motion factor	- GTSAM iSAM2 solver - Parallel threads for sensor processing and optimization -Fixed lag smoothing based on iSAM2 with lag time between 5s and 10s	- Visual features using FAST+KLT+RANSAC -PCL based plane and line features using Eucliedean clustering and outlier removal for reducing size of pcls, associating lowest and highest curvatures to set of candidate planes and lines, segmentation using point-to-model distance and fitting planes using PROSAC robust fitting - Plane representation using unit normal and distance to origin, defines specific operators for residual definition - Line representation using rotation matrix and two scalars for direction of line and closest point between line and origin, defines specific operators for residual defnition - Numerical derivatives of planes and lines for NLOPT obtained through symmetric difference method - Dynamic Covariance Scaling based robust cost functions for outlier handling
11.	Contact forces pre-integration for the whole body estimation of legged robots 2021	- IMU - Joint encoders - Contact force measurements obtained through inverse dynamics	- State priors -Legged Odometry factors (for each foot in contact assuming reliable contact detection) - Preintegrated IMU deltas -Preintegrated contact force factors by transforming the foot contact wrenches to the base frame and relating it to the centroidal states (COM position, velocity and Angular momentum) considering also a COM bias term - Centroidal kinematics factor relating the base state with the centroidal states through kinematic measurements - Bias and drift priors	- Ceres solver NLOPT - Keyframes created at 4 Hz frequency	- Accounts for model errors leading to CoM biases - Generalized preintergration theory for proprioceptive measurements applied to IMU factors and contact force factors (centroidal dynamics) - Legged Odometry based keyframes for every foot in contact, created at low rates to keep the factor graph from saturating

[GiulioRomualdi]

Another interesting resource is https://gtsam.org/2019/09/18/legged-robot-factors-part-I.html In this example factor graph optimization is used to estimate the base and the contacts of a bipedal robot