Calculate fs and ss gradients together, SPOT panel number, add matrix hooks

4 files changed, 126 insertions, 251 deletions

diff --git a/libcrystfel/src/crystfel-mille.c b/libcrystfel/src/crystfel-mille.c
index d4c75ef0..84240fc4 100644
--- a/libcrystfel/src/crystfel-mille.c
+++ b/libcrystfel/src/crystfel-mille.c
@@ -74,94 +74,61 @@ int mille_label(int hierarchy_level, int member_index, enum gparam param)
 
 
 void write_mille(Mille *mille, int n, UnitCell *cell,
-                 struct reflpeak *rps, struct image *image)
+                 struct reflpeak *rps, struct image *image,
+                 gsl_matrix **panel_matrices)
 {
-	float local_gradients[9];
-	float global_gradients[64];
-	int labels[6];
 	int i;
-	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
 
 	for ( i=0; i<n; i++ ) {
 
-		signed int h, k, l;
-		double xl, yl, zl, kpred;
+		float local_gradients_fs[9];
+		float local_gradients_ss[9];
+		float global_gradients_fs[64];
+		float global_gradients_ss[64];
+		int labels[6];
 		int j;
 		const struct detgeom_panel_group *group;
 
-		get_indices(rps[i].refl, &h, &k, &l);
-		kpred = get_kpred(rps[i].refl);
-		xl = h*asx + k*bsx + l*csx;
-		yl = h*asy + k*bsy + l*csy;
-		zl = h*asz + k*bsz + l*csz;
-
-		/* fs terms: local */
+		/* Local gradients for fs and ss */
 		for ( j=0; j<9; j++ ) {
-			local_gradients[j] = fs_gradient(rv[j], rps[i].refl,
-			                                 cell, rps[i].panel);
+			fs_ss_gradient(rv[j], rps[i].refl, cell,
+			               &image->detgeom->panels[rps[i].peak->pn],
+			               panel_matrices[rps[i].peak->pn],
+			               &local_gradients_fs[j],
+			               &local_gradients_ss[j]);
 		}
 
-		/* fs terms: global */
+		/* Global gradients for fs and ss */
 		j = 0;
-		group = rps[i].panel->group;
+		group = image->detgeom->panels[rps[i].peak->pn].group;
 		while ( group != NULL ) {
 
-			global_gradients[j] = -1.0;
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_TX);
-			j++;
-
-			global_gradients[j] = -xl / (kpred+zl);
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_TZ);
-			j++;
+			fs_ss_gradient(GPARAM_DET_TX, rps[i].refl, cell,
+			               &image->detgeom->panels[rps[i].peak->pn],
+			               panel_matrices[rps[i].peak->pn],
+			               &global_gradients_fs[j],
+			               &global_gradients_ss[j]);
 
-			global_gradients[j] = 0.0;
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_RZ);
+			labels[j] = mille_label(group->hierarchy_level,
+			                        group->member_index,
+			                        GPARAM_DET_TX);
 			j++;
-
 			group = group->parent;
 		}
 
+		/* Add fs measurement */
 		mille_add_measurement(mille,
-		                      9, local_gradients,
-		                      j, global_gradients, labels,
+		                      9, local_gradients_fs,
+		                      j, global_gradients_fs, labels,
 		                      fs_dev(&rps[i], image->detgeom),
-		                      0.65*rps[i].panel->pixel_pitch);
-
-		/* ss terms: local */
-		for ( j=0; j<9; j++ ) {
-			local_gradients[j] = ss_gradient(rv[j], rps[i].refl,
-			                                 cell, rps[i].panel);
-		}
-
-		/* ss terms: global */
-		j = 0;
-		group = rps[i].panel->group;
-		while ( group != NULL ) {
-
-			global_gradients[j] = -1.0;
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_TY);
-			j++;
-
-			global_gradients[j] = -yl / (kpred+zl);
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_TZ);
-			j++;
-
-			global_gradients[j] = 0.0;
-			labels[j] = mille_label(group->hierarchy_level, group->member_index, GPARAM_DET_RZ);
-			j++;
-
-			group = group->parent;
-		}
+		                      0.65*image->detgeom->panels[rps[i].peak->pn].pixel_pitch);
 
+		/* Add ss measurement */
 		mille_add_measurement(mille,
-		                      9, local_gradients,
-		                      j, global_gradients, labels,
+		                      9, local_gradients_ss,
+		                      j, global_gradients_ss, labels,
 		                      ss_dev(&rps[i], image->detgeom),
-		                      0.65*rps[i].panel->pixel_pitch);
+		                      0.65*image->detgeom->panels[rps[i].peak->pn].pixel_pitch);
 	}
 }
 
diff --git a/libcrystfel/src/crystfel-mille.h b/libcrystfel/src/crystfel-mille.h
index bdd66f37..865d453c 100644
--- a/libcrystfel/src/crystfel-mille.h
+++ b/libcrystfel/src/crystfel-mille.h
@@ -29,6 +29,8 @@
 #ifndef CRYSTFEL_MILLE_H
 #define CRYSTFEL_MILLE_H
 
+#include <gsl/gsl_matrix.h>
+
 typedef void *Mille;
 
 #include "cell.h"
@@ -49,7 +51,8 @@ extern void crystfel_mille_free(Mille *m);
 extern int mille_label(int hierarchy_level, int member_index, enum gparam param);
 
 extern void write_mille(Mille *mille, int n, UnitCell *cell,
-                        struct reflpeak *rps, struct image *image);
+                        struct reflpeak *rps, struct image *image,
+                        gsl_matrix **panel_matrices);
 
 extern void crystfel_mille_delete_last_record(Mille *m);
 
diff --git a/libcrystfel/src/predict-refine.c b/libcrystfel/src/predict-refine.c
index e2adacf5..87a7ce9b 100644
--- a/libcrystfel/src/predict-refine.c
+++ b/libcrystfel/src/predict-refine.c
@@ -139,153 +139,111 @@ double r_gradient(int param, Reflection *refl, UnitCell *cell, double wavelength
 }
 
 
-/* Returns dfs_refl/dP, where P = any parameter *
+/* Returns dfs_refl/dP and fss_refl/dP, where P = any parameter
  * fs_refl is the fast scan position of refl in panel 'p',
  * in metres (not pixels) */
-double fs_gradient(int param, Reflection *refl, UnitCell *cell,
-                   struct detgeom_panel *p)
+int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
+                   struct detgeom_panel *p, gsl_matrix *panel_Minv,
+                   float *fsg, float *ssg)
 {
-	signed int h, k, l;
-	double xl, zl, kpred;
-	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-
-	get_indices(refl, &h, &k, &l);
-	kpred = get_kpred(refl);
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
-	xl = h*asx + k*bsx + l*csx;
-	zl = h*asz + k*bsz + l*csz;
+	//signed int h, k, l;
+	//double xl, yl, zl, kpred;
+	//double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
+
+	//get_indices(refl, &h, &k, &l);
+	//kpred = get_kpred(refl);
+	//cell_get_reciprocal(cell, &asx, &asy, &asz,
+	//                          &bsx, &bsy, &bsz,
+	//                          &csx, &csy, &csz);
+	//xl = h*asx + k*bsx + l*csx;
+	//yl = h*asy + k*bsy + l*csy;
+	//zl = h*asz + k*bsz + l*csz;
 
 	switch ( param ) {
 
 		case GPARAM_ASX :
-		return h * p->cnz * p->pixel_pitch / (kpred + zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_BSX :
-		return k * p->cnz * p->pixel_pitch / (kpred + zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_CSX :
-		return l * p->cnz * p->pixel_pitch / (kpred + zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_ASY :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_BSY :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_CSY :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_ASZ :
-		return -h * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_BSZ :
-		return -k * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_CSZ :
-		return -l * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_TX :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_TY :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_TZ :
-		return -xl / (kpred+zl);
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_RX :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_RY :
-		return 0.0;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		case GPARAM_DET_RZ :
-		return -1;
+		*fsg = 0.0;
+		*ssg = 0.0;
+		return 0;
 
 		default:
-		ERROR("Positional gradient requested for parameter %i?\n", param);
-		abort();
+		return 1;
 
 	}
 
 }
 
 
-/* Returns dss_refl/dP, where P = any parameter
- * ss_refl is the slow scan position of refl in panel 'p',
- * in metres (not pixels) */
-double ss_gradient(int param, Reflection *refl, UnitCell *cell,
-                   struct detgeom_panel *p)
-{
-	signed int h, k, l;
-	double yl, zl, kpred;
-	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-
-	get_indices(refl, &h, &k, &l);
-	kpred = get_kpred(refl);
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
-	yl = h*asy + k*bsy + l*csy;
-	zl = h*asz + k*bsz + l*csz;
-
-	switch ( param ) {
-
-		case GPARAM_ASX :
-		return 0.0;
-
-		case GPARAM_BSX :
-		return 0.0;
-
-		case GPARAM_CSX :
-		return 0.0;
-
-		case GPARAM_ASY :
-		return h * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_BSY :
-		return k * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_CSY :
-		return l * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_ASZ :
-		return -h * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_BSZ :
-		return -k * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_CSZ :
-		return -l * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_DET_TX :
-		return 0.0;
-
-		case GPARAM_DET_TY :
-		return 0.0;
-
-		case GPARAM_DET_TZ :
-		return -yl / (kpred+zl);
-
-		case GPARAM_DET_RX :
-		return 0.0;
-
-		case GPARAM_DET_RY :
-		return 0.0;
-
-		case GPARAM_DET_RZ :
-		return -1;
-
-		default :
-		ERROR("Positional gradient requested for parameter %i?\n", param);
-		abort();
-
-	}
-}
-
-
 static int cmpd2(const void *av, const void *bv)
 {
 	struct reflpeak *a, *b;
@@ -409,7 +367,6 @@ static int pair_peaks(struct image *image, Crystal *cr,
 
 		rps[n].refl = refl;
 		rps[n].peak = f;
-		rps[n].panel = &image->detgeom->panels[f->pn];
 		n++;
 
 	}
@@ -513,7 +470,8 @@ int refine_radius(Crystal *cr, struct image *image)
 }
 
 
-static int iterate(struct reflpeak *rps, int n, UnitCell *cell, struct image *image)
+static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
+                   struct image *image, gsl_matrix **panel_matrices)
 {
 	int i;
 	gsl_matrix *M;
@@ -544,78 +502,23 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell, struct image *im
 	for ( i=0; i<n; i++ ) {
 
 		int k;
-		double gradients[num_params];
+		double r_gradients[num_params];
+		float fs_gradients[num_params];
+		float ss_gradients[num_params];
 		double w;
 
 		w = rps[i].Ih;
 
+		/* Calculate the gradients w.r.t. all parameters, for the
+		 * three measurements (r, fs, ss) of this reflpeak */
 		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = r_gradient(rv[k], rps[i].refl, cell,
-			                          image->lambda);
-		}
+			r_gradients[k] = r_gradient(rv[k], rps[i].refl, cell,
+			                            image->lambda);
+			fs_ss_gradient(rv[k], rps[i].refl, cell,
+			               &image->detgeom->panels[rps[i].peak->pn],
+			               panel_matrices[rps[i].peak->pn],
+			               &fs_gradients[k], &ss_gradients[k]);
 
-		for ( k=0; k<num_params; k++ ) {
-
-			int g;
-			double v_c, v_curr;
-
-			for ( g=0; g<num_params; g++ ) {
-
-				double M_c, M_curr;
-
-				/* Matrix is symmetric */
-				if ( g > k ) continue;
-
-				M_c = w * gradients[g] * gradients[k];
-				M_curr = gsl_matrix_get(M, k, g);
-				gsl_matrix_set(M, k, g, M_curr + M_c);
-				gsl_matrix_set(M, g, k, M_curr + M_c);
-
-			}
-
-			v_c = w * r_dev(&rps[i]);
-			v_c *= -gradients[k];
-			v_curr = gsl_vector_get(v, k);
-			gsl_vector_set(v, k, v_curr + v_c);
-
-		}
-
-		/* Positional fs terms */
-		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = fs_gradient(rv[k], rps[i].refl, cell,
-			                           rps[i].panel);
-		}
-
-		for ( k=0; k<num_params; k++ ) {
-
-			int g;
-			double v_c, v_curr;
-
-			for ( g=0; g<num_params; g++ ) {
-
-				double M_c, M_curr;
-
-				/* Matrix is symmetric */
-				if ( g > k ) continue;
-
-				M_c = gradients[g] * gradients[k];
-				M_curr = gsl_matrix_get(M, k, g);
-				gsl_matrix_set(M, k, g, M_curr + M_c);
-				gsl_matrix_set(M, g, k, M_curr + M_c);
-
-			}
-
-			v_c = fs_dev(&rps[i], image->detgeom);
-			v_c *= -gradients[k];
-			v_curr = gsl_vector_get(v, k);
-			gsl_vector_set(v, k, v_curr + v_c);
-
-		}
-
-		/* Positional ss terms */
-		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = ss_gradient(rv[k], rps[i].refl, cell,
-			                           rps[i].panel);
 		}
 
 		for ( k=0; k<num_params; k++ ) {
@@ -630,15 +533,18 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell, struct image *im
 				/* Matrix is symmetric */
 				if ( g > k ) continue;
 
-				M_c = gradients[g] * gradients[k];
+				M_c = w * r_gradients[g] * r_gradients[k];
+				M_c += w * fs_gradients[g] * fs_gradients[k];
+				M_c += w * ss_gradients[g] * ss_gradients[k];
 				M_curr = gsl_matrix_get(M, k, g);
 				gsl_matrix_set(M, k, g, M_curr + M_c);
 				gsl_matrix_set(M, g, k, M_curr + M_c);
 
 			}
 
-			v_c = ss_dev(&rps[i], image->detgeom);
-			v_c *= -gradients[k];
+			v_c = -w * r_dev(&rps[i]) * r_gradients[k];
+			v_c += -fs_dev(&rps[i], image->detgeom) * fs_gradients[k];
+			v_c += -ss_dev(&rps[i], image->detgeom) * ss_gradients[k];
 			v_curr = gsl_vector_get(v, k);
 			gsl_vector_set(v, k, v_curr + v_c);
 
@@ -709,13 +615,13 @@ static double pred_residual(struct reflpeak *rps, int n, struct detgeom *det)
 
 	r = 0.0;
 	for ( i=0; i<n; i++ ) {
-		r += pow(rps[i].panel->pixel_pitch*fs_dev(&rps[i], det), 2.0);
+		r += pow(det->panels[rps[i].peak->pn].pixel_pitch*fs_dev(&rps[i], det), 2.0);
 	}
 	res += r;
 
 	r = 0.0;
 	for ( i=0; i<n; i++ ) {
-		r += pow(rps[i].panel->pixel_pitch*ss_dev(&rps[i], det), 2.0);
+		r += pow(det->panels[rps[i].peak->pn].pixel_pitch*ss_dev(&rps[i], det), 2.0);
 	}
 	res += r;
 
@@ -744,6 +650,7 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	double max_I;
 	RefList *reflist;
 	char tmp[256];
+	gsl_matrix *panel_matrices[64];
 
 	rps = malloc(image_feature_count(image->features)
 	                       * sizeof(struct reflpeak));
@@ -784,7 +691,7 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	/* Refine (max 10 cycles) */
 	for ( i=0; i<10; i++ ) {
 		update_predictions(cr);
-		if ( iterate(rps, n, crystal_get_cell(cr), image) )
+		if ( iterate(rps, n, crystal_get_cell(cr), image, panel_matrices) )
 		{
 			crystal_set_reflections(cr, NULL);
 			return 1;
@@ -802,7 +709,7 @@ int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 	#ifdef HAVE_MILLEPEDE
 	if ( mille != NULL ) {
 		profile_start("mille-calc");
-		write_mille(mille, n, crystal_get_cell(cr), rps, image);
+		write_mille(mille, n, crystal_get_cell(cr), rps, image, panel_matrices);
 		profile_end("mille-calc");
 	}
 	#endif
diff --git a/libcrystfel/src/predict-refine.h b/libcrystfel/src/predict-refine.h
index 6d15f1e9..e2f912d0 100644
--- a/libcrystfel/src/predict-refine.h
+++ b/libcrystfel/src/predict-refine.h
@@ -29,6 +29,8 @@
 #ifndef PREDICT_REFINE_H
 #define PREDICT_REFINE_H
 
+#include <gsl/gsl_matrix.h>
+
 struct reflpeak;
 
 /** Enumeration of parameters which may want to be refined */
@@ -58,8 +60,6 @@ struct reflpeak {
 	Reflection *refl;
 	struct imagefeature *peak;
 	double Ih;   /* normalised */
-	struct detgeom_panel *panel;  /* panel the reflection appears on
-                                       * (we assume this never changes) */
 };
 
 /**
@@ -80,10 +80,8 @@ extern double ss_dev(struct reflpeak *rp, struct detgeom *det);
 extern double r_gradient(int param, Reflection *refl, UnitCell *cell,
                          double wavelength);
 
-extern double fs_gradient(int param, Reflection *refl, UnitCell *cell,
-                          struct detgeom_panel *p);
-
-extern double ss_gradient(int param, Reflection *refl, UnitCell *cell,
-                          struct detgeom_panel *p);
+extern int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
+                          struct detgeom_panel *p, gsl_matrix *panel_Minv,
+                          float *fsg, float *ssg);
 
 #endif	/* PREDICT_REFINE_H */