Ask Your Question
1

How can I get ICP covariances using laser_scan_matcher?

asked 2011-11-22 03:15:05 -0500

In scan_tools laser_scan_matcher there's a parameter called do_covariance_computation which makes laser_scan_matcher calculate covariances but not publish them.

I would like to get the covariances of the ICP, since there's no topic published for this I am trying to get them adding some code to the laser_scan_matcher. At the moment I am adding this

std::cout << "cov_x_m: " << output_.cov_x_m->size1 << " " << output_.cov_x_m->size2 << std::endl

Doing so I get the following:

...
cov_x_m: 3 3 
cov_x_m: 3284753240 3220715007 
cov_x_m: 3 3 
cov_x_m: 3284753240 3220715007 
...

If I print the data when size is 3x3 I get something like:

...
cov_x_m: 3 3 COV: -0 0.00218628 0.00205292 0.000400059 0.000501095 -0.000222392 -0.00112383 0.000397344 -0.000885956 
cov_x_m: 3 3 COV: 0.000150193 0.00153727 0.00446424 -0.0020142 0.000833389 0.000602156 -0.00217345 0.00121861 -0.00240148 
...

My questions are:

  • Is this the right way to get this information?
  • Why the covariance sizes are not always 3x3?
  • Is normal that these covariances are so similar to 0?

Thanks in advance

edit retag flag offensive close merge delete

Comments

Same questions here ! can someone give more details in how obtaining the covariances ? Please
Anas gravatar imageAnas ( 2011-11-24 01:54:35 -0500 )edit

did you find answer for your problem ?

sai gravatar imagesai ( 2014-03-12 15:12:37 -0500 )edit
1

This is quite old but I suppose so, at the end I used the code: covariance[0] = gsl_matrix_get(output_.cov_x_m, 0, 0); to get an element of the 3x3 matrix.

martimorta gravatar imagemartimorta ( 2014-03-13 00:46:47 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2011-11-24 03:46:49 -0500

Anas gravatar image

(this is not a complete answer)

Hi,

I am trying to get the covariances of the ICP, I changed the code of laser_scan_matcher.cpp to this

geometry_msgs::PoseWithCovariance pose_msg;
  pose_msg.pose.position.x = w2b_.getOrigin().getX();
  pose_msg.pose.position.y = w2b_.getOrigin().getY();
  pose_msg.pose.orientation.z = sin(getYawFromQuaternion(w2b_.getRotation())/2);
  pose_msg.pose.orientation.w = cos(getYawFromQuaternion(w2b_.getRotation())/2);
  pose_msg.covariance[0] = output_.cov_x_m->data[0];
  pose_msg.covariance[1] = output_.cov_x_m->data[1];
  pose_msg.covariance[5] = output_.cov_x_m->data[2];
  pose_msg.covariance[6] = output_.cov_x_m->data[3];
  pose_msg.covariance[7] = output_.cov_x_m->data[4];
  pose_msg.covariance[11] = output_.cov_x_m->data[5];
  pose_msg.covariance[30] = output_.cov_x_m->data[6];
  pose_msg.covariance[31] = output_.cov_x_m->data[7];
  pose_msg.covariance[35] = output_.cov_x_m->data[8];
pose_publisher_.publish(pose_msg);

But I am not sure about what I have done (mainly the indexes), can I have a confirmation ? I also have small values. is the covariance between 0 and 1 ?

Thank you.

edit flag offensive delete link more
0

answered 2017-03-17 19:31:13 -0500

tb12 gravatar image

there are parameters publish_pose_with_covariance and publish_pose_with_covariance_stamped which will publish the ICP covariance (if it is being calculated) or instead a diagonal 1e-9 matrix if the ICP covariance is not calculated. I am not sure if this parameter was available when this question was asked, but I am posting this for anyone who comes across the question from now on and wants an answer :)

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

Stats

Asked: 2011-11-22 03:15:05 -0500

Seen: 695 times

Last updated: Mar 17 '17