void estimate(KalmanFilter f) {
/* Calculate innovation */
multiply_matrix(f.observation_model, f.predicted_state,
f.innovation);
subtract_matrix(f.observation, f.innovation,
f.innovation);
/* Calculate innovation covariance */
multiply_by_transpose_matrix(f.predicted_estimate_covariance,
f.observation_model,
f.vertical_scratch);
multiply_matrix(f.observation_model, f.vertical_scratch,
f.innovation_covariance);
add_matrix(f.innovation_covariance, f.observation_noise_covariance,
f.innovation_covariance);
/* Invert the innovation covariance.
Note: this destroys the innovation covariance.
TODO: handle inversion failure intelligently. */
destructive_invert_matrix(f.innovation_covariance,
f.inverse_innovation_covariance);
/* Calculate the optimal Kalman gain.
Note we still have a useful partial product in vertical scratch
from the innovation covariance. */
multiply_matrix(f.vertical_scratch, f.inverse_innovation_covariance,
f.optimal_gain);
/* Estimate the state */
multiply_matrix(f.optimal_gain, f.innovation,
f.state_estimate);
add_matrix(f.state_estimate, f.predicted_state,
f.state_estimate);
/* Estimate the state covariance */
multiply_matrix(f.optimal_gain, f.observation_model,
f.big_square_scratch);
subtract_from_identity_matrix(f.big_square_scratch);
multiply_matrix(f.big_square_scratch, f.predicted_estimate_covariance,
f.estimate_covariance);
}
static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
{
double **src_mat = NULL;
double **src_mat_T = NULL;
double **dest_mat = NULL;
double **dest_mat_T = NULL;
double **src_dest_mat = NULL;
double *S_vec = NULL;
double **R_mat = NULL;
double **R_mat_T = NULL;
double **mat_mn1 = NULL;
double **mat_mn2 = NULL;
double **mat_nm1 = NULL;
double **mat_nm2 = NULL;
double **mat_nn1 = NULL;
double **E_mat = NULL;
double **P_mat = NULL;
double **Q_mat = NULL;
double *D_vec = NULL;
double *one_vec = NULL;
double trace1 = 0.0;
double trace2 = 0.0;
int numactive; /* NUMBER OF ACTIVE CONTROL POINTS */
int m, n, i, j;
int status;
/* CALCULATE THE NUMBER OF VALID CONTROL POINTS */
for (i = numactive = 0; i < cp->count; i++) {
if (cp->status[i] > 0)
numactive++;
}
m = numactive;
n = ndims;
src_mat = G_alloc_matrix(m, n);
dest_mat = G_alloc_matrix(m, n);
for (i = numactive = 0; i < cp->count; i++) {
if (cp->status[i] > 0) {
src_mat[numactive][0] = cp->e1[i];
src_mat[numactive][1] = cp->n1[i];
src_mat[numactive][2] = cp->z1[i];
dest_mat[numactive][0] = cp->e2[i];
dest_mat[numactive][1] = cp->n2[i];
dest_mat[numactive][2] = cp->z2[i];
numactive++;
}
}
D_vec = G_alloc_vector(ndims);
src_mat_T = G_alloc_matrix(n, m);
dest_mat_T = G_alloc_matrix(n, m);
src_dest_mat = G_alloc_matrix(n, n);
R_mat = G_alloc_matrix(n, n);
R_mat_T = G_alloc_matrix(n, n);
mat_mn1 = G_alloc_matrix(m, n);
mat_mn2 = G_alloc_matrix(m, n);
mat_nm1 = G_alloc_matrix(n, m);
mat_nm2 = G_alloc_matrix(n, m);
mat_nn1 = G_alloc_matrix(n, n);
E_mat = G_alloc_matrix(m, m);
P_mat = G_alloc_matrix(ndims, ndims);
Q_mat = G_alloc_matrix(ndims, ndims);
transpose_matrix(m, n, dest_mat, dest_mat_T);
for (i = 0; i < m; i++) {
for (j = 0; j < m; j++) {
if (i != j) {
E_mat[i][j] = -1.0 / (double)m;
}
else{
E_mat[i][j] = 1.0 - 1.0 / (double)m;
}
}
}
matmult(n, m, m, dest_mat_T, E_mat, mat_nm1);
matmult(n, m, n, mat_nm1, src_mat, src_dest_mat);
copy_matrix(n, n, src_dest_mat, P_mat);
copy_matrix(n, n, src_dest_mat, mat_nn1);
status = G_math_svduv(D_vec, mat_nn1, P_mat, n, Q_mat, n);
if (status == 0)
status = MSUCCESS;
transpose_matrix(n, n, P_mat, mat_nn1);
/* rotation matrix */
matmult(n, n, n, Q_mat, mat_nn1, R_mat_T);
transpose_matrix(n, n, R_mat_T, R_mat);
/* scale */
matmult(n, n, n, src_dest_mat, R_mat_T, mat_nn1);
trace1 = trace(n, n, mat_nn1);
transpose_matrix(m, n, src_mat, src_mat_T);
matmult(n, m, m, src_mat_T, E_mat, mat_nm1);
matmult(n, m, n, mat_nm1, src_mat, mat_nn1);
trace2 = trace(n, n, mat_nn1);
OR[14] = trace1 / trace2;
/* shifts */
matmult(m, n, n, src_mat, R_mat_T, mat_mn1);
scale_matrix(m, n, OR[14], mat_mn1, mat_mn2);
subtract_matrix(m, n, dest_mat, mat_mn2, mat_mn1);
scale_matrix(m, n, 1.0 / m, mat_mn1, mat_mn2);
transpose_matrix(m, n, mat_mn2, mat_nm1);
S_vec = G_alloc_vector(n);
one_vec = G_alloc_vector(m);
for (i = 0; i < m; i++){
one_vec[i] = 1.0;
}
matrix_multiply(n, m, mat_nm1, one_vec, S_vec);
/* matrix to vector */
for (i = 0; i < ndims; i++) {
for (j = 0; j < ndims; j++) {
OR[i * ndims + j] = R_mat[i][j];
}
}
G_free_matrix(src_mat);
G_free_matrix(src_mat_T);
G_free_matrix(dest_mat);
G_free_matrix(dest_mat_T);
G_free_matrix(src_dest_mat);
G_free_vector(D_vec);
G_free_matrix(E_mat);
G_free_matrix(P_mat);
G_free_matrix(Q_mat);
G_free_matrix(R_mat);
G_free_matrix(R_mat_T);
G_free_matrix(mat_mn1);
G_free_matrix(mat_mn2);
G_free_matrix(mat_nm1);
G_free_matrix(mat_nm2);
G_free_matrix(mat_nn1);
G_free_vector(S_vec);
G_free_vector(one_vec);
return status;
}
int transform(const char * source, const char * destination, const char * output){
char src_pts_name[256];
char dest_pts_name[256];
char out_param_name[256];
int n=3;
int m=0;
int m2=0;
int k,l;
double **src_mat=NULL;
double **dest_mat=NULL;
double **dest_mat_T=NULL;
double **src_mat_T=NULL;
double **E_mat=NULL;
double **C_mat=NULL;
double **C_mat_interm=NULL;
double **D_mat_interm=NULL;
double **P_mat=NULL;
double *D_vec=NULL;
double *T_vec=NULL;
double *one_vec=NULL;
double **D_mat=NULL;
double **Q_mat=NULL;
double **P_mat_T=NULL;
double **R_mat=NULL;
double trace1=0.0;
double trace2=0.0;
double scal=0.0;
double ppm=0.0;
FILE *outfile;
printf("\n*******************************\n");
printf( "* helmparms3d v%1.2f *\n",VERS);
printf( "* (c) U. Niethammer 2011 *\n");
printf( "* http://helmparms3d.sf.net *\n");
printf( "*******************************\n");
memset(src_pts_name,0,sizeof(src_pts_name));
memset(dest_pts_name,0,sizeof(dest_pts_name));
memset(out_param_name,0,sizeof(out_param_name));
strcpy(src_pts_name, source);
strcpy(dest_pts_name, destination);
strcpy(out_param_name, output);
m=get_m_size(src_pts_name);
m2=get_m_size(dest_pts_name);
if(m2!=m){
printf("Error, number of source and destination points is not equal!\n");
}
else
{
src_mat=matrix(m,m, src_mat);
dest_mat=matrix(m,m, dest_mat);
read_points(src_pts_name, src_mat);
read_points(dest_pts_name, dest_mat);
D_vec=vector(n, D_vec);
E_mat=matrix(m, m, E_mat);
P_mat=matrix(m, m, P_mat);
D_mat=matrix(m, m, D_mat);
Q_mat=matrix(m, m, Q_mat);
P_mat_T=matrix(m, m, P_mat_T);
R_mat=matrix(m, m, R_mat);
dest_mat_T=matrix(m, m, dest_mat_T);
C_mat=matrix(m, m, C_mat);
C_mat_interm=matrix(m, m, C_mat_interm);
src_mat_T=matrix(m, m, src_mat_T);
D_mat_interm=matrix(m, m, D_mat_interm);
transpose_matrix(m, m, dest_mat, dest_mat_T);
if(debug)printf("%s_T:\n",dest_pts_name);
if(debug)plot_matrix(stdout, n, m, dest_mat_T);
for(k=0;k<m;k++){
for(l=0;l<m;l++){
if(k!=l){
E_mat[k][l]=-1.0/(double)m;
}
else{
E_mat[k][l]=1.0-1.0/(double)m;
}
}
}
if(debug)printf("E:\n");
if(debug)plot_matrix(stdout, m, m, E_mat);
if(debug)printf("dest_mat_T:\n");
if(debug)plot_matrix(stdout, n, m, dest_mat_T);
matmult(dest_mat_T, m, m, E_mat, m, m, C_mat_interm, m, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, m, C_mat_interm);
matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n);
if(debug)printf("C:\n");
if(debug)plot_matrix(stdout, n, n, C_mat);
copy_matrix(n,n,C_mat,P_mat);
if(debug)printf("P:\n");
if(debug)plot_matrix(stdout, n, n, P_mat);
//Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n].
svd(n, n, C_mat, P_mat, D_vec, Q_mat);
transpose_matrix(n, n, P_mat, P_mat_T);
if(debug)printf("P\n");
if(debug)plot_matrix(stdout, n, n, P_mat);
if(debug)printf("P_T\n");
if(debug)plot_matrix(stdout, n, n, P_mat_T);
if(debug)printf("D_vec\n");
if(debug)plot_vector(stdout, n, D_vec);
for(k=0;k<n;k++){
for(l=0;l<n;l++){
D_mat[k][l]=0.0;
D_mat[l][l]=D_vec[l];
}
}
if(debug)printf("D\n");
if(debug)plot_matrix(stdout, n, n, D_mat);
matmult(Q_mat, n, n, P_mat_T, n, n, R_mat, n, n);
if(debug)printf("R_trans:\n");
if(debug)plot_matrix(stdout, n, n, R_mat);
matmult(C_mat, m, n, R_mat, n, m, C_mat_interm, m, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, n, C_mat_interm);
trace1=trace(n,n,C_mat_interm);
if(debug)printf("\ntra=%lf\n\n",trace1);
transpose_matrix(m, m, src_mat, src_mat_T);
if(debug)printf("%s_T:\n",src_pts_name);
if(debug)plot_matrix(stdout, n, m, src_mat_T);
init_matrix(m,m,C_mat);
init_matrix(m,m,C_mat_interm);
matmult(src_mat_T, m, m, E_mat, m, m, C_mat_interm, n, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, m, C_mat_interm);
matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n);
if(debug)printf("C:\n");
if(debug)plot_matrix(stdout, n, n, C_mat);
trace2=trace(n,n,C_mat);
if(debug)printf("\ntra=%lf\n\n",trace2);
scal=trace1/trace2;
ppm=scal-1.0;
if(debug)printf("\nscal = %10.10lf\nscal = %10.10lf ppm\n\n",scal, ppm);
init_matrix(m,m,C_mat);
init_matrix(m,m,C_mat_interm);
matmult(src_mat, m, n, R_mat, n,m, D_mat_interm, m, n);
if(debug)printf("C_mat_interm:\n");
if(debug)plot_matrix(stdout, m, n, D_mat_interm);
scal_matrix(m, n, scal, D_mat_interm, C_mat_interm);
if(debug)printf("C_mat_interm:\n");
if(debug)plot_matrix(stdout, m, n, C_mat_interm);
subtract_matrix(m, n, dest_mat, C_mat_interm, D_mat_interm);
if(debug)plot_matrix(stdout, m, n, D_mat_interm);
scal_matrix(m, n, 1.0/m, D_mat_interm, C_mat_interm);
if(debug)plot_matrix(stdout, m, n, C_mat_interm);
init_matrix(m,m,src_mat_T);
transpose_matrix(m, m, C_mat_interm, src_mat_T);
if(debug)plot_matrix(stdout, n, m, src_mat_T);
T_vec=vector(m, T_vec);
one_vec=vector(m, one_vec);
for(k=0;k<m;k++){
one_vec[k]=1.0;
}
matrix_multiply(n, m, src_mat_T, one_vec, T_vec);
if(debug)printf("T:\n");
if(debug)plot_vector(stdout, 3, T_vec);
outfile = fopen(out_param_name, "w");
if(outfile == NULL){
printf("Error writing %s\r\n",out_param_name);
exit(-1);
}
init_matrix(m,m,src_mat_T);
transpose_matrix(m, m, R_mat, src_mat_T);
plot_matrix(outfile, n, n, src_mat_T);
printf("R =\n");fflush(stdout);
plot_matrix(stdout, n, n, src_mat_T);
printf("\n");fflush(stdout);
plot_vector(outfile, 3, T_vec);
printf("T =\n");fflush(stdout);
plot_vector(stdout, 3, T_vec);
printf("\n");fflush(stdout);
fprintf(outfile, "%10.10lf\n", scal);
printf("s = %10.10lf (= %10.10lf ppm)\n\n",scal, ppm);fflush(stdout);
fclose(outfile);
freevector(D_vec);
freevector(T_vec);
freevector(one_vec);
freematrix(m, src_mat);
freematrix(m, dest_mat);
freematrix(m, E_mat);
freematrix(m, P_mat);
freematrix(m, D_mat);
freematrix(m, Q_mat);
freematrix(m, P_mat_T);
freematrix(m, R_mat);
freematrix(m, dest_mat_T);
freematrix(m, C_mat);
freematrix(m, C_mat_interm);
freematrix(m, src_mat_T);
freematrix(m, D_mat_interm);
printf("\n...done\n");
}
}
