diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 3d7750a8261d35324bc2a04a705497f22e725989..b5e1d9b70ff52dca17bc257e19c910a1774564be 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -733,7 +733,7 @@ bool CollisionManagerAlgNode::get_last_odom(void)
 bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_pose, Eigen::Matrix3d& _blink_cov, const bool _front)
 {
   geometry_msgs::TransformStamped tf_aux;
-  tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink, tf_laser_map;
+  tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink;
   ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
 
   if (!load_tf_blink_laser(_front))
@@ -758,14 +758,10 @@ 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());
 
-  tf_laser_map = tf_map_laser.inverse();
-  Eigen::Quaternion<double> rot(tf_laser_map.getRotation().w(), tf_laser_map.getRotation().x(), tf_laser_map.getRotation().y(), tf_laser_map.getRotation().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> laser_map_rot(rot);
-  _blink_cov = laser_map_rot*_covariances;
-
-  for (unsigned int i=0; i<9; i++)
-    _blink_cov(i) = std::fabs(_blink_cov(i))*(_covariances(i) < 0? -1: 1);
+  Eigen::Transform<double,3,Eigen::Affine> map_laser_rot(rot);
+  _blink_cov = map_laser_rot.rotation()*_covariances*map_laser_rot.rotation().transpose();
 
   if (this->config_.debug)
   {
@@ -805,7 +801,7 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
 bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _odom, Eigen::Matrix3d& _odom_cov, Eigen::Vector3d& _odom_diff, const bool _front)
 {
   geometry_msgs::TransformStamped tf_aux;
-  tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_laser_odom, tf_odom_blink;
+  tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_odom_blink;
   Eigen::Vector3d old_odom;
   ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
 
@@ -845,11 +841,10 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
   tf_odom_new_blink = tf_odom_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
   _odom << tf_odom_new_blink.getOrigin().x(), tf_odom_new_blink.getOrigin().y(), tf2::getYaw(tf_odom_new_blink.getRotation());
 
-  tf_laser_odom = tf_odom_laser.inverse();
-  Eigen::Quaternion<double> rot(tf_laser_odom.getRotation().w(), tf_laser_odom.getRotation().x(), tf_laser_odom.getRotation().y(), tf_laser_odom.getRotation().z());
+  Eigen::Quaternion<double> rot(tf_odom_laser.getRotation().w(), tf_odom_laser.getRotation().x(), tf_odom_laser.getRotation().y(), tf_odom_laser.getRotation().z());
   
-  Eigen::Transform<double,3,Eigen::Affine> laser_odom_rot(rot);
-  _odom_cov = laser_odom_rot*_covariances;
+  Eigen::Transform<double,3,Eigen::Affine> odom_laser_rot(rot);
+  _odom_cov = odom_laser_rot.rotation()*_covariances*odom_laser_rot.rotation().transpose();
 
   for (unsigned int i=0; i<9; i++)
     _odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);