0
$\begingroup$

Rosanswers logo

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


Originally posted by martimorta on ROS Answers with karma: 843 on 2011-11-22

Post score: 1


Original comments

Comment by Anas on 2011-11-24:
Same questions here ! can someone give more details in how obtaining the covariances ? Please

Comment by sai on 2014-03-12:
did you find answer for your problem ?

Comment by martimorta on 2014-03-13:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by Anas with karma: 126 on 2011-11-24

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.