uves_utils_polynomial.c

00001 /*                                                                              *
00002  *   This file is part of the ESO UVES Pipeline                                 *
00003  *   Copyright (C) 2004,2005 European Southern Observatory                      *
00004  *                                                                              *
00005  *   This library is free software; you can redistribute it and/or modify       *
00006  *   it under the terms of the GNU General Public License as published by       *
00007  *   the Free Software Foundation; either version 2 of the License, or          *
00008  *   (at your option) any later version.                                        *
00009  *                                                                              *
00010  *   This program is distributed in the hope that it will be useful,            *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of             *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
00013  *   GNU General Public License for more details.                               *
00014  *                                                                              *
00015  *   You should have received a copy of the GNU General Public License          *
00016  *   along with this program; if not, write to the Free Software                *
00017  *   Foundation, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA       *
00018  *                                                                              */
00019 
00020 /*
00021  * $Author: amodigli $
00022  * $Date: 2012/01/12 16:44:43 $
00023  * $Revision: 1.68 $
00024  * $Name: uves-5_0_0 $
00025  * $Log: uves_utils_polynomial.c,v $
00026  * Revision 1.68  2012/01/12 16:44:43  amodigli
00027  * fixed compiler warnings with CPL6
00028  *
00029  * Revision 1.67  2011/12/08 14:03:32  amodigli
00030  * Fix warnings with CPL6
00031  *
00032  * Revision 1.66  2010/09/24 09:32:08  amodigli
00033  * put back QFITS dependency to fix problem spot by NRI on FIBER mode (with MIDAS calibs) data
00034  *
00035  * Revision 1.64  2007/09/11 17:08:49  amodigli
00036  * mooved uves_polynomial_convert_from_plist_midas to uves_dfs
00037  *
00038  * Revision 1.63  2007/08/21 13:08:26  jmlarsen
00039  * Removed irplib_access module, largely deprecated by CPL-4
00040  *
00041  * Revision 1.62  2007/06/20 15:34:50  jmlarsen
00042  * Changed indentation
00043  *
00044  * Revision 1.61  2007/06/20 08:30:00  amodigli
00045  * added index parameter to support FIBER mode lintab in uves_polynomial_convert_from_plist_midas
00046  *
00047  * Revision 1.60  2007/06/06 08:17:33  amodigli
00048  * replace tab with 4 spaces
00049  *
00050  * Revision 1.59  2007/05/03 15:23:08  jmlarsen
00051  * Removed dead code
00052  *
00053  * Revision 1.58  2007/05/03 15:18:29  jmlarsen
00054  * Added function to add polynomials
00055  *
00056  * Revision 1.57  2007/04/27 07:21:51  jmlarsen
00057  * Polyfit: Changed error code from ILLEGAL_INPUT to SINGULAR_MATRIX
00058  *
00059  * Revision 1.56  2007/04/24 12:50:29  jmlarsen
00060  * Replaced cpl_propertylist -> uves_propertylist which is much faster
00061  *
00062  * Revision 1.55  2007/03/23 08:01:55  jmlarsen
00063  * Fixed mixed code and declarations
00064  *
00065  * Revision 1.54  2007/03/19 15:10:03  jmlarsen
00066  * Optimization in 2d fitting: do not call pow too often
00067  *
00068  * Revision 1.53  2007/03/13 15:35:11  jmlarsen
00069  * Made a few time optimizations
00070  *
00071  * Revision 1.52  2007/03/05 10:20:49  jmlarsen
00072  * Added uves_polynomial_delete_const()
00073  *
00074  * Revision 1.51  2007/01/15 08:48:51  jmlarsen
00075  * Shortened lines
00076  *
00077  * Revision 1.50  2006/11/24 09:36:49  jmlarsen
00078  * Workaround for slow uves_propertylist_get_size
00079  *
00080  * Revision 1.49  2006/11/15 15:02:15  jmlarsen
00081  * Implemented const safe workarounds for CPL functions
00082  *
00083  * Revision 1.47  2006/11/15 14:04:08  jmlarsen
00084  * Removed non-const version of parameterlist_get_first/last/next which is
00085  * already in CPL, added const-safe wrapper, unwrapper and deallocator functions
00086  *
00087  * Revision 1.46  2006/11/13 14:23:55  jmlarsen
00088  * Removed workarounds for CPL const bugs
00089  *
00090  * Revision 1.45  2006/11/06 15:19:42  jmlarsen
00091  * Removed unused include directives
00092  *
00093  * Revision 1.44  2006/09/08 14:06:29  jmlarsen
00094  * Removed profiling code
00095  *
00096  * Revision 1.43  2006/09/06 14:46:21  jmlarsen
00097  * Added missing newline in uves_polynomial_dump()
00098  *
00099  * Revision 1.42  2006/08/17 14:11:25  jmlarsen
00100  * Use assure_mem macro to check for memory allocation failure
00101  *
00102  * Revision 1.41  2006/08/17 13:56:53  jmlarsen
00103  * Reduced max line length
00104  *
00105  * Revision 1.40  2006/07/03 13:27:52  jmlarsen
00106  * Moved failing assertion to where it should be
00107  *
00108  * Revision 1.39  2006/06/01 14:43:17  jmlarsen
00109  * Added missing documentation
00110  *
00111  * Revision 1.38  2006/05/05 13:59:03  jmlarsen
00112  * Support fitting zero-degree polynomial
00113  *
00114  * Revision 1.37  2006/04/24 09:28:29  jmlarsen
00115  * Added function to create zero-polynomial
00116  *
00117  * Revision 1.36  2006/03/27 09:41:37  jmlarsen
00118  * Added timing markers
00119  *
00120  * Revision 1.35  2006/03/09 10:52:25  jmlarsen
00121  * Renamed pow->power
00122  *
00123  * Revision 1.34  2006/03/03 13:54:11  jmlarsen
00124  * Changed syntax of check macro
00125  *
00126  * Revision 1.33  2005/12/19 16:17:56  jmlarsen
00127  * Replaced bool -> int
00128  *
00129  */
00130 
00131 #ifdef HAVE_CONFIG_H
00132 #  include <config.h>
00133 #endif
00134 
00135 /*----------------------------------------------------------------------------*/
00155 /*----------------------------------------------------------------------------*/
00156 
00157 /*-----------------------------------------------------------------------------
00158                                 Defines
00159  -----------------------------------------------------------------------------*/
00160 
00161 /*
00162  *  When storing a 2d polynomial in a table,
00163  *  these column names are used
00164  */
00165 #define COLUMN_ORDER1 "Order1"
00166 #define COLUMN_ORDER2 "Order2"
00167 #define COLUMN_COEFF  "Coeff"
00168 
00171 /*-----------------------------------------------------------------------------
00172                                 Includes
00173  -----------------------------------------------------------------------------*/
00174 #include <uves_utils_polynomial.h>
00175 
00176 #include <uves_utils.h>
00177 #include <uves_utils_wrappers.h>
00178 #include <uves_dump.h>
00179 #include <uves_msg.h>
00180 #include <uves_error.h>
00181 
00182 #include <cpl.h>
00183 
00184 /*-----------------------------------------------------------------------------
00185                             Typedefs
00186  -----------------------------------------------------------------------------*/
00189 struct _polynomial 
00190 {
00192     cpl_polynomial *pol; 
00193 
00195     cpl_vector *vec;
00196     double *vec_data;
00197 
00198     int dimension;  /* for efficiency */
00199 
00201     double *shift;
00202 
00204     double *scale;
00205 };
00206 
00207 /*-----------------------------------------------------------------------------
00208                             Implementation
00209  -----------------------------------------------------------------------------*/
00210 /*----------------------------------------------------------------------------*/
00221 /*----------------------------------------------------------------------------*/
00222 polynomial *
00223 uves_polynomial_new(const cpl_polynomial *pol)
00224 {
00225     polynomial *p = NULL;
00226     int i;
00227     
00228     /* Test input */
00229     assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00230 
00231     /* Allocate and initialize struct */
00232     p = cpl_calloc(1, sizeof(polynomial)) ;
00233     assure_mem( p );
00234 
00235     check( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");
00236 
00237     /* Allocate vector */
00238     p->vec = cpl_vector_new(p->dimension);
00239     assure_mem( p->vec );
00240     p->vec_data = cpl_vector_get_data(p->vec);
00241 
00242     /* Shifts are initialized to zero, scales to 1 */
00243     p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
00244     assure_mem( p->shift );
00245 
00246     p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
00247     assure_mem( p->scale );
00248     for (i = 0; i <= p->dimension; i++)
00249     p->scale[i] = 1.0;
00250 
00251     check( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
00252     
00253   cleanup:
00254     if (cpl_error_get_code() != CPL_ERROR_NONE)
00255     uves_polynomial_delete(&p);
00256     
00257     return p;
00258 }
00259 
00260 /*----------------------------------------------------------------------------*/
00268 /*----------------------------------------------------------------------------*/
00269 polynomial *
00270 uves_polynomial_new_zero(int dim)
00271 {
00272     polynomial *result = NULL;
00273     cpl_polynomial *p = NULL;
00274 
00275     assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);
00276 
00277     p = cpl_polynomial_new(dim);
00278     assure_mem( p );
00279 
00280     result = uves_polynomial_new(p);
00281     assure_mem( result );
00282 
00283   cleanup:
00284     uves_free_polynomial(&p);
00285 
00286     return result;
00287 }
00288 
00289 /*----------------------------------------------------------------------------*/
00296 /*----------------------------------------------------------------------------*/
00297 void 
00298 uves_polynomial_delete(polynomial **p)
00299 {
00300     uves_polynomial_delete_const((const polynomial **)p);
00301 }
00302 
00303 /*----------------------------------------------------------------------------*/
00310 /*----------------------------------------------------------------------------*/
00311 void 
00312 uves_polynomial_delete_const(const polynomial **p)
00313 {
00314     if (*p == NULL) return;
00315     cpl_polynomial_delete((*p)->pol);
00316     cpl_vector_delete((*p)->vec);
00317     cpl_free((*p)->shift);
00318     cpl_free((*p)->scale);
00319     uves_free(*p);
00320     *p = NULL;
00321     return;
00322 }
00323 /*----------------------------------------------------------------------------*/
00329 /*----------------------------------------------------------------------------*/
00330 int
00331 uves_polynomial_get_degree(const polynomial *p)
00332 {
00333     int result = -1;
00334     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00335     
00336     result = cpl_polynomial_get_degree(p->pol);
00337 
00338   cleanup:
00339     return result;
00340 }
00341 
00342 /*----------------------------------------------------------------------------*/
00348 /*----------------------------------------------------------------------------*/
00349 polynomial *
00350 uves_polynomial_duplicate(const polynomial *p)
00351 {
00352     polynomial *result = NULL;
00353     int dimension;
00354     int i;
00355 
00356     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00357     dimension = uves_polynomial_get_dimension(p);
00358 
00359     check( result = uves_polynomial_new(p->pol),
00360        "Error allocating polynomial");
00361     
00362     for (i = 0; i <= dimension; i++)
00363     {
00364         result->shift[i] = p->shift[i];
00365         result->scale[i] = p->scale[i];
00366     }
00367 
00368   cleanup:
00369     if (cpl_error_get_code() != CPL_ERROR_NONE)
00370     {
00371         uves_polynomial_delete(&result);
00372         return NULL;
00373     }
00374     
00375     return result;
00376 }
00377 
00378 
00379 /*----------------------------------------------------------------------------*/
00390 /*----------------------------------------------------------------------------*/
00391 cpl_table *
00392 uves_polynomial_convert_to_table(const polynomial *p)
00393 {
00394     cpl_table *t = NULL; /* Result */
00395     int degree;
00396     int i, j, row;
00397 
00398     /* Check input */
00399     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00400     assure( uves_polynomial_get_dimension(p) == 2, 
00401         CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
00402     
00403     degree = cpl_polynomial_get_degree(p->pol);
00404 
00405     /* Allocate space for 3 shifts, 3 scale factors and all
00406        coefficients */
00407     t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
00408     cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
00409     cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
00410     cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);
00411 
00412     row = 0;
00413 
00414     /* First write the shifts, write non-garbage to coeff columns (which are not used) */
00415     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00416     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00417     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;
00418 
00419     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00420     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00421     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;
00422 
00423     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00424     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00425     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;
00426 
00427     /* Then the scale factors */
00428     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00429     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00430     cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[0]); row++;
00431 
00432     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00433     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00434     cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[1]); row++;
00435 
00436     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00437     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00438     cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;
00439 
00440     /* And then write the coefficients */
00441     for (i = 0; i <= degree; i++){
00442     for (j = 0; j+i <= degree; j++){
00443         double coeff;
00444         cpl_size power[2];
00445         power[0] = i;
00446         power[1] = j;
00447         
00448         coeff = cpl_polynomial_get_coeff(p->pol, power);
00449         cpl_table_set_int   (t, COLUMN_ORDER1, row, power[0]);
00450         cpl_table_set_int   (t, COLUMN_ORDER2, row, power[1]);
00451         cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
00452         
00453         row++;
00454     }
00455     }
00456 
00457   cleanup:
00458     return t;
00459 }
00460 
00461 /*----------------------------------------------------------------------------*/
00470 /*----------------------------------------------------------------------------*/
00471 polynomial *
00472 uves_polynomial_convert_from_table(cpl_table *t)
00473 {
00474     polynomial *p = NULL;  /* Result */
00475     cpl_polynomial *pol = NULL;
00476     cpl_type type;
00477     int i;
00478     
00479     /* Only 2d supported */
00480     check( pol = cpl_polynomial_new(2), "Error initializing polynomial");
00481 
00482     /* Check table format */
00483     assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
00484     assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT, 
00485        "No '%s' column found in table", COLUMN_ORDER1);
00486     assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
00487        "No '%s' column found in table", COLUMN_ORDER2);
00488     assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
00489        "No '%s' column found in table", COLUMN_COEFF );
00490     
00491     type = cpl_table_get_column_type(t, COLUMN_ORDER1);
00492     assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
00493        "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
00494        uves_tostring_cpl_type(type));
00495     
00496     type = cpl_table_get_column_type(t, COLUMN_ORDER2);
00497     assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
00498        "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
00499        uves_tostring_cpl_type(type));
00500     
00501     type = cpl_table_get_column_type(t, COLUMN_COEFF);
00502     assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
00503        "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
00504        uves_tostring_cpl_type(type));
00505 
00506     assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
00507        "Table must contain at least one coefficient");
00508     
00509     /* Read the coefficients */
00510     for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
00511     double coeff;
00512     cpl_size power[2];
00513     
00514     check(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
00515         power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
00516         coeff  = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
00517            "Error reading table row %d", i);
00518     
00519     uves_msg_debug("Pol.coeff.(%" CPL_SIZE_FORMAT ", %" CPL_SIZE_FORMAT ") = %e", power[0], power[1], coeff);
00520 
00521     check( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
00522     }
00523     p = uves_polynomial_new(pol);
00524 
00525     /* Read shifts and rescaling */
00526     uves_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
00527     uves_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
00528     uves_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
00529     uves_polynomial_shift  (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
00530     uves_polynomial_shift  (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
00531     uves_polynomial_shift  (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
00532 
00533   cleanup:
00534     uves_free_polynomial(&pol);
00535     if (cpl_error_get_code() != CPL_ERROR_NONE)
00536     uves_polynomial_delete(&p);
00537 
00538     return p;
00539 }
00540 
00541 
00542 /*----------------------------------------------------------------------------*/
00548 /*----------------------------------------------------------------------------*/
00549 int
00550 uves_polynomial_get_dimension(const polynomial *p)
00551 {
00552     int dim = -1;
00553     assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00554 
00555 /* slow     check( dim = cpl_polynomial_get_dimension(p->pol), "Error reading dimension"); */
00556     dim = p->dimension;
00557     
00558   cleanup:
00559     return dim;
00560 }
00561 
00562 /*----------------------------------------------------------------------------*/
00570 /*----------------------------------------------------------------------------*/
00571 void uves_polynomial_dump(const polynomial *p, FILE *stream)
00572 {
00573     if (p == NULL)
00574     fprintf(stream, "Null polynomial\n");
00575     else {
00576     int i;
00577     cpl_polynomial_dump(p->pol, stream);
00578     fprintf(stream, "shift_y \t= %f  \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
00579     for (i = 1; i <= uves_polynomial_get_dimension(p); i++)
00580         {
00581         fprintf(stream, "shift_x%d \t= %f  \tscale_x%d \t= %f\n", 
00582             i, p->shift[i], i, p->scale[i]);
00583         }
00584     }
00585     return;
00586 }
00587 
00588 /*----------------------------------------------------------------------------*/
00602 /*----------------------------------------------------------------------------*/
00603 cpl_error_code
00604 uves_polynomial_rescale(polynomial *p, int varno, double scale)
00605 {
00606     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00607     assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
00608        CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00609 
00610     /*  Rescaling an x variable by the factor S corresponds to:  
00611      *    p'(x) := p(x/S)  =
00612      *  cpl( (x/S -  shiftx ) /    scalex  ) * scaley + shifty  = 
00613      *  cpl( (x - (S*shiftx)) / (S*scalex) ) * scaley + shifty      */
00614 
00615     /*  Rescaling the y variable by the factor S corresponds to:  
00616      *    p'(x) := S*p(x)  =
00617      *  S * ( cpl((x - shiftx)/scalex) * scaley     + shifty )  = 
00618      *        cpl((x - shiftx)/scalex) * (S*scaley) + (S*shifty) 
00619      *
00620      *  therefore the implementation is the same in the two cases. */
00621      
00622     p->shift[varno] *= scale;
00623     p->scale[varno] *= scale;
00624 
00625   cleanup:
00626     return cpl_error_get_code();
00627 }
00628 
00629 /*----------------------------------------------------------------------------*/
00643 /*----------------------------------------------------------------------------*/
00644 cpl_error_code
00645 uves_polynomial_shift(polynomial *p, int varno, double shift)
00646 {
00647     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00648     assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
00649        CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00650 
00651     /* The implementation is similar for x and y variables because
00652      *  p(x-S)  =
00653      *  cpl( (x-S - shiftx)   / scalex ) * scaley + shifty  = 
00654      *  cpl( (x - (shiftx+S)) / scalex ) * scaley + shifty
00655      * and
00656      *  p(x) + S  =
00657      *  cpl( (x - shiftx)/scalex ) * scaley + shifty + S  = 
00658      *  cpl( (x - shiftx)/scalex ) * scaley + (shifty+S)      */
00659 
00660     p->shift[varno] += shift;
00661 
00662   cleanup:
00663     return cpl_error_get_code();
00664 }
00665 
00666 /*----------------------------------------------------------------------------*/
00675 /*----------------------------------------------------------------------------*/
00676 double
00677 uves_polynomial_evaluate_1d(const polynomial *p, double x)
00678 {
00679     double result = 0;
00680     
00681     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00682     assure(uves_polynomial_get_dimension(p) == 1, 
00683        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00684     
00685     check( result = 
00686        cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
00687        * p->scale[0] + p->shift[0],
00688        "Could not evaluate polynomial");
00689     
00690   cleanup:
00691     return result;
00692 }
00693 
00694 
00695 /*----------------------------------------------------------------------------*/
00705 /*----------------------------------------------------------------------------*/
00706 
00707 double
00708 uves_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
00709 {
00710     double result = 0;
00711 
00712     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00713     assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
00714        "Polynomial must be 2d. It's %dd", p->dimension);
00715     {
00716         double scale = p->scale[0];
00717         double shift = p->shift[0];
00718 
00719         //    cpl_vector_set(p->vec, 0, (x1 - p->shift[1]) / p->scale[1]);
00720         //    cpl_vector_set(p->vec, 1, (x2 - p->shift[2]) / p->scale[2]);
00721         p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
00722         p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
00723         
00724         result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
00725     }
00726 
00727   cleanup:
00728     return result;
00729 }
00730 
00731 /*----------------------------------------------------------------------------*/
00744 /*----------------------------------------------------------------------------*/
00745 double
00746 uves_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
00747 {
00748     double result = 0;
00749     cpl_size power[1];
00750     double coeff0;
00751 
00752     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00753     assure(uves_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT, 
00754        "Polynomial must be 1d");
00755     
00756     /* Solving p(x) = value corresponds to solving
00757        <=>    cpl_p( (x-xshift)/xscale )*yscale + yshift = value
00758        <=>    cpl_p( (x-xshift)/xscale ) + (yshift - value)/yscale = 0 
00759 
00760        So   1) find zero point for the polynomial   cpl() + (yshift-value)/yscale
00761        Then 2) shift and rescale the result
00762     */
00763 
00764     power[0] = 0;
00765     check(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
00766         cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
00767       "Error setting coefficient");
00768 
00769     check( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
00770                    &result, multiplicity), "Could not find root");
00771     /* Restore polynomial */
00772     cpl_polynomial_set_coeff(p->pol, power, coeff0);
00773     
00774     /* Shift solution */
00775     result = result * p->scale[1] + p->shift[1];
00776 
00777   cleanup:
00778     return result;
00779 }
00780 
00781 /*----------------------------------------------------------------------------*/
00798 /*----------------------------------------------------------------------------*/
00799 double
00800 uves_polynomial_solve_2d(const polynomial *p, double value, double guess,
00801              int multiplicity, int varno, double x_value)
00802 {
00803     double result = 0;
00804     polynomial *pol_1d = NULL;
00805 
00806     assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00807         "Illegal variable number: %d", varno);
00808 
00809     check( pol_1d = uves_polynomial_collapse(p, varno, x_value),
00810        "Could not collapse polynomial");
00811 
00812     check( result = uves_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
00813        "Could not find root");
00814 
00815   cleanup:
00816     uves_polynomial_delete(&pol_1d);
00817     return result;
00818 }
00819 
00820 /*----------------------------------------------------------------------------*/
00829 /*----------------------------------------------------------------------------*/
00830 double
00831 uves_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
00832 {
00833     double result = 0;
00834     cpl_size power[2];
00835 
00836     assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00837         "Illegal variable number (%d)", varno);
00838 
00839     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00840     assure(uves_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
00841        "Polynomial must be 2d. It's %dd", uves_polynomial_get_dimension(p));
00842 
00843     /*  d/dx_i [ p(x) ] =
00844      *  d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
00845      *  [ d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley ]
00846      */
00847 
00848     /* Shift, scale  (x1, x2) */
00849     x1 = (x1 - p->shift[1])/p->scale[1];
00850     x2 = (x2 - p->shift[2])/p->scale[2];
00851  
00852     /* Get derivative of cpl polynomial.
00853      * 
00854      */
00855     {
00856     int degree = cpl_polynomial_get_degree(p->pol);
00857     double yj = 1;  /* y^j */
00858     int i, j;
00859     
00860     result = 0;
00861     for (j = 0, yj = 1;
00862          j <= degree; j++,
00863          yj *= (varno == 1) ? x2 : x1)
00864         {
00865         /*  Proof by example (degree = 3): For each j account for these terms
00866          *  using Horner's rule:
00867          *
00868          * d/dx     y^j * [  c_3j x^3 +  c_2j x^2 +  c_1j x^1 + c_0j ]   =
00869          *
00870          *          y^j * [ 3c_3j x^2 + 2c_2j x^1 + 1c_1j ]     =
00871          *
00872          *          y^j * [ ((3c_3j) x + 2c_2j) x + 1c_1j ]
00873          */
00874 
00875         double sum = 0;
00876         for (i = degree; i >= 1; i--)
00877             {
00878             double c_ij;
00879 
00880             power[0] = (varno == 1) ? i : j;
00881             power[1] = (varno == 1) ? j : i;
00882             
00883             c_ij = cpl_polynomial_get_coeff(p->pol, power);
00884             
00885             sum += (i * c_ij);
00886             if (i >= 2) sum *= (varno == 1) ? x1 : x2;
00887             }
00888 
00889         /* Collect terms */
00890         result += yj * sum;
00891         }
00892     }
00893 
00894     result *= p->scale[0];
00895 
00896 
00897 /* Old code: This method (valid for varno = 2)
00898    of getting the derivative of
00899    the CPL polynomial is slow because of the call to 
00900    uves_polynomial_collapse()
00901 
00902    check( pol_1d = uves_polynomial_collapse(p, 1, x1);
00903    dummy = cpl_polynomial_eval_1d(pol_1d->pol, (x2 - pol_1d->shift[1])/pol_1d->scale[1], &result),
00904    "Error evaluating derivative");
00905 */
00906     
00907   cleanup:
00908     return result;
00909 }
00910 
00911 /*----------------------------------------------------------------------------*/
00918 /*----------------------------------------------------------------------------*/
00919 double
00920 uves_polynomial_derivative_1d(const polynomial *p, double x)
00921 {
00922     double result = 0;
00923     double dummy;
00924 
00925     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00926     assure(uves_polynomial_get_dimension(p) == 1, 
00927        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00928     
00929     check( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
00930        "Error evaluating derivative");
00931     
00932   cleanup:
00933     return result;
00934 }
00935 
00936 /*----------------------------------------------------------------------------*/
00943 /*----------------------------------------------------------------------------*/
00944 polynomial *
00945 uves_polynomial_add_2d(const polynomial *p1, const polynomial *p2)
00946 {
00947     polynomial *result = NULL;
00948     cpl_polynomial *pol = NULL;
00949 
00950     assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00951     assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00952     assure(uves_polynomial_get_dimension(p1) == 2, 
00953        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00954     assure(uves_polynomial_get_dimension(p2) == 2, 
00955        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00956 
00957     /* cpl_polynomial1((x - shift_x1)/scale_x1) * scale_y1 + shift_y1
00958        +
00959        cpl_polynomial2((x - shift_x2)/scale_x2) * scale_y2 + shift_y2
00960        = ???
00961        Not easy.
00962 
00963        Use brute force:
00964     */
00965     
00966     {
00967         int degree, i, j;
00968 
00969         degree = uves_max_int(uves_polynomial_get_degree(p1),
00970                               uves_polynomial_get_degree(p2));
00971         
00972         pol = cpl_polynomial_new(2);
00973         for (i = 0; i <= degree; i++)
00974             for (j = 0; j <= degree; j++) {
00975                 double coeff1, coeff2;
00976                 cpl_size power[2];
00977 
00978                 /* Simple: add coefficients of the same power */
00979                 coeff1 = uves_polynomial_get_coeff_2d(p1, i, j);
00980                 coeff2 = uves_polynomial_get_coeff_2d(p2, i, j);
00981                 
00982                 power[0] = i;
00983                 power[1] = j;
00984                 cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
00985             }
00986     }
00987 
00988     result = uves_polynomial_new(pol);
00989    
00990   cleanup:
00991     uves_free_polynomial(&pol);
00992     return result;
00993 }
00994 
00995 /*----------------------------------------------------------------------------*/
01008 /*----------------------------------------------------------------------------*/
01009 static cpl_error_code
01010 derivative_cpl_polynomial(cpl_polynomial *p, int varno)
01011 {
01012     int dimension, degree;
01013     int i, j;
01014     cpl_size power[2];
01015     
01016     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01017     dimension = cpl_polynomial_get_dimension(p);
01018     degree = cpl_polynomial_get_degree(p);
01019     assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT, 
01020         "Illegal dimension: %d", dimension);
01021     assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01022         "Illegal variable number: %d", varno);
01023     
01024     if (dimension == 1)
01025     {
01026         /*  a_i := (i+1) * a_(i+1) */
01027         for(i = 0; i <= degree; i++)
01028         {
01029             double coeff;
01030             power[0] = i+1;
01031             /* power[1] is ignored */
01032             
01033             coeff = cpl_polynomial_get_coeff(p, power);
01034                 
01035             power[0] = i;            
01036             cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01037         }
01038     }
01039     
01040     if (dimension == 2)
01041     {
01042         /*  a_ij := (i+1) * a_{(i+1),j} */
01043         for(i = 0; i <= degree; i++)
01044         {
01045             for(j = 0; i + j <= degree; j++)
01046             {
01047                 double coeff;
01048                 power[varno - 1] = i+1;    /* varno == 1:    0,1  */ 
01049                 power[2 - varno] = j;      /* varno == 2:    1,0  */
01050                 
01051                 coeff = cpl_polynomial_get_coeff(p, power);
01052                 
01053                 power[varno - 1] = i;
01054                 
01055                 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01056             }
01057         }
01058     }
01059 
01060   cleanup:
01061     return cpl_error_get_code();
01062 }
01063 
01064 /*----------------------------------------------------------------------------*/
01074 /*----------------------------------------------------------------------------*/
01075 cpl_error_code
01076 uves_polynomial_derivative(polynomial *p, int varno)
01077 {
01078     int dimension;
01079     
01080     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01081     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01082     assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
01083         "Illegal variable number: %d", varno);
01084 
01085 
01086     /*   d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
01087      *     sum_j d(cpl)/dx_j ( (x - shiftx) / scalex ) * scaley * dx_j/dx_i / scalex_j =
01088      *     d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley/scalex_i,
01089      * 
01090      * so transform :      shifty -> 0
01091      *                     shiftx -> shiftx
01092      *                     scaley -> scaley/scalex_i
01093      *                     scalex -> scalex
01094      *                       cpl  -> d(cpl)/dx_i
01095      */
01096 
01097     p->shift[0] = 0;
01098     p->scale[0] = p->scale[0] / p->scale[varno];
01099 
01100     check( derivative_cpl_polynomial(p->pol, varno),
01101        "Error calculating derivative of CPL-polynomial");
01102     
01103   cleanup:
01104     return cpl_error_get_code();
01105 }
01106 
01107 
01108 /*----------------------------------------------------------------------------*/
01117 /*----------------------------------------------------------------------------*/
01118 double
01119 uves_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
01120 {
01121     polynomial *pp = NULL;
01122     int dimension;
01123     double result = 0;
01124     double factorial;
01125     
01126     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01127     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01128     assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01129     assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
01130     assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);
01131 
01132     /* Calculate the coefficient as
01133      * d^N p / (dx1^degree1 dx2^degree2)  /  (degree1! * degree2!)
01134      * evaluated in (0,0)
01135     */
01136 
01137     pp = uves_polynomial_duplicate(p);
01138 
01139     factorial = 1;
01140     while(degree1 > 0)
01141     {
01142         check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
01143 
01144         factorial *= degree1;
01145         degree1 -= 1;
01146     }
01147 
01148     while(degree2 > 0)
01149     {
01150         check( uves_polynomial_derivative(pp, 2), "Error calculating derivative");
01151 
01152         factorial *= degree2;
01153         degree2 -= 1;
01154     }
01155     
01156     check( result = uves_polynomial_evaluate_2d(pp, 0, 0) / factorial,
01157        "Error evaluating polynomial");
01158     
01159   cleanup:
01160     uves_polynomial_delete(&pp);
01161     return result;
01162 }
01163 /*----------------------------------------------------------------------------*/
01173 /*----------------------------------------------------------------------------*/
01174 double
01175 uves_polynomial_get_coeff_1d(const polynomial *p, int degree)
01176 {
01177     polynomial *pp = NULL;
01178     int dimension;
01179     double result = 0;
01180     double factorial;
01181     
01182     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01183     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01184     assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01185     assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
01186     
01187     /* Calculate the coefficient as
01188      *  d^degree p/dx^degree  /  (degree1! * degree2!)
01189      * evaluated in 0.
01190      */
01191     
01192     pp = uves_polynomial_duplicate(p);
01193     
01194     factorial = 1;
01195     while(degree > 0)
01196     {
01197         check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
01198         
01199         factorial *= degree;
01200         degree -= 1;
01201     }
01202     
01203     check( result = uves_polynomial_evaluate_1d(pp, 0) / factorial,
01204        "Error evaluating polynomial");
01205     
01206   cleanup:
01207     uves_polynomial_delete(&pp);
01208     return result;
01209 }
01210 
01211 
01212 /*----------------------------------------------------------------------------*/
01228 /*----------------------------------------------------------------------------*/
01229 polynomial *
01230 uves_polynomial_collapse(const polynomial *p, int varno, double value)
01231 {
01232     polynomial     *result  = NULL;
01233     cpl_polynomial *pol     = NULL;
01234     cpl_size            *power     = NULL;
01235 
01236     int i, j;
01237     int degree, dimension;
01238     
01239     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01240     dimension = uves_polynomial_get_dimension(p);
01241     assure(dimension  > 0, CPL_ERROR_ILLEGAL_INPUT,
01242        "Polynomial has non-positive dimension: %d", dimension);
01243     assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
01244        "Don't collapse a 1d polynomial. Evaluate it!");
01245 
01246     /* To generalize this function to work with dimensions higher than 2,
01247        also changes needs to be made below (use varno properly). For now,
01248        support only 2d. */
01249     assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
01250     
01251     assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
01252        "Wrong variable number");
01253     value = (value - p->shift[varno]) / p->scale[varno];
01254 
01255     /* Compute new coefficients */
01256     degree = cpl_polynomial_get_degree(p->pol);
01257     pol    = cpl_polynomial_new(dimension - 1);
01258     power = cpl_malloc(sizeof(cpl_size) * dimension);
01259     assure_mem( power );
01260     for (i = 0; i <= degree; i++) 
01261     {
01262         double coeff;
01263         
01264         power[2-varno] = i;   /* map 2->0  and 1->1 */
01265         
01266         /* Collect all terms with x^i  (using Horner's rule) */
01267         coeff = 0;
01268         for (j = degree - i; j >= 0; j--) 
01269         {
01270             power[varno-1] = j;  /* map 2->1 and 1->0 */
01271             coeff += cpl_polynomial_get_coeff(p->pol, power);
01272             if (j > 0) coeff *= value;
01273         }
01274         /* Write coefficient in 1d polynomial */
01275         power[0] = i;
01276         cpl_polynomial_set_coeff(pol, power, coeff);
01277     }
01278     
01279     /* Wrap the polynomial */
01280     result = uves_polynomial_new(pol);
01281 
01282     /* Copy the shifts and scales, skip variable number varno */
01283     j = 0;
01284     for(i = 0; i <= dimension - 1; i++) 
01285     {
01286         if (i == varno) 
01287         {
01288             /* Don't copy */
01289             j += 2;
01290             /* For the remainder of this for loop, j = i+1 */
01291         }
01292         else 
01293         {
01294             result->shift[i] = p->shift[j];
01295             result->scale[i] = p->scale[j];
01296             j += 1;
01297         }
01298     }
01299     
01300     assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
01301        "Error collapsing polynomial");
01302     
01303   cleanup:
01304     cpl_free(power); power = NULL;
01305     uves_free_polynomial(&pol);
01306     if (cpl_error_get_code() != CPL_ERROR_NONE)
01307     {
01308         uves_polynomial_delete(&result);
01309     }
01310     return result;
01311 }
01312 
01313 
01314 
01315 /*----------------------------------------------------------------------------*/
01335 /*----------------------------------------------------------------------------*/
01336 polynomial * uves_polynomial_fit_1d(
01337     const cpl_vector    *   x_pos,
01338     const cpl_vector    *   values,
01339     const cpl_vector    *   sigmas,
01340     int                     poly_deg,
01341     double              *   mse)
01342 {
01343     int                 nc ;
01344     int                 np ;
01345     cpl_matrix      *   ma = NULL;
01346     cpl_matrix      *   mb = NULL;
01347     cpl_matrix      *   mx = NULL;
01348     const double    *   x_pos_data ;
01349     const double    *   values_data ;
01350     const double    *   sigmas_data = NULL;
01351     double              mean_x, mean_z;
01352     polynomial      *   result = NULL;
01353     cpl_polynomial  *   out ;
01354     cpl_vector      *   x_val = NULL;
01355     int                 i, j ;
01356     
01357     /* Check entries */
01358     assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
01359     assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT, 
01360         "Polynomial degree is %d. Must be non-negative", poly_deg);
01361     np = cpl_vector_get_size(x_pos) ;
01362     
01363     nc = 1 + poly_deg ;
01364     assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
01365         "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
01366         np, poly_deg, nc);
01367 
01368     /* Fill up look-up table for coefficients to compute */
01369     /* Initialize matrices */
01370     /* ma contains the polynomial terms for each input point. */
01371     /* mb contains the values */
01372     ma = cpl_matrix_new(np, nc) ;
01373     mb = cpl_matrix_new(np, 1) ;
01374 
01375     /* Get mean values */
01376     mean_x = cpl_vector_get_mean(x_pos);
01377     mean_z = cpl_vector_get_mean(values);
01378 
01379     /* Fill up matrices, shift */
01380     x_pos_data = cpl_vector_get_data_const(x_pos) ;
01381     values_data = cpl_vector_get_data_const(values) ;
01382     if (sigmas != NULL)
01383     {
01384         sigmas_data = cpl_vector_get_data_const(sigmas) ;
01385     }
01386 
01387     if (sigmas != NULL)
01388     {
01389         for (i=0 ; i<np ; i++) 
01390         {
01391             /* Catch division by zero */
01392             if (sigmas_data[i] == 0)
01393             {
01394                 uves_free_matrix(&ma) ;
01395                 uves_free_matrix(&mb) ;
01396                 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01397                    "Sigmas must be non-zero");
01398             }
01399             for (j=0 ; j<nc ; j++) 
01400             {
01401                 cpl_matrix_set(ma, i, j,  
01402                        uves_pow_int(x_pos_data[i] - mean_x, j) /
01403                        sigmas_data[i]) ;
01404             }
01405             /* mb contains surface values (z-axis) */
01406             cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
01407         }
01408     }
01409     else  /* Use sigma = 1 */
01410     {
01411         for (i=0 ; i<np ; i++) 
01412         {
01413             for (j=0 ; j<nc ; j++) 
01414             {
01415                 cpl_matrix_set(ma, i, j,  
01416                        uves_pow_int(x_pos_data[i] - mean_x, j) / 1);
01417             }
01418             /* mb contains surface values (z-values) */
01419             cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01420         }
01421     }
01422     
01423     /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
01424     check( mx = cpl_matrix_solve_normal(ma, mb),
01425        "Could not invert matrix");
01426     uves_free_matrix(&ma);
01427     uves_free_matrix(&mb);
01428 
01429     /* Store coefficients for output */
01430     out = cpl_polynomial_new(1) ;
01431     cpl_size deg=0;
01432     for (deg=0 ; deg<nc ; deg++) {
01433         cpl_polynomial_set_coeff(out, &deg, cpl_matrix_get(mx, deg, 0)) ;
01434     }
01435     uves_free_matrix(&mx);
01436 
01437     /* If requested, compute mean squared error */
01438     if (mse != NULL) {
01439         *mse = 0.00 ;
01440         x_val = cpl_vector_new(1) ;
01441         for (i=0 ; i<np ; i++)
01442         {
01443         double residual;
01444         cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
01445         /* Subtract from the true value, square, accumulate */
01446         residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
01447         *mse += residual*residual;
01448         }
01449         uves_free_vector(&x_val) ;
01450         /* Average the error term */
01451         *mse /= (double)np ;
01452     }
01453 
01454     /* Create and shift result */
01455     result = uves_polynomial_new(out);
01456     uves_free_polynomial(&out);
01457 
01458     uves_polynomial_shift(result, 0, mean_z);
01459     uves_polynomial_shift(result, 1, mean_x);
01460 
01461   cleanup:
01462     uves_free_vector(&x_val);
01463     uves_free_matrix(&ma);
01464     uves_free_matrix(&mb);
01465     uves_free_matrix(&mx);
01466     return result;
01467 }
01468 
01469 
01470 /*----------------------------------------------------------------------------*/
01514 /*----------------------------------------------------------------------------*/
01515 polynomial *
01516 uves_polynomial_fit_2d(
01517     const cpl_bivector     *  xy_pos,
01518     const cpl_vector       *  values,
01519     const cpl_vector       *  sigmas,
01520     int                       poly_deg1,
01521     int                       poly_deg2,
01522     double                 *  mse,
01523     double                 *  red_chisq,
01524     polynomial             ** variance)
01525 {
01526     int                 nc ;
01527     int                 degx, degy ;
01528     int             *   degx_tab ;
01529     int             *   degy_tab ;
01530     int                 np ;
01531     cpl_matrix      *   ma ;
01532     cpl_matrix      *   mb ;
01533     cpl_matrix      *   mx ;
01534     cpl_matrix      *   mat;
01535     cpl_matrix      *   mat_ma;
01536     cpl_matrix      *   cov = NULL;
01537     const double    *   xy_pos_data_x ;
01538     const double    *   xy_pos_data_y ;
01539     const double    *   values_data ;
01540     const double    *   sigmas_data = NULL;
01541     const cpl_vector*   xy_pos_x;
01542     const cpl_vector*   xy_pos_y;
01543     double              mean_x, mean_y, mean_z;
01544     cpl_polynomial  *   out ;
01545     cpl_polynomial  *   variance_cpl ;
01546     polynomial      *   result         = NULL;
01547     cpl_size             *   powers ;
01548 
01549     /* Check entries */
01550     assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
01551     assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree1 is %d", poly_deg1);
01552     assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree2 is %d", poly_deg2);
01553     np = cpl_bivector_get_size(xy_pos) ;
01554 
01555     /* Can't calculate variance and chi_sq without sigmas */
01556     assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL, 
01557         CPL_ERROR_ILLEGAL_INPUT, 
01558         "Cannot calculate variance or chi_sq without knowing");
01559 
01560     /* Fill up look-up table for coefficients to compute */
01561     nc = (1 + poly_deg1)*(1 + poly_deg2) ;     /* rectangular matrix */
01562     
01563     assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX, "%d coefficients. Only %d points", nc, np);
01564     /* The error code here is set to SINGULAR_MATRIX, in order to allow the caller
01565        to detect when too many coefficients are fitted to too few points */
01566 
01567     /* Need an extra point to calculate reduced chi^2 */
01568     assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT, 
01569        "%d coefficients. %d points. Cannot calculate chi square", nc, np);
01570     
01571     degx_tab = cpl_malloc(nc * sizeof(int)) ;
01572     assure_mem( degx_tab );
01573 
01574     degy_tab = cpl_malloc(nc * sizeof(int)) ;
01575     if (degy_tab == NULL) {
01576     cpl_free(degx_tab);
01577     assure_mem( false );
01578     }
01579 
01580     {
01581         int i=0 ;
01582         for (degy=0 ; degy<=poly_deg2 ; degy++) {     /* rectangular matrix */
01583             for (degx=0 ; degx<=poly_deg1 ; degx++) {
01584                 degx_tab[i] = degx ;
01585                 degy_tab[i] = degy ;
01586                 i++ ;
01587             }
01588         }
01589     }
01590     
01591     /* Initialize matrices */
01592     /* ma contains the polynomial terms in the order described */
01593     /* above in each column, for each input point. */
01594     /* mb contains the values */
01595     ma = cpl_matrix_new(np, nc) ;
01596     mb = cpl_matrix_new(np, 1) ;
01597 
01598     /* Get the mean of each variable */
01599     xy_pos_x = cpl_bivector_get_x_const(xy_pos);
01600     xy_pos_y = cpl_bivector_get_y_const(xy_pos);
01601 
01602     mean_x = cpl_vector_get_mean(xy_pos_x);
01603     mean_y = cpl_vector_get_mean(xy_pos_y);
01604     mean_z = cpl_vector_get_mean(values);
01605 
01606     /* Fill up matrices. At the same time shift the data
01607        so that it is centered around zero */
01608     xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x) ;
01609     xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y) ;
01610     values_data   = cpl_vector_get_data_const(values) ;
01611     if (sigmas != NULL)
01612     {
01613         sigmas_data = cpl_vector_get_data_const(sigmas) ;
01614     }
01615 
01616     if (sigmas != NULL)
01617     {
01618             int i;
01619         for (i=0 ; i<np ; i++) {
01620                 double *ma_data = cpl_matrix_get_data(ma);
01621                 double *mb_data = cpl_matrix_get_data(mb);
01622 
01623                 int j = 0;
01624                 double valy = 1;
01625 
01626         /* Catch division by zero */
01627         if (sigmas_data[i] == 0)
01628             {
01629             uves_free_matrix(&ma) ;
01630             uves_free_matrix(&mb) ;
01631             cpl_free(degx_tab) ;
01632             cpl_free(degy_tab) ;
01633             assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01634                                "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
01635             }
01636 
01637                 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01638                     double valx = 1; 
01639                     for (degx=0 ; degx<=poly_deg1 ; degx++) {
01640                         ma_data[j + i*nc] = valx * valy / sigmas_data[i];
01641                         valx *= (xy_pos_data_x[i] - mean_x);
01642                         j++;
01643                     }
01644                     valy *= (xy_pos_data_y[i] - mean_y);
01645                 }
01646 
01647         /* mb contains surface values (z-axis) */
01648 
01649         mb_data[0 + i*1] = (values_data[i] - mean_z) / sigmas_data[i];
01650         }
01651     }
01652     else  /* Use sigma = 1 */
01653     {
01654             int i;
01655         for (i=0 ; i<np ; i++) {
01656                 double *ma_data = cpl_matrix_get_data(ma);
01657                 double *mb_data = cpl_matrix_get_data(mb);
01658 
01659                 double valy = 1;
01660                 int j = 0;
01661                 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01662                     double valx = 1; 
01663                     for (degx=0 ; degx<=poly_deg1 ; degx++) {
01664                         ma_data[j + i*nc] = valx * valy / 1;
01665                         valx *= (xy_pos_data_x[i] - mean_x);
01666                         j++;
01667                     }
01668                     valy *= (xy_pos_data_y[i] - mean_y);
01669                 }
01670 
01671         /* mb contains surface values (z-axis) */
01672 //        cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01673         mb_data[0 + i*1] = (values_data[i] - mean_z) / 1;
01674         }
01675     }
01676     
01677     /* If variance polynomial is requested, 
01678        compute covariance matrix = (A^T * A)^-1 */
01679     if (variance != NULL)
01680     {
01681         mat    = cpl_matrix_transpose_create(ma);
01682         if (mat != NULL)
01683         {
01684             mat_ma = cpl_matrix_product_create(mat, ma);
01685             if (mat_ma != NULL)
01686             {
01687                 cov          = cpl_matrix_invert_create(mat_ma);
01688                 /* Here, one might do a (paranoia) check that the covariance
01689                    matrix is symmetrical and has positive eigenvalues (so that
01690                    the returned variance polynomial is guaranteed to be positive) */
01691 
01692                 variance_cpl = cpl_polynomial_new(2);
01693             }
01694         }
01695         uves_free_matrix(&mat);
01696         uves_free_matrix(&mat_ma);
01697     }
01698 
01699     /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
01700     mx = cpl_matrix_solve_normal(ma, mb) ;
01701 
01702     uves_free_matrix(&ma) ;
01703     uves_free_matrix(&mb) ;
01704     if (mx == NULL) {
01705         cpl_free(degx_tab) ;
01706         cpl_free(degy_tab) ;
01707     uves_free_matrix(&cov) ;
01708         assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed") ;
01709     }
01710 
01711     /* Store coefficients for output */
01712     out = cpl_polynomial_new(2) ;
01713     powers = cpl_malloc(2 * sizeof(cpl_size)) ;
01714     if (powers == NULL) {
01715         cpl_free(degx_tab) ;
01716         cpl_free(degy_tab) ;
01717     uves_free_matrix(&mx) ;
01718     uves_free_matrix(&cov) ;
01719     uves_free_polynomial(&out) ;
01720     assure_mem( false );
01721     }
01722 
01723     {
01724         int i;
01725     for (i = 0 ; i < nc ; i++)
01726     {
01727         powers[0] = degx_tab[i] ;
01728         powers[1] = degy_tab[i] ;
01729         cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0)) ;
01730         
01731         /* Create variance polynomial (if requested) */
01732         if (variance != NULL &&                   /* Requested? */
01733         cov != NULL && variance_cpl != NULL   /* covariance computation succeeded? */
01734         )
01735         {
01736                     int j;
01737             for (j = 0; j < nc; j++)
01738             {
01739                 double coeff;
01740                 /* Add cov_ij to the proper coeff:
01741                    cov_ij * dp/d(ai) * dp/d(aj) =
01742                    cov_ij * (x^degx[i] * y^degy[i]) * (x^degx[i] * y^degy[i]) =
01743                    cov_ij * x^(degx[i]+degx[j]) * y^(degy[i] + degy[j]),
01744                    
01745                    i.e. add cov_ij to coeff (degx[i]+degx[j], degy[i]+degy[j]) */
01746                 powers[0] = degx_tab[i] + degx_tab[j] ;
01747                 powers[1] = degy_tab[i] + degy_tab[j] ;
01748                 
01749                 coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
01750                 cpl_polynomial_set_coeff(variance_cpl, powers, 
01751                              coeff + cpl_matrix_get(cov, i, j)) ;
01752             }
01753         }
01754     }
01755     }
01756     
01757     cpl_free(powers) ;
01758     cpl_free(degx_tab) ;
01759     cpl_free(degy_tab) ;
01760     uves_free_matrix(&cov) ;
01761     uves_free_matrix(&mx) ;
01762     
01763     /* Create and shift result */
01764     result = uves_polynomial_new(out);
01765     uves_free_polynomial(&out);
01766     uves_polynomial_shift(result, 0, mean_z);
01767     uves_polynomial_shift(result, 1, mean_x);
01768     uves_polynomial_shift(result, 2, mean_y);
01769 
01770     /* Wrap up variance polynomial */
01771     if (variance != NULL)
01772     {
01773         *variance = uves_polynomial_new(variance_cpl);
01774         uves_free_polynomial(&variance_cpl);
01775             /* The variance of the fit does not change
01776            when a constant is added to the a_00
01777            coefficient of the polynomial, so don't:
01778            uves_polynomial_shift(*variance, 0, mean_z); */
01779         uves_polynomial_shift(*variance, 1, mean_x);
01780         uves_polynomial_shift(*variance, 2, mean_y);
01781 
01782         /* Maybe here add a consistency check that the variance polynomial is 
01783            positive at all input points */
01784     }  
01785 
01786     /* If requested, compute mean squared error */
01787     if (mse != NULL || red_chisq != NULL) 
01788     {
01789             int i;
01790 
01791         if (mse       != NULL) *mse = 0.00 ;
01792         if (red_chisq != NULL) *red_chisq = 0.00 ;
01793         for (i = 0 ; i < np ; i++) 
01794         {
01795             double regress = uves_polynomial_evaluate_2d(result, 
01796                                  xy_pos_data_x[i],
01797                                  xy_pos_data_y[i]);
01798             /* Subtract from the true value, square, accumulate */
01799             if (mse != NULL)
01800             {
01801                 double residual = values_data[i] - regress;
01802                 *mse += residual*residual;
01803             }
01804             if (red_chisq != NULL)
01805             {
01806                 *red_chisq += uves_pow_int((values_data[i] - regress) /
01807                                sigmas_data[i], 2);
01808             }
01809         }
01810         /* Get average */
01811         if (mse       != NULL)  *mse       /= (double) np ;
01812         
01813         if (red_chisq != NULL)
01814         {
01815             passure( np > nc, "%d %d", np, nc); /* Was already checked */
01816             *red_chisq /= (double) (np - nc) ;
01817         }
01818     }
01819 
01820   cleanup:
01821     return result ;
01822 }
01823 
01824 

Generated on 9 Mar 2012 for UVES Pipeline Reference Manual by  doxygen 1.6.1