这段代码定义了一系列在位姿(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
总结
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
函数:
SE3FromPlane
函数:
makeHyperplaneUnique
函数:
这段代码实现了姿态和超平面之间的各种转换函数,包括从旋转矩阵提取法向量、从法向量构建旋转矩阵、从SE2/SE3姿态构建线/平面对象,以及确保超平面的唯一表示。通过这些转换函数,可以方便地在姿态和几何对象之间进行转换,提高代码的可读性和可维护性。