math - 如何将 3D 协方差矩阵投影到给定的图像平面(姿势)

Question

我有一个 3d 点的 3x3 协方差矩阵，我想知道等效的 2d 协方差(对于图像平面中的 u,v)，给定图像位姿 [Xc,Yc,Zc,q0,q1, q2,q3] ,

3d 协方差可以是一个 3d 椭圆，然后将其投影到平面上得到 2d 椭圆，最后将椭圆转换为 2d 矩阵，但是这很长，

代数解决这个问题的任何直接方法都会有所帮助

P. S:任何线索或解决方案的引用(不需要代码)，也会有所帮助，我会用代码重写答案(用c++)

我还标记了 kalman filter ，因为我认为它与它有关

Answer 1

如何分析性地表达转换后变量的协方差？

您可以使用 uncertainty propagation equations 解析得到一阶近似值.特别是关于非线性组合的段落，基本解释如下:

Knowing the covariance C_x on a variable x and the jacobian matrix J_f of a function f, a first order approximation of the covariance on f(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，其中:

1. 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]

2. f_persp 将针孔透视模型应用于相机坐标系中的 3D 点:f_persp( [Xc; Yc; Zc] ) = [Xc/Zc; Yc/Zc]

3. 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 by Xc=f_pose(Xw), xn=f_persp(Xc) and xi=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 的雅可比矩阵很容易解析表示:

1. f_intr 在 xn 中是线性的，因此 J_f_intr = [fx, s; 0, fy]是一个常数

2. J_f_persp(Xc) = [1/Zc, 0, -Xc/Zc²; 0, 1/Zc, -Yc/Zc²]

3. 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

同样，这是一阶近似，但针孔投影函数“不是很非线性”，这意味着对于大多数应用来说，这种近似通常足够接近。