如何将 3D 协方差矩阵投影到给定的图像平面（姿势）
How to Project 3D Covariance Matrix to a given Image Plane (Pose)
我有一个 3d 点的 3x3 协方差矩阵，我想知道等效的 2d 协方差（对于图像平面中的 u,v），给定图像姿态 [Xc,Yc,Zc,q0,q1,q2,q3]，
3d 协方差可以是 3d 椭圆的长（几何）方式，然后将其投影到平面中得到 2d 椭圆，最后将椭圆转换为 2d 矩阵，但这很长，
任何直接的方法，以代数方式解决这个问题都会有所帮助
P。 S: 任何线索或对解决方案的引用（不需要代码）也会有所帮助，我将用代码重写一个答案（在 c++ 中）
我也标记了卡尔曼滤波器，因为我认为它与它有关
作为最初的想法，协方差矩阵的特征值分解给出旋转矩阵（特征向量）和幅度（特征值）。
如果我们只知道相机平面的旋转矩阵Rc，我们可以写成：
N = Rc * eigenvectors;
New_covariance = N*eigenvalues*inv(N) #main relation of decomposition
但是，我们必须确保特征向量排列为X,Y,Z
最后，我们将New_covariance的上2X2矩阵作为投影二维协方差（删除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;
};
如何分析地表达转换变量的协方差？
您可以使用 uncertainty propagation equations 解析地获得一阶近似值。特别是，关于 non-linear 组合的段落基本上解释了以下内容：
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，其中：
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有助于表达组合函数的导数：
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的雅可比矩阵很容易解析表达：
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
同样，这是一阶近似，但针孔投影函数是 "not very non linear"，这意味着对于大多数应用来说，这种近似通常足够接近。
我有一个 3d 点的 3x3 协方差矩阵，我想知道等效的 2d 协方差（对于图像平面中的 u,v），给定图像姿态 [Xc,Yc,Zc,q0,q1,q2,q3]，
3d 协方差可以是 3d 椭圆的长（几何）方式，然后将其投影到平面中得到 2d 椭圆，最后将椭圆转换为 2d 矩阵，但这很长，
任何直接的方法，以代数方式解决这个问题都会有所帮助
P。 S: 任何线索或对解决方案的引用（不需要代码）也会有所帮助，我将用代码重写一个答案（在 c++ 中）
我也标记了卡尔曼滤波器，因为我认为它与它有关
作为最初的想法，协方差矩阵的特征值分解给出旋转矩阵（特征向量）和幅度（特征值）。
如果我们只知道相机平面的旋转矩阵Rc，我们可以写成：
N = Rc * eigenvectors;
New_covariance = N*eigenvalues*inv(N) #main relation of decomposition
但是，我们必须确保特征向量排列为X,Y,Z
最后，我们将New_covariance的上2X2矩阵作为投影二维协方差（删除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;
};
如何分析地表达转换变量的协方差？
您可以使用 uncertainty propagation equations 解析地获得一阶近似值。特别是，关于 non-linear 组合的段落基本上解释了以下内容：
Knowing the covariance
C_xon a variablexand the jacobian matrixJ_fof 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.^Tis 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] = [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有助于表达组合函数的导数：
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
同样，这是一阶近似，但针孔投影函数是 "not very non linear"，这意味着对于大多数应用来说，这种近似通常足够接近。