Fitting orthogonal planes to a point set

Question

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)?

Answer 1

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 [1] 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 [2].

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 [3]. For a simple plane, such voting table is dimension-less (we only accumulate the number of votes for each - see [3]). 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 [1] 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 : )

Answer 2

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.

Edit Update to answer your question.

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[256]:= 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[[2]]]
FullSimplify[g[n1, n2, n3, m1, m2, m3, l1, l2, l3] /. s[[2]]]
FullSimplify[f[n1, n2, n3, c1, m1, m2, m3, c2] /. s[[2]]]
{n1, n2, n3}.{n1, n2, n3} - 1 /. s[[2]]
{m1, m2, m3}.{m1, m2, m3} - 1 /. s[[2]]
{n1, n2, n3}.{m1, m2, m3} /. s[[2]]


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

Out[264]= {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[265]= 1.07099*10^-29

Out[266]= -1.83184*10^-17

Out[267]= 2.17663

Out[268]= 0.

Out[269]= 0.

Out[270]= 5.55112*10^-17

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