坐标变换-tf::transform

  tf::Transform r_to_w(tf::createQuaternionFromYaw(robot_pose_[2]),tf::Vector3(robot_pose_[0],robot_pose_[1],0));
  tf::Transform t_to_w(tf::createQuaternionFromYaw(target_pose_[2]),tf::Vector3(target_pose_[0],target_pose_[1],0));
  tf::Transform t_to_r;
  t_to_r = r_to_w.inverse()*t_to_w;

  tf::Vector3 pose;
  pose[0] = t_to_r.getOrigin().x();
  pose[1] = t_to_r.getOrigin().y();
  pose[2] = tf::getYaw(t_to_r.getRotation());

 欧式旋转群对加法没有封闭性，但是对乘法具有封闭性，因此，不同时刻，同一物体的欧式变换要用乘法计算。