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


    问题描述

    姿态估计是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} ]

  • 相关阅读:
    jquery on()动态绑定元素的的点击事件无反应的问题记录
    【分享】开源富文本编辑器之间的较量
    【分享】JS如何为复制的Web文本添加其他信息
    HTML table导出到Excel中的解决办法
    Vue跨门槛系列之实例的阐述
    Vue.js 基本功能了解一下~
    JS数据结构的栈和队列操作
    CSS宽度高度的百分比取值基于谁
    CSS个人笔记
    使用word设置标题级别, 自动生成和大纲对应的多级列表, 自动生成索引目录
  • 原文地址:https://www.cnblogs.com/luyb/p/7804126.html
Copyright © 2020-2023  润新知