tf::transformPose,tf::Transform,tf::Stamped,tf::Pose用法

1.tf::transformPose

tf::transformPose是一个用于对姿态进行坐标变换的函数。它允许将一个姿态从一个坐标系转换到另一个坐标系，并返回转换后的姿态。

以下是tf::transformPose的详细用法：

bool tf::Transformer::transformPose(const std::string& target_frame,
                                     const tf::Stamped<tf::Pose>& stamped_in,
                                     tf::Stamped<tf::Pose>& stamped_out) const;

target_frame：目标坐标系的名称，表示要将指定的姿态转换到的坐标系。
stamped_in：输入姿态的时间戳和值。它是一个tf::Stamped类型的对象。时间戳指定该姿态的时间，并且姿态的坐标系由此stamped_in对象所定义。
stamped_out：输出参数，表示转换后的姿态。它是一个tf::Stamped类型的对象，用于存储转换后的结果。时间戳和坐标系将与输入姿态一致。

该函数用于将 stamped_in 的姿态从其所属的坐标系转换到target_frame中指定的坐标系。转换后的姿态将存储在stamped_out参数中。

函数返回一个布尔值，表示是否成功进行坐标变换。如果转换成功，则返回true，否则返回false。

以下是一个示例代码，展示了如何使用tf::transformPose进行姿态的坐标变换：

#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pose_transform_example");
  ros::NodeHandle nh;

  tf::TransformListener listener;

  tf::Stamped<tf::Pose> pose_in;
  // 填充输入姿态的时间戳、坐标系和姿态值
  pose_in.stamp_ = ros::Time::now();
  pose_in.frame_id_ = "base_link";
  pose_in.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
  pose_in.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));

  tf::Stamped<tf::Pose> pose_out;
  std::string target_frame = "target_frame";

  try
  {
    // 进行姿态的坐标变换
    bool success = listener.transformPose(target_frame, pose_in, pose_out);

    if (success)
    {
      // 打印转换后的姿态
      ROS_INFO("Transformed pose: [%f, %f, %f]",
               pose_out.getOrigin().x(),
               pose_out.getOrigin().y(),
               pose_out.getOrigin().z());
    }
    else
    {
      ROS_ERROR("Failed to transform pose!");
    }
  }
  catch (tf::TransformException& ex)
  {
    ROS_ERROR("Transform exception: %s", ex.what());
  }

  return 0;
}

在上述代码中，使用 tf::Stamped类型的对象pose_in表示输入姿态，并通过setOrigin()和setRotation()方法设置其位置和旋转。同时设置pose_in的时间戳和坐标系。

创建一个目标坐标系的名称target_frame，并创建一个tf::Stamped类型的对象pose_out作为输出参数。

然后，使用tf::TransformListener的transformPose()方法进行姿态的坐标变换。如果成功进行了坐标变换，则转换后的姿态将存储在pose_out中，并可以通过其getOrigin()方法获得位置信息和旋转信息。

最后，在成功进行坐标变换后，输出转换后的姿态信息；如果发生异常或失败，则输出相应的错误信息。

需要根据实际应用场景，适当调整坐标系名称和其他参数，并确保正确引入所需的头文件。

2.tf::Transform

tf::Transform 是TF库中用于表示坐标系之间的变换的类。它包含了旋转和平移变换的信息，可以用于表示从一个坐标系到另一个坐标系的变换。

以下是tf::Transform类的定义和常用方法：

class tf::Transform {
public:
  tf::Transform();
  tf::Transform(const tf::Quaternion& q, const tf::Vector3& v);
  
  void setRotation(const tf::Quaternion& q);
  void setOrigin(const tf::Vector3& v);
  
  const tf::Quaternion& getRotation() const;
  const tf::Vector3& getOrigin() const;

  tf::Vector3 operator*(const tf::Vector3& v) const;
  tf::Transform operator*(const tf::Transform& t) const;
};

tf::Transform()：默认构造函数创建一个无转换的坐标系，即单位旋转矩阵和零平移向量。
tf::Transform(const tf::Quaternion& q, const tf::Vector3& v)：构造函数创建一个具有指定旋转和平移的坐标系变换，其中q表示旋转的四元数，v表示平移的向量。
setRotation(const tf::Quaternion& q)：设置变换的旋转部分，参数q为旋转的四元数。
setOrigin(const tf::Vector3& v)：设置变换的平移部分，参数v为平移的向量。
const tf::Quaternion& getRotation() const：获取变换的旋转部分，返回旋转的四元数。
const tf::Vector3& getOrigin() const：获取变换的平移部分，返回平移的向量。
tf::Vector3 operator*(const tf::Vector3& v) const：重载乘法运算符，将一个向量乘以该变换矩阵，返回变换后的向量。
tf::Transform operator*(const tf::Transform& t) const：重载乘法运算符，将该变换矩阵与另一个变换矩阵相乘，返回两个变换组合后的结果。

以下是一个示例代码，演示了如何使用tf::Transform类进行坐标变换：

#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "transform_example");
  ros::NodeHandle nh;

  tf::Vector3 translation(1.0, 0.0, 0.0);       // 平移向量
  tf::Quaternion rotation(0.0, 0.0, 0.0, 1.0); // 旋转四元数

  tf::Transform transform;
  transform.setOrigin(translation);  // 设置平移部分
  transform.setRotation(rotation);  // 设置旋转部分

  tf::Vector3 point(1.0, 2.0, 3.0);  // 待变换的向量
  tf::Vector3 transformedPoint = transform * point; // 变换后的向量

  ROS_INFO("Original point: [%f, %f, %f]", point.x(), point.y(), point.z());
  ROS_INFO("Transformed point: [%f, %f, %f]", transformedPoint.x(), transformedPoint.y(), transformedPoint.z());

  return 0;
}

以上代码中，首先创建了一个tf::Vector3类型的平移向量translation，表示在x轴方向上平移1.0个单位长度。

然后，创建了一个tf::Quaternion类型的旋转四元数rotation，表示单位旋转。

接下来，使用默认构造函数创建了一个无转换的坐标系变换transform，并通过setOrigin()和setRotation()方法分别设置变换的平移和旋转。

之后，定义了一个待变换的向量point，并使用操作符*将该向量与变换矩阵相乘，得到变换后的向量transformedPoint。

最后，输出原始向量和变换后的向量的坐标。

例子：

centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
                                                                   tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);

这段代码用于创建一个tf::Stamped类型的变量centered_laser_pose_，它表示一个时间戳的姿态变换。

代码中使用了tf::Transform的构造函数来创建一个具有平移和旋转的坐标变换。这个坐标变换的平移部分是零向量tf::Vector3(0, 0, 0)，即表示平移到原点。旋转部分通过tf::createQuaternionFromRPY()函数创建，它接受Roll、Pitch和Yaw角度（在这里Roll和Pitch角度均为0，Yaw角度为angle_center）并返回对应的旋转四元数。传递给tf::Transform构造函数的参数就是这个旋转四元数和平移向量。

最后，tf::Stamped类型的变量是包含了时间戳信息的姿态变换。调用ros::Time::now()来获取当前时间作为时间戳，并将之前创建的坐标变换和时间戳传递给tf::Stamped类的构造函数，生成了最终的centered_laser_pose_对象。

这段代码用于创建一个表示激光雷达相对于某个参考坐标系平移到原点并在当前时间的坐标变换。

3.tf::Stamped

tf::Stamped是用于在姿态或变换中附加时间戳信息的模板类。该类是TF库中用于存储具有时间戳的姿态和变换的容器，在机器人系统中经常被使用。

以下是tf::Stamped类的定义和常用方法：

template <class T>
class tf::Stamped {
public:
  tf::Stamped();
  tf::Stamped(const T& value, const ros::Time& stamp, const std::string& frame_id);
  
  const T& getData() const;
  ros::Time getStamp() const;
  std::string getFrameId() const;
  
  void setData(const T& data);
  void setStamp(const ros::Time& stamp);
  void setFrameId(const std::string& frame_id);

  tf::Stamped<T> operator*(const tf::Transform& transform) const;
};

tf::Stamped()：默认构造函数创建一个空的tf::Stamped对象。
tf::Stamped(const T& value, const ros::Time& stamp, const std::string& frame_id)：构造函数创建一个附加了时间戳和坐标系的tf::Stamped对象，其中value表示姿态或变换的值，stamp表示时间戳，frame_id表示坐标系的名称。
const T& getData() const：获取姿态或变换的值。
ros::Time getStamp() const：获取时间戳。
std::string getFrameId() const：获取坐标系的名称。
void setData(const T& data)：设置姿态或变换的值。
void setStamp(const ros::Time& stamp)：设置时间戳。
void setFrameId(const std::string& frame_id)：设置坐标系的名称。
tf::Stamped operator*(const tf::Transform& transform) const：重载乘法运算符，将tf::Stamped对象与一个变换矩阵相乘，返回新的tf::Stamped对象。

tf::Stamped是一个模板类，可以将任何类型的姿态或变换作为其T参数。通常情况下，tf::Stamped和tf::Stamped是最常用的两种类型，分别用于存储带时间戳的姿态和变换。

以下是一个示例代码，演示如何使用tf::Stamped类来存储带时间戳的姿态变换：

#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stamped_example");
  ros::NodeHandle nh;

  tf::Pose pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(1.0, 2.0, 3.0));
  ros::Time stamp = ros::Time::now();
  std::string frame_id = "base_link";

  tf::Stamped<tf::Pose> stamped_pose(pose, stamp, frame_id);

  ROS_INFO("Stamped pose data: [%f, %f, %f, %f, %f, %f]",
           stamped_pose.getData().getOrigin().x(),
           stamped_pose.getData().getOrigin().y(),
           stamped_pose.getData().getOrigin().z(),
           stamped_pose.getData().getRotation().x(),
           stamped_pose.getData().getRotation().y(),
           stamped_pose.getData().getRotation().z());

  ROS_INFO("Stamped pose timestamp: %f", stamped_pose.getStamp().toSec());
  ROS_INFO("Stamped pose frame id: %s", stamped_pose.getFrameId().c_str());

  return 0;
}

在上述代码中，首先创建了一个tf::Pose类型的姿态对象pose，表示旋转四元数和平移向量。

然后，使用ros::Time::now()来获取当前时间并赋值给stamp变量。

最后，创建了一个tf::Stamped类型的对象stamped_pose，并通过构造函数分别传递了姿态数据、时间戳和坐标系名称。

通过调用对象的方法，可以获取姿态数据、时间戳和坐标系名称，并在终端中输出。

除了上述提到的常用方法之外，tf::Stamped类还提供了其他一些辅助方法和操作符重载，下面是它们的详细说明：

bool operator==(const Stamped& other) const：重载相等运算符，判断两个tf::Stamped对象是否相等。
bool operator!=(const Stamped& other) const：重载不等运算符，判断两个tf::Stamped对象是否不相等。
void stamp_ = ros::Time(0)：时间戳的默认值为0，表示没有指定时间戳。
bool is_zero() const：判断时间戳是否为0，即是否没有指定时间戳。
void setData(const T& data, bool copy = true)：设置姿态或变换的值，并选择是否进行深拷贝，默认进行拷贝操作。
void setValidity(bool valid)：设置tf::Stamped对象的有效性。如果valid为true，则对象被认为是有效的；如果valid为false，则对象被认为是无效的。
bool isValid() const：判断tf::Stamped对象是否有效。
std::string frame_id_：坐标系名称，可通过setFrameId()方法进行设置和获取。

以上是tf::Stamped类的常用方法和属性。根据具体的应用场景和需求，可以灵活使用这些方法来操作和管理附带时间戳的姿态和变换数据。请根据实际需要选择适当的方法来使用。

4.tf::Pose

tf::Pose是TF库中的一个类，用于表示3D空间中的姿态（位置和方向）。它提供了许多方法和操作符重载，用于创建、获取、设置和变换姿态。

以下是tf::Pose类的定义和常用方法：

class tf::Pose {
public:
  Pose();
  Pose(const tf::Quaternion& rotation, const tf::Vector3& translation);
    
  const tf::Quaternion& getRotation() const;
  const tf::Vector3& getOrigin() const;
  tf::Vector3 getBasisX() const;
  tf::Vector3 getBasisY() const;
  tf::Vector3 getBasisZ() const;
  
  void setRotation(const tf::Quaternion& rotation);
  void setOrigin(const tf::Vector3& translation);
    
  void setRotation(double x, double y, double z, double w);
  void setOrigin(double x, double y, double z);
  
  void setIdentity();
  
  void mult(const Pose& pose1, const Pose& pose2);
  Pose operator*(const Pose& p) const;

  tf::Point operator*(const tf::Point& pt) const;
  tf::Vector3 operator*(const tf::Vector3& vec) const;
};

Pose()：默认构造函数创建一个单位姿态，表示无旋转和平移。
Pose(const tf::Quaternion& rotation, const tf::Vector3& translation)：构造函数创建一个具有给定旋转和平移的姿态对象。
const tf::Quaternion& getRotation() const：获取姿态对象的旋转四元数。
const tf::Vector3& getOrigin() const：获取姿态对象的平移向量。
tf::Vector3 getBasisX() const：获取姿态对象的X轴方向的向量。
tf::Vector3 getBasisY() const：获取姿态对象的Y轴方向的向量。
tf::Vector3 getBasisZ() const：获取姿态对象的Z轴方向的向量。
void setRotation(const tf::Quaternion& rotation)：设置姿态对象的旋转四元数。
void setOrigin(const tf::Vector3& translation)：设置姿态对象的平移向量。
void setRotation(double x, double y, double z, double w)：按给定的四个参数设置姿态对象的旋转四元数。
void setOrigin(double x, double y, double z)：按给定的三个参数设置姿态对象的平移向量。
void setIdentity()：将姿态对象设置为单位姿态，即无旋转和平移。
void mult(const Pose& pose1, const Pose& pose2)：将两个姿态对象进行组合，将结果存储在当前对象中。
Pose operator*(const Pose& p) const：重载乘法运算符，将当前姿态对象和另一个姿态对象相乘，返回新的姿态对象。
tf::Point operator*(const tf::Point& pt) const：重载乘法运算符，将当前姿态对象和一个点对象相乘，返回变换后的点对象。
tf::Vector3 operator*(const tf::Vector3& vec) const：重载乘法运算符，将当前姿态对象和一个向量对象相乘，返回变换后的向量对象。

除了以上方法，tf::Pose还提供了一些其他方法和辅助函数，用于进行坐标变换、插值、单位化、求逆等操作。具体使用方法可以根据实际需求和具体问题进行查阅和使用。

以下是一个示例代码，演示如何使用tf::Pose类来表示和操作姿态数据：

#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pose_example");
  ros::NodeHandle nh;

  // Create a pose with rotation and translation
  tf::Quaternion rotation(0, 0, 0, 1); // No rotation
  tf::Vector3 translation(1.0, 2.0, 3.0); // Translation

  // Create a Pose object
  tf::Pose pose(rotation, translation);

  // Get the rotation and origin
  tf::Quaternion poseRotation = pose.getRotation();
  tf::Vector3 poseOrigin = pose.getOrigin();

  // Print the rotation and origin
  ROS_INFO("Pose rotation: [%f, %f, %f, %f]", poseRotation.x(), poseRotation.y(), poseRotation.z(), poseRotation.w());
  ROS_INFO("Pose origin: [%f, %f, %f]", poseOrigin.x(), poseOrigin.y(), poseOrigin.z());

  // Set a new rotation and origin
  tf::Quaternion newRotation(1, 0, 0, 0); // New rotation quaternion
  tf::Vector3 newTranslation(-1.0, -2.0, -3.0); // New translation vector
  pose.setRotation(newRotation);
  pose.setOrigin(newTranslation);

  // Get the updated rotation and origin
  poseRotation = pose.getRotation();
  poseOrigin = pose.getOrigin();

  // Print the updated rotation and origin
  ROS_INFO("Updated pose rotation: [%f, %f, %f, %f]", poseRotation.x(), poseRotation.y(), poseRotation.z(), poseRotation.w());
  ROS_INFO("Updated pose origin: [%f, %f, %f]", poseOrigin.x(), poseOrigin.y(), poseOrigin.z());

  return 0;
}

在上述代码中，我们首先创建了一个旋转四元数rotation，它表示没有旋转的情况。然后，我们创建了一个平移向量translation，表示在x、y和z轴上的平移。

然后，我们使用这些旋转和平移参数创建了一个tf::Pose对象pose。

接下来，我们使用getRotation()和getOrigin()方法从pose对象中获取旋转四元数和平移向量，并打印到终端中。

然后，我们创建了一个新的旋转四元数newRotation和一个新的平移向量newTranslation，并使用setRotation()和setOrigin()方法将它们设置到pose对象中。

最后，我们再次使用getRotation()和getOrigin()方法从pose对象中获取更新后的旋转四元数和平移向量，并在终端中打印。

在实际应用中，可以通过调用getRotation()和getOrigin()方法来获取姿态对象的旋转和平移信息，并根据需求使用setRotation()和setOrigin()方法来设置新的旋转和平移。还可以使用乘法运算符重载函数对姿态对象进行变换操作，以及使用其他辅助方法进行坐标系转换、插值、单位化等操作。

tf::Pose 可以通过tf::Transform构造。tf::Transform是另一个在TF库中用于表示姿态变换的类。它包含了一个旋转（tf::Quaternion）和平移（tf::Vector3）的组合，用于描述姿态变换。

tf::Transform tfTransform; // Create a tf::Transform object
tf::Pose tfPose(tfTransform); // Use tf::Transform object to construct a tf::Pose object

tf::Pose tfPose; // Create a tf::Pose object
tf::Transform tfTransform(tfPose); // Use tf::Pose object to construct a tf::Transform object

由于两者的构造函数和旋转/平移的获取和设置方法类似，可以使用tf::Transform对象作为参数来构造tf::Pose对象，或者使用一个已经存在的tf::Pose对象来初始化tf::Transform对象。