fovis covariance matrix

edit

Hello to all,

I am using the fovis_ros package to obtain visual odometry. The idea is to use /VO topic (fovis) with the /odom topic (wheels encoders) for fuse both with "robot_pose_ekf" for improving the odometry.

The problem is that fovis_ros does not provide covariance matrix for the topic /VO (nav_msgs/Odometry) and this is necessary for EKF. It returns this error: Covariance specified for measurement on topic vo is zero

How can i get a correct values of covariance from fovis_ros?

In the source code, concretely in odometer_base.hpp class, i have found this piece of code, which calculates a covariance matrix for twist message.

    // add covariance
    const Eigen::MatrixXd& motion_cov = visual_odometer_->getMotionEstimateCov();
    for (int i=0;i<6;i++){
      for (int j=0;j<6;j++){
        odom_msg_.twist.covariance[j*6+i] = motion_cov(i,j);

The obtained values for Twist covariance matrix are too small to be considered good:

[1.0240318769963366e-06, 7.692441466426239e-10, -1.9911528089072995e-07, -5.2796957491041375e-09, -2.5633597789858596e-07, -8.60147349444699e-08, 7.692441466426444e-10, 9.711796626980829e-07, -2.4700831577990073e-07, 2.556300559715661e-07, 3.419041953132108e-09, 3.2210942118914344e-08, -1.9911528089072992e-07, -2.470083157799008e-07, 7.664208205682412e-07, -6.317154910573681e-08, 5.162586172474304e-08, 9.394388105875607e-09, -5.279695749104114e-09, 2.556300559715661e-07, -6.317154910573681e-08, 7.424932382586588e-08, 2.479056779025341e-09, 9.87872901479658e-09, -2.563359778985859e-07, 3.419041953132118e-09, 5.162586172474303e-08, 2.479056779025348e-09, 7.035120877212811e-08, 2.3448211634054404e-08, -8.601473494446988e-08, 3.2210942118914344e-08, 9.394388105875604e-09, 9.878729014796584e-09, 2.34482116340544e-08, 6.328458778537417e-08]

And the covariance values for Pose message are all zeros.

Extra: I am using an Asus ProLive camera and openni2_launch.

Thank you very much in advance ;)