雅可比矩阵验证

最新推荐文章于 2024-10-28 11:49:15 发布

快乐得小萝卜

最新推荐文章于 2024-10-28 11:49:15 发布

阅读量584

点赞数

分类专栏： SLAM 文章标签：矩阵线性代数算法

本文链接：https://blog.youkuaiyun.com/weixin_41738773/article/details/130094504

版权

SLAM 专栏收录该内容

17 篇文章

订阅专栏

文章介绍了在SimultaneousLocalizationAndMapping(SLAM)优化中，雅可比矩阵的重要性。通过数值求导和手动解析求导的结果对比，可以定位雅可比矩阵计算的错误。文中提供了一个具体的案例，展示了如何对旋转矩阵（使用四元数表示）和位置向量的函数求导，并用C++代码实现了数值求导的过程，以验证解析导数的正确性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

雅可比矩阵验证

在slam优化中雅可比矩阵是及其重要的量，一旦手动求的雅可比矩阵求解错误就会导致整体的优化出现错误，因此可以采用数值求导结果和手动解析求导的雅可比矩阵进行比较从而定位出问题在哪里。

简单的数学原理： $\frac{f(x+\delta x) - f(x)}{ \delta x}$ 当 $\delta x$ 趋近0时候，数值导数与解析导数相等。

一个案例

$f(x)= R_1 R_2^T p$ 求 $f (p)$ 对于 $R_1, R_2, p$ 的导数，在这里，采用四元数作为旋转表达

解析雅可比

（采用右导数）

$\frac{\partial f(p)}{ \partial \delta \psi_1} = - R_1 [ R_2^T P]^{×}$

$\frac{\partial f(p)}{ \partial \delta \psi_2} = R_1 [ R_2^T P]^{×}$

$\frac{\partial f(p)}{ \partial \delta p} = R_1 R_2^T$

代码

#include "bits/stdc++.h"

#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"

using namespace std;

Eigen::Matrix3d skew(Eigen::Vector3d _in){
    Eigen::Matrix3d res;
    res << 0., -_in(2), _in(1),
            _in(2), 0., -_in(0),
            -_in(1), _in(0), 0.;
    return res;
}


Eigen::Vector3d func(Eigen::Quaterniond _r1, Eigen::Quaterniond _r2, Eigen::Vector3d _p){
    return _r1 * _r2.conjugate() * _p;
}



int main(void){

    // jacobian 矩阵是 3 × （3 + 3 + 3）  维度的 
    Eigen::AngleAxisd rotation_vector1(0.8, Eigen::Vector3d(0.3, 0.4, 0.2).normalized());
    Eigen::Quaterniond r1(rotation_vector1);
    Eigen::AngleAxisd rotation_vector2(0.8, Eigen::Vector3d(0.1, 0.4, 0.4).normalized());
    Eigen::Quaterniond r2(rotation_vector2);
    r2 = r2.normalized();
    Eigen::Vector3d p(1, 1, 1);

    std::cout << "手动求解雅可比矩阵" << std::endl;
    std::cout << "df/dr1 = " << std::endl << -r1.matrix() * skew(r2.conjugate() * p) << std::endl;
    std::cout << "df/dr2 = " << std::endl << r1.matrix() * skew(r2.conjugate() * p) << std::endl;
    std::cout << "df/dp = " << std::endl << r1.matrix() * r2.conjugate().matrix() << std::endl;

    Eigen::Vector3d res = func(r1, r2, p);
    
    double eps = 1e-8;
    Eigen::Vector3d inc0(eps, 0, 0);
    Eigen::Vector3d inc1(0, eps, 0);
    Eigen::Vector3d inc2(0, 0, eps);

    // 对 r1 的数值
    Eigen::Matrix3d dfdr1;
    Eigen::Vector3d resDelta = func(r1 * Eigen::Quaterniond(1., inc0[0]/2, inc0[1]/2, inc0[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1 * Eigen::Quaterniond(1., inc1[0]/2, inc1[1]/2, inc1[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1 * Eigen::Quaterniond(1., inc2[0]/2, inc2[1]/2, inc2[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 2) = (resDelta - res)/eps;

    // 对 r2 的数值
    Eigen::Matrix3d dfdr2;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc0[0]/2, inc0[1]/2, inc0[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc1[0]/2, inc1[1]/2, inc1[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc2[0]/2, inc2[1]/2, inc2[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 2) = (resDelta - res)/eps;

    // 对 p 的数值
    Eigen::Matrix3d dfdp;
    resDelta = func(r1, r2, p + inc0);
    dfdp.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1, r2, p + inc1);
    dfdp.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1, r2, p + inc2);
    dfdp.block<3, 1>(0, 2) = (resDelta - res)/eps;

    std::cout << "数值雅可比矩阵" << std::endl;
    std::cout << "df/dr1 = " << std::endl << dfdr1 << std::endl;
    std::cout << "df/dr2 = " << std::endl << dfdr2 << std::endl;
    std::cout << "df/dp = " << std::endl << dfdp << std::endl;

    

    return 0;
}