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

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

主要功能

calcW: 计算矩阵W，该矩阵与旋转矩阵和指数有关。根据不同的theta和sigma值，进行特定的矩阵计算。
calcW_derivatives: 计算矩阵W及其关于theta和sigma的导数。
calcWInv: 计算矩阵W的逆矩阵，这同样与旋转矩阵和指数有关。

命名空间

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

主要步骤

初始化常数和变量: 包括单位矩阵、常数0、1、0.5等。
计算矩阵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)变换和其相关操作。这些计算对于机器人学、计算机视觉和几何运算等领域非常重要。

cpp 复制代码

#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_one
      A = (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_dsigma
      B = (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