CvMat *Morphining::GetAffineMatrix(vector<Coordinate> coord1,vector<Coordinate>coord2,bool debug)
{
//b_temp:affine transformation,b_temp_inv:inverse of affine transform
CvMat *b_temp = cvCreateMat(3, 3, CV_32FC1);
CvMat *b_temp_inv = cvCreateMat(3, 3, CV_32FC1);
CvMat *B = cvCreateMat(3, 3, CV_32FC1);
CvMat *X = cvCreateMat(3, 3, CV_32FC1);
CvMat *X_Inv = cvCreateMat(3, 3, CV_32FC1);
// x1' = w11 * x1 + w12 * y1 + w13
// y1' = w21 * x1 + w22 * y1 + w13
// x2' = w11 * x2 + w12 * y2 + w13
// y2' = w21 * x2 + w22 * y2 + w13
// x3' = w11 * x3 + w12 * y3 + w13
// y3' = w21 * x3 + w22 * y3 + w13
// Ax = b x:left or right b:front face
for(int i = 0;i < 3;i++)
{
cvSetReal2D(B, 0, i, coord1[i].x);
cvSetReal2D(B, 1, i, coord1[i].y);
cvSetReal2D(B, 2, i, 1);
}
for(int i = 0;i < 3;i++)
{
cvSetReal2D(X, 0 , i, coord2[i].x);
cvSetReal2D(X, 1 , i, coord2[i].y);
cvSetReal2D(X, 2 , i, 1);
}
cvInv(X, X_Inv);
cvmMul(B,X_Inv,b_temp);
cvInv(b_temp,b_temp_inv);
if(debug)
{
cout << "b_temp:\n";
PrintMatrix(b_temp,3,3);
cout << "b_temp_inv:\n";
PrintMatrix(b_temp_inv,3,3);
}
cvReleaseMat(&B);
cvReleaseMat(&X);
cvReleaseMat(&X_Inv);
cvReleaseMat(&b_temp);
//return b_temp;
return b_temp_inv;
}
CvMat* Panoramic::GetAffineMatrix(vector<Coordinate> adjustCoord,vector<Coordinate> adjustedCoord)
{
//affineMatrix:affine transformation,b_temp_inv:inverse of affine transform
// x1' = w11 * x1 + w12 * y1 + w13
// y1' = w21 * x1 + w22 * y1 + w13
// x2' = w11 * x2 + w12 * y2 + w13
// y2' = w21 * x2 + w22 * y2 + w13
// x3' = w11 * x3 + w12 * y3 + w13
// y3' = w21 * x3 + w22 * y3 + w13
// Ax = b x:left or right b:front face
m_affineMatrix = cvCreateMat(3, 3, CV_32FC1);
CvMat *invAffineMatrix = cvCreateMat(3, 3, CV_32FC1);
CvMat *B = cvCreateMat(3, 3, CV_32FC1);
CvMat *X = cvCreateMat(3, 3, CV_32FC1);
CvMat *X_Inv = cvCreateMat(3, 3, CV_32FC1);
for(int i = 0;i < 3;i++)
{
cvSetReal2D(B, 0, i, adjustCoord[i].x);
cvSetReal2D(B, 1, i, adjustCoord[i].y);
cvSetReal2D(B, 2, i, 1);
cvSetReal2D(X, 0 , i, adjustedCoord[i].x);
cvSetReal2D(X, 1 , i, adjustedCoord[i].y);
cvSetReal2D(X, 2 , i, 1);
}
cvInv(X, X_Inv);
cvmMul(B,X_Inv,m_affineMatrix);
cvInv(m_affineMatrix,invAffineMatrix);
if(m_debug)
{
cout << "inverse affine matrix\n";
PrintMatrix(invAffineMatrix,3,3);
}
cvReleaseMat(&B);
cvReleaseMat(&X);
cvReleaseMat(&X_Inv);
cvReleaseMat(&m_affineMatrix);
return invAffineMatrix;
}
/*
* \param Intrinsic : 3 by 3 camera intrinsic matrix
* \param bazProjMat : 3 by 4 projection matrix = K[RT] obtained by calling "augment.GetProjectionMatrix(0)" function
* \arRT : 4 by 4 [R t] matrix for rendering
*/
void BazARTracker::GetARToolKitRTfromBAZARProjMat(CvMat *Intrinsic, CvMat *bazProjMat, CvMat *arRT)
{
int i, j;
CvMat *temps = cvCreateMat(3, 4, CV_64F);
// inverse matrix with intrinsic camera parameters
CvMat *invK = cvCloneMat(Intrinsic);
cvInv(Intrinsic, invK);
// multiply bazProjMat with inverted intrinsic camera parameter matrix -> extract intrinsic parameters
cvMatMul(invK, bazProjMat, temps);
cvmSetIdentity(arRT);
for(i=0; i<temps->rows; i++)
{
for(j=0; j<temps->cols;j++) cvmSet(arRT, i, j, cvmGet(temps, i,j));
}
cvReleaseMat(&temps);
cvReleaseMat(&invK);
}
int CUKF::UnscentedUpdate(int idf, int idx)
{
// Set up some variables
int dim = XX->rows;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
// int i,j,jj;
// int row = dim;
// int col = N;
// cvRepeat(XX, ss);
// // for (i=0; i<row; i++) for (j=0; j<col; j++)
// // ss->data.db[i*col+j] = XXA->data.db[i];
//
// col -= dim;
// for(i=0; i<row; i++) for(j=1, jj=0; j<col+1; j++, jj++)
// ss->data.db[i*col+j] += Ps->data.db[i*col+jj];
// for(i=0; i<row; i++) for(j=1+dim, jj=0; j<col+1+dim; j++, jj++)
// ss->data.db[i*col+j] -= Ps->data.db[i*col+jj];
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double *ssdb = ss->data.db;
double *Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
// Transform samples according to observation model to obtain the predicted observation samples
CvMat *zs = cvCreateMat(2, N, CV_64FC1);
// CvMat *base = cvCreateMat(3, EP, CV_64FC1);
int Nxv = 3;
int f = Nxv + idf*2 - 2;
// PrintCvMat(ss, "ss");
double *zsdb = zs->data.db;
for(j=0; j<N; j++)
{
double dx = ssdb[f*ss->cols+j] - ssdb[0*ss->cols+j];
double dy = ssdb[(f+1)*ss->cols+j] - ssdb[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
zsdb[0*zs->cols+j] = d;
zsdb[1*zs->cols+j] = atan2(dy,dx) - ssdb[2*ss->cols+j];
}
// zz = repvec(z,N);
// dz = feval(dzfunc, zz, zs); % compute correct residual
// zs = zz - dz; % offset zs from z according to correct residual
// PrintCvMat(zs, "zs");
CvMat *zz = cvCreateMat(2, N, CV_64FC1);
CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *zprev = cvCreateMat(2, 1, CV_64FC1);
zprev->data.db[0] = zf[idx].x;
zprev->data.db[1] = zf[idx].y;
cvRepeat(zprev, zz);
cvSub(zz, zs, dz);
double *dzdb = dz->data.db;
for(j=0; j<dz->cols; j++) dzdb[1*dz->cols+j] = PI2PI(dzdb[1*dz->cols+j]);
cvSub(zz, dz, zs);
// PrintCvMat(zs, "zs");
CvMat *zm = cvCreateMat(2, 1, CV_64FC1);
CvMat *dx = cvCreateMat(dim ,N, CV_64FC1);
// CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *repx = cvCreateMat(dim ,N, CV_64FC1);
CvMat *repzm = cvCreateMat(2, N, CV_64FC1);
CvMat *Pxz = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Pzz = cvCreateMat(2, 2, CV_64FC1);
CvMat *dxu = cvCreateMat(dim, 1, CV_64FC1);
CvMat *dxl = cvCreateMat(dim, N-1, CV_64FC1);
CvMat *dzu = cvCreateMat(2, 1, CV_64FC1);
CvMat *dzl = cvCreateMat(2, N-1, CV_64FC1);
CvMat *dztu = cvCreateMat(1, 2, CV_64FC1);
CvMat *dztl = cvCreateMat(N-1, 2, CV_64FC1);
CvMat *dxdzu = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dxdzl = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dzdzu = cvCreateMat(2, 2, CV_64FC1);
CvMat *dzdzl = cvCreateMat(2, 2, CV_64FC1);
double *zmdb = zm->data.db;
zmdb[0] = kappa*zs->data.db[0*zs->cols];
zmdb[1] = kappa*zs->data.db[1*zs->cols];
for(j=1; j<N; j++)
{
zm->data.db[0] += 0.5*zsdb[0*zs->cols+j];
zm->data.db[1] += 0.5*zsdb[1*zs->cols+j];
}
cvScale(zm, zm, 1/(double)(scale));
// Calculate predicted observation mean
cvRepeat(XX, repx);
cvRepeat(zm, repzm);
// PrintCvMat(ss, "ss");
// PrintCvMat(zs, "zs");
// PrintCvMat(repx, "repx");
// PrintCvMat(repzm, "repzm");
// Calculate observation covariance and the state-observation correlation matrix
cvSub(ss, repx, dx);
cvSub(zs, repzm, dz);
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
double *dxdb = dx->data.db;
double *dzudb = dzu->data.db;
double *dzldb = dzl->data.db;
double *dxudb = dxu->data.db;
double *dxldb = dxl->data.db;
// dx, dz를 1열과 나머지 열로 분할
for(i=0; i<2; i++) dzudb[i] = dzdb[i*dz->cols];
for(i=0; i<2; i++) for(j=1, jj=0; j<N; j++, jj++)
dzldb[i*dzl->cols+jj] = dzdb[i*dz->cols+j];
for(i=0; i<dim; i++) dxudb[i] = dxdb[i*dx->cols];
for(i=0; i<dim; i++) for(j=1, jj=0; j<N; j++, jj++)
dxldb[i*dxl->cols+jj] = dxdb[i*dx->cols+j];
cvT(dzu, dztu);
cvT(dzl, dztl);
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dztl, "dztl");
cvMatMul(dxu, dztu, dxdzu);
cvScale(dxdzu, dxdzu, 2*kappa);
cvMatMul(dxl, dztl, dxdzl);
cvAdd(dxdzu, dxdzl, Pxz);
cvScale(Pxz, Pxz, 1/(double)(2*scale));
cvMatMul(dzu, dztu, dzdzu);
cvScale(dzdzu, dzdzu, 2*kappa);
cvMatMul(dzl, dztl, dzdzl);
cvAdd(dzdzu, dzdzl, Pzz);
cvScale(Pzz, Pzz, 1/(double)(2*scale));
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
// PrintCvMat(dzu, "dzu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dzl, "dzl");
// PrintCvMat(dztl, "dztl");
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dxdzu, "dxdzu");
// PrintCvMat(dxdzl, "dxdzl");
// PrintCvMat(dzdzu, "dzdzu");
// PrintCvMat(dzdzl, "dzdzl");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Pzz, "Pzz");
// Compute Kalman gain
CvMat *S = cvCreateMat(2, 2, CV_64FC1);
CvMat *p = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sct = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sc = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sci = cvCreateMat(2, 2, CV_64FC1);
CvMat *Scit = cvCreateMat(2, 2, CV_64FC1);
CvMat *Wc = cvCreateMat(dim, 2, CV_64FC1);
CvMat *W = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Wz = cvCreateMat(dim, 1, CV_64FC1);
CvMat *Wct = cvCreateMat(2, dim, CV_64FC1);
CvMat *WcWc = cvCreateMat(dim, dim, CV_64FC1);
// % Compute Kalman gain
cvAdd(Pzz, R, S);
//cholesky decomposition이 실패하면 업데이트는 하지 않는다.
if(choldc(S, p, Sct) < 0)
{
TRACE("idf : %d\n", idf);
PrintCvMat(UD, "UD");
PrintCvMat(U, "U");
PrintCvMat(D, "D");
PrintCvMat(V, "V");
PrintCvMat(zprev, "zprev");
PrintCvMat(XX, "XX");
PrintCvMat(PX, "PX");
PrintCvMat(Ps, "Ps");
PrintCvMat(ss, "ss");
for(j=0; j<N; j++)
{
double dx = ss->data.db[f*ss->cols+j] - ss->data.db[0*ss->cols+j];
double dy = ss->data.db[(f+1)*ss->cols+j] - ss->data.db[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
double angle = atan2(dy,dx) - ss->data.db[2*ss->cols+j];
TRACE("%.4f %.4f %.4f %.4f %.4f %.4f %.4f\n", dx, dy, d2, d, angle, atan2(dy, dx), ss->data.db[2*ss->cols+j]);
}
PrintCvMat(zs, "zs");
PrintCvMat(zz, "zz");
PrintCvMat(zm, "zm");
PrintCvMat(dx, "dx");
PrintCvMat(dz, "dz");
PrintCvMat(Pxz, "Pxz");
PrintCvMat(Pzz, "Pzz");
PrintCvMat(R, "R");
return -1;
}
cvT(Sct, Sc);
cvInv(Sc, Sci);
cvT(Sci, Scit);
// PrintCvMat(S, "S");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sci, "Sci");
cvMatMul(Pxz, Sci, Wc);
// PrintCvMat(Wc, "Wc");
cvMatMul(Wc, Scit, W);
// PrintCvMat(W, "W");
cvSub(zprev, zm, zprev);
cvMatMul(W, zprev, Wz);
cvAdd(XX, Wz, XX);
cvT(Wc, Wct);
cvMatMul(Wc, Wct, WcWc);
cvSub(PX, WcWc, PX);
// PrintCvMat(Pzz, "Pzz");
// PrintCvMat(R, "R");
// PrintCvMat(S, "S");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sci, "Sci");
// PrintCvMat(Scit, "Scit");
// PrintCvMat(W, "W");
// PrintCvMat(zprev, "zprev");
// PrintCvMat(zm, "zm");
// PrintCvMat(Wc, "Wc");
// PrintCvMat(Wct, "Wct");
// PrintCvMat(WcWc, "WcWc");
// PrintCvMat(PX, "Px");
//
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&zs);
cvReleaseMat(&zz);
cvReleaseMat(&dz);
cvReleaseMat(&zprev);
cvReleaseMat(&zm);
cvReleaseMat(&dx);
cvReleaseMat(&repx);
cvReleaseMat(&repzm);
cvReleaseMat(&Pxz);
cvReleaseMat(&Pzz);
cvReleaseMat(&dxu);
cvReleaseMat(&dxl);
cvReleaseMat(&dzu);
cvReleaseMat(&dzl);
cvReleaseMat(&dztu);
cvReleaseMat(&dztl);
cvReleaseMat(&dxdzu);
cvReleaseMat(&dxdzl);
cvReleaseMat(&dzdzu);
cvReleaseMat(&dzdzl);
cvReleaseMat(&S);
cvReleaseMat(&p);
cvReleaseMat(&Sct);
cvReleaseMat(&Sc);
cvReleaseMat(&Sci);
cvReleaseMat(&Scit);
cvReleaseMat(&Wc);
cvReleaseMat(&W);
cvReleaseMat(&Wz);
cvReleaseMat(&Wct);
cvReleaseMat(&WcWc);
return 0;
}
CvMat* LineSegmentIntersectionDenormalizer::denormalize(const CvMat* nmodel, const CvMat* T1, const CvMat* T2) {
cvInv(T1, invT);
CvMat* model = matMul(invT, nmodel);
return model;
}
//coord1:校正用之點 coord2:被校正之點
//可執行四個以上的轉換
CvMat* Morphining::GetProjectionMatrix(vector<Coordinate> adjust,vector<Coordinate> adjusted,bool debug)
{
//spec://cvSetReal2D(cvMat,row,col);
if(m_numFeature < 4)
{
printf("Not enough tiles points to execute perspective transform!\n");
exit(0);
}
//Element of PerspectiveTransformation
CvMat *b = cvCreateMat( 8 , 1 , CV_32FC1);
CvMat *MUL_8_2N = cvCreateMat( 8 , 2*m_numFeature , CV_32FC1);
CvMat *b_temp = cvCreateMat( 3 , 3 , CV_32FC1);
CvMat *b_temp_inv = cvCreateMat( 3 , 3 , CV_32FC1);
//Size:2n*1
for(int i = 0;i < m_numFeature;i++)
{
cvSetReal2D(U, i, 0, adjusted[i].x);
cvSetReal2D(U, i + m_numFeature, 0, adjusted[i].y);
}
//Size:2n*8
for(int i = 0; i < m_numFeature;i++)
{
double col_6 = -1.0 * adjust[i].x * cvGetReal2D(U,i,0);
double col_7 = -1.0 * adjust[i].y * cvGetReal2D(U,i,0);
double col_6_pair = -1.0 * adjust[i].x * cvGetReal2D(U,i + m_numFeature,0);
double col_7_pair = -1.0 * adjust[i].y * cvGetReal2D(U,i + m_numFeature,0);
cvSetReal2D(W, i, 0, adjust[i].x);
cvSetReal2D(W, i, 1, adjust[i].y);
cvSetReal2D(W, i, 2, 1);
cvSetReal2D(W, i, 3, 0);
cvSetReal2D(W, i, 4, 0);
cvSetReal2D(W, i, 5, 0);
cvSetReal2D(W, i, 6, col_6);
cvSetReal2D(W, i, 7, col_7);
cvSetReal2D(W, i + m_numFeature, 0, 0);
cvSetReal2D(W, i + m_numFeature, 1, 0);
cvSetReal2D(W, i + m_numFeature, 2, 0);
cvSetReal2D(W, i + m_numFeature, 3, adjust[i].x);
cvSetReal2D(W, i + m_numFeature, 4, adjust[i].y);
cvSetReal2D(W, i + m_numFeature, 5, 1);
cvSetReal2D(W, i + m_numFeature, 6, col_6_pair);
cvSetReal2D(W, i + m_numFeature, 7, col_7_pair);
}
if(debug)
{
std::cout << "U:\n";
PrintMatrix(U, 2*m_numFeature,1,1);
std::cout << "W:\n";
PrintMatrix(W, 2*m_numFeature,8,1);
}
if(W->cols == W->rows )cvInv(W,MUL_8_2N);
else { //pseudo inverse
cvTranspose(W,T);
cvmMul(T,W,MulResult);
cvInv(MulResult,Inv);
cvmMul(Inv,T,MUL_8_2N);
/*printf("T\n");
PrintMatrix(T, 8,2*m_numFeature,1);*/
/*printf("MulResult\n");
PrintMatrix(MulResult, 8,2*m_numFeature,1);*/
/*printf("Inv\n");
PrintMatrix(Inv,2*m_numFeature, 8,1);*/
}
cvmMul(MUL_8_2N,U,b);
cvSetReal2D(b_temp, 0, 0, cvGetReal2D(b,0,0)); cvSetReal2D(b_temp, 0, 1, cvGetReal2D(b,1,0)); cvSetReal2D(b_temp, 0, 2, cvGetReal2D(b,2,0));
cvSetReal2D(b_temp, 1, 0, cvGetReal2D(b,3,0)); cvSetReal2D(b_temp, 1, 1, cvGetReal2D(b,4,0)); cvSetReal2D(b_temp, 1, 2, cvGetReal2D(b,5,0));
cvSetReal2D(b_temp, 2, 0, cvGetReal2D(b,6,0)); cvSetReal2D(b_temp, 2, 1, cvGetReal2D(b,7,0)); cvSetReal2D(b_temp, 2, 2, 1);
cvInv(b_temp,b_temp_inv);
if(debug)
{
std::cout << "b_temp\n";
PrintMatrix(b_temp,3, 3,1);
std::cout << "b_temp_inv\n";
PrintMatrix(b_temp_inv,3, 3,1);
std::cout << "......................................................................\n";
std::cout << "......................................................................\n";
}
cvReleaseMat(&b);
cvReleaseMat(&MUL_8_2N);
cvReleaseMat(&b_temp);
//return b_temp;
return b_temp_inv;
}
ofxPlane ofxPlane::bestFitPlaneEquation(int n, ofPoint pts[]){
float sum_i_x = 0;
float sum_i_xx = 0;
float sum_i_xy = 0;
float sum_i_yy = 0;
float sum_i_y = 0;
float sum_i_xz = 0;
float sum_i_yz = 0;
float sum_i_z = 0;
for(int i = 0; i < n; i++){
sum_i_x += pts[i].x;
sum_i_xx += pts[i].x * pts[i].x;
sum_i_xy += pts[i].x * pts[i].y;
sum_i_yy += pts[i].y * pts[i].y;
sum_i_y += pts[i].y;
sum_i_xz += pts[i].x * pts[i].z;
sum_i_yz += pts[i].y * pts[i].z;
sum_i_z += pts[i].z;
}
/*
If you have n data points (x[i], y[i], z[i]), compute the 3x3 symmetric matrix A whose entries are:
sum_i x[i]*x[i], sum_i x[i]*y[i], sum_i x[i]
sum_i x[i]*y[i], sum_i y[i]*y[i], sum_i y[i]
sum_i x[i], sum_i y[i], n
*/
CvMat* A = cvCreateMat( 3, 3, CV_32FC1 );
CvMat* invA = cvCreateMat( 3, 3, CV_32FC1 );
CV_MAT_ELEM(*A,float,0,0) = sum_i_xx; CV_MAT_ELEM(*A,float,0,1) = sum_i_xy; CV_MAT_ELEM(*A,float,0,2) = sum_i_x;
CV_MAT_ELEM(*A,float,1,0) = sum_i_xy; CV_MAT_ELEM(*A,float,1,1) = sum_i_yy; CV_MAT_ELEM(*A,float,1,2) = sum_i_y;
CV_MAT_ELEM(*A,float,2,0) = sum_i_x; CV_MAT_ELEM(*A,float,2,1) = sum_i_y; CV_MAT_ELEM(*A,float,2,2) = (float)n;
/*
Also compute the 3 element vector b:
{sum_i x[i]*z[i], sum_i y[i]*z[i], sum_i z[i]}
*/
CvMat* b = cvCreateMat( 3,1, CV_32FC1 );
CV_MAT_ELEM(*b,float,0,0) = sum_i_xz; CV_MAT_ELEM(*b,float,1,0) = sum_i_yz; CV_MAT_ELEM(*b,float,2,0) = sum_i_z;
CvMat* x = cvCreateMat(3,1,CV_32FC1);
cvInv(A,invA);
cvMatMul(invA,b,x);
//cvSolve(&A, &b, &x, CV_LU); // solve (Ax=b) for x
float planeA = CV_MAT_ELEM(*x,float,0,0);
float planeB = CV_MAT_ELEM(*x,float,1,0);
float planeC = -1;
float planeD = -1*CV_MAT_ELEM(*x,float,2,0);
float z = (-planeD/planeC);
ofPoint pointRes = ofPoint(0,0,z);
ofxPlane planeRes;
ofVec3f normalRes = ofVec3f(planeA,planeB,planeC);
normalRes.normalize();
/*
Then solve Ax = b for the given A and b. The three components of the solution vector are the
coefficients to the least-square fit plane {a,b,c}.
Note that this is the "ordinary least squares" fit, which is appropriate only when z is expected to
be a linear function of x and y. If you are looking more generally for a "best fit plane" in 3-space,
you may want to learn about "geometric" least squares.
Note also that this will fail if your points are in a line, as your example points are.
*/
cvReleaseMat(&A);
cvReleaseMat(&invA);
cvReleaseMat(&b);
cvReleaseMat(&x);
return ofxPlane(pointRes,normalRes);
}
