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