坐标变换-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());
欧式旋转群对加法没有封闭性,但是对乘法具有封闭性,因此,不同时刻,同一物体的欧式变换要用乘法计算。