summaryrefslogtreecommitdiff
path: root/src/ProjectionTangentialLSQ.cxx
diff options
context:
space:
mode:
Diffstat (limited to 'src/ProjectionTangentialLSQ.cxx')
-rw-r--r--src/ProjectionTangentialLSQ.cxx129
1 files changed, 88 insertions, 41 deletions
diff --git a/src/ProjectionTangentialLSQ.cxx b/src/ProjectionTangentialLSQ.cxx
index 3afa2e3..b201d31 100644
--- a/src/ProjectionTangentialLSQ.cxx
+++ b/src/ProjectionTangentialLSQ.cxx
@@ -34,9 +34,15 @@ ProjectionTangentialLSQ::comp_params(const Hills *h, ViewParams *parms) {
const Hill *tmp, *m1, *m2;
double a_center_tmp, scale_tmp, a_nick_tmp;
- if (h->get_num() < 3) {
+ if (h->get_num() < 2) {
fprintf(stderr, "Please position at least 3 hills\n");
return 1;
+ } else if (h->get_num() > 3) {
+ fprintf(stderr, "Performing calibration\n");
+ parms->k0 = 0.0;
+ parms->k1 = 0.0;
+ parms->u0 = 0.0;
+ parms->v0 = 0.0;
}
m1 = h->get(0);
@@ -64,10 +70,7 @@ ProjectionTangentialLSQ::comp_params(const Hills *h, ViewParams *parms) {
parms->scale = scale_tmp;
parms->a_nick = a_nick_tmp;
parms->a_tilt = 0.0;
- parms->k0 = 0.0;
- parms->k1 = 0.0;
- parms->u0 = 0.0;
- parms->v0 = 0.0;
+
if (angle_dist(parms->a_center, m1->alph) > pi_d/2.0) {
parms->a_center = parms->a_center + pi_d;
}
@@ -101,6 +104,7 @@ ProjectionTangentialLSQ::angle_dist(double a1, double a2) {
struct data {
const Hills *h;
+ const ViewParams *old_params;
};
#define CALL(A) A(c_view, c_nick, c_tilt, scale, k0, k1, u0, v0, m->a_view, m->a_nick)
@@ -108,15 +112,30 @@ struct data {
static int
lsq_f (const gsl_vector * x, void *data, gsl_vector * f) {
struct data *dat = (struct data *) data;
+ double c_view, c_nick, c_tilt, scale, k0, k1, u0, v0;
+
+ c_view = gsl_vector_get (x, 0);
+ c_nick = gsl_vector_get (x, 1);
+ c_tilt = gsl_vector_get (x, 2);
+ scale = gsl_vector_get (x, 3);
+ if (x->size >= 6) {
+ k0 = gsl_vector_get (x, 4);
+ k1 = gsl_vector_get (x, 5);
+#if 0
+ u0 = gsl_vector_get (x, 6);
+ v0 = gsl_vector_get (x, 7);
+#else
+ u0 = dat->old_params->u0;
+ v0 = dat->old_params->v0;
+#endif
- double c_view = gsl_vector_get (x, 0);
- double c_nick = gsl_vector_get (x, 1);
- double c_tilt = gsl_vector_get (x, 2);
- double scale = gsl_vector_get (x, 3);
- double k0 = gsl_vector_get (x, 4);
- double k1 = gsl_vector_get (x, 5);
- double u0 = gsl_vector_get (x, 6);
- double v0 = gsl_vector_get (x, 7);
+
+ } else {
+ k0 = dat->old_params->k0;
+ k1 = dat->old_params->k1;
+ u0 = dat->old_params->u0;
+ v0 = dat->old_params->v0;
+ }
for (int i=0; i<dat->h->get_num(); i++) {
Hill *m = dat->h->get(i);
@@ -135,15 +154,29 @@ lsq_f (const gsl_vector * x, void *data, gsl_vector * f) {
static int
lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) {
struct data *dat = (struct data *) data;
-
- double c_view = gsl_vector_get (x, 0);
- double c_nick = gsl_vector_get (x, 1);
- double c_tilt = gsl_vector_get (x, 2);
- double scale = gsl_vector_get (x, 3);
- double k0 = gsl_vector_get (x, 4);
- double k1 = gsl_vector_get (x, 5);
- double u0 = gsl_vector_get (x, 6);
- double v0 = gsl_vector_get (x, 7);
+ double c_view, c_nick, c_tilt, scale, k0, k1, u0, v0;
+
+ c_view = gsl_vector_get (x, 0);
+ c_nick = gsl_vector_get (x, 1);
+ c_tilt = gsl_vector_get (x, 2);
+ scale = gsl_vector_get (x, 3);
+ if (x->size >= 6) {
+ k0 = gsl_vector_get (x, 4);
+ k1 = gsl_vector_get (x, 5);
+#if 0
+ u0 = gsl_vector_get (x, 6);
+ v0 = gsl_vector_get (x, 7);
+#else
+ u0 = dat->old_params->u0;
+ v0 = dat->old_params->v0;
+
+#endif
+ } else {
+ k0 = dat->old_params->k0;
+ k1 = dat->old_params->k1;
+ u0 = dat->old_params->u0;
+ v0 = dat->old_params->v0;
+ }
for (int i=0; i<dat->h->get_num(); i++) {
Hill *m = dat->h->get(i);
@@ -152,19 +185,27 @@ lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) {
gsl_matrix_set (J, 2*i, 1, CALL(mac_x_dc_nick));
gsl_matrix_set (J, 2*i, 2, CALL(mac_x_dc_tilt));
gsl_matrix_set (J, 2*i, 3, CALL(mac_x_dscale));
- gsl_matrix_set (J, 2*i, 4, CALL(mac_x_dk0));
- gsl_matrix_set (J, 2*i, 5, CALL(mac_x_dk1));
- gsl_matrix_set (J, 2*i, 6, CALL(mac_x_du0));
- gsl_matrix_set (J, 2*i, 7, CALL(mac_x_dv0));
+ if (x->size >= 6) {
+ gsl_matrix_set (J, 2*i, 4, CALL(mac_x_dk0));
+ gsl_matrix_set (J, 2*i, 5, CALL(mac_x_dk1));
+#if 0
+ gsl_matrix_set (J, 2*i, 6, CALL(mac_x_du0));
+ gsl_matrix_set (J, 2*i, 7, CALL(mac_x_dv0));
+#endif
+ }
gsl_matrix_set (J, 2*i+1, 0, CALL(mac_y_dc_view));
gsl_matrix_set (J, 2*i+1, 1, CALL(mac_y_dc_nick));
gsl_matrix_set (J, 2*i+1, 2, CALL(mac_y_dc_tilt));
gsl_matrix_set (J, 2*i+1, 3, CALL(mac_y_dscale));
- gsl_matrix_set (J, 2*i+1, 4, CALL(mac_y_dk0));
- gsl_matrix_set (J, 2*i+1, 5, CALL(mac_y_dk1));
- gsl_matrix_set (J, 2*i+1, 6, CALL(mac_y_du0));
- gsl_matrix_set (J, 2*i+1, 7, CALL(mac_y_dv0));
+ if (x->size >= 6) {
+ gsl_matrix_set (J, 2*i+1, 4, CALL(mac_y_dk0));
+ gsl_matrix_set (J, 2*i+1, 5, CALL(mac_y_dk1));
+#if 0
+ gsl_matrix_set (J, 2*i+1, 6, CALL(mac_y_du0));
+ gsl_matrix_set (J, 2*i+1, 7, CALL(mac_y_dv0));
+#endif
+ }
}
return GSL_SUCCESS;
@@ -178,18 +219,19 @@ lsq_fdf (const gsl_vector * x, void *data, gsl_vector * f, gsl_matrix * J) {
return GSL_SUCCESS;
}
-#define NUM_PARAMS 8
int
ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
const gsl_multifit_fdfsolver_type *T;
gsl_multifit_fdfsolver *s;
gsl_multifit_function_fdf f;
struct data dat;
- double x_init[NUM_PARAMS];
+ double x_init[8];
gsl_vector_view x;
int status;
+ int num_params = h->get_num()>3?6:4;
dat.h = h;
+ dat.old_params = parms;
x_init[0] = parms->a_center;
x_init[1] = parms->a_nick;
@@ -200,24 +242,24 @@ ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
x_init[6] = parms->u0;
x_init[7] = parms->v0;
- x = gsl_vector_view_array (x_init, NUM_PARAMS);
+ x = gsl_vector_view_array (x_init, num_params);
f.f = &lsq_f;
f.df = &lsq_df;
f.fdf = &lsq_fdf;
f.n = h->get_num() * 2;
- f.p = NUM_PARAMS;
+ f.p = num_params;
f.params = &dat;
T = gsl_multifit_fdfsolver_lmsder;
- s = gsl_multifit_fdfsolver_alloc (T, h->get_num() * 2, NUM_PARAMS);
+ s = gsl_multifit_fdfsolver_alloc (T, h->get_num() * 2, num_params);
gsl_multifit_fdfsolver_set (s, &f, &x.vector);
- for (int i=0; i<10; i++) {
+ for (int i=0; i<100; i++) {
status = gsl_multifit_fdfsolver_iterate (s);
- fprintf(stderr, "gsl_multifit_fdfsolver_iterate: status %d\n", status);
if (status) {
+ fprintf(stderr, "gsl_multifit_fdfsolver_iterate: %d\n", status);
break;
}
@@ -227,10 +269,15 @@ ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
parms->a_nick = gsl_vector_get(s->x, 1);
parms->a_tilt = gsl_vector_get(s->x, 2);
parms->scale = gsl_vector_get(s->x, 3);
- parms->k0 = gsl_vector_get(s->x, 4);
- parms->k1 = gsl_vector_get(s->x, 5);
- parms->u0 = gsl_vector_get(s->x, 6);
- parms->v0 = gsl_vector_get(s->x, 7);
+
+ if (num_params == 6) {
+ parms->k0 = gsl_vector_get(s->x, 4);
+ parms->k1 = gsl_vector_get(s->x, 5);
+#if 0
+ parms->u0 = gsl_vector_get(s->x, 6);
+ parms->v0 = gsl_vector_get(s->x, 7);
+#endif
+ }
gsl_multifit_fdfsolver_free (s);