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

Question

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

有一个很长的（几何）方式，3d协方差可以是一个3d椭圆，然后将它投影到平面给出2d椭圆，最后将椭圆转换为2d矩阵，但这很长，

任何直接的方式，解决这个代数都会有所帮助

P上。 S：任何线索或对解决方案的引用（不需要代码），也会有所帮助，我将用代码重写一个答案（在c ++中）

我也标记了卡尔曼过滤器，因为我认为它与它有关

Answer 1

如何分析表达变换变量的协方差？

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

  

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

如果我理解你的问题，你就可以在世界坐标系中表示的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，主点坐标{{1} }和cx）：cy

2. f_intr( [xn; yn] ) = [fx, s, cx; 0, fy, cy] . [xn; yn; 1] = [fx . xn + s . yn + cx; fy . yn + cy]将针孔透视模型应用于相机坐标系中的3D点：f_persp

3. f_persp( [Xc; Yc; Zc] ) = [Xc/Zc; Yc/Zc]应用3D刚性变换（即旋转f_pose，翻译R_cw）将世界坐标系中的3D点映射到摄像机坐标系中的3D点：t_cw

chain rule of derivatives有助于表达组合函数的导数：

  

如果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的jacobian矩阵很容易通过分析表达：

1. f_pose在f_intr中是线性的，因此xn是常量

2. J_f_intr = [fx, s; 0, fy]

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

同样，这是一阶近似，但针孔投影函数不是非线性＆＃34;，这意味着这种近似通常足够接近大多数应用。

Answer 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;
};