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;
//}
}
将双目标定校正后的参数导入即可。测试可行