【C++】sophus : rotation_matrix.hpp 处理旋转矩阵的辅助函数 (二)

server/2024/12/16 18:50:57/

这段代码属于Sophus 命名空间,提供了一些处理旋转矩阵的辅助函数,具体功能如下:

  1. isOrthogonal函数

  • 用途:检查一个方阵是否为正交矩阵

  • 实现方法:通过计算矩阵与其转置矩阵相乘后与单位矩阵的差值的范数是否小于一个很小的常数来判断。

isScaledOrthogonalAndPositive函数

  • 用途:检查一个方阵是否为“缩放正交”矩阵且行列式为正。

  • 实现方法:首先计算矩阵的行列式,如果行列式小于等于零则直接返回 false。否则,计算比例平方并检查矩阵与其转置矩阵相乘后与单位矩阵乘比例的差值的无穷范数是否小于一个很小的常数。

  • 6ab52fbfefd2818cab241415de48f6ce.png

makeRotationMatrix函数

  • 用途:接收一个方阵(2x2或更大),返回最接近的正交矩阵且行列式为正。

  • 实现方法:使用 JacobiSVD 分解计算 U 和 V 矩阵,并调整单位矩阵的最后一个元素以确保返回的矩阵的行列式为 1。

这三个函数主要用于验证和生成旋转矩阵,确保它们符合特定的数学性质,如正交性和行列式为正。

/// 旋转矩阵辅助函数#pragma once
#include <Eigen/Core>#include <Eigen/SVD>
#include "types.hpp"
namespace Sophus {
/// 接收任意方阵,如果它是正交的则返回 true。template <class D>SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {  // 使用 D 类型的标量  using Scalar = typename D::Scalar;  // 定义标量类型  static int const N = D::RowsAtCompileTime;  // 在编译时确定矩阵的行数  static int const M = D::ColsAtCompileTime;  // 在编译时确定矩阵的列数static_assert(N == M, "必须是方阵");  // 确保矩阵是方阵  static_assert(N >= 2, "编译时维数必须 >= 2");  // 确保矩阵的维数 >= 2return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <  // 判断矩阵是否正交         Constants<Scalar>::epsilon();  // 判断标准是与单位矩阵的差值的范数小于一个很小的常数}
/// 接收任意方阵,如果它是"缩放正交"且行列式为正则返回 true。template <class D>SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {  // 使用 D 类型的标量  using Scalar = typename D::Scalar;  // 定义标量类型  static int const N = D::RowsAtCompileTime;  // 在编译时确定矩阵的行数  static int const M = D::ColsAtCompileTime;  // 在编译时确定矩阵的列数  using std::pow;  // 使用 std 库的 pow 函数  using std::sqrt;  // 使用 std 库的 sqrt 函数Scalar det = sR.determinant();  // 计算矩阵的行列式if (det <= Scalar(0)) {  // 如果行列式 <= 0 则返回 false    return false;  }Scalar scale_sqr = pow(det, Scalar(2. / N));  // 计算比例平方static_assert(N == M, "必须是方阵");  // 确保矩阵是方阵  static_assert(N >= 2, "编译时维数必须 >= 2");  // 确保矩阵的维数 >= 2return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())  // 判断矩阵是否"缩放正交"             .template lpNorm<Eigen::Infinity>() <  // 判断标准是与单位矩阵乘比例的差值的无穷范数小于一个很小的常数         sqrt(Constants<Scalar>::epsilon());}
/// 接收任意方阵(2x2或更大),返回最接近的正交矩阵且行列式为正。template <class D>SOPHUS_FUNC std::enable_if_t<    std::is_floating_point<typename D::Scalar>::value,    Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>makeRotationMatrix(Eigen::MatrixBase<D> const& R) {  // 使用 D 类型的标量  using Scalar = typename D::Scalar;  // 定义标量类型  static int const N = D::RowsAtCompileTime;  // 在编译时确定矩阵的行数  static int const M = D::ColsAtCompileTime;  // 在编译时确定矩阵的列数static_assert(N == M, "必须是方阵");  // 确保矩阵是方阵  static_assert(N >= 2, "编译时维数必须 >= 2");  // 确保矩阵的维数 >= 2Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(  // 使用JacobiSVD分解计算      R, Eigen::ComputeFullU | Eigen::ComputeFullV);  // 计算完整的 U 和 V 矩阵// 确定正交矩阵 U*V' 的行列式  Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();  // 从单位矩阵 D 开始,将最后一个元素设置为 d (+1 或 -1),以使 det(U*D*V') = 1。  Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();  Diag(N - 1, N - 1) = d;  return svd.matrixU() * Diag * svd.matrixV().transpose();  // 返回最接近的正交矩阵}
}  // namespace Sophus

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

相关文章

家校通小程序实战教程09搭建部门管理APIs

目录 1 创建APIs2 完整代码3 代码解释3.1 获取原始数据3.2 平铺数据3.3 构建树形结构3.4 组装树形结构3.5 数据返回 4 执行测试总结 我们现在已经调用了antd实现了前端的界面&#xff0c;光有界面还是不够的&#xff0c;还需要和数据源进行交互&#xff0c;本节介绍后端API的搭…

Flutter踩坑记录(一)debug运行生成的项目,不能手动点击运行

问题 IOS14设备&#xff0c;切后台划掉&#xff0c;二次启动崩溃。 原因 IOS14以上 flutter 不支持debugger模式下的二次启动 。 要二次启动需要以release方式编译工程安装至手机。 操作步骤 清理项目&#xff1a;在命令行中运行flutter clean来清理之前的构建文件。重新构…

css 权重

发现宝藏 前些天发现了一个巨牛的人工智能学习网站&#xff0c;通俗易懂&#xff0c;风趣幽默&#xff0c;忍不住分享一下给大家。【宝藏入口】。 CSS 权重&#xff08;或称为 CSS 优先级&#xff09;决定了当多个 CSS 规则作用于同一元素时&#xff0c;哪一条规则会被应用。…

Web前端技术宝典:期末冲刺指南

本文将为大家整理一份 Web 前端期末复习资料&#xff0c;内容涵盖 HTML、CSS、JavaScript 和常用的前端框架等方面的知识&#xff0c;帮助大家高效复习。 Web前端技术宝典&#xff1a;期末冲刺指南 1. HTML基础2. CSS基础3. JavaScript基础4. 前端框架5. 常见考试题型结语 1. …

Java中的Stream

1. 什么是 Stream&#xff1f; Stream 是 Java 8 引入的一种新方式&#xff0c;目的是帮助我们更简洁、更高效地处理集合&#xff08;如 List、Set、Map 等&#xff09;。你可以把 Stream 想象成一条“流水线”&#xff0c;数据就像是流水线上的原材料&#xff0c;经过流水线的…

第六章 状态模式优化代码

目录 一、场景概述 二、状态模式优化代码 一、场景概述 我们在日常开发过程中一定会遇到下述场景&#xff1a;业务中有类似订单这样需要修改状态的功能模块&#xff0c;订单状态的修改变化为 未支付 -> 已支付 -> 已完成 因此我们每次在修改状态之前&#xff0c;都得先…

React基础学习

React基础 &#x1f4e3; &#x1f4e3; &#x1f4e3; &#x1f4e2;&#x1f4e2;&#x1f4e2; ☀️☀️点开就是缘分认识一下&#xff0c;我是小冷。是一个兴趣驱动自学练习两年半的的Java工程师。 &#x1f4d2; 一位十分喜欢将知识分享出来的Java博主⭐️⭐️⭐️&#x…

OpenCV中的图片矫正

一、实验原理 基于计算机视觉中的透视变换&#xff08;Perspective Transformation&#xff09;&#xff0c;也称为单应性&#xff08;Homography&#xff09;。透视变换是一种几何变换&#xff0c;用于将图像从一个平面映射到另一个平面&#xff0c;同时保持直线的直线性。这种…