VINS-mono 三角化求解3D空间点座标

1、数学推导

1.1 向量叉乘

• 在数学和向量代数领域，叉积（英语：Cross product）又称向量积（英语：Vector product），是对三维空间中的两个向量的二元运算，使用符号 ${\displaystyle \times }$。与点积不同，它的运算结果是向量。对于线性无关的两个向量 ${\displaystyle \mathbf {a} }$和 ${\displaystyle \mathbf {b} }$，它们的叉积写作 ${\displaystyle \mathbf {a} \times \mathbf {b} }$，是 ${\displaystyle \mathbf {a} }$和 ${\displaystyle \mathbf {b} }$所在平面的法线向量，与 ${\displaystyle \mathbf {a} }$和 ${\displaystyle \mathbf {b} }$都垂直。叉积被广泛运用于数学、物理、工程学、计算机科学领域。
• 叉积和点积一样依赖于欧几里德空间的度量，但与点积之不同的是，叉积还依赖于定向或右手定则。
• 两个向量 ${\displaystyle \mathbf {a} }$和 ${\displaystyle \mathbf {b} }$的叉积仅在三维空间中有定义，写作 ${\displaystyle \mathbf {a} \times \mathbf {b} }$。在物理学中，叉积有时也被写成 ${\displaystyle \mathbf {a}^ \wedge \mathbf {b} }$，但在数学中${\displaystyle \mathbf {a}^ \wedge \mathbf {b} }$是外代数中的外积。
  
• 叉积 ${\displaystyle \mathbf {a} \times \mathbf {b} }$是与 ${\displaystyle \mathbf {a} }$和${\displaystyle \mathbf {b} }$都垂直的向量 ${\displaystyle \mathbf {c} }$。其方向由右手定则决定，模长等于以两个向量为边的平行四边形的面积。
• 叉积可以定义为：
  ${\displaystyle \mathbf {a} \times \mathbf {b} =\|\mathbf {a} \|\|\mathbf {b} \|\sin(\theta )\ \mathbf {n} }$
  其中 ${\displaystyle \theta }$表示 ${\displaystyle \mathbf {a} }$和 ${\displaystyle \mathbf {b} }$在它们所定义的平面上的夹角（ ${\displaystyle 0^{\circ }\leq \theta \leq 180^{\circ }}$）

三维座标相乘：
${\displaystyle {\begin{aligned}\mathbf {a} \times \mathbf {b} &=(a_{2}b_{3}-a_{3}b_{2})\mathbf {i} +(a_{3}b_{1}-a_{1}b_{3})\mathbf {j} +(a_{1}b_{2}-a_{2}b_{1})\mathbf {k} \\&={\begin{vmatrix}\mathbf {i} &\mathbf {j} &\mathbf {k} \\a_{1}&a_{2}&a_{3}\\b_{1}&b_{2}&b_{3}\\\end{vmatrix}}\end{aligned}}}$

叉积表示：
$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}$

求导:
${\displaystyle {\frac {d}{dt}}(\mathbf {a} \times \mathbf {b} )={\frac {d\mathbf {a} }{dt}}\times \mathbf {b} +\mathbf {a} \times {\frac {d\mathbf {b} }{dt}}}$

1.2 数学推算

• 由上推算可知，叉积表示：
  $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}$ （1-1）
  其中$\hat{a}$为向量$a$的反对称矩阵
• 对于，𝑋为三维空间点在世界座标系下的齐次座标$X =\begin{bmatrix} x\\ y\\ z\\ 1\end{bmatrix}$，和${t=\begin{bmatrix} r_1\\r_2 \\r_3 \end{bmatrix}}=\begin{bmatrix} R & t \end{bmatrix}$为世界座标系到相机座标系的变换。和 $x = \begin{bmatrix}u\\ v\\ 1\end{bmatrix}$为归一化平面座标，${\lambda}$为深度值，有：
  $\lambda x= TX$ => $\lambda x \times TX =0$ => $\hat{x}TX=0$ （1-2）
• 将（1-1）带入（1-2）展开有：
  $\hat{x}TX= \begin{bmatrix} 0 & -1 & v\\ 1 & 0 & -u\\ -v & u &0 \end{bmatrix}\begin{bmatrix} r_1\\ r_2\\ r_3 \end{bmatrix}X=\begin{bmatrix} -r_2 +vr_3\\ r_1-ur_3\\ -vr_1+ur_2 \end{bmatrix}X$ （1-3）
• 其中$\begin{bmatrix} -r_2 +vr_3\\ r_1-ur_3\\ -vr_1+ur_2 \end{bmatrix}$，第一行 叉乘(-u)，第二行叉乘(-v)，二者相加，可得到第三行，因此，其线性相关，保留前两行即可，有：
  $\begin{bmatrix}-r_2 +vr_3\\ r_1-ur_3 \end{bmatrix}X=0$
• 因此，已知一个归一化平面座标$x$和变化$T_{cw}$，可以构建两个关于X的线性方程组，有两个以上的图像观测，即可求出X:
  $\begin{bmatrix}-r_2^{(1)} +vr_3^{(1)}\\ r_1^{(1)}-ur_3^{(1)} \\ -r_2^{(2)} +vr_3^{(2)}\\ r_1^{(2)}-ur_3^{(2)}\\ \vdots\end{bmatrix}X=0$
• 上述方程没有非零解，使用SVD求最小二乘解，解可能不满足齐次座标形式（第四个元素为1）， 齐次座标 X 即为H的最小奇异值的奇异向量。(SVD好重要–直接线性变换DLT都用到它求解)
  因此，$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}$
• 求得空间点座标，但是这个解几何意义不明确[1],属于代数最小误差解。不等价于最小重投影误差，也不是最小化3D点距离误差。
• 在VINS-Mono中给出了归一化平面座标$x = \begin{bmatrix}u\\ v\\ 1\end{bmatrix}$，如果只是给出像素座标$x^{'} = \begin{bmatrix}u^{'} \\ v^{'} \\ 1\end{bmatrix}$，并且已知相机内参𝐾，求解3D点座标方式类似。
  $\lambda x{'}= KTX$ => $\lambda x{'} \times KTX =0$ => $\hat{x}{'} KTX=0$ => $\hat{x}{'} PX=0$

2、代码实现

2.1 VINS-Mono 三角化

//三角化两帧间某个对应特征点的深度
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	Matrix4d design_matrix = 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);
	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.2 orb-slam 三角化

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    cv::Mat A(4,4,CV_32F);
 
    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
 
    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    cout<<"vt.row()"<<vt.rows<<std::endl;
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);///
}

2.3 Triangulate

void triangulate ( const Eigen::Matrix3d& K, 
				   const Eigen::Matrix4d T1, const Eigen::Matrix4d& T2, 
				   const Eigen::Vector2d& uu1, const Eigen::Vector2d& uu2, 
				   Eigen::Vector4d& X )
{
	// construct P1 P2
	const Eigen::Matrix<double, 3, 4> P1 = K * T1.block(0,0, 3, 4);
	const Eigen::Matrix<double, 3, 4> P2 = K * T2.block(0, 0, 3, 4);
	
	// get vectors
	const Eigen::Matrix<double, 1, 4>& P11 = P1.block(0, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P12 = P1.block(1, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P13 = P1.block(2, 0, 1, 4);
	
	const Eigen::Matrix<double, 1, 4>& P21 = P2.block(0, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P22 = P2.block(1, 0, 1, 4);
	const Eigen::Matrix<double, 1, 4>& P23 = P2.block(2, 0, 1, 4);
	
	const double& u1 = uu1[0];
	const double& v1 = uu1[1];
	const double& u2 = uu2[0];
	const double& v2 = uu2[1];
	
	// construct H matrix.
	Eigen::Matrix4d H;
	H.block(0, 0, 1, 4) = v1 * P13 - P12;
	H.block(1, 0, 1, 4) = P11 - u1 * P13;
	H.block(2, 0, 1, 4) = v2 * P23 - P22;
	H.block(3, 0, 1, 4) = P21 - u2 * P23;
	
	// SVD
	Eigen::JacobiSVD<Eigen::MatrixXd> svd ( H, Eigen::ComputeFullU | Eigen::ComputeFullV );
	Eigen::Matrix4d V = svd.matrixV();
	
	X = V.block(0, 3, 4, 1);
	X = X / X(3, 0);
} // triangulate