如何将3D协方差矩阵投影到给定的图像平面（姿态）？

Question

我有一个3x3的协方差矩阵，用于3D点，并希望知道在图像平面上（对应于u,v）的等效2D协方差，给定图像姿态[Xc,Yc,Zc,q0,q1,q2,q3]，

有一个很长的（几何）方法，将3D协方差转换为3D椭圆，然后投影到平面上得到2D椭圆，最后将椭圆转换为2D矩阵，但这很麻烦，

任何直接的代数求解方法都会有所帮助，

P.S.：任何线索或有关解决方案的参考资料（不需要代码），也会有所帮助，我将以C++编写答案。

我还标记了卡尔曼滤波，因为我认为它与此相关。

- Yasin Yousif

Answer 1

如何在转换变量上表达协方差的解析式？

您可以使用不确定性传播方程来获得一阶近似解析式。特别地，在非线性组合段落中，基本解释了以下内容：

已知变量x上的协方差C_x和函数f的雅可比矩阵J_f，则f(x)的协方差的一阶近似为：C_f(x) = J_f . C_x . J_f^T，其中.^T是转置运算符。

如果我正确理解了您的问题，您拥有以世界坐标系表示的3D点的协方差，表示为C_Xw。您想要该点在图像平面中的投影的协方差，表示为C_xi。让我们用将3D世界坐标映射到图像坐标的函数f表示。然后我们有：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] = [fx . xn + s . yn + cx; fy . yn + cy]

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

3. f_pose 应用三维刚体变换（即旋转 R_cw，平移 t_cw），将世界坐标系中的三维点映射到相机坐标系中的三维点：f_pose( [Xw; Yw; Zw] ) = R_cw . [Xw; Yw; Zw] + t_cw

导数的链式法则有助于表达复合函数的导数：

如果 f = f_intr o f_persp o f_pose，并且用 Xc=f_pose(Xw)，xn=f_persp(Xc) 和 xi=f_intr(xn) 表示，则我们有以下结果：

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

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

- BConic

---

太好了，我想我的答案基本上是错误的。 - Yasin Yousif

@YasinYousif，您的初始方法忽略了透视映射。这可能接近于正交相机的解决方案。 - BConic

我已经在这里用不同的方式推导了雅可比矩阵 https://stats.stackexchange.com/questions/497283/how-to-derive-camera-jacobian - Pavel Komarov

Answer 2

作为初始想法，协方差矩阵的特征值分解给出一个旋转矩阵（特征向量）和大小（特征值）。

如果我们只知道相机平面的旋转矩阵Rc，我们可以写成：

N = Rc * eigenvectors;

New_covariance = N*eigenvalues*inv(N) #分解的主要关系

但是，我们必须确保特征向量按X、Y、Z排列。

最后，我们将New_covariance的上部2X2矩阵作为投影的2D协方差（删除Z轴，因为它垂直于图像平面）。

更新：

这里有一个使用Eigen库的实现：

Eigen::Matrix3f points_cov_2d(VectorXf cov_p,Quaternionf quatcam ,
   float z_m,float f_x,float f_y){
 Matrix3f cov3d;
 cov3d << cov_p(0),cov_p(1),cov_p(2),
          cov_p(3),cov_p(4),cov_p(5),
          cov_p(6),cov_p(7),cov_p(8);
 SelfAdjointEigenSolver<MatrixXf> eigenSolver(cov3d);
 Vector3f eigs = eigenSolver.eigenvalues();
 Matrix3f vecs = eigenSolver.eigenvectors();
 Matrix3f n_vecs = quatcam.toRotationMatrix()*vecs;
 Matrix3f cov2d = n_vecs*eigs*n_vecs.inverse();
 cov2d = cov2d *(1/z_m/z_m);
 cov2d(0)*=(f_x*f_x);
 cov2d(4)*=(f_y*f_y);
 cov2d(3)*=(f_x*f_y);
 cov2d(1)*=(f_x*f_y);
 cov2d.block<1,3>(2,0) << 0,0,0;
 cov2d.block<3,1>(0,2) << 0,0,0;
 return cov2d;
};

- Yasin Yousif