c++ gtsam/geometry/Pose3.h详细介绍

gtsam/geometry/Pose3.h 是 GTSAM 库中的一个头文件，定义了 Pose3 类。Pose3 用于表示三维空间中的位姿（即位置和姿态），它是处理和表示三维空间变换的核心类之一。以下是对 Pose3 类的详细介绍：

`Pose3` 类概述

Pose3 类用于表示在三维空间中的一个位置和方向。它由一个 Rot3 对象（表示旋转）和一个 Point3 对象（表示平移）组成。Pose3 结合了旋转和位移，允许进行各种变换操作，如将点从一个坐标系转换到另一个坐标系。

主要功能

位姿表示：
- Pose3 结合了旋转 (Rot3) 和位移 (Point3)。
- 可以表示三维空间中的任何位置和方向。
转换操作：
- 将点从一个坐标系转换到另一个坐标系。
- 支持将其他 Pose3 对象与当前位姿进行组合。
基本运算：
- 位姿组合：将两个位姿结合成一个新位姿。
- 位姿逆：计算一个位姿的逆变换。

主要成员函数

构造函数

Pose3();  // 默认构造函数，表示单位变换（旋转为单位矩阵，平移为零向量）
Pose3(const Rot3& R, const Point3& t);  // 从旋转 (R) 和位移 (t) 创建
Pose3(const Matrix& M);  // 从4x4变换矩阵创建

访问旋转和位移

Rot3 rotation() const;  // 返回旋转部分
Point3 translation() const;  // 返回位移部分

变换操作

Point3 transformTo(const Point3& point) const;  // 将点从当前坐标系变换到世界坐标系
Pose3 inverse() const;  // 返回位姿的逆变换
Pose3 compose(const Pose3& other) const;  // 将两个位姿组合成一个新位姿

转换矩阵

Matrix matrix() const;  // 返回4x4的变换矩阵表示

输出

std::string ToString() const;  // 返回位姿的字符串表示

示例代码

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <iostream>int main() {// 创建旋转和位移gtsam::Rot3 rotation = gtsam::Rot3::Rz(45 * M_PI / 180.0); // 绕Z轴旋转45度gtsam::Point3 translation(1.0, 2.0, 3.0); // 位移// 创建 Pose3 对象gtsam::Pose3 pose(rotation, translation);// 输出 Pose3 的旋转部分和位移部分std::cout << "Rotation:\n" << pose.rotation().matrix() << std::endl;std::cout << "Translation:\n" << pose.translation().transpose() << std::endl;// 计算并输出 Pose3 的逆变换gtsam::Pose3 pose_inverse = pose.inverse();std::cout << "Inverse Pose:\n" << pose_inverse.ToString() << std::endl;// 创建一个点，并将其从当前坐标系变换到世界坐标系gtsam::Point3 point(1.0, 0.0, 0.0);gtsam::Point3 transformed_point = pose.transformTo(point);std::cout << "Transformed Point:\n" << transformed_point.transpose() << std::endl;return 0;
}