Vaguely working LSQ lattice refinement (doesn't quite work)

git-svn-id: svn://cook.msm.cam.ac.uk:745/diff-tomo/dtr@214 bf6ca9ba-c028-0410-8290-897cf20841d1

1 files changed, 127 insertions, 168 deletions

diff --git a/src/refine.c b/src/refine.c
index ec74b8d..f9c7a32 100644
--- a/src/refine.c
+++ b/src/refine.c
@@ -19,6 +19,9 @@
 #include <stdio.h>
 #include <assert.h>
 #include <string.h>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_vector.h>
+#include <gsl/gsl_linalg.h>
 
 #include "displaywindow.h"
 #include "gtk-valuegraph.h"
@@ -32,197 +35,153 @@
 #include "utils.h"
 
 typedef enum {
-	INDEX_A = 1<<0,
-	INDEX_B = 1<<1,
-	INDEX_C = 1<<2
-} RefinementIndex;
-
-#if 0
-static const char *refine_decode(RefinementIndex i) {
-
-	switch ( i ) {
-	
-		case INDEX_A			 : return "a--";
-		case INDEX_B			 : return "-b-";
-		case INDEX_C			 : return "--c";
-		case INDEX_A | INDEX_B		 : return "ab-";
-		case INDEX_A | INDEX_C		 : return "a-c";
-		case INDEX_B | INDEX_C		 : return "-bc";
-		case INDEX_A | INDEX_B | INDEX_C : return "abc";
-	
-		default				 : return "---";
-	
-	}
-
-}
-#endif
-
-/* Return the "cell delta" to add to "cell" in order to make "feature" fit its partner */
-static Basis refine_cell_delta(Basis *cell, ImageFeature *feature, RefinementIndex *sharedr) {
-
-	double dix, diy;
-	double dx, dy, dz, twotheta;
-	double old_x, old_y;
-	RefinementIndex index, delta, shared;
-	signed int h, k, l;
-	double a11, a12, a13, a21, a22, a23, a31, a32, a33, det;
-	double dh, dk, dl;
-	Basis cd;
-	
-	h = feature->reflection->h;
-	k = feature->reflection->k;
-	l = feature->reflection->l;
-	
-	cd.a.x = 0.0;	cd.a.y = 0.0;	cd.a.z = 0.0;
-	cd.b.x = 0.0;	cd.b.y = 0.0;	cd.b.z = 0.0;
-	cd.c.x = 0.0;	cd.c.y = 0.0;	cd.c.z = 0.0;
-	
-	/* Determine the difference vector */
-	dix = feature->partner->x - feature->x;
-	diy = feature->partner->y - feature->y;
-//	printf("RF: Feature %3i: %3i %3i %3i dev = %f %f px\n", i, h, k, l, dix, diy);
-	
-	/* Map the difference vector to the relevant tilted plane */
-	old_x = feature->partner->x;
-	old_y = feature->partner->y;
-	feature->partner->x = dix + feature->partner->parent->x_centre;
-	feature->partner->y = diy + feature->partner->parent->y_centre;
-	mapping_map_to_space(feature->partner, &dx, &dy, &dz, &twotheta);
-	feature->partner->x = old_x;
-	feature->partner->y = old_y;
-//	printf("RF: dev=%8e %8e %8e (%5f mrad)\n", dx, dy, dz, twotheta*1e3);
-	
-	/* Select the basis vectors which are allowed to be altered */
-	index = 0;
-	if ( h ) index |= INDEX_A;
-	if ( k ) index |= INDEX_B;
-	if ( l ) index |= INDEX_C;
-	assert(index != 0);	/* Can't refine using the central beam! */
-	
-	/* Set up the coordinate transform from hkl to xyz */
-	a11 = cell->a.x;  a12 = cell->a.y;  a13 = cell->a.z;
-	a21 = cell->b.x;  a22 = cell->b.y;  a23 = cell->b.z;
-	a31 = cell->c.x;  a32 = cell->c.y;  a33 = cell->c.z;
-	
-	/* Invert the matrix to get dh,dk,dl from dx,dy,dz */
-	det = a11*(a22*a33 - a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32 - a22*a31);
-	dh = ((a22*a33-a23*a32)*dx + (a23*a31-a21*a33)*dy + (a21*a32-a22*a31)*dz) / det;
-	dk = ((a13*a32-a12*a33)*dx + (a11*a33-a13*a31)*dy + (a12*a31-a11*a32)*dz) / det;
-	dl = ((a12*a23-a13*a22)*dx + (a13*a21-a11*a23)*dy + (a11*a22-a12*a21)*dz) / det;
-//	printf("RF: dev(hkl) = %f %f %f\n", dh, dk, dl);
-	
-	delta = 0;
-	if ( fabs(dh)/(fabs(dh)+fabs(dk)+fabs(dl)) > 0.001 ) delta |= INDEX_A;
-	if ( fabs(dk)/(fabs(dh)+fabs(dk)+fabs(dl)) > 0.001 ) delta |= INDEX_B;
-	if ( fabs(dl)/(fabs(dh)+fabs(dk)+fabs(dl)) > 0.001 ) delta |= INDEX_C;
-	
-	shared = index & delta;
-	
-//	printf("RF: index: %s\nRF: delta: %s\nRF: shared: %s\n", refine_decode(index), refine_decode(delta), refine_decode(shared));
-	
-	if ( shared == 0 ) {
-		/* No indices left - 'pure shear' (delta is perpendicular (in the abc basis) to index) */
-		shared = index;
-	//	printf("RF: Pure shear.\n");
-	}
-	
-	if ( shared & INDEX_A ) {
-		double w = (double)abs(h) / (abs(h)+abs(k)+abs(l));
-	//	printf("RF: w(a) = %f\n", w);
-		cd.a.x = w*dx / (double)h;
-		cd.a.y = w*dy / (double)h;
-		cd.a.z = w*dz / (double)h;
-	}
-	if ( shared & INDEX_B ) {
-		double w = (double)abs(k) / (abs(h)+abs(k)+abs(l));
-	//	printf("RF: w(b) = %f\n", w);
-		cd.b.x = w*dx / (double)k;
-		cd.b.y = w*dy / (double)k;
-		cd.b.z = w*dz / (double)k;
-	}
-	if ( shared & INDEX_C ) {
-		double w = (double)abs(l) / (abs(h)+abs(k)+abs(l));
-	//	printf("RF: w(c) = %f\n", w);
-		cd.c.x = w*dx / (double)l;
-		cd.c.y = w*dy / (double)l;
-		cd.c.z = w*dz / (double)l;
-	}
-	
-//	printf("RF: Distortion along x: %+8e = %+8e\n", h*cd.a.x + k*cd.b.x + l*cd.c.x, dx);
-//	printf("RF: Distortion along y: %+8e = %+8e\n", h*cd.a.y + k*cd.b.y + l*cd.c.y, dy);
-//	printf("RF: Distortion along z: %+8e = %+8e\n", h*cd.a.z + k*cd.b.z + l*cd.c.z, dz);
-	
-	*sharedr = shared;
-	
-	return cd;
-
-}
+	AXIS_X = 1,
+	AXIS_Y = 2
+} Axis;
 
 /* Use the IPR algorithm to make "cell" fit the given image */
 static ImageFeature *refine_fit_image(Basis *cell, ImageRecord *image, ReflectionList *cell_lattice) {
 	
 	ImageFeatureList *flist;
-	int i;
-	Basis cd; /* Overall cell delta */
-	int n_a = 0;
-	int n_b = 0;
-	int n_c = 0;
-	int done = FALSE;
-	
-	cd.a.x = 0.0;	cd.a.y = 0.0;	cd.a.z = 0.0;
-	cd.b.x = 0.0;	cd.b.y = 0.0;	cd.b.z = 0.0;
-	cd.c.x = 0.0;	cd.c.y = 0.0;	cd.c.z = 0.0;
+	gsl_matrix *M;
+	gsl_vector *q;
+	gsl_vector *p;
+	int j, ns;
+	Axis axis;
+	double dax = 0.0, dbx = 0.0, dcx = 0.0;
+	double day = 0.0, dby = 0.0, dcy = 0.0;
+	double dlx = 0.0, dly = 0.0, dlz = 0.0;
 	
 	if ( !image->rflist ) {
 		image->rflist = reproject_get_reflections(image, cell_lattice);
 	}
 	flist = image->features;
 	
-	for ( i=0; i<image->rflist->n_features; i++ ) {
+	M = gsl_matrix_alloc(3, 3);
+	p = gsl_vector_alloc(3);
+	
+	for ( axis = AXIS_X; axis <= AXIS_Y; axis++ ) {
+	
+		gsl_permutation *perm;
+		int s;
+		
+		gsl_matrix_set_zero(M);
+		gsl_vector_set_zero(p);
+		
+		ns = 0;
+		for ( j=0; j<image->rflist->n_features; j++ ) {
+		
+			double val;
+			ImageFeature *rf;
+			signed int h, k, l;
+			double xy;
+			double dix, diy, dx, dy;
+			double old_x, old_y;
+			
+			rf = &image->rflist->features[j];
+			
+			if ( !rf->partner ) continue;
+			
+			h = rf->reflection->h;
+			k = rf->reflection->k;
+			l = rf->reflection->l;
+			
+			/* Determine the difference vector */
+			dix = rf->partner->x - rf->x;
+			diy = rf->partner->y - rf->y;
+			printf("RF: Feature %3i: %3i %3i %3i dev = %f %f px ", j, h, k, l, dix, diy);
+			
+			old_x = rf->partner->x;
+			old_y = rf->partner->y;
+			rf->partner->x = dix + rf->partner->parent->x_centre;
+			rf->partner->y = diy + rf->partner->parent->y_centre;
+			mapping_scale(rf->partner, &dx, &dy);
+			rf->partner->x = old_x;
+			rf->partner->y = old_y;
+			printf("=> %f %f nm^-1\n", dx/1e9, dy/1e9);
+			
+			xy = 0;
+			switch ( axis ) {
+				case AXIS_X : xy = dx; break;
+				case AXIS_Y : xy = dy; break;
+			}
+			
+			/* Elements of "p" */
+			val = gsl_vector_get(p, 0);	val += xy * h;	gsl_vector_set(p, 0, val);
+			val = gsl_vector_get(p, 1);	val += xy * k;	gsl_vector_set(p, 1, val);
+			val = gsl_vector_get(p, 2);	val += xy * l;	gsl_vector_set(p, 2, val);
+			
+			/* Elements of "M" */
+			val = gsl_matrix_get(M, 0, 0);	val += h * h;	gsl_matrix_set(M, 0, 0, val);
+			val = gsl_matrix_get(M, 0, 1);	val += k * h;	gsl_matrix_set(M, 0, 1, val);
+			val = gsl_matrix_get(M, 0, 2);	val += l * h;	gsl_matrix_set(M, 0, 2, val);
+			
+			val = gsl_matrix_get(M, 1, 0);	val += h * k;	gsl_matrix_set(M, 1, 0, val);
+			val = gsl_matrix_get(M, 1, 1);	val += k * k;	gsl_matrix_set(M, 1, 1, val);
+			val = gsl_matrix_get(M, 1, 2);	val += l * k;	gsl_matrix_set(M, 1, 2, val);
+			
+			val = gsl_matrix_get(M, 2, 0);	val += h * l;	gsl_matrix_set(M, 2, 0, val);
+			val = gsl_matrix_get(M, 2, 1);	val += k * l;	gsl_matrix_set(M, 2, 1, val);
+			val = gsl_matrix_get(M, 2, 2);	val += l * l;	gsl_matrix_set(M, 2, 2, val);
+			
+			ns++;
+		
+		}
 		
-		Basis this_cd;
-		RefinementIndex shared;
+		if ( !ns ) {
+			printf("RF: No partners found\n");
+			gsl_matrix_free(M);
+			gsl_vector_free(p);
+			return NULL;
+		}	
 		
-		/* Skip if no partner */
-		if ( !image->rflist->features[i].partner ) continue;
+		/* Do the fitting */
+		perm = gsl_permutation_alloc(M->size1);
+		gsl_linalg_LU_decomp(M, perm, &s);
 		
-		this_cd = refine_cell_delta(cell, &image->rflist->features[i], &shared);
-		cd = basis_add(cd, this_cd);
-		if ( shared & INDEX_A ) n_a++;
-		if ( shared & INDEX_B ) n_b++;
-		if ( shared & INDEX_C ) n_c++;
+		q = gsl_vector_alloc(3);	/* This is the "answer" */
+		gsl_vector_set_zero(q);
+		gsl_linalg_LU_solve(M, perm, p, q);
 		
-		done = TRUE;
+		switch ( axis ) {
+			case AXIS_X : {
+				dax = gsl_vector_get(q, 0);
+				dbx = gsl_vector_get(q, 1);	/* These are the deviations, in the direction "x" of the image coordinate  */
+				dcx = gsl_vector_get(q, 2);	/* system, of a,b and c */
+				break;
+			}
+			case AXIS_Y : {
+				day = gsl_vector_get(q, 0);
+				dby = gsl_vector_get(q, 1);	/* These are the deviations, in the direction "y" of the image coordinate  */
+				dcy = gsl_vector_get(q, 2);	/* system, of a,b and c */
+				break;
+			}
+		}
 		
-	//	break;
+		gsl_permutation_free(perm);
 		
 	}
 	
-	if ( !done ) {
-		printf("RF: No partners found.\n");
-		return NULL;
-	}
+	gsl_matrix_free(M);
+	gsl_vector_free(p);
+	gsl_vector_free(q);
 	
-	//printf("RF: n_a=%i, n_b=%i, n_c=%i\n", n_a, n_b, n_c);
-	if ( n_a ) {
-		cd.a.x /= (double)n_a;	cd.a.y /= (double)n_a;	cd.a.z /= (double)n_a;
-	}
-	if ( n_b ) {
-		cd.b.x /= (double)n_b;	cd.b.y /= (double)n_b;	cd.b.z /= (double)n_b;
-	}
-	if ( n_c ) {
-		cd.c.x /= (double)n_c;	cd.c.y /= (double)n_c;	cd.c.z /= (double)n_c;
-	}
+	printf("a should change by %f %f nm^-1 in the image plane\n", dax/1e9, day/1e9);
+	printf("b should change by %f %f nm^-1 in the image plane\n", dbx/1e9, dby/1e9);
+	printf("c should change by %f %f nm^-1 in the image plane\n", dcx/1e9, dcy/1e9);
 	
-//	printf("RF: Total distortion(a) = %+8e %+8e %+8e\n", cd.a.x, cd.a.y, cd.a.z);
-//	printf("RF: Total distortion(b) = %+8e %+8e %+8e\n", cd.b.x, cd.b.y, cd.b.z);
-//	printf("RF: Total distortion(c) = %+8e %+8e %+8e\n", cd.c.x, cd.c.y, cd.c.z);
+	/* Update the cell */
+	mapping_rotate(dax, day, 0.0, &dlx, &dly, &dlz, image->omega, image->tilt);
+	printf("a changed by %f %f %f nm^-1 in reciprocal space (%f %f %f %%)\n", dlx, dly, dlz, 100*dlx/cell->a.x, 100*dlx/cell->a.y, 100*dlx/cell->a.z);
+	cell->a.x += dlx;  cell->a.y += dly;  cell->a.z += dlz;
 	
-	cell->a.x += cd.a.x;	cell->a.y += cd.a.y;	cell->a.z += cd.a.z;
-	cell->b.x += cd.b.x;	cell->b.y += cd.b.y;	cell->b.z += cd.b.z;
-	cell->c.x += cd.c.x;	cell->c.y += cd.c.y;	cell->c.z += cd.c.z;
+	mapping_rotate(dbx, dby, 0.0, &dlx, &dly, &dlz, image->omega, image->tilt);
+	printf("b changed by %f %f %f nm^-1 in reciprocal space (%f %f %f %%)\n", dlx, dly, dlz, 100*dlx/cell->b.x, 100*dlx/cell->b.y, 100*dlx/cell->b.z);
+	cell->b.x += dlx;  cell->b.y += dly;  cell->b.z += dlz;
 	
-	//return image->rflist->features[i].partner;
+	mapping_rotate(dcx, dcy, 0.0, &dlx, &dly, &dlz, image->omega, image->tilt);
+	printf("c changed by %f %f %f nm^-1 in reciprocal space (%f %f %f %%)\n", dlx, dly, dlz, 100*dlx/cell->c.x, 100*dlx/cell->c.y, 100*dlx/cell->c.z);
+	cell->c.x += dlx;  cell->c.y += dly;  cell->c.z += dlz;
 	
 	return NULL;