1 : /******************************************************************************
2 : * $Id: thinplatespline.cpp 24925 2012-09-16 10:07:00Z rouault $
3 : *
4 : * Project: GDAL Warp API
5 : * Purpose: Implemenentation of 2D Thin Plate Spline transformer.
6 : * Author: VIZRT Development Team.
7 : *
8 : * This code was provided by Gilad Ronnen (gro at visrt dot com) with
9 : * permission to reuse under the following license.
10 : *
11 : ******************************************************************************
12 : * Copyright (c) 2004, VIZRT Inc.
13 : *
14 : * Permission is hereby granted, free of charge, to any person obtaining a
15 : * copy of this software and associated documentation files (the "Software"),
16 : * to deal in the Software without restriction, including without limitation
17 : * the rights to use, copy, modify, merge, publish, distribute, sublicense,
18 : * and/or sell copies of the Software, and to permit persons to whom the
19 : * Software is furnished to do so, subject to the following conditions:
20 : *
21 : * The above copyright notice and this permission notice shall be included
22 : * in all copies or substantial portions of the Software.
23 : *
24 : * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
25 : * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
26 : * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
27 : * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
28 : * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
29 : * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
30 : * DEALINGS IN THE SOFTWARE.
31 : ****************************************************************************/
32 :
33 : #ifdef HAVE_ARMADILLO
34 : /* Include before #define A(r,c) because armadillo uses A in its include files */
35 : #include "armadillo"
36 : #endif
37 :
38 : #include "thinplatespline.h"
39 :
40 : #ifdef HAVE_FLOAT_H
41 : # include <float.h>
42 : #elif defined(HAVE_VALUES_H)
43 : # include <values.h>
44 : #endif
45 :
46 : #ifndef FLT_MAX
47 : # define FLT_MAX 1e+37
48 : # define FLT_MIN 1e-37
49 : #endif
50 :
51 : VizGeorefSpline2D* viz_xy2llz;
52 : VizGeorefSpline2D* viz_llz2xy;
53 :
54 : /////////////////////////////////////////////////////////////////////////////////////
55 : //// vizGeorefSpline2D
56 : /////////////////////////////////////////////////////////////////////////////////////
57 :
58 : #define A(r,c) _AA[ _nof_eqs * (r) + (c) ]
59 : #define Ainv(r,c) _Ainv[ _nof_eqs * (r) + (c) ]
60 :
61 :
62 : #define VIZ_GEOREF_SPLINE_DEBUG 0
63 :
64 : static int matrixInvert( int N, double input[], double output[] );
65 :
66 24 : void VizGeorefSpline2D::grow_points()
67 :
68 : {
69 24 : int new_max = _max_nof_points*2 + 2 + 3;
70 : int i;
71 :
72 24 : if( _max_nof_points == 0 )
73 : {
74 12 : x = (double *) VSIMalloc( sizeof(double) * new_max );
75 12 : y = (double *) VSIMalloc( sizeof(double) * new_max );
76 12 : u = (double *) VSIMalloc( sizeof(double) * new_max );
77 12 : unused = (int *) VSIMalloc( sizeof(int) * new_max );
78 12 : index = (int *) VSIMalloc( sizeof(int) * new_max );
79 36 : for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
80 : {
81 24 : rhs[i] = (double *) VSICalloc( sizeof(double), new_max );
82 24 : coef[i] = (double *) VSICalloc( sizeof(double), new_max );
83 : }
84 : }
85 : else
86 : {
87 12 : x = (double *) VSIRealloc( x, sizeof(double) * new_max );
88 12 : y = (double *) VSIRealloc( y, sizeof(double) * new_max );
89 12 : u = (double *) VSIRealloc( u, sizeof(double) * new_max );
90 12 : unused = (int *) VSIRealloc( unused, sizeof(int) * new_max );
91 12 : index = (int *) VSIRealloc( index, sizeof(int) * new_max );
92 36 : for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
93 : {
94 24 : rhs[i] = (double *)
95 24 : VSIRealloc( rhs[i], sizeof(double) * new_max );
96 24 : coef[i] = (double *)
97 24 : VSIRealloc( coef[i], sizeof(double) * new_max );
98 : }
99 : }
100 :
101 24 : _max_nof_points = new_max - 3;
102 24 : }
103 :
104 46 : int VizGeorefSpline2D::add_point( const double Px, const double Py, const double *Pvars )
105 : {
106 46 : type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
107 : int i;
108 :
109 46 : if( _nof_points == _max_nof_points )
110 12 : grow_points();
111 :
112 46 : i = _nof_points;
113 : //A new point is added
114 46 : x[i] = Px;
115 46 : y[i] = Py;
116 138 : for ( int j = 0; j < _nof_vars; j++ )
117 92 : rhs[j][i+3] = Pvars[j];
118 46 : _nof_points++;
119 46 : return 1;
120 : }
121 :
122 0 : bool VizGeorefSpline2D::change_point(int index, double Px, double Py, double* Pvars)
123 : {
124 0 : if ( index < _nof_points )
125 : {
126 0 : int i = index;
127 0 : x[i] = Px;
128 0 : y[i] = Py;
129 0 : for ( int j = 0; j < _nof_vars; j++ )
130 0 : rhs[j][i+3] = Pvars[j];
131 : }
132 :
133 0 : return( true );
134 : }
135 :
136 0 : bool VizGeorefSpline2D::get_xy(int index, double& outX, double& outY)
137 : {
138 : bool ok;
139 :
140 0 : if ( index < _nof_points )
141 : {
142 0 : ok = true;
143 0 : outX = x[index];
144 0 : outY = y[index];
145 : }
146 : else
147 : {
148 0 : ok = false;
149 0 : outX = outY = 0.0f;
150 : }
151 :
152 0 : return(ok);
153 : }
154 :
155 0 : int VizGeorefSpline2D::delete_point(const double Px, const double Py )
156 : {
157 0 : for ( int i = 0; i < _nof_points; i++ )
158 : {
159 0 : if ( ( fabs(Px - x[i]) <= _tx ) && ( fabs(Py - y[i]) <= _ty ) )
160 : {
161 0 : for ( int j = i; j < _nof_points - 1; j++ )
162 : {
163 0 : x[j] = x[j+1];
164 0 : y[j] = y[j+1];
165 0 : for ( int k = 0; k < _nof_vars; k++ )
166 0 : rhs[k][j+3] = rhs[k][j+3+1];
167 : }
168 0 : _nof_points--;
169 0 : type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED;
170 0 : return(1);
171 : }
172 : }
173 0 : return(0);
174 : }
175 :
176 12 : int VizGeorefSpline2D::solve(void)
177 : {
178 : int r, c, v;
179 : int p;
180 :
181 : // No points at all
182 12 : if ( _nof_points < 1 )
183 : {
184 0 : type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
185 0 : return(0);
186 : }
187 :
188 : // Only one point
189 12 : if ( _nof_points == 1 )
190 : {
191 0 : type = VIZ_GEOREF_SPLINE_ONE_POINT;
192 0 : return(1);
193 : }
194 : // Just 2 points - it is necessarily 1D case
195 12 : if ( _nof_points == 2 )
196 : {
197 0 : _dx = x[1] - x[0];
198 0 : _dy = y[1] - y[0];
199 0 : double fact = 1.0 / ( _dx * _dx + _dy * _dy );
200 0 : _dx *= fact;
201 0 : _dy *= fact;
202 :
203 0 : type = VIZ_GEOREF_SPLINE_TWO_POINTS;
204 0 : return(2);
205 : }
206 :
207 : // More than 2 points - first we have to check if it is 1D or 2D case
208 :
209 12 : double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
210 : double delx, dely;
211 : double xx, yy;
212 12 : double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
213 : double SSxx, SSyy, SSxy;
214 :
215 58 : for ( p = 0; p < _nof_points; p++ )
216 : {
217 46 : xx = x[p];
218 46 : yy = y[p];
219 :
220 46 : xmax = MAX( xmax, xx );
221 46 : xmin = MIN( xmin, xx );
222 46 : ymax = MAX( ymax, yy );
223 46 : ymin = MIN( ymin, yy );
224 :
225 46 : sumx += xx;
226 46 : sumx2 += xx * xx;
227 46 : sumy += yy;
228 46 : sumy2 += yy * yy;
229 46 : sumxy += xx * yy;
230 : }
231 12 : delx = xmax - xmin;
232 12 : dely = ymax - ymin;
233 :
234 12 : SSxx = sumx2 - sumx * sumx / _nof_points;
235 12 : SSyy = sumy2 - sumy * sumy / _nof_points;
236 12 : SSxy = sumxy - sumx * sumy / _nof_points;
237 :
238 12 : if ( delx < 0.001 * dely || dely < 0.001 * delx ||
239 : fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
240 : {
241 : int p1;
242 :
243 0 : type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
244 :
245 0 : _dx = _nof_points * sumx2 - sumx * sumx;
246 0 : _dy = _nof_points * sumy2 - sumy * sumy;
247 0 : double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
248 0 : _dx *= fact;
249 0 : _dy *= fact;
250 :
251 0 : for ( p = 0; p < _nof_points; p++ )
252 : {
253 0 : double dxp = x[p] - x[0];
254 0 : double dyp = y[p] - y[0];
255 0 : u[p] = _dx * dxp + _dy * dyp;
256 0 : unused[p] = 1;
257 : }
258 :
259 0 : for ( p = 0; p < _nof_points; p++ )
260 : {
261 0 : int min_index = -1;
262 0 : double min_u = 0;
263 0 : for ( p1 = 0; p1 < _nof_points; p1++ )
264 : {
265 0 : if ( unused[p1] )
266 : {
267 0 : if ( min_index < 0 || u[p1] < min_u )
268 : {
269 0 : min_index = p1;
270 0 : min_u = u[p1];
271 : }
272 : }
273 : }
274 0 : index[p] = min_index;
275 0 : unused[min_index] = 0;
276 : }
277 :
278 0 : return(3);
279 : }
280 :
281 12 : type = VIZ_GEOREF_SPLINE_FULL;
282 : // Make the necessary memory allocations
283 12 : if ( _AA )
284 0 : CPLFree(_AA);
285 12 : if ( _Ainv )
286 0 : CPLFree(_Ainv);
287 :
288 12 : _nof_eqs = _nof_points + 3;
289 :
290 12 : if( _nof_eqs > INT_MAX / _nof_eqs )
291 : {
292 0 : fprintf(stderr, "Too many coefficients. Computation aborted.\n");
293 0 : return 0;
294 : }
295 :
296 12 : _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
297 12 : _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
298 :
299 12 : if( _AA == NULL || _Ainv == NULL )
300 : {
301 0 : fprintf(stderr, "Out-of-memory while allocating temporary arrays. Computation aborted.\n");
302 0 : return 0;
303 : }
304 :
305 : // Calc the values of the matrix A
306 48 : for ( r = 0; r < 3; r++ )
307 144 : for ( c = 0; c < 3; c++ )
308 108 : A(r,c) = 0.0;
309 :
310 58 : for ( c = 0; c < _nof_points; c++ )
311 : {
312 46 : A(0,c+3) = 1.0;
313 46 : A(1,c+3) = x[c];
314 46 : A(2,c+3) = y[c];
315 :
316 46 : A(c+3,0) = 1.0;
317 46 : A(c+3,1) = x[c];
318 46 : A(c+3,2) = y[c];
319 : }
320 :
321 58 : for ( r = 0; r < _nof_points; r++ )
322 158 : for ( c = r; c < _nof_points; c++ )
323 : {
324 112 : A(r+3,c+3) = base_func( x[r], y[r], x[c], y[c] );
325 112 : if ( r != c )
326 66 : A(c+3,r+3 ) = A(r+3,c+3);
327 : }
328 :
329 : #if VIZ_GEOREF_SPLINE_DEBUG
330 :
331 : for ( r = 0; r < _nof_eqs; r++ )
332 : {
333 : for ( c = 0; c < _nof_eqs; c++ )
334 : fprintf(stderr, "%f", A(r,c));
335 : fprintf(stderr, "\n");
336 : }
337 :
338 : #endif
339 :
340 : // Invert the matrix
341 12 : int status = matrixInvert( _nof_eqs, _AA, _Ainv );
342 :
343 12 : if ( !status )
344 : {
345 0 : fprintf(stderr, " There is a problem to invert the interpolation matrix\n");
346 0 : return 0;
347 : }
348 :
349 : // calc the coefs
350 36 : for ( v = 0; v < _nof_vars; v++ )
351 188 : for ( r = 0; r < _nof_eqs; r++ )
352 : {
353 164 : coef[v][r] = 0.0;
354 1288 : for ( c = 0; c < _nof_eqs; c++ )
355 1124 : coef[v][r] += Ainv(r,c) * rhs[v][c];
356 : }
357 :
358 12 : return(4);
359 : }
360 :
361 2564 : int VizGeorefSpline2D::get_point( const double Px, const double Py, double *vars )
362 : {
363 : int v, r;
364 : double tmp, Pu;
365 : double fact;
366 2564 : int leftP=0, rightP=0, found = 0;
367 :
368 2564 : switch ( type )
369 : {
370 : case VIZ_GEOREF_SPLINE_ZERO_POINTS :
371 0 : for ( v = 0; v < _nof_vars; v++ )
372 0 : vars[v] = 0.0;
373 0 : break;
374 : case VIZ_GEOREF_SPLINE_ONE_POINT :
375 0 : for ( v = 0; v < _nof_vars; v++ )
376 0 : vars[v] = rhs[v][3];
377 0 : break;
378 : case VIZ_GEOREF_SPLINE_TWO_POINTS :
379 0 : fact = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
380 0 : for ( v = 0; v < _nof_vars; v++ )
381 0 : vars[v] = ( 1 - fact ) * rhs[v][3] + fact * rhs[v][4];
382 0 : break;
383 : case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL :
384 0 : Pu = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
385 0 : if ( Pu <= u[index[0]] )
386 : {
387 0 : leftP = index[0];
388 0 : rightP = index[1];
389 : }
390 0 : else if ( Pu >= u[index[_nof_points-1]] )
391 : {
392 0 : leftP = index[_nof_points-2];
393 0 : rightP = index[_nof_points-1];
394 : }
395 : else
396 : {
397 0 : for ( r = 1; !found && r < _nof_points; r++ )
398 : {
399 0 : leftP = index[r-1];
400 0 : rightP = index[r];
401 0 : if ( Pu >= u[leftP] && Pu <= u[rightP] )
402 0 : found = 1;
403 : }
404 : }
405 :
406 0 : fact = ( Pu - u[leftP] ) / ( u[rightP] - u[leftP] );
407 0 : for ( v = 0; v < _nof_vars; v++ )
408 0 : vars[v] = ( 1.0 - fact ) * rhs[v][leftP+3] +
409 0 : fact * rhs[v][rightP+3];
410 0 : break;
411 : case VIZ_GEOREF_SPLINE_FULL :
412 7692 : for ( v = 0; v < _nof_vars; v++ )
413 5128 : vars[v] = coef[v][0] + coef[v][1] * Px + coef[v][2] * Py;
414 :
415 12818 : for ( r = 0; r < _nof_points; r++ )
416 : {
417 10254 : tmp = base_func( Px, Py, x[r], y[r] );
418 30762 : for ( v= 0; v < _nof_vars; v++ )
419 20508 : vars[v] += coef[v][r+3] * tmp;
420 : }
421 2564 : break;
422 : case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED :
423 0 : fprintf(stderr, " A point was added after the last solve\n");
424 0 : fprintf(stderr, " NO interpolation - return values are zero\n");
425 0 : for ( v = 0; v < _nof_vars; v++ )
426 0 : vars[v] = 0.0;
427 0 : return(0);
428 : break;
429 : case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED :
430 0 : fprintf(stderr, " A point was deleted after the last solve\n");
431 0 : fprintf(stderr, " NO interpolation - return values are zero\n");
432 0 : for ( v = 0; v < _nof_vars; v++ )
433 0 : vars[v] = 0.0;
434 0 : return(0);
435 : break;
436 : default :
437 0 : return(0);
438 : break;
439 : }
440 2564 : return(1);
441 : }
442 :
443 10366 : double VizGeorefSpline2D::base_func( const double x1, const double y1,
444 : const double x2, const double y2 )
445 : {
446 10366 : if ( ( x1 == x2 ) && (y1 == y2 ) )
447 106 : return 0.0;
448 :
449 10260 : double dist = ( x2 - x1 ) * ( x2 - x1 ) + ( y2 - y1 ) * ( y2 - y1 );
450 :
451 10260 : return dist * log( dist );
452 : }
453 :
454 : #ifdef HAVE_ARMADILLO
455 :
456 12 : static int matrixInvert( int N, double input[], double output[] )
457 : {
458 : try
459 : {
460 12 : arma::mat matInput(input,N,N,false);
461 12 : const arma::mat& matInv = arma::inv(matInput);
462 : int row, col;
463 94 : for(row = 0; row < N; row++)
464 644 : for(col = 0; col < N; col++)
465 1124 : output[row * N + col] = matInv.at(row, col);
466 12 : return true;
467 : //arma::mat matInv(output,N,N,false);
468 : //return arma::inv(matInv, matInput);
469 : }
470 0 : catch(...)
471 : {
472 0 : fprintf(stderr, "matrixInvert(): error occured.\n");
473 0 : return false;
474 : }
475 2139 : }
476 :
477 : #else
478 :
479 : static int matrixInvert( int N, double input[], double output[] )
480 : {
481 : // Receives an array of dimension NxN as input. This is passed as a one-
482 : // dimensional array of N-squared size. It produces the inverse of the
483 : // input matrix, returned as output, also of size N-squared. The Gauss-
484 : // Jordan Elimination method is used. (Adapted from a BASIC routine in
485 : // "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)
486 :
487 : // Array elements 0...N-1 are for the first row, N...2N-1 are for the
488 : // second row, etc.
489 :
490 : // We need to have a temporary array of size N x 2N. We'll refer to the
491 : // "left" and "right" halves of this array.
492 :
493 : int row, col;
494 :
495 : #if 0
496 : fprintf(stderr, "Matrix Inversion input matrix (N=%d)\n", N);
497 : for ( row=0; row<N; row++ )
498 : {
499 : for ( col=0; col<N; col++ )
500 : {
501 : fprintf(stderr, "%5.2f ", input[row*N + col ] );
502 : }
503 : fprintf(stderr, "\n");
504 : }
505 : #endif
506 :
507 : int tempSize = 2 * N * N;
508 : double* temp = (double*) new double[ tempSize ];
509 : double ftemp;
510 :
511 : if (temp == 0) {
512 :
513 : fprintf(stderr, "matrixInvert(): ERROR - memory allocation failed.\n");
514 : return false;
515 : }
516 :
517 : // First create a double-width matrix with the input array on the left
518 : // and the identity matrix on the right.
519 :
520 : for ( row=0; row<N; row++ )
521 : {
522 : for ( col=0; col<N; col++ )
523 : {
524 : // Our index into the temp array is X2 because it's twice as wide
525 : // as the input matrix.
526 :
527 : temp[ 2*row*N + col ] = input[ row*N+col ]; // left = input matrix
528 : temp[ 2*row*N + col + N ] = 0.0f; // right = 0
529 : }
530 : temp[ 2*row*N + row + N ] = 1.0f; // 1 on the diagonal of RHS
531 : }
532 :
533 : // Now perform row-oriented operations to convert the left hand side
534 : // of temp to the identity matrix. The inverse of input will then be
535 : // on the right.
536 :
537 : int max;
538 : int k=0;
539 : for (k = 0; k < N; k++)
540 : {
541 : if (k+1 < N) // if not on the last row
542 : {
543 : max = k;
544 : for (row = k+1; row < N; row++) // find the maximum element
545 : {
546 : if (fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ))
547 : {
548 : max = row;
549 : }
550 : }
551 :
552 : if (max != k) // swap all the elements in the two rows
553 : {
554 : for (col=k; col<2*N; col++)
555 : {
556 : ftemp = temp[k*2*N + col];
557 : temp[k*2*N + col] = temp[max*2*N + col];
558 : temp[max*2*N + col] = ftemp;
559 : }
560 : }
561 : }
562 :
563 : ftemp = temp[ k*2*N + k ];
564 : if ( ftemp == 0.0f ) // matrix cannot be inverted
565 : {
566 : delete[] temp;
567 : return false;
568 : }
569 :
570 : for ( col=k; col<2*N; col++ )
571 : {
572 : temp[ k*2*N + col ] /= ftemp;
573 : }
574 :
575 : int i2 = k*2*N ;
576 : for ( row=0; row<N; row++ )
577 : {
578 : if ( row != k )
579 : {
580 : int i1 = row*2*N;
581 : ftemp = temp[ i1 + k ];
582 : for ( col=k; col<2*N; col++ )
583 : {
584 : temp[ i1 + col ] -= ftemp * temp[ i2 + col ];
585 : }
586 : }
587 : }
588 : }
589 :
590 : // Retrieve inverse from the right side of temp
591 :
592 : for (row = 0; row < N; row++)
593 : {
594 : for (col = 0; col < N; col++)
595 : {
596 : output[row*N + col] = temp[row*2*N + col + N ];
597 : }
598 : }
599 :
600 : #if 0
601 : fprintf(stderr, "Matrix Inversion result matrix:\n");
602 : for ( row=0; row<N; row++ )
603 : {
604 : for ( col=0; col<N; col++ )
605 : {
606 : fprintf(stderr, "%5.2f ", output[row*N + col ] );
607 : }
608 : fprintf(stderr, "\n");
609 : }
610 : #endif
611 :
612 : delete [] temp; // free memory
613 : return true;
614 : }
615 : #endif
|
