ORB-SLAM3源码学习：ImuTypes.cc：Eigen::Matrix3f RightJacobianSO3计算右雅可比矩阵
前言
计算右雅可比矩阵这个函数涉及到了函数重载，可以接受不同的参数来实现计算右雅可比矩阵。
代码分析
右雅可比矩阵：
/**
* @brief 计算右雅可比
* @param xyz 李代数
* @return Jr
*/
Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z)
{
Eigen::Matrix3f I;
I.setIdentity();//创建并初始化一个单位矩阵。
const float d2 = x * x + y * y + z * z;
const float d = sqrt(d2);
Eigen::Vector3f v;
v << x, y, z;
Eigen::Matrix3f W = Sophus::SO3f::hat(v);//反对称矩阵
if (d < eps)
{
return I;
}
else
{
return I - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);
}
}
/**
* @brief 计算右雅可比
* @param v so3
* @return Jr
*/
Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v)
{
return RightJacobianSO3(v(0), v(1), v(2));
}结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。