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

Posted 2023-02-21

【中文标题】如何将 3D 协方差矩阵投影到给定的图像平面（姿势）

【英文标题】：How to Project 3D Covariance Matrix to a given Image Plane (Pose)

【发布时间】：2018-03-12 15:55:26

【问题描述】：

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

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

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

P. S：任何线索或解决方案的参考（不需要代码），也会有所帮助，我会用代码重写答案（用c ++）

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

【参考方案1】：

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

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

知道变量 x 上的协方差 C_x 和函数 f 的雅可比矩阵 J_f，f(x) 上的协方差的一阶近似由以下公式给出：C_f(x) = J_f . C_x . J_f^T，其中 @ 987654330@是转置运算符。

如果我正确理解了您的问题，那么您在世界坐标系中表示 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] = [fx . xn + s . yn + cx; fy . 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 有助于表达组合函数的导数：

如果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 的雅可比矩阵很容易解析表示：

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

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

【讨论】：

那太好了，我想我的回答基本上是错误的。

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

我在这里以不同的方式推导出雅可比行列stats.stackexchange.com/questions/497283/…

【参考方案2】：

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

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

N = Rc * eigenvectors;

New_covariance = N*eigenvalues*inv(N) #main relation of decomposition

但是，我们必须确保特征向量排列为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;
;