三维重建之三角测量计算双目相机坐标系下三维坐标

三角测量以计算 3D 坐标

注意：三角测量必须在双目相机立体匹配后计算，对于核校正后的图像
对极校正请参考我上一篇博文：相机成像+单应变换+相机标定+立体校正[0]

下图为立体校正后的成像平面布局：

1.视差测距法：

根据三角形相似原理得出深度z

相机坐标、世界坐标、图像坐标的变换关系：

算法实现的具体原理：
极线校正（立体校正）后获得相机在新坐标系下的投影矩阵Q

stereoRectify(camera.cameraMatrix1, camera.distCoeffts1, 
camera.cameraMatrix2, camera.distCoeffts2, camera.size_,
		camera.R, camera.T,R1, R2, P1, P2, Q);

如上，可以实现双目相机得到的左右双图像推导3D坐标点的算法：
参考：opencv双目视觉特征匹配点三角测量得到三维坐标[0]

void StereoTo3D(const::vector<Point2f> ptsL, const::vector<Point2f> ptsR, Mat Q, Mat T, vector<Point3f>& pts3D)
{
	//1三角测量
	if (ptsL.size() != ptsR.size())
	{
		cout << "匹配点数量出现错误" << endl;
		return;
	}
	float cx = -Q.at<double>(0, 3);
	float cy = -Q.at<double>(1, 3);
	float f = Q.at<double>(2, 3);

	float B = sqrt(T.at<double>(0, 0) * T.at<double>(0, 0) + T.at<double>(1, 0) * T.at<double>(1, 0) + T.at<double>(2, 0) * T.at<double>(2, 0));
	pts3D.resize(ptsL.size());
	for (int i = 0; i < ptsL.size(); i++)
	{
		pts3D[i].x = (ptsL[i].x - cx) * B / (ptsL[i].x - ptsR[i].x);
		pts3D[i].y = (ptsL[i].y - cy) * B / (ptsL[i].x - ptsR[i].x);
		pts3D[i].z = f * B / (ptsL[i].x - ptsR[i].x);
	}
	assert(!diffs.empty());
}

2.最小二乘求解

三角测量（triangulation), 就是在不同的姿态下观察同一个对象，根据图像上的同名点的像素坐标，计算对象的三维坐标。

在双目视觉中，左右摄像头是分开的，可以同时获取左右视图。根据同名匹配点和左右摄像机的投影矩阵，可以计算出同名匹配点的世界坐标。

参考：三角测量计算点的 3D 坐标


[0]

/**
 * @param T1 相机O1的投影矩阵
 * @param T2 相机O2的投影矩阵
 * @param p1 相机O1的图像的像素点
 * @param p2 相机O2的图像的像素点
 * @param out 输出的三维坐标
 */
void triangulatePoint(Matrix<double, 3, 4>& T1, Matrix<double, 3, 4>& T2, Vector2d& p1, Vector2d&p2, Vector3d& out){
    // 构建Ax = 0;
    Matrix<double, 4, 4> A;
    A.row(0) = p1(0)*T1.row(2) - T1.row(0);
    A.row(1) = p1(1)*T1.row(2) - T1.row(1);
    A.row(2) = p2(0)*T2.row(2) - T2.row(0);
    A.row(3) = p2(1)*T2.row(2) - T2.row(1);

    // SVD 分解
    Eigen::JacobiSVD<MatrixXd> solver(A, Eigen::ComputeFullU|Eigen::ComputeThinV);
    MatrixXd V = solver.matrixV();
    VectorXd X = V.rightCols(1);

    out[0] = X(0)/X(3);
    out[1] = X(1)/X(3);
    out[2] = X(2)/X(3);
}

OpenCV库，也已经实现了cv::triangulatePoints，其中，P1,P2为极线校正后的相机的投影矩阵

void triangulateByOpenCV(Matrix<double, 3, 4>& T1, Matrix<double, 3, 4>& T2, Vector2d& p1, Vector2d&p2, Vector3d& out){

    Mat T_mat1, T_mat2;
    cv::eigen2cv(T1, T_mat1);
    cv::eigen2cv(T2, T_mat2);

    std::vector<Point2d> p1s,p2s;
    p1s.push_back(Point2d(p1.x(), p1.y()));
    p2s.push_back(Point2d(p2.x(), p2.y()));
    Mat pts_4d;
    cv::triangulatePoints(T_mat1, T_mat2, p1s, p2s, pts_4d);

    Mat X = pts_4d.col(0);
    X /=X.at<double>(3,0);

    out[0] = X.at<double>(0, 0);
    out[1] = X.at<double>(1, 0);
    out[2] = X.at<double>(2, 0);
}