How can I get ICP covariances using laser_scan_matcher?

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

flag of United Kingdom of Great Britain and Northern Ireland

841 ●14 ●18 ●34 http://www.linkedin.co...

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 ( 2011-11-24 01:54:35 -0600 )edit

did you find answer for your problem ?

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

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

add a comment

0

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

Anas
126 ●3 ●4 ●8

(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

add a comment

0

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

tb12
21 ●3 ●4

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

add a comment

How can I get ICP covariances using laser_scan_matcher?

Comments

2 Answers

Question Tools

Stats

Related questions

How can I get ICP covariances using laser_scan_matcher? edit

Comments

2 Answers

Question Tools

Stats

Related questions

How can I get ICP covariances using laser_scan_matcher?