fovis covariance matrix

Question

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 ;)

---

Originally posted by Jose Luis on ROS Answers with karma: 375 on 2015-01-26

Post score: 1

---

Original comments

Comment by sophye_turtlebot on 2015-05-07:
HI Jose did you find any solution, i have the same problem

Comment by Jose Luis on 2015-05-20:
@sophye_turtlebot, as Miquel Massot said in your question "fovis_ros error", I have modified odometer_base.hpp to set a fixed covariance matrix. I do not have the correct (empiric) values for covariance, but this arrangement works, at least EKF does not complain. Are you using a laser?

Answer 1

Hi Jose Luís,

Any visual odometer will give you its output in speeds rather than in poses. The pose in the topic is the only integration of the twist. The given motion covariance is the bare output of libfovis. To better understand how they calculate it, I encourage you to read their paper.

About the EKF, I would integrate the VO in speeds and do some tests with it. With those values, were you moving at a speed rate or were you static?

---

Originally posted by Miquel Massot with karma: 1471 on 2015-02-06

This answer was ACCEPTED on the original site

Post score: 0