The problem:
I have a set of measurements, represented by 3D points, and corresponding measurement uncertainties, represented by a $3\times3$ covariance matrix for each point. I would like to fit a plane through these points in a meaningful way, while taking into account the known uncertainties of the points.
What I tried:
The best I can do so far is scalar weighting of each point (similarly to [1]) based on e.g. the trace or determinant of the corresponding covariance matrix, which ignores a lot of important information in the matrix (in my case, the covariance ellipsoids are significantly elongated in one direction, which has the highest measurement uncertainty due to the measuring process).
I have also tried a method, which is described in [2] and takes the full covariance matrices into consideration, and approaches this as a MLE problem, which seems reasonable. However, I've implemented the method in Matlab and it seems to be very unstable, sometimes giving better results than plain least-squares fit, sometimes giving totally nonsential results. Maybe I'm just misinterpreting the paper or unable to implement the method properly...
The question:
Is there some implementation of a similar algorithm available? If not, could you at least recommend some other methods, possibly with a related paper or some tutorial describing them? I would also be glad for any insights into the method described in [2] and how to implement it for plane fitting, since I seem to be failing to do so.
Thank you in advance!
[1]: J. Weingarten, G. Gruener, and R. Siegwart, "Probabilistic plane fitting in 3D and an application to robotic mapping," ICRA, 2004.
[2]: W. Chojnacki, M. Brooks, A. van den Hengel and D. Gawley, "On the fitting of surfaces to data with covariances," PAMI, 2000.
So it turns out that the instability of the method from [1] is a known property, and is discussed in a follow-up paper [3] (end of sec. 3), where an update is proposed. The update lies in reducing the optimalization problem to a similar one, which is well defined, and the final solution may be calculated from the reduced solution, as is described in sec. 4 and 5 of [3].
For anyone looking for a method to fit a surface to a set of points with known covariances (a common problem in computer vision), I refer the reader to [1] and [3]. I have tested the modified method from [3] in Matlab and it performs much more stably than the original algorithm from [1].
[3]: W. Chojnacki, M. Brooks, A. van den Hengel, D. Gawley, "FNS and HEIV: Relating Two Vision Parameter Estimation Frameworks", ICIAP, 2003.