# Least Squares Solution for the Closest 3D Point to a Set of Planes

I have a set of 2D planes in 3D space, each defined by a point on the plane and a normal vector to the plane (so vertically oriented planes are allowed). I need to find the point in 3D space that has the minimum sum of squared distances to all the planes. What's the right way to formulate this problem as a least squares regression?

(The regression is underspecified for fewer than 3 mutually intersecting planes.)

It would be even more ideal if I could use RANSAC to discard outliers, because the planes are unlikely to all intersect at a point.

The equation of any of the planes can be written as $$\newcommand{x}{\mathbf x}\newcommand{v}{\mathbf v}\newcommand{c}{c} \x \cdot \v_i + \c_i = 0$$ where $$\x$$ is the set of coordinates of a point on the plane, $$\v_i$$ is the normal vector to the plane, and $$\c_i$$ is a constant. You can find $$\c_i$$ by evaluating $$-\x \cdot \v_i$$ where $$\x$$ is set to the coordinates of the known point on the plane.

The distance from a point $$\x$$ to the plane is $$\frac{\lvert\x \cdot \v_i + \c_i\rvert}{\lVert\v_i\rVert}$$

and the square of the distance is therefore $$f(\x) = \frac{1}{\lVert\v_i\rVert^2} \left((\x \cdot \v_i)^2 + 2 c_i (\x \cdot \v_i) + c_i^2\right).$$

This comes down to a quadratic polynomial over the coordinates of $$\x.$$ Add together the polynomials from all planes and you still have a quadratic polynomial over the coordinates of $$\x$$, which is to be minimized.

If we represent the vectors by column vectors of coordinates, then $$(\x \cdot \v_i)^2$$ is $$(\x^T \v_i)^2$$ in matrix notation, and $$(\x^T \v_i)^2 = (\x^T \v_i)(\v_i^T \x) = \x^T A \x$$ where $$A = \v_i \v_i^T.$$ So $$f(\x)$$ is a quadratic form, which may help.

• Much appreciated, this is exactly what I needed. – Luke Hutchison Dec 21 '19 at 3:59
• Note that the absolute value on $\lvert\x \cdot \v_i + \c_i\rvert$ make the distance an unsigned distance, but the solution works just as well if you omit the absolute value and use the signed distance instead. For computation I usually prefer the signed distance. – David K Jul 22 '20 at 22:54
• Ah, thanks for clarifying! I was getting negative distances, and apparently I didn't see the absolute value bars :-) – Luke Hutchison Jul 23 '20 at 0:02
• Follow-up question: I'm using this to find the optimal translation mapping one point cloud to a similar point cloud. Solving the quadratic form for $x$ does give the right solution much of the time -- at least it gives a good estimate, and then I can move the query points closer to the solution, find the closest point to each query point again, then solve the quadratic form again. But in some cases, this converges slowly, and/or gives the wrong solution -- I think when there are many planes that are almost normal to the vector between the query point and the point on the plane. (ctd...) – Luke Hutchison Oct 12 '20 at 22:30
• OK, I think I had a misconception of the magnitude of the translation. If you are just correcting for a kind of "drift" or noise which is (usually) smaller than the distance between points in one cloud, then nearest neighbor will (usually) find the point that your particular point traveled to. Interesting. I still think that you might be able to summarize this in a question that could attract more interest (and maybe better answers). – David K Oct 16 '20 at 3:53

Fix a single plane, say defined by $$a(x-x_0)+b(y-y0)+c(z-z_0) =0$$ (this is the normal vector point form). What is the (signed) distance from a point $$(x,y,z)$$ to it? Well, if the vector $$(a,b,c)$$ has unit length, it's exactly the left hand side of that equation (it is the appropriate scalar projection onto the normal vector). This is just a linear function in $$(x,y,z)$$.

This suggests the way to solve the original question: We should do is rescale the normal vectors to have unit length then find the least square solution of the equations defining membership in the planes.