使用OpenCV进行双目标定的代码实现

//.cpp
//双目标定函数 输入为CAMERACALIB& cameraCalib在头文件中定义
void CCalibratnMethod::CameraCalibration(CAMERACALIB& cameraCalib)
{
    m_cameraCalib = cameraCalib;
    cvutMatrix::Matrix<double> Rl(3,3); //左边图像的旋转矩阵
    cvutMatrix::Matrix<double> Tl(3,1);//左边图像的平移矩阵
    cvutMatrix::Matrix<double> Rr(3,3); //右边图像的旋转矩阵
    cvutMatrix::Matrix<double> Tr(3,1);//右边图像的平移矩阵 
    cvutMatrix::Matrix<double> steroR(3,3);       
    cvutMatrix::Matrix<double> steroT(3,1);
    cvutMatrix::Matrix<double> intrinsic_matrixL(3,3);
    cvutMatrix::Matrix<double> distortion_coeffsL(4,1);
    cvutMatrix::Matrix<double> intrinsic_matrixR(3,3);
    cvutMatrix::Matrix<double> distortion_coeffsR(4,1);

    ifstream fsImgL(cameraCalib.m_sFilePathL); //左摄像头定标所用图像文件的路径
    ifstream fsImgR(cameraCalib.m_sFilePathR); 
    //左相机标定图像角点提取
    CornerDetection(fsImgL,m_imagePointSeqL);
    //右相机标定图像角点提取
    CornerDetection(fsImgR,m_imagePointSeqR);
    //左图角点转换
    CvSize square_size = cvSize((int)m_cameraCalib.m_dSquareSize,(int)m_cameraCalib.m_dSquareSize);
    cvutMatrix::Matrix<double> object_pointsL(bordSize.width*bordSize.height*sucesses,3);
    cvutMatrix::Matrix<double> image_pointsL(m_imagePointSeqL.cvseq->total,2);
    cvutMatrix::Matrix<int> point_countsL(sucesses,1);
    CornerConvert(m_imagePointSeqL,object_pointsL,image_pointsL,point_countsL);
    //右图角点转换
    cvutMatrix::Matrix<double> object_pointsR(bordSize.width*bordSize.height*sucesses,3);

    cvutMatrix::Matrix<double> image_pointsR(m_imagePointSeqL.cvseq->total,2);
    cvutMatrix::Matrix<int> point_countsR(sucesses,1);
    CornerConvert(m_imagePointSeqR,object_pointsR,image_pointsR,point_countsR);
    //左相机标定
    cvutMatrix::Matrix<double> rotation_vectorsL(sucesses,3);
    cvutMatrix::Matrix<double> translation_vectorsL(sucesses,3);
    cvCalibrateCamera2(object_pointsL.cvmat,//角点的三维坐标
                       image_pointsL.cvmat,//角点
                       point_countsL.cvmat,//每幅图像的角点数量
                       m_imgSize,//图像大小
                       intrinsic_matrixL.cvmat,
                       distortion_coeffsL.cvmat,
                       rotation_vectorsL.cvmat,
                       translation_vectorsL.cvmat,
                       0);
    //右相机标定
    cvutMatrix::Matrix<double> rotation_vectorsR(sucesses,3);
    cvutMatrix::Matrix<double> translation_vectorsR(sucesses,3);
    cvCalibrateCamera2(object_pointsR.cvmat,//角点的三维坐标
                       image_pointsR.cvmat,//角点
                       point_countsR.cvmat,//每幅图像的角点数量
                       m_imgSize,//图像大小
                       intrinsic_matrixR.cvmat,
                       distortion_coeffsR.cvmat,
                       rotation_vectorsR.cvmat,
                       translation_vectorsR.cvmat,
                       0);
    //双目标定
    cvutMatrix::Matrix<double> E(3,3);
    cvutMatrix::Matrix<double> F(3,3);