三角化公式推導&手撕代碼

前言

三角測量是在已知相機參數和圖像中匹配點的情況下，求解這些匹配點對應的空間點三維座標的方法。針對單目與雙目系統，三角測量的使用方法有所不同。雙目視覺測距原理參見：雙目視覺測距原理深度剖析：一個被忽略的小問題，與雙目相機相比，單目相機拍攝的兩張圖片沒有固定的位姿，也就沒有準確的基線，所以其三角化是指根據相機的運動來估計特徵點的深度信息。

這裏主要介紹直接線性變換法（Direct Linear Transform，DLT）。

直接線性變換法

設三維空間點$P$在世界座標系下的齊次座標爲$X=[x,y,z,1]^T$，相應的在兩個視角的投影點分別爲$p1$和$p2$，它們在相機座標系下的座標爲：
$\overrightarrow {x_1}=[x_1,y_1,1]^T,\overrightarrow {x_2}=[x_2,y_2,1]^T$

兩幅圖像對應的相機投影矩陣分別爲$P_1、P_2$（3*4維），
$P_1=[P_{11},P_{12},P_{13}]^T,P_2=[P_{21},P_{22},P_{23}]^T$其中，$P_{11}、P_{12}、P_{13}$分別是投影矩陣$P_1$的第1-3行；$P_{21}、P_{22}、P_{23}$分別是投影矩陣$P_2$的第1-3行；在理想情況下，有：
$\overrightarrow {x_1}=P_1X,\overrightarrow {x_2}=P_2X$對於$\overrightarrow {x_1}$，在其兩側分別叉乘其本身，可知：
$\overrightarrow {x_1}\times(P_1X)=0 \tag1$即：
$\begin{bmatrix} x_1 \\ y_1 \\ 1 \end{bmatrix}\times\begin{bmatrix} P_{11}X \\P_{12}X \\ P_{13}X \end{bmatrix}=\begin{bmatrix} 0 & -1 & y_1 \\ 1 & 0 & -x_1 \\ -y_1 & x_1& 0\end{bmatrix}\begin{bmatrix} P_{11}X \\P_{12}X \\ P_{13}X \end{bmatrix}=0$可以得到：
$\begin{bmatrix} -P_{12}X+y_1P_{13}X \\P_{11}X-x_1P_{13}X \\ x_1P_{12}X-y_1P_{11}X \end{bmatrix} =0 \tag2$

其中，第三個方程可以由前兩個進行線性變換得到，因此每個視角可以提供兩個約束條件，聯合第二個視角，可以得到:$AX=0\tag3$

其中，$A=\begin{bmatrix} x_1P_{13}- P_{11} \\ y_1P_{13}-P_{12} \\ x_2P_{23}- P_{21} \\ y_2P_{23}-P_{22} \end{bmatrix}$

針對上述方程，當視角點數較少且不存在外點時可直接對矩陣$A$進行SVD分解來求解，得出$X$座標；當存在外點（錯誤的匹配點），則採用RANSAC（隨機一致性採樣）的方法進行估計。

當點數多於2個時，還可以採用最小二乘法進行求解；由於每次觀測可以提供兩個約束條件，因此當存在$n$次觀測時，$A \in R^{2n\times4}$，使用最小二乘法求解$AX=0$，即是求：
$\min_{X}||AX||^2_2, \quad s.t.||X||=1 \tag4$

對$A^TA$進行SVD分解：
$A^TA=\sum_{i=1}^4\sigma^2_iu_iu_j^T \tag5$

其中，$\sigma_i$爲奇異值，且由大到小排列，$u_i、u_j$正交。
則可知，當$X=u_4$時，公式(4)將取得最小值，且最小值爲$\sigma_4^2$。

上述DLT求解中，是已知$\overrightarrow {x_1}$和投影矩陣$P$求解對應點的三維空間座標$X$；除此之外，DLT還可以用來求解PnP問題，只不過已知條件變成了$\overrightarrow {x_1}$和$X$，推導過程也與上述過程類似。

代碼

如下代碼是直接對矩陣$A$進行SVD分解的方式來求解。有關最小二乘法求解及代碼，可參考：《視覺SLAM十四講》ch13.3：三角化公式推導及代碼詳解

#include <iostream>
#include <math/matrix_svd.h>
#include "math/vector.h"
#include "math/matrix.h"

using namespace std;

int main(int argc, char* argv[])
{
	math::Vec2f p1;
	p1[0] = 0.289986; p1[1] = -0.0355493;
	math::Vec2f p2;
	p2[0] = 0.316154; p2[1] = 0.0898488;

	math::Matrix<double, 3, 4> P1, P2;
	P1(0, 0) = 0.919653;    P1(0, 1) = -0.000621866; P1(0, 2) = -0.00124006; P1(0, 3) = 0.00255933;
	P1(1, 0) = 0.000609954; P1(1, 1) = 0.919607; P1(1, 2) = -0.00957316; P1(1, 3) = 0.0540753;
	P1(2, 0) = 0.00135482;  P1(2, 1) = 0.0104087; P1(2, 2) = 0.999949;    P1(2, 3) = -0.127624;

	P2(0, 0) = 0.920039;    P2(0, 1) = -0.0117214;  P2(0, 2) = 0.0144298;   P2(0, 3) = 0.0749395;
	P2(1, 0) = 0.0118301;   P2(1, 1) = 0.920129;  P2(1, 2) = -0.00678373; P2(1, 3) = 0.862711;
	P2(2, 0) = -0.0155846;  P2(2, 1) = 0.00757181; P2(2, 2) = 0.999854;   P2(2, 3) = -0.0887441;

	/* 構造A矩陣 */
	math::Matrix<double, 4, 4> A;
	for (int i = 0; i<4; i++)
	{
		// p1
		A(0, i) = p1[0] * P1(2, i) - P1(0, i);
		A(1, i) = p1[1] * P1(2, i) - P1(1, i);

		// p2
		A(2, i) = p2[0] * P2(2, i) - P2(0, i);
		A(3, i) = p2[1] * P2(2, i) - P2(1, i);
	}

	// SVD分解
	math::Matrix<double, 4, 4> V;
	math::matrix_svd<double, 4, 4>(A, nullptr, nullptr, &V);
	math::Vec3f X;
	X[0] = V(0, 3) / V(3, 3);
	X[1] = V(1, 3) / V(3, 3);
	X[2] = V(2, 3) / V(3, 3);

	std::cout << " trianglede point is :" << X[0] << " " << X[1] << " " << X[2] << std::endl;
	std::cout << " the result should be " << "2.14598 -0.250569 6.92321\n" << std::endl;

	system("pause");
	return 0;
}

OpenCV也對三角測量進行了封裝（triangulatePoints()），原理大致相同，使用起來更加方便、簡潔。

// 歸一化，將像素座標轉換到相機座標(非齊次座標)
Point2d pixel2cam(const Point2d& p, const Mat& K)
{
	/* // 等價
	Mat x = (Mat_<double>(3, 1) << p.x, p.y, 1);
	x = K.inv()*x;

	return Point2d(
		x.at<double>(0,0),x.at<double>(1,0)
	);
	*/
	return Point2d(
		(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0), // 像素座標系->圖像座標系->相機座標系
		(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
	);
}
// 返回的是三維空間點
void triangulation(const vector<KeyPoint>& keypoint_1, const vector<KeyPoint>& keypoint_2, const vector<DMatch>& matches, const Mat& R, const Mat& t, vector<Point3d>& space_points)
{
	// T1、T2爲外參矩陣，視第一個相機座標系爲世界座標系，則其R = I, T = 0
	Mat T1 = (Mat_<double>(3, 4) <<
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0
		);

	Mat T2 = (Mat_<double>(3, 4) <<
		R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
		);

	// 定義相機的內參矩陣
	Mat K = (Mat_<double>(3, 3) <<
		529.0, 0, 325.1,
		0, 521.0, 249.9,
		0, 0, 1
		);

	// 將所有匹配的特徵點轉化爲歸一化座標
	vector<Point2d> pts_1, pts_2;

	for (DMatch m : matches)
	{
		// 將像素座標轉換至相機座標
		pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
		pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
	}

	Mat pts_4d;
	cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

	// 轉換爲非齊次座標
	for (int i = 0; i < pts_4d.cols; i++)
	{
		Mat x = pts_4d.col(i);  // 第i個三維點對應的齊次座標
		x /= x.at<double>(3, 0);   
		space_points.push_back(Point3d(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0)));
	}
}

參考

• 《視覺SLAM14講》-chapter7
• 深藍學院《基於圖像的三維模型重建》