【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)变换和其相关操作。这些计算对于机器人学、计算机视觉和几何运算等领域非常重要。
#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