algorithmopencvcomputer-visionpose-estimationuncertainty

Uncertainty on pose estimate when minimizing measurement errors

Let's say I want to estimate the camera pose for a given image I and I have a set of measurements (e.g. 2D points ui and their associated 3D coordinates Pi) for which I want to minimize the error (e.g. the sum of squared reprojection errors).

My question is: How do I compute the uncertainty on my final pose estimate ?

To make my question more concrete, consider an image I from which I extracted 2D points ui and matched them with 3D points Pi. Denoting Tw the camera pose for this image, which I will be estimating, and piT the transformation mapping the 3D points to their projected 2D points. Here is a little drawing to clarify things:

enter image description here

My objective statement is as follows: minimization objective statement

There exist several techniques to solve the corresponding non-linear least squares problem, consider I use the following (approximate pseudo-code for the Gauss-Newton algorithm):

enter image description here

I read in several places that JrT.Jr could be considered an estimate of the covariance matrix for the pose estimate. Here is a list of more accurate questions:

  1. Can anyone explain why this is the case and/or know of a scientific document explaining this in details ?
  2. Should I be using the value of Jr on the last iteration or should the successive JrT.Jr be somehow combined ?
  3. Some people say that this actually is an optimistic estimate of the uncertainty, so what would be a better way to estimate the uncertainty ?

Thanks a lot, any insight on this will be appreciated.

Solution

  • The full mathematical argument is rather involved, but in a nutshell it goes like this:

    1. The outer product (Jt * J) of the Jacobian matrix of the reprojection error at the optimum times itself is an approximation of the Hessian matrix of least squares error. The approximation ignores terms of order three and higher in the Taylor expansion of the error function at the optimum. See here (pag 800-801) for proof.
    2. The inverse of the Hessian matrix is an approximation of the covariance matrix of the reprojection errors in a neighborhood of the optimal values of the parameters, under a local linear approximation of parameters-to-errors transformation (pag 814 above ref).

    I do not know where the "optimistic" comment comes from. The main assumption underlying the approximation is that the behavior of the cost function (the reproj. error) in a small neighborhood of the optimum is approximately quadratic.