How to Project 3D Covariance Matrix to a given Image Plane (Pose)

Question

I have a 3x3 covariance matrix for 3d-point and I want to know the equivalent 2d covariance (for u,v in image plane) , given the image pose [Xc,Yc,Zc,q0,q1,q2,q3] ,

There's a long (geometric) way that the 3d covariance could be a 3d ellipse , then projecting it into plane give 2d ellipse ,lastly converting the ellipse to 2d matrix , but this is long,

Any direct way, to solve this algebraically will help

P. S: any clues or reference to a solution (no need for code) ,will also help, and I will rewrite an answer with code (in c++)

I tagged also kalman filter , cause I think it's related to it

Answer 1

How do I express analytically the covariance on a transformed variable?

You can get a first order approximation analytically by using the uncertainty propagation equations. In particular, the paragraph on non-linear combinations explains basically the following:

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.

If I understood your question corerctly, you have the covariance on the 3D point expressed in the world coordinate frame, denoted C_Xw. You want the covariance of the projection of this point in the image plane, denoted C_xi. Let's denote by f the function mapping 3D world coordinates to image coordinates. Then we have: C_xi = J_f . C_Xw . J_f^T.

How do I compute the jacobian matrix J_f?

In practice, f is the pinhole projection function and can be decomposed as follows: f = f_intr o f_persp o f_pose, where:

1. f_intr applies the intrinsics camera coefficients (i.e. horizontal & vertical focal lengths fx and fy, skew s, principal point coordinates cx and 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 applies the pinhole perspective model to a 3D point in the camera coordinate frame: f_persp( [Xc; Yc; Zc] ) = [Xc/Zc; Yc/Zc]

3. f_pose applies the 3D rigid transformation (i.e. rotation R_cw, translation t_cw) mapping a 3D point in the world coordinate frame into a 3D point in the camera coordinate frame: f_pose( [Xw; Yw; Zw] ) = R_cw . [Xw; Yw; Zw] + t_cw

The chain rule of derivatives helps expressing the derivative of composed functions:

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 )

The jacobian matrices of f_intr, f_persp and f_pose are quite easy to express analytically:

1. f_intr is linear in xn, hence J_f_intr = [fx, s; 0, fy] is a constant

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

3. f_pose is linear in Xw, hence J_f_pose = R_cw is a constant

Final expression

Finally, we get the following analytical expression:

C_xi = J_f . C_Xw . J_f^T

where J_f = [fx, s; 0, fy] . [1/Zc, 0, -Xc/Zc²; 0, 1/Zc, -Yc/Zc²] . R_cw

Again, this is a first order approximation, but the pinhole projection function is "not very non linear", meaning that such an approximation is typically close enough for most applications.