约定
A. \(\mathbf{a}^T\mathbf{b}\)默认为一个数值。
B. \(\mathbf{q}_i\)表示矩阵\(Q\)的第\(i\)个行组成的向量。
C. \(q_i\)表示向量\(\mathbf{q}\)的第\(i\)个元素。
D.
$$
\mathbf{q}^\wedge =\begin{bmatrix}
0 & -q_2 & q_1\\
q_2 & 0 & -q_0\\
-q_1 & q_0 & 0
\end{bmatrix}
$$
问题
现在已知一直线，在相机平面内的一些观测点，其中相机位姿为\(R\)、\(\mathbf{t}\)，相机参数为\(K\)，其中直线上一个点在相机里的像素坐标为\((u,v)\)，现在想建立直线和相机位姿以及观测直接的关系
这里设空间直线为如下的参数方程
(1)
$$
\mathbf{p} = \mathbf{n}k + \mathbf{m}
$$
公式中约定\(\mathbf{n}\)为单位方向向量，然后将直线上一个点往相机里投影，根据投影方程，可以得到
(2)
$$
s
\begin{bmatrix}
u\\
v\\
1
\end{bmatrix}
=
K(R\mathbf{p}+\mathbf{t})
\\
\Downarrow
\\
sK^{-1}\begin{bmatrix}
u\\
v\\
1
\end{bmatrix}
=R\mathbf{p}+\mathbf{t}
$$
这里令
(3)
$$
\mathbf{a} = K^{-1}\begin{bmatrix}
u\\
v\\
1
\end{bmatrix}
$$
将公式(3)带入(2)并约掉\(s\)有
(4)
$$
\begin{bmatrix}
a_0/a_2\\
a_1/a_2
\end{bmatrix}
=
\begin{bmatrix}
(\mathbf{r}_0 \cdot \mathbf{p} + t_0)/(\mathbf{r}_2 \cdot \mathbf{p} + t_2)\\
(\mathbf{r}_1 \cdot \mathbf{p} + t_1)/(\mathbf{r}_2 \cdot \mathbf{p} + t_2)
\end{bmatrix}
\\
\Downarrow
\\
\begin{bmatrix}
(\mathbf{r}_0 – a_0/a_2 \mathbf{r}_2)\cdot \mathbf{p}\\
(\mathbf{r}_1 – a_1/a_2 \mathbf{r}_2)\cdot \mathbf{p}
\end{bmatrix}
=
\begin{bmatrix}
t_2a_0/a_2 – t_0\\
t_2a_1/a_2 – t_1
\end{bmatrix}
$$
进一步，令
(5)
$$
V = \begin{bmatrix}
(\mathbf{r}_0 – a_0/a_2 \mathbf{r}_2)^T\\
(\mathbf{r}_1 – a_1/a_2 \mathbf{r}_2)^T
\end{bmatrix}_{2 \times 3}
\\
\mathbf{w} = \begin{bmatrix}
t_2a_0/a_2 – t_0\\
t_2a_1/a_2 – t_1
\end{bmatrix}
$$
则将公式(1),(5)带进公式(4)有
(6)
$$
\mathbf{w}=V\mathbf{p}=kV\mathbf{n}+V\mathbf{m}
$$
消去公式(6)中的\(k\)有
(7)
$$
\frac{w_0 – \mathbf{v}_0 \cdot \mathbf{m}}{w_1-\mathbf{v}_1 \cdot \mathbf{m}}=\frac{\mathbf{v}_0\cdot \mathbf{n}}{\mathbf{v}_1\cdot \mathbf{n}}
\\
\Downarrow
\\
(w_0 \mathbf{v}_1-w_1\mathbf{v}_0)\cdot \mathbf{n} +\mathbf{m}^T (\mathbf{v}_1\mathbf{v}_0^T-\mathbf{v}_0\mathbf{v}_1^T)\mathbf{n}=0
$$
为了简洁，这里记
(8)
$$
\mathbf{b}=w_0 \mathbf{v}_1-w_1\mathbf{v}_0
\\
\mathbf{c}^\wedge = \mathbf{v}_1\mathbf{v}_0^T-\mathbf{v}_0\mathbf{v}_1^T
$$
将(8)带进(7)有
(9)
$$
(\mathbf{b}^T + \mathbf{m}^T\mathbf{c}^\wedge)\mathbf{n} = \mathbf{n}^T(\mathbf{b} – \mathbf{c}^\wedge \mathbf{m}) =0
$$
由于\(\mathbf{n}\)是单位方向向量，因此这里不妨设置
(10)
$$
\mathbf{n}=\begin{bmatrix}
\sin \theta\\
\cos \theta \cos \beta\\
\cos \theta \sin \beta
\end{bmatrix}
$$
则最终带入方程有
(11)
$$
\sin \theta (b_0 + c_2m_1 – c_1m_2) +\cos \theta \cos \beta (b_1 + c_0m_2 – c_2m_0) +\cos \theta \sin \beta(b_2+c_1m_0 – c_0m_1)=0
$$
从公式(11)可以看出，已知多组相机参数以及观测，则可以拟合出最佳直线参数\(\mathbf{n}\)与\(\mathbf{m}\)。
同样地，如果已知直线与相机参数，以及观测，则要求直线上某个点的3d位置，根据公式(6)可以得到
(12)
$$
k = \frac{\mathbf{n}^TV^T(\mathbf{w}-V\mathbf{m})}{\mathbf{n}^TV^TV\mathbf{n}}
$$
最终将\(k\)代入公式(1)的直线方程，可得到3d点位置。