OpenCVを使って最小二乗法で解く

//ホモグラフィを作成していく
double a1[75];//25*3
double b1[25];//25*1
double c1[3];//結果3
CvMat Ma, Mb, Mc ,Md ,Me;
cvInitMatHeader( &Ma, 25, 3, CV_64FC1, a1 );
cvInitMatHeader( &Mb, 25, 1, CV_64FC1, b1 );
cvInitMatHeader( &Mc, 3, 1, CV_64FC1, c1 );

//1行目のホモグラフィを求める
//for(i=0;i<25;i++){
for(i=0;i<25;i++){
a1[i*3]=displayPX[i];
a1[i*3+1]=displayPY[i];
a1[i*3+2]=1;
b1[i]=cameraPX[i];

}
cvSolve(&Ma,&Mb,&Mc,CV_SVD);
homographyP[0]=c1[0];
homographyP[1]=c1[1];
homographyP[2]=c1[2];

//2行目のホモグラフィを求める
for(i=0;i<25;i++){
a1[i*3]=displayPX[i];
a1[i*3+1]=displayPY[i];
a1[i*3+2]=1;
b1[i]=cameraPY[i];
}
cvSolve(&Ma,&Mb,&Mc,CV_SVD);
homographyP[3]=c1[0];
homographyP[4]=c1[1];
homographyP[5]=c1[2];

//3行目のホモグラフィを求める
for(i=0;i<25;i++){
a1[i*3]=displayPX[i];
a1[i*3+1]=displayPY[i];
a1[i*3+2]=1;
b1[i]=1;
}
//特異値分解を使って最小二乗法で解く
cvSolve(&Ma,&Mb,&Mc,CV_SVD);
homographyP[6]=c1[0];
homographyP[7]=c1[1];
homographyP[8]=c1[2];

//逆行列を求める
cvInitMatHeader( &Md, 3, 3, CV_64FC1, homographyP );
cvInitMatHeader( &Me, 3, 3, CV_64FC1, homographyRP );
cvInvert(&Md,&Me,CV_SVD);