C++ AAM_Shape::Mat2Point方法代码示例

//============================================================================
void ontrackcam(int pos)
{
	if(c == 0)
	{
		c = cvCreateMat(1, g_cam->nModes(), CV_64FC1);cvZero(c);
		s = cvCreateMat(1, g_cam->__shape.nPoints()*2, CV_64FC1);
		t = cvCreateMat(1, g_cam->__texture.nPixels(), CV_64FC1);
	}

	double var;
	//registrate appearance parameters
	for(int i = 0; i < n; i++)
	{
		var = 3*sqrt(g_cam->Var(i))*(double(b_c[i])/offset-1.0);
		cvmSet(c, 0, i, var);
	}

	//generate shape and texture instance
	g_cam->CalcLocalShape(s, c);
	g_cam->CalcTexture(t, c);
	
	//warp texture instance from base mesh to current shape instance
	aam_s.Mat2Point(s);
	int w = aam_s.GetWidth(), h = aam_s.MaxY()-aam_s.MinY();
	 aam_s.Translate(w, h);
	if(image == 0)image = cvCreateImage(cvSize(w*2,h*2), 8, 3);
	cvSet(image, cvScalar(128, 128, 128));
	g_cam->DrawAppearance(image, aam_s, t);
	
	cvNamedWindow("Combined Appearance Model",1);
	cvShowImage("Combined Appearance Model", image);
	
	if(cvWaitKey(10) == '27')
	{
		cvReleaseImage(&image);
		cvReleaseMat(&s);
		cvReleaseMat(&t);
		cvReleaseMat(&c);
		cvDestroyWindow("Parameters");
		cvDestroyWindow("Combined Appearance Model");
	}
}

//============================================================================
void AAM_IC::Fit(const IplImage* image, 		AAM_Shape& Shape, 
				int max_iter /* = 30 */, 	bool showprocess /* = false */)
{
	//initialize some stuff
	double t = gettime;
	const CvMat* A0 = __texture.GetMean();
	CvMat p; cvGetCols(__search_pq, &p, 4, 4+__shape.nModes());
	Shape.Point2Mat(__current_s);
	SetAllParamsZero();
	__shape.CalcParams(__current_s, __search_pq);
	IplImage* Drawimg = 0;
	
	for(int iter = 0; iter < max_iter; iter++)
	{
		if(showprocess)
		{	
			if(Drawimg == 0)	Drawimg = cvCloneImage(image);	
			else cvCopy(image, Drawimg);
			Shape.Mat2Point(__current_s);
			Draw(Drawimg, Shape, 2);
			mkdir("result");
			char filename[100];
			sprintf(filename, "result/Iter-%02d.jpg", iter);
			cvSaveImage(filename, Drawimg);
			
		}
		
		//check the current shape
		AAM_Common::CheckShape(__current_s, image->width, image->height);
		
		//warp image to mesh shape mesh
		__paw.CalcWarpTexture(__current_s, image, __warp_t);
		AAM_TDM::NormalizeTexture(A0, __warp_t);
		cvSub(__warp_t, A0, __error_t);
		
		 //calculate updates (and scale to account for linear lighting gain)
		cvGEMM(__error_t, __G, 1, NULL, 1, __delta_pq, CV_GEMM_B_T);
		
		//check for parameter convergence
		if(cvNorm(__delta_pq) < 1e-6)	break;

		//apply inverse compositional algorithm to update parameters
		InverseCompose(__delta_pq, __current_s, __update_s);
		
		//smooth shape
		cvAddWeighted(__current_s, 0.4, __update_s, 0.6, 0, __update_s);
		//update parameters
		__shape.CalcParams(__update_s, __search_pq);
		//calculate constrained new shape
		__shape.CalcShape(__search_pq, __update_s);
		
		//check for shape convergence
		if(cvNorm(__current_s, __update_s, CV_L2) < 0.001)	break;
		else cvCopy(__update_s, __current_s);	
	}

	Shape.Mat2Point(__current_s);
		
	t = gettime-t;
	printf("AAM IC Fitting time cost %.3f millisec\n", t);
	
	cvReleaseImage(&Drawimg);
}

//============================================================================
void AAM_Basic::DrawAppearance(IplImage* image)
{
	AAM_Shape Shape; Shape.Mat2Point(__current_s);
	AAM_PAW paw;
	paw.Train(Shape, __cam.__Points, __cam.__Storage, __cam.__paw.GetTri(), false);
	int x1, x2, y1, y2, idx1, idx2;
	int tri_idx, v1, v2, v3;
	int xby3, idxby3;
	int minx, miny, maxx, maxy;
	AAM_Shape refShape;	refShape.Mat2Point(__cam.__MeanS);
	refShape.Translate(-refShape.MinX(), -refShape.MinY());
	double minV, maxV;
	cvMinMaxLoc(__t_m, &minV, &maxV);
	cvConvertScale(__t_m, __t_m, 255/(maxV-minV), -minV*255/(maxV-minV));
	byte* pimg;
	double* fastt = __t_m->data.db;

	minx = Shape.MinX(); miny = Shape.MinY();
	maxx = Shape.MaxX(); maxy = Shape.MaxY();

	if( minx < 0 )
		minx = 0;
	else if(minx >= image->width)
		minx = image->width - 1;

	if( miny < 0 )
		miny = 0;
	else if(miny >= image->height)
		miny = image->height - 1;

	if( maxx < 0 )
		maxx = 0;
	else if(maxx >= image->width)
		maxx = image->height - 1;

	if( maxy < 0 )
		maxy = 0;
	else if(maxy >= image->height)
		maxy = image->height - 1;

	for(int y = miny; y < maxy; y++)
	{
		y1 = y-miny;
		pimg = (byte*)(image->imageData + image->widthStep*y);
		for(int x = minx; x < maxx; x++)
		{
			x1 = x-minx;
			idx1 = paw.__rect[y1][x1];
			if(idx1 >= 0)
			{
				tri_idx = paw.PixTri(idx1);
				v1 = paw.Tri(tri_idx, 0);
				v2 = paw.Tri(tri_idx, 1);
				v3 = paw.Tri(tri_idx, 2);
		
				x2 = paw.__alpha[idx1]*refShape[v1].x + paw.__belta[idx1]*refShape[v2].x +  
					paw.__gamma[idx1]*refShape[v3].x;
				y2 = paw.__alpha[idx1]*refShape[v1].y + paw.__belta[idx1]*refShape[v2].y +  
					paw.__gamma[idx1]*refShape[v3].y;
				
				xby3 = 3*x; 
				idx2 = __cam.__paw.__rect[y2][x2];		idxby3 = 3*idx2;
				pimg[xby3] = fastt[idxby3];
				pimg[xby3+1] = fastt[idxby3+1];
				pimg[xby3+2] = fastt[idxby3+2];
			}
		}
	}
}

//============================================================================
void AAM_Basic::Fit(const IplImage* image, AAM_Shape& Shape,
					int max_iter /* = 30 */,bool showprocess /* = false */)
{
	//intial some stuff
	double t = gettime;
	double e1, e2;
	const int np = 5;
	double k_values[np] = {1, 0.5, 0.25, 0.125, 0.0625};
	int k;
	IplImage* Drawimg = 0;

	Shape.Point2Mat(__s);
	InitParams(image);
	CvMat subcq;
	cvGetCols(__current_c_q, &subcq, 0, 4); cvCopy(__q, &subcq);
	cvGetCols(__current_c_q, &subcq, 4, 4+__cam.nModes()); cvCopy(__c, &subcq);

	//calculate error
	e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);

	//do a number of iteration until convergence
	for(int iter = 0; iter <max_iter; iter++)
	{
		if(showprocess)
		{
			if(Drawimg == 0)	Drawimg = cvCloneImage(image);
			else cvCopy(image, Drawimg);
			__cam.CalcShape(__s, __current_c_q);
			Shape.Mat2Point(__s);
			Draw(Drawimg, Shape, 2);
			#ifdef TARGET_WIN32
			mkdir("result");
			#else
			mkdir("result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
			#endif
			char filename[100];
			sprintf(filename, "result/ter%d.bmp", iter);
			cvSaveImage(filename, Drawimg);
		}

		// predict parameter update
		cvGEMM(__delta_t, __G, 1, NULL, 0, __delta_c_q, CV_GEMM_B_T);

		//force first iteration
		if(iter == 0)
		{
			cvAdd(__current_c_q, __delta_c_q, __current_c_q);
			CvMat c; cvGetCols(__current_c_q, &c, 4, 4+__cam.nModes());
			//constrain parameters
			__cam.Clamp(&c);
			e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);
		}

		//find largest step size which reduces texture EstResidual
		else
		{
			for(k = 0; k < np; k++)
			{
				cvScaleAdd(__delta_c_q, cvScalar(k_values[k]), __current_c_q,  __update_c_q);
				//constrain parameters
				CvMat c; cvGetCols(__update_c_q, &c, 4, 4+__cam.nModes());
				__cam.Clamp(&c);

				e2 = EstResidual(image, __update_c_q, __s, __t_m, __t_s, __delta_t);
				if(e2 <= e1)	break;
			}
		}

		//check for convergence
		if(iter > 0)
		{
			if(k == np)
			{
				e1 = e2;
				cvCopy(__update_c_q, __current_c_q);
			}

			else if(fabs(e2-e1)<0.001*e1)	break;
			else if (cvNorm(__delta_c_q)<0.001)	break;
			else
			{
				cvCopy(__update_c_q, __current_c_q);
				e1 = e2;
			}
		}
	}

	cvReleaseImage(&Drawimg);
	__cam.CalcShape(__s, __current_c_q);
	Shape.Mat2Point(__s);
	t = gettime - t;
	printf("AAM-Basic Fitting time cost: %.3f millisec\n", t);
}

//============================================================================
int AAM_Basic::Fit(const IplImage* image, AAM_Shape& Shape, 
					int max_iter /* = 30 */,bool showprocess /* = false */)
{
	//intial some stuff
	double t = curtime;
	double e1, e2, e3;
	double k_v[6] = {-1,-1.15,-0.7,-0.5,-0.2,-0.0625};
	Shape.Point2Mat(__current_s);
	
	InitParams(image, __current_s, __current_c);
	__cam.__shape.CalcParams(__current_s, __p, __current_q);
	cvZero(__current_c);
	IplImage* Drawimg = 
		cvCreateImage(cvGetSize(image), image->depth, image->nChannels);	
	//mkdir("result");
	//char filename[100];
	//calculate error
	e3 = EstResidual(image, __current_c, __current_s, __delta_t);
	if(e3 == -1) return 0;

	int iter;

	//do a number of iteration until convergence
	for( iter = 0; iter <max_iter; iter++)
	{
		// predict pose and parameter update
		// __delta_t rosszul számolódik. Kiiratás ld. AAM_Sahpe::Mat2Point()
		//cvGEMM(__delta_t, __Rq, 1, NULL, 0, __delta_q, CV_GEMM_B_T);
		cvGEMM(__delta_t, __Rc, 1, NULL, 0, __delta_c, CV_GEMM_B_T);

		// if the prediction above didn't improve th fit,
        // try amplify and later damp the prediction
		for(int k = 0; k < 6; k++)
		{
			cvScaleAdd(__delta_q, cvScalar(k_v[k]), __current_q,  __update_q);
			cvScaleAdd(__delta_c, cvScalar(k_v[k]), __current_c,  __update_c);
			__cam.Clamp(__update_c);//constrain parameters				
			e2 = EstResidual(image, __update_c, __current_s, __delta_t);

			if(k==0) e1 = e2;
			else if(e2 != -1 && e2 < e1)break;
		}
		//check for convergence
		if((iter>max_iter/3&&fabs(e2-e3)<0.01*e3) || e2<0.001 ) 
		{
			break;
		}
		else if (cvNorm(__delta_c)<0.001 && cvNorm(__delta_q)<0.001) 
		{
			break;
		}
		else
		{
			cvCopy(__update_q, __current_q);
			cvCopy(__update_c, __current_c);
			e3 = e2;
		}
	}

	__cam.CalcShape(__current_s, __current_c, __current_q); 
	Shape.Mat2Point(__current_s);
	t = curtime - t;
	if( AAM_DEBUG_MODE ) printf("AAM-Basic Fitting time cost: %.3f\n", t);

	return iter;
}