1 /* solve and invert matrices
2 *
3 * 19/4/20 kleisauke
4 * - from im_matinv
5 */
6
7 /*
8
9 This file is part of VIPS.
10
11 VIPS is free software; you can redistribute it and/or modify
12 it under the terms of the GNU Lesser General Public License as published by
13 the Free Software Foundation; either version 2 of the License, or
14 (at your option) any later version.
15
16 This program is distributed in the hope that it will be useful,
17 but WITHOUT ANY WARRANTY; without even the implied warranty of
18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 GNU Lesser General Public License for more details.
20
21 You should have received a copy of the GNU Lesser General Public License
22 along with this program; if not, write to the Free Software
23 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
24 02110-1301 USA
25
26 */
27
28 /*
29
30 These files are distributed with VIPS - http://www.vips.ecs.soton.ac.uk
31
32 */
33
34 /*
35 #define DEBUG
36 */
37
38 #ifdef HAVE_CONFIG_H
39 #include <config.h>
40 #endif /*HAVE_CONFIG_H*/
41 #include <vips/intl.h>
42
43 #include <stdio.h>
44 #include <stdlib.h>
45 #include <string.h>
46 #include <math.h>
47
48 #include <vips/vips.h>
49
50 /* Our state.
51 */
52 typedef struct _VipsMatrixinvert {
53 VipsOperation parent_instance;
54
55 VipsImage *in;
56 VipsImage *out;
57
58 /* .. and cast to a matrix.
59 */
60 VipsImage *mat;
61
62 /* The LU decomposed matrix.
63 */
64 VipsImage *lu;
65 } VipsMatrixinvert;
66
67 typedef VipsOperationClass VipsMatrixinvertClass;
68
69 G_DEFINE_TYPE( VipsMatrixinvert, vips_matrixinvert, VIPS_TYPE_OPERATION );
70
71 static void
vips_matrixinvert_dispose(GObject * gobject)72 vips_matrixinvert_dispose( GObject *gobject )
73 {
74 VipsMatrixinvert *matrix = (VipsMatrixinvert *) gobject;
75
76 VIPS_UNREF( matrix->mat );
77 VIPS_UNREF( matrix->lu );
78
79 G_OBJECT_CLASS( vips_matrixinvert_parent_class )->dispose( gobject );
80 }
81
82 /* DBL_MIN is smallest *normalized* double precision float
83 */
84 #define TOO_SMALL (2.0 * DBL_MIN)
85
86 /* Save a bit of typing.
87 */
88 #define ME( m, i, j ) (*VIPS_MATRIX( (m), (i), (j) ))
89
90 /**
91 * lu_decomp:
92 * @mat: matrix to decompose
93 *
94 * This function takes any square NxN #VipsImage.
95 * It returns a #VipsImage which is (N+1)xN.
96 *
97 * It calculates the PLU decomposition, storing the upper and diagonal parts
98 * of U, together with the lower parts of L, as an NxN matrix in the first
99 * N rows of the new matrix. The diagonal parts of L are all set to unity
100 * and are not stored.
101 *
102 * The final row of the new #VipsImage has only integer entries, which
103 * represent the row-wise permutations made by the permutation matrix P.
104 *
105 * The scale and offset members of the input #VipsImage are ignored.
106 *
107 * See:
108 *
109 * PRESS, W. et al, 1992. Numerical Recipies in C; The Art of Scientific
110 * Computing, 2nd ed. Cambridge: Cambridge University Press, pp. 43-50.
111 *
112 * Returns: the decomposed matrix on success, or NULL on error.
113 */
114 static VipsImage *
lu_decomp(VipsImage * mat)115 lu_decomp( VipsImage *mat )
116 {
117 int i, j, k;
118 double *row_scale;
119 VipsImage *lu;
120
121 if ( !(row_scale = VIPS_ARRAY( NULL, mat->Xsize, double )) ) {
122 return( NULL );
123 }
124
125 if( !(lu = vips_image_new_matrix( mat->Xsize, mat->Xsize + 1 )) ) {
126 g_free( row_scale );
127 return( NULL );
128 }
129
130 /* copy all coefficients and then perform decomposition in-place */
131 memcpy( VIPS_MATRIX( lu, 0, 0), VIPS_MATRIX( mat, 0, 0),
132 mat->Xsize * mat->Xsize * sizeof( double ) );
133
134 for( i = 0; i < mat->Xsize; ++i ) {
135 row_scale[i] = 0.0;
136
137 for( j = 0; j < mat->Xsize; ++j ) {
138 double abs_val = fabs( ME( lu, i, j ) );
139
140 /* find largest in each ROW */
141 if( abs_val > row_scale[i] )
142 row_scale[i] = abs_val;
143 }
144
145 if( !row_scale[i] ) {
146 vips_error( "matrixinvert", "singular matrix" );
147 g_object_unref( lu );
148 g_free( row_scale );
149 return( NULL );
150 }
151
152 /* fill array with scaling factors for each ROW */
153 row_scale[i] = 1.0 / row_scale[i];
154 }
155
156 for( j = 0; j < mat->Xsize; ++j ) { /* loop over COLs */
157 double max = -1.0;
158 int i_of_max;
159
160 /* not needed, but stops a compiler warning */
161 i_of_max = 0;
162
163 /* loop over ROWS in upper-half, except diagonal */
164 for( i = 0; i < j; ++i )
165 for( k = 0; k < i; ++k )
166 ME( lu, i, j ) -= ME( lu, i, k ) * ME( lu, k, j );
167
168 /* loop over ROWS in diagonal and lower-half */
169 for( i = j; i < mat->Xsize; ++i ) {
170 double abs_val;
171
172 for( k = 0; k < j; ++k )
173 ME( lu, i, j ) -= ME( lu, i, k ) * ME( lu, k, j );
174
175 /* find largest element in each COLUMN scaled so that */
176 /* largest in each ROW is 1.0 */
177 abs_val = row_scale[i] * fabs( ME( lu, i, j ) );
178
179 if( abs_val > max ) {
180 max = abs_val;
181 i_of_max = i;
182 }
183 }
184
185 if( fabs( ME( lu, i_of_max, j ) ) < TOO_SMALL ) {
186 /* divisor is near zero */
187 vips_error( "matrixinvert", "singular or near-singular matrix" );
188 g_object_unref( lu );
189 g_free( row_scale );
190 return( NULL );
191 }
192
193 if( i_of_max != j ) {
194 /* swap ROWS */
195 for( k = 0; k < mat->Xsize; ++k ) {
196 double temp = ME( lu, j, k );
197 ME( lu, j, k ) = ME( lu, i_of_max, k );
198 ME( lu, i_of_max, k ) = temp;
199 }
200
201 row_scale[i_of_max] = row_scale[j];
202 /* no need to copy this scale back up - we won't use it */
203 }
204
205 /* record permutation */
206 ME( lu, j, mat->Xsize ) = i_of_max;
207
208 /* divide by best (largest scaled) pivot found */
209 for( i = j + 1; i < mat->Xsize; ++i )
210 ME( lu, i, j ) /= ME( lu, j, j );
211 }
212 g_free( row_scale );
213
214 return( lu );
215 }
216
217 /**
218 * lu_solve:
219 * @lu: matrix to solve
220 * @vec: name for output matrix
221 *
222 * Solve the system of linear equations Ax=b, where matrix A has already
223 * been decomposed into LU form in VipsImage *lu. Input vector b is in
224 * vec and is overwritten with vector x.
225 *
226 * See:
227 *
228 * PRESS, W. et al, 1992. Numerical Recipies in C; The Art of Scientific
229 * Computing, 2nd ed. Cambridge: Cambridge University Press, pp. 43-50.
230 *
231 * See also: vips__matrixtranspose(), vips__matrixmultiply().
232 *
233 * Returns: 0 on success, -1 on error
234 */
235 static int
lu_solve(VipsImage * lu,double * vec)236 lu_solve( VipsImage *lu, double *vec )
237 {
238 int i, j;
239
240 if( lu->Xsize + 1 != lu->Ysize ) {
241 vips_error( "matrixinvert", "not an LU decomposed matrix" );
242 return( -1 );
243 }
244
245 for( i = 0; i < lu->Xsize; ++i ) {
246 int i_perm = ME( lu, i, lu->Xsize );
247
248 if( i_perm != i ) {
249 double temp = vec[i];
250 vec[i] = vec[i_perm];
251 vec[i_perm] = temp;
252 }
253 for( j = 0; j < i; ++j )
254 vec[i] -= ME( lu, i, j ) * vec[j];
255 }
256
257 for( i = lu->Xsize - 1; i >= 0; --i ) {
258
259 for( j = i + 1; j < lu->Xsize; ++j )
260 vec[i] -= ME( lu, i, j ) * vec[j];
261
262 vec[i] /= ME( lu, i, i );
263 }
264
265 return( 0 );
266 }
267
268 static int
vips_matrixinvert_solve(VipsMatrixinvert * matrix)269 vips_matrixinvert_solve( VipsMatrixinvert *matrix )
270 {
271 VipsImage *out = matrix->out;
272
273 int i, j;
274 double *vec;
275
276 if( !(matrix->lu = lu_decomp( matrix->mat ) ) )
277 return( -1 );
278
279 if( !(vec = VIPS_ARRAY( matrix, matrix->lu->Xsize, double )) )
280 return( -1 );
281
282 for( j = 0; j < matrix->lu->Xsize; ++j ) {
283 for( i = 0; i < matrix->lu->Xsize; ++i )
284 vec[i] = 0.0;
285
286 vec[j] = 1.0;
287
288 if( lu_solve( matrix->lu, vec ) )
289 return( -1 );
290
291 for( i = 0; i < matrix->lu->Xsize; ++i )
292 ME( out, i, j ) = vec[i];
293 }
294
295 return( 0 );
296 }
297
298 static int
vips_matrixinvert_direct(VipsMatrixinvert * matrix)299 vips_matrixinvert_direct( VipsMatrixinvert *matrix )
300 {
301 VipsObjectClass *class = VIPS_OBJECT_GET_CLASS( matrix );
302 VipsImage *in = matrix->mat;
303 VipsImage *out = matrix->out;
304
305 switch( matrix->mat->Xsize ) {
306 case 1:
307 {
308 double det = ME( in, 0, 0 );
309
310 if( fabs( det ) < TOO_SMALL ) {
311 /* divisor is near zero */
312 vips_error( class->nickname,
313 "%s", _( "singular or near-singular matrix" ) );
314 return( -1 );
315 }
316
317 ME( out, 0, 0 ) = 1.0 / det;
318 }
319 break;
320
321 case 2:
322 {
323 double det = ME( in, 0, 0 ) * ME( in, 1, 1 ) -
324 ME( in, 0, 1 ) * ME( in, 1, 0 );
325
326 double tmp;
327
328 if( fabs( det ) < TOO_SMALL ) {
329 /* divisor is near zero */
330 vips_error( class->nickname,
331 "%s", _( "singular or near-singular matrix" ) );
332 return( -1 );
333 }
334
335 tmp = 1.0 / det;
336 ME( out, 0, 0 ) = tmp * ME( in, 1, 1 );
337 ME( out, 0, 1 ) = -tmp * ME( in, 0, 1 );
338 ME( out, 1, 0 ) = -tmp * ME( in, 1, 0 );
339 ME( out, 1, 1 ) = tmp * ME( in, 0, 0 );
340 }
341 break;
342
343 case 3:
344 {
345 double det;
346 double tmp;
347
348 det = ME( in, 0, 0 ) * ( ME( in, 1, 1 ) *
349 ME( in, 2, 2 ) - ME( in, 1, 2 ) * ME( in, 2, 1 ) );
350 det -= ME( in, 0, 1 ) * ( ME( in, 1, 0 ) *
351 ME( in, 2, 2 ) - ME( in, 1, 2 ) * ME( in, 2, 0) );
352 det += ME( in, 0, 2) * ( ME( in, 1, 0 ) *
353 ME( in, 2, 1 ) - ME( in, 1, 1 ) * ME( in, 2, 0 ) );
354
355 if( fabs( det ) < TOO_SMALL ) {
356 /* divisor is near zero */
357 vips_error( class->nickname,
358 "%s", _( "singular or near-singular matrix" ) );
359 return( -1 );
360 }
361
362 tmp = 1.0 / det;
363
364 ME( out, 0, 0 ) = tmp * ( ME( in, 1, 1 ) * ME( in, 2, 2 ) -
365 ME( in, 1, 2 ) * ME( in, 2, 1 ) );
366 ME( out, 1, 0 ) = tmp * ( ME( in, 1, 2 ) * ME( in, 2, 0 ) -
367 ME( in, 1, 0 ) * ME( in, 2, 2 ) );
368 ME( out, 2, 0 ) = tmp * ( ME( in, 1, 0 ) * ME( in, 2, 1 ) -
369 ME( in, 1, 1 ) * ME( in, 2, 0 ) );
370
371 ME( out, 0, 1 ) = tmp * ( ME( in, 0, 2 ) * ME( in, 2, 1 ) -
372 ME( in, 0, 1 ) * ME( in, 2, 2 ) );
373 ME( out, 1, 1 ) = tmp * ( ME( in, 0, 0 ) * ME( in, 2, 2 ) -
374 ME( in, 0, 2 ) * ME( in, 2, 0 ) );
375 ME( out, 2, 1 ) = tmp * ( ME( in, 0, 1 ) * ME( in, 2, 0 ) -
376 ME( in, 0, 0 ) * ME( in, 2, 1 ) );
377
378 ME( out, 0, 2 ) = tmp * ( ME( in, 0, 1 ) * ME( in, 1, 2 ) -
379 ME( in, 0, 2 ) * ME( in, 1, 1 ) );
380 ME( out, 1, 2 ) = tmp * ( ME( in, 0, 2 ) * ME( in, 1, 0 ) -
381 ME( in, 0, 0 ) * ME( in, 1, 2 ) );
382 ME( out, 2, 2 ) = tmp * ( ME( in, 0, 0 ) * ME( in, 1, 1 ) -
383 ME( in, 0, 1 ) * ME( in, 1, 0 ) );
384 }
385 break;
386
387 /* TODO(kleisauke):
388 * We sometimes use 4x4 matrices, could we also make a
389 * direct version for those? For e.g.:
390 * https://stackoverflow.com/a/1148405/10952119 */
391 default:
392 g_assert( 0 );
393 return( -1 );
394 }
395
396 return( 0 );
397 }
398
399 static int
vips_matrixinvert_build(VipsObject * object)400 vips_matrixinvert_build( VipsObject *object )
401 {
402 VipsObjectClass *class = VIPS_OBJECT_GET_CLASS( object );
403 VipsMatrixinvert *matrix = (VipsMatrixinvert *) object;
404
405 if( VIPS_OBJECT_CLASS( vips_matrixinvert_parent_class )->
406 build( object ) )
407 return( -1 );
408
409 if( vips_check_matrix( class->nickname, matrix->in, &matrix->mat ) )
410 return( -1 );
411
412 if( matrix->mat->Xsize != matrix->mat->Ysize ) {
413 vips_error( class->nickname, "%s", _( "non-square matrix" ) );
414 return( -1 );
415 }
416
417 g_object_set( matrix,
418 "out", vips_image_new_matrix( matrix->mat->Xsize,
419 matrix->mat->Ysize ),
420 NULL );
421
422 /* Direct path for < 4x4 matrices
423 */
424 if( matrix->mat->Xsize >= 4 ) {
425 if( vips_matrixinvert_solve( matrix ) )
426 return( -1 );
427 }
428 else {
429 if( vips_matrixinvert_direct( matrix ) )
430 return( -1 );
431 }
432
433 return( 0 );
434 }
435
436 static void
vips_matrixinvert_class_init(VipsMatrixinvertClass * class)437 vips_matrixinvert_class_init( VipsMatrixinvertClass *class )
438 {
439 GObjectClass *gobject_class = G_OBJECT_CLASS( class );
440 VipsObjectClass *vobject_class = VIPS_OBJECT_CLASS( class );
441
442 gobject_class->dispose = vips_matrixinvert_dispose;
443 gobject_class->set_property = vips_object_set_property;
444 gobject_class->get_property = vips_object_get_property;
445
446 vobject_class->nickname = "matrixinvert";
447 vobject_class->description = _( "invert an matrix" );
448 vobject_class->build = vips_matrixinvert_build;
449
450 VIPS_ARG_IMAGE( class, "in", 0,
451 _( "Input" ),
452 _( "An square matrix" ),
453 VIPS_ARGUMENT_REQUIRED_INPUT,
454 G_STRUCT_OFFSET( VipsMatrixinvert, in ) );
455
456 VIPS_ARG_IMAGE( class, "out", 1,
457 _( "Output" ),
458 _( "Output matrix" ),
459 VIPS_ARGUMENT_REQUIRED_OUTPUT,
460 G_STRUCT_OFFSET( VipsMatrixinvert, out ) );
461 }
462
463 static void
vips_matrixinvert_init(VipsMatrixinvert * matrix)464 vips_matrixinvert_init( VipsMatrixinvert *matrix )
465 {
466 }
467
468 /**
469 * vips_matrixinvert: (method)
470 * @m: matrix to invert
471 * @out: (out): output matrix
472 * @...: %NULL-terminated list of optional named arguments
473 *
474 * This operation calculates the inverse of the matrix represented in @m.
475 * The scale and offset members of the input matrix are ignored.
476 *
477 * See also: vips_matrixload().
478 *
479 * Returns: 0 on success, -1 on error
480 */
481 int
vips_matrixinvert(VipsImage * m,VipsImage ** out,...)482 vips_matrixinvert( VipsImage *m, VipsImage **out, ... )
483 {
484 va_list ap;
485 int result;
486
487 va_start( ap, out );
488 result = vips_call_split( "matrixinvert", ap, m, out );
489 va_end( ap );
490
491 return( result );
492 }
493