【C++】sophus : geometry.hpp 位姿（SE2 和 SE3）和（2D 直线\3D 平面）转换函数 (五)

这段代码定义了一系列在位姿（SE2 和 SE3）和几何实体（2D 直线和 3D 平面）之间进行转换的函数。它利用了 Sophus 库中已有的旋转表示（SO2 和 SO3）。

以下是函数的详细解释：

1. SO2 与直线（2D）：

• normalFromSO2(SO2<T> const& R_foo_line)

  ：从旋转矩阵R_foo_line 中提取 y 轴作为参考系 "foo" 中的直线法向量。

• SO2FromNormal(Vector2<T> normal_foo)

  ：根据参考系 "foo" 中的直线法向量构造旋转矩阵R_foo_line。

2. SO3 与平面（3D）：

• normalFromSO3(SO3<T> const& R_foo_plane)

  ：从旋转矩阵R_foo_plane 中提取 z 轴作为参考系 "foo" 中的平面法向量。

• rotationFromNormal(Vector3<T> const& normal_foo, ...)

  ：根据参考系 "foo" 中的平面法向量构造旋转矩阵R_foo_plane。它还接受可选参数，用于提示平面参考系的 x 轴和 y 轴方向。此函数确保平面法向量不接近零，并检查提示方向之间的正交性。

• SO3FromNormal(Vector3<T> const& normal_foo)

  ：使用rotationFromNormal 从平面法向量创建 SO3 对象。

3. SE2 与直线（2D）：

• lineFromSE2(SE2<T> const& T_foo_line)

  ：从 2D 位姿T_foo_line 中提取直线信息。它使用normalFromSO2 获取直线法向量，并使用平移分量作为到原点的距离。

• SE2FromLine(Line2<T> const& line_foo)

  ：根据参考系 "foo" 中的直线定义构造 2D 位姿T_foo_line。它假设直线由其自身参考系的 x 轴定义。

4. SE3 与平面（3D）：

• planeFromSE3(SE3<T> const& T_foo_plane)

  ：从 3D 位姿T_foo_plane 中提取平面信息。它使用normalFromSO3 获取平面法向量，并使用平移分量作为到原点的距离（沿负法向量方向）。

• SE3FromPlane(Plane3<T> const& plane_foo)

  ：根据参考系 "foo" 中的平面定义构造 3D 位姿T_foo_plane。它假设平面由其自身参考系的 XY 平面定义。

5. 超平面：

• makeHyperplaneUnique(Eigen::Hyperplane<T, N> const& plane)

  ：通过在必要时翻转法向量和偏移量以使偏移量为非负数，来确保超平面的唯一表示。

总而言之，这段代码提供了在 Sophus 库的位姿表示（SE2 和 SE3）的上下文中，表示和操作 2D 和 3D 空间中的直线和平面功能。它包含在这些表示之间进行转换的函数，并执行必要的有效性和唯一性检查。

简单来说，这段代码提供了一些数学工具函数，可以在位姿（包括位置和旋转）和几何对象（直线和平面）之间进行转换。比如，给定一个机器人的位姿，可以计算出它“看到”的某个平面的方程；反过来，给定一个平面的方程，也可以计算出一个和这个平面相关的位姿。这样做的好处是方便在机器人、计算机视觉等领域进行几何计算。

/// 变换位姿和超平面之间的关系。#pragma once // 防止头文件被重复包含#include "se2.hpp" // 导入 SE2 相关的头文件#include "se3.hpp" // 导入 SE3 相关的头文件#include "so2.hpp" // 导入 SO2 相关的头文件#include "so3.hpp" // 导入 SO3 相关的头文件#include "types.hpp" // 导入常用类型定义的头文件namespace Sophus { // 定义命名空间 Sophus/// 输入旋转 ``R_foo_plane``，返回沿 y 轴的对应直线法向量（在参考系 ``foo`` 中）。///template <class T>Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {  return R_foo_line.matrix().col(1); // 返回旋转矩阵的第二列}/// 输入参考系 foo 中的直线法向量，构建对应的旋转矩阵 ``R_foo_line``。// 前置条件：``normal_foo`` 不能接近零。///template <class T>SO2<T> SO2FromNormal(Vector2<T> normal_foo) {  SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "{}",                normal_foo.transpose()); // 确保法向量的范数大于零  normal_foo.normalize(); // 归一化法向量  return SO2<T>(normal_foo.y(), -normal_foo.x()); // 返回旋转矩阵}/// 输入旋转 ``R_foo_plane``，返回沿 z 轴的对应平面法向量（在参考系 ``foo`` 中）。///template <class T>Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {  return R_foo_plane.matrix().col(2); // 返回旋转矩阵的第三列}/// 输入参考系 foo 中的平面法向量，构建对应的旋转矩阵 ``R_foo_plane``。// 注意：``plane`` 的坐标系定义为法向量沿正 z 轴指向。可以为 ``plane`` 坐标系的 x 轴和 y 轴指定提示。// 前置条件：/// - ``normal_foo``、``xDirHint_foo`` 和 ``yDirHint_foo`` 不能接近零。/// - ``xDirHint_foo`` 和 ``yDirHint_foo`` 必须大致垂直。///template <class T>Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,                              Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0), T(0)),                              Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1), T(0))) {  SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),                "xDirHint ({}) 和 yDirHint ({}) 必须垂直。",                xDirHint_foo.transpose(), yDirHint_foo.transpose()); // 确保两个向量垂直  using std::abs;  using std::sqrt;  T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); // 计算 xDirHint_foo 的平方范数  T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); // 计算 yDirHint_foo 的平方范数  T const normal_foo_sqr_length = normal_foo.squaredNorm(); // 计算 normal_foo 的平方范数  SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",                xDirHint_foo.transpose()); // 确保 xDirHint_foo 的范数大于零  SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",                yDirHint_foo.transpose()); // 确保 yDirHint_foo 的范数大于零  SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "{}",                normal_foo.transpose()); // 确保 normal_foo 的范数大于零  Matrix3<T> basis_foo;  basis_foo.col(2) = normal_foo; // 将法向量设置为第三列  if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {    xDirHint_foo.normalize(); // 归一化 xDirHint_foo  }  if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {    yDirHint_foo.normalize(); // 归一化 yDirHint_foo  }  if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {    basis_foo.col(2).normalize(); // 归一化法向量  }  T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); // 计算法向量和 xDirHint_foo 的点积的绝对值  T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); // 计算法向量和 yDirHint_foo 的点积的绝对值  if (abs_x_dot_z < abs_y_dot_z) {    // basis_foo.z 和 xDirHint_foo 不平行。    basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); // 设置第二列为叉乘结果并归一化    basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); // 设置第一列为叉乘结果  } else {    // basis_foo.z 和 yDirHint_foo 不平行。    basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); // 设置第一列为叉乘结果并归一化    basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); // 设置第二列为叉乘结果  }  T det = basis_foo.determinant(); // 计算行列式  // 检查行列式是否为 1  SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),                "基础的行列式不是 1，而是 {}。基础是 \n{}\n", det, basis_foo);  return basis_foo; // 返回基础矩阵}/// 输入参考系 foo 中的平面法向量，构建对应的旋转矩阵 ``R_foo_plane``。// 详细信息请参阅 ``rotationFromNormal``。///template <class T>SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {  return SO3<T>(rotationFromNormal(normal_foo)); // 调用 rotationFromNormal 函数并返回 SO3}/// 返回一个直线（相对于参考系 ``foo``），给定在参考系 ``foo`` 中的 ``line`` 的位姿。// 注意：平面由 ``line`` 坐标系的 X 轴定义。///template <class T>Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {  return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); // 返回参数化直线}/// 返回位姿 ``T_foo_line``，给定参考系 ``foo`` 中的直线。// 注意：直线由 ``line`` 坐标系的 X 轴定义。///template <class T>SE2<T> SE2FromLine(Line2<T> const& line_foo) {  T const d = line_foo.offset(); // 获取直线的偏移量  Vector2<T> const n = line_foo.normal(); // 获取直线的法向量  SO2<T> const R_foo_plane = SO2FromNormal(n); // 从法向量构建 SO2  return SE2<T>(R_foo_plane, -d * n); // 返回 SE2}/// 返回一个平面（相对于参考系 ``foo``），给定在参考系 ``foo`` 中的 ``plane`` 的位姿。// 注意：平面由 ``plane`` 坐标系的 XY 平面定义。///template <class T>Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {  return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); // 返回超平面}/// 返回位姿 ``T_foo_plane``，给定参考系 ``foo`` 中的平面。// 注意：平面由 ``plane`` 坐标系的 XY 平面定义。///template <class T>SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {  T const d = plane_foo.offset(); // 获取平面的偏移量  Vector3<T> const n = plane_foo.normal(); // 获取平面的法向量  SO3<T> const R_foo_plane = SO3FromNormal(n); // 从法向量构建 SO3  return SE3<T>(R_foo_plane, -d * n); // 返回 SE3}/// 接收一个超平面，返回其唯一表示，确保 ``offset`` 为非负。///template <class T, int N>Eigen::Hyperplane<T, N> makeHyperplaneUnique(Eigen::Hyperplane<T, N> const& plane) {  if (plane.offset() >= 0) {    return plane; // 如果偏移量为非负，直接返回超平面  }  return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset()); // 否则，返回法向量和偏移量取反的超平面}}  // namespace Sophus

总结

1. normalFromSO2函数：

• 输入：SO2旋转矩阵R_foo_line

• 输出：对应的法向量，沿y轴方向

• 使用：用于提取旋转矩阵的第二列（y轴方向）

SO2FromNormal函数：

• 输入：参考系foo中的线法向量normal_foo

• 输出：对应的SO2旋转矩阵

• 注意：法向量normal_foo必须归一化且不得接近零

normalFromSO3函数：

• 输入：SO3旋转矩阵R_foo_plane

• 输出：对应的平面法向量，沿z轴方向

• 使用：用于提取旋转矩阵的第三列（z轴方向）

rotationFromNormal函数：

• 输入：参考系foo中的平面法向量normal_foo，以及x轴和y轴的提示方向

• 输出：对应的3x3旋转矩阵

• 注意：normal_foo,xDirHint_foo,yDirHint_foo必须归一化且不得接近零，且x轴和y轴方向必须垂直

SO3FromNormal函数：

• 输入：参考系foo中的平面法向量normal_foo

• 输出：对应的SO3旋转矩阵

• 使用：调用rotationFromNormal函数并返回SO3对象

lineFromSE2函数：

• 输入：SE2姿态T_foo_line

• 输出：对应的线对象

• 使用：提取线的法向量和平移量

SE2FromLine函数：

• 输入：参考系foo中的线对象line_foo

• 输出：对应的SE2姿态

• 使用：提取线的法向量和偏移量，并计算其SE2姿态

planeFromSE3函数：

• 输入：SE3姿态T_foo_plane

• 输出：对应的平面对象

• 使用：提取平面的法向量和平移量

SE3FromPlane函数：

• 输入：参考系foo中的平面对象plane_foo

• 输出：对应的SE3姿态

• 使用：提取平面的法向量和偏移量，并计算其SE3姿态

makeHyperplaneUnique函数：

• 输入：超平面plane

• 输出：唯一表示的超平面，确保偏移量不为负

• 使用：将法向量和偏移量取负，以确保偏移量不为负

这段代码实现了姿态和超平面之间的各种转换函数，包括从旋转矩阵提取法向量、从法向量构建旋转矩阵、从SE2/SE3姿态构建线/平面对象，以及确保超平面的唯一表示。通过这些转换函数，可以方便地在姿态和几何对象之间进行转换，提高代码的可读性和可维护性。