我有一个 3d 点的 3x3 协方差矩阵,我想知道等效的 2d 协方差(对于图像平面中的 u,v),给定图像位姿 [Xc,Yc,Zc,q0,q1, q2,q3]
,
3d 协方差可以是一个 3d 椭圆,然后将其投影到平面上得到 2d 椭圆,最后将椭圆转换为 2d 矩阵,但是这很长,
代数解决这个问题的任何直接方法都会有所帮助
P. S:任何线索或解决方案的引用(不需要代码),也会有所帮助,我会用代码重写答案(用c++)
我还标记了 kalman filter ,因为我认为它与它有关
请您参考如下方法:
如何分析性地表达转换后变量的协方差?
您可以使用 uncertainty propagation equations 解析得到一阶近似值.特别是关于非线性组合的段落,基本解释如下:
Knowing the covariance
C_x
on a variablex
and the jacobian matrixJ_f
of a functionf
, a first order approximation of the covariance onf(x)
is given by:C_f(x) = J_f . C_x . J_f^T
, where.^T
is the transposition operator.
如果我正确理解了您的问题,那么您就有了在世界坐标系中表示的 3D 点的协方差,表示为 C_Xw
。您想要该点在图像平面中的投影的协方差,表示为 C_xi
。我们用 f
表示将 3D 世界坐标映射到图像坐标的函数。然后我们有: C_xi = J_f 。 C_Xw 。 J_f^T
.
如何计算雅可比矩阵J_f
?
在实践中,f
是针孔投影函数,可以分解如下:f = f_intr o f_persp o f_pose
,其中:
f_intr
应用内在相机系数(即水平和垂直焦距fx
和fy
,倾斜s
,主点坐标cx
和cy
):f_intr( [xn; yn] ) = [fx, s, cx; 0, fy, cy] 。 [xn; yn; 1] = [外汇。 xn + 小号。 yn + cx;飞。 yn + cy]
f_persp
将针孔透视模型应用于相机坐标系中的 3D 点:f_persp( [Xc; Yc; Zc] ) = [Xc/Zc; Yc/Zc]
f_pose
应用 3D 刚性变换(即旋转R_cw
,平移t_cw
)映射世界坐标系中的 3D 点进入相机坐标系中的 3D 点:f_pose( [Xw; Yw; Zw] ) = R_cw 。 [Xw; Yw; Zw] + t_cw
chain rule of derivatives有助于表达组合函数的导数:
If
f = f_intr o f_persp o f_pose
, and denoting byXc=f_pose(Xw)
,xn=f_persp(Xc)
andxi=f_intr(xn)
, then we have the following:
J_f( Xw ) = J_f_intr( xn ) . J_f_persp( Xc ) . J_f_pose( Xw )
f_intr
、f_persp
和 f_pose
的雅可比矩阵很容易解析表示:
f_intr
在xn
中是线性的,因此J_f_intr = [fx, s; 0, fy]
是一个常数J_f_persp(Xc) = [1/Zc, 0, -Xc/Zc²; 0, 1/Zc, -Yc/Zc²]
f_pose
在Xw
中是线性的,因此J_f_pose = R_cw
是一个常数
最终表达
最后,我们得到如下解析表达式:
C_xi = J_f 。 C_Xw 。 J_f^T
其中 J_f = [fx, s; 0, fy] 。 [1/Zc, 0, -Xc/Zc²; 0, 1/Zc, -Yc/Zc²] 。 R_cw
同样,这是一阶近似,但针孔投影函数“不是很非线性”,这意味着对于大多数应用来说,这种近似通常足够接近。