-
Notifications
You must be signed in to change notification settings - Fork 5
/
affine.c
97 lines (85 loc) · 4.59 KB
/
affine.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
// Copyright (c) 2015 Osamu Hirose
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
#include<stdio.h>
#include<assert.h>
#include<math.h>
#include"util.h"
#include"lapack.h"
#include"info.h"
int affine(double ** W, /* D+1 x D | Linear map */
double ** T, /* M x D | Moved points */
double ** P, /* M+1 x N+1 | Matching probablity */
double *** C, /* 4 x max(M,N) x D | Working memory */
double * S, /* nlp x M x D | Working wemory (3D) */
const double ** X, /* N x D | Point set 1 (Data) */
const double ** Y, /* M x D | Point set 2 (Data) */
const int size[3], /* M, N, D | D must be 2 or 3 */
const double prms[2], /* parameters: nloop, omg */
const int verb /* flag: verbose */
){
int i,m,n,d,M,N,D,lp,info; char uplo='U';
int nlp=(int)prms[0]; double omg=prms[1],reg=1e-9;
double sgm2=0,noise,pres1=1e10,pres2=1e20,conv;
double mX[3],mY[3],A[9],B[9],a[3];
double **F,**PXc,**Xc,**Yc,**C1;
M=size[0];N=size[1];D=size[2]; assert(D<=3);
F=W;PXc=C[0];Xc=C[1];Yc=C[2];C1=C[3];
/* initialize */
for(m=0;m<M;m++)for(d=0;d<D;d++) T[m][d]=Y[m][d];
for(m=0;m<M;m++)for(n=0;n<N;n++) sgm2+=dist2(X[n],Y[m],D);sgm2/=M*N*D;
/* main computation */
for(lp=0;lp<nlp;lp++){noise=(pow(2.0*M_PI*sgm2,0.5*D)*M*omg)/(N*(1-omg));
if(S)for(m=0;m<M;m++)for(d=0;d<D;d++) S[m+d*M+lp*M*D]=T[m][d];
/* compute matching probability P */
for(n=0;n<=N;n++) P[M][n]=0;
for(m=0;m<=M;m++) P[m][N]=0;
for(m=0;m< M;m++)for(n=0;n<N;n++) P[m][n]=exp(-dist2(X[n],T[m],D)/(2.0*sgm2))+reg;
for(m=0;m< M;m++)for(n=0;n<N;n++) P[M][n]+=P[m][n];
for(m=0;m<=M;m++)for(n=0;n<N;n++) P[m][n]/=P[M][n]+noise;
for(m=0;m< M;m++)for(n=0;n<N;n++) P[m][N]+=P[m][n];
for(m=0;m< M;m++) P[M][N]+=P[m][N];
/* centerize X and Y */
for(d=0;d<D;d++){mX[d]=0;for(n=0;n<N;n++) mX[d]+=X[n][d]*P[M][n];mX[d]/=P[M][N];}
for(d=0;d<D;d++){mY[d]=0;for(m=0;m<M;m++) mY[d]+=Y[m][d]*P[m][N];mY[d]/=P[M][N];}
for(n=0;n<N;n++)for(d=0;d<D;d++) Xc[n][d]=X[n][d]-mX[d];
for(m=0;m<M;m++)for(d=0;d<D;d++) Yc[m][d]=Y[m][d]-mY[d];
/* compute A=Yc'*diag(P1)*Yc and B=Xc'*P'*Yc */
for(m=0;m<M;m++)for(d=0;d<D;d++){PXc[m][d]=0;for(n=0;n<N;n++) PXc[m][d]+=P[m][n]*Xc[n][d];}
for(d=0;d<D;d++)for(i=0;i<D;i++) A[d+i*D]=B[d+i*D]=d==i?reg:0;
for(d=0;d<D;d++)for(i=0;i<D;i++)for(m=0;m<M;m++) A[d+i*D]+=Yc [m][d]*Yc[m][i]*P[m][N];
for(d=0;d<D;d++)for(i=0;i<D;i++)for(m=0;m<M;m++) B[i+d*D]+=PXc[m][d]*Yc[m][i]; //transpose
/* solve AW=B and compute transformation T */
dposv_(&uplo,&D,&D,A,&D,B,&D,&info); assert(!info);
for(d=0;d<D;d++)for(i=0;i<D;i++) F[d][i]=B[i+d*D]; // transpose
for(d=0;d<D;d++){a[d]=mX[d];for(i=0;i<D;i++) a[d]-=F[d][i]*mY[i];}
for(m=0;m<M;m++)for(d=0;d<D;d++){T[m][d]=a[d];for(i=0;i<D;i++)T[m][d]+=Y[m][i]*F[d][i];}
/* compute sgm2 (corresponds to residual) */
pres2=pres1;pres1=sgm2;sgm2=0;
for(m=0;m<M;m++)for(d=0;d<D;d++){C1[m][d]=0;for(i=0;i<D;i++) C1[m][d]+=Yc[m][i]*F[d][i];}
for(n=0;n<N;n++)for(d=0;d<D;d++) sgm2+=SQ(Xc[n][d])*P[M][n];
for(m=0;m<M;m++)for(d=0;d<D;d++) sgm2-=PXc [m][d]*C1[m][d];
sgm2/=P[M][N]*D;
/* check convergence */
conv=log(pres2)-log(sgm2 );
if(verb) printOptIndex('a',lp,P[M][N],sqrt(sgm2),noise,conv);
if(fabs(conv)<1e-8)break;
}
return lp;
}