gtsamPose3 格式

gtsam::Pose3是GTSAM库中的一个类，用于表示3D空间中的姿态（位置和方向）。它在机器人感知、定位和导航等领域具有广泛的应用。

gtsam::Pose3类的定义如下：

class Pose3 {
public:
    // 构造函数
    Pose3();
    Pose3(const Rot3& R, const Point3& t);
    Pose3(const Pose3& pose);
    
    // 获取姿态的旋转矩阵
    Rot3 rotation() const;
    
    // 获取姿态的平移向量
    Point3 translation() const;
    
    // 设置姿态的旋转矩阵和平移向量
    void setPose(const Rot3& R, const Point3& t);
    
    // 运算符重载
    Pose3 operator*(const Pose3& pose) const;
    Pose3& operator*=(const Pose3& pose);
    bool operator==(const Pose3& pose) const;
    
    // 其他常用函数
    Point3 transformFrom(const Point3& point) const;
    Point3 transformTo(const Point3& point) const;
    Pose3 inverse() const;
};

gtsam::Pose3类提供了一系列操作函数，可以用于姿态之间的乘法、求逆、转换等操作。可以通过调用rotation()和translation()函数获取姿态的旋转矩阵和平移向量。

使用gtsam::Pose3类，可以方便地表示和计算3D空间中的姿态信息，对于机器人的姿态估计和运动规划等任务非常有用