v.rectify: use ccmath svd

git-svn-id: https://svn.osgeo.org/grass/grass/trunk@51547 15284696-431f-4ddb-bdfa-cd5b030d7da7

vector/v.rectify/crs.h

@@ -27,20 +27,6 @@ int CRS_georef_3d(double, double, double,
 		  double *, double *, double *,
 		  int);
 
-/* helmertg.c */
-int CRS_compute_georef_equations_hg(struct Control_Points_3D *,
-                                    double *, double *);
-int CRS_georef_hg(double, double, double,
-                  double *, double *, double *,
-		  double *);
-
-/* helmertq.c */
-int CRS_compute_georef_equations_hq(struct Control_Points_3D *,
-                                    double *, double *);
-int CRS_georef_hq(double, double, double,
-                  double *, double *, double *,
-		  double *);
-
 /* orthorot.c */
 int CRS_compute_georef_equations_or(struct Control_Points_3D *,
                                     double *, double *);

vector/v.rectify/orthorot.c

@@ -40,7 +40,7 @@
 static int calccoef(struct Control_Points_3D *, double *, int);
 static int calcscale(struct Control_Points_3D *, double *);
 
-void transpose_matrix(int, double **, double **);
+void transpose_matrix(int, int, double **, double **);
 void matmult(int, int, double **, double **, double **);
 void copy_matrix(int, int, double **, double **);
 void init_matrix(int, int, double **);
@@ -49,8 +49,6 @@ void matrix_multiply(int, int, double **, double *, double *);
 void subtract_matrix(int, int, double **, double **, double **);
 double trace(int, int, double **);
 
-void svd(int, int, double **, double **, double *, double **);
-
 /***********************************************************************
 
   TRANSFORM A SINGLE COORDINATE PAIR.
@@ -298,9 +296,12 @@ static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
     double *S_vec = NULL;
     double **R_mat = NULL;
     double **R_mat_T = NULL;
-    double **C_mat = NULL;
-    double **C_mat_interm = NULL;
-    double **D_mat_interm = NULL;
+    double **mat_mn1 = NULL;
+    double **mat_mn2 = NULL;
+    double **mat_nm1 = NULL;
+    double **mat_nm2 = NULL;
+    double **mat_nn1 = NULL;
+    double **mat_nn2 = NULL;
     double **E_mat = NULL;
     double **P_mat = NULL;
     double **P_mat_T = NULL;
@@ -310,7 +311,7 @@ static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
     double trace1 = 0.0;
     double trace2 = 0.0;
     int numactive;		/* NUMBER OF ACTIVE CONTROL POINTS */
-    int i, j;
+    int m, n, i, j;
     int status;
 
     /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */
@@ -319,9 +320,11 @@ static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
 	if (cp->status[i] > 0)
 	    numactive++;
     }
+    m = numactive;
+    n = ndims;
 
-    src_mat = G_alloc_matrix(numactive, numactive);
-    dest_mat = G_alloc_matrix(numactive, numactive);
+    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) {
@@ -339,80 +342,78 @@ static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
 
     D_vec = G_alloc_vector(ndims);
 
-    src_mat_T = G_alloc_matrix(numactive, numactive);
-    dest_mat_T = G_alloc_matrix(numactive, numactive);
-    R_mat = G_alloc_matrix(ndims, ndims);
-    R_mat_T = G_alloc_matrix(numactive, numactive);
+    src_mat_T = G_alloc_matrix(n, m);
+    dest_mat_T = G_alloc_matrix(n, m);
+    R_mat = G_alloc_matrix(n, n);
+    R_mat_T = G_alloc_matrix(n, n);
 
-    C_mat = G_alloc_matrix(numactive, numactive);
-    C_mat_interm = G_alloc_matrix(numactive, numactive);
-    D_mat_interm = G_alloc_matrix(numactive, numactive);
-    E_mat = G_alloc_matrix(numactive, numactive);
-    P_mat = G_alloc_matrix(numactive, numactive);
-    P_mat_T = G_alloc_matrix(numactive, numactive);
-    Q_mat = G_alloc_matrix(numactive, numactive);
+    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);
+    mat_nn2 = G_alloc_matrix(n, n);
 
-    transpose_matrix(numactive, dest_mat, dest_mat_T);
+    E_mat = G_alloc_matrix(m, m);
+    P_mat = G_alloc_matrix(ndims, ndims);
+    P_mat_T = G_alloc_matrix(ndims, ndims);
+    Q_mat = G_alloc_matrix(ndims, ndims);
 
-    for (i = 0; i < numactive; i++) {
-	for (j = 0; j < numactive; j++) {
+    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)numactive;
+		E_mat[i][j] = -1.0 / (double)m;
 	    }
 	    else{
-		E_mat[i][j] = 1.0 - 1.0 / (double)numactive;
+		E_mat[i][j] = 1.0 - 1.0 / (double)m;
 	    }
 	}
     }
 
-    matmult(numactive, numactive, dest_mat_T, E_mat, C_mat_interm);
-    matmult(ndims, numactive, C_mat_interm, src_mat, C_mat);
-    copy_matrix(ndims, ndims, C_mat, P_mat);
-
-#if 1
-    svd(ndims, ndims, C_mat, P_mat, D_vec, Q_mat);
-    status = MSUCCESS;
+    matmult(n, m, dest_mat_T, E_mat, mat_nm1);
+    matmult(n, m, mat_nm1, src_mat, mat_nm2);
+    copy_matrix(ndims, ndims, mat_nm2, P_mat);
+    copy_matrix(ndims, ndims, mat_nm2, mat_nn1);
 
-#else
-    /* not working... */
-    status = G_math_svduv(D_vec, C_mat, Q_mat, ndims, P_mat, ndims);
+    status = G_math_svduv(D_vec, mat_nn1, P_mat, ndims, Q_mat, ndims);
 
     if (status == 0)
 	status = MSUCCESS;
-#endif
 
-    transpose_matrix(ndims, P_mat, P_mat_T);
+    transpose_matrix(n, n, P_mat, P_mat_T);
 
     /* rotation matrix */
-    matmult(ndims, ndims, Q_mat, P_mat_T, R_mat_T);
-    transpose_matrix(ndims, R_mat_T, R_mat);
+    matmult(n, n, Q_mat, P_mat_T, R_mat_T);
+    transpose_matrix(n, n, R_mat_T, R_mat);
 
     /* scale */
-    matmult(numactive, ndims, C_mat, R_mat_T, C_mat_interm);
-    trace1 = trace(ndims, ndims, C_mat_interm);
+    matmult(n, n, mat_nm2, R_mat_T, mat_nn2);
+    trace1 = trace(n, n, mat_nn2);
 
-    transpose_matrix(numactive, src_mat, src_mat_T);
-    matmult(numactive, numactive, src_mat_T, E_mat, C_mat_interm);
-    matmult(ndims, numactive, C_mat_interm, src_mat, C_mat);
-    trace2 = trace(ndims, ndims, C_mat);
+    transpose_matrix(m, n, src_mat, src_mat_T);
+    matmult(n, m, src_mat_T, E_mat, mat_nm1);
+    matmult(n, m, mat_nm1, src_mat, mat_nm2);
+    trace2 = trace(n, n, mat_nm2);
 
     OR[14] = trace1 / trace2;
 
     /* shifts */
-    matmult(numactive, ndims, src_mat, R_mat_T, D_mat_interm);
-    scale_matrix(numactive, ndims, OR[14], D_mat_interm, C_mat_interm);
-    subtract_matrix(numactive, ndims, dest_mat, C_mat_interm, D_mat_interm);
-    scale_matrix(numactive, ndims, 1.0 / numactive, D_mat_interm, C_mat_interm);
-    transpose_matrix(numactive, C_mat_interm, D_mat_interm);
+    matmult(m, 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(ndims);
-    one_vec = G_alloc_vector(numactive);
+    S_vec = G_alloc_vector(n);
+    one_vec = G_alloc_vector(m);
 
-    for (i = 0; i < numactive; i++){
+    for (i = 0; i < m; i++){
 	one_vec[i] = 1.0;
     }
 
-    matrix_multiply(ndims, numactive, D_mat_interm, one_vec, S_vec);
+    matrix_multiply(n, m, mat_nm1, one_vec, S_vec);
 
     /* matrix to vector */
     for (i = 0; i < ndims; i++) {
@@ -432,9 +433,12 @@ static int calccoef(struct Control_Points_3D *cp, double OR[], int ndims)
     G_free_matrix(Q_mat);
     G_free_matrix(R_mat);
     G_free_matrix(R_mat_T);
-    G_free_matrix(C_mat);
-    G_free_matrix(C_mat_interm);
-    G_free_matrix(D_mat_interm);
+    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_matrix(mat_nn2);
     G_free_vector(S_vec);
     G_free_vector(one_vec);
 
@@ -503,13 +507,13 @@ static int calcscale(struct Control_Points_3D *cp, double OR[])
     return MSUCCESS;
 }
 
-void transpose_matrix(int n, double **src_matrix, double **dest_matrix)
+void transpose_matrix(int m, int n, double **src_matrix, double **dest_matrix)
 {
     int i, j;
 
-    for(i = 0; i < n; i++) {
+    for(i = 0; i < m; i++) {
 	for(j = 0; j < n; j++)  {
-	    dest_matrix[i][j] = src_matrix[j][i];
+	    dest_matrix[j][i] = src_matrix[i][j];
 	}
     }
 }
@@ -600,4 +604,3 @@ void matrix_multiply(int n, int m, double **mat, double *iv,
 	}
     }
 }
-

vector/v.rectify/svdm.c

@@ -1,291 +0,0 @@
-/*
-   Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
-   University of Manchester, UK.  All rights reserved.
-
-   Redistribution and use in source and binary forms, with or without modification,
-   are permitted provided that the following conditions are met:
-
-   Redistributions of source code must retain the above copyright notice, this list
-   of conditions and the following disclaimer.
-
-   Redistributions in binary form must reproduce the above copyright notice, this
-   list of conditions and the following disclaimer in the documentation and/or other
-   materials provided with the distribution.
-
-   Neither the name of the University of Manchester nor the names of its contributors
-   may be used to endorse or promote products derived from this software without
-   specific prior written permission.
-
-
-   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
-   EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
-   OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT
-   SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
-   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <math.h>
-#include <stdlib.h>
-
-/* svd function */
-#define SIGN(u, v)     ( (v)>=0.0 ? fabs(u) : -fabs(u) )
-#define MAX(x, y)     ( (x) >= (y) ? (x) : (y) )
-
-static double radius(double u, double v)
-{
-    double w;
-
-    u = fabs(u);
-    v = fabs(v);
-    if (u > v) {
-	w = v / u;
-	return (u * sqrt(1. + w * w));
-    }
-    else {
-	if (v) {
-	    w = u / v;
-	    return (v * sqrt(1. + w * w));
-	}
-	else
-	    return 0.0;
-    }
-}
-
-/*
-   Given matrix a[m][n], m>=n, using svd decomposition a = p d q' to get
-   p[m][n], diag d[n] and q[n][n].
- */
-void svd(int m, int n, double **a, double **p, double *d, double **q)
-{
-    int flag, i, its, j, jj, k, l, nm, nm1 = n - 1, mm1 = m - 1;
-    double c, f, h, s, x, y, z;
-    double anorm = 0, g = 0, scale = 0;
-    double *r = (double *)malloc(sizeof(double) * n);
-
-    for (i = 0; i < m; i++)
-	for (j = 0; j < n; j++)
-	    p[i][j] = a[i][j];
-    /*
-       for (i = m; i < n; i++)
-       p[i][j] = 0;
-     */
-
-    /* Householder reduction to bidigonal form */
-    for (i = 0; i < n; i++) {
-	l = i + 1;
-	r[i] = scale * g;
-	g = s = scale = 0.0;
-	if (i < m) {
-	    for (k = i; k < m; k++)
-		scale += fabs(p[k][i]);
-	    if (scale) {
-		for (k = i; k < m; k++) {
-		    p[k][i] /= scale;
-		    s += p[k][i] * p[k][i];
-		}
-		f = p[i][i];
-		g = -SIGN(sqrt(s), f);
-		h = f * g - s;
-		p[i][i] = f - g;
-		if (i != nm1) {
-		    for (j = l; j < n; j++) {
-			for (s = 0.0, k = i; k < m; k++)
-			    s += p[k][i] * p[k][j];
-			f = s / h;
-			for (k = i; k < m; k++)
-			    p[k][j] += f * p[k][i];
-		    }
-		}
-		for (k = i; k < m; k++)
-		    p[k][i] *= scale;
-	    }
-	}
-	d[i] = scale * g;
-	g = s = scale = 0.0;
-	if (i < m && i != nm1) {
-	    for (k = l; k < n; k++)
-		scale += fabs(p[i][k]);
-	    if (scale) {
-		for (k = l; k < n; k++) {
-		    p[i][k] /= scale;
-		    s += p[i][k] * p[i][k];
-		}
-		f = p[i][l];
-		g = -SIGN(sqrt(s), f);
-		h = f * g - s;
-		p[i][l] = f - g;
-		for (k = l; k < n; k++)
-		    r[k] = p[i][k] / h;
-		if (i != mm1) {
-		    for (j = l; j < m; j++) {
-			for (s = 0.0, k = l; k < n; k++)
-			    s += p[j][k] * p[i][k];
-			for (k = l; k < n; k++)
-			    p[j][k] += s * r[k];
-		    }
-		}
-		for (k = l; k < n; k++)
-		    p[i][k] *= scale;
-	    }
-	}
-	anorm = MAX(anorm, fabs(d[i]) + fabs(r[i]));
-    }
-
-    /* Accumulation of right-hand transformations */
-    for (i = n - 1; i >= 0; i--) {
-	if (i < nm1) {
-	    if (g) {
-		for (j = l; j < n; j++)
-		    q[j][i] = (p[i][j] / p[i][l]) / g;
-		for (j = l; j < n; j++) {
-		    for (s = 0.0, k = l; k < n; k++)
-			s += p[i][k] * q[k][j];
-		    for (k = l; k < n; k++)
-			q[k][j] += s * q[k][i];
-		}
-	    }
-	    for (j = l; j < n; j++)
-		q[i][j] = q[j][i] = 0.0;
-	}
-	q[i][i] = 1.0;
-	g = r[i];
-	l = i;
-    }
-    /* Accumulation of left-hand transformations */
-    for (i = n - 1; i >= 0; i--) {
-	l = i + 1;
-	g = d[i];
-	if (i < nm1)
-	    for (j = l; j < n; j++)
-		p[i][j] = 0.0;
-	if (g) {
-	    g = 1.0 / g;
-	    if (i != nm1) {
-		for (j = l; j < n; j++) {
-		    for (s = 0.0, k = l; k < m; k++)
-			s += p[k][i] * p[k][j];
-		    f = (s / p[i][i]) * g;
-		    for (k = i; k < m; k++)
-			p[k][j] += f * p[k][i];
-		}
-	    }
-	    for (j = i; j < m; j++)
-		p[j][i] *= g;
-	}
-	else
-	    for (j = i; j < m; j++)
-		p[j][i] = 0.0;
-	++p[i][i];
-    }
-    /* diagonalization of the bidigonal form */
-    for (k = n - 1; k >= 0; k--) {	/* loop over singlar values */
-	for (its = 0; its < 30; its++) {	/* loop over allowed iterations */
-	    flag = 1;
-	    for (l = k; l >= 0; l--) {	/* test for splitting */
-		nm = l - 1;	/* note that r[l] is always
-				 * zero */
-		if (fabs(r[l]) + anorm == anorm) {
-		    flag = 0;
-		    break;
-		}
-		if (fabs(d[nm]) + anorm == anorm)
-		    break;
-	    }
-	    if (flag) {
-		c = 0.0;	/* cancellation of r[l], if
-				 * l>1 */
-		s = 1.0;
-		for (i = l; i <= k; i++) {
-		    f = s * r[i];
-		    if (fabs(f) + anorm != anorm) {
-			g = d[i];
-			h = radius(f, g);
-			d[i] = h;
-			h = 1.0 / h;
-			c = g * h;
-			s = (-f * h);
-			for (j = 0; j < m; j++) {
-			    y = p[j][nm];
-			    z = p[j][i];
-			    p[j][nm] = y * c + z * s;
-			    p[j][i] = z * c - y * s;
-			}
-		    }
-		}
-	    }
-	    z = d[k];
-	    if (l == k) {	/* convergence */
-		if (z < 0.0) {
-		    d[k] = -z;
-		    for (j = 0; j < n; j++)
-			q[j][k] = (-q[j][k]);
-		}
-		break;
-	    }
-	    if (its == 100) {
-		/* svd: No convergence in 100 svd iterations, non_fatal error */
-		return;
-	    }
-	    x = d[l];		/* shift from bottom 2-by-2 minor */
-	    nm = k - 1;
-	    y = d[nm];
-	    g = r[nm];
-	    h = r[k];
-	    f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
-	    g = radius(f, 1.0);
-	    /* next QR transformation */
-	    f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
-	    c = s = 1.0;
-	    for (j = l; j <= nm; j++) {
-		i = j + 1;
-		g = r[i];
-		y = d[i];
-		h = s * g;
-		g = c * g;
-		z = radius(f, h);
-		r[j] = z;
-		c = f / z;
-		s = h / z;
-		f = x * c + g * s;
-		g = g * c - x * s;
-		h = y * s;
-		y = y * c;
-		for (jj = 0; jj < n; jj++) {
-		    x = q[jj][j];
-		    z = q[jj][i];
-		    q[jj][j] = x * c + z * s;
-		    q[jj][i] = z * c - x * s;
-		}
-		z = radius(f, h);
-		d[j] = z;	/* rotation can be arbitrary
-				 * id z=0 */
-		if (z) {
-		    z = 1.0 / z;
-		    c = f * z;
-		    s = h * z;
-		}
-		f = (c * g) + (s * y);
-		x = (c * y) - (s * g);
-		for (jj = 0; jj < m; jj++) {
-		    y = p[jj][j];
-		    z = p[jj][i];
-		    p[jj][j] = y * c + z * s;
-		    p[jj][i] = z * c - y * s;
-		}
-	    }
-	    r[l] = 0.0;
-	    r[k] = f;
-	    d[k] = x;
-	}
-    }
-    free(r);
-
-    /* dhli add: the original code does not sort the eigen value
-       should do that and change the eigen vector accordingly
-     */
-
-}