【C++】sophus : interpolate_details interpolate 两李群元素之间平滑插值 （十一）

interpolate_details 

这段代码为 Sophus 库的插值模块提供了一组工具，用于处理各种 Lie 群（如 SO(2)、SE(3)、Sim(3) 等）的特性查询，特别是关于路径歧义的检查。这些工具定义在 interp_details 命名空间中，通过模板化设计支持多种类型。

功能和用途

  1. 支持查询：

  • 检查特定类型（如 SO(2)、SE(3) 等）是否支持插值或其他操作。

  • 提供静态布尔值 supported 标志表示支持性。

路径歧义检查：

  • 检测在插值过程中是否存在“最短路径”的歧义问题。

  • 使用群元素的对数（log）来分析旋转角度是否接近 ππ（即方向翻转的特殊情况）。

核心设计

  1. Traits 模板结构：

  • 模板类 Traits<Group> 为不同群类型定义了特化实现。

  • 提供两个主要接口：

    • supported

      ：指明该群类型是否支持特性查询。

    • hasShortestPathAmbiguity(const Group&)

      ：检查给定群元素是否存在路径歧义。

路径歧义的判定逻辑：

  • 对于旋转相关群（如 SO(2)、SO(3) 等），通过计算对数映射的角度，并检测其是否接近 ππ 来判断。

  • 判断公式：

    7bbe926d175cc71471899e5afcf3adf7.png

  • 对非旋转群（如笛卡尔群 Cartesian），路径歧义不存在，直接返回 false。

特性与实现

ccc7b78eb3296e3502119f3b86a4725d.png

代码结构总结

  1. Traits 模板类：

  • 通过模板特化为不同群类型定义支持性和路径歧义逻辑。

静态接口：
  • supported

    ：是否支持路径歧义检查。

  • hasShortestPathAmbiguity

    ：检查特定群元素是否存在歧义。

递归委托：

  • RxSO 和 SE 群类型将路径歧义检测委托给对应的旋转群（SO(2) 或 SO(3)）。

优点

  1. 模块化设计：

  • 每个群类型的逻辑清晰，便于扩展和维护。

复用性高：

  • 高度复用旋转群的歧义检查逻辑（如 RxSO 群和 SE 群）。

效率与精度：

  • 使用精确的数学公式检测路径歧义，结合机器精度避免误判。

应用场景

  1. 插值与路径规划：

  • 在 Lie 群上进行插值时，确保路径选择正确无歧义。

运动学与控制：

  • 检查旋转群的插值路径，避免意外的翻转。

优化问题：

  • 确保在非线性优化中，旋转路径的选择符合最短路径原则。

示例

检查 SO(2) 群的路径歧义：
Sophus::SO2<double> foo_T_bar = Sophus::SO2<double>::exp(M_PI); // 旋转 180 度bool ambiguity = Sophus::interp_details::Traits<Sophus::SO2<double>>::hasShortestPathAmbiguity(foo_T_bar);// ambiguity == true
检查 SE(3) 群的路径歧义：
Sophus::SE3<double> foo_T_bar = Sophus::SE3<double>::rot(Sophus::SO3<double>::exp(Eigen::Vector3d(0, 0, M_PI)));bool ambiguity = Sophus::interp_details::Traits<Sophus::SE3<double>>::hasShortestPathAmbiguity(foo_T_bar);// ambiguity == true

#pragma once  // 防止头文件被多次包含
#include "cartesian.hpp"  // 引入自定义的cartesian.hpp头文件#include "rxso2.hpp"  // 引入自定义的rxso2.hpp头文件#include "rxso3.hpp"  // 引入自定义的rxso3.hpp头文件#include "se2.hpp"  // 引入自定义的se2.hpp头文件#include "se3.hpp"  // 引入自定义的se3.hpp头文件#include "sim2.hpp"  // 引入自定义的sim2.hpp头文件#include "sim3.hpp"  // 引入自定义的sim3.hpp头文件#include "so2.hpp"  // 引入自定义的so2.hpp头文件#include "so3.hpp"  // 引入自定义的so3.hpp头文件
namespace Sophus {  // 开始命名空间Sophusnamespace interp_details {  // 开始命名空间interp_details
template <class Group>struct Traits;  // 定义模板结构体Traits
template <class Scalar, int Dim>struct Traits<Cartesian<Scalar, Dim>> {  // 对模板结构体Traits进行特化，适用于Cartesian类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(Cartesian<Scalar, Dim> const&) {    return false;  // 确定笛卡尔坐标系不存在最短路径模糊性  }};
template <class Scalar>struct Traits<SO2<Scalar>> {  // 对模板结构体Traits进行特化，适用于SO2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {    using std::abs;  // 使用标准库中的abs函数    Scalar angle = abs(foo_T_bar.log());  // 获取角度的绝对值    Scalar const kPi = Constants<Scalar>::pi();  // 定义常量π    return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();  // 判断是否存在最短路径模糊性  }};
template <class Scalar>struct Traits<RxSO2<Scalar>> {  // 对模板结构体Traits进行特化，适用于RxSO2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<SO3<Scalar>> {  // 对模板结构体Traits进行特化，适用于SO3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {    using std::abs;  // 使用标准库中的abs函数    Scalar angle = abs(foo_T_bar.logAndTheta().theta);  // 获取角度的绝对值    Scalar const kPi = Constants<Scalar>::pi();  // 定义常量π    return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();  // 判断是否存在最短路径模糊性  }};
template <class Scalar>struct Traits<RxSO3<Scalar>> {  // 对模板结构体Traits进行特化，适用于RxSO3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());  // 调用SO3类型的方法进行判断  }};
template <class Scalar>struct Traits<SE2<Scalar>> {  // 对模板结构体Traits进行特化，适用于SE2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<SE3<Scalar>> {  // 对模板结构体Traits进行特化，适用于SE3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());  // 调用SO3类型的方法进行判断  }};
template <class Scalar>struct Traits<Sim2<Scalar>> {  // 对模板结构体Traits进行特化，适用于Sim2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(        foo_T_bar.rxso2().so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<Sim3<Scalar>> {  // 对模板结构体Traits进行特化，适用于Sim3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(        foo_T_bar.rxso3().so3());  // 调用SO3类型的方法进行判断  }};
}  // namespace interp_details}  // namespace Sophus

interpolate

该代码提供了一个在李群（Lie Group）上的插值函数，能够在两个李群元素之间进行平滑插值。

功能描述
  • 函数名称：interpolate
  • 参数：
    • foo_T_bar：第一个李群元素。
    • foo_T_baz：第二个李群元素。
    • p：插值因子，范围为[0, 1]，默认为 0.5。
  • 返回值：一个李群元素foo_T_quiz，表示插值后的结果。

    • 当p=0 时，返回foo_T_bar。

    • 当p=1 时，返回foo_T_baz。

    • 对于其他值，则返回两者之间按比例插值的中间值。

实现细节

  1. 插值计算：

  • 利用李群的逆变性质，先计算foo_T_bar.inverse() * foo_T_baz 得到两个李群元素之间的相对变化。

  • 将相对变化映射到李代数（Lie Algebra）进行线性插值：p * log(...)。

  • 使用G::exp 将结果从李代数映射回李群。

  • 最终通过左乘foo_T_bar 将插值结果变换回目标李群坐标。

边界检查：

  • 确保插值因子p 在[0, 1] 范围内，否则抛出错误。

支持的李群类型：

  • 通过interp_details::Traits<G>::supported 进行静态检查，确保李群类型支持插值操作。

用途

该函数适用于在李群表示的几何对象（如旋转、位姿等）之间进行平滑变换，广泛应用于计算机视觉、机器人学等领域中的插值问题。

/// @file  // 文件注释/// 李群的插值。
#pragma once  // 防止头文件被多次包含
#include <Eigen/Eigenvalues>  // 引入用于特征值计算的Eigen库头文件
#include "interpolate_details.hpp"  // 引入自定义的interpolate_details.hpp头文件
namespace Sophus {  // 开始命名空间Sophus
/// 这个函数在两个李群元素``foo_T_bar``和``foo_T_baz``之间进行插值，/// 插值因子为[0, 1]范围内的``alpha``。// 它返回姿态``foo_T_quiz``，其中``quiz``是``bar``和``baz``之间的一个帧。/// 如果``alpha=0``，它返回``foo_T_bar``。如果是1，它返回``foo_T_baz``。// （由于李群上的插值具有逆变性，我们可以等效地将输入参数视为``bar_T_foo``、/// ``baz_T_foo``，返回值为``quiz_T_foo``。）// 前提条件：``p``必须在[0, 1]范围内。///template <class G, class Scalar2 = typename G::Scalar>std::enable_if_t<interp_details::Traits<G>::supported, G> interpolate(    G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {  using Scalar = typename G::Scalar;  // 获取标量类型  Scalar inter_p(p);  // 将p转换为标量类型  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),                "p ({}) must in [0, 1].", (inter_p));  // 确保p在[0, 1]范围内  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());  // 进行插值计算并返回结果}
}  // namespace Sophus

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.rhkb.cn/news/491270.html

如若内容造成侵权/违法违规/事实不符，请联系长河编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

C 进阶 — 动态内存管理

C 进阶 — 动态内存管理

C 进阶 — 动态内存管理 主要内容 动态内存函数 mallocfreecallocrealloc 常见动态内存错误 经典练习 柔性数组 一 动态内存函数 之前已介绍的内存开辟方式有 int val 20;//在栈空间上开辟四个字节 char arr[10] {0};//在栈空间上开辟10个字节的连续空间上述开辟空间…
阅读更多...
Ubuntu K8s

Ubuntu K8s

https://serious-lose.notion.site/Ubuntu-K8s-d8d6a978ad784c1baa2fc8c531fbce68?pvs74 2 核 2G Ubuntu 20.4 IP 172.24.53.10 kubeadmkubeletkubectl版本1.23.01.23.01.23.0 kubeadm、kubelet 和 kubectl 是 Kubernetes 生态系统中的三个重要组件 kubeadm&#xff1a; 主…
阅读更多...
Java中的自定义异常

Java中的自定义异常

引言 Java编程中&#xff0c;异常处理是很关键的一步操作&#xff0c;它可以帮助我们更好的管理程序中的错误问题&#xff0c;Java本身内置了许多异常&#xff0c;但项目中我们需要自定义异常来提升错误问题的可读性和用户的产品体验&#xff0c;本文将深入探讨Java中的自定义异…
阅读更多...
【深度学习项目】目标检测之YOLO系列详解（一）

【深度学习项目】目标检测之YOLO系列详解（一）

介绍 YOLO&#xff08;You Only Look Once&#xff09;是一种实时目标检测算法&#xff0c;由Joseph Redmon等人提出。与传统的基于滑动窗口和区域提案的目标检测方法不同&#xff0c;YOLO将目标检测问题框架化为一个单一的回归问题&#xff0c;直接从图像像素预测边界框和类别…
阅读更多...
C# 位运算

C# 位运算

一、数据大小对应关系 说明&#xff1a; 将一个数据每左移一位&#xff0c;相当于乘以2。因此&#xff0c;左移8位就是乘以2的8次方&#xff0c;即256。 二、转换 1、 10进制转2进制字符串 #region 10进制转2进制字符串int number1 10;string binary Convert.ToString(num…
阅读更多...
计算机视觉单阶段实例分割实践指南与综述

计算机视觉单阶段实例分割实践指南与综述

概述 原文地址&#xff1a;https://towardsdatascience.com/single-stage-instance-segmentation-a-review-1eeb66e0cc49 实例分割是一项具有挑战性的计算机视觉任务&#xff0c;需要预测对象实例及其每像素分割掩码。这使其成为语义分割和目标检测的混合体。 自 Mask R-CNN …
阅读更多...
Oracle中COUNT函数对NULL和空字符串的处理方式

Oracle中COUNT函数对NULL和空字符串的处理方式

Oracle中&#xff0c;使用COUNT函数的时候&#xff0c;COUNT()和COUNT(null)得到的结果都是0&#xff0c;也就是说&#xff0c;如果我们COUNT中选择的那列属性中为null的或者的那行是不会被计数的。MySQL中count(null)效果和Oracle中一样&#xff0c;但是count()能正常计数。 在…
阅读更多...
sqlmap详解

sqlmap详解

一.sqlmap -u URL --forms sqlmap -u http://192.168.11.136:1337//978345210/index.php --forms 针对特定的 URL 进行 SQL 注入测试&#xff0c;特别是针对表单&#xff08;form&#xff09;的 POST 注入 forms&#xff1a;这个参数告诉 sqlmap 解析并测试目标 URL 中的表单…
阅读更多...
Flink CDC Schema Evolution 详解

Flink CDC Schema Evolution 详解

Flink CDC Schema Evolution 详解 github原文 glimpse flink-cdc-3 glimpse 源码基于 ~/project/flink_src/flink-cdc master !4 ❯ git remote -v origin https://github.com/apache/flink-cdc.git (fetch) origin https://github.com/apache/flink-cdc.git (push) ~/p…
阅读更多...
【Linux】结构化命令：if-then语句

【Linux】结构化命令：if-then语句

结构化命令structured command&#xff1a;允许脚本根据条件跳过部分命令&#xff0c;改变执行流程。 1、if-then语句 格式1&#xff1a; if command then commands fi 格式2&#xff1a; if command; then commands fi 运行if之后的command命令&#xff0c;如果它的退出状态码…
阅读更多...
23.DDD与微服务

23.DDD与微服务

学习视频来源&#xff1a;DDD独家秘籍视频合集 https://space.bilibili.com/24690212/channel/collectiondetail?sid1940048&ctype0 文章目录 DDD与微服务的关系1. DDD可以用微服务实现&#xff0c;也可以不用微服务实现2. DDD是微服务拆分的必须参考项之一3. 微服务架构…
阅读更多...
IDEA 未启用lombok插件的Bug

IDEA 未启用lombok插件的Bug

项目中maven已引用了lombok依赖&#xff0c;之前运行没有问题的&#xff0c;但有时启动会提示&#xff1a; java: You arent using a compiler supported by lombok, so lombok will not work and has been disabled. Your processor is: com.sun.proxy.$Proxy8 Lombok support…
阅读更多...
51c嵌入式~单片机~合集3

51c嵌入式~单片机~合集3

我自己的原文哦~ https://blog.51cto.com/whaosoft/12362395 一、STM32代码远程升级之IAP编程 IAP是什么 有时项目上需要远程升级单片机程序&#xff0c;此时需要接触到IAP编程。 IAP即为In Application Programming&#xff0c;解释为在应用中编程&#xff0c;用户自己的…
阅读更多...
Jenkins容器使用宿主机Docker（五）

Jenkins容器使用宿主机Docker（五）

DevOps之安装和配置 Jenkins (一) DevOps 之 CI/CD入门操作 (二) Sonar Qube介绍和安装&#xff08;三&#xff09; Harbor镜像仓库介绍&安装 &#xff08;四&#xff09; Jenkins容器使用宿主机Docker&#xff08;五&#xff09; Jenkins流水线初体验&#xff08;六&#…
阅读更多...
Python爬虫之Scrapy框架基础入门

Python爬虫之Scrapy框架基础入门

Scrapy 是一个用于Python的开源网络爬虫框架&#xff0c;它为编写网络爬虫来抓取网站数据并提取结构化信息提供了一种高效的方法。Scrapy可以用于各种目的的数据抓取&#xff0c;如数据挖掘、监控和自动化测试等。 【1】安装 pip install scrapy安装成功如下所示&#xff1a;…
阅读更多...
C++对象数组对象指针对象指针数组

C++对象数组对象指针对象指针数组

一、对象数组 对象数组中的每一个元素都是同类的对象&#xff1b; 例1 对象数组成员的初始化 #include<iostream> using namespace std;class Student { public:Student( ){ };Student(int n,string nam,char s):num(n),name(nam),sex(s){};void display(){cout<&l…
阅读更多...
git中的命令

git中的命令

目录 git的命令1.1 创建版本库1.2 添加文件和修改提交文件1.创建文件readme.txt, 此刻文件在工作区&#xff08;WorkSpace&#xff09;2.使用命令 git add readme.txt添加到暂存区里面去。3.提交文件到主分支4.修改文件&#xff0c;在文件中加入一行。查看git的状态 1.3 版本回…
阅读更多...
《Keras3 minist 手写数字AI模型训练22秒精度达到：0.97》

《Keras3 minist 手写数字AI模型训练22秒精度达到：0.97》

《Keras3 minist 手写数字AI模型训练22秒精度达到&#xff1a;0.97》 一、修改源码加上如下两条代码二、源码修改如下三、Keras3 minist 训练22秒结束&#xff0c;训练过程截图四、Keras3 minist 源码截图 一、修改源码加上如下两条代码 import os os.environ["KERAS_BAC…
阅读更多...
docker 容器相互访问

docker 容器相互访问

目前采用 network 方式 1. 创建自定义网络 docker network create network-group 如下 2. 相互访问的容器更改&#xff08;目前演示redis 以及netcore api 访问redis &#xff09; //redis 原有容器删除 跟之前区别就是加入 --network network-group docker run \ -p 6379:…
阅读更多...
YOLOv8目标检测(三*)_最佳超参数训练

YOLOv8目标检测(三*)_最佳超参数训练

YOLOv8目标检测(一)_检测流程梳理&#xff1a;YOLOv8目标检测(一)_检测流程梳理_yolo检测流程-CSDN博客 YOLOv8目标检测(二)_准备数据集&#xff1a;YOLOv8目标检测(二)_准备数据集_yolov8 数据集准备-CSDN博客 YOLOv8目标检测(三)_训练模型&#xff1a;YOLOv8目标检测(三)_训…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023