Revision history [back]

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