opencv双目视觉特征匹配点三角测量得到三维坐标_opencv 三角测量 csdn

void StereoTo3D(vector<Point2f> ptsL, vector<Point2f> ptsR, vector<Point3f> &pts3D, Mat Q,Mat T)
{
	//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);
	}
 
	//2基于cvPerspectiveTransform函数
	//CvMat *src = cvCreateMat(1, 1, CV_32FC3);//待变换点  
	//CvMat *dst = cvCreateMat(1, 1, CV_32FC3);//变换后点  
	//CvMat *perMat = cvCreateMat(4, 4, CV_32FC1);//透视投影变换矩阵  
	//*perMat = Q;
	//pts3D.resize(ptsL.size());
	//for (int i = 0; i < ptsL.size(); i++)
	//{
	//	cvZero(src);
	//	cvZero(dst);
	//	for (int j = 0; j < 3; j++)
	//	{
	//		float *p = (float*)cvPtr2D(src, 0, 0);
	//		*p++ = ptsL[i].x;
	//		*p++ = ptsL[i].y;
	//		*p = ptsL[i].x - ptsR[i].x;
	//	}
	//	cvPerspectiveTransform(src, dst, perMat);
	//	float *pp = (float*)cvPtr2D(dst, 0, 0);
	//	pts3D[i].x = *pp++;
	//	pts3D[i].y = *pp++;
	//	pts3D[i].z = *pp;
	//}
}

将双目标定校正后的参数导入即可。测试可行