implement save/restore of distortion parameters

8 files changed, 100 insertions, 34 deletions

diff --git a/src/GipfelWidget.cxx b/src/GipfelWidget.cxx
index 6b51aa5..782a4c3 100644
--- a/src/GipfelWidget.cxx
+++ b/src/GipfelWidget.cxx
@@ -101,6 +101,8 @@ GipfelWidget::load_image(char *file) {
 	set_tilt_angle(md->get_tilt());
 	set_projection((Projection::Projection_t) md->get_projection_type());
 	set_focal_length_35mm(md->get_focal_length_35mm());
+	md->get_distortion_params(&pan->parms.k0, &pan->parms.k1,
+		&pan->parms.u0, &pan->parms.v0);
 
 	delete md;
 
@@ -132,6 +134,8 @@ GipfelWidget::save_image(char *file) {
 	md->set_tilt(get_tilt_angle());
 	md->set_focal_length_35mm(get_focal_length_35mm());
 	md->set_projection_type((int) get_projection());
+	md->set_distortion_params(pan->parms.k0, pan->parms.k1,
+		pan->parms.u0, pan->parms.v0);
 
 	ret = md->save_image(img_file, file);
 	delete md;
diff --git a/src/ImageMetaData.H b/src/ImageMetaData.H
index 5796964..e2d8044 100644
--- a/src/ImageMetaData.H
+++ b/src/ImageMetaData.H
@@ -15,6 +15,10 @@ class ImageMetaData {
 		double direction;
 		double nick;
 		double tilt;
+		double k0;
+		double k1;
+		double u0;
+		double v0;
 		double focal_length_35mm;
 		double scale;
 		int    projection_type;
@@ -37,6 +41,8 @@ class ImageMetaData {
 		double get_tilt();
 		double get_focal_length_35mm();
 		int    get_projection_type();
+		void get_distortion_params(double *_k0, double *_k1,
+			double *_u0, double *_v0);
 
 		void set_longitude(double v);
 		void set_latitude(double v);
@@ -46,5 +52,7 @@ class ImageMetaData {
 		void set_tilt(double v);
 		void set_focal_length_35mm(double v);
 		void set_projection_type(int v);
+		void set_distortion_params(double _k0, double _k1,
+			double _u0, double _v0);
 };
 #endif
diff --git a/src/ImageMetaData.cxx b/src/ImageMetaData.cxx
index 6519239..8e7d381 100644
--- a/src/ImageMetaData.cxx
+++ b/src/ImageMetaData.cxx
@@ -24,6 +24,10 @@ ImageMetaData::ImageMetaData() {
 	direction = 0.0;
 	nick = 0.0;
 	tilt = 0.0;
+	k0 = 0.0;
+	k1 = 0.0;
+	u0 = 0.0;
+	v0 = 0.0;
 	focal_length_35mm = 35.0;
 	scale = NAN;
 	projection_type = 0;
@@ -124,7 +128,7 @@ ImageMetaData::load_image_exif(char *name) {
 
 
 #define GIPFEL_FORMAT_1 "gipfel: longitude %lf, latitude %lf, height %lf, direction %lf, nick %lf, tilt %lf, scale %lf, projection type %d"
-#define GIPFEL_FORMAT_2 "gipfel: longitude %lf, latitude %lf, height %lf, direction %lf, nick %lf, tilt %lf, focal_length_35mm %lf, projection type %d"
+#define GIPFEL_FORMAT_2 "gipfel: longitude %lf, latitude %lf, height %lf, direction %lf, nick %lf, tilt %lf, focal_length_35mm %lf, projection type %d, k0 %lf, k1 %lf, u0 %lf, v0 %lf"
 
 int
 ImageMetaData::load_image_jpgcom(char *name) {
@@ -133,9 +137,9 @@ ImageMetaData::load_image_jpgcom(char *name) {
     pid_t pid;
     int status;
     char buf[1024];
-    double lo, la, he, dir, ni, ti, fr;
+    double lo, la, he, dir, ni, ti, fr, _k0, _k1, _u0, _v0;
     int pt;
-    int ret = 1;
+    int n, ret = 1;
 
     args[0] = "rdjpgcom";
     args[1] = name;
@@ -145,8 +149,8 @@ ImageMetaData::load_image_jpgcom(char *name) {
 
     if (p) {
         while (fgets(buf, sizeof(buf), p) != NULL) {
-            if (sscanf(buf, GIPFEL_FORMAT_2,
-                    &lo, &la, &he, &dir, &ni, &ti, &fr, &pt) >= 7) {
+            if ((n = sscanf(buf, GIPFEL_FORMAT_2,
+                    &lo, &la, &he, &dir, &ni, &ti, &fr, &pt, &_k0, &_k1, &_u0, &_v0)) >= 7) {
 
                 longitude = lo;
                 latitude  = la;
@@ -157,6 +161,13 @@ ImageMetaData::load_image_jpgcom(char *name) {
                 focal_length_35mm = fr;
                 projection_type = pt;
 
+				if (n == 11) {
+					k0 = _k0;
+					k1 = _k1;
+					u0 = _u0;
+					v0 = _v0;
+				}
+
                 ret = 0;
 
                 break;
@@ -226,7 +237,8 @@ ImageMetaData::save_image_jpgcom(char *in_img, char *out_img) {
         nick,
         tilt,
         focal_length_35mm,
-        projection_type);
+        projection_type,
+		k0, k1, u0, v0);
 
     // try to save gipfel data in JPEG comment section
     args[0] = "wrjpgcom";
@@ -338,3 +350,21 @@ void
 ImageMetaData::set_projection_type(int v) {
 	projection_type = v;
 }
+
+void
+ImageMetaData::get_distortion_params(double *_k0, double *_k1,
+	double *_u0, double *_v0) {
+	*_k0 = k0;	
+	*_k1 = k1;	
+	*_u0 = u0;	
+	*_v0 = v0;	
+}
+
+void
+ImageMetaData::set_distortion_params(double _k0, double _k1,
+	double _u0, double _v0) {
+	k0 = _k0;	
+	k1 = _k1;	
+	u0 = _u0;	
+	v0 = _v0;	
+}
diff --git a/src/Panorama.H b/src/Panorama.H
index 505c56d..bfb39b8 100644
--- a/src/Panorama.H
+++ b/src/Panorama.H
@@ -23,7 +23,6 @@ class Panorama {
 		Hills *mountains;
 		Hills *close_mountains;
 		Hills *visible_mountains;
-		ViewParams parms;
 		Projection::Projection *proj;
 		Projection::Projection_t projection_type;
 
@@ -62,6 +61,8 @@ class Panorama {
 		double pi_d, deg2rad;
 
 	public:
+		ViewParams parms;
+
 		Panorama();
 
 		~Panorama();
diff --git a/src/Panorama.cxx b/src/Panorama.cxx
index a5c4baf..b227ebf 100644
--- a/src/Panorama.cxx
+++ b/src/Panorama.cxx
@@ -598,4 +598,3 @@ Panorama::get_coordinates(double a_view, double a_nick, double *x, double *y) {
 
 	return 0;
 }
-
diff --git a/src/ProjectionTangentialLSQ.cxx b/src/ProjectionTangentialLSQ.cxx
index 8c48105..243d0dc 100644
--- a/src/ProjectionTangentialLSQ.cxx
+++ b/src/ProjectionTangentialLSQ.cxx
@@ -24,8 +24,6 @@ static double sec(double a) {
 
 #include "lsq_funcs.c"
 
-static double k0 = 0.0, k1 = 0.0;
-
 static double
 comp_tilt(double tan_nick_view, double tan_dir_view, double n_scale,
 	double tan_nick_m, double tan_dir_m,
@@ -65,11 +63,15 @@ ProjectionTangentialLSQ::comp_params(const Hills *h, ViewParams *parms) {
 		parms->a_center = a_center_tmp;
 		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;
 		}
 
-
 		lsq(h, parms);
 
 		return 0;
@@ -101,7 +103,7 @@ struct data {
 	const Hills *h;
 };
 
-#define CALL(A) A(c_view, c_nick, c_tilt, scale, k0, k1, m->a_view, m->a_nick) 
+#define CALL(A) A(c_view, c_nick, c_tilt, scale, k0, k1, u0, v0, m->a_view, m->a_nick) 
 
 static int
 lsq_f (const gsl_vector * x, void *data, gsl_vector * f) {
@@ -113,6 +115,8 @@ lsq_f (const gsl_vector * x, void *data, gsl_vector * f) {
 	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);
 
 	for (int i=0; i<dat->h->get_num(); i++) {
 		Hill *m = dat->h->get(i);
@@ -138,6 +142,8 @@ lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) {
 	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);
 
 	for (int i=0; i<dat->h->get_num(); i++) {
 		Hill *m = dat->h->get(i);
@@ -148,6 +154,8 @@ lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) {
 		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));
 
 		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));
@@ -155,6 +163,8 @@ lsq_df (const gsl_vector * x, void *data, gsl_matrix * J) {
 		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));
 	}
 
 	return GSL_SUCCESS;
@@ -175,7 +185,7 @@ ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
 	gsl_multifit_fdfsolver *s;
 	gsl_multifit_function_fdf f;
 	struct data dat;
-	double x_init[6];
+	double x_init[8];
 	gsl_vector_view x;
 	int status;
 
@@ -185,10 +195,12 @@ ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
 	x_init[1] = parms->a_nick;
 	x_init[2] = parms->a_tilt;
 	x_init[3] = parms->scale;
-	x_init[4] = k0;
-	x_init[5] = k1;
+	x_init[4] = parms->k0;
+	x_init[5] = parms->k1;
+	x_init[6] = parms->u0;
+	x_init[7] = parms->v0;
 
-	x = gsl_vector_view_array (x_init, 6);
+	x = gsl_vector_view_array (x_init, 8);
 
    f.f = &lsq_f;
    f.df = &lsq_df;
@@ -198,7 +210,7 @@ ProjectionTangentialLSQ::lsq(const Hills *h, ViewParams *parms) {
    f.params = &dat;
 
 	T = gsl_multifit_fdfsolver_lmsder;
-	s = gsl_multifit_fdfsolver_alloc (T, h->get_num() * 2, 6);
+	s = gsl_multifit_fdfsolver_alloc (T, h->get_num() * 2, 8);
 	gsl_multifit_fdfsolver_set (s, &f, &x.vector);
 
     for (int i=0; i<30; i++) {
@@ -215,12 +227,16 @@ 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);
-	k0 = gsl_vector_get(s->x, 4);
-	k1 = gsl_vector_get(s->x, 5);
+	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);
 
 	gsl_multifit_fdfsolver_free (s);
 
-	fprintf(stderr, "k0 %f k1 %f\n", k0, k1);
+	fprintf(stderr, "k0 %f k1 %f u0 %f v0 %f\n",
+		parms->k0, parms->k1, parms->u0, parms->v0);
+
 	return 0;
 }
 
@@ -229,9 +245,9 @@ ProjectionTangentialLSQ::get_coordinates(double a_view, double a_nick,
 	const ViewParams *parms, double *x, double *y) {
 
 	*x = mac_x(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale,
-		k0, k1, a_view, a_nick); 
+		parms->k0, parms->k1, parms->u0, parms->v0, a_view, a_nick); 
 	*y = mac_y(parms->a_center, parms->a_nick, parms->a_tilt, parms->scale,
-		k0, k1, a_view, a_nick); 
+		parms->k0, parms->k1, parms->u0, parms->v0, a_view, a_nick); 
 }
 
 double
diff --git a/src/ViewParams.H b/src/ViewParams.H
index d321253..ee18fa6 100644
--- a/src/ViewParams.H
+++ b/src/ViewParams.H
@@ -13,6 +13,10 @@ class ViewParams {
 		double scale;
 		double a_nick;
 		double a_tilt;
+		double k0;
+		double k1;
+		double u0;
+		double v0;
 };
 
 #endif
diff --git a/src/lsq_funcs.mac b/src/lsq_funcs.mac
index d57008e..3a2cb33 100644
--- a/src/lsq_funcs.mac
+++ b/src/lsq_funcs.mac
@@ -2,26 +2,26 @@
  * This is the basic pinhole projection model with distortion correction
  */
 
-x_undist_unrot : tan(m_view - c_view) $
-y_undist_unrot : tan(c_nick - m_nick) $
-d : y_undist_unrot ^ 2 + x_undist_unrot ^ 2$
-dist_fact : d ^ 2 * k1 + d * k0$
-x_unrot : x_undist_unrot * (1 + dist_fact)$
-y_unrot : y_undist_unrot * (1 + dist_fact)$
-x : scale * (y_unrot * sin(c_tilt) + x_unrot * cos(c_tilt))$
-y : scale * (y_unrot * cos(c_tilt) - x_unrot * sin(c_tilt))$
+x_undist_unrot : tan(m_view - c_view);
+y_undist_unrot : tan(c_nick - m_nick); 
+d : (u0 - x_undist_unrot) ^ 2 + (v0 - y_undist_unrot) ^ 2;
+dist_fact : d ^ 2 * k1 + d * k0;
+x_unrot : x_undist_unrot * (1 + dist_fact);
+y_unrot : y_undist_unrot * (1 + dist_fact);
+x : scale * (y_unrot * sin(c_tilt) + x_unrot * cos(c_tilt));
+y : scale * (y_unrot * cos(c_tilt) - x_unrot * sin(c_tilt));
 
 /*
  * Some mangling for C code generation
  */
 
-x_expand : trigexpand(x)$
-y_expand : trigexpand(y)$
+x_expand : trigexpand(x);
+y_expand : trigexpand(y);
 
-args: "(double c_view, double c_nick, double c_tilt, double scale, double k0, double k1, double m_view, double m_nick)"$
+args: "(double c_view, double c_nick, double c_tilt, double scale, double k0, double k1, double u0, double v0, double m_view, double m_nick)";
 
 printfunc(name, expression) := sprint("static double", name, args, "{ return ", string(subst(pow, "^", expression)), ";}", "
-")$
+");
 
 printfunc("mac_x", x_expand);
 printfunc("mac_y", y_expand);
@@ -32,9 +32,13 @@ printfunc("mac_x_dc_tilt", diff(x_expand, c_tilt));
 printfunc("mac_x_dscale", diff(x_expand, scale));
 printfunc("mac_x_dk0", diff(x_expand, k0));
 printfunc("mac_x_dk1", diff(x_expand, k1));
+printfunc("mac_x_du0", diff(x_expand, u0));
+printfunc("mac_x_dv0", diff(x_expand, v0));
 printfunc("mac_y_dc_view", diff(y_expand, c_view));
 printfunc("mac_y_dc_nick", diff(y_expand, c_nick));
 printfunc("mac_y_dc_tilt", diff(y_expand, c_tilt));
 printfunc("mac_y_dscale", diff(y_expand, scale));
 printfunc("mac_y_dk0", diff(y_expand, k0));
 printfunc("mac_y_dk1", diff(y_expand, k1));
+printfunc("mac_y_du0", diff(x_expand, u0));
+printfunc("mac_y_dv0", diff(x_expand, v0));