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

news/2024/12/16 23:32:12/

这段代码定义了一系列在位姿(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姿态构建线/平面对象,以及确保超平面的唯一表示。通过这些转换函数,可以方便地在姿态和几何对象之间进行转换,提高代码的可读性和可维护性。


http://www.ppmy.cn/news/1555708.html

相关文章

OmniParser在windows上的安装(第三步)

按照OmniParser官方的技术文档&#xff0c;OmiParser是可以在网页端直接运行的&#xff0c;但是我尝试了&#xff0c;无法生成网页链接&#xff0c;原因是TCP没有响应&#xff0c;无法访问服务端&#xff08;我觉得应该是服务器的IP地址是M国&#xff0c;因此无法进行Ping通&am…

基础库urllib的使用

学习爬虫&#xff0c;其基本的操作便是模拟浏览器向服务器发出请求&#xff0c;那么我们需要从哪个地方做起呢?请求需要我们自己构造吗?我们需要关心请求这个数据结构怎么实现吗?需要了解 HTTP、TCP、IP层的网络传输通信吗?需要知道服务器如何响应以及响应的原理吗? 可能…

Envoy 服务发现原理大揭秘与核心要点概述

1 Envoy动态配置介绍 动态资源&#xff0c;是指由envoy通过xDS协议发现所需要的各项配置的机制&#xff0c;相关的配置信息保存 于称之为管理服务器&#xff08;Management Server &#xff09;的主机上&#xff0c;经由xDS API向外暴露&#xff1b;下面是一个 纯动态资源的基…

Golang学习笔记_07——基本类型

Golang学习笔记_04——递归函数 Golang学习笔记_05——延迟调用 Golang学习笔记_06——变量和常量 文章目录 基本类型1. 介绍2. 类型转换3. 类型推断4. 示例 2. 零值2.1 定义2.2 特性2.3 各类数据类型的零值2.4 零值的用途2.5 零值 && nil 比较2.6 示例 源码 基本类型 …

MySQL--》解析事务从隔离级别到死锁处理

目录 初识事务 事务特性 事务并发 事务隔离 初识事务 事务&#xff1a;是一组操作的集合&#xff0c;它是不可分割的工作单位&#xff0c;事务会把所有的操作作为一个整体一起向系统提交或撤销操作请求&#xff0c;即这些操作要么同时成功&#xff0c;要么同时失败&#x…

【论文笔记】Visual Prompt Tuning

&#x1f34e;个人主页&#xff1a;小嗷犬的个人主页 &#x1f34a;个人网站&#xff1a;小嗷犬的技术小站 &#x1f96d;个人信条&#xff1a;为天地立心&#xff0c;为生民立命&#xff0c;为往圣继绝学&#xff0c;为万世开太平。 基本信息 标题: Visual Prompt Tuning 作者…

python制造一个报错

在Python中&#xff0c;制造一个报错&#xff08;异常&#xff09;可以通过多种方式实现。最常见的方法之一是直接引发一个异常。以下是一些例子&#xff0c;展示了如何制造不同类型的报错&#xff1a; 引发ValueError异常&#xff1a; try:# 制造一个 ValueError 异常raise Va…

Vue3之TypeScript的支持

随着前端技术的飞速发展&#xff0c;Vue 3与TypeScript的组合已成为许多开发者的首选技术栈。Vue 3作为Vue.js的最新版本&#xff0c;引入了众多革新性的功能和优化&#xff0c;而TypeScript作为一种强大的静态类型检查语言&#xff0c;其严谨的类型系统和智能代码提示极大地增…