Mat cameraMatrixL = (Mat_<double>(3, 3) << 94.757998, 0.000000, 282.926747,
0.000000, 903.292721, 264.512904,
0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << -0.088038, 0.262578, 0.014441, -0.017534,0);
Mat cameraMatrixR = (Mat_<double>(3, 3) << 949.817085, 0.000000, 217.826100,
0.000000, 889.933032, 257.097059,
0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.091178, 0.411535, 0.016533, -0.021789,0);
Mat T = (Mat_<double>(3, 1) << -4.725344, 0.72337, 3.068972);
Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);
Mat recEx = (Mat_<double>(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<len;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<len;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<len;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<len;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];
}
}
}