# How can I get ICP covariances using laser_scan_matcher?

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?

edit retag close merge delete

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

( 2014-03-12 15:12:37 -0600 )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.

( 2014-03-13 00:46:47 -0600 )edit

Sort by » oldest newest most voted

(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.

more

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

more