Ask Your Question
1

fovis covariance matrix

asked 2015-01-26 09:23:19 -0600

Jose Luis gravatar image

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

edit retag flag offensive close merge delete

Comments

HI Jose did you find any solution, i have the same problem

sophye_turtlebot gravatar imagesophye_turtlebot ( 2015-05-07 06:03:49 -0600 )edit

@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?

Jose Luis gravatar imageJose Luis ( 2015-05-20 08:55:19 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2015-02-06 07:57:37 -0600

Miquel Massot gravatar image

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?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-01-26 09:23:19 -0600

Seen: 313 times

Last updated: Feb 06 '15