【C++】sophus : sim_details.hpp 实现了矩阵函数 W、其导数，以及其逆 (十七)

server/2024/12/24 5:17:49/

5653d5ac4002742681a7c73bfd8b3e4a.png

这段代码主要用于计算与旋转矩阵和指数相关的矩阵运算，是Sophus库的一部分，Sophus是用于几何运算的C++库。以下是对代码的总结：

主要功能

  • calcW: 计算矩阵W，该矩阵与旋转矩阵和指数有关。根据不同的theta和sigma值，进行特定的矩阵计算。

  • calcW_derivatives: 计算矩阵W及其关于theta和sigma的导数。

  • calcWInv: 计算矩阵W的逆矩阵，这同样与旋转矩阵和指数有关。

命名空间

  • Sophus::details: 包含所有实现细节的命名空间。

主要步骤

  1. 初始化常数和变量: 包括单位矩阵、常数0、1、0.5等。

  2. 计算矩阵W:

  • 当sigma接近0时，采用简化的计算方法。

  • 当theta接近0时，采用特定的公式计算A和B。

  • 在常规情况下，使用theta和sigma的值计算A、B和C，并组合成矩阵W。

计算W的导数:

  • 根据theta和sigma的值，计算A、B、C及其导数A_dsigma, B_dsigma, C_dsigma, A_dtheta, B_dtheta。

  • 处理特殊情况：当theta或sigma接近0时，使用特定公式计算。

计算W的逆矩阵:

  • 处理特殊情况：当sigma的平方或theta的平方接近0时，采用特定公式计算a和b。

  • 在常规情况下，使用theta、sigma和scale的值计算a、b和c，并组合成W的逆矩阵。

主要函数

  • calcW(Matrix<Scalar, N, N> const &Omega, Scalar const theta, Scalar const sigma): 返回矩阵W。

  • calcW_derivatives(Scalar const theta, Scalar const sigma, Scalar &A, Scalar &B, Scalar &C, Scalar &A_dsigma, Scalar &B_dsigma, Scalar &C_dsigma, Scalar &A_dtheta, Scalar &B_dtheta): 计算W及其导数。

  • calcWInv(Matrix<Scalar, N, N> const &Omega, Scalar const theta, Scalar const sigma, Scalar const scale): 返回矩阵W的逆矩阵。

用途

这些函数用于处理与旋转和平移相关的矩阵运算，特别是在Sophus库中，用于计算SE(3)变换和其相关操作。这些计算对于机器人学、计算机视觉和几何运算等领域非常重要。

0fd31f63fe1941c2dc927506eaa109f7.png

#pragma once // 防止头文件被重复包含
#include "types.hpp" // 包含types头文件
namespace Sophus { // Sophus命名空间namespace details { // 详细实现命名空间
template <class Scalar, int N>Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const &Omega, // 计算矩阵W                           Scalar const theta, Scalar const sigma) {  using std::abs;  using std::cos;  using std::exp;  using std::sin;  static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity(); // 单位矩阵  static Scalar const one(1); // 常数1  static Scalar const half(0.5); // 常数0.5  Matrix<Scalar, N, N> const Omega2 = Omega * Omega; // Omega的平方  Scalar const scale = exp(sigma); // 计算指数  Scalar A, B, C; // 定义A, B, C  if (abs(sigma) < Constants<Scalar>::epsilon()) { // 如果sigma接近0    C = one; // C = 1    if (abs(theta) < Constants<Scalar>::epsilon()) { // 如果theta接近0      A = half; // A = 0.5      B = Scalar(1. / 6.); // B = 1/6    } else {      Scalar theta_sq = theta * theta; // theta的平方      A = (one - cos(theta)) / theta_sq; // 计算A      B = (theta - sin(theta)) / (theta_sq * theta); // 计算B    }  } else {    C = (scale - one) / sigma; // 计算C    if (abs(theta) < Constants<Scalar>::epsilon()) { // 如果theta接近0      Scalar sigma_sq = sigma * sigma; // sigma的平方      A = ((sigma - one) * scale + one) / sigma_sq; // 计算A      B = (scale * half * sigma_sq + scale - one - sigma * scale) /          (sigma_sq * sigma); // 计算B    } else {      Scalar theta_sq = theta * theta; // theta的平方      Scalar a = scale * sin(theta); // 计算a      Scalar b = scale * cos(theta); // 计算b      Scalar c = theta_sq + sigma * sigma; // 计算c      A = (a * sigma + (one - b) * theta) / (theta * c); // 计算A      B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); // 计算B    }  }  return A * Omega + B * Omega2 + C * I; // 返回矩阵W}
template <class Scalar>void calcW_derivatives(Scalar const theta, Scalar const sigma, Scalar &A,                       Scalar &B, Scalar &C, Scalar &A_dsigma, Scalar &B_dsigma,                       Scalar &C_dsigma, Scalar &A_dtheta, Scalar &B_dtheta) { // 计算W的导数  using std::abs;  using std::cos;  using std::exp;  using std::sin;  static Scalar const zero(0.0); // 常数0  static Scalar const one(1.0); // 常数1  static Scalar const half(0.5); // 常数0.5  static Scalar const two(2.0); // 常数2  static Scalar const three(3.0); // 常数3  Scalar const theta_sq = theta * theta; // theta的平方  Scalar const theta_c = theta * theta_sq; // theta的三次方  Scalar const sin_theta = sin(theta); // sin(theta)  Scalar const cos_theta = cos(theta); // cos(theta)Scalar const scale = exp(sigma); // 计算指数  Scalar const sigma_sq = sigma * sigma; // sigma的平方  Scalar const sigma_c = sigma * sigma_sq; // sigma的三次方if (abs(sigma) < Constants<Scalar>::epsilon()) { // 如果sigma接近0    C = one; // C = 1    C_dsigma = half; // C_dsigma = 0.5    if (abs(theta) < Constants<Scalar>::epsilon()) { // 如果theta接近0      A = half; // A = 0.5      B = Scalar(1. / 6.); // B = 1/6      A_dtheta = A_dsigma = zero; // A_dtheta和A_dsigma = 0      B_dtheta = B_dsigma = zero; // B_dtheta和B_dsigma = 0    } else {      A = (one - cos_theta) / theta_sq; // 计算A      B = (theta - sin_theta) / theta_c; // 计算B      A_dtheta = (theta * sin_theta + two * cos_theta - two) / theta_c; // 计算A_dtheta      B_dtheta = -(two * theta - three * sin_theta + theta * cos_theta) /                 (theta_c * theta); // 计算B_dtheta      A_dsigma = (sin_theta - theta * cos_theta) / theta_c; // 计算A_dsigma      B_dsigma =          (half - (cos_theta + theta * sin_theta - one) / theta_sq) / theta_sq; // 计算B_dsigma    }  } else {    C = (scale - one) / sigma; // 计算C    C_dsigma = (scale * (sigma - one) + one) / sigma_sq; // 计算C_dsigma    if (abs(theta) < Constants<Scalar>::epsilon()) { // 如果theta接近0      A = ((sigma - one) * scale + one) / sigma_sq; // 计算A      B = (scale * half * sigma_sq + scale - one - sigma * scale) / sigma_c; // 计算B      A_dsigma = (scale * (sigma_sq - two * sigma + two) - two) / sigma_c; // 计算A_dsigma      B_dsigma = (scale * (half * sigma_c - (one + half) * sigma_sq +                           three * sigma - three) +                  three) /                 (sigma_c * sigma); // 计算B_dsigma      A_dtheta = B_dtheta = zero; // A_dtheta和B_dtheta = 0    } else {      Scalar const a = scale * sin_theta; // 计算a      Scalar const b = scale * cos_theta; // 计算b      Scalar const b_one = b - one; // 计算b_one      Scalar const theta_b_one = theta * b_one; // 计算theta_b_one      Scalar const c = theta_sq + sigma_sq; // 计算c      Scalar const c_sq = c * c; // 计算c的平方      Scalar const theta_sq_c = theta_sq * c; // 计算theta_sq * c      Scalar const a_theta = theta * a; // 计算a_theta      Scalar const b_theta = theta * b; // 计算b_theta      Scalar const c_theta = theta * c; // 计算c_theta      Scalar const a_sigma = sigma * a; // 计算a_sigma      Scalar const b_sigma = sigma * b; // 计算b_sigma      Scalar const two_sigma = sigma * two; // 计算two_sigma      Scalar const two_theta = theta * two; // 计算two_theta      Scalar const sigma_b_one = sigma * b_one; // 计算sigma_b_oneA = (a_sigma - theta_b_one) / c_theta; // 计算A      A_dtheta = (two * (theta_b_one - a_sigma)) / c_sq +                 (b_sigma - b + a_theta + one) / c_theta +                 (theta_b_one - a_sigma) / theta_sq_c; // 计算A_dtheta      A_dsigma = (a - b_theta + a_sigma) / c_theta -                 (two_sigma * (theta - b_theta + a_sigma)) / (theta * c_sq); // 计算A_dsigmaB = (C - (sigma_b_one + a_theta) / (c)) * one / (theta_sq); // 计算B      B_dtheta =          ((two_theta * (b_sigma - sigma + a_theta)) / c_sq -           ((a + b_theta - a_sigma)) / c) /              theta_sq -          (two * ((scale - one) / sigma - (b_sigma - sigma + a_theta) / c)) /              theta_c; // 计算B_dtheta      B_dsigma =          -((b_sigma + a_theta + b_one) / c + (scale - one) / sigma_sq -            (two_sigma * (sigma_b_one + a_theta)) / c_sq - scale / sigma) /          theta_sq; // 计算B_dsigma    }  }}
template <class Scalar, int N>Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const &Omega,                              Scalar const theta, Scalar const sigma,                              Scalar const scale) { // 计算矩阵W的逆  using std::abs;  using std::cos;  using std::sin;  static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity(); // 单位矩阵  static Scalar const half(0.5); // 常数0.5  static Scalar const one(1); // 常数1  static Scalar const two(2); // 常数2  Matrix<Scalar, N, N> const Omega2 = Omega * Omega; // Omega的平方  Scalar const scale_sq = scale * scale; // scale的平方  Scalar const theta_sq = theta * theta; // theta的平方  Scalar const sin_theta = sin(theta); // sin(theta)  Scalar const cos_theta = cos(theta); // cos(theta)Scalar a, b, c; // 定义a, b, c  if (abs(sigma * sigma) < Constants<Scalar>::epsilon()) { // 如果sigma的平方接近0    c = one - half * sigma; // 计算c    a = -half; // 计算a    if (abs(theta_sq) < Constants<Scalar>::epsilon()) { // 如果theta的平方接近0      b = Scalar(1. / 12.); // 计算b    } else {      b = (theta * sin_theta + two * cos_theta - two) /          (two * theta_sq * (cos_theta - one)); // 计算b    }  } else {    Scalar const scale_cu = scale_sq * scale; // 计算scale的立方    c = sigma / (scale - one); // 计算c    if (abs(theta_sq) < Constants<Scalar>::epsilon()) { // 如果theta的平方接近0      a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); // 计算a      b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /          (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); // 计算b    } else {      Scalar const s_sin_theta = scale * sin_theta; // 计算s_sin_theta      Scalar const s_cos_theta = scale * cos_theta; // 计算s_cos_theta      a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /          (theta * (scale_sq - two * s_cos_theta + one)); // 计算a      b = -scale *          (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -           scale * sigma + sigma * cos_theta - sigma) /          (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +                       two * s_cos_theta + scale - one)); // 计算b    }  }  return a * Omega + b * Omega2 + c * I; // 返回矩阵W的逆}
}  // namespace details}  // namespace Sophus

http://www.ppmy.cn/server/152124.html

相关文章

计算机网络之王道考研读书笔记-1

计算机网络之王道考研读书笔记-1

第 1 章 计算机网络体系结构 1.1 计算机网络概述 1.1.1 计算机网络概念 internet(互连网)&#xff1a;泛指由多个计算机网络互连而成的计算机网络。这些网络之间可使用任意通信协议。 Internet(互联网或因特网)&#xff1a;指当前全球最大的、开放的、由众多网络和路由器互连…
阅读更多...
深度学习笔记27_RNN实现阿尔茨海默病诊断（pytorch）

深度学习笔记27_RNN实现阿尔茨海默病诊断（pytorch）

&#x1f368; 本文为&#x1f517;365天深度学习训练营 中的学习记录博客&#x1f356; 原作者&#xff1a;K同学啊 | 接辅导、项目定制 一、我的环境 1.语言环境&#xff1a;Python 3.9 2.编译器&#xff1a;Pycharm 3.深度学习环境&#xff1a;pyTorch 2.1.2 二、GPU设置 若…
阅读更多...
建造者模式（Builder Pattern）

建造者模式（Builder Pattern）

建造者模式&#xff08;Builder Pattern&#xff09; 建造者模式是一种 创建型设计模式&#xff0c;它将一个复杂对象的构造与它的表示分离&#xff0c;使得同样的构建过程可以创建不同的表示。 原理 核心思想&#xff1a;通过一步步的构建过程来创建复杂对象&#xff0c;而不…
阅读更多...
RK3588平台上YOLOv8模型转换与CentOS 7.8 Docker镜像拉取超时问题解决指南

RK3588平台上YOLOv8模型转换与CentOS 7.8 Docker镜像拉取超时问题解决指南

RK3588平台上YOLOv8模型转换与CentOS 7.8 Docker镜像拉取超时问题解决指南 一、RK3588平台上YOLOv8模型从PT转换为RKNN 背景介绍 YOLOv8的原生模型包含了后处理步骤&#xff0c;其中一些形状超出了RK3588的矩阵计算限制&#xff0c;因此需要对输出层进行一些裁剪。本文将详细…
阅读更多...
如何保证开源AI呼入机器人和AI呼出机器人的服务质量？

如何保证开源AI呼入机器人和AI呼出机器人的服务质量？

如何保证开源AI呼入机器人和AI呼出机器人的服务质量&#xff1f; 确保开源AI呼入机器人和AI呼出机器人的服务质量是企业成功部署这些智能系统的关键。高质量的服务不仅能够提高客户满意度&#xff0c;还能增强企业的市场竞争力。以下是实现这一目标的几个关键策略和技术措施&a…
阅读更多...
Redis 存储断点续传文件状态的最佳实践

Redis 存储断点续传文件状态的最佳实践

目录 1. Redis 中存储文件上传状态使用 Hash 存储文件状态存储分块上传状态使用 TTL 进行状态过期管理 2. Redis 与数据库保持一致方法 1&#xff1a;定期同步方法 2&#xff1a;实时同步方法 3&#xff1a;双写机制方法 4&#xff1a;系统重启后的恢复 3. 一致性保障4. 总结5.…
阅读更多...
用C#(.NET8)开发一个NTP（SNTP）服务

用C#(.NET8)开发一个NTP（SNTP）服务

完整源码&#xff0c;附工程下载&#xff0c;工程其实也就下面两个代码。 想在不能上网的服务器局域网中部署一个时间服务NTP&#xff0c;当然系统自带该服务&#xff0c;可以开启&#xff0c;本文只是分享一下该协议报文和能跑的源码。网上作为服务的源码不太常见&#xff0c;…
阅读更多...
第十七章：反射+设计模式

第十七章：反射+设计模式

一、反射 1. 反射(Reflection)&#xff1a;允许在程序运行状态中&#xff0c;可以获取任意类中的属性和方法&#xff0c;并且可以操作任意对象内部的属 性和方法&#xff0c;这种动态获取类的信息及动态操作对象的属性和方法对应的机制称为反射机制。 2. 类对象 和 类的对象(实…
阅读更多...
最新文章

Copyright @ 2022~2023