[转]OpenCV三角测量重建triangulatePoints原理解析

opencv源代码注释
附上opencv三角测量函数的主要代码和注释
cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D) { // preallocate SVD matrices on stack cv::Matx<double, 4, 4> matrA; cv::Matx<double, 4, 4> matrU; cv::Matx<double, 4, 1> matrW; cv::Matx<double, 4, 4> matrV; CvMat* projPoints[2] = {projPoints1, projPoints2}; CvMat* projMatrs[2] = {projMatr1, projMatr2}; /* Solve system for each point */ for( int i = 0; i < numPoints; i++ )/* For each point */ { /* Fill matrix for current point */ for( int j = 0; j < 2; j++ )/* For each view */ { double x,y; x = cvmGet(projPoints[j],0,i); y = cvmGet(projPoints[j],1,i); // 注1:构造系数矩阵A for( int k = 0; k < 4; k++ ) { matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k); matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k); } } // 注2:SVD分解A矩阵 /* Solve system for current point */ cv::SVD::compute(matrA, matrW, matrU, matrV); /* Copy computed point */ cvmSet(points4D,0,i,matrV(3,0));/* X */ cvmSet(points4D,1,i,matrV(3,1));/* Y */ cvmSet(points4D,2,i,matrV(3,2));/* Z */ cvmSet(points4D,3,i,matrV(3,3));/* W */ } }