Obtain Surface Projection Covariance from gtsam

[Thomas Eberhard]

When computing the projection of a point from a camera to a flat surface, e.g. drone image.
I am interested in computing an approximate Covariance for the projected point on the surface. I want to do this under the assumption that the covariance of the position and orientation are known. 

In this setting I wanted to ask if I can employ GTSAM to achieve an approximate Covariance for the surface projection of a camera point or if there are other ways to achieve this you know of.

Best, 
Thomas

[Dellaert, Frank]

If you create a PinholeBaseK camera and call project, with known pose and point and with optional Jacobians, then you’ll get a 2*6 Jacobian F and a 2*3 Jacobian G. I think then the answer is just

 

Cov = F * Cov_pose * F’ + G * Cov_point * G’

 

Mattias can sanity check me 😊

 

Frank

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/2abdf79e-706b-430e-9bbd-0f1377a832cdn%40googlegroups.com.

[Dellaert, Frank]

In general, btw, you could create *any* expression in GTSAM using Expression.h, using constants for things you know, and Expression<T>s for anything that is variable or uncertain, and then the expression will have OptionalJacobians (use Evaluate), and then do the same:

 

Expression<T1> t1(1); // 1 is an example key, you can use any key you want

Expression<T2> t2(2);

 

Expression<T> f = ….;

 

T f_value = f.evaluate(t1_value, t2_value, …, H1, H2, …);

 

Matrix  Cov_f = H1 * Cov1 * H1’ + H2 * Cov2 * H2’ + …

 

Might not compile as stated 😊 Needs tightening.

 

Frank

[Thomas Eberhard]

Hi thanks for the quick answer and support.
The way I understand it this would give me the covariance of a pixel in the image space given a covariance of the pose and the observed point. I am interested in obtaining the covariance of a point given an error of a pixel and an error of the pose.
Would you suggest to inverse the equations like this to obtain it or should I consider a different route:
G'(Cov_pixel - F * Cov_pose * F’)G =  Cov_point

Best, 
Thomas

[Matías Mattamala]

Hi Thomas,

In that case the procedure should be the same as Frank shared in his follow-up answer, but instead of using the "project" method, you will need to use "backproject" to get the Jacobians, since that one maps from pixels to 3D points (https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/PinholePose.h#L139)

Note that backproject returns 3 Jacobians: with respect to the pose, the point in pixels, and the depth. You will need to assume some noise for the depth if you have a monocular camera though, and that should be also scaled by the same factor you use to determine the 3D point (obtained from stereo, depth image, or learning method I guess).

Then to compute the covariance of the 3D point you can proceed as Frank said: Matrix  Cov_f = H1 * Cov1 * H1’ + H2 * Cov2 * H2’ + …

Best,
Matias

[Thomas Eberhard]

Hi, thanks again for your ongoing support in this issue.

Setting aside of backprojection I have first looked into the mathematics of projection.
I have one more question. I have read the math.pdf paper as well as the source code.
I am able to identify the SE(3) mathematics on page 19 with the derivative in Pose3::transformTo.
I think that the code that is applied when computing the Jacobians is essentially the mathematics on page 19.
My understanding in this case is that this is a derivative with respect to an angular velocity and an velocity.
I have not yet reached a full understanding of the angular velocity but with respect to the velocity I am unsure.
Let's consider the case where the angular error is 0 and focus on the positional error. My understanding is that the velocity that generates the SE(3) group is related to the positional error by a formula layed out in LieGroups.pdf on page 15. In particular this would mean that the parametrization of the velocity is different of the parametrization of space. I.e. if my position has a standard deviation of 1m I first need to compute what corresponding standard deviation of the velocity is implied?
I believe that otherwise the computed elipsis would systematically differ from a MonteCarlo Sample of the projected point.
Is this understanding correct?
I think this is relevant as the error of the point itself has a standard deviation parametrized in meter that does not have to consider a velocity.

Best,
Thomas

[Matías Mattamala]

Hi Thomas,

If I understand correctly, the main confusion is about the derivatives being computed with respect to velocities as it is indicated in math.pdf, page 19. Hence, it wouldn't make sense to compute the corresponding covariance using such Jacobians, because they are related to velocities, not to actual positions/orientations.

The important point here, is the fact that when we are computing derivatives of poses (SE(3) objects), we are not interesting in differentiating with respect to the matrix components, but the "intrinsic" dimensions of the pose, which corresponds to a 6x1 vector with the 3 orientations in the first components, and 3 position coordinates in the last ones. Such vector is defined as "xi" in math.pdf,  and it's known as a "tangent vector", because it lies in a tangent plane defined on the SE(3) manifold, which locally describes the manifold using its intrinsic dimensionality. If we differentiate tangent vectors w.r.t time, we obtain velocities, which also lie on the tangent plane, and that's why sometimes people use "tangent vector" or "velocities" to refer to the same 6x1 vector. If this is still confusing, perhaps it can help to take a look at the blog posts we wrote a while ago, which include more visualizations: https://gtsam.org/2021/02/23/uncertainties-part2.html

That being said, the Jacobians approach to compute the covariance should work, because covariance matrices for both pose/position and velocities are defined on the tangent space of SE(3), and the Jacobians depend on spatial coordinates (i.e, meters), not time.

Finally, I must also say that if you compare this method using the Jacobians vs a Monte Carlo approach will differ for sure. Using the Jacobians is just a first order approximation, while Monte Carlo should allow you to sample the true distribution. There is a paper that presents more advanced ways to propagate uncertainties in camera models by using higher order terms, but perhaps it may be overkill: http://ncfrn.mcgill.ca/members/pubs/barfoot_tro14.pdf (also there important notation and conventions differences with GTSAM)

Best,
Matias