问题描述
姿态估计是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}
]