Get Covariance of Twist and Pose in vins_estimator/odometry

[mikodham]

I followed https://github.com/HKUST-Aerial-Robotics/VINS-Mono/issues/74 to get pose covariance, which can then be published in visualization.cpp as topic named vins_estimator/odometry as shown in the figure below.

On visualization.cpp, On estimator.cpp, function Estimator::optimization(), right before double2vector() function is called

However, any idea how to get twist covariance?

[zxytql]

@mikodham Hi, thanks for your contribution above! I try your code and I find that sometimes the cov value is large. Is this normal?