# Fitting orthogonal planes to a point set

I have a set of 3d points to which I want to fit two planes. I know the assignment of points to the planes so I don't need any RANSAC or similar. Currently, I'm using a PCA-based approach to fit two planes and they are rather orthogonal (+/- 2 deg) but I'd like to integrate this knowledge directly into the plane fitting so that the planes are orthogonal by design.

Has someone got an idea for a closed form solution or will I have to do some numerical optimization (should probably work nicely as I already have very good initial values for both planes)?

• Maybe a naieve suggestion, but could you parameterize your two orthogonal planes (I make it 5 variables) and do a least squares minimization? Parameters $\theta$, $\phi$ and $\psi$ rotations about $x$- $y$- and $z$- axes respectively, and the last two for a translation in the $x$-$y$ plane. Cost function, minimum of distance from $x$-$z$ and $y$-$z$ plane after rotation and translation. – Steve Feb 2 '16 at 12:07

I think your problem can be written as an optimization problem.

$\{x_i\}$ is the set of points for plane 1, $\{x_j\}$ for plane 2 respectively. Their orthonormal vectors are $n_1$ and $n_2$ with constraints: $|n_1|=1$, $|n_2|=1$ and $n_1n_2=0$. $\{\lambda_i\}$ the set of lagrange multiplier.

The functional under constraints reads $$\sum_i (x_i n_1-c_1)^2 +\sum_j(x_jn_2-c_2)^2+\lambda_1 n_1n_2+\lambda_2(n_1n_1-1)+\lambda_3(n_2n_2-1)$$

So you are looking for the minimum of the functional with parameter set $\{n_{1x},n_{1y},n_{1z},n_{2x},n_{2y},n_{2z},c_1,c_2,\lambda_1,\lambda_2\lambda_3\}$.

Of course, if you parametrize the conditions into the normal vectors of the planes you can skip the lagrange multiplier.

This approach will guarantee orthogonality (within the numerical accuracy of representing $\lambda_i$). You can easily see this, if you don't use lagrange multipliers but choose spherical coordinates for your normal vectors.

Let's say $\phi_i, \theta_i$ are the spherical coordinates ($\phi_i\in[-\pi,\pi], \theta_i\in[0,\pi]$), $|n_i|=1$ is then already in the parametrization and the additional condition $n_1n_2=0$ will lead to a condition on $\theta_2$:

$\cot\theta_2=\frac{\sin\theta_1(\cos \phi_1 \cos\phi_2+\sin \phi_1 \sin\phi_2)}{\cos \theta_1}$ with $\theta_1 \neq \frac{\pi}{2}$, which you can use to express $n_2$ with $\{\phi_1,\theta_1,\phi_2\}$.

Now, if you plug this into the posted functional you will see that it obeys this parametrization as well and you are left with the set of free parameters $\{c_1,c_2,\phi_1,\phi_2,\theta_1\}$.

But as you can see, if you choose such a map for $\mathbb{R}^3$, you have to deal with all degenerancies of the map, which is unfovarable in this case.

Playing around with a test case I like to add some more information about numerical minimization with lagrange constraints. Numerical minimzation routines have problems with saddle points. One way to circumvent such a problem is not to minimize the functional stated above, but to minimze the norm of its gradient $|\nabla f|$.

Here is my mathematica test case:

$f$ is the functional without constraints. $g$ are the lagrange multiplier and constraints. $ll$ is the functional as written above. $lll$ is the norm of the gradient of the functional above.

In:= ClearAll["Global*"]
x1 = RandomReal[1, {17, 3}];
x2 = RandomReal[1, {17, 3}];
vars = {n1, n2, n3, c1, m1, m2, m3, c2, l1, l2, l3}
f[n1_, n2_, n3_, c1_, m1_, m2_, m3_, c2_] =
Total[(x1.{n1, n2, n3} - c1)^2] + Total[(x2.{m1, m2, m3} - c2)^2];
g[n1_, n2_, n3_, m1_, m2_, m3_, l1_, l2_, l3_] =
l1 ({n1, n2, n3}.{n1, n2, n3} - 1) +
l2 ({m1, m2, m3}.{m1, m2, m3} - 1) + l3 ({n1, n2, n3}.{m1, m2, m3});
ll[n1_, n2_, n3_, c1_, m1_, m2_, m3_, c2_, l1_, l2_, l3_] =
f[n1, n2, n3, c1, m1, m2, m3, c2] +
g[n1, n2, n3, m1, m2, m3, l1, l2, l3];
lll[n1_, n2_, n3_, c1_, m1_, m2_, m3_, c2_, l1_, l2_, l3_] :=
Table[D[ll @@ vars, vars[[i]]], {i, 1, Length[vars]}].Table[
D[ll @@ vars, vars[[i]]], {i, 1, Length[vars]}]
s = FindMinimum[
lll[n1, n2, n3, c1, m1, m2, m3, c2, l1, l2,
l3], {{n1, 1.0}, {n2, 0}, {n3, 1}, {m1, 0.0}, {m2, 1}, {m3,
1}, {c1, 1}, {c2, 2}, {l1, 1}, {l2, 2}, {l3, 3}}]
FullSimplify[lll[n1, n2, n3, c1, m1, m2, m3, c2, l1, l2, l3] /. s[]]
FullSimplify[g[n1, n2, n3, m1, m2, m3, l1, l2, l3] /. s[]]
FullSimplify[f[n1, n2, n3, c1, m1, m2, m3, c2] /. s[]]
{n1, n2, n3}.{n1, n2, n3} - 1 /. s[]
{m1, m2, m3}.{m1, m2, m3} - 1 /. s[]
{n1, n2, n3}.{m1, m2, m3} /. s[]

Out= {n1, n2, n3, c1, m1, m2, m3, c2, l1, l2, l3}

Out= {6.88327*10^-30, {n1 -> 0.441163, n2 -> -0.600498,
n3 -> 0.666916, m1 -> 0.243615, m2 -> 0.795371, m3 -> 0.55501,
c1 -> 0.244619, c2 -> 0.855017, l1 -> -1.43598, l2 -> -0.740652,
l3 -> -0.329995}}

Out= 1.07099*10^-29

Out= -1.83184*10^-17

Out= 2.17663

Out= 0.

Out= 0.

Out= 5.55112*10^-17
`

As you can see, the solution planes are orthonormal within numerical noise.

• Good approach. Yet, the output doesn't for sure guarantee orthogonality, right? – Tolga Birdal Feb 7 '16 at 10:48

Here I devise a novel strategy, based on only 3D points, that I think, would work.

I will parametrize a 3D plane by a point $\mathbf{p}$ and its normal $\mathbf{n}$. Imaging you take a pair of oriented points $\mathbf{p}_1$ and $\mathbf{p}_2$, on the point cloud $\mathbf{P}$ with corresponding normals $\mathbf{n}_1$ and $\mathbf{n}_2$. Let $\mathbf{d}$ denote the vector joining two points, i.e. $\mathbf{d}=\mathbf{p}_1-\mathbf{p}_2$ Ideally, two points would be located on orthogonal planes iff :

$$\lvert \angle(\mathbf{n}_1, \mathbf{n}_2) \rvert = \pi \mathbin{/} 2 \\ \lvert \angle(\mathbf{n}_1, \mathbf{d}) \rvert \leq \pi \mathbin{/} 2 \\ \lvert \angle(\mathbf{n}_2, \mathbf{d}) \rvert \leq \pi \mathbin{/} 2$$

More over if one wants to penalize the size constraint (for example when the extent of the planes are known), he/she could as well write: $$\lVert \mathbf{d} \rVert < \tau_{d}$$ where $\tau_{d}$ is a threshold on the 3D extent of the planes. As I already mentioned, these are ideal cases. In real life, e.g. for the data coming from RGBD sensors, this is never the case. So it is better to introduce a noise threshold, where certain tolerance is maintained for the sake of robustness. Then :

$$\pi \mathbin{/} 2-\epsilon_n < \angle(\mathbf{n}_1, \mathbf{n}_2) < \pi \mathbin{/} 2+\epsilon_n \\ \lvert \angle(\mathbf{n}_1, \mathbf{d}) \rvert < \pi \mathbin{/} 2 + \epsilon_d\\ \lvert \angle(\mathbf{n}_2, \mathbf{d}) \rvert < \pi \mathbin{/} 2 + \epsilon_d$$ where $\epsilon_d$ and $\epsilon_n$ trades-off noise tolerance to accuracy.

Note that, to guarantee the angles to be lying in range $[-\pi, \pi]$, and to be robust to small angles, the formula from Birdal and Ilic 2015  can be used:

$$\angle(\mathbf{v_1},\mathbf{v_2}) = \tan^{-1}\big( \frac{ \Vert \mathbf{v_1} \times \mathbf{v_2} \rVert}{\mathbf{v_1} \cdot \mathbf{v_2}}\big)$$

From this points, one could always write a RANSAC loop, where random point pairs are tested for the satisfaction of these constraints. However, a better idea (which would make this more efficient and possibly robust) is to employ a somewhat generalized Hough voting procedure, again similar to 1 or .

We sample the scene for $N$ reference points. We fix each such reference point and pair it with all other $N-1$ points, found on the sampled scene. We compute the pair features, which are in this case:

$$F(\mathbf{p}_1, \mathbf{p}_2) = \big(\angle(\mathbf{n}_1, \mathbf{n}_2), \angle(\mathbf{n}_1, \mathbf{d}), \angle(\mathbf{n}_2, \mathbf{d}), \lVert \mathbf{d} \rVert \big)$$

For orthogonal planes, given the point pair, the definition is immediate (the normals of two points uniquely define the orthogonal planes). However, finding best such candidate requires care if the scene is cluttered. Thus, we evaluate the aforementioned constraints and create a voting table, similar to . For a simple plane, such voting table is dimension-less (we only accumulate the number of votes for each - see ). However, for the orthogonal case, some care must be taken. This, I describe as follows.

Once an oriented pair of points is found to be on orthogonal planes, the reference point ($\mathbf{p}_1$) defines the first plane. Then the orthogonal counterpart, can freely rotate around the normal of the reference and is also free to slide orthogonally on this infinite reference plane. This creates two degrees of freedom, which can be represented in 2D polar space: ($\theta$, $\rho$). $\theta$ denotes rotation around normal, and $\rho$ is the point to plane distance on the 2D reference plane to the reference point. Note that once the intersection line of the two planes $\mathbf{l}=\{a,b,c,d\}$ is computed, this reduces to point to line distance. The vote ($\theta$, $\rho$) can be casted by transforming the point pair to the origin, and aligning $\mathbf{n}_1$ with $y$ axis. So,

$$\theta=\angle(\mathbf{l}, \mathbf{x}) \\ \rho=\frac{\lvert c \rvert}{\mathbf{\sqrt{a^2+b^2}}}$$

where $\mathbf{x}$ is the x-axis. (Notice the resemblance to the Hough transform?) The voting is performed locally for each reference point, resulting in $\mathbf{\theta}=\{\theta_1,...,\theta_N\}$ and $\mathbf{\rho}=\{\rho_1,...,\rho_N\}$. Note that this is also a local coordinate system and these entities vary depending on the selected pair point. However, for each reference point, if the pair lies on the orthogonal planes, ideally they should vote for the same local reference frame. The voting also requires quantization of the local reference frame, which can be chosen reasonably depending on the problem size.

Given all the solutions of the orthogonality for every reference point, the task is then to cluster them. Birdal and Ilic  employ an agglomerative clustering, while other choices are also possible. Note that, for the clustering stage the standard parameterization $[a,b,c,d]$ is a better choice, as it reduces the dimension.

Due to quantization, noise and artifacts, the plane fitting to this end is a rough one. It is acceptably good but a refinement might be desirable. For that purpose, I would use a non-linear Levenberg Marquardt adjustment procedure, in which the distance from the point cloud to the orthogonal planes are jointly minimized. This can easily be done by minimizing the objective:

$$E(\mathbf{P}, \{\mathbf{W}_1,\mathbf{W}_2\}) = \sum_{i=1}^M min( Q(\mathbf{p}_i, \mathbf{W}_1), Q(\mathbf{p}_i, \mathbf{W}_2) )$$

where $\mathbf{W}_1=\{\mathbf{W}_1,\mathbf{W}_2\}$ denotes the orthogonal planes, $\mathbf{P} \in \mathbb{R}^3$ is the point cloud, $Q(\mathbf{p}, \mathbf{P})$ is the point to plane distance (like ICP) and $\mathbf{p}_i$ represents each point on the point cloud.

If you really like to enforce the orthogonality in this stage, it could be done by a trivial regularization:

$$\arg\min_{\mathbf{W1}, \mathbf{W2} } E(\mathbf{P}, \{\mathbf{W}_1,\mathbf{W}_2\})\\ s.t. \mathbf{W}_1^n \cdot \mathbf{W}_2^n = 0$$

or minimize: $$E(\mathbf{P}, \{\mathbf{W}_1,\mathbf{W}_2\}) + \lambda (\mathbf{W}_1^n \cdot \mathbf{W}_2^n)$$

One drawback is that, points in the scene should cover a significant area of the geometric structure that you're looking for. If there is too much clutter, eveen though the scheme above would still work, it would be better to incorporate some robust functions into the formulation:

$$E_{robust}(\mathbf{P}, \{\mathbf{W}_1,\mathbf{W}_2\}) = \Phi(E(\mathbf{P}, \{\mathbf{W}_1,\mathbf{W}_2\})) + \lambda (\mathbf{W}_1^n \cdot \mathbf{W}_2^n)$$ where $\Phi$ is an M-Estimator, such as Tukey's Biweights or Huber function. $\mathbf{W}^n$ is the normal part (first 3 coefficients) of the plane.

This regularization actually constrains the orthogonality, but uses 8 parameters (4 for each plane). If you would like to reduce the dimension by a re-parameterization, here is how to do it:

$$\mathbf{x} =\{a,b,c,d,\theta,\rho\}$$

Here, $\{a,b,c,d\}$ denotes the equation of the first plane and $(\theta,\rho)$ are the parameters of the second plane in polar space, around the normal of the first plane as described above. The distance function then involves intersecting the planes, and doing the line-distance computation at each stage (as devised above). This is computationally more cumbersome, but causes the orthogonal planes to move together. Instead of polar parametrization, one could equivalently use a simple vector $\mathbf{v}=(\delta x, \delta y)$, which is the orthogonal, scaled direction vector from the center of the reference point to the intersection line:

$$\mathbf{x}_2 =\{a,b,c,d,\mathbf{v}\}$$

Both parameterizations $\mathbf{x}$ and $\mathbf{x}_2$ alleviate the need for regularization. One should try and see what would work the best.

This completes the procedure. So, a plane fitting and its refinement is devised.

Actually, now I think I must publish this idea. But, anyways, there you go : )

• I'm sorry, but I haven't found any usable solution in your answer. As far as I've understood it, your solution boils down to applying an LM optimization that even does not even guarantee that the planes are orthogonal. I already have the correct assignments of the points to the planes so that a RANSAC or voting process is not required. – FooBar Jan 31 '16 at 15:12
• Check again, I updated that part. – Tolga Birdal Jan 31 '16 at 15:16
• That also does not guarantee the orthogonality. The data is not from an RGBD-Sensor, they are outputs from another algorithm so that there is no clutter, and the points are close to the planes. Why do you try to optimize both W1 and W2 if they are dependent? Does that not just increase the number of optimized values. The Sum[min(Q(pi,W1),Q(pi,W2))] is not correct as the point-plane assignments are known. Using LM can be a valid approach, but I'd rather find a representation of the two planes that guarantees the orthogonality while keeping the number of parameters minimal ( i.e. 5). – FooBar Jan 31 '16 at 15:38
• Why is Sum[min(Q(p_i,W_1),Q(p_i,W_2))] not correct? It is a general form even if you have the assignments (during optimization there might be a danger of shifting to the other plane. Points don't always stay the same, especially around the line of intersection). So, this is valid if you have the assignments actually. But anyways, I updated my answer. – Tolga Birdal Jan 31 '16 at 17:07
• By the way, your question clearly states that : " I'd like to integrate this knowledge directly into the plane fitting so that the planes are orthogonal by design". This procedure I described here exactly does that. If you don't know that your planes are exactly orthogonal (+-2 might still be a large error), then you cannot use an orthogonal parameterization. In this case, regularization works the best. However, the fitting algorithm I have developed, guarantees orthogonality in the fitting phase. Then, parameterizations of $\mathbf{x}$ and $\mathbf{x}_2$ might be more suitable. – Tolga Birdal Jan 31 '16 at 17:17