• 姿态估计中的雅可比求导


    问题描述

    姿态估计是SLAM中的一个基础问题。基于重投影误差的问题描述一般为求解下列的优化问题

    [min_{mathbf{T}}mathbf{f},quad mathbf{f}=mathbf{e}^Tmathbf{e}=parallel p'-p parallel^2 ]

    其中(p)是观测到的(世界坐标系下的)三维点(mathbf{p})的图像投影,(p')是计算得到的(mathbf{p})的图像投影,满足

    [p'=lambdamathbf{K}mathbf{D}mathbf{T}mathbf{p} ]

    其中,(p=(u,v,1)^{T})(p'=(u',v',1)^{T})(mathbf{p}=(x,y,z,1)^{T})(mathbf{K})是相机内参矩阵,(mathbf{T})是两个不同位姿之间的相对变换矩阵。

    [mathbf{K} = egin{bmatrix} f_x & 0 & c_x\ 0 & f_y & c_y\ 0 & 0 & 1 end{bmatrix} ]

    [mathbf{D} = egin{bmatrix} mathbf{I_{3 imes 3}} & mathbf{0} end{bmatrix} ]

    [mathbf{T} = egin{bmatrix} mathbf{R} & mathbf{t} \ mathbf{0}^T & 1 end{bmatrix} ]

    推导

    最优化(mathbf{f})通常会近似为在每步迭代求解一个线性方程组(这块就不细讲了)

    [mathbf{H}mathbf{Delta mathbf{x}}=-mathbf{b} ]

    其中

    [mathbf{H}=mathbf{J}^Tmathbf{J} ]

    [mathbf{J}=frac{partial mathbf{e(mathbf{x}oxplusDelta mathbf{x})}}{partial Delta mathbf{x}} ]

    [mathbf{b} = mathbf{J}^Tmathbf{e} ]

    于是问题的关键转化为雅克比矩阵的计算。令

    [hat{mathbf{p}}=mathbf{T}mathbf{p}=(hat{x},hat{y},hat{z},1)^T ]

    [ar{mathbf{p}}=lambdahat{mathbf{p}}=(frac{hat{x}}{hat{z}},frac{hat{y}}{hat{z}},1,1)^T=(ar{x},ar{y},1,1)^T ]

    [p'=mathbf{K}mathbf{D}ar{mathbf{p}} =(f_xar{x}+c_x,f_yar{y}+c_y,1)^T ]

    那么,

    [frac{partial mathbf{e}}{partialdelta} =frac{partial mathbf{e}}{partial p'} frac{partial p'}{partial ar{mathbf{p}}} frac{partial ar{mathbf{p}}}{partial hat{mathbf{p}}} frac{partial hat{mathbf{p}}}{partial delta} ]

    我们容易推出以下等式

    [frac{partial mathbf{e}}{partial p'} = egin{bmatrix} mathbf{I}_{2 imes2} & mathbf{0}_{2 imes1}\ mathbf{0}_{2 imes1}^T & 0 end{bmatrix} ]

    [frac{partial p'}{partial ar{mathbf{p}}}= egin{bmatrix} f_x & 0 & 0 & 0 \ 0 & f_y & 0 & 0 \ 0 & 0 & 0 & 0 end{bmatrix} ]

    [frac{partial ar{mathbf{p}}}{partial hat{mathbf{p}}}= frac{1}{hat{z}^2}egin{bmatrix} hat{z} & 0 & -hat{x} & 0\ 0 & hat{z} & -hat{y} & 0 \ 0 & 0 & 0 & 0\ 0 & 0 & 0 & 0 end{bmatrix} ]

    [frac{partial hat{mathbf{p}}}{partialdelta} =(mathbf{T}mathbf{p})^{odot} =egin{bmatrix} mathbf{I}_{3 imes 3} & -{[hat{mathbf{p}}]_{1:3}}^{wedge}\ mathbf{0}_{3 imes1}^T & mathbf{0}_{3 imes1}^T end{bmatrix} ]

    最后一个等式参见State Estimation of Robotics第7章公式(7.179)。
    将上面的式子放在一起,便有

    [frac{partial mathbf{e}}{partialdelta} =frac{1}{hat{z}^2} egin{bmatrix} f_xhat{z} & 0 & -f_xhat{x} & -f_xhat{x}hat{y} & f_x(hat{x}^2+hat{z}^2) & -f_xhat{y}hat{z}\ 0 & f_yhat{z} & -f_yhat{y} & -f_y(hat{y}^2+hat{z}^2) & f_yhat{x}hat{y} & f_yhat{x}hat{z}\ & mathbf{0}_{3 imes 1}^T & & & mathbf{0}_{3 imes 1}^T & end{bmatrix} ]

  • 相关阅读:
    Qt QTimer定时器相关
    C#Datetime和long之间转换
    C# 把图片资源转成字节数组写入到数据库
    Qt QProcess启动和关闭外部程序
    Qt绘图
    有哪些十分惊艳的书值得推荐3
    Stack Overflow 推荐编程书单
    《编写可读代码的艺术》的读书笔记 (脑图)
    [apache spark]洞见纽约车辆事故|bluemix|apache spark
    [lean scala]|How to create a SBT project with Intellij IDEA
  • 原文地址:https://www.cnblogs.com/luyb/p/7804126.html
Copyright © 2020-2023  润新知