在上一节内容中,我们讲到使用牛顿法、高斯牛顿法、LM法进行非线性优化,在优化过程中需要计算雅可比矩阵。
一、计算雅可比矩阵
上面公式中表示第i个相机观测的第j个目标的像素坐标和重投影坐标之间的误差,也即是待优化的目标函数。我们知道求雅可比矩阵是函数对其中的变量进行求导,而在该目标函数中的变量是相机的位姿和目标的位置。
其中的维度是2x6 维:
这个推导可以由链式法则得到。
的维度为2x3维:
求H矩阵:
通过图片来直观的kaH矩看J和H的关系。
举个栗子:
假设有两个相机观测6个目标:
目标函数:
我们可以发现H矩阵是很稀疏的。
再举个栗子:
假设有3个相机和4个目标,每个相机能观测到全部的目标。
二、边缘化(Marginalization)
事实上相机不可能拍摄到每一个目标,所以H矩阵是很稀疏的,我们可以对其进行边缘化。
以后再更。。。