坐標變換-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());

 歐式旋轉群對加法沒有封閉性，但是對乘法具有封閉性，因此，不同時刻，同一物體的歐式變換要用乘法計算。