VINS-Mono / VINS-Fusion中triangulatePoint()函数通过三角化求解空间点坐标，代码所体现的数学描述不是很直观，查找资料，发现参考文献[1]对这个问题进行详细解释，记录笔记以备忘。
1. VINS-Mono中相关代码 void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d) { Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero(); design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); Eigen::Vector4d triangulated_point; triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); point_3d(0) = triangulated_point(0) / triangulated_point(3); point_3d(1) = triangulated_point(1) / triangulated_point(3); point_3d(2) = triangulated_point(2) / triangulated_point(3); } 2. 数学推导先介绍一个重要的向量叉乘向矩阵运算转换的性质[4],
\[
\mathbf{a \times b = \hat{a}b}=\begin{bmatrix}0 &-a_3 &a_2 \\ a_3 &0 &-a_1 \\ -a_2 &a_1 &0\end{bmatrix}\begin{bmatrix}b_1\\b_2 \\b_3\end{bmatrix}
\tag{2-1}
\]
其中，\(\mathbf{\hat{a}}\)表示向量\(\mathbf{a}\)对应的反对称矩阵(skew-symmetric matrix)。
对于，\(X\)为三维空间点在世界坐标系下的齐次坐标\(X=\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\)，和 \(T=\begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \\ \mathbf{r_3}\end{bmatrix}=\begin{bmatrix}R &\mathbf{t}\end{bmatrix}\)为世界坐标系到相机坐标系的变换。和 \(\mathbf{x}=\begin{bmatrix}u\\v\\1\end{bmatrix}\)为归一化平面坐标，\(\lambda\)为深度值，有：
\[
\lambda \mathbf{x} = TX \\
\Rightarrow \lambda\mathbf{x}\times TX=0 \\
\Rightarrow \mathbf{\hat{x}}TX=0 \\
\tag{2-2}
\]
借助式(2-1)展开，有：
\[
\begin{align}
\mathbf{\hat{x}}TX &= \begin{bmatrix}0 &-1 &v \\ 1 &0 &-u \\ -v &u &0\end{bmatrix} \begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \\ \mathbf{r_3}\end{bmatrix}X\\
&=\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \\ -v\mathbf{r_1} + u\mathbf{r_2}\end{bmatrix}X\\
\tag{2-3}
\end{align}
\]
其中\(\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \\ -v\mathbf{r_1} + u\mathbf{r_2}\end{bmatrix}\)，第一行\(\times -u\),第二行\(\times -v\),再相加，即可得到第三行，因此，其线性相关，保留前两行即可，有
\[
\begin{bmatrix}-\mathbf{r_2}+v\mathbf{r_3} \\ \mathbf{r_1}-u\mathbf{r_3} \end{bmatrix}X=0
\tag{2-4}
\]
因此，已知一个归一化平面坐标\(\mathbf{x}\)和变换\(T_{cw}\),可以构建两个关于\(X\)的线性方程组。有两个以上的图像观测，即可求出\(X\)
\[
\begin{bmatrix}-\mathbf{r_2}^{(1)}+v^{(1)}\mathbf{r_3}^{(1)} \\ \mathbf{r_1}^{(1)}-u^{(1)}\mathbf{r_3}^{(1)} \\ -\mathbf{r_2}^{(2)}+v^{(2)}\mathbf{r_3}^{(2)} \\ \mathbf{r_1}^{(2)}-u^{(2)}\mathbf{r_3}^{(2)} \\ \vdots\end{bmatrix}X=0
\tag{2-5}
\]
上述方程没有非零解，使用SVD求最小二乘解，解可能不满足齐次坐标形式（第四个元素为1），因此，
\[
X = \begin{bmatrix}x\\y\\z\\1\end{bmatrix} = \begin{bmatrix}x_0/x_3\\x_1/x_3\\x_2/x_3\\x_3/x_3\end{bmatrix}
\tag{2-6}
\]
求得空间点坐标，但是这个解几何意义不明确[1],属于代数最小误差解。不等价于最小重投影误差，也不是最小化3D点距离误差。详细解释参考[1]中PPT。
在VINS-Mono中给出了归一化平面坐标\(\mathbf{x}=\begin{bmatrix}u\\v\\1\end{bmatrix}\)，如果只是给出像素坐标\(\mathbf{x'}=\begin{bmatrix}u'\\v'\\1\end{bmatrix}\)，并且已知相机内参\(K\)，求解3D点坐标方式类似。
\[
\begin{align}
&\space\lambda\mathbf{x}' = KTX \\
\Rightarrow &\space\lambda\mathbf{x}'\times KTX=0 \\
\Rightarrow &\space\mathbf{\hat{x}'}KTX=0 \\
\Rightarrow &\space\mathbf{\hat{x}'}PX=0 \\
\tag{3-1}
\end{align}
\]
其中，$\mathbf{\hat{x}'}PX=0 $的形式与式(2-3)的形式一致，同理可以得到式(2-4)的方程。
在多视几何的公式推导中，多次使用"共线向量叉乘等于0"的性质，将等式的某一项置为0，式(2-2)中应用了该性质，在本质矩阵推导中，也应用了该性质，如本质矩阵的前两步推导：
\[
\begin{align}
X_2 &= RX_1 + T \\
\hat{T}X_2 &= \hat{T}RX_1+\hat{T}T \\
\tag{4-1}
\end{align}
\]
在多视几何公式推导中，另一个常用的性质是"互相垂直的两个向量点乘为零"，在此不再展开。
[1] Lecture 7.2 Triangulation https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_7_2-triangulation.pdf