Pose estimation by aligning non-collinear points

Question

As shown in Fig.1, consider $3$ line segments of equal length each containing $4$ points. Let $j = 1,\dots,3$ and $i = 1,\dots,4$ be the index for the line segment and the points on the line segment respectively. $^j\mathbf{T}$ is the (rigid) transformation matrix that describes the pose (position and orientation) of the $j^\mathrm{th}$ line segment with respect to origin $O$. Similarly $^j\mathbf{x}_i = \,^{\!j}(x_i, y_i)$ is the coordiate of the $i^\mathrm{th}$ point on $j^\mathrm{th}$ line segment with respect to origin.

As illustrated in Fig.2 suppose that after measurement only the pose of the second line segment deviates from its true value and is therefore denoted by $^2\mathbf{T'}$.

What I know after the measurement (Fig.2) are as follows:

• Coordinates of $^1\mathbf{x}_i$, $^2\mathbf{x'}_i$ and $^3\mathbf{x}_i$ for $i = 1,\dots,4$ with respect to $O$.
• Values of $^1\mathbf{T}$, $^2\mathbf{T'}$, $^3\mathbf{T}$

Constraints

• The points on the line segment are fixed. That is when I transform the line segment, the points move together with the line segment.

My goal is to displace (translate + rotate) the second line segment with pose $^2\mathbf{T'}$ such that the points become collinear as illustrated in Fig.1. Or in other words displace the second line segment such that $^2\mathbf{T'}$ coincides with $^2\mathbf{T}$.

Approach

This can be treated as an optimization problem where we wish to find $^2\mathbf{T}$ such that the shortest distance between the points $^2\mathbf{x'}_i$ and the straight line joining $^1\mathbf{x}_i$ and $^3\mathbf{x}_i$ is minimized. The problem is if I have over 10000 points and over 30 line segments, the computational time is huge.

Since the points are non-collinear, the grey dotted line connecting the points form an angle at points that lie on second line segment. Is it possible to estimate $^2\mathbf{T}$ iteratively just by using the angle information formed by this grey dotted line as shown in Fig.2? For instance I can assign a vector to each point on the second line segment that divides the angle in half. Then use this vector to find how or in which direction the line segment should be moved.

Any comments or suggestions are welcome.

Answer 1

Optimization problem

---

Not sure if the cost function for the optimization problem is formulated properly but here is the basic idea:

If I define the points $\{^{2}\mathbf{x_1}, ^{2}\mathbf{x_2}, \dots ^{2}\mathbf{x_4}\}$ on second line segment as $^{2\!}X$, what I am looking for is to find $\{^{2}\mathbf{x_1'}, ^{2}\mathbf{x_2'}, \dots ^{2}\mathbf{x_4'}\}$ or $^{2\!}X'$ such that $$^{2\!}X' = [\mathbf{R}\, \vert\,\mathbf{t}]_{3\times 3} \, ^{2\!}X$$ or $$ \underbrace{ \begin{bmatrix} ^2 x_1' & ^2 x_2' & \dots & ^2 x_4'\\ ^2 y_1' & ^2 y_2' & \dots & ^2 y_4'\\ 1 & 1 & \dots & 1 \end{bmatrix}}_{^{2\!} X'} = \underbrace{ \begin{bmatrix} r_{11} & r_{12} & t_x\\ r_{21} & r_{22} & t_y\\ 0 & 0 & 1 \end{bmatrix}}_{\mathbf{^2 T'}} \, \underbrace{ \begin{bmatrix} ^2 x_1 & ^2 x_2 & \dots & ^2 x_4\\ ^2 y_1 & ^2 y_2 & \dots & ^2 y_4\\ 1 & 1 & \dots & 1 \end{bmatrix}}_{^{2\!} X} $$ Here the components of Rotation Matrix $\mathbf{R}$ and $\mathbf{t}$ are also unknown.

By using the points on first and the third line segment I obtain the equations of straight lines that passes through these points or in other words, the equations of lines joining the corrosponding points on first and third line segments. By denoting the straight line with $f(x)$, for a line segment with 4 points (as shown in the illustration) we obtain $$ \begin{bmatrix} f_1(x)\\ f_2(x)\\ \vdots\\ f_4(x) \end{bmatrix} $$ The cost function then takes the form $$ \begin{pmatrix} \begin{bmatrix} f_1(^2 x_1)\\ f_2(^2 x_2)\\ \vdots\\ f_4(^2 x_4) \end{bmatrix} - \begin{bmatrix} ^2 y_1\\ ^2 y_2\\ \vdots\\ ^2 y_4 \end{bmatrix} \end{pmatrix}^2 \leftarrow \mathrm{min} $$ that satisfies the condition $$^{2\!}X' = [\mathbf{R}\, \vert\,\mathbf{t}]_{3\times 3} \, ^{2\!}X$$ as mentioned above.