- BA本质是把PnP问题当成李代数上的非线性最小二乘问题。
- 线性问题的方式:先求相机位姿,再求空间点位置
问题:给定
n
个三维空间点
P
和投影
p
,希望计算相机的位姿
R
和
t⃗
,对应的李代数为
ξ
. 假设:
Pi=[Xi,Yi,Zi]T
,则像素位置与空间位置的关系为:
s⎡⎣⎢uivi1⎤⎦⎥=Kexp(ξ∧)⎡⎣⎢⎢⎢XiYiZi1⎤⎦⎥⎥⎥
写在矩阵形式为:
siu⃗ i=Kexp(ξ∧)Pi
上式中隐含有一个
齐次坐标到非齐次坐标的转换
。把误差求和,构建最小二乘问题,寻找最好的相机位姿:
ξ∗=argminξ12∑i=1n|u⃗ i−1siKexp(ξ∧)Pi|22
上式为重投影误差.
应用G-N或L-M等方法之前,要知道误差项关于变量的导数,即:
e(x⃗ +△x⃗ )≈e(x)+J△x
这里:
*
e
的意义为像素坐标误差,为2维
*
x
为相机位姿,6维变量
*
J
为
2×6
的矩阵,需要进一步推导。
设变换到相机坐标系下的空间点坐标为
P′
:
P′=(exp(ξ∧)P)1:3=[X′,Y′,Z′]⊤
则相机投影模型为:
su⃗ =KP′
即:
⎡⎣⎢susvs⎤⎦⎥=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥[X′Y′Z′]
所以:
u=fxX′Z′+cx, v=fyY′Z′+cy
考虑对
ξ∧
的左乘扰动量
δξ
关于
e
的变化,根据链式法则有:
∂e∂δξ=limδξ→0e(δξ⊕ξ)δξ=∂e∂P′∂P′∂δξ
其中符号
⊕
为李代数上的左乘扰动。所以有:
∂e∂P′=−[∂u∂X′∂v∂X′∂u∂Y′∂v∂Y′∂u∂Z′∂v∂Z′]=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥
式
(???)
的第二项为变换后的点关于李代数的导数:
∂TP∂δξ=(TP)⊙=[I0⊤−P′∧0⊤]
又由于:
∂P′∂δξ=[I,−P′∧]
将这两项相乘后雅可比矩阵为:
∂e∂δξ=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2−fxX′Y′Z′2−fy−fyY′2Z′2fx+fxX2Z′2fyX′Y′Z′2−fxY′Z′fyX′Z′⎤⎦⎥
该矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系.
另一方面,除了要优化位姿,也得优化特征点的空间位置,即需要讨论
e
关于空间点
P
的导数。
利用链式法则,有:
∂e∂P=∂e∂P′∂P′∂P
第一项与前面的式子相同。第二项按照定义有:
P′=exp(ξ∧)P=RP+t⃗
对
P
求导后只剩下
R
,所以:
∂e∂P=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥R
上式即为
相机方程关于相机特征点的导数矩阵方程
。