【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)

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