Triangulation and Linear Systems

Question

I'd like to ask your help to solve a linear system related to a triangulation problem involving two rays (vectors).

Let $a\textbf{p}_{l}$ ($a \in \mathbb{R}$) be the ray $l$ through $O_{l}$ ($a = 0$) and $\textbf{p}_{l}$ ($a = 1$). Let $T$ + $bR\textbf{p}_{r}$, $b \in \mathbb{R}$, the ray $r$ through $O_{r}$ ($b = 0$) and $\textbf{p}_{r}$ ($b = 1$). Let $\textbf{w} = \textbf{p}_{l} \times \textit{R}\textbf{p}_{r}$, the vector orthogonal to both $l$ and $r$, and $a\textbf{p}_{l} \hspace{1pt} + \hspace{1pt} c\textbf{w}$, $c \in \mathbb{R}$, the line $w$ through $a\textbf{p}_{l}$ (for some fixed $a$) and parallel to $\textbf{w}$.

All vectors and coordinates are referred to the left reference frame. $R$ and $T$ are the rotation and translation matrices correlating $l$ and $r$. The points $\textbf{p}_{l}$ and $\textbf{p}_{r}$ are known as well and defined as:

$$ \textbf{p}_{\textit{l}} = \begin{bmatrix} x_{\textit{l}}\\ y_{\textit{l}}\\ z_{\textit{l}} \end{bmatrix}, \hspace{10pt} \textbf{p}_{\textit{r}} = \begin{bmatrix} x_{\textit{r}}\\ y_{\textit{r}}\\ z_{\textit{r}} \end{bmatrix} $$

I need to determine the endpoints of the segment, $s$, belonging to the line parallel to $\textbf{w}$ that joins $l$ and $r$, $a_{0}\textbf{p}_{l}$ and $T \hspace{1pt} + \hspace{1pt} b_{0}R\textbf{p}_{r}$, by solving:

$$ a\textbf{p}_{l} - bR\textbf{p}_{r} + c(\textbf{p}_{l} \times \textit{R}\textbf{p}_{r}) = \textit{T} $$

After that, $P'$ is defined as the midpoint of the segment $s$. Any thoughts on how I can solve this linear system? It may be simple, but I got stuck on it.

Thanks in advance for any help.

Answer 1

Rewrite your equation in matrix form: $$\begin{bmatrix}\mathbf p_l & -R\mathbf p_r & \mathbf w\end{bmatrix} \begin{bmatrix}a\\b\\c\end{bmatrix} = T.$$ If the two rays are not parallel, the matrix on the left is invertible, hence the equation’s solution is simply $$\begin{bmatrix}a\\b\\c\end{bmatrix} = \begin{bmatrix}\mathbf p_l & -R\mathbf p_r & \mathbf w\end{bmatrix}^{-1} T.$$ Ultimately, you want the midpoint of $a\mathbf p_l$ and $bR\mathbf p_r$. That calculation can be added to the cascade to get $$P' = \frac12 \begin{bmatrix}\mathbf p_l & R\mathbf p_r & \mathbf 0\end{bmatrix} \begin{bmatrix}\mathbf p_l & -R\mathbf p_r & \mathbf p_l \times R\mathbf p_r\end{bmatrix}^{-1} T.$$

The point $P'$ can be computed in other ways. Observe that the line parallel to $\mathbf w$ on which it lies is the intersection of two planes parallel to $\mathbf w$ that contain each of the respective rays. $P'$ is then the orthogonal projection of the midpoint of $O_l$ and $O_r$ onto this line. Alternatively, note that the plane through $P'$ perpendicular to $\mathbf w$ is parallel to both rays and lies halfway between them. The midpoint of $O_l$ and $O_r$ also lies on this plane, which gives you a way to construct it, after which you can compute $P'$ as the intersection of the three planes.