Closed yhabib29 closed 2 years ago
Wow, I always thought the two methods were the same, but it seems not. Thanks for the detailed investigation.
It looks like this was fixed in the ROS2 but not the ROS1 in https://github.com/rpng/open_vins/pull/226
Can you try the following?
Eigen::Vector3d quat_diff_vec = quat_diff.block(0, 0, 3, 1);
Eigen::Vector3d cov_vec = covariance.block(0, 0, 3, 3).inverse() * 2 * quat_diff.block(0, 0, 3, 1);
double ori_nees = 2 * quat_diff_vec.dot(cov_vec);
Eigen::Vector3d errpos = state_ekf.block(4, 0, 3, 1) - state_gt.block(5, 0, 3, 1);
double pos_nees = errpos.transpose() * covariance.block(3, 3, 3, 3).inverse() * errpos;
EDIT: You can try this PR branch https://github.com/rpng/open_vins/pull/237
Yes, I just tried your PR and it builds without problems ! Thanks
There is an error while building open_vins using Eigen 3.4.0:
Eigen linked Issue: #1664
Solution: Replace line 709 in
open_vins/ov_msckf/src/ros/ROS1Visualizer.cpp
by :