- BA本质是把PnP问题当成李代数上的非线性最小二乘问题。
- 线性问题的方式:先求相机位姿,再求空间点位置
问题:给定
n
n
个三维空间点和投影
p
p
,希望计算相机的位姿和
t⃗
t
→
,对应的李代数为
ξ
ξ
. 假设:
Pi=[Xi,Yi,Zi]T
P
i
=
[
X
i
,
Y
i
,
Z
i
]
T
,则像素位置与空间位置的关系为:
写在矩阵形式为:
上式中隐含有一个
齐次坐标到非齐次坐标的转换
。把误差求和,构建最小二乘问题,寻找最好的相机位姿:
上式为重投影误差.
应用G-N或L-M等方法之前,要知道误差项关于变量的导数,即:
这里:
* e e 的意义为像素坐标误差,为2维
* 为相机位姿,6维变量
* J J 为 2×6 2 × 6 的矩阵,需要进一步推导。
设变换到相机坐标系下的空间点坐标为
P′
P
′
:
则相机投影模型为:
即:
所以:
考虑对 ξ∧ ξ ∧ 的左乘扰动量 δξ δ ξ 关于 e e 的变化,根据链式法则有:
其中符号
⊕
⊕
为李代数上的左乘扰动。所以有:
式 (???) (???) 的第二项为变换后的点关于李代数的导数:
又由于:
将这两项相乘后雅可比矩阵为:
该矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系.
另一方面,除了要优化位姿,也得优化特征点的空间位置,即需要讨论
e
e
关于空间点的导数。
利用链式法则,有:
第一项与前面的式子相同。第二项按照定义有:
对 P P 求导后只剩下 R R ,所以:
上式即为
相机方程关于相机特征点的导数矩阵方程
。