Least squares solution for 2 perpendicular lines in vector notation

Question

I have the convex hull of the corner of a building in 2D and I am trying to fit 2 perpendicular lines to the set of points on that hull to get me the orientation of the corner. I have a solution but its not useful for cases where one of the lines passes through the origin. Please let me know if any of the derivations aren't clear, I have only included the key results for the sake of being concise but can add more.

Say I have 2 sets of 2D points $\mathcal{X}_1$ and $\mathcal{X}_2$ belonging to line $l_1: \mathbf{x}^t\mathbf{n}=1$ and $l_2: \mathbf{x}^t\mathbf{Rn}/\alpha=1$ where $\mathbf{R}$ is a 90 degrees rotation matrix. This means that the distance of the line from the origin is given by one over the magnitude of $\mathbf{n}$.

Solution 1:

I can formulate a cost function:

$C = ||\mathcal{X}_1\mathbf{n}-\mathbf{1}_1||_2^2+||\mathcal{X}_2\mathbf{Rn}/\alpha – \mathbf{1}_2||_2^2$

where $\mathbf{1}_i$ is a vector of 1s with length equal to the number of points in set $i$

I can solve this equation easily giving:

$\mathbf{n} = \mathcal{X}_1^+\mathbf{1}_1$

$\alpha = ||\mathcal{X}_2\mathbf{Rn}||_1/N_2$

where $N_2$ is the number of points in set 2.

Firstly, I find it curious that the solution for $\mathbf{n}$ does not depend on any of the points in set 2, but also as mentioned, for a line that passes through the origin the answer is difficult to compute.

Solution 2

Instead, if I formulate the lines as $l_1: \mathbf{x}^t\mathbf{n}=d_1$ and $l_2: \mathbf{x}^t\mathbf{Rn}=d_2$ but this time constrain $\mathbf{n}$ to be a unit vector I can write my cost function using a lagrange multiplier to keep the constraint:

$C = ||\mathcal{X}_1\mathbf{n}-\mathbf{d}_1||_2^2+||\mathcal{X}_2\mathbf{Rn} – \mathbf{d}_2||_2^2 + \lambda(\mathbf{n^Tn}-1)$

This solution I am having trouble getting to a solution for – perhaps I am missing a constraint? Anyway if I differentiate with respect to $\mathbf{n}$ and set to 0 I get the following:

$(\mathcal{X}_1^T\mathcal{X}_1 + \mathbf{R}^T\mathcal{X}_2^T\mathcal{X_2}\mathbf{R} + \lambda\mathbf{I})\mathbf{n}=\mathcal{X}_1\mathbf{d}_1 + \mathbf{R}^T\mathcal{X}_2^T\mathbf{d}_2$

differentiating with respect to $\mathbf{d}_1$ and setting to 0 (noting that $\mathbf{d}_i=d_i\mathbf{1}_i$ and hence differentiating with respect to either the vector or scalar version and setting to 0 is equivalent)

I get $\mathbf{d}_1 = \mathcal{X}_1\mathbf{n}$ and similarly $\mathbf{d}_2 = \mathcal{X}_2\mathbf{Rn}$. First off this seems odd as it is an exact solution assuming no noise, but also when I substitute this to above everything cancels to give me $\lambda\mathbf{n}=0$ and I have no idea where to go from here.

Any help in how to get solution 2 making use of Lagrange multipliers would be grand – I have tried looking at formulating the problem in homogenous coordinates and am in the process of seeing whether that would work, but I a confused as to why the above gives no clear solution (or where my errors might be).

EDIT:

As requested in the comments here is a link to 4 different sets of
sample data

For context, the purpose of this task is that I have some 3D data of the corner of a building and I am trying to work out the corner planes to align it with a coordinate axis. I have aligned the ground plane with a principal axis and then collapsed the points into 2D (effectively getting a plan view). Finally, to get the data presented I kept taking convex hulls until I had at least 50 points with a certain separation.

The data is unlabelled, and the process assigns points to a line based on proximity, although arguably a better classification scheme is likely possible since I know that it will be a corner.

EDIT 2

To clarify my question isn't how to find a solution to the problem – I already have one, my question is why solution 2 using Lagrange multipliers doesn't lead to a straightforward solution despite the almost trivial difference in formulation between Solution 1 and Solution 2.

Answer 1

Calling

$$ g_1(x,y) = y-y_0-m(x-x_0)\\ g_2(x,y) = y-y_0+\frac 1m(x-x_0) $$

and $T_1, T_2$ the data sets, we can formulate the minimization problem

$$ \min_{x_0,y_0,m}\left(\sum_{p_k\in T_1}g_1(p_k)^2+\sum_{p_k\in T_2}g_2(p_k)^2\right) $$

Follows a MATHEMATICA script to perform the calculations

Data preparation

g1[x_, y_] := y - y0 - m (x - x0)
g2[x_, y_] := y - y0 + 1/m (x - x0)
m = 1.5;
x0 = 2; y0 = 1; e = 0.2;
T1 = Table[{x + RandomReal[{-e, e}], y0 + m (x + RandomReal[{-e, e}] - x0)}, {x, 0, 10, 1/2}]
T2 = Table[{x + RandomReal[{-e, e}], y0 - 1/m (x + RandomReal[{-e, e}] - x0)}, {x, 0, 10, 1/2}]

Data processing

Clear[x0, y0, m]
f = Sum[g1[First[T1[[k]]], Last[T1[[k]]]]^2, {k, 1, Length[T1]}] + Sum[g2[First[T2[[k]]], Last[T2[[k]]]]^2, {k, 1, Length[T2]}];
sol = Minimize[f, {x0, y0, m}]
gr1 = ListPlot[T1];
gr2 = ListPlot[T2];
gr3 = ContourPlot[(g1[x, y] /. sol[[2]]) == 0, {x, 0, 10}, {y, -5, 5},
ContourStyle -> Red];
gr4 = ContourPlot[(g2[x, y] /. sol[[2]]) == 0, {x, 0, 10}, {y, -5, 5},
ContourStyle -> Red];
Show[gr1, gr2, gr3, gr4, PlotRange -> All, AspectRatio -> 1.6]

NOTE

Considering the furnished data with the sets $T_1, T_2$ mixed, the algorithm is as follows. Given a corner defined by

$$ p = \Lambda(p_0,\vec v,\lambda_1,\lambda_2) = \cases{p_0+\lambda_1\vec v\\ p_0+\lambda_2\vec v^{\top}},\ \ \ \{\lambda_1 \ge 0, \lambda_2 \ge 0, \vec v\cdot \vec v^{\top} = 0,\ ||\vec v||= 1\} $$

we calculate for each data point $p_k$ the minimum euclidean distance to $\Lambda(p_0,\vec v,\lambda_1,\lambda_2)$ which is $\left|\Lambda(p_0,\vec v,\lambda_1^k,\lambda_2^k)-p_k\right|$ and after that we minimize

$$ f(p_0,\vec v)=\sum_{k=1}^n\left|\Lambda(p_0,\vec v,\lambda_1^k,\lambda_2^k)-p_k\right| $$

The optimization MATHEMATICA script below implements this algorithm.

Clear[dist]
dist[vx_?NumericQ, vy_?NumericQ, px_?NumericQ, py_?NumericQ, pk_]:= Module[{lambda, d1,d2, v1 = {vx, vy}, v2 = {vy, -vx}, p0 = {px, py}},
lambda = -v1.(p0 - pk)/(v1.v1); 
If[lambda > 0, d1 = Sqrt[(p0 - pk + lambda v1).(p0 - pk + lambda v1)], 
d1 = Sqrt[(p0 - pk).(p0 - pk)]]; 
lambda = -v2.(p0 - pk)/(v2.v2); 
If[lambda > 0, d2 = Sqrt[(p0 - pk + lambda v2).(p0 - pk + lambda v2)], 
d2 = Sqrt[(p0 - pk).(p0 - pk)]]; Return[Min[d1, d2]]
]
sol = NMinimize[{Sum[dist[vx, vy, px, py, data[[k]]], {k, 1, Length[data]}], vx^2 + vy^2 == 1, -0.3 < px < 0.2, 0 < py < 0.4}, {vx, vy, px, py}, Method -> "DifferentialEvolution"]

The results are shown below. Here data represents each of the furnished sets 1.txt, 2.txt, 3.txt,4.txt