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

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

1. isOrthogonal函数：

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

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

isScaledOrthogonalAndPositive函数：

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

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

• 

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");  // 确保矩阵的维数 >= 2
  return (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");  // 确保矩阵的维数 >= 2
  return (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");  // 确保矩阵的维数 >= 2
  Eigen::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