这里写自定义目录标题
基于LM的双目图像校准算法源码
代码中调用的部分函数已作为资源上传，这个代码仅供咱们学习讨论使用，如果需要工程使用，需要进一步优化。这是多年前写得代码，不足之处请批评指正。
float rotate[9];
float distTwoPoint(vect2f_t a,vect2f_t b)
{
return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}
int mvGetPointIndex(vect2f_t *src,int nPoint,int *index,float stepx,float stepy,int isLimit)
{
float thet0 = PI/2,thet1 = -PI/2;
float min = 4000;
float initDist,dist;
vect2f_t nearpoint0,nearpoint1;
int i;
while(1)
{
index[0] = rand()%nPoint;
index[1] = rand()%nPoint;
if(fabs(src[index[0]].x) > (stepx * HOMOGRAPHY_RANGE_POINT_NUM) || fabs(src[index[0]].y) > (stepy * HOMOGRAPHY_RANGE_POINT_NUM)
|| fabs(src[index[1]].x) > (stepx * HOMOGRAPHY_RANGE_POINT_NUM) || fabs(src[index[1]].y) > (stepy * HOMOGRAPHY_RANGE_POINT_NUM))
{
if(isLimit)
continue;
}
initDist = distTwoPoint(src[index[0]],src[index[1]]);
if(initDist > 10.0f && initDist < 70.0f)
break;
}
//index[0] = 0;
//index[1] = 2;
nearpoint0.x = -(src[index[1]].y - src[index[0]].y)*sin(thet0) + src[index[0]].x;
nearpoint0.y = (src[index[1]].x - src[index[0]].x)*sin(thet0) + src[index[0]].y;
nearpoint1.x = -(src[index[1]].y - src[index[0]].y)*sin(thet1) + src[index[0]].x;
nearpoint1.y = (src[index[1]].x - src[index[0]].x)*sin(thet1) + src[index[0]].y;
for( i = 0; i < nPoint; ++i )
{
if(i == index[0] || i == index[1])
continue;
dist = distTwoPoint(src[i],nearpoint0);
if(min > dist)
{
index[2] = i;
min = dist;
}
dist = distTwoPoint(src[i],nearpoint1);
if(min > dist)
{
index[2] = i;
min = dist;
}
}
nearpoint0.x = -(src[index[0]].y - src[index[2]].y)*sin(thet0) + src[index[2]].x;
nearpoint0.y = (src[index[0]].x - src[index[2]].x)*sin(thet0) + src[index[2]].y;
nearpoint1.x = -(src[index[0]].y - src[index[2]].y)*sin(thet1) + src[index[2]].x;
nearpoint1.y = (src[index[0]].x - src[index[2]].x)*sin(thet1) + src[index[2]].y;
min = 4000;
for( i = 0; i < nPoint; ++i )
{
if(i == index[0] || i == index[1]|| i == index[2])
continue;
dist = distTwoPoint(src[i],nearpoint0);
if(min > dist)
{
index[3] = i;
min = dist;
}
dist = distTwoPoint(src[i],nearpoint1);
if(min > dist)
{
index[3] = i;
min = dist;
}
}
dist = distTwoPoint(src[index[0]],src[index[1]]);
//printf("%f ",dist);
if(dist < 10.0f || dist > 110.0f)
return 0;
dist = distTwoPoint(src[index[1]],src[index[2]]);
//printf("%f ",dist);
if(dist < 1.0f|| dist > 110.0f)
return 0;
dist = distTwoPoint(src[index[2]],src[index[3]]);
//printf("%f ",dist);
if(dist < 1.0f|| dist > 110.0f)
return 0;
dist = distTwoPoint(src[index[0]],src[index[2]]);
//printf("%f ",dist);
if(dist < 1.0f || dist > 110.0f)
return 0;
dist = distTwoPoint(src[index[3]],src[index[1]]);
//printf("%f ",dist);
if(dist < 1.0f || dist > 110.0f)
return 0;
dist = distTwoPoint(src[index[0]],src[index[3]]);
//printf("%f\n",dist);
if(dist < 1.0f || dist > 110.0f)
return 0;
return 1;
}
float getErrHomography(vect2f_t *src,vect2f_t *dst,float *R,int nPoint,float stepx,float stepy)
{
int j;
float errSum = 0.0f;
float x1,y1;
int count = 0;
for(j = 0;j < nPoint;j++)
{
if(fabs(src[j].x) > (stepx * HOMOGRAPHY_RANGE_POINT_NUM) || fabs(src[j].y) > (stepy * HOMOGRAPHY_RANGE_POINT_NUM))
{
continue;
}
x1 = (R[0] * src[j].x + R[1] * src[j].y + R[2])/(R[6] * src[j].x + R[7] * src[j].y + R[8]);
y1 = (R[3] * src[j].x + R[4] * src[j].y + R[5])/(R[6] * src[j].x + R[7] * src[j].y + R[8]);
errSum += fabs(x1 - dst[j].x);
errSum += fabs(y1 - dst[j].y);
count++;
}
errSum = errSum/(2.0f *count);
return errSum;
}
int mvFindHomographyProj2Img(vect2f_t *src,vect2f_t *dst,float *R,int nPoint,
float stepx,float stepy,int iterations)
{
CvMat _J,_B,_JtJiJtB;
float J[8][8],eB[8],JtJiJtB[8],filterJtJiJtB[8],Rf[9];
memset(R,0,32);
double a[8][8];
double b[8], x[9];
CvMat A = cvMat( 8, 8, CV_64FC1, a );
CvMat B = cvMat( 8, 1, CV_64FC1, b );
CvMat X = cvMat( 8, 1, CV_64FC1, x );
int i,j,k;
int index[4];
float errSum = 0.0f,minErr = 10000.0f;
memset(filterJtJiJtB,0,32);
for(k = 0;k < 2000;k++)
{
if(0 == mvGetPointIndex(src,nPoint,index,stepx,stepy,1))
continue;
for( i = 0; i < 4; ++i )
{
a[i + 0][0] = src[index[i]].x;
a[i + 0][1] = src[index[i]].y;
a[i + 0][2] = 1;
a[i + 0][3] = 0;
a[i + 0][4] = 0;
a[i + 0][5] = 0;
a[i + 0][6] = -dst[index[i]].x * src[index[i]].x;
a[i + 0][7] = -dst[index[i]].x * src[index[i]].y;
a[i + 4][0] = 0;
a[i + 4][1] = 0;
a[i + 4][2] = 0;
a[i + 4][3] = src[index[i]].x;
a[i + 4][4] = src[index[i]].y;
a[i + 4][5] = 1;
a[i + 4][6] = -dst[index[i]].y * src[index[i]].x;
a[i + 4][7] = -dst[index[i]].y * src[index[i]].y;
b[i + 0] = dst[index[i]].x;
b[i + 4] = dst[index[i]].y;
}
cvSolve( &A, &B, &X, CV_SVD );
x[8] = 1;
for(i = 0;i < 9;i++)
{
Rf[i] = (float)x[i];
}
errSum = getErrHomography(src,dst,Rf,nPoint,stepx,stepy);
if(errSum < 0.01f)
{
minErr = errSum;
for(i = 0;i < 9;i++)
{
R[i] = (float)x[i];
}
break;
}
if(minErr > errSum)
{
for(i = 0;i < 9;i++)
{
R[i] = (float)x[i];
}
minErr = errSum;
}
/*
printf("%f %f %f\n",x[0],x[1],x[2]);
printf("%f %f %f\n",x[3],x[4],x[5]);
printf("%f %f %f\n",x[6],x[7],x[8]);
printf("\n");
*/
}
errSum = minErr;
if(errSum > 1.0f)
return 0;
for(k = 0;k < iterations;k++)
{
vect2f_t _dstCalc;
if(0 == mvGetPointIndex(src,nPoint,index,stepx,stepy,1))
continue;
for( i = 0; i < 4; ++i )
{
_dstCalc.x = (R[0] * src[index[i]].x + R[1] * src[index[i]].y + R[2])/(R[6] * src[index[i]].x + R[7] * src[index[i]].y + R[8]);
_dstCalc.y = (R[3] * src[index[i]].x + R[4] * src[index[i]].y + R[5])/(R[6] * src[index[i]].x + R[7] * src[index[i]].y + R[8]);
J[i + 0][0] = src[index[i]].x;
J[i + 0][1] = src[index[i]].y;
J[i + 0][2] = 1;
J[i + 0][3] = 0;
J[i + 0][4] = 0;
J[i + 0][5] = 0;
J[i + 0][6] = -dst[index[i]].x * src[index[i]].x;
J[i + 0][7] = -dst[index[i]].x * src[index[i]].y;
J[i + 4][0] = 0;
J[i + 4][1] = 0;
J[i + 4][2] = 0;
J[i + 4][3] = src[index[i]].x;
J[i + 4][4] = src[index[i]].y;
J[i + 4][5] = 1;
J[i + 4][6] = -dst[index[i]].y * src[index[i]].x;
J[i + 4][7] = -dst[index[i]].y * src[index[i]].y;
eB[i + 0] = _dstCalc.x - dst[index[i]].x;
eB[i + 4] = _dstCalc.y - dst[index[i]].y;
b[i + 0] = dst[index[i]].x;
b[i + 4] = dst[index[i]].y;
}
for(i = 0;i <8;i++)
{
for(j = 0;j < 8;j++)
{
a[i][j] = (double)J[i][j];
}
}
cvSolve( &A, &B, &X, CV_SVD );
float errSum1 = 0.0f;
for(i = 0;i < 9;i++)
{
Rf[i] = (float)x[i];
}
errSum1 = getErrHomography(src,dst,Rf,nPoint,stepx,stepy);
if(errSum1 > 0.001f)
{
continue;
}
_J = cvMat(8, 8, CV_32FC1, (float *)&J[0][0]);
_JtJiJtB = cvMat(8, 1, CV_32FC1,(float *)&JtJiJtB[0]);
_B = cvMat(8, 1, CV_32FC1, (float *)&eB[0]);
cvSolve( &_J, &_B, &_JtJiJtB, CV_SVD );
//cvSolve(&_JtJ,&_JtB,&_JtJiJtB,CV_SVD);
//mvMatrixMutl(&JtJi[0][0],8,8,JtB,1,JtJiJtB);
float Rtemp[9],JtJiJtBTemp[9];
for(j = 0;j < 9;j++)
{
JtJiJtBTemp[j] = filterJtJiJtB[j];
Rtemp[j] = R[j];
filterJtJiJtB[j] = 0.9f * filterJtJiJtB[j] + 0.1f * JtJiJtB[j];
R[j] -= filterJtJiJtB[j];
}
float errSum2 = 0.0f;
errSum2 = getErrHomography(src,dst,R,nPoint,stepx,stepy);
if(errSum2 > errSum)
{
for(j = 0;j < 9;j++)
{
R[j] = Rtemp[j];
filterJtJiJtB[j] = JtJiJtBTemp[j];
}
}
errSum2 = getErrHomography(src,dst,R,nPoint,stepx,stepy);
if(errSum2 < 0.1f)
{
break;
}
}
R[8] = 1.0f;
return 1;
}
float gR[20][3][3];
void mvInitCalibCameraPointTest(const vect2f_t* objectPoints,
const vect2f_t* imagePoints, int width,int hight,float stepx,float stepy,int imageCnt)
{
int i,j,k;
vect2f_t *pObjPoint = (vect2f_t *)objectPoints,*pImgPoint = (vect2f_t *)imagePoints;
vect3f_t *rotate3dAngle = (vect3f_t *)malloc(imageCnt * sizeof(vect3f_t));
vect3f_t *translation3d = (vect3f_t *)malloc(imageCnt * sizeof(vect3f_t));
float R[3][3];
float k1 = 0.010245f;
float k2 = 0.001011245f;
float fx = 122.0f,fy = 122.0f;
float cx = 150.0f,cy = 50.0f;
float lamder = 1.0f,err = 0.0f;
float x,y,x1,y1,x2,y2,x3,y3;
srand((unsigned) time(NULL));
//cx = (float)(rand() % 200);
//cy = (float)(rand() % 200);
//printf("fx,fy,cx,cy = %f,%f,%f,%f\n\n",fx,fy,cx,cy);
for(i = 0;i < imageCnt;i++)
{
rotate3dAngle[i].x = ((float)(rand() % 90) - 45.0f) * PI/180.0f;
rotate3dAngle[i].y = ((float)(rand() % 90) - 45.0f) * PI/180.0f;
rotate3dAngle[i].z = ((float)(rand() % 90) - 45.0f) * PI/180.0f;
translation3d[i].x = (float)(rand() % 201);
translation3d[i].y = (float)(rand() % 201);
translation3d[i].z = (float)(400 + (rand() % 100));//(float)(rand() % 51);
//printf("z init = %f\n",translation3d[i].z);
}
for(i = 0;i < imageCnt;i++)
{
float aerf,belt,garmer;
aerf = rotate3dAngle[i].z;
belt = rotate3dAngle[i].x;
garmer = rotate3dAngle[i].y;
R[0][0] = cos(aerf) * cos(belt);
R[0][1] = cos(aerf) * sin(belt)* sin(garmer) - sin(aerf) * cos(garmer);
R[0][2] = cos(aerf) * sin(belt)* cos(garmer) + sin(aerf) * sin(garmer);
R[1][0] = sin(aerf) * cos(belt);
R[1][1] = sin(aerf) * sin(belt)* sin(garmer) + cos(aerf) * cos(garmer);
R[1][2] = sin(aerf) * sin(belt)* cos(garmer) - cos(aerf) * sin(garmer);
R[2][0] = -sin(belt);
R[2][1] = sin(garmer) * cos(belt);
R[2][2] = cos(garmer) * cos(belt);
#if 0
H[0][0] = fx * R[0][0] + cx * R[2][0];
H[0][1] = fx * R[0][1] + cx * R[2][1];
H[0][2] = fx * R[0][2] + cx * R[2][2];
H[0][3] = fx * translation3d[i].x + cx * translation3d[i].z;
H[1][0] = fy * R[1][0] + cy * R[2][0];
H[1][1] = fy * R[1][1] + cy * R[2][1];
H[1][2] = fy * R[1][2] + cy * R[2][2];
H[1][3] = fy * translation3d[i].y + cx * translation3d[i].z;
H[2][0] = R[2][0];
H[2][1] = R[2][1];
H[2][2] = R[2][2];
H[2][3] = translation3d[i].z;
//printf("%f ",R[0][0] * R[0][0] + R[1][0] * R[1][0] + R[2][0] * R[2][0]);
//printf("%f ",R[0][1] * R[0][1] + R[1][1] * R[1][1] + R[2][1] * R[2][1]);
//printf("%f\n",R[0][2] * R[0][2] + R[1][2] * R[1][2] + R[2][2] * R[2][2]);
R[0][0] = H[0][0];
R[0][1] = H[0][1];
R[0][2] = H[0][3];
R[1][0] = H[1][0];
R[1][1] = H[1][1];
R[1][2] = H[1][3];
R[2][0] = H[2][0];
R[2][1] = H[2][1];
R[2][2] = H[2][3];
gR[i][0][0] = H[0][0];
gR[i][0][1] = H[0][1];
gR[i][0][2] = H[0][3];
gR[i][1][0] = H[1][0];
gR[i][1][1] = H[1][1];
gR[i][1][2] = H[1][3];
gR[i][2][0] = H[2][0];
gR[i][2][1] = H[2][1];
gR[i][2][2] = H[2][3];
#else
gR[i][0][0] = (float)(fx * R[0][0] + cx * R[2][0]);
gR[i][0][1] = (float)(fx * R[0][1] + cx * R[2][1]);
gR[i][0][2] = (float)(fx * translation3d[i].x + cx * translation3d[i].z);
gR[i][1][0] = (float)(fy * R[1][0] + cy * R[2][0]);
gR[i][1][1] = (float)(fy * R[1][1] + cy * R[2][1]);
gR[i][1][2] = (float)(fy * translation3d[i].y + cx * translation3d[i].z);
gR[i][2][0] = (float)R[2][0];
gR[i][2][1] = (float)R[2][1];
gR[i][2][2] = (float)(translation3d[i].z);
R[0][2] = translation3d[i].x;
R[1][2] = translation3d[i].y;
R[2][2] = translation3d[i].z;
#endif
for(j = 0;j < hight;j++)
{
for(k = 0;k < width;k++)
{
pObjPoint[j * width + k].x = stepx * (k + 1);
pObjPoint[j * width + k].y = stepy * (j + 1);
x = stepx * (k + 1);
y = stepy * (j + 1);
x1 = (R[0][0] * x + R[0][1] * y + R[0][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
y1 = (R[1][0] * x + R[1][1] * y + R[1][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
double x2y2square = x1 * x1 + y1 * y1;
double square2 = x2y2square * x2y2square;
double square3 = x2y2square * x2y2square * x2y2square;
double square4 = x2y2square * x2y2square * x2y2square * x2y2square;
double scalek1 = k1 * square2;
double scalek2 = k2 * square4;
x2 = x1 * (1 + k1 * (float)square2 + k2 * (float)square4);
y2 = y1 * (1 + k1 * (float)square2 + k2 * (float)square4);
x3 = fx * x2 + cx;
y3 = fy * y2 + cy;
pImgPoint[(j) * width + k].x = (float)x3;
pImgPoint[(j) * width + k].y = (float)y3;
}
}
//float estR[9];
//mvFindHomography(pObjPoint,pImgPoint,estR,(width - 1) * (hight - 1),200);
//printf("%f %f %f\n",estR[0],estR[1],estR[2]);
//printf("%f %f %f\n",estR[3],estR[4],estR[5]);
//printf("%f %f %f\n",estR[6],estR[7],estR[8]);
//printf("\n\n");
pObjPoint += width * hight;
pImgPoint += width * hight;
}
}
int mvCalibCameraParams(const vect2f_t* objectPoints,
const vect2f_t* imagePoints, int width,int hight,int imageCnt,float stepx,float stepy,
matrix_t* A)
{
int hcount = 0;
int i,j,k,m,n,p;
int homographyCnt = width * hight;
float mHt[3][3],**gHt;
vect2f_t *pObjPoint = 0,*pImgPoint = 0;
double h11,h12,h13,h21,h22,h23;
double err = 0.0f;
float fx = 0.0f,fy = 0.0f,cx = 0.0f,cy = 0.0f;
float maxPoint = 0.0f,minPoint = 100000.0f;
float *x0Param = (float *)malloc((imageCnt * imageCnt * imageCnt * imageCnt * imageCnt) * sizeof(float));
float *x1Param = (float *)malloc((imageCnt * imageCnt * imageCnt * imageCnt * imageCnt) * sizeof(float));
float *y0Param = (float *)malloc((imageCnt * imageCnt * imageCnt * imageCnt * imageCnt) * sizeof(float));
float *y1Param = (float *)malloc((imageCnt * imageCnt * imageCnt * imageCnt * imageCnt) * sizeof(float));
double *k1k2 = (double *)malloc(4 * homographyCnt * imageCnt * sizeof(double));
double *pd = (double *)malloc(2 * homographyCnt * imageCnt * sizeof(double));
int *validImage = (int *)malloc(4 * imageCnt);
gHt = (float **)malloc(imageCnt * sizeof(*gHt));
if(gHt == NULL)
return 0;
for(i = 0;i < imageCnt;i++)
{
gHt[i] = (float *)malloc(9 * sizeof(float));
if(gHt[i] == NULL)
return 0;
}
if(width < 2 || hight < 2 || imageCnt < 10)
{
return 0;
}
if(objectPoints == NULL || imagePoints == NULL || A == NULL)
return 0;
/*
double mfx = 1.22f,mfy = 1.22f;
double mcx = 13.0f,mcy = 12.0f;
double mlamder = 0.2f;
float mi[9],mit[9],mtim[9];
mi[0] = 1/mfx; mi[1] = 0; mi[2] = -mcx/mfx;
mi[3] = 0; mi[4] = 1/mfy; mi[5] = -mcy/mfy;
mi[6] = 0; mi[7] = 0; mi[8] = 1;
mit[0] = 1/mfx; mit[1] = 0; mit[2] = 0;
mit[3] = 0; mit[4] = 1/mfy; mit[5] = 0;
mit[6] = -mcx/mfx; mit[7] = -mcy/mfy; mit[8] = 1;
mvMatrixMutl(mit,3,3,mi,3,mtim);
b[0] = 1/(mfx * mfx);
b[1] = 0.0f;
b[2] = 1/(mfy * mfy);
b[3] = -mcx/(mfx * mfx);
b[4] = -mcy/(mfy * mfy);
b[5] = (mcx/mfx) * (mcx/mfx) + (mcy/mfy) * (mcy/mfy);
fx = sqrt(fabs(1.0f/b[0]));
fy = sqrt(fabs(1.0f/b[2]));
cx = -b[3] * fx * fx;
cy = -b[4] * fy * fy;
float b[7] = {mtim[0],mtim[1],mtim[4],mtim[6],mtim[7],mtim[8],1.0f};
*/
pObjPoint = (vect2f_t *)objectPoints;
pImgPoint = (vect2f_t *)imagePoints;
CvMat _pt1, _pt2,_mHt;
double Hdata[7][7],Bb[7],Bres[7];
int count = 0,failFalg = 0;
for(i = 0;i < imageCnt;i++)
{
float *pGht = gHt[i];
/*
printf("%f %f %f\n",gR[i][0][0],gR[i][0][1],gR[i][0][2]);
printf("%f %f %f\n",gR[i][1][0],gR[i][1][1],gR[i][1][2]);
printf("%f %f %f\n",gR[i][2][0],gR[i][2][1],gR[i][2][2]);
*/
validImage[i] = mvFindHomographyProj2Img(&pObjPoint[i * homographyCnt],
&pImgPoint[i * homographyCnt],pGht,homographyCnt,stepx,stepy,200);
if(validImage[i] == 0)
{
printf("i falid %d\n",i);
continue;
}
/*
if(fabs(gR[i][0][0] - pGht[0]) > 1.0f || fabs(gR[i][0][1] - pGht[1]) > 1.0f)
{
printf("\n");
printf("%f %f %f\n",gR[i][0][0],gR[i][0][1],gR[i][0][2]);
printf("%f %f %f\n",gR[i][1][0],gR[i][1][1],gR[i][1][2]);
printf("%f %f %f\n",gR[i][2][0],gR[i][2][1],gR[i][2][2]);
printf("\n");
printf("%f %f %f\n",pGht[0],pGht[1],pGht[2]);
printf("%f %f %f\n",pGht[3],pGht[4],pGht[5]);
printf("%f %f %f\n",pGht[6],pGht[7],pGht[8]);
printf("\n\n");
}
*/
}
for(p = 0;p < (imageCnt - 2);p++)
{
for(k = p + 1;k < (imageCnt - 1);k++)
{
for(m = k + 1;m < imageCnt;m++)
{
failFalg = 0;
int index[3] = {p,k,m};
if(validImage[p] == 0)
continue;
if(validImage[k] == 0)
continue;
if(validImage[m] == 0)
continue;
for(n = 0;n < 3;n++)
{
//float estR[9];
//cvFindHomography(&_pt1,&_pt2,&_mHt);
float *pGht = gHt[index[n]];
mHt[0][0] = pGht[0];
mHt[0][1] = pGht[1];
mHt[0][2] = pGht[2];
mHt[1][0] = pGht[3];
mHt[1][1] = pGht[4];
mHt[1][2] = pGht[5];
mHt[2][0] = pGht[6];
mHt[2][1] = pGht[7];
mHt[2][2] = pGht[8];
h11 = mHt[0][0];
h12 = mHt[1][0];
h13 = mHt[2][0];
h21 = mHt[0][1];
h22 = mHt[1][1];
h23 = mHt[2][1];
/*
printf("\n");
printf("%f %f %f\n",mHt[0][0],mHt[0][1],mHt[0][2]);
printf("%f %f %f\n",mHt[1][0],mHt[1][1],mHt[1][2]);
printf("%f %f %f\n",mHt[2][0],mHt[2][1],mHt[2][2]);
printf("\n");
*/
/*
h11 = gR[n][0][0];
h12 = gR[n][1][0];
h13 = gR[n][2][0];
h21 = gR[n][0][1];
h22 = gR[n][1][1];
h23 = gR[n][2][1];
*/
double v12[7] = {h11 * h21,h11 * h22 + h12 * h21,h12 * h22,h13 * h21 + h11 * h23,h13 * h22 + h12 * h23,h13 * h23,h13 * h23};
double v11[7] = {h11 * h11,h11 * h12 + h12 * h11,h12 * h12,h13 * h11 + h11 * h13,h13 * h12 + h12 * h13,h13 * h13,h13 * h13};
double v22[7] = {h21 * h21,h21 * h22 + h22 * h21,h22 * h22,h23 * h21 + h21 * h23,h23 * h22 + h22 * h23,h23 * h23,h23 * h23};
for(i = 0;i < 6;i++)
{
Hdata[2 * n][i] = v12[i];
}
Hdata[2 * n][6] = v12[6];
Bb[2 * n + 0] = Hdata[2 * n][0];
for(i = 0;i < 6;i++)
{
Hdata[2 * n + 1][i] = (v11[i] - v22[i]);
}
Hdata[2 * n + 1][6] = (v11[0] - v22[0]);
Bb[2 * n + 1] = Hdata[2 * n + 1][0];
/*
double sum0 = 0.0f, sum1 = 0.0f;
for(i = 0;i < 6;i++)
{
sum0 += v12[i] * b[i];
sum1 += (v11[i] - v22[i]) * b[i];
}
if(fabs(sum0) > 0.0001f || fabs(sum1) > 0.0001f )
{
failFalg = 1;
}
*/
}
/*
if(failFalg)
{
continue;
}
*/
double max = -10000.0f;
for(i = 0;i < 6;i++)
{
for(j = 0;j < 6;j++)
{
if(fabs(Hdata[i][j]) > max)
{
max = fabs(Hdata[i][j]);
}
}
}
Hdata[6][0] = 0;
Hdata[6][1] = 0;
Hdata[6][2] = 0;
Hdata[6][3] = 0;
Hdata[6][4] = 0;
Hdata[6][5] = 0;
Hdata[6][6] = 1;
for(i = 0;i < 7;i++)
{
for(j = 0;j < 7;j++)
{
Hdata[i][j] = Hdata[i][j]/max;
}
}
Bb[6] = 1.0f;
for(i = 0;i < 7;i++)
{
Bb[i] = Bb[i]/max;
}
//mvMatrixTrans((float *)H.pdata,6,6,(float *)&Ht[0]);
//mvMatrixMutl(&Ht[0],6,6,H.pdata,6,&HtH[0]);
//mvJacobiEigens_32f(HtH,W,B.pdata,6,0.0001f);
_pt1 = cvMat(7, 7, CV_64FC1, (double *)&Hdata[0][0] );
_pt2 = cvMat(7, 1, CV_64FC1, (double *)&Bb[0] );
_mHt = cvMat(7, 1, CV_64FC1, Bres);
cvSolve( &_pt1, &_pt2, &_mHt, CV_SVD );
double B11,B12,B13,B22,B23,B33,B00,deltFx,deltFy,deltCx,deltCy;
B11 = Bres[0];
B12 = Bres[1];
B13 = Bres[2];
B22 = Bres[3];
B23 = Bres[4];
B33 = Bres[5];
B00 = Bres[6];
deltFx = sqrt(fabs(1.0f/B11));
deltFy = sqrt(fabs(1.0f/B13));
deltCx = fabs(-B22 * deltFx * deltFx);
deltCy = fabs(-B23 * deltFy * deltFy);
double a1 = mHt[0][0] - deltCx * mHt[2][0] ;
double b1 = mHt[0][1] - deltCy * mHt[2][1] ;
double scale = deltFx/deltFy;
deltFy = sqrt((a1 * a1 + scale * b1 * b1)/fabs(1 - mHt[2][0] * mHt[2][0]));
deltFx = scale * deltFy;
x0Param[count] = (float)deltCx;
y0Param[count] = (float)deltCy;
count++;
//printf("%d:fx,fy,cx,cy = %f,%f,%f,%f\n",count,deltFx,deltFy,deltCx,deltCy);
//printf("%d:fx,fy,cx,cy = %f,%f,%f,%f\n",count,fx,fy,cx,cy);
}
}
}
//fx = gR[n][0][0] - cx * gR[n][2][0]
//printf("%d:fx,fy,cx,cy = %f,%f,%f,%f\n",count,fx/count,fy/count,cx/count,cy/count);
cx = getBestVal(x0Param,count,4.0f);
cy = getBestVal(y0Param,count,4.0f);
count = 0;
for(i = 0;i < imageCnt;i++)
{
if(validImage[i] == 0)
continue;
float *pGht = gHt[i];
mHt[0][0] = pGht[0];
mHt[0][1] = pGht[1];
mHt[0][2] = pGht[2];
mHt[1][0] = pGht[3];
mHt[1][1] = pGht[4];
mHt[1][2] = pGht[5];
mHt[2][0] = pGht[6];
mHt[2][1] = pGht[7];
mHt[2][2] = pGht[8];
double r31,r32;
r31 = mHt[2][0];
r32 = mHt[2][1];
#if 0
deltFx = 122.0f;
deltFy = 122.0f;
double r11,r12,r21,r22;
r31 = mHt[2][0];
r32 = mHt[2][1];
r11 = (mHt[0][0] - deltCx * r31)/deltFx;
r21 = (mHt[1][0] - deltCy * r31)/deltFx;
r12 = (mHt[0][1] - deltCx * r32)/deltFy;
r22 = (mHt[1][1] - deltCy * r32)/deltFy;
double mod0,mod1;
mod0 = r11 * r11 + r21 * r21 + r31 * r31;
mod1 = r12 * r12 + r22 * r22 + r32 * r32;
printf("mod = %f %f\n",mod0 * 40000.0f,mod1 * 40000.0f);
if(fabs(mod0 - 1.0f) > 0.1f || fabs(mod1 - 1.0f) > 0.1f)
{
continue;
}
#endif
double a,b,c,d,yy;
a = (mHt[0][0] - cx * r31);
b = (mHt[1][0] - cy * r31);
c = (mHt[0][1] - cx * r32);
d = (mHt[1][1] - cy * r32);
double ff = (a * c * (b * b - d * d) - d * b * (a * a - c * c))
/(a * c * (r32 * r32 - r31 * r31) + r31 * r32 * (a * a - c * c));
if(fabs(ff) < 0.001f)
continue;
yy = 1.0f/ff;
if(ff < 0.0f)
continue;
fy = (float)sqrt(ff);
fx = (float)sqrt((a * a - c * c)/((r32 * r32 - r31 * r31) - (b * b - d * d) * yy));
//fx = scale * deltFy;
//printf("f = %f %f\n",fx,fy);
x0Param[count] = fx;
y0Param[count] = fy;
count++;
}
fx = getBestVal(x0Param,count,4.0f);
fy = getBestVal(y0Param,count,4.0f);
printf("fx,fy,cx,cy = %f,%f,%f,%f\n",fx,fy,cx,cy);
float mi[9],R[9];
mi[0] = 1/fx; mi[1] = 0; mi[2] = -cx/fx;
mi[3] = 0; mi[4] = 1/fy; mi[5] = -cy/fy;
mi[6] = 0; mi[7] = 0; mi[8] = 1;
float k1 = 0.0f,k2 = 0.0f;
int kCnt = 0;
double _k[2];
for(n = 0;n < imageCnt;n++)
{
if(validImage[n] == 0)
continue;
float *pGht = gHt[n];
mvMatrixMutl(mi,3,3,pGht,3,R);
//printf("%f\n",R[0] * R[0] + R[3] * R[3] + R[6] * R[6]);
//printf("%f\n",R[1] * R[1] + R[4] * R[4] + R[7] * R[7]);
memcpy(pGht,R,36);
double *_k1k2 = &k1k2[4 * kCnt * homographyCnt];
double *d = &pd[2 * kCnt * homographyCnt];
double x1,y1,x2,y2;
for(i = 0;i < homographyCnt;i++)
{
vect2f_t pT = pObjPoint[n * homographyCnt + i];
vect2f_t iT = pImgPoint[n * homographyCnt + i];
x1 = (R[0] * pT.x + R[1] * pT.y + R[2])/(R[6] * pT.x + R[7] * pT.y + R[8]);
y1 = (R[3] * pT.x + R[4] * pT.y + R[5])/(R[6] * pT.x + R[7] * pT.y + R[8]);
double x2y2square = x1 * x1 + y1 * y1;
double square2 = x2y2square * x2y2square;
double square3 = x2y2square * x2y2square * x2y2square;
double square4 = x2y2square * x2y2square * x2y2square * x2y2square;
x2 = (iT.x - cx)/fx;
y2 = (iT.y - cy)/fy;
_k1k2[4 * i + 0] = x1 * square2;
_k1k2[4 * i + 1] = x1 * square4;
d[2 * i + 0] = x2 - x1;
_k1k2[4 * i + 2] = y1 * square2;
_k1k2[4 * i + 3] = y1 * square4;
d[2 * i + 1] = y2 - y1;
}
//x0Param[2 * kCnt + 0] = sqrt(1.0f/(R[0] * R[0] + R[3] * R[3] + R[6] * R[6]));
//x0Param[2 * kCnt + 1] = sqrt(1.0f/(R[1] * R[1] + R[4] * R[4] + R[7] * R[7]));
//printf("z0 = %f\n",sqrt(1.0f/(R[0] * R[0] + R[3] * R[3] + R[6] * R[6])));
//printf("z1 = %f\n",sqrt(1.0f/(R[1] * R[1] + R[4] * R[4] + R[7] * R[7])));
kCnt++;
}
_pt1 = cvMat(kCnt * homographyCnt * 2, 2, CV_64FC1, k1k2);
_pt2 = cvMat(kCnt * homographyCnt * 2, 1, CV_64FC1, pd );
_mHt = cvMat(2, 1, CV_64FC1, _k);
cvSolve( &_pt1, &_pt2, &_mHt, CV_SVD );
_k[0] = _k[0] * 2.0f;
_k[1] = _k[1] * 1.2f;
//float z = getBestVal(x0Param,kCnt * 2,4.0f);
//printf("k1,k2 = %f,%f\n",_k[0],_k[1]);
A->pdata[0] = fx;
A->pdata[1] = 0;
A->pdata[2] = cx;
A->pdata[3] = 0;
A->pdata[4] = fy;
A->pdata[5] = cy;
A->pdata[6] = 0;
A->pdata[7] = 0;
A->pdata[8] = 1;
mvBPCalcCameraParams(objectPoints,imagePoints,validImage,width,hight,imageCnt,stepx,stepy,A,_k,gHt);
for(i = 0;i < imageCnt;i++)
{
free(gHt[i]);
}
free(gHt);
free(x0Param);
free(x1Param);
free(y0Param);
free(y1Param);
free(k1k2);
free(pd);
free(validImage);
return 1;
}
float yt[1000];
float xt[1000];
float getBestVal(float *pVal,int count,float limit)
{
float maxValid = 0;
int validCnt;
int i,j;
float validVal = 0.0f;
float validSum = 0.0f;
for(i = 0;i < (count - 1);i++)
{
validCnt = 0;
for(j = i;j < count;j++)
{
if(fabs(pVal[i] - pVal[j]) < limit)
{
validCnt++;
}
}
if(validCnt > (int)maxValid)
{
maxValid = (float)validCnt;
validVal = pVal[i];
}
}
validCnt = 0;
for(j = 0;j < count;j++)
{
if(fabs(validVal - pVal[j]) < limit)
{
validCnt++;
validSum += pVal[j];
}
}
return (validSum/validCnt);
}
void LMTest(void)
{
int i,j;
float a1 = 1;
float b1 = 1;
float c1 = 1;
float d1 = 1;
#define LM_SIZE 4
#define PARAM_NUM 16
for(i = 0;i < 1000;i++)
{
xt[i] = (i - 500) * 0.1f;
float temp = 2.5f * xt[i];
yt[i] = 2.36f* temp * temp + 3.345f * temp + 4.1f;
}
float J[LM_SIZE][LM_SIZE],Jt[LM_SIZE][LM_SIZE],JtJ[LM_SIZE][LM_SIZE],P[LM_SIZE],e[LM_SIZE],Je[LM_SIZE],JtJi[LM_SIZE][LM_SIZE],JtJiJ[LM_SIZE][LM_SIZE];
CvMat _J, _Jt,_JtJ,_P,_e,_Je,_JtJi;
_J = cvMat(LM_SIZE, LM_SIZE, CV_32FC1, (double *)&J[0][0] );
_Jt = cvMat(LM_SIZE, LM_SIZE, CV_32FC1, (double *)&Jt[0][0] );
_JtJ = cvMat(LM_SIZE, LM_SIZE, CV_32FC1, (double *)&JtJ[0][0] );
_JtJi = cvMat(LM_SIZE, LM_SIZE, CV_32FC1, (double *)&JtJi[0][0] );
_P = cvMat(LM_SIZE, 1, CV_32FC1, P);
_e = cvMat(LM_SIZE, 1, CV_32FC1, e);
_Je = cvMat(LM_SIZE, 1, CV_32FC1, Je);
float temp;
for(j = 0;j < 6000;j++)
{
for(i = 0;i < LM_SIZE;i++)
{
int idx = rand() % 1000;
printf("idx = %d\n",idx);
temp = d1 * xt[idx];
float ytt0 = a1 * temp * temp + b1 * temp + c1;
J[i][0] = temp * temp;
J[i][1] = temp;
J[i][2] = 1;
J[i][3] = (2.0f * a1 * temp + b1) * xt[idx];
e[i] = ytt0 - yt[idx];
}
mvMatrixTrans((float *)&J[0][0],LM_SIZE,LM_SIZE,(float *)&Jt[0][0]);
mvMatrixMutl((float *)&Jt[0][0],LM_SIZE,LM_SIZE,(float *)&J[0][0],LM_SIZE,(float *)&JtJ[0][0]);
cvInvert(&_JtJ,&_JtJi,CV_SVD);
mvMatrixMutl((float *)&JtJi[0][0],LM_SIZE,LM_SIZE,(float *)&Jt[0][0],LM_SIZE,(float *)&JtJiJ[0][0]);
mvMatrixMutl((float *)&JtJiJ[0][0],LM_SIZE,LM_SIZE,(float *)e,1,(float *)&P[0]);
//cvGEMM(&_J,&_e,1,NULL,1,&_Je);
//cvGEMM(&_JtJi,&_Je,1,NULL,1,&_P);
//cvSolve( &_J, &_e, &_P, CV_SVD );
a1 = a1 - 0.2f * P[0];
b1 = b1 - 0.2f * P[1];
c1 = c1 - 0.2f * P[2];
d1 = d1 - 0.2f * P[3];
float err = 0.0f;
float ytt0;
for(i = 0;i < 1000;i++)
{
temp = d1 * xt[i];
ytt0 = a1 * temp * temp + b1 * temp + c1;
err += ytt0 - yt[i];
}
printf("err = %f\n",err);
}
printf("a,b,c = %f,%f,%f",a1,b1,c1);
}
double getErrHomParams(vect2f_t *src,vect2f_t *dst,float *H,double fx,
double fy,double cx,double cy,int nPoint)
{
double x1,y1,x3,y3;
float *pGht = H;
float mi[9],R[9];
int i;
double errSum = 0.0f;
mi[0] = (float)1/(float)fx; mi[1] = 0; mi[2] = -(float)cx/(float)fx;
mi[3] = 0; mi[4] = (float)1/(float)fy; mi[5] = -(float)cy/(float)fy;
mi[6] = 0; mi[7] = 0; mi[8] = 1;
mvMatrixMutl(mi,3,3,H,3,R);
vect2f_t *_imgPoint = src;
vect2f_t *_prjPoint = dst;
for(i = 0;i < nPoint;i++)
{
vect2f_t pT = _prjPoint[i];
x1 = (R[0] * pT.x + R[1] * pT.y + R[2])/(R[6] * pT.x + R[7] * pT.y + R[8]);
y1 = (R[3] * pT.x + R[4] * pT.y + R[5])/(R[6] * pT.x + R[7] * pT.y + R[8]);
/*
double x2y2square = x1 * x1 + y1 * y1;
double square2 = x2y2square * x2y2square;
double square3 = x2y2square * x2y2square * x2y2square;
double square4 = x2y2square * x2y2square * x2y2square * x2y2square;
double scalek1 = k1 * square2;
double scalek2 = k2 * square4;
x2 = x1 * (1 + k1 * square2 + k2 * square4);
y2 = y1 * (1 + k1 * square2 + k2 * square4);
*/
x3 = fx * x1 + cx;
y3 = fy * y1 + cy;
errSum += fabs(x3 - _imgPoint[i].x);
errSum += fabs(y3 - _imgPoint[i].y);
}
errSum = errSum/(2.0f *nPoint);
return errSum;
}
double getErrCameraParams(vect2f_t *src,vect2f_t *dst,double *param,int nPoint)
{
int j;
double errSum = 0.0f;
double x,y,x1,y1,x2,y2,x3,y3;
double fx,fy,cx,cy,k1,k2,R[3][3];
fx = param[0];
fy = param[1];
cx = param[2];
cy = param[3];
k1 = param[4];
k2 = param[5];
R[0][0] = param[6];
R[0][1] = param[7];
R[0][2] = param[8];
R[1][0] = param[9];
R[1][1] = param[10];
R[1][2] = param[11];
R[2][0] = param[12];
R[2][1] = param[13];
R[2][2] = param[14];
for(j = 0;j < nPoint;j++)
{
x = (double)src[j].x;
y = (double)src[j].y;
x1 = (double)(R[0][0] * x + R[0][1] * y + R[0][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
y1 = (double)(R[1][0] * x + R[1][1] * y + R[1][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
double x2y2square = x1 * x1 + y1 * y1;
double square2 = x2y2square * x2y2square;
double square3 = x2y2square * x2y2square * x2y2square;
double square4 = x2y2square * x2y2square * x2y2square * x2y2square;
x2 = x1 * (1 + k1 * square2 + k2 * square4);
y2 = y1 * (1 + k1 * square2 + k2 * square4);
x3 = fx * x2 + cx;
y3 = fy * y2 + cy;
errSum += fabs(x3 - dst[j].x);
errSum += fabs(y3 - dst[j].y);
}
errSum = errSum/(2.0f *nPoint);
return errSum;
}
void mvBPCalcCameraParams(const vect2f_t* objectPoints,
const vect2f_t* imagePoints, int *validImage,int width,int hight,
int imageCnt, float stepx,float stepy,matrix_t* A,double *k,float **gHt)
{
double deltFx = 0.0f,deltFy = 0.0f,deltCx = 0.0f,deltCy = 0.0f,deltK1 = 0.0f,deltK2 = 0.0f;
double deltR11 = 0.0f,deltR12 = 0.0f,deltTx = 0.0f;
double deltR21 = 0.0f,deltR22 = 0.0f,deltTy = 0.0f;
double deltR31 = 0.0f,deltR32 = 0.0f,deltTz = 0.0f;
//float dFx,dFy,dCx,dCy,dK1,dK2,dR11,dR12,dR13,dR21,dR22,dR23,dR31,dR32,dR33;
double J[16][15],e[16],delt[15],filterDelt[15],param[15],filterDeltTemp[15],paramTemp[15];
double k1,k2,fx,fy,cx,cy;
double x,y,x1,y1,x2,y2,x3,y3;
int j;
float miu = 0.2f;
float scale = 0.2f;
double R[3][3];
int iner = 0;
k1 = 0.000001f;k2 = 0.000000000001f;
fx = A->pdata[0];fy = A->pdata[4];cx = A->pdata[2];cy = A->pdata[5];
//LMTest();
CvMat _pt1, _pt2,_delt;
_pt1 = cvMat(16, 15, CV_64FC1, (double *)&J[0][0] );
_pt2 = cvMat(16, 1, CV_64FC1, (double *)&e[0] );
_delt = cvMat(15, 1, CV_64FC1, delt);
param[0] = fx;
param[1] = fy;
param[2] = cx;
param[3] = cy;
param[4] = k[0];
param[5] = k[1];
double lastErr = 10000.0f;
double gErr;
for(int n = 0;n < imageCnt;n++)
{
vect2f_t *pObjectPoints = (vect2f_t *)&objectPoints[n * width * hight];
vect2f_t *pImagePoints = (vect2f_t *)&imagePoints[n * width * hight];
if(validImage[n] == 0)
continue;
memset(filterDelt,0,15 * sizeof(double));
float *pGht = gHt[n];
R[0][0] = pGht[0];
R[0][1] = pGht[1];
R[0][2] = pGht[2];
R[1][0] = pGht[3];
R[1][1] = pGht[4];
R[1][2] = pGht[5];
R[2][0] = pGht[6];
R[2][1] = pGht[7];
R[2][2] = pGht[8];
param[6] = R[0][0];
param[7] = R[0][1];
param[8] = R[0][2];
param[9] = R[1][0];
param[10] = R[1][1];
param[11] = R[1][2];
param[12] = R[2][0];
param[13] = R[2][1];
param[14] = R[2][2];
gErr = getErrCameraParams(pObjectPoints,pImagePoints,param,(width * hight));
if(gErr > lastErr)
{
continue;
}
lastErr = gErr;
iner = 0;
for(;;)
{
fx = param[0];
fy = param[1];
cx = param[2];
cy = param[3];
k1 = param[4];
k2 = param[5];
R[0][0] = param[6];
R[0][1] = param[7];
R[0][2] = param[8];
R[1][0] = param[9];
R[1][1] = param[10];
R[1][2] = param[11];
R[2][0] = param[12];
R[2][1] = param[13];
R[2][2] = param[14];
int idxrtan[8];
while(!mvGetPointIndex(pObjectPoints,width * hight,&idxrtan[0],stepx,stepy,0));
while(!mvGetPointIndex(pObjectPoints,width * hight,&idxrtan[4],stepx,stepy,0));
for(j = 0;j < 8;j++)
{
int idx = idxrtan[j];
x = (double)pObjectPoints[idx].x;
y = (double)pObjectPoints[idx].y;
x1 = (double)(R[0][0] * x + R[0][1] * y + R[0][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
y1 = (double)(R[1][0] * x + R[1][1] * y + R[1][2])/(R[2][0] * x + R[2][1] * y + R[2][2]);
double x2y2square = x1 * x1 + y1 * y1;
double square2 = x2y2square * x2y2square;
double square3 = x2y2square * x2y2square * x2y2square;
double square4 = x2y2square * x2y2square * x2y2square * x2y2square;
x2 = x1 * (1 + k1 * square2 + k2 * square4);
y2 = y1 * (1 + k1 * square2 + k2 * square4);
x3 = fx * x2 + cx;
y3 = fy * y2 + cy;
double dx3dx2 = fx;
double dy3dy2 = fy;
double dx2dx1 = 1 + k1 * x2y2square \
+ 4.0f * x1 * x1 * x2y2square + k2 * square4\
+ 8.0f * k2 * x1 * x1 * square3;
double dy2dy1 = 1 + k1 * x2y2square \
+ 4.0f * y1 * y1 * x2y2square + k2 * square4\
+ 8.0f * k2 * y1 * y1 * square3;
J[2 * j + 0][0] = x2;
J[2 * j + 0][1] = 0;
J[2 * j + 0][2] = 1;
J[2 * j + 0][3] = 0;
J[2 * j + 0][4] = dx3dx2 * x1 * square2;
J[2 * j + 0][5] = dx3dx2 * x1 * square4;
J[2 * j + 0][6] = fx * dx2dx1 * (x);
J[2 * j + 0][7] = fx * dx2dx1 * (y);
J[2 * j + 0][8] = fx * dx2dx1 * (1);
J[2 * j + 0][9] = fx * dx2dx1 * (0);
J[2 * j + 0][10] = fx * dx2dx1 * (0);
J[2 * j + 0][11] = fx * dx2dx1 * (0);
J[2 * j + 0][12] = fx * dx2dx1 * (-x1 * x);
J[2 * j + 0][13] = fx * dx2dx1 * (-x1 * y);
J[2 * j + 0][14] = fx * dx2dx1 * (x1);
e[2 * j + 0] = x3 - pImagePoints[idx].x;
J[2 * j + 1][0] = 0;
J[2 * j + 1][1] = y2;
J[2 * j + 1][2] = 0;
J[2 * j + 1][3] = 1;
J[2 * j + 1][4] = dy3dy2 * y1 * square2;
J[2 * j + 1][5] = dy3dy2 * y1 * square4;
J[2 * j + 1][6] = fy * dy2dy1 * (0);
J[2 * j + 1][7] = fy * dy2dy1 * (0);
J[2 * j + 1][8] = fy * dy2dy1 * (0);
J[2 * j + 1][9] = fy * dy2dy1 * (x);
J[2 * j + 1][10] = fy * dy2dy1 * (y);
J[2 * j + 1][11] = fy * dy2dy1 * (1);
J[2 * j + 1][12] = fy * dy2dy1 * (-y1 * x);
J[2 * j + 1][13] = fy * dy2dy1 * (-y1 * y);
J[2 * j + 1][14] = fy * dy2dy1 * (y1);
e[2 * j + 1] = y3 - pImagePoints[idx].y;
}
cvSolve( &_pt1, &_pt2, &_delt, CV_SVD );
memcpy(filterDeltTemp,filterDelt,sizeof(filterDelt));
memcpy(paramTemp,param,sizeof(param));
for(j = 0;j < 15;j++)
{
filterDelt[j] = 0.8f * filterDelt[j] + 0.2f * delt[j];
}
for(j = 0;j < 15;j++)
{
param[j] = param[j] - 0.2f * filterDelt[j];
}
gErr = getErrCameraParams(pObjectPoints,pImagePoints,param,(width * hight));
if(gErr > lastErr)
{
memcpy(filterDelt,filterDeltTemp,sizeof(filterDelt));
memcpy(param,paramTemp,sizeof(param));
//break;
}
else
{
lastErr = gErr;
}
iner++;
if(iner > 5000)
{
n = imageCnt;
break;
}
if(gErr < 0.1f)
{
n = imageCnt;
break;
}
}
}
k[0] = k1 * 1.2f;
k[1] = k2 * 1.2f;
printf("best = %f,%f,%f,%f,%f,%f\n",fx,fy,cx,cy,k1,k2);
printf("ERR = %f\n",gErr);
if(gErr > 0.1f)
{
printf("calib image failed!\n\n\n\n");
}
A->pdata[0] = (float)fx;
A->pdata[1] = 0;
A->pdata[2] = (float)cx;
A->pdata[3] = 0;
A->pdata[4] = (float)fy;
A->pdata[5] = (float)cy;
A->pdata[6] = 0;
A->pdata[7] = 0;
A->pdata[8] = 1;
}
欢迎使用Markdown编辑器
你好！ 这是你第一次使用 Markdown编辑器 所展示的欢迎页。如果你想学习如何使用Markdown编辑器, 可以仔细阅读这篇文章，了解一下Markdown的基本语法知识。
新的改变
我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客：
- 全新的界面设计 ，将会带来全新的写作体验；
- 在创作中心设置你喜爱的代码高亮样式，Markdown 将代码片显示选择的高亮样式 进行展示；
- 增加了 图片拖拽 功能，你可以将本地的图片直接拖拽到编辑区域直接展示；
- 全新的 KaTeX数学公式 语法；
- 增加了支持甘特图的mermaid语法1 功能；
- 增加了 多屏幕编辑 Markdown文章功能；
- 增加了 焦点写作模式、预览模式、简洁写作模式、左右区域同步滚轮设置 等功能，功能按钮位于编辑区域与预览区域中间；
- 增加了 检查列表 功能。
功能快捷键
撤销：Ctrl/Command + Z
重做：Ctrl/Command + Y
加粗：Ctrl/Command + B
斜体：Ctrl/Command + I
标题：Ctrl/Command + Shift + H
无序列表：Ctrl/Command + Shift + U
有序列表：Ctrl/Command + Shift + O
检查列表：Ctrl/Command + Shift + C
插入代码：Ctrl/Command + Shift + K
插入链接：Ctrl/Command + Shift + L
插入图片：Ctrl/Command + Shift + G
合理的创建标题，有助于目录的生成
直接输入1次#，并按下space后，将生成1级标题。
输入2次#，并按下space后，将生成2级标题。
以此类推，我们支持6级标题。有助于使用TOC语法后生成一个完美的目录。
如何改变文本的样式
强调文本 强调文本
加粗文本 加粗文本
标记文本
删除文本
引用文本
H2O is是液体。
210 运算结果是 1024.
插入链接与图片
链接: link.
图片: 
带尺寸的图片:
居中的图片: 
居中并且带尺寸的图片:
当然，我们为了让用户更加便捷，我们增加了图片拖拽功能。
如何插入一段漂亮的代码片
去博客设置页面，选择一款你喜欢的代码片高亮样式，下面展示同样高亮的 代码片.
// An highlighted block
var foo = 'bar';
生成一个适合你的列表
- 项目
- 项目
- 项目
- 项目
- 项目1
- 项目2
- 项目3
- 计划任务
- 完成任务
创建一个表格
一个简单的表格是这么创建的：
| 项目 | Value |
|---|---|
| 电脑 | $1600 |
| 手机 | $12 |
| 导管 | $1 |
设定内容居中、居左、居右
使用:---------:居中
使用:----------居左
使用----------:居右
| 第一列 | 第二列 | 第三列 |
|---|---|---|
| 第一列文本居中 | 第二列文本居右 | 第三列文本居左 |
SmartyPants
SmartyPants将ASCII标点字符转换为“智能”印刷标点HTML实体。例如：
| TYPE | ASCII | HTML |
|---|---|---|
| Single backticks | 'Isn't this fun?' | ‘Isn’t this fun?’ |
| Quotes | "Isn't this fun?" | “Isn’t this fun?” |
| Dashes | -- is en-dash, --- is em-dash | – is en-dash, — is em-dash |
创建一个自定义列表
-
Markdown
- Text-to- HTML conversion tool Authors
- John
- Luke
如何创建一个注脚
一个具有注脚的文本。2
注释也是必不可少的
Markdown将文本转换为 HTML。
KaTeX数学公式
您可以使用渲染LaTeX数学表达式 KaTeX:
Gamma公式展示 Γ ( n ) = ( n − 1 ) ! ∀ n ∈ N \Gamma(n) = (n-1)!\quad\forall n\in\mathbb N Γ(n)=(n−1)!∀n∈N 是通过欧拉积分
Γ ( z ) = ∫ 0 ∞ t z − 1 e − t d t   . \Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,. Γ(z)=∫0∞tz−1e−tdt.
你可以找到更多关于的信息 LaTeX 数学表达式here.
新的甘特图功能，丰富你的文章
- 关于 甘特图 语法，参考 这儿,
UML 图表
可以使用UML图表进行渲染。 Mermaid. 例如下面产生的一个序列图：:
这将产生一个流程图。:
- 关于 Mermaid 语法，参考 这儿,
FLowchart流程图
我们依旧会支持flowchart的流程图：
- 关于 Flowchart流程图 语法，参考 这儿.
导出与导入
导出
如果你想尝试使用此编辑器, 你可以在此篇文章任意编辑。当你完成了一篇文章的写作, 在上方工具栏找到 文章导出 ，生成一个.md文件或者.html文件进行本地保存。
导入
如果你想加载一篇你写过的.md文件，在上方工具栏可以选择导入功能进行对应扩展名的文件导入，
继续你的创作。
注脚的解释 ↩︎
2575
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
