//============================================================================
void AAM_Basic::CalcJacobianMatrix(const file_lists& pts_files,
const file_lists& img_files,
double disp_scale /* = 0.2 */,
double disp_angle /* = 20 */,
double disp_trans /* = 5.0 */,
double disp_std /* = 1.0 */,
int nExp /* = 30 */)
{
CvMat* J = cvCreateMat(__cam.nModes()+4, __cam.__texture.nPixels(), CV_64FC1);
CvMat* d = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* o = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* oo = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* t = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t_m = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t_s = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t1 = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t2 = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* u = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* c = cvCreateMat(1, __cam.nModes(), CV_64FC1);
CvMat* s = cvCreateMat(1, __cam.__shape.nPoints()*2, CV_64FC1);
CvMat* q = cvCreateMat(1, 4, CV_64FC1);
CvMat* p = cvCreateMat(1, __cam.__shape.nModes(),CV_64FC1);
CvMat* lamda = cvCreateMat(1, __cam.__texture.nModes(), CV_64FC1);
double theta = disp_angle * CV_PI / 180;
double aa = MAX(fabs(disp_scale*cos(theta)), fabs(disp_scale*sin(theta)));
cvmSet(d,0,0,aa); cvmSet(d,0,1,aa); cvmSet(d,0,2,disp_trans); cvmSet(d,0,3,disp_trans);
for(int nmode = 0; nmode < __cam.nModes(); nmode++)
cvmSet(d,0,4+nmode,disp_std*sqrt(__cam.Var(nmode)));
srand(unsigned(time(0)));
cvSetZero(u);cvSetZero(J);
for(int i = 0; i < pts_files.size(); i++)
{
IplImage* image = cvLoadImage(img_files[i].c_str(), -1);
AAM_Shape Shape;
if(!Shape.ReadAnnotations(pts_files[i]))
Shape.ScaleXY(image->width, image->height);
Shape.Point2Mat(s);
//calculate current texture vector
__cam.__paw.CalcWarpTexture(s, image, t);
__cam.__texture.NormalizeTexture(__cam.__MeanG, t);
//calculate appearance parameters
__cam.__shape.CalcParams(s, p, q);
__cam.__texture.CalcParams(t, lamda);
__cam.CalcParams(c, p, lamda);
//update appearance and pose parameters
CvMat subo;
cvGetCols(o, &subo, 0, 4); cvCopy(q, &subo);
cvGetCols(o, &subo, 4, 4+__cam.nModes()); cvCopy(c, &subo);
//get optimal EstResidual
EstResidual(image, o, s, t_m, t_s, t1);
for(int j = 0; j < nExp; j++)
{
printf("Pertubing (%d/%d) for image (%d/%d)...\r", j, nExp, i, pts_files.size());
for(int l = 0; l < 4+__cam.nModes(); l++)
{
double D = cvmGet(d,0,l);
double v = rand_in_between(-D, D);
cvCopy(o, oo); CV_MAT_ELEM(*oo,double,0,l) += v;
EstResidual(image, oo, s, t_m, t_s, t2);
cvSub(t1, t2, t2);
cvConvertScale(t2, t2, 1.0/v);
//accumulate into l-th row
CvMat Jl; cvGetRow(J, &Jl, l);
cvAdd(&Jl, t2, &Jl);
CV_MAT_ELEM(*u, double, 0, l) += 1.0;
}
}
cvReleaseImage(&image);
}
//normalize
for(int l = 0; l < __cam.nModes()+4; l++)
{
CvMat Jl; cvGetRow(J, &Jl, l);
cvConvertScale(&Jl, &Jl, 1.0/cvmGet(u,0,l));
}
CvMat* JtJ = cvCreateMat(__cam.nModes()+4, __cam.nModes()+4, CV_64FC1);
CvMat* InvJtJ = cvCreateMat(__cam.nModes()+4, __cam.nModes()+4, CV_64FC1);
cvGEMM(J, J, 1, NULL, 0, JtJ, CV_GEMM_B_T);
cvInvert(JtJ, InvJtJ, CV_SVD);
cvMatMul(InvJtJ, J, __G);
cvReleaseMat(&J); cvReleaseMat(&d); cvReleaseMat(&o);
cvReleaseMat(&oo); cvReleaseMat(&t); cvReleaseMat(&t_s);
cvReleaseMat(&t_m); cvReleaseMat(&t1); cvReleaseMat(&t2);
cvReleaseMat(&u); cvReleaseMat(&c); cvReleaseMat(&s);
cvReleaseMat(&q); cvReleaseMat(&p); cvReleaseMat(&lamda);
cvReleaseMat(&JtJ); cvReleaseMat(&InvJtJ);
}
//============================================================================
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;
}
