void dist2(double* D, double* X1,int m1, double* X2, int m2, int dim)
{
int i,j,k;
double v,t;
if(!D||!X1||!X2) return;
for(i=0;i<m1;i++){
for(j=0;j<m2;j++){
v = 0;
for(k=0;k<dim;k++) {
t = MAT_GET(X1,k,i,m1)-MAT_GET(X2,k,j,m2);
v += t*t;
}
MAT_SET(D,j,i,v,m1);
}
}
return;
}
/*-------------------------------------------------------------------
C - the optimal assignment.
T - the cost of the optimal assignment.
A - a square cost matrix.
thre- use average*thre as the occlusion
*/
bool DPMatchingOneContourOpen( int *C, double &T, // output
double *A, double thre, // input
int M, int N // A is MxN matrix
)
{
double uplimit = 1000000;
double pen1 = thre;
bool bSucc = true;
int ii,jj;
double dTmp,dTmp1,dTmp2,dTmp3;
double *D = new double[M*N]; // DP matrix
int *links = new int[M*N];
//- initialization
dTmp = MAT_GET(A,0,0,M);
if(dTmp<pen1) { // MAT_SET(D,1,1,dTmp,M);
MAT_SET(D,0,0,dTmp,M);
MAT_SET(links,0,0,1,M);
}
else {
MAT_SET(D,0,0,pen1,M);
MAT_SET(links,0,0,2,M);
}
for(jj=1;jj<N;jj++)
{
dTmp1 = MAT_GET(A,jj,0,M)+(jj-1)*pen1;
dTmp2 = MAT_GET(D,jj-1,0,M)+pen1;
if(dTmp1<dTmp2) {
MAT_SET(D,jj,0,dTmp1,M);
MAT_SET(links,jj,0,1,M);
}
else {
MAT_SET(D,jj,0,dTmp2,M);
MAT_SET(links,jj,0,2,M);
}
}
for(ii=1;ii<M;++ii)
{
dTmp1 = MAT_GET(A,0,ii,M)+(ii-1)*pen1;
dTmp2 = MAT_GET(D,0,ii-1,M)+pen1;
if(dTmp1<dTmp2) {
MAT_SET(D,0,ii,dTmp1,M);
MAT_SET(links,0,ii,1,M);
}
else {
MAT_SET(D,0,ii,dTmp2,M);
MAT_SET(links,0,ii,2,M);
}
}
//- DP looping
for(ii=1;ii<M;++ii)
{
for(jj=1;jj<N;++jj)
{
dTmp1 = MAT_GET(D,jj-1,ii-1,M) + MAT_GET(A,jj,ii,M);
dTmp2 = MAT_GET(D,jj,ii-1,M) + pen1;
dTmp3 = MAT_GET(D,jj-1,ii,M) + pen1;
if(dTmp1<=dTmp2 && dTmp1<=dTmp3) {
MAT_SET(D,jj,ii,dTmp1,M);
MAT_SET(links,jj,ii,1,M);
}
else if(dTmp2<=dTmp3) {
MAT_SET(D,jj,ii,dTmp2,M);
MAT_SET(links,jj,ii,2,M);
}
else {
MAT_SET(D,jj,ii,dTmp3,M);
MAT_SET(links,jj,ii,3,M);
}
}
}
//- Get the mapping result
int OCL = 4*N; // ocllusion using this index
for(ii=0;ii<M;++ii)
C[ii] = OCL;
T = MAT_GET(D,N-1,M-1,M); //D(M,N);
if(T < uplimit)
{
ii = M-1;
jj = N-1;
while(ii>=0 && jj>=0)
{
switch(MAT_GET(links,jj,ii,M)) //links(ii,jj)
{
case 1:
C[ii] = jj;
--ii;
--jj;
break;
case 2:
--ii;
break;
case 3:
--jj;
break;
default:
printf("links[ii,jj]=%d, FAILED!!",MAT_GET(links,jj,ii,M));
delete []D;
delete []links;
return false;
}
}
}
else
{// terminate before final result
printf("Terminate without computing C, T=%lf",T);
bSucc = false;
}
delete []D;
delete []links;
return bSucc;
}
/*-------------------------------------------------------------------
C - the optimal assignment.
T - the cost of the optimal assignment.
A - a square cost matrix,
A(r,c) is the match cost from the r-th point in contour 1 to
the c-th curve on countour 2
thre- use average*thre as the occlusion
*/
bool DPMatchingFixStartPoint( int *C, double &T, // output
double *A, double thre, // input
int M, int N // A is MxN matrix
)
{
double uplimit = 100000000;
double pen1 = thre;
bool bSucc = true;
int pt1,pt2;
double dTmp,dTmp1,dTmp2,dTmp3;
//mexPrintf("from FixStartPoint!\n");
// double *D = new double[M*N]; // DP matrix, D[y,x] is the cost of contour 1 at x
// int *links = new int[M*N]; // matching to contour 2 at y /*
// the above two lines are GLOBAL now, to accelerate the code
/*
for(pt1=0;pt1<M;++pt1)
{
for(pt2=0;pt2<N;++pt2)
{
MAT_SET(D, pt2,pt1,uplimit+1,M);
MAT_SET(links,pt2,pt1,-1, M);
}
}//*/
//- initialization
//mexPrintf("pen1=%lf\n",pen1);
dTmp = MAT_GET(A,0,0,M);
if(dTmp<pen1) { // MAT_SET(D,1,1,dTmp,M);
MAT_SET(D,0,0,dTmp,M);
MAT_SET(links,0,0,1,M);
}
else {
MAT_SET(D,0,0,pen1,M);
MAT_SET(links,0,0,2,M);
}
//pt1 = 0;
//pt2 = 0;
//mexPrintf("\nlinks(%2d,%2d)=%d, D(%2d,%2d)=%lf \n\n",
// pt1,pt2, MAT_GET(links,pt2,pt1,M), pt1,pt2, MAT_GET(D,pt2,pt1,M));
for(pt2=1;pt2<N;pt2++)
{
dTmp1 = MAT_GET(A,pt2,0,M)+pt2*pen1;
dTmp3 = MAT_GET(D,pt2-1,0,M)+pen1;
if(dTmp1<dTmp3) {
MAT_SET(D,pt2,0,dTmp1,M);
MAT_SET(links,pt2,0,1,M);
}
else {
MAT_SET(D,pt2,0,dTmp3,M);
MAT_SET(links,pt2,0,3,M);
}
//pt1 = 0;
//mexPrintf("links(%2d,%2d)=%d, D(%2d,%2d)=%lf \n",
// pt1,pt2, MAT_GET(links,pt2,pt1,M), pt1,pt2, MAT_GET(D,pt2,pt1,M));
}
for(pt1=1;pt1<M;++pt1)
{
dTmp1 = MAT_GET(A,0,pt1,M)+pt1*pen1;
dTmp2 = MAT_GET(D,0,pt1-1,M)+pen1;
if(dTmp1<dTmp2) {
MAT_SET(D,0,pt1,dTmp1,M);
MAT_SET(links,0,pt1,1,M);
}
else {
MAT_SET(D,0,pt1,dTmp2,M);
MAT_SET(links,0,pt1,2,M);
}
//pt2 = 0;
//mexPrintf("links(%2d,%2d)=%d, D(%2d,%2d)=%lf \n",
// pt1,pt2,MAT_GET(links,pt2,pt1,M), pt1,pt2, MAT_GET(D,pt2,pt1,M));
}
//- DP looping
for(pt1=1;pt1<M;++pt1)
{
for(pt2=1;pt2<N;++pt2)
{
dTmp1 = MAT_GET(D,pt2-1,pt1-1,M) + MAT_GET(A,pt2,pt1,M);
dTmp2 = MAT_GET(D,pt2,pt1-1,M) + pen1;
dTmp3 = MAT_GET(D,pt2-1,pt1,M) + pen1;
if(dTmp1<=dTmp2 && dTmp1<=dTmp3) {
MAT_SET(D,pt2,pt1,dTmp1,M);
MAT_SET(links,pt2,pt1,1,M);
}
else if(dTmp2<=dTmp3) {
MAT_SET(D,pt2,pt1,dTmp2,M);
MAT_SET(links,pt2,pt1,2,M);
}
else {
MAT_SET(D,pt2,pt1,dTmp3,M);
MAT_SET(links,pt2,pt1,3,M);
}
}
}
//- Get the mapping result
int OCL = 4*N; // ocllusion using this index
for(pt1=0;pt1<M;++pt1)
C[pt1] = OCL;
T = MAT_GET(D,N-1,M-1,M); //D(M,N);
if(T < uplimit)
{
pt1 = M-1;
pt2 = N-1;
while(pt1>=0 && pt2>=0)
{
switch(MAT_GET(links,pt2,pt1,M)) //links(pt1,pt2)
{
case 1:
C[pt1] = pt2;
--pt1;
--pt2;
break;
case 2:
//C[pt1] = OCL;
--pt1;
break;
case 3:
--pt2;
break;
default:
printf("links[pt1,pt2]=%d, FAILED!!",MAT_GET(links,pt2,pt1,M));
return false;
}
//mexPrintf("pt1=%d, pt2=%d, link(pt1,pt2)=%d, D(pt1,pt2)=%lf \n",
// pt1,pt2, MAT_GET(links,pt2,pt1,M), MAT_GET(D,pt2,pt1,M));
}
}
else
{// terminate before final result
printf("Terminate without computing C, T=%lf",T);
bSucc = false;
}
return bSucc;
}
//----------------------------------------------------------------------
void bellman_ford_allpair(double *dis_mat, double *ang_mat,
double *X, double *Y, int n_V,
double *E, int n_E)
{
const double INF_DIS = n_V*n_E;
const double INF_THRE = INF_DIS-2;
const double PI = 3.14159265;
int n_E1,i,s,u,v,e;
double dx,dy,w,ang;
bool bstop;
int *Ep1 = new int[n_E];
int *Ep2 = new int[n_E];
double *Ew = new double[n_E];
// initialize dis_mat and ang_mat
double *pD,*pA;
pD = dis_mat;
pA = ang_mat;
for(u=0;u<n_V*n_V;u++) {
*(pD++) = INF_DIS;
*(pA++) = -10;
}
// set viewable values
double *pu = E;
double *pv = E+n_E;
double *pw = pv+n_E;
for(e=0;e<n_E;e++)
{
u = (int)pu[e]-1;
v = (int)pv[e]-1;
w = pw[e];
Ep1[e] = u;
Ep2[e] = v;
Ew[e] = w;
// set dis_mat
MAT_SET(dis_mat,u,v,w,n_V);
MAT_SET(dis_mat,v,u,w,n_V);
// set ang_mat
dx = X[v]-X[u];
dy = Y[v]-Y[u];
ang = atan2(dy,dx);
MAT_SET(ang_mat,u,v,ang,n_V);
MAT_SET(ang_mat,v,u,(ang>0)?(ang-PI):(ang+PI),n_V);
}
// shortest pathes from every start point s
int *U = new int[2*n_E];
int *V = new int[2*n_E];
double *W = new double[2*n_E];
for(s=0;s<n_V;s++)
{
pD = dis_mat+s*n_V;
pA = ang_mat+s*n_V;
pD[s] = 0;
//pA[s] = -100;
/*/ reduce the size of graph
memcpy(U,Ep1,n_E*sizeof(int));
memcpy(V,Ep2,n_E*sizeof(int));
memcpy(W,Ew, n_E*sizeof(double));
n_E1 = n_E;/*/
n_E1 = 0;
for(e=0;e<n_E;e++)
{
u = Ep1[e];
v = Ep2[e];
w = Ew[e];
if(pD[u]>INF_THRE) // this node is not viewable from s
{
U[n_E1] = v;
V[n_E1] = u;
W[n_E1] = w;
n_E1++;
}
if(pD[v]>INF_THRE) // this node is not viewable from s
{
U[n_E1] = u;
V[n_E1] = v;
W[n_E1] = w;
n_E1++;
}
}
/* Relaxation using standard bellman-ford*/
bstop = false;
for(i=1; i<n_V-1 && !bstop; i++)
{
bstop = 1;
for(e=0; e<n_E1; ++e)
{
/* Relax for each edge */
u = U[e];
v = V[e];
w = W[e];
if(pD[v]>pD[u]+w) {
pD[v] = pD[u]+w;
pA[v] = pA[u];
//pP[v] = u+1;
bstop = 0;
}
/*
if(pD[u]>pD[v]+w) {
pD[u] = pD[v]+w;
pA[u] = pA[v];
//pP[v] = u+1;
bstop = 0;
}/**/
}
/*printf("i=%d\n",i);*/
}
}//of start point s
delete []U;
delete []V;
delete []W;
delete []Ep1;
delete []Ep2;
delete []Ew;
}
void compu_dist_matrix( double *pSC1, double *pSC2,
int nSamp1, int nSamp2, int nBin1, int nBin2,
int n_dist, int n_theta, int nType,
double *pDist, double *cost_mat
)
{
double *cost_mat_tmp;
double *sc1,*sc2,*pCur;
double *pRowDist;
int r,c,k,k1,k2,ia,id,ir, n_delta, N;
double dis,tmp,minDis,dist1,dist2,res_dis;
/*------------------------------------------------
type : 0 - using the "Hausdorff" distance as in Belongie's paper
1 - find the minimum distance with respect all possible global
rotations /
printf("nType=%d\n",nType); /**/
pRowDist = (double*)malloc(nSamp1*sizeof(double));
if(pRowDist==0) printf("error when malloc!");
switch(nType)
{
case 1:
/* compute the distance between the r-th point in shape 1
and the c-th point in shape 2 */
cost_mat_tmp = (double*)malloc(nSamp1*nSamp2*sizeof(double));
res_dis = 10000;
for(ir=0;ir<n_theta;ir+=2)
{
/* printf("ir=%d\n",ir); */
for(r=0;r<nSamp1;r++) pRowDist[r] = 10000;
n_delta = ir*n_dist;
dist2 = 0;
for(c=0;c<nSamp2;c++)
{
sc2 = pSC2+c*nBin2;
minDis = 10000;
for(r=0;r<nSamp1;r++)
{
sc1 = pSC1+r*nBin1;
dis = 0;
for(k1=0;k1<nBin1;k1++)
{
k2 = k1+n_delta;
if(k2>=nBin2)
k2 = k2-nBin2;
tmp = sc1[k1]+sc2[k2];
if(tmp>0.000001)
dis += (sc1[k1]-sc2[k2])*(sc1[k1]-sc2[k2])/tmp;
}
MAT_SET(cost_mat_tmp,c,r,dis,nSamp1);
if(dis<minDis) minDis = dis;
if(dis<pRowDist[r]) pRowDist[r] = dis;
}
dist2 += minDis;
}
dist2 /= nSamp2;
/* compute the shape context distance */
dist1 = 0;
for(r=0;r<nSamp1;r++) dist1 += pRowDist[r];
dist1 /= nSamp1;
minDis = (dist1<dist2)? dist2:dist1;
if(minDis<res_dis) {
res_dis = minDis;
memcpy(cost_mat,cost_mat_tmp,nSamp1*nSamp2*sizeof(double));
}
}/* of ir=0..n_theta */
free(cost_mat_tmp);
break;
case 0:
default:
/*
N = nBin1*nSamp1;
for(r=0; r<N; ++r)
if(pSC1[r]<0.00001) pSC1[r]=0.00001;
N = nBin2*nSamp1;
for(c=0; c<N; ++c)
if(pSC2[c]<0.00001) pSC2[c]=0.00001;
//*/
/* compute the distance between the r-th point in shape 1
and the c-th point in shape 2 */
for(r=0;r<nSamp1;r++) pRowDist[r] = 10000;
dist2 = 0;
for(c=0;c<nSamp2;c++)
{
sc2 = pSC2+c*nBin2;
minDis = 10000;
for(r=0;r<nSamp1;r++)
{
sc1 = pSC1+r*nBin1;
dis = 0;
for(k=0;k<nBin1;k++)
{
tmp = sc1[k]+sc2[k];
if(tmp>0.000001)
dis += (sc1[k]-sc2[k])*(sc1[k]-sc2[k])/tmp;
}
MAT_SET(cost_mat,c,r,dis,nSamp1);
if(dis<minDis) minDis = dis;
if(dis<pRowDist[r]) pRowDist[r] = dis;
}
dist2 += minDis;
}
dist2 /= nSamp2;
/* compute the shape context distance */
dist1 = 0;
for(r=0;r<nSamp1;r++) dist1 += pRowDist[r];
dist1 /= nSamp1;
res_dis = (dist1<dist2)?dist2:dist1;
break;
}
/* return */
free(pRowDist);
*pDist = res_dis;
}