stereo map test

1.

generate map.
int  generate()
{
    {
        double *testT = (double*)T.data;

        double *testrecEx = (double*)recEx.data;

        double *testcameraMatrixL = (double*)cameraMatrixL.data;
        double *testcameraMatrixR = (double*)cameraMatrixR.data;

        double *testdistCoeffL = (double*)distCoeffL.data;
        double *testdistCoeffR = (double*)distCoeffR.data;

        testT = testT;
    }

    double r1[9];
    double r2[9];

    double p1[12];
    double p2[12];

    CvMat _R1 = cvMat(3, 3, CV_64F, r1);
    CvMat _R2 = cvMat(3, 3, CV_64F, r2);
    CvMat _P1 = cvMat(3, 4, CV_64F, p1);
    CvMat _P2 = cvMat(3, 4, CV_64F, p2);

    double q[16];

    CvMat matQ= cvMat(4, 4, CV_64F, q);

    CvSize newImgSize = imageSize;
    CvRect roi1;
    CvRect roi2;
    double alpha = 0;//-1;//0;

    CvMat _cameraMatrix1 = cameraMatrixL;
    CvMat _cameraMatrix2 = cameraMatrixR;

    CvMat _distCoeffs1 = distCoeffL;
    CvMat _distCoeffs2 = distCoeffR;

    CvSize imageSize2 = imageSize;
    CvMat matR = recEx;
    CvMat matT = T;
    int flags = CALIB_ZERO_DISPARITY;//1;//0;// -1;//CALIB_ZERO_DISPARITY;  //        -1&1024    1024    int

//    void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
  //                    const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
    //                  CvSize imageSize, const CvMat* matR, const CvMat* matT,
      //                CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
        //              CvMat* matQ, int flags, double alpha, CvSize newImgSize,
          //            CvRect* roi1, CvRect* roi2 )
    cvStereoRectify( &_cameraMatrix1, &_cameraMatrix2,
                      &_distCoeffs1,  &_distCoeffs2,
                     imageSize2, &matR,  &matT,
                      &_R1,  &_R2,  &_P1,  &_P2,
                      &matQ, flags,  alpha, newImgSize,
                      &roi1, &roi2 );
    
    

    //CV_CALIB_ZERO_DISPARITY  CALIB_ZERO_DISPARITY
    //stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, recEx, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        //0, imageSize, &validROIL, &validROIR);

    //stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, recEx, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        //-1, imageSize, &validROIL, &validROIR);

    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, recEx, T, Rl, Rr, Pl, Pr, Q, CV_CALIB_ZERO_DISPARITY,
        0, imageSize, &validROIL, &validROIR);
    
    //stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, recEx, T, Rl, Rr, Pl, Pr, Q, CV_CALIB_ZERO_DISPARITY,
        //1, imageSize, &validROIL, &validROIR);

    //stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, recEx, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        //1, imageSize, &validROIL, &validROIR);

    {//test
        double *testp1 = (double*)Pl.data;
        double *testp2 = (double*)Pr.data;

        double *testRl = (double*)Rl.data;
        double *testRr = (double*)Rr.data;

        CvMat cvQ = Q;
        double f_tx = -1.0 / (cvQ.data.db[3*4+2]);
        double f_f = cvQ.data.db[2*4+3];
        double f_cx_cx1 = - cvQ.data.db[3*4+3] / (cvQ.data.db[3*4+2]);
    }

    memcpy((char*)Pl.data,(char*)p1,sizeof(double)*12);
    memcpy((char*)Pr.data,(char*)p2,sizeof(double)*12);

    memcpy((char*)Rl.data,(char*)r1,sizeof(double)*9);
    memcpy((char*)Rr.data,(char*)r2,sizeof(double)*9);

    memcpy((char*)Q.data,(char*)q,sizeof(double)*16);

    double *testp1 = (double*)Pl.data;
    double *testp2 = (double*)Pr.data;

    double *testRl = (double*)Rl.data;
    double *testRr = (double*)Rr.data;

    double *testq = (double*)Q.data;

    //initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);

    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);//171030
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

    //initUndistortRectifyMap2(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);//171030
    //initUndistortRectifyMap2(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

    //initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);//171030
    //initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

    //float *datamapx  = (float*)mapLx.data;
    float *datamapy  = (float*)mapLy.data;
 

}

 

2.

map image.
int checkStereo()
{
    Mat imgsrc = imread("0.jpg",1);
    int width = imgsrc.cols;
    int height = imgsrc.rows;

    if(width <= 0) {
        return 1;
    }

    unsigned char *pLeftU = 0;
    unsigned char *pRightU=0;

    unsigned char *bufRgb = 0;
    bufRgb=(unsigned char *)malloc(3000 * 480 * 3);

    bufRgb = (unsigned char *)imgsrc.data;

    if(bufRgb ==0) {
        return 1;
    }

    ReadBufferBMP(bufRgb, width, height,&pLeftU,&pRightU);

    Mat imgLeftGray(height, width/2, CV_8UC3, pLeftU, 0); 
    Mat imgRightGray(height, width/2, CV_8UC3, pRightU, 0); 

    remap(imgLeftGray, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
    remap(imgRightGray, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    {
        char buf[120];
        sprintf(buf, "leftimage%d.bmp",0);
    
        cv::imwrite(buf, rectifyImageL);

        sprintf(buf, "rightimage%d.bmp",0);
    
        cv::imwrite(buf, rectifyImageR);

        sprintf(buf, "srcleftimage%d.bmp",0);
    
        cv::imwrite(buf, imgLeftGray);

        sprintf(buf, "srcrightimage%d.bmp",0);
    
        cv::imwrite(buf, imgRightGray);
    }

    
    if(iCheckSoft) {
        rectifyImageL = imread("stereoleft.bmp",1);//E:\armtest
        rectifyImageR = imread("stereoright.bmp",1);//E:\armtest
    }

    unsigned char *leftdata = (unsigned char *)rectifyImageL.data;
    unsigned char *rightdata = (unsigned char *)rectifyImageR.data;

    unsigned char *mapcom = 0;
    mapcom =(unsigned char *)malloc(3000 * 480 * 3);
    for(int i = 0;i< 480;i++) {
        for(int j = 0;j< 640;j ++) {
            mapcom[width*3*i + j*3] = leftdata[(int)(imgLeftGray.step*i) + j*3];
            mapcom[width*3*i + j*3+1] = leftdata[(int)(imgLeftGray.step*i) + j*3+1];
            mapcom[width*3*i + j*3+2] = leftdata[(int)(imgLeftGray.step*i) + j*3+2];

            mapcom[width*3*i + (j+640)*3] = rightdata[(int)(imgLeftGray.step*i) + j*3];
            mapcom[width*3*i + (j+640)*3+1] = rightdata[(int)(imgLeftGray.step*i) + j*3+1];
            mapcom[width*3*i + (j+640)*3+2] = rightdata[(int)(imgLeftGray.step*i) + j*3+2];
        }
    }

    Mat imgGrayCom(height, width, CV_8UC3, mapcom, 0); 
}