/** Computes the optical center of camera from camera projection matrix (http://en.wikipedia.org/wiki/Camera_matrix )
* @param pM : Camera projection matrix (3x4).
* @return : Optical center of camera.
*/
Point3D Utility::getOpticalCenter( CvMat* pM )
{
CvMat *A = cvCreateMat(3, 3, CV_64FC1);
CvMat *Ainv = cvCreateMat(3, 3, CV_64FC1);
CvMat *b = cvCreateMat(3, 1, CV_64FC1);
for(int i=0; i<3; ++i)
{
for(int j=0; j<3; ++j)
cvmSet(A, i, j, cvmGet(pM,i,j));
cvmSet(b, i, 0, cvmGet(pM, i,3));
}
cvInvert(A, Ainv);
CvMat *oc = cvCreateMat(3, 1, CV_64FC1);
cvMatMul(Ainv, b, oc);
Point3D toRet;
toRet.x = -1 * cvmGet(oc, 0, 0); //NULL SPACE OF MATRIX pM
toRet.y = -1 * cvmGet(oc, 1, 0);
toRet.z = -1 * cvmGet(oc, 2, 0);
cvReleaseMat(&A);
cvReleaseMat(&Ainv);
cvReleaseMat(&b);
cvReleaseMat(&oc);
return toRet;
}
int Matrix::pseudoinverse(Matrix* matA)
{
//Comprobaciones previas
//To do
//Pinv calulation
CvMat* pinvAux=cvCreateMat(Matrix::numFilas,Matrix::numColumnas,CV_32FC1);
CvMat* sourceAux=cvCreateMat(Matrix::numFilas,Matrix::numColumnas,CV_32FC1);
for(int fila=1;fila<=numFilas;fila++)
{
for(int columna=1;columna<=numColumnas;columna++)
{
//printf("%f\n",sourceAux->data.fl[1]);
sourceAux->data.fl[(fila-1)*numColumnas+columna-1]=matA->getValueData(fila,columna);
//printf("%f\n",sourceAux->data.fl[(fila-1)*numColumnas+columna-1]);
}
}
cvInvert(sourceAux,pinvAux,CV_SVD);
for(int fila=1;fila<=numFilas;fila++)
{
for(int columna=1;columna<=numColumnas;columna++)
{
Matrix::setValueData(pinvAux->data.fl[(fila-1)*numColumnas+columna-1],fila,columna);
}
}
//End
return 1;
}
void glbScreenPoint2GlobePoint(GlbPoint3d p1, GlbRotmat r, GlbPoint3d &p2)
{
CvMat *GlbRotmat = cvCreateMat(3, 3, CV_32FC1);
GlbRotmat->data.fl[0] = r.r11;
GlbRotmat->data.fl[1] = r.r12;
GlbRotmat->data.fl[2] = r.r13;
GlbRotmat->data.fl[3] = r.r21;
GlbRotmat->data.fl[4] = r.r22;
GlbRotmat->data.fl[5] = r.r23;
GlbRotmat->data.fl[6] = r.r31;
GlbRotmat->data.fl[7] = r.r32;
GlbRotmat->data.fl[8] = r.r33;
cvInvert( GlbRotmat, GlbRotmat);
r.r11 = GlbRotmat->data.fl[0] ;
r.r12 = GlbRotmat->data.fl[1];
r.r13 = GlbRotmat->data.fl[2];
r.r21 = GlbRotmat->data.fl[3];
r.r22 = GlbRotmat->data.fl[4];
r.r23 = GlbRotmat->data.fl[5];
r.r31 = GlbRotmat->data.fl[6];
r.r32 = GlbRotmat->data.fl[7];
r.r33 = GlbRotmat->data.fl[8];
glbGlobePoint2ScreenPoint(p1, r, p2);
cvReleaseMat(&GlbRotmat);
}
/*IplImage* HomographyCalculationThread::rectifyImage(IplImage* inImage, IplImage* outImage,double oldFirstApexX,double oldFirstApexY,double width,double height)
{
double x1 = 0;
double x2 = 0;
double x3 = 0;
double y_position = 0;
double x_position = 0;
uchar* data = (uchar *)inImage->imageData;
uchar* dataOut = (uchar *)outImage->imageData;
for(int row=0;row<height;row++) {
for(int col=0;col<width;col++) {
x1 = cvmGet(mHMatrix,0,0) * col + cvmGet(mHMatrix,0,1) * row + cvmGet(mHMatrix,0,2);
x2 = cvmGet(mHMatrix,1,0) * col + cvmGet(mHMatrix,1,1) * row + cvmGet(mHMatrix,1,2);
x3 = cvmGet(mHMatrix,2,0) * col + cvmGet(mHMatrix,2,1) * row + 1;
y_position = x2/x3 + oldFirstApexY;
if(inImage->height < y_position) {
y_position = (inImage->height-1);
}
x_position = x1/x3 + oldFirstApexX;
if(inImage->width < x_position) {
x_position = (inImage->width-1);
}
int temp_y = (int)y_position;
int temp_x = (int)x_position;
if(dataOut!=NULL && data!=NULL) {
dataOut[row*outImage->widthStep+col*outImage->nChannels] = data[temp_y*inImage->widthStep+temp_x*inImage->nChannels];
dataOut[row*outImage->widthStep+col*outImage->nChannels+1] = data[temp_y*inImage->widthStep+temp_x*inImage->nChannels+1];
dataOut[row*outImage->widthStep+col*outImage->nChannels+2] = data[temp_y*inImage->widthStep+temp_x*inImage->nChannels+2];
}
}
}
cvReleaseMat(&mHMatrix);
return outImage;
}
*/
void HomographyCalculationThread::correctHomographicMatrix(IplImage* inImage,CvMat* invH,double lastPictureApexX,double lastPictureApexY,double width,double height)
{
CvMat *hCoeff = cvCreateMat(3,3,CV_32FC1);
CvMat* multipleMat = cvCreateMat(3,3,CV_32FC1);
double old_height = inImage->height;
double y_position = lastPictureApexY;
double x_position = lastPictureApexX;
for (int i=1;i<10;i++) {
double x1 = cvmGet(invH,0,0) * (x_position) + cvmGet(invH,0,1) * (y_position) + cvmGet(invH,0,2);
double x2 = cvmGet(invH,1,0) * (x_position) + cvmGet(invH,1,1) * (y_position) + cvmGet(invH,1,2);
double x3 = cvmGet(invH,2,0) * (x_position) + cvmGet(invH,2,1) * (y_position) + 1;
x1 = x1/x3;
x2 = x2/x3;
double H_coeff = ((x1/width)+(x2/height))/2 - 0.01;
for(int coeffRow=0;coeffRow<3;coeffRow++) {
for(int coeffCol=0;coeffCol<3;coeffCol++) {
cvmSet(hCoeff,coeffRow,coeffCol,H_coeff);
cvmSet(multipleMat,coeffRow,coeffCol,cvmGet(mHMatrix,coeffRow,coeffCol));
}
}
cvMul(multipleMat,hCoeff,mHMatrix);
cvInvert(mHMatrix,invH);
}
cvReleaseMat(&multipleMat);
cvReleaseMat(&hCoeff);
}
//! Performs one step of extremum interpolation.
void interpolateStep(int r, int c, ResponseLayer *t, ResponseLayer *m, ResponseLayer *b, double* xi, double* xr, double* xc )
//void interpolateStep()
{
CvMat* dD, * H, * H_inv, X;
double x[3] = { 0 };
dD = deriv3D( r, c, t, m, b );
H = hessian3D( r, c, t, m, b );
H_inv = CreateMat( 3, 3, CV_64FC1 );
cvInvert( H, H_inv, CV_SVD ); // incomplete check after invert() => CreateSVD()
//cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
InitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP ); //check
cvGEMM( H_inv, dD, -1, NULL, 0, &X, 0 ); //incomplete
free(&dD);
free(&H);
free(&H_inv);
//cvReleaseMat( &dD );
//cvReleaseMat( &H );
//cvReleaseMat( &H_inv );
*xi = x[2];
*xr = x[1];
*xc = x[0];
}
Point2D getOptFlow(IplImage* currentFrame,Point2D p,IplImage* preFrame)
{
Point2D temp;
double b[2];
b[0]=0;b[1]=0;
double M11=0,M12=0,M22=0;
for(int i = -OPTICAL_FLOW_POINT_AREA/2; i < OPTICAL_FLOW_POINT_AREA/2; i++)
{
for (int j = -OPTICAL_FLOW_POINT_AREA/2;j < OPTICAL_FLOW_POINT_AREA/2;j++)
{
temp = partial(currentFrame,Point2D(p.row+i,p.col+j));
M11 += temp.dcol*temp.dcol;
M12 += temp.dcol*temp.drow;
M22 += temp.drow*temp.drow;
b[0] += temp.dcol*(pixval8U(currentFrame,p.row+i,p.col+j)-pixval8U(preFrame,p.row+i,p.col+j));
b[1] += temp.drow*(pixval8U(currentFrame,p.row+i,p.col+j)-pixval8U(preFrame,p.row+i,p.col+j));
}
}
double a[] = {M11,M12,M12,M22};
CvMat M=cvMat(2, 2, CV_64FC1, a);
CvMat *Mi = cvCloneMat(&M);
cvInvert(&M,Mi,CV_SVD);
temp.col=0;
temp.row=0;
b[0] = -b[0];
b[1] = -b[1];
CvMat Mb = cvMat(2,1,CV_64FC1,b);
CvMat *Mr = cvCloneMat(&Mb);
cvMatMul( Mi, &Mb, Mr);
double vy = (cvmGet(Mr,1,0));
double vx = (cvmGet(Mr,0,0));
return (Point2D(vy,vx));
}
Ematrix Ematrix::inverse() const{
int r,c;
if (rows != cols) { printf("INV: Matrix must be square\n");}
assert(rows == cols);
Ematrix mat(rows,cols,reservedRows,reservedCols);
CvMat* ocvMat = cvCreateMat(rows,cols,CV_32FC1);
int id=0;
for (r = 0; r<rows; r++){
for ( c = 0; c<cols; c++)
cvmSet(ocvMat,r,c, val[id+c]);
id += reservedCols;
}
CvMat* ocvMatInv = cvCreateMat(rows,cols,CV_32FC1);
cvInvert(ocvMat, ocvMatInv, CV_LU );
id = 0;
for (r = 0; r<rows; r++){
for (c = 0; c<cols; c++)
mat.val[id+c] = (float)cvmGet(ocvMatInv,r,c);
id += reservedCols;
}
cvReleaseMat(&ocvMat);
cvReleaseMat(&ocvMatInv);
return mat;
}
//============================================================================
void AAM_IC::CalcHessian(CvMat* H, const CvMat* SD)
{
CvMat* HH = cvCreateMat(H->rows, H->cols, CV_64FC1);
cvMulTransposed(SD, HH, 0);// Equation (65)
cvInvert(HH, H, CV_SVD);
cvReleaseMat(&HH);
}
void blur_function(const IplImage *latent_image, IplImage *blur_image, const CvMat *hom1, const CvMat *hom2)
{
const int T = 20;
const int tau = 10;
CvMat *id_mat = cvCreateMat(3, 3, CV_32FC1);
cvSetIdentity(id_mat, cvRealScalar(1));
CvMat *invhom1 = cvCreateMat(3, 3, CV_32FC1);
cvInvert(hom1, invhom1, CV_LU);
CvMat *h1 = cvCreateMat(3, 3, CV_32FC1);
CvMat *h2 = cvCreateMat(3, 3, CV_32FC1);
CvSize size = cvSize(latent_image->width, latent_image->height);
IplImage *temp = cvCreateImage(size, latent_image->depth, latent_image->nChannels);
IplImage *blur = cvCreateImage(size, IPL_DEPTH_32F, latent_image->nChannels);
cvSetZero(blur);
for (int i = 1; i <= tau; ++i)
{
cvAddWeighted(id_mat, (double)(T-i)/T, invhom1, (double)i/T, 0, h1);
cvAddWeighted(id_mat, (double)(T-i)/T, hom2, (double)i/T, 0, h2);
cvWarpPerspective(latent_image, temp, h1, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
cvAdd(blur, temp, blur, NULL);
cvWarpPerspective(latent_image, temp, h2, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
cvAdd(blur, temp, blur, NULL);
}
cvAdd(blur, latent_image, blur, NULL);
cvConvertScale(blur, blur_image, 1.0/(2*tau+1), 0);
cvReleaseMat(&id_mat);
cvReleaseMat(&invhom1);
cvReleaseMat(&h1);
cvReleaseMat(&h2);
cvReleaseImage(&temp);
cvReleaseImage(&blur);
}
//============================================================================
void AAM_Basic::CalcGradientMatrix(const CvMat* CParams,
const CvMat* vCDisps,
const CvMat* vPoseDisps,
const std::vector<AAM_Shape>& AllShapes,
const std::vector<IplImage*>& AllImages)
{
int npixels = __cam.__texture.nPixels();
int np = __cam.nModes();
// do model parameter experiments
{
printf("Calculating parameter gradient matrix...\n");
CvMat* GParam = cvCreateMat(np, npixels, CV_64FC1);cvZero(GParam);
CvMat* GtG = cvCreateMat(np, np, CV_64FC1);
CvMat* GtGInv = cvCreateMat(np, np, CV_64FC1);
// estimate Rc
EstCParamGradientMatrix(GParam, CParams, AllShapes, AllImages, vCDisps);
__Rc = cvCreateMat(np, npixels, CV_64FC1);
cvGEMM(GParam, GParam, 1, NULL, 0, GtG, CV_GEMM_B_T);
cvInvert(GtG, GtGInv, CV_SVD );
cvMatMul(GtGInv, GParam, __Rc);
cvReleaseMat(&GtG);
cvReleaseMat(&GtGInv);
cvReleaseMat(&GParam);
}
// do pose experiments, this is for global shape normalization
{
printf("Calculating pose gradient matrix...\n");
CvMat* GtG = cvCreateMat(4, 4, CV_64FC1);
CvMat* GtGInv = cvCreateMat(4, 4, CV_64FC1);
CvMat* GPose = cvCreateMat(4, npixels, CV_64FC1); cvZero(GPose);
// estimate Rt
EstPoseGradientMatrix(GPose, CParams, AllShapes, AllImages, vPoseDisps);
__Rq = cvCreateMat(4, npixels, CV_64FC1);
cvGEMM(GPose, GPose, 1, NULL, 0, GtG, CV_GEMM_B_T);
cvInvert(GtG, GtGInv, CV_SVD);
cvMatMul(GtGInv, GPose, __Rq);
cvReleaseMat(&GtG);
cvReleaseMat(&GtGInv);
cvReleaseMat(&GPose);
}
}
void KalmanSensor::update_K(CvMat *P_pred) {
// K = P * trans(H) * inv(H*P*trans(H) + R)
cvTranspose(H, H_trans);
cvMatMul(P_pred, H_trans, K);
cvMatMul(H, K, R_tmp);
cvScaleAdd(R_tmp, cvScalar(1), R, R_tmp);
cvInvert(R_tmp, R_tmp);
cvMatMul(H_trans, R_tmp, K);
cvMatMul(P_pred, K, K);
}
int main (int argc, char **argv)
{
int i, j;
int nrow = 3;
int ncol = 3;
CvMat *src, *dst, *mul;
double det;
CvRNG rng = cvRNG (time (NULL)); /* 乱数の初期化 */
// (1) 行列のメモリ確保
src = cvCreateMat (nrow, ncol, CV_32FC1);
dst = cvCreateMat (ncol, nrow, CV_32FC1);
mul = cvCreateMat (nrow, nrow, CV_32FC1);
// (2) 行列srcに乱数を代入
printf ("src\n");
cvmSet (src, 0, 0, 1);
for (i = 0; i < src->rows; i++)
{
for (j = 0; j < src->cols; j++)
{
cvmSet (src, i, j, cvRandReal (&rng));
printf ("% lf\t", cvmGet (src, i, j));
}
printf ("\n");
}
// (3) 行列srcの逆行列を求めて，行列dstに代入
det = cvInvert (src, dst, CV_SVD);
// (4) 行列srcの行列式を表示
printf ("det(src)=%lf\n", det);
// (5) 行列dstの表示
printf ("dst\n");
for (i = 0; i < dst->rows; i++)
{
for (j = 0; j < dst->cols; j++)
{
printf ("% lf\t", cvmGet (dst, i, j));
}
printf ("\n");
}
// (6) 行列srcとdstの積を計算して確認
cvMatMul (src, dst, mul);
printf ("mul\n");
for (i = 0; i < mul->rows; i++)
{
for (j = 0; j < mul->cols; j++)
{
printf ("% lf\t", cvmGet (mul, i, j));
}
printf ("\n");
}
// (7) 行列のメモリを開放
cvReleaseMat (&src);
cvReleaseMat (&dst);
cvReleaseMat (&mul);
return 0;
}
PerspectiveTransform * PerspectiveTransform::accumulateInverse(const Transform * T2) const
{
PerspectiveTransform * m = new PerspectiveTransform;
PerspectiveTransform T2_inv(*dynamic_cast<const PerspectiveTransform *>(T2)); //make a copy
cvInvert(T2_inv, T2_inv);
cvMatMul(T2_inv, *this, *m); //don't need casts as opencv will check dims agree.
matMul(T2_inv.data.db, data.db, m->data.db, 3, 3, 3);
return m;
}
CvMat* HomographyCalculationThread::calculateHomographicMatrix(double newPictureApexX[],double newPictureApexY[],double pictureApexXPosition[],double pictureApexYPosition[])
{
CvMat* mmat = cvCreateMat(3,3,CV_32FC1);
CvMat* a = cvCreateMat(POINTS*2,9,CV_32FC1);
for(int count=1;count<POINTS+1;count++) {
cvmSet(a,2*count-2,0,newPictureApexX[count-1]);
cvmSet(a,2*count-2,1,newPictureApexY[count-1]);
cvmSet(a,2*count-2,2,1);
cvmSet(a,2*count-2,3,0);
cvmSet(a,2*count-2,4,0);
cvmSet(a,2*count-2,5,0);
cvmSet(a,2*count-2,6,(-newPictureApexX[count-1]*pictureApexXPosition[count-1]));
cvmSet(a,2*count-2,7,(-pictureApexXPosition[count-1]*newPictureApexY[count-1]));
cvmSet(a,2*count-2,8,-pictureApexXPosition[count-1]);
cvmSet(a,2*count-1,0,0);
cvmSet(a,2*count-1,1,0);
cvmSet(a,2*count-1,2,0);
cvmSet(a,2*count-1,3,newPictureApexX[count-1]);
cvmSet(a,2*count-1,4,newPictureApexY[count-1]);
cvmSet(a,2*count-1,5,1);
cvmSet(a,2*count-1,6,(-newPictureApexX[count-1]*pictureApexYPosition[count-1]));
cvmSet(a,2*count-1,7,(-pictureApexYPosition[count-1]*newPictureApexY[count-1]));
cvmSet(a,2*count-1,8,-pictureApexYPosition[count-1]);
}
CvMat* U = cvCreateMat(8,8,CV_32FC1);
CvMat* D = cvCreateMat(8,9,CV_32FC1);
CvMat* V = cvCreateMat(9,9,CV_32FC1);
CvMat* V22 = cvCreateMat(3,3,CV_32FC1);
mHMatrix = cvCreateMat(3,3,CV_32FC1);
CvMat* invH = cvCreateMat(3,3,CV_32FC1);
cvSVD(a, D, U, V, CV_SVD_U_T|CV_SVD_V_T);
for(int a=0;a<3;a++) {
for(int b=0;b<3;b++) {
cvmSet(mmat,a,b,cvmGet(V,8,a*3+b));
cvmSet(V22,a,b,(1/cvmGet(V,8,4)));
}
}
cvMul(mmat,V22,mHMatrix);
cvInvert(mHMatrix,invH);
cvReleaseMat(&U);
cvReleaseMat(&D);
cvReleaseMat(&V);
cvReleaseMat(&V22);
cvReleaseMat(&a);
cvReleaseMat(&mmat);
return invH;
}
void ConformalResizing::Constrian(const ConstrainUnits& unit, CvMat*& M)
{
// Preprocess unit to make Matrix M less singular
double meanX(0), meanY(0);
for (int i = 0; i < unit.n; i++)
{
meanX += unit.pnts[i].x;
meanY += unit.pnts[i].y;
}
meanX /= unit.n;
meanY /= unit.n;
int n = unit.n * 2;
M = cvCreateMat(n, n, CV_64F);
CvMat* A = cvCreateMat(n, 4, CV_64F);
CvMat* Q = cvCreateMat(n, 4, CV_64F);
CvMat* P = cvCreateMat(4, 4, CV_64F);
// Initial A
cvZero(A);
for (int i = 0; i < unit.n; i++)
{
double x = unit.pnts[i].x - meanX;
double y = unit.pnts[i].y - meanY;
CV_MAT_ELEM(*A, double, 2*i, 0) = x;
CV_MAT_ELEM(*A, double, 2*i, 1) = -y;
CV_MAT_ELEM(*A, double, 2*i, 2) = 1;
CV_MAT_ELEM(*A, double, 2*i+1, 0) = y;
CV_MAT_ELEM(*A, double, 2*i+1, 1) = x;
CV_MAT_ELEM(*A, double, 2*i+1, 3) = 1;
}
cvMulTransposed(A, P, 1); // P = (A^T * A)
cvInvert(P, P, CV_SVD_SYM); // P = (A^T * A)^(-1)
cvMatMul(A, P, Q);
cvGEMM(Q, A, 1, NULL, 0, M, CV_GEMM_B_T);
// M = M - I
double* d = M->data.db;
for (int i = 0; i < n; i++, d += n+1)
{
*d -= 1;
}
cvReleaseMat(&A);
cvReleaseMat(&Q);
cvReleaseMat(&P);
}
double pkmGaussianMixtureModel::multinormalDistribution(const CvMat *pts, const CvMat *mean, const CvMat *covar)
{
int dimensions = 2;
// add a tiny bit because of small samples
CvMat *covarShifted = cvCreateMat(2, 2, CV_64FC1);
cvAddS( covar, cvScalarAll(0.001), covarShifted);
// calculate the determinant
double det = cvDet(covarShifted);
// invert covariance
CvMat *covarInverted = cvCreateMat(2, 2, CV_64FC1);
cvInvert(covarShifted, covarInverted);
double ff = (1.0/(2.0*(double)PI))*(pow(det,-0.5));
CvMat *centered = cvCreateMat(2, 1, CV_64FC1);
cvSub(pts, mean, centered);
CvMat *invxmean = cvCreateMat(2, 1, CV_64FC1);
//cvGEMM(covarInverted, centered, 1., NULL, 1., invxmean);
cvMatMul(covarInverted, centered, invxmean);
cvMul(centered, invxmean, invxmean);
CvScalar sum = cvSum(invxmean);
/*
printf("covar: %f %f %f %f\n", cvmGet(covar, 0, 0), cvmGet(covar, 0, 1), cvmGet(covar, 1, 0), cvmGet(covar, 1, 1));
printf("covarShifted: %f %f %f %f\n", cvmGet(covarShifted, 0, 0), cvmGet(covarShifted, 0, 1), cvmGet(covarShifted, 1, 0), cvmGet(covarShifted, 1, 1));
printf("det: %f\n", det);
printf("covarInverted: %f %f %f %f\n", cvmGet(covarInverted, 0, 0), cvmGet(covarInverted, 0, 1), cvmGet(covarInverted, 1, 0), cvmGet(covarShifted, 1, 1));
printf("ff: %f\n", ff);
printf("pts: %f %f)\n", cvmGet(pts, 0, 0), cvmGet(pts, 1, 0));
printf("mean: %f %f)\n", cvmGet(mean, 0, 0), cvmGet(mean, 1, 0));
printf("centered: %f %f)\n", cvmGet(centered, 0, 0), cvmGet(centered, 1, 0));
printf("invxmean: %f %f)\n", cvmGet(invxmean, 0, 0), cvmGet(invxmean, 1, 0));
printf("scalar: %f %f %f %f\n", sum.val[0], sum.val[1], sum.val[2], sum.val[3]);
*/
cvReleaseMat(&covarShifted);
cvReleaseMat(&covarInverted);
cvReleaseMat(¢ered);
cvReleaseMat(&invxmean);
return ff * exp(-0.5*sum.val[0]);
}
bool MixGaussian::AddGaussian(
CvMat * MeanMat,
CvMat * CovMat,
float Weight)
{
if(_nMixture == MAX_NUM_GAUSSIAN)
return false;
_MeanMat[_nMixture] = cvCloneMat(MeanMat);
_CovMat [_nMixture] = cvCloneMat(CovMat);
_CovMatI[_nMixture] = cvCloneMat(CovMat);
cvInvert(CovMat, _CovMatI[_nMixture]);
_Weight [_nMixture] = Weight;
_nMixture++;
return true;
}
AffineTransform * AffineTransform::accumulateInt(const Transform * T2, bool bInvert) const
{
AffineTransform * m = new AffineTransform;
// Just expand mat. multiplication to avoid converting to 3x3
const double * s1 = 0, * s2 = 0;
double invMatData[3*3]; //here so stays in scope when pointer copied
if(bInvert)
{
//Invert 3x3 mat
double T2MatData[3*3];
CvMat invMat = cvMat(3, 3, CV_64FC1, invMatData);
CvMat T2Mat = cvMat(3, 3, CV_64FC1, T2MatData);
const double * affineT2Data = dynamic_cast<const AffineTransform *>(T2)->data_;
for(int i=0; i<6; i++)
T2MatData[i] = affineT2Data[i];
//Last row of affine mat
T2MatData[6] = 0;
T2MatData[7] = 0;
T2MatData[8] = 1;
cvInvert(&T2Mat, &invMat);
s1 = invMatData;
s2 = this->data_;
}
else
{
s1 = this->data_;
s2 = dynamic_cast<const AffineTransform *>(T2)->data_;
}
for(int row=0; row<2; row++)
{
(*m)(row,0) = s1[0]*s2[0] + s1[1]*s2[0+3];
(*m)(row,1) = s1[0]*s2[1] + s1[1]*s2[1+3];
(*m)(row,2) = s1[0]*s2[2] + s1[1]*s2[2+3] + s1[2];
s1 += COLS; //skip to second
}
return m;
}
matrix matrix::inverse() const{
int r;
if (rows != cols) { printf("INV: Matrix must be square\n");}
assert(rows == cols);
matrix mat(rows,cols);
CvMat* ocvMat = cvCreateMat(rows,cols,CV_32FC1);
for (r = 0; r<rows; r++)
for (int c = 0; c<cols; c++)
cvmSet(ocvMat,r,c,get(r,c));
CvMat* ocvMatInv = cvCreateMat(rows,cols,CV_32FC1);
cvInvert(ocvMat, ocvMatInv, CV_LU );
for (r = 0; r<rows; r++)
for (int c = 0; c<cols; c++)
mat.set(r,c, (float)cvmGet(ocvMatInv,r,c));
cvReleaseMat(&ocvMat);
cvReleaseMat(&ocvMatInv);
return mat;
}
//! Performs one step of extremum interpolation.
void FastHessian::interpolateStep(int r, int c, ResponseLayer *t, ResponseLayer *m, ResponseLayer *b,
double* xi, double* xr, double* xc )
{
CvMat* dD, * H, * H_inv, X;
double x[3] = { 0 };
dD = deriv3D( r, c, t, m, b );
H = hessian3D( r, c, t, m, b );
H_inv = cvCreateMat( 3, 3, CV_64FC1 );
cvInvert( H, H_inv, CV_SVD );
cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
cvGEMM( H_inv, dD, -1, NULL, 0, &X, 0 );
cvReleaseMat( &dD );
cvReleaseMat( &H );
cvReleaseMat( &H_inv );
*xi = x[2];
*xr = x[1];
*xc = x[0];
}
void interp_step( IplImage*** dog_pyr, int octv, int intvl, int r, int c,
double* xi, double* xr, double* xc )
{
CvMat* dD, * H, * H_inv, X;
double x[3] = { 0 };
dD = deriv_3D( dog_pyr, octv, intvl, r, c );
H = hessian_3D( dog_pyr, octv, intvl, r, c );
H_inv = cvCreateMat( 3, 3, CV_64FC1 );
cvInvert( H, H_inv, CV_SVD );
cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
cvGEMM( H_inv, dD, -1, NULL, 0, &X, 0 );
cvReleaseMat( &dD );
cvReleaseMat( &H );
cvReleaseMat( &H_inv );
*xi = x[2];
*xr = x[1];
*xc = x[0];
}
static double luck_pixel(int x, int y, const CvMat *hom1, const CvMat *hom2)
{
CvMat *invhom1 = cvCreateMat(3, 3, CV_32FC1);
cvInvert(hom1, invhom1, CV_LU);
//inhom1到上一帧的映射变换
CvPoint2D32f src = cvPoint2D32f(x, y);
CvPoint2D32f d1, d2;
CvMat pt_src = cvMat(1, 1, CV_32FC2, &src);
CvMat pt_dst = cvMat(1, 1, CV_32FC2, &d1);
cvPerspectiveTransform(&pt_src, &pt_dst, invhom1);
//透视转换成上一帧
pt_dst = cvMat(1, 1, CV_32FC2, &d2);
cvPerspectiveTransform(&pt_src, &pt_dst, hom2);
//透视转换为下一帧
//得到d1和d2,为前后一帧相对应的点
double dis = (src.x-d1.x)*(src.x-d1.x)+(src.y-d1.y)*(src.y-d1.y);
dis += (src.x-d2.x)*(src.x-d2.x)+(src.y-d2.y)*(src.y-d2.y);
double luck = exp(-dis/(2*SIGMA_L*SIGMA_L));
cvReleaseMat(&invhom1);
return luck;
}
/*
Performs one step of extremum interpolation. Based on Eqn. (3) in Lowe's paper.
@param dog_pyr difference of Gaussians scale space pyramid
@param octv octave of scale space
@param intvl interval being interpolated
@param r row being interpolated
@param c column being interpolated
@param xi output as interpolated subpixel increment to interval
@param xr output as interpolated subpixel increment to row
@param xc output as interpolated subpixel increment to col
*/
static void interp_step( IplImage*** dog_pyr, int octv, int intvl, int r, int c,
double* xi, double* xr, double* xc )
{
CvMat* dD, * H, * H_inv, X;
double x[3] = { 0 };
//在DoG金字塔中计算某点的x方向、y方向以及尺度方向上的偏导数，结果存放在列向量dD中
dD = deriv_3D( dog_pyr, octv, intvl, r, c );
//在DoG金字塔中计算某点的3*3海森矩阵
H = hessian_3D( dog_pyr, octv, intvl, r, c );
H_inv = cvCreateMat( 3, 3, CV_64FC1 );//海森矩阵的逆阵
cvInvert( H, H_inv, CV_SVD );
cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
//X = - H^(-1) * dD，H的三个元素分别是x,y,σ方向上的偏移量(具体见SIFT算法说明)
cvGEMM( H_inv, dD, -1, NULL, 0, &X, 0 );
cvReleaseMat( &dD );
cvReleaseMat( &H );
cvReleaseMat( &H_inv );
*xi = x[2];//σ方向(层方向)偏移量
*xr = x[1];//y方向上偏移量
*xc = x[0];//x方向上偏移量
}
int opticaltri( CvMat * &clean_texture, int verts )
{
char * im1fname = "conhull-dirty-thresh.jpg";
char * im2fname = "conhull-clean-thresh.jpg";
int count = MAX_COUNT;
char * status;
CvPoint2D32f * source_points;
CvPoint2D32f * dest_points;
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
// count = opticalflow( im1fname, im2fname, source_points, dest_points, status );
count = findsiftpoints( "conhull-dirty.jpg", "conhull-clean.jpg", source_points, dest_points, status );
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 50;
int offset = 200;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
// delaunay_points[i] =
(cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total*2; i++ ) {
if((i == 0) || (i == total)) {
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
}
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
if(i < total)
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
else
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_RIGHT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
int is_corner = 0;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height - 1)) ||(curtri.points[j] == cvPoint(image1->width - 1,0)) ||(curtri.points[j] == cvPoint(image1->width - 1,image1->height - 1))) {
is_corner++;
}
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(is_corner < 3) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
/*
if((i == 3019) && (y == 1329) && (x > 2505) && (x < 2584)) {
printf("Range %d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ )
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
for(int k = 0; k < verts; k++) {
double x = clean_texture->data.fl[2*k+0];
double y = clean_texture->data.fl[2*k+1];
// check to see if we're inside this triangle
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
clean_texture->data.fl[2*k+0] = sourcex;
clean_texture->data.fl[2*k+1] = sourcey;
// cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
cvReleaseImage( &clean_nonthresh );
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
// draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
void MyMat::Inv(const MyMat& src)
{
cvInvert(src.m_data, m_data);
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
CvMat *savgolFilter(CvMat &z, double ra, double rb, double theta){
int d=2;
int k=1; // Always these, according to the matlab code (Leo)
ra = max(1.5,ra);
rb = max(1.5,rb);
double ira2 = 1.0/(ra*ra);
double irb2 = 1.0/(rb*rb);
double wr = floor(max(ra,rb));
double wd = 2*wr+1;
double sint = sin(theta);
double cost = cos(theta);
CvMat*xx= cvCreateMat(2*d+1, 1, CV_64FC1);
cvSetZero(xx);
CvMat *temp=cvCreateMat(2*d+1, 1, CV_64FC1);
for (int u=-wr;u<=wr;u++)
for (int v=-wr;v<=wr;v++) {
double ai=-u*sint+v*cost; //distance along major axis
double bi= u*cost+v*sint; //distance along minor axis
if (ai*ai*ira2+bi*bi*irb2 <= 1) {
cvSet(temp, cvScalar(ai));
cvSetReal2D(temp, 0, 0, 1.0);
double dTemp=1;
for (int i=0;i<2*d+1-1;i++) {
dTemp=dTemp*cvGetReal2D(temp,i+1,0);
cvSetReal2D(temp,i+1,0,dTemp);
}
cvAdd(xx,temp,xx);
}
}
cvReleaseMat(&temp);
CvMat *A=cvCreateMat(d+1,d+1,CV_64FC1);
for (int i=0;i<d+1;i++)
for (int j=i;j<=i+d;j++)
cvSetReal2D(A,j-i,i,cvGetReal2D(xx,j,0));
cvInvert(A,A,CV_LU);
CvMat *zz=cvCreateMat(wd,wd, CV_64FC1);
CvMat *yy=cvCreateMat(d+1,1, CV_64FC1);
CvMat *result=cvCreateMat(d+1,1, CV_64FC1);
CvMat *filt=cvCreateMat(wd,wd, CV_32FC1);
cvSetZero(filt);
for (int u=-wr;u<=wr;u++)
for (int v=-wr;v<=wr;v++) {
cvSetZero(zz);
cvSetReal2D(zz,v+wr,u+wr,1);
cvSetZero(yy);
double ai=-u*sint+v*cost; //distance along major axis
double bi= u*cost+v*sint; //distance along minor axis
if (ai*ai*ira2+bi*bi*irb2 <= 1) {
cvSet(yy, cvScalar(ai));
cvSetReal2D(yy, 0, 0, 1.0);
double dTemp=1;
for (int i=0;i<d+1-1;i++) {
dTemp=dTemp*cvGetReal2D(yy,i+1,0);
cvSetReal2D(yy,i+1,0,dTemp);
}
cvMatMul(A,yy,result);
cvSetReal2D(filt,v+wr,u+wr,cvGetReal2D(result,k-1,0));
}
}
cvReleaseMat(&zz);
cvReleaseMat(&yy);
cvReleaseMat(&xx);
cvReleaseMat(&A);
CvMat *ztemp= cvCreateMat(z.rows+filt->rows-1,z.cols+filt->cols-1,CV_32FC1);
cvCopyMakeBorder(&z,ztemp,cvPoint((filt->cols-1)/2,(filt->rows-1)/2),IPL_BORDER_CONSTANT);
CvMat *filteredtemp= cvCreateMat(ztemp->rows,ztemp->cols,CV_32FC1);
cvFilter2D(ztemp,filteredtemp,filt,cvPoint((filt->cols-1)/2,(filt->rows-1)/2));
CvMat *filtered = cvCreateMat(z.rows,z.cols,CV_32FC1);
cvGetSubRect(filteredtemp,filtered,cvRect((filt->cols-1)/2,(filt->rows-1)/2,z.cols,z.rows));
return filtered;
}
//추후 수정
void FkPaperKeyboard_TypeA::cornerVerification(IplImage* srcImage){
CvSize size = cvGetSize(srcImage);
IplImage* eigImage = cvCreateImage(size, IPL_DEPTH_8U,1);
IplImage* tempImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* grayImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* veriImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* dstImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* mask = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* mask2 = cvCreateImage(size, IPL_DEPTH_8U, 1);
CvRect rect = cvRect(10, 10, 640 - 20, 480 - 20);
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3, CV_32FC1);
CvMat* warp_matrix_invert = cvCreateMat(3,3, CV_32FC1);
CvMat* result = cvCreateMat(3, 1, CV_32FC1);
CvMat* dst = cvCreateMat(3, 1,CV_32FC1);
int keyButtonCornerCount = 316;
cvCvtColor(srcImage, grayImage, CV_BGR2GRAY);
cvSetImageROI(grayImage, rect);
cvSetImageROI(mask, rect);
cvSetImageROI(dstImage, rect);
cvSetImageROI(mask2, rect);
// 150~255사이의 값만 추출해서 마스크에 저장
cvInRangeS(grayImage, cvScalar(100, 100, 100), cvScalar(255, 255, 255), mask);
cvCopy(mask, mask2);
//cvShowImage("mask", mask);
//cvShowImage("mask2", mask2);
// 20,20? 150 미만의 값을 제외하기 위해 0인 값(mask)과 추출한 값(mask2)을 XOR 연산 한다.
cvFloodFill(mask, cvPoint(10, 10), cvScalar(0, 0, 0));
cvXor(mask2, mask, dstImage);
//cvShowImage("mask3", mask);
//cvShowImage("mask4", mask2);
//cvShowImage("dstImage", dstImage);
// 최종 연산된 이미지에서 코너 추출(각 키패드의 코너)
cvGoodFeaturesToTrack(dstImage, eigImage, tempImage, keyButtonCorner, &keyButtonCornerCount, 0.01, 7, NULL, 7, 0);
cvFindCornerSubPix (dstImage, keyButtonCorner, keyButtonCornerCount,cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvResetImageROI(dstImage);
for(int i =0 ; i < 316 ; i++){
keyButtonCorner[i].x += rect.x;
keyButtonCorner[i].y += rect.y;
}
initKeyButtonCorner();
srcQuad[CLOCKWISE_1].x = keyButtonCorner[315].x+10;
srcQuad[CLOCKWISE_1].y = keyButtonCorner[315].y-10;
srcQuad[CLOCKWISE_5].x = keyButtonCorner[31].x + 10;
srcQuad[CLOCKWISE_5].y = keyButtonCorner[31].y + 10;
srcQuad[CLOCKWISE_7].x = keyButtonCorner[0].x - 10;
srcQuad[CLOCKWISE_7].y = keyButtonCorner[0].y + 10;
srcQuad[CLOCKWISE_11].x = keyButtonCorner[290].x - 10;
srcQuad[CLOCKWISE_11].y = keyButtonCorner[290].y - 10;
dstQuad[CLOCKWISE_1].x = 640;
dstQuad[CLOCKWISE_1].y = 0;
dstQuad[CLOCKWISE_5].x = 640;
dstQuad[CLOCKWISE_5].y = 480;
dstQuad[CLOCKWISE_7].x = 0;
dstQuad[CLOCKWISE_7].y = 480;
dstQuad[CLOCKWISE_11].x = 0;
dstQuad[CLOCKWISE_11].y = 0;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
cvWarpPerspective(dstImage, veriImage, warp_matrix);
detectKeyButtonCorner(veriImage);
cvInvert(warp_matrix, warp_matrix_invert);
for(int i = 0 ; i < 316 ; i++){
cvmSet(dst, 0, 0, keyButtonCorner[i].x);
cvmSet(dst, 1, 0, keyButtonCorner[i].y);
cvmSet(dst, 2, 0, 1);
cvMatMul(warp_matrix_invert, dst, result);
float t = cvmGet(result, 2,0);
keyButtonCorner[i].x = cvmGet(result, 0,0)/t ;
keyButtonCorner[i].y = cvmGet(result, 1,0)/t ;
}
cvResetImageROI(srcImage);
cvResetImageROI(mask);
cvReleaseImage(&eigImage);
cvReleaseImage(&tempImage);
cvReleaseImage(&grayImage);
cvReleaseImage(&veriImage);
cvReleaseImage(&dstImage);
cvReleaseImage(&mask);
cvReleaseImage(&mask2);
cvReleaseMat(&warp_matrix);
cvReleaseMat(&warp_matrix_invert);
cvReleaseMat(&result);
cvReleaseMat(&dst);
}
void FkPaperKeyboard_TypeA::setKeyButton(IplImage* srcImage){
//꼭지점
CvPoint2D32f srcQuad[4], dstQuad[4];
IplImage* perspectiveTransImage = cvCreateImage(cvSize(640,480), IPL_DEPTH_8U, 3);
CvMat* warp_matrix = cvCreateMat(3,3, CV_32FC1);
CvMat* warp_matrix_invert = cvCreateMat(3,3, CV_32FC1);
CvMat* result = cvCreateMat(3, 1, CV_32FC1);
CvMat* dst = cvCreateMat(3, 1,CV_32FC1);
sortPaperKeyboardCorner();
srcQuad[CLOCKWISE_1] = keyboardCorner[1];
srcQuad[CLOCKWISE_5] = keyboardCorner[2];
srcQuad[CLOCKWISE_7] = keyboardCorner[3];
srcQuad[CLOCKWISE_11] = keyboardCorner[0];
dstQuad[CLOCKWISE_1].x = 640;
dstQuad[CLOCKWISE_1].y = 0;
dstQuad[CLOCKWISE_5].x = 640;
dstQuad[CLOCKWISE_5].y = 480;
dstQuad[CLOCKWISE_7].x = 0;
dstQuad[CLOCKWISE_7].y = 480;
dstQuad[CLOCKWISE_11].x = 0;
dstQuad[CLOCKWISE_11].y = 0;
// 원근변환 후 검사한다.
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix); // 변환 값 계산
cvWarpPerspective(srcImage, perspectiveTransImage, warp_matrix); // 투영 변환 행렬을 구한다
cornerVerification(perspectiveTransImage); // 추출된 행렬의 유효성을 검사한다
cvInvert(warp_matrix, warp_matrix_invert); //
//cvShowImage("srcImage",srcImage);
//cvShowImage("perspectiveTransImage", perspectiveTransImage);
for(int i = 0 ; i < 316 ; i++){
cvmSet(dst, 0, 0, keyButtonCorner[i].x);
cvmSet(dst, 1, 0, keyButtonCorner[i].y);
cvmSet(dst, 2, 0, 1);
cvMatMul(warp_matrix_invert, dst, result);
float t = cvmGet(result, 2,0);
keyButtonCorner[i].x = cvmGet(result, 0,0)/t;
keyButtonCorner[i].y = cvmGet(result, 1,0)/t;
}
setKeyButtonArea(keyButtonCorner, 0, 16);
setKeyButtonArea(keyButtonCorner, 64, 14);
setKeyButtonArea(keyButtonCorner, 120, 14);
setKeyButtonArea(keyButtonCorner, 176, 13);
setKeyButtonArea(keyButtonCorner, 228, 12);
setKeyButtonArea(keyButtonCorner, 276, 7);
setDirectionKeyButtonArea(keyButtonCorner, 306, 2, 77);
setDirectionKeyButtonArea(keyButtonCorner, 304, 4, 76);
setDirectionKeyButtonArea(keyButtonCorner, 308, 3, 78);
cvReleaseImage(&perspectiveTransImage);
cvReleaseMat(&warp_matrix);
cvReleaseMat(&warp_matrix_invert);
cvReleaseMat(&result);
cvReleaseMat(&dst);
}
//特征匹配
void SiftMatch::on_matchButton_clicked()
{
//若用户勾选了水平排列按钮
if(ui->radioButton_horizontal->isChecked())
{
//将2幅图片合成1幅图片,img1在左，img2在右
stacked = stack_imgs_horizontal(img1, img2);//合成图像，显示经距离比值法筛选后的匹配结果
}
else//用户勾选了垂直排列按钮
{
verticalStackFlag = true;//垂直排列标识设为true
//将2幅图片合成1幅图片,img1在上，img2在下
stacked = stack_imgs( img1, img2 );//合成图像，显示经距离比值法筛选后的匹配结果
}
//根据图1的特征点集feat1建立k-d树，返回k-d树根给kd_root
kd_root = kdtree_build( feat1, n1 );
Point pt1,pt2;//连线的两个端点
double d0,d1;//feat2中每个特征点到最近邻和次近邻的距离
int matchNum = 0;//经距离比值法筛选后的匹配点对的个数
//遍历特征点集feat2，针对feat2中每个特征点feat，选取符合距离比值条件的匹配点，放到feat的fwd_match域中
for(int i = 0; i < n2; i++ )
{
feat = feat2+i;//第i个特征点的指针
//在kd_root中搜索目标点feat的2个最近邻点，存放在nbrs中，返回实际找到的近邻点个数
int k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS );
if( k == 2 )
{
d0 = descr_dist_sq( feat, nbrs[0] );//feat与最近邻点的距离的平方
d1 = descr_dist_sq( feat, nbrs[1] );//feat与次近邻点的距离的平方
//若d0和d1的比值小于阈值NN_SQ_DIST_RATIO_THR，则接受此匹配，否则剔除
if( d0 < d1 * NN_SQ_DIST_RATIO_THR )
{ //将目标点feat和最近邻点作为匹配点对
pt2 = Point( cvRound( feat->x ), cvRound( feat->y ) );//图2中点的坐标
pt1 = Point( cvRound( nbrs[0]->x ), cvRound( nbrs[0]->y ) );//图1中点的坐标(feat的最近邻点)
if(verticalStackFlag)//垂直排列
pt2.y += img1->height;//由于两幅图是上下排列的，pt2的纵坐标加上图1的高度，作为连线的终点
else
pt2.x += img1->width;//由于两幅图是左右排列的，pt2的横坐标加上图1的宽度，作为连线的终点
cvLine( stacked, pt1, pt2, CV_RGB(255,0,255), 1, 8, 0 );//画出连线
matchNum++;//统计匹配点对的个数
feat2[i].fwd_match = nbrs[0];//使点feat的fwd_match域指向其对应的匹配点
}
}
free( nbrs );//释放近邻数组
}
qDebug()<<tr("经距离比值法筛选后的匹配点对个数：")<<matchNum<<endl;
//显示并保存经距离比值法筛选后的匹配图
//cvNamedWindow(IMG_MATCH1);//创建窗口
//cvShowImage(IMG_MATCH1,stacked);//显示
//保存匹配图
QString name_match_DistRatio = name1;//文件名，原文件名去掉序号后加"_match_DistRatio"
cvSaveImage(name_match_DistRatio.replace( name_match_DistRatio.lastIndexOf(".",-1)-1 , 1 , "_match_DistRatio").toAscii().data(),stacked);
//利用RANSAC算法筛选匹配点,计算变换矩阵H，
//无论img1和img2的左右顺序，H永远是将feat2中的特征点变换为其匹配点，即将img2中的点变换为img1中的对应点
H = ransac_xform(feat2,n2,FEATURE_FWD_MATCH,lsq_homog,4,0.01,homog_xfer_err,3.0,&inliers,&n_inliers);
//若能成功计算出变换矩阵，即两幅图中有共同区域
if( H )
{
qDebug()<<tr("经RANSAC算法筛选后的匹配点对个数：")<<n_inliers<<endl;
// //输出H矩阵
// for(int i=0;i<3;i++)
// qDebug()<<cvmGet(H,i,0)<<cvmGet(H,i,1)<<cvmGet(H,i,2);
if(verticalStackFlag)//将2幅图片合成1幅图片,img1在上，img2在下
stacked_ransac = stack_imgs( img1, img2 );//合成图像，显示经RANSAC算法筛选后的匹配结果
else//将2幅图片合成1幅图片,img1在左，img2在右
stacked_ransac = stack_imgs_horizontal(img1, img2);//合成图像，显示经RANSAC算法筛选后的匹配结果
//img1LeftBound = inliers[0]->fwd_match->x;//图1中匹配点外接矩形的左边界
//img1RightBound = img1LeftBound;//图1中匹配点外接矩形的右边界
//img2LeftBound = inliers[0]->x;//图2中匹配点外接矩形的左边界
//img2RightBound = img2LeftBound;//图2中匹配点外接矩形的右边界
int invertNum = 0;//统计pt2.x > pt1.x的匹配点对的个数，来判断img1中是否右图
//遍历经RANSAC算法筛选后的特征点集合inliers，找到每个特征点的匹配点，画出连线
for(int i=0; i<n_inliers; i++)
{
feat = inliers[i];//第i个特征点
pt2 = Point(cvRound(feat->x), cvRound(feat->y));//图2中点的坐标
pt1 = Point(cvRound(feat->fwd_match->x), cvRound(feat->fwd_match->y));//图1中点的坐标(feat的匹配点)
//qDebug()<<"pt2:("<<pt2.x<<","<<pt2.y<<")--->pt1:("<<pt1.x<<","<<pt1.y<<")";//输出对应点对
/*找匹配点区域的边界
if(pt1.x < img1LeftBound) img1LeftBound = pt1.x;
if(pt1.x > img1RightBound) img1RightBound = pt1.x;
if(pt2.x < img2LeftBound) img2LeftBound = pt2.x;
if(pt2.x > img2RightBound) img2RightBound = pt2.x;//*/
//统计匹配点的左右位置关系，来判断图1和图2的左右位置关系
if(pt2.x > pt1.x)
invertNum++;
if(verticalStackFlag)//垂直排列
pt2.y += img1->height;//由于两幅图是上下排列的，pt2的纵坐标加上图1的高度，作为连线的终点
else//水平排列
pt2.x += img1->width;//由于两幅图是左右排列的，pt2的横坐标加上图1的宽度，作为连线的终点
cvLine(stacked_ransac,pt1,pt2,CV_RGB(255,0,255),1,8,0);//在匹配图上画出连线
}
//绘制图1中包围匹配点的矩形
//cvRectangle(stacked_ransac,cvPoint(img1LeftBound,0),cvPoint(img1RightBound,img1->height),CV_RGB(0,255,0),2);
//绘制图2中包围匹配点的矩形
//cvRectangle(stacked_ransac,cvPoint(img1->width+img2LeftBound,0),cvPoint(img1->width+img2RightBound,img2->height),CV_RGB(0,0,255),2);
//cvNamedWindow(IMG_MATCH2);//创建窗口
//cvShowImage(IMG_MATCH2,stacked_ransac);//显示经RANSAC算法筛选后的匹配图
//保存匹配图
QString name_match_RANSAC = name1;//文件名，原文件名去掉序号后加"_match_RANSAC"
cvSaveImage(name_match_RANSAC.replace( name_match_RANSAC.lastIndexOf(".",-1)-1 , 1 , "_match_RANSAC").toAscii().data(),stacked_ransac);
/*程序中计算出的变换矩阵H用来将img2中的点变换为img1中的点，正常情况下img1应该是左图，img2应该是右图。
此时img2中的点pt2和img1中的对应点pt1的x坐标的关系基本都是：pt2.x < pt1.x
若用户打开的img1是右图，img2是左图，则img2中的点pt2和img1中的对应点pt1的x坐标的关系基本都是：pt2.x > pt1.x
所以通过统计对应点变换前后x坐标大小关系，可以知道img1是不是右图。
如果img1是右图，将img1中的匹配点经H的逆阵H_IVT变换后可得到img2中的匹配点*/
//若pt2.x > pt1.x的点的个数大于内点个数的80%，则认定img1中是右图
if(invertNum > n_inliers * 0.8)
{
qDebug()<<tr("img1中是右图");
CvMat * H_IVT = cvCreateMat(3, 3, CV_64FC1);//变换矩阵的逆矩阵
//求H的逆阵H_IVT时，若成功求出，返回非零值
if( cvInvert(H,H_IVT) )
{
// //输出H_IVT
// for(int i=0;i<3;i++)
// qDebug()<<cvmGet(H_IVT,i,0)<<cvmGet(H_IVT,i,1)<<cvmGet(H_IVT,i,2);
cvReleaseMat(&H);//释放变换矩阵H，因为用不到了
H = cvCloneMat(H_IVT);//将H的逆阵H_IVT中的数据拷贝到H中
cvReleaseMat(&H_IVT);//释放逆阵H_IVT
//将img1和img2对调
IplImage * temp = img2;
img2 = img1;
img1 = temp;
//cvShowImage(IMG1,img1);
//cvShowImage(IMG2,img2);
ui->mosaicButton->setEnabled(true);//激活全景拼接按钮
}
else//H不可逆时，返回0
{
cvReleaseMat(&H_IVT);//释放逆阵H_IVT
QMessageBox::warning(this,tr("警告"),tr("变换矩阵H不可逆"));
}
}
else
ui->mosaicButton->setEnabled(true);//激活全景拼接按钮
}
else //无法计算出变换矩阵，即两幅图中没有重合区域
{
QMessageBox::warning(this,tr("警告"),tr("两图中无公共区域"));
}
ui->radioButton_horizontal->setEnabled(false);//禁用排列方向选择按钮
ui->radioButton_vertical->setEnabled(false);
ui->matchButton->setEnabled(false);//禁用特征匹配按钮
}
CV_IMPL void
cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* corners,
int count, CvSize win, CvSize zeroZone,
CvTermCriteria criteria )
{
cv::AutoBuffer<float> buffer;
const int MAX_ITERS = 100;
const float drv_x[] = { -1.f, 0.f, 1.f };
const float drv_y[] = { 0.f, 0.5f, 0.f };
float *maskX;
float *maskY;
float *mask;
float *src_buffer;
float *gx_buffer;
float *gy_buffer;
int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1;
int win_rect_size = (win_w + 4) * (win_h + 4);
double coeff;
CvSize size, src_buf_size;
int i, j, k, pt_i;
int max_iters = 10;
double eps = 0;
CvMat stub, *src = (CvMat*)srcarr;
src = cvGetMat( srcarr, &stub );
if( CV_MAT_TYPE( src->type ) != CV_8UC1 )
CV_Error( CV_StsBadMask, "" );
if( !corners )
CV_Error( CV_StsNullPtr, "" );
if( count < 0 )
CV_Error( CV_StsBadSize, "" );
if( count == 0 )
return;
if( win.width <= 0 || win.height <= 0 )
CV_Error( CV_StsBadSize, "" );
size = cvGetMatSize( src );
if( size.width < win_w + 4 || size.height < win_h + 4 )
CV_Error( CV_StsBadSize, "" );
/* initialize variables, controlling loop termination */
switch( criteria.type )
{
case CV_TERMCRIT_ITER:
eps = 0.f;
max_iters = criteria.max_iter;
break;
case CV_TERMCRIT_EPS:
eps = criteria.epsilon;
max_iters = MAX_ITERS;
break;
case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
eps = criteria.epsilon;
max_iters = criteria.max_iter;
break;
default:
assert( 0 );
CV_Error( CV_StsBadFlag, "" );
}
eps = MAX( eps, 0 );
eps *= eps; /* use square of error in comparsion operations. */
max_iters = MAX( max_iters, 1 );
max_iters = MIN( max_iters, MAX_ITERS );
buffer.allocate( win_rect_size * 5 + win_w + win_h + 32 );
/* assign pointers */
maskX = buffer;
maskY = maskX + win_w + 4;
mask = maskY + win_h + 4;
src_buffer = mask + win_w * win_h;
gx_buffer = src_buffer + win_rect_size;
gy_buffer = gx_buffer + win_rect_size;
coeff = 1. / (win.width * win.width);
/* calculate mask */
for( i = -win.width, k = 0; i <= win.width; i++, k++ )
{
maskX[k] = (float)exp( -i * i * coeff );
}
if( win.width == win.height )
{
maskY = maskX;
}
else
{
coeff = 1. / (win.height * win.height);
for( i = -win.height, k = 0; i <= win.height; i++, k++ )
{
maskY[k] = (float) exp( -i * i * coeff );
}
}
for( i = 0; i < win_h; i++ )
{
for( j = 0; j < win_w; j++ )
{
mask[i * win_w + j] = maskX[j] * maskY[i];
}
}
/* make zero_zone */
if( zeroZone.width >= 0 && zeroZone.height >= 0 &&
zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h )
{
for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ )
{
for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ )
{
mask[i * win_w + j] = 0;
}
}
}
/* set sizes of image rectangles, used in convolutions */
src_buf_size.width = win_w + 2;
src_buf_size.height = win_h + 2;
/* do optimization loop for all the points */
for( pt_i = 0; pt_i < count; pt_i++ )
{
CvPoint2D32f cT = corners[pt_i], cI = cT;
int iter = 0;
double err;
do
{
CvPoint2D32f cI2;
double a, b, c, bb1, bb2;
IPPI_CALL( icvGetRectSubPix_8u32f_C1R( (uchar*)src->data.ptr, src->step, size,
src_buffer, (win_w + 2) * sizeof( src_buffer[0] ),
cvSize( win_w + 2, win_h + 2 ), cI ));
/* calc derivatives */
icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]),
gx_buffer, win_w * sizeof(gx_buffer[0]),
src_buf_size, drv_x, drv_y, buffer );
icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]),
gy_buffer, win_w * sizeof(gy_buffer[0]),
src_buf_size, drv_y, drv_x, buffer );
a = b = c = bb1 = bb2 = 0;
/* process gradient */
for( i = 0, k = 0; i < win_h; i++ )
{
double py = i - win.height;
for( j = 0; j < win_w; j++, k++ )
{
double m = mask[k];
double tgx = gx_buffer[k];
double tgy = gy_buffer[k];
double gxx = tgx * tgx * m;
double gxy = tgx * tgy * m;
double gyy = tgy * tgy * m;
double px = j - win.width;
a += gxx;
b += gxy;
c += gyy;
bb1 += gxx * px + gxy * py;
bb2 += gxy * px + gyy * py;
}
}
{
double A[4];
double InvA[4];
CvMat matA, matInvA;
A[0] = a;
A[1] = A[2] = b;
A[3] = c;
cvInitMatHeader( &matA, 2, 2, CV_64F, A );
cvInitMatHeader( &matInvA, 2, 2, CV_64FC1, InvA );
cvInvert( &matA, &matInvA, CV_SVD );
cI2.x = (float)(cI.x + InvA[0]*bb1 + InvA[1]*bb2);
cI2.y = (float)(cI.y + InvA[2]*bb1 + InvA[3]*bb2);
}
err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y);
cI = cI2;
}
while( ++iter < max_iters && err > eps );
/* if new point is too far from initial, it means poor convergence.
leave initial point as the result */
if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height )
{
cI = cT;
}
corners[pt_i] = cI; /* store result */
}
}
