read stereo para

Mat cameraMatrixL = (Mat_(3, 3) << 94.757998, 0.000000, 282.926747,
                                          0.000000, 903.292721, 264.512904,
                                            0, 0, 1);

Mat distCoeffL = (Mat_(5, 1) <<   -0.088038, 0.262578, 0.014441, -0.017534,0);

Mat cameraMatrixR = (Mat_(3, 3) << 949.817085, 0.000000, 217.826100,
                                            0.000000, 889.933032, 257.097059,
                                            0, 0, 1);

Mat distCoeffR = (Mat_(5, 1) << -0.091178, 0.411535, 0.016533, -0.021789,0);

Mat T = (Mat_(3, 1) << -4.725344, 0.72337, 3.068972);
Mat rec = (Mat_(3, 1) << -0.00306, -0.03207, 0.00206);

Mat recEx = (Mat_(3, 3) << 0.999446, -0.002541, 0.033172,
                                            0.002462, 0.999994, 0.002420,
                                            -0.033178, -0.002337, 0.999447);//ll add
Mat R;//R 旋转矩阵

 

int parseStereoData(char *pImuString,float *dataout,int *pnum) 
{
    int len = strlen(pImuString);
    //printf("pImuString size:%d\n",len);
    
    int i;
    char timeTmp[30];
    for(i = 0;i         if(pImuString[i] == ' ') {    
            timeTmp[i] = '\0';
            break;
        }else {
            timeTmp[i] = pImuString[i];
        }
    }
    
    dataout[0] = atof(timeTmp);
//    printf("parseImuData time:%s,time:%ld\n",timeTmp,*time);
    
    char accTmp[20];
    int j = 0;
    
    //aac 3
    i++;
    for(;i         if(pImuString[i] == ' ') {    
            accTmp[j] = '\0';
            break;
        } else {
            accTmp[j] = pImuString[i];
            j++;
        }
    }
    
    dataout[1] = atof(accTmp);
    //printf("parseImuData accTmp:%s,acc[0]:%f\n",accTmp,acc[0]);
    
    
    j = 0;
    i++;
    for(;i         if(pImuString[i] == ' ' || pImuString[i] == '\n') {    
            accTmp[j] = '\0';
            break;
        } else {
            accTmp[j] = pImuString[i];
            j++;
        }
    }
    
    
    dataout[2] = atof(accTmp);
    //printf("parseImuData accTmp2:%s,acc[1]:%f\n",accTmp,acc[1]);
    
    if(pImuString[i] == '\n') {
        return 0;
    }

    j = 0;
    i++;
    for(;i         if(pImuString[i] == ' ' || pImuString[i] == '\n') {    
            accTmp[j] = '\0';
            break;
        } else {
            accTmp[j] = pImuString[i];
            j++;
        }
    }
    
    dataout[3] = atof(accTmp);
    //printf("parseImuData accTmp3:%s,acc[2]:%f\n",accTmp,acc[2]);
    
    //agnleTmp[j] = '\0';
    
    //angle[2]= atof(agnleTmp);

    
    return 0;
}
 

void read()

{

    FILE* gOneRead_fm = 0;   
    gOneRead_fm = fopen( "StereoCamera_0_Int.txt", "r+" );

    char line[1024];
    
    int iPicNum = 0;
    char *iNum = 0;

    float dcameraMatrixL[9];
    float ddistCoeffL[4];

    while(1)
    {
        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }
        
        float dataout[4];
        
        int pnum;
        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixL[0] = dataout[0];
        dcameraMatrixL[1] = dataout[1];
        dcameraMatrixL[2] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixL[3] = dataout[0];
        dcameraMatrixL[4] = dataout[1];
        dcameraMatrixL[5] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixL[6] = dataout[0];
        dcameraMatrixL[7] = dataout[1];
        dcameraMatrixL[8] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        ddistCoeffL[0] = dataout[0];
        ddistCoeffL[1] = dataout[1];
        ddistCoeffL[2] = dataout[2];
        ddistCoeffL[3] = dataout[3];

        break;
    }

    if(gOneRead_fm) {
        fclose( gOneRead_fm );
        gOneRead_fm = 0;
    }

    gOneRead_fm = fopen( "StereoCamera_1_Int.txt", "r+" );//fm 

    float dcameraMatrixR[9];
    float ddistCoeffR[4];

    float dr[9];
    float dt[3];
    
    int wread = 0;
    int hread = 0;

    while(1)
    {
        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }
        
        float dataout[4];
        
        int pnum;
        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixR[0] = dataout[0];
        dcameraMatrixR[1] = dataout[1];
        dcameraMatrixR[2] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixR[3] = dataout[0];
        dcameraMatrixR[4] = dataout[1];
        dcameraMatrixR[5] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dcameraMatrixR[6] = dataout[0];
        dcameraMatrixR[7] = dataout[1];
        dcameraMatrixR[8] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        ddistCoeffR[0] = dataout[0];
        ddistCoeffR[1] = dataout[1];
        ddistCoeffR[2] = dataout[2];
        ddistCoeffR[3] = dataout[3];

        //R,T
        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dr[0] = dataout[0];
        dr[1] = dataout[1];
        dr[2] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dr[3] = dataout[0];
        dr[4] = dataout[1];
        dr[5] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dr[6] = dataout[0];
        dr[7] = dataout[1];
        dr[8] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        dt[0] = dataout[0];
        dt[1] = dataout[1];
        dt[2] = dataout[2];

        iNum = fgets(line, 128, gOneRead_fm);
        if(0 == iNum) {
            break;
        }

        parseImuData((char*)line,(float*)dataout,&pnum); 
        wread = (int)(dataout[0]);
        hread = (int)(dataout[1]);

        break;
    }

    if(gOneRead_fm) {
        fclose( gOneRead_fm );
        gOneRead_fm = 0;
    }

    if(1)
    {
        imageWidth = wread;                      
        imageHeight = hread;

        imageSize.width = imageWidth;
        imageSize.height = imageHeight;

       double *data = (double*)(cameraMatrixL.data); 
       for(int i=0;i<9;i++){
          data[i] = dcameraMatrixL[i];
       }

       data = (double*)(distCoeffL.data); 
       for(int i=0;i<4;i++){
          data[i] = ddistCoeffL[i];
       }

       data[4] = 0;
   
       data = (double*)cameraMatrixR.data; 
       for(int i=0;i<9;i++){
          data[i] = dcameraMatrixR[i];
       }
   
       data = (double*)distCoeffR.data; 
       for(int i=0;i<4;i++){
          data[i] = ddistCoeffR[i];
       }
       data[4] = 0;

       data = (double*)T.data; 
       for(int i=0;i<3;i++){
          data[i] = dt[i];
       }
   
       data = (double*)recEx.data; 
       for(int i=0;i<9;i++){
          data[i] = dr[i];
       }
    }

 

}