三角测量公式推导

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/YunLaowang/article/details/89640279

前言

三角测量是在已知相机参数和图像中匹配点的情况下，求解这些匹配点对应的空间点三维坐标的方法。
针对单目与双目系统，三角测量的使用方法有所不同。双目视觉测距原理参见：双目视觉测距原理深度剖析：一个被忽略的小问题，与双目相机相比，单目相机拍摄的两张图片没有固定的位姿，也就没有准确的基线，所以其三角化是指根据相机的运动来估计特征点的深度信息。

这里主要介绍直接线性变换法（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$即：
$\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$其中第三个方程可以由前两个进行线性变换得到，因此每个视角可以提供两个约束条件，联合第二个视角，可以得到: $AX=0$其中， $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}$
求解上述方程即可得出 $X$坐标，当视角点数较少且不存在外点时可通过对矩阵 $A$进行SVD分解来求解；当点数多于2个时，还可以采用最小二乘法进行求解；当存在外点（错误的匹配点），则采用RANSAC（随机一致性采样）的方法进行估计。
上述DLT求解中，是已知 $\overrightarrow {x_1}$和投影矩阵 $P$求解对应点的三维空间坐标 $X$；除此之外，DLT还可以用来求解PnP问题，只不过已知条件变成了 $\overrightarrow {x_1}$和 $X$，推导过程也与上述过程类似。

代码

#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
• 深蓝学院《基于图像的三维模型重建》