void cvSoftmaxDer(CvMat * X, CvMat * dE_dY, CvMat * dE_dY_afder) {
CV_FUNCNAME("cvSoftmaxDer");
__BEGIN__;
const int nr = X->rows, nc = X->cols, dtype = CV_MAT_TYPE(X->type);
CvMat * Y = cvCreateMat(nr, nc, dtype);
CvMat * dE_dY_transpose = cvCreateMat(nr, nc, dtype);
CvMat * sum = cvCreateMat(nr, 1, dtype);
CvMat * sum_repeat = cvCreateMat(nr, nc, dtype);
cvSoftmax(X, Y);
if (dE_dY->rows==nc && dE_dY->cols==nr){
cvTranspose(dE_dY,dE_dY_transpose);
cvMul(Y,dE_dY_transpose,dE_dY_afder);
}else{
cvMul(Y,dE_dY,dE_dY_afder);
}
cvReduce(dE_dY_afder,sum,-1,CV_REDUCE_SUM);
cvRepeat(sum,sum_repeat);
cvMul(Y,sum_repeat,sum_repeat);
cvSub(dE_dY_afder,sum_repeat,dE_dY_afder);
cvReleaseMat(&dE_dY_transpose);
cvReleaseMat(&sum);
cvReleaseMat(&sum_repeat);
cvReleaseMat(&Y);
__END__;
}
//------------------------------------------------------------------------------
// Color Similarity Matrix Calculation
//------------------------------------------------------------------------------
CvMat *colorsim(int nbins, double sigma) {
CvMat *xc=cvCreateMat(1,nbins, CV_32FC1);
CvMat *yr=cvCreateMat(nbins,1, CV_32FC1);
CvMat *x=cvCreateMat(nbins,nbins, CV_32FC1);
CvMat *y=cvCreateMat(nbins,nbins, CV_32FC1);
CvMat *m=cvCreateMat(x->rows,x->rows, CV_32FC1);
// Set x,y directions
for (int j=0;j<nbins;j++) {
cvSetReal2D(xc,0,j,(j+1-0.5)/nbins);
cvSetReal2D(yr,j,0,(j+1-0.5)/nbins);
}
// Set u,v, meshgrids
for (int i=0;i<x->rows;i++) {
cvRepeat(xc,x);
cvRepeat(yr,y);
}
CvMat *sub = cvCreateMat(x->rows,y->cols,CV_32FC1);
cvSub(x,y,sub);
cvAbs(sub,sub);
cvMul(sub,sub,sub);
cvConvertScale(sub,sub,-1.0/(2*sigma*sigma));
cvExp(sub,sub);
cvSubRS(sub,cvScalar(1.0),m);
cvReleaseMat(&xc);
cvReleaseMat(&yr);
cvReleaseMat(&x);
cvReleaseMat(&y);
cvReleaseMat(&sub);
return m;
}
//! assuming row vectors (a row is a sample)
void cvSoftmax(CvMat * src, CvMat * dst){
CV_FUNCNAME("cvSoftmax");
__BEGIN__;
CV_ASSERT(cvCountNAN(src)<1);
cvExp(src,dst);
CV_ASSERT(cvCountNAN(dst)<1);
const int dtype = CV_MAT_TYPE(src->type);
CvMat * sum = cvCreateMat(src->rows,1,dtype);
CvMat * sum_repeat = cvCreateMat(src->rows,src->cols,dtype);
cvReduce(dst,sum,-1,CV_REDUCE_SUM);
CV_ASSERT(cvCountNAN(sum)<1);
cvRepeat(sum,sum_repeat);
cvDiv(dst,sum_repeat,dst);
cvReleaseMat(&sum);
cvReleaseMat(&sum_repeat);
__END__;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
CvMat *tgso (CvMat &tmap, int ntex, double sigma, double theta, CvMat &tsim, int useChi2) {
CvMat *roundTmap=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
CvMat *comp=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
for (int i=0;i<tmap.rows;i++)
for (int j=0;j<tmap.cols;j++)
cvSetReal2D(roundTmap,i,j,cvRound(cvGetReal2D(&tmap,i,j)));
cvSub(&tmap,roundTmap,comp);
if (cvCountNonZero(comp)) {
printf("texton labels not integral");
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
double min,max;
cvMinMaxLoc(&tmap,&min,&max);
if (min<1 && max>ntex) {
char *msg=new char[50];
printf(msg,"texton labels out of range [1,%d]",ntex);
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
double wr=floor(sigma); //sigma=radius (Leo)
CvMat *x=cvCreateMat(1,wr-(-wr)+1, CV_64FC1);
CvMat *y=cvCreateMat(wr-(-wr)+1,1, CV_64FC1);
CvMat *u=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *v=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *gamma=cvCreateMat(u->rows,v->rows, CV_64FC1);
// Set x,y directions
for (int j=-wr;j<=wr;j++) {
cvSetReal2D(x,0,(j+wr),j);
cvSetReal2D(y,(j+wr),0,j);
}
// Set u,v, meshgrids
for (int i=0;i<u->rows;i++) {
cvRepeat(x,u);
cvRepeat(y,v);
}
// Compute the gamma matrix from the grid
for (int i=0;i<u->rows;i++)
for (int j=0;j<u->cols;j++)
cvSetReal2D(gamma,i,j,atan2(cvGetReal2D(v,i,j),cvGetReal2D(u,i,j)));
cvReleaseMat(&x);
cvReleaseMat(&y);
CvMat *sum=cvCreateMat(u->rows,u->cols, CV_64FC1);
cvMul(u,u,u);
cvMul(v,v,v);
cvAdd(u,v,sum);
CvMat *mask=cvCreateMat(u->rows,u->cols, CV_8UC1);
cvCmpS(sum,sigma*sigma,mask,CV_CMP_LE);
cvConvertScale(mask,mask,1.0/255);
cvSetReal2D(mask,wr,wr,0);
int count=cvCountNonZero(mask);
cvReleaseMat(&u);
cvReleaseMat(&v);
cvReleaseMat(&sum);
CvMat *sub=cvCreateMat(mask->rows,mask->cols, CV_64FC1);
CvMat *side=cvCreateMat(mask->rows,mask->cols, CV_8UC1);
cvSubS(gamma,cvScalar(theta),sub);
cvReleaseMat(&gamma);
for (int i=0;i<mask->rows;i++){
for (int j=0;j<mask->cols;j++) {
double n=cvmGet(sub,i,j);
double n_mod = n-floor(n/(2*M_PI))*2*M_PI;
cvSetReal2D(side,i,j, 1 + int(n_mod < M_PI));
}
}
cvMul(side,mask,side);
cvReleaseMat(&sub);
cvReleaseMat(&mask);
CvMat *lmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
CvMat *rmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
cvCmpS(side,1,lmask,CV_CMP_EQ);
cvCmpS(side,2,rmask,CV_CMP_EQ);
int count1=cvCountNonZero(lmask), count2=cvCountNonZero(rmask);
if (count1 != count2) {
printf("Bug: imbalance\n");
}
CvMat *rlmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
CvMat *rrmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
cvConvertScale(lmask,rlmask,1.0/(255*count)*2);
cvConvertScale(rmask,rrmask,1.0/(255*count)*2);
cvReleaseMat(&lmask);
cvReleaseMat(&rmask);
cvReleaseMat(&side);
int h=tmap.rows;
int w=tmap.cols;
CvMat *d = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat *coltemp = cvCreateMat(h*w,1,CV_32FC1);
CvMat *tgL = cvCreateMat(h,w, CV_32FC1);
CvMat *tgR = cvCreateMat(h,w, CV_32FC1);
CvMat *temp = cvCreateMat(h,w,CV_8UC1);
CvMat *im = cvCreateMat(h,w, CV_32FC1);
CvMat *sub2 = cvCreateMat(h,w,CV_32FC1);
CvMat *sub2t = cvCreateMat(w,h,CV_32FC1);
CvMat *prod = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat reshapehdr,*reshape;
CvMat* tgL_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* tgR_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* im_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat *tg=cvCreateMat(h,w,CV_32FC1);
cvZero(tg);
if (useChi2 == 1){
CvMat* temp_add1 = cvCreateMat(h,w,CV_32FC1);
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvPow(sub2,sub2,2.0);
cvAdd(tgL,tgR,temp_add1);
cvAddS(temp_add1,cvScalar(0.0000000001),temp_add1);
cvDiv(sub2,temp_add1,sub2);
cvAdd(tg,sub2,tg);
}
cvScale(tg,tg,0.5);
cvReleaseMat(&temp_add1);
}
else{// if not chi^2
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvAbs(sub2,sub2);
cvTranspose(sub2,sub2t);
reshape=cvReshape(sub2t,&reshapehdr,0,h*w);
cvGetCol(d,coltemp,i);
cvCopy(reshape,coltemp);
}
cvMatMul(d,&tsim,prod);
cvMul(prod,d,prod);
CvMat *sumcols=cvCreateMat(h*w,1,CV_32FC1);
cvSetZero(sumcols);
for (int i=0;i<prod->cols;i++) {
cvGetCol(prod,coltemp,i);
cvAdd(sumcols,coltemp,sumcols);
}
reshape=cvReshape(sumcols,&reshapehdr,0,w);
cvTranspose(reshape,tg);
cvReleaseMat(&sumcols);
}
//Smooth the gradient now!!
tg=fitparab(*tg,sigma,sigma/4,theta);
cvMaxS(tg,0,tg);
cvReleaseMat(&im_pad);
cvReleaseMat(&tgL_pad);
cvReleaseMat(&tgR_pad);
cvReleaseMat(&rlmask);
cvReleaseMat(&rrmask);
cvReleaseMat(&im);
cvReleaseMat(&tgL);
cvReleaseMat(&tgR);
cvReleaseMat(&temp);
cvReleaseMat(&coltemp);
cvReleaseMat(&sub2);
cvReleaseMat(&sub2t);
cvReleaseMat(&d);
cvReleaseMat(&prod);
return tg;
}
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;
}
int CUKF::UnscentedTransform(int model)
{
// Set up some variables
int dim = cvGetSize(XX).height;
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);
// PrintCvMat(U, "U");
// PrintCvMat(D, "D");
// PrintCvMat(V, "V");
// PrintCvMat(Ds, "Ds");
// PrintCvMat(UD, "UD");
// PrintCvMat(PX, "PX");
// PrintCvMat(Ps, "Ps");
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
//for sigma points display
//cvReleaseMat(&m_sigmaPointst_1);
//cvReleaseMat(&m_sigmaPointst);
//m_sigmaPointst_1 = cvCreateMat(dim, N, CV_64FC1);
//m_sigmaPointst = cvCreateMat(dim, N, CV_64FC1);
//
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double* ssdb;
double* Psdb;
ssdb = ss->data.db;
Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
//ss->data.db[i*ss->cols+j] += Ps->data.db[i*Ps->cols+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++)
{
//ss->data.db[i*ss->cols+j] -= Ps->data.db[i*Ps->cols+jj];
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
//cvRepeat(ss, m_sigmaPointst_1);
// Transform samples according to motion model
CvMat *ys;
CvMat *base;
CvMat *delta;
CvMat *reptemp;
int ysdim;
double *ysdb;
double *basedb;
double *deltadb;
double *reptempdb;
switch(model)
{
case MOTION_MODEL:
ysdim = dim-2;
ys = cvCreateMat(ysdim, N, CV_64FC1);
base = cvCreateMat(ysdim, N, CV_64FC1);
delta = cvCreateMat(ysdim, N, CV_64FC1);
reptemp = cvCreateMat(ysdim, 1, CV_64FC1);
cvRepeat(ss, ys);
ysdb = ys->data.db;
basedb = base->data.db;
deltadb = delta->data.db;
reptempdb = reptemp->data.db;
for(j=0; j<N; j++)
{
double v = ssdb[(dim-2)*N+j];
double g = ssdb[(dim-1)*N+j];
ysdb[0*N+j] = ssdb[0*N+j] + v*DT_CONTROLS*cos(g+ssdb[2*N+j]);
ysdb[1*N+j] = ssdb[1*N+j] + v*DT_CONTROLS*sin(g+ssdb[2*N+j]);
ysdb[2*N+j] = ssdb[2*N+j] + v*DT_CONTROLS*sin(g)/WHEELBASE;
}
for(i=0; i<ys->rows; i++) reptempdb[reptemp->cols*i] = ysdb[ys->cols*i];
cvRepeat(reptemp, base);
cvSub(base, ys, delta);
for(i=0; i<base->cols; i++) basedb[2*base->cols+i] = PI2PI(basedb[2*base->cols+i]);
cvSub(base, delta, ys);
//PrintCvMat(ys, "ys");
//PrintCvMat(reptemp, "reptemp");
//PrintCvMat(base, "base");
//PrintCvMat(delta, "delta");
//PrintCvMat(ys, "ys");
cvReleaseMat(&base);
cvReleaseMat(&delta);
cvReleaseMat(&reptemp);
//cvRepeat(ys, m_sigmaPointst);
break;
case AUGMENT_MODEL:
//PrintCvMat(ss, "ss");
ysdim = dim;
ys = cvCreateMat(dim, N, CV_64FC1);
double phi, r, b, s, c;
//PrintCvMat(XX, "XX");
for(j=0; j<N; j++)
{
phi = ssdb[2*ss->cols+j];
r = ssdb[(ss->rows-2)*ss->cols+j];
b = ssdb[(ss->rows-1)*ss->cols+j];
s = sin(phi+b);
c = cos(phi+b);
ssdb[(ss->rows-2)*ss->cols+j] = ssdb[0*ss->cols+j] + r*c;
ssdb[(ss->rows-1)*ss->cols+j] = ssdb[1*ss->cols+j] + r*s;
}
cvCopy(ss, ys);
//PrintCvMat(Ps, "Ps");
//PrintCvMat(ss, "ss");
break;
}
CvMat *y = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dy = cvCreateMat(ysdim, N, CV_64FC1);
CvMat *dyu = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dytu = cvCreateMat(1, ysdim, CV_64FC1);
CvMat *dyl = cvCreateMat(ysdim, N-1, CV_64FC1);
CvMat *dytl = cvCreateMat(N-1, ysdim, CV_64FC1);
CvMat *temp = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *Y = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *temp1 = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *repy = cvCreateMat(ysdim, N, CV_64FC1);
double *ydb = y->data.db;
double *dydb = dy->data.db;
double *dyudb = dyu->data.db;
double *dytudb = dytu->data.db;
double *dyldb = dyl->data.db;
double *dytldb = dytl->data.db;
double *tempdb = temp->data.db;
double *Ydb = Y->data.db;
double *temp1db = temp1->data.db;
double *repydb = repy->data.db;
ysdb = ys->data.db;
for(i=0; i<y->rows; i++) ydb[i] = 2*kappa*ysdb[ys->cols*i];
for(i=0; i<y->rows; i++)
{
for(j=1; j<N; j++)
{
ydb[i] += ysdb[ys->cols*i+j];
}
}
for(i=0; i<y->rows; i++) ydb[i] /= (2*scale);
for(i=0; i<y->rows; i++) temp1db[i] = ydb[y->cols*i];
cvRepeat(temp1, repy);
cvSub(ys, repy, dy);
// PrintCvMat(temp1, "temp1");
// PrintCvMat(repy, "repy");
// PrintCvMat(dy, "dy");
// for(j=0; j<N; j++)
// {
// dy->data.db[0*N+j] = ys->data.db[0*N+j] - y->data.db[0];
// dy->data.db[1*N+j] = ys->data.db[1*N+j] - y->data.db[1];
// dy->data.db[2*N+j] = ys->data.db[2*N+j] - y->data.db[2];
// }
for(i=0; i<ysdim; i++) dyudb[i] = dydb[i*dy->cols];
for(i=0; i<ysdim; i++) for(j=1, jj=0; j<N; j++, jj++)
dyldb[i*dyl->cols+jj] = dydb[i*dy->cols+j];
// PrintCvMat(ys, "ys");
// PrintCvMat(dy, "dy");
// PrintCvMat(dyu, "dyu");
// PrintCvMat(dyl, "dyl");
cvT(dyu, dytu);
cvT(dyl, dytl);
cvMatMul(dyu, dytu, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 2*kappa);
// PrintCvMat(Y, "Y");
cvMatMul(dyl, dytl, temp);
// PrintCvMat(temp, "temp");
cvAdd(Y, temp, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 1/(double)(2*scale));
// PrintCvMat(Y, "Y");
cvReleaseMat(&XX);
cvReleaseMat(&PX);
XX = cvCreateMat(y->rows, y->cols, CV_64FC1);
PX = cvCreateMat(Y->rows, Y->cols, CV_64FC1);
cvCopy(y, XX);
cvCopy(Y, PX);
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&ys);
cvReleaseMat(&y);
cvReleaseMat(&dy);
cvReleaseMat(&dyu);
cvReleaseMat(&dytu);
cvReleaseMat(&dyl);
cvReleaseMat(&dytl);
cvReleaseMat(&temp);
cvReleaseMat(&Y);
cvReleaseMat(&temp1);
cvReleaseMat(&repy);
return 0;
}
int CUKF::EllipseFitting()
{
int i;
double n = EP-1;
double inc = 2*PI/n;
CvMat *p2by2 = cvCreateMat(2, 2, CV_64FC1);
// CvMat *phi = cvCreateMat(1, EP, CV_64FC1);
CvMat *circ = cvCreateMat(2, EP, CV_64FC1);
CvMat *p = cvCreateMat(2, EP, CV_64FC1);
CvMat *r = cvCreateMat(2, 2, CV_64FC1);
CvMat *a = cvCreateMat(2, EP, CV_64FC1);
cvRepeat(PX, p2by2);
//PrintCvMat(p2by2, "p2by2");
for(i=0; i<circ->cols; i++)
{
circ->data.db[i] = 2*cos(inc*i);
circ->data.db[circ->cols+i] = 2*sin(inc*i);
}
sqrtm_2by2(p2by2, r);
cvMatMul(r, circ, a);
for(i=0; i<p->cols; i++)
{
EllipseV->data.db[p->cols+i] = a->data.db[a->cols+i]*DRAW_SCALE + XX->data.db[1];
EllipseV->data.db[i] = a->data.db[i]*DRAW_SCALE + XX->data.db[0];
}
//PrintCvMat(XX, "XX");
//PrintCvMat(PX, "PX");
////PrintCvMat(p2by2, "p2by2");
//PrintCvMat(circ, "circ");
//PrintCvMat(r, "r");
//PrintCvMat(a, "a");
// PrintCvMat(EllipseV, "EllipseV");
//<--------------vehicle ellipse fitting
int nf = (XX->rows-3)/2; // number of features
for(int f=0; f<nf; f++)
{
p2by2->data.db[0] = PX->data.db[(3+f*2)*PX->cols+(3+f*2)];
p2by2->data.db[1] = PX->data.db[(3+f*2)*PX->cols+(3+f*2)+1];
p2by2->data.db[2] = PX->data.db[(3+f*2+1)*PX->cols+(3+f*2)];
p2by2->data.db[3] = PX->data.db[(3+f*2+1)*PX->cols+(3+f*2)+1];
// PrintCvMat(PX, "PX");
// PrintCvMat(p2by2, "p2by2");
sqrtm_2by2(p2by2, r);
cvMatMul(r, circ, a);
for(i=0; i<p->cols; i++)
{
EllipseF[f]->data.db[p->cols+i] = a->data.db[a->cols+i]*DRAW_SCALE + XX->data.db[(f*2)+4];
EllipseF[f]->data.db[i] = a->data.db[i]*DRAW_SCALE + XX->data.db[(f*2)+3];
}
// PrintCvMat(p2by2, "p2by2");
// PrintCvMat(circ, "circ");
// PrintCvMat(r, "r");
// PrintCvMat(a, "a");
//PrintCvMat(EllipseF[f], "EllipseF");
}
//<--------------feature ellipse fitting
cvReleaseMat(&p2by2);
cvReleaseMat(&circ );
cvReleaseMat(&p);
cvReleaseMat(&r);
cvReleaseMat(&a);
return 0;
}
