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

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

Pose3 类概述

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

主要功能

1. 位姿表示：

   • Pose3 结合了旋转 (Rot3) 和位移 (Point3)。
   • 可以表示三维空间中的任何位置和方向。

2. 转换操作：

   • 将点从一个坐标系转换到另一个坐标系。
   • 支持将其他 Pose3 对象与当前位姿进行组合。

3. 基本运算：

   • 位姿组合：将两个位姿结合成一个新位姿。
   • 位姿逆：计算一个位姿的逆变换。

主要成员函数

• 构造函数

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;
}

总结

Pose3 类在 GTSAM 中扮演着重要角色，提供了一种方便的方式来处理三维空间中的变换。它结合了旋转和位移，支持多种操作，如变换、组合和逆变换，非常适合用于处理SLAM（同时定位与建图）和其他需要三维空间变换的应用。