位姿误差雅可比的N种写法

winwonnn

已于 2023-03-26 18:33:58 修改

阅读量424

点赞数

文章标签：其他

于 2023-03-26 18:22:41 首次发布

本文链接：https://blog.csdn.net/u013213111/article/details/129781451

版权

对位姿误差的推导做个整理，不同的资料上求导的对象以及是左扰动还是右扰动会略有差别，注意区分。

推导中常用的公式：
1.李群的伴随性质
adjoin
——From《视觉SLAM十四讲》式(4.48)
2.李群雅可比的近似公式
有时候在推导过程中会把雅可比近似为单位矩阵了。
jacob ——From State Estimation for Robotics Eq.7.91

各种推导：
1.十四讲/g2o
这是最常见的一种吧，具体推导可见十四讲以及g2o中 EdgeSE3Expmap类型Jacobian的计算——
error 2.State Estimation for Robotics
这里误差的定义中相乘的顺序跟十四讲不一样，取个逆的区别吧。 $G_{kl}$ 就是误差雅可比，具体推导见书。
jacob 3.MATLAB
代码如下。推导方式我觉得应该是跟State Estimation for Robotics 类似的，李群雅可比矩阵的近似项取多了一些，但是没想明白Mi和Mj的正负号？2021b这里的展开项跟想象的也有些不同，系数？为啥没有三次项呢？BTW，2021b中图优化默认的求导是用数值方法，并不是这个公式，难道不够好用吗？

        function [e, Jaci, Jacj] = poseErrorSE3(Toi, Toj, Tij) %
        %poseErrorSE3

            Tio = robotics.core.internal.SEHelpers.tforminvSE3(Toi);
            Tji = robotics.core.internal.SEHelpers.tforminvSE3(Tij);

            e = robotics.core.internal.SEHelpers.veelogmSE3(Tji*Tio*Toj);

            tau = e(1:3);
            phi = e(4:6);
            A = robotics.core.internal.SEHelpers.skew(phi);
            B = robotics.core.internal.SEHelpers.skew(tau);

            Mi = [-A, -B; zeros(3), -A];
            Mj = [ A, B; zeros(3), A];
			
			%2020a
            Jaci = (eye(6) + 0.5* Mi)*robotics.core.internal.SEHelpers.adjointSE3(Tji);
            Jacj = -(eye(6) + 0.5* Mj);
            %2021b
            %Jaci = (eye(6) + 0.5*Mi + (1/12)*MSq  - (1/720)*MSqSq )*robotics.core.internal.SEHelpers.adjointSE3(Tji);
            %Jacj = -(eye(6) + 0.5*Mj + (1/12)*MSq - (1/720)*MSqSq );
        end
    end