Get Covariance of Twist and Pose in vins_estimator/odometry

HKUST-Aerial-Robotics / VINS-Fusion

An optimization-based multi-sensor state estimator

GNU General Public License v3.0

3.35k stars 1.36k forks source link

Open mikodham opened 2 years ago

mikodham commented 2 years ago

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 commented 1 month ago

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