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.