Fixed covariance rotation bug

src/collision_manager_alg_node.cpp

@@ -484,11 +484,14 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
   tf_map_new_blink = tf_map_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
   _blink_pose << tf_map_new_blink.getOrigin().x(), tf_map_new_blink.getOrigin().y(), tf2::getYaw(tf_map_new_blink.getRotation());
 
-  Eigen::Quaternion<double> rot(tf_map_laser.getOrigin().w(), tf_map_laser.getOrigin().x(), tf_map_laser.getOrigin().y(), tf_map_laser.getOrigin().z());
+  Eigen::Quaternion<double> rot(tf_map_laser.getRotation().w(), tf_map_laser.getRotation().x(), tf_map_laser.getRotation().y(), tf_map_laser.getRotation().z());
   
   Eigen::Transform<double,3,Eigen::Affine> map_laser_rot(rot);
   _blink_cov = map_laser_rot*_covariances;
 
+  for (unsigned int i=0; i<9; i++)
+    _blink_cov(i) = std::fabs(_blink_cov(i))*(_covariances(i) < 0? -1: 1);
+
   if (this->config_.debug)
   {
     if (_front)