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: 2007/09/11 17:08:49 $
00023  * $Revision: 1.64 $
00024  * $Name: uves-3_9_0 $
00025  * $Log: uves_utils_polynomial.c,v $
00026  * Revision 1.64  2007/09/11 17:08:49  amodigli
00027  * mooved uves_polynomial_convert_from_plist_midas to uves_dfs
00028  *
00029  * Revision 1.63  2007/08/21 13:08:26  jmlarsen
00030  * Removed irplib_access module, largely deprecated by CPL-4
00031  *
00032  * Revision 1.62  2007/06/20 15:34:50  jmlarsen
00033  * Changed indentation
00034  *
00035  * Revision 1.61  2007/06/20 08:30:00  amodigli
00036  * added index parameter to support FIBER mode lintab in uves_polynomial_convert_from_plist_midas
00037  *
00038  * Revision 1.60  2007/06/06 08:17:33  amodigli
00039  * replace tab with 4 spaces
00040  *
00041  * Revision 1.59  2007/05/03 15:23:08  jmlarsen
00042  * Removed dead code
00043  *
00044  * Revision 1.58  2007/05/03 15:18:29  jmlarsen
00045  * Added function to add polynomials
00046  *
00047  * Revision 1.57  2007/04/27 07:21:51  jmlarsen
00048  * Polyfit: Changed error code from ILLEGAL_INPUT to SINGULAR_MATRIX
00049  *
00050  * Revision 1.56  2007/04/24 12:50:29  jmlarsen
00051  * Replaced cpl_propertylist -> uves_propertylist which is much faster
00052  *
00053  * Revision 1.55  2007/03/23 08:01:55  jmlarsen
00054  * Fixed mixed code and declarations
00055  *
00056  * Revision 1.54  2007/03/19 15:10:03  jmlarsen
00057  * Optimization in 2d fitting: do not call pow too often
00058  *
00059  * Revision 1.53  2007/03/13 15:35:11  jmlarsen
00060  * Made a few time optimizations
00061  *
00062  * Revision 1.52  2007/03/05 10:20:49  jmlarsen
00063  * Added uves_polynomial_delete_const()
00064  *
00065  * Revision 1.51  2007/01/15 08:48:51  jmlarsen
00066  * Shortened lines
00067  *
00068  * Revision 1.50  2006/11/24 09:36:49  jmlarsen
00069  * Workaround for slow uves_propertylist_get_size
00070  *
00071  * Revision 1.49  2006/11/15 15:02:15  jmlarsen
00072  * Implemented const safe workarounds for CPL functions
00073  *
00074  * Revision 1.47  2006/11/15 14:04:08  jmlarsen
00075  * Removed non-const version of parameterlist_get_first/last/next which is
00076  * already in CPL, added const-safe wrapper, unwrapper and deallocator functions
00077  *
00078  * Revision 1.46  2006/11/13 14:23:55  jmlarsen
00079  * Removed workarounds for CPL const bugs
00080  *
00081  * Revision 1.45  2006/11/06 15:19:42  jmlarsen
00082  * Removed unused include directives
00083  *
00084  * Revision 1.44  2006/09/08 14:06:29  jmlarsen
00085  * Removed profiling code
00086  *
00087  * Revision 1.43  2006/09/06 14:46:21  jmlarsen
00088  * Added missing newline in uves_polynomial_dump()
00089  *
00090  * Revision 1.42  2006/08/17 14:11:25  jmlarsen
00091  * Use assure_mem macro to check for memory allocation failure
00092  *
00093  * Revision 1.41  2006/08/17 13:56:53  jmlarsen
00094  * Reduced max line length
00095  *
00096  * Revision 1.40  2006/07/03 13:27:52  jmlarsen
00097  * Moved failing assertion to where it should be
00098  *
00099  * Revision 1.39  2006/06/01 14:43:17  jmlarsen
00100  * Added missing documentation
00101  *
00102  * Revision 1.38  2006/05/05 13:59:03  jmlarsen
00103  * Support fitting zero-degree polynomial
00104  *
00105  * Revision 1.37  2006/04/24 09:28:29  jmlarsen
00106  * Added function to create zero-polynomial
00107  *
00108  * Revision 1.36  2006/03/27 09:41:37  jmlarsen
00109  * Added timing markers
00110  *
00111  * Revision 1.35  2006/03/09 10:52:25  jmlarsen
00112  * Renamed pow->power
00113  *
00114  * Revision 1.34  2006/03/03 13:54:11  jmlarsen
00115  * Changed syntax of check macro
00116  *
00117  * Revision 1.33  2005/12/19 16:17:56  jmlarsen
00118  * Replaced bool -> int
00119  *
00120  */
00121 
00122 #ifdef HAVE_CONFIG_H
00123 #  include <config.h>
00124 #endif
00125 
00126 /*----------------------------------------------------------------------------*/
00146 /*----------------------------------------------------------------------------*/
00147 
00148 /*-----------------------------------------------------------------------------
00149                                 Defines
00150  -----------------------------------------------------------------------------*/
00151 
00152 /*
00153  *  When storing a 2d polynomial in a table,
00154  *  these column names are used
00155  */
00156 #define COLUMN_ORDER1 "Order1"
00157 #define COLUMN_ORDER2 "Order2"
00158 #define COLUMN_COEFF  "Coeff"
00159 
00162 /*-----------------------------------------------------------------------------
00163                                 Includes
00164  -----------------------------------------------------------------------------*/
00165 #include <uves_utils_polynomial.h>
00166 
00167 #include <uves_utils.h>
00168 #include <uves_utils_wrappers.h>
00169 #include <uves_dump.h>
00170 #include <uves_msg.h>
00171 #include <uves_error.h>
00172 
00173 #include <cpl.h>
00174 
00175 /*-----------------------------------------------------------------------------
00176                             Typedefs
00177  -----------------------------------------------------------------------------*/
00180 struct _polynomial 
00181 {
00183     cpl_polynomial *pol; 
00184 
00186     cpl_vector *vec;
00187     double *vec_data;
00188 
00189     int dimension;  /* for efficiency */
00190 
00192     double *shift;
00193 
00195     double *scale;
00196 };
00197 
00198 /*-----------------------------------------------------------------------------
00199                             Implementation
00200  -----------------------------------------------------------------------------*/
00201 /*----------------------------------------------------------------------------*/
00212 /*----------------------------------------------------------------------------*/
00213 polynomial *
00214 uves_polynomial_new(const cpl_polynomial *pol)
00215 {
00216     polynomial *p = NULL;
00217     int i;
00218     
00219     /* Test input */
00220     assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00221 
00222     /* Allocate and initialize struct */
00223     p = cpl_calloc(1, sizeof(polynomial)) ;
00224     assure_mem( p );
00225 
00226     check( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");
00227 
00228     /* Allocate vector */
00229     p->vec = cpl_vector_new(p->dimension);
00230     assure_mem( p->vec );
00231     p->vec_data = cpl_vector_get_data(p->vec);
00232 
00233     /* Shifts are initialized to zero, scales to 1 */
00234     p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
00235     assure_mem( p->shift );
00236 
00237     p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
00238     assure_mem( p->scale );
00239     for (i = 0; i <= p->dimension; i++)
00240     p->scale[i] = 1.0;
00241 
00242     check( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
00243     
00244   cleanup:
00245     if (cpl_error_get_code() != CPL_ERROR_NONE)
00246     uves_polynomial_delete(&p);
00247     
00248     return p;
00249 }
00250 
00251 /*----------------------------------------------------------------------------*/
00259 /*----------------------------------------------------------------------------*/
00260 polynomial *
00261 uves_polynomial_new_zero(int dim)
00262 {
00263     polynomial *result = NULL;
00264     cpl_polynomial *p = NULL;
00265 
00266     assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);
00267 
00268     p = cpl_polynomial_new(dim);
00269     assure_mem( p );
00270 
00271     result = uves_polynomial_new(p);
00272     assure_mem( result );
00273 
00274   cleanup:
00275     uves_free_polynomial(&p);
00276 
00277     return result;
00278 }
00279 
00280 /*----------------------------------------------------------------------------*/
00287 /*----------------------------------------------------------------------------*/
00288 void 
00289 uves_polynomial_delete(polynomial **p)
00290 {
00291     uves_polynomial_delete_const((const polynomial **)p);
00292 }
00293 
00294 /*----------------------------------------------------------------------------*/
00301 /*----------------------------------------------------------------------------*/
00302 void 
00303 uves_polynomial_delete_const(const polynomial **p)
00304 {
00305     if (*p == NULL) return;
00306     cpl_polynomial_delete((*p)->pol);
00307     cpl_vector_delete((*p)->vec);
00308     cpl_free((*p)->shift);
00309     cpl_free((*p)->scale);
00310     uves_free(*p);
00311     *p = NULL;
00312     return;
00313 }
00314 /*----------------------------------------------------------------------------*/
00320 /*----------------------------------------------------------------------------*/
00321 int
00322 uves_polynomial_get_degree(const polynomial *p)
00323 {
00324     int result = -1;
00325     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00326     
00327     result = cpl_polynomial_get_degree(p->pol);
00328 
00329   cleanup:
00330     return result;
00331 }
00332 
00333 /*----------------------------------------------------------------------------*/
00339 /*----------------------------------------------------------------------------*/
00340 polynomial *
00341 uves_polynomial_duplicate(const polynomial *p)
00342 {
00343     polynomial *result = NULL;
00344     int dimension;
00345     int i;
00346 
00347     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00348     dimension = uves_polynomial_get_dimension(p);
00349 
00350     check( result = uves_polynomial_new(p->pol),
00351        "Error allocating polynomial");
00352     
00353     for (i = 0; i <= dimension; i++)
00354     {
00355         result->shift[i] = p->shift[i];
00356         result->scale[i] = p->scale[i];
00357     }
00358 
00359   cleanup:
00360     if (cpl_error_get_code() != CPL_ERROR_NONE)
00361     {
00362         uves_polynomial_delete(&result);
00363         return NULL;
00364     }
00365     
00366     return result;
00367 }
00368 
00369 
00370 /*----------------------------------------------------------------------------*/
00381 /*----------------------------------------------------------------------------*/
00382 cpl_table *
00383 uves_polynomial_convert_to_table(const polynomial *p)
00384 {
00385     cpl_table *t = NULL; /* Result */
00386     int degree;
00387     int i, j, row;
00388 
00389     /* Check input */
00390     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00391     assure( uves_polynomial_get_dimension(p) == 2, 
00392         CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
00393     
00394     degree = cpl_polynomial_get_degree(p->pol);
00395 
00396     /* Allocate space for 3 shifts, 3 scale factors and all
00397        coefficients */
00398     t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
00399     cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
00400     cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
00401     cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);
00402 
00403     row = 0;
00404 
00405     /* First write the shifts, write non-garbage to coeff columns (which are not used) */
00406     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00407     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00408     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;
00409 
00410     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00411     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00412     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;
00413 
00414     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00415     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00416     cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;
00417 
00418     /* Then the scale factors */
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->scale[0]); 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->scale[1]); row++;
00426 
00427     cpl_table_set_int   (t, COLUMN_ORDER1, row, -1);
00428     cpl_table_set_int   (t, COLUMN_ORDER2, row, -1);
00429     cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;
00430 
00431     /* And then write the coefficients */
00432     for (i = 0; i <= degree; i++){
00433     for (j = 0; j+i <= degree; j++){
00434         double coeff;
00435         int power[2];
00436         power[0] = i;
00437         power[1] = j;
00438         
00439         coeff = cpl_polynomial_get_coeff(p->pol, power);
00440         cpl_table_set_int   (t, COLUMN_ORDER1, row, power[0]);
00441         cpl_table_set_int   (t, COLUMN_ORDER2, row, power[1]);
00442         cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
00443         
00444         row++;
00445     }
00446     }
00447 
00448   cleanup:
00449     return t;
00450 }
00451 
00452 /*----------------------------------------------------------------------------*/
00461 /*----------------------------------------------------------------------------*/
00462 polynomial *
00463 uves_polynomial_convert_from_table(cpl_table *t)
00464 {
00465     polynomial *p = NULL;  /* Result */
00466     cpl_polynomial *pol = NULL;
00467     cpl_type type;
00468     int i;
00469     
00470     /* Only 2d supported */
00471     check( pol = cpl_polynomial_new(2), "Error initializing polynomial");
00472 
00473     /* Check table format */
00474     assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
00475     assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT, 
00476        "No '%s' column found in table", COLUMN_ORDER1);
00477     assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
00478        "No '%s' column found in table", COLUMN_ORDER2);
00479     assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
00480        "No '%s' column found in table", COLUMN_COEFF );
00481     
00482     type = cpl_table_get_column_type(t, COLUMN_ORDER1);
00483     assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
00484        "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
00485        uves_tostring_cpl_type(type));
00486     
00487     type = cpl_table_get_column_type(t, COLUMN_ORDER2);
00488     assure(type == CPL_TYPE_INT   , CPL_ERROR_INVALID_TYPE,
00489        "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
00490        uves_tostring_cpl_type(type));
00491     
00492     type = cpl_table_get_column_type(t, COLUMN_COEFF);
00493     assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
00494        "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
00495        uves_tostring_cpl_type(type));
00496 
00497     assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
00498        "Table must contain at least one coefficient");
00499     
00500     /* Read the coefficients */
00501     for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
00502     double coeff;
00503     int power[2];
00504     
00505     check(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
00506         power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
00507         coeff  = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
00508            "Error reading table row %d", i);
00509     
00510     uves_msg_debug("Pol.coeff.(%d, %d) = %e", power[0], power[1], coeff);
00511 
00512     check( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
00513     }
00514     p = uves_polynomial_new(pol);
00515 
00516     /* Read shifts and rescaling */
00517     uves_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
00518     uves_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
00519     uves_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
00520     uves_polynomial_shift  (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
00521     uves_polynomial_shift  (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
00522     uves_polynomial_shift  (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
00523 
00524   cleanup:
00525     uves_free_polynomial(&pol);
00526     if (cpl_error_get_code() != CPL_ERROR_NONE)
00527     uves_polynomial_delete(&p);
00528 
00529     return p;
00530 }
00531 
00532 
00533 /*----------------------------------------------------------------------------*/
00539 /*----------------------------------------------------------------------------*/
00540 int
00541 uves_polynomial_get_dimension(const polynomial *p)
00542 {
00543     int dim = -1;
00544     assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00545 
00546 /* slow     check( dim = cpl_polynomial_get_dimension(p->pol), "Error reading dimension"); */
00547     dim = p->dimension;
00548     
00549   cleanup:
00550     return dim;
00551 }
00552 
00553 /*----------------------------------------------------------------------------*/
00561 /*----------------------------------------------------------------------------*/
00562 void uves_polynomial_dump(const polynomial *p, FILE *stream)
00563 {
00564     if (p == NULL)
00565     fprintf(stream, "Null polynomial\n");
00566     else {
00567     int i;
00568     cpl_polynomial_dump(p->pol, stream);
00569     fprintf(stream, "shift_y \t= %f  \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
00570     for (i = 1; i <= uves_polynomial_get_dimension(p); i++)
00571         {
00572         fprintf(stream, "shift_x%d \t= %f  \tscale_x%d \t= %f\n", 
00573             i, p->shift[i], i, p->scale[i]);
00574         }
00575     }
00576     return;
00577 }
00578 
00579 /*----------------------------------------------------------------------------*/
00593 /*----------------------------------------------------------------------------*/
00594 cpl_error_code
00595 uves_polynomial_rescale(polynomial *p, int varno, double scale)
00596 {
00597     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00598     assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
00599        CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00600 
00601     /*  Rescaling an x variable by the factor S corresponds to:  
00602      *    p'(x) := p(x/S)  =
00603      *  cpl( (x/S -  shiftx ) /    scalex  ) * scaley + shifty  = 
00604      *  cpl( (x - (S*shiftx)) / (S*scalex) ) * scaley + shifty      */
00605 
00606     /*  Rescaling the y variable by the factor S corresponds to:  
00607      *    p'(x) := S*p(x)  =
00608      *  S * ( cpl((x - shiftx)/scalex) * scaley     + shifty )  = 
00609      *        cpl((x - shiftx)/scalex) * (S*scaley) + (S*shifty) 
00610      *
00611      *  therefore the implementation is the same in the two cases. */
00612      
00613     p->shift[varno] *= scale;
00614     p->scale[varno] *= scale;
00615 
00616   cleanup:
00617     return cpl_error_get_code();
00618 }
00619 
00620 /*----------------------------------------------------------------------------*/
00634 /*----------------------------------------------------------------------------*/
00635 cpl_error_code
00636 uves_polynomial_shift(polynomial *p, int varno, double shift)
00637 {
00638     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00639     assure(0 <= varno && varno <= uves_polynomial_get_dimension(p), 
00640        CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00641 
00642     /* The implementation is similar for x and y variables because
00643      *  p(x-S)  =
00644      *  cpl( (x-S - shiftx)   / scalex ) * scaley + shifty  = 
00645      *  cpl( (x - (shiftx+S)) / scalex ) * scaley + shifty
00646      * and
00647      *  p(x) + S  =
00648      *  cpl( (x - shiftx)/scalex ) * scaley + shifty + S  = 
00649      *  cpl( (x - shiftx)/scalex ) * scaley + (shifty+S)      */
00650 
00651     p->shift[varno] += shift;
00652 
00653   cleanup:
00654     return cpl_error_get_code();
00655 }
00656 
00657 /*----------------------------------------------------------------------------*/
00666 /*----------------------------------------------------------------------------*/
00667 double
00668 uves_polynomial_evaluate_1d(const polynomial *p, double x)
00669 {
00670     double result = 0;
00671     
00672     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00673     assure(uves_polynomial_get_dimension(p) == 1, 
00674        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00675     
00676     check( result = 
00677        cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
00678        * p->scale[0] + p->shift[0],
00679        "Could not evaluate polynomial");
00680     
00681   cleanup:
00682     return result;
00683 }
00684 
00685 
00686 /*----------------------------------------------------------------------------*/
00696 /*----------------------------------------------------------------------------*/
00697 
00698 double
00699 uves_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
00700 {
00701     double result = 0;
00702 
00703     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00704     assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
00705        "Polynomial must be 2d. It's %dd", p->dimension);
00706     {
00707         double scale = p->scale[0];
00708         double shift = p->shift[0];
00709 
00710         //    cpl_vector_set(p->vec, 0, (x1 - p->shift[1]) / p->scale[1]);
00711         //    cpl_vector_set(p->vec, 1, (x2 - p->shift[2]) / p->scale[2]);
00712         p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
00713         p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
00714         
00715         result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
00716     }
00717 
00718   cleanup:
00719     return result;
00720 }
00721 
00722 /*----------------------------------------------------------------------------*/
00735 /*----------------------------------------------------------------------------*/
00736 double
00737 uves_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
00738 {
00739     double result = 0;
00740     int power[1];
00741     double coeff0;
00742 
00743     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00744     assure(uves_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT, 
00745        "Polynomial must be 1d");
00746     
00747     /* Solving p(x) = value corresponds to solving
00748        <=>    cpl_p( (x-xshift)/xscale )*yscale + yshift = value
00749        <=>    cpl_p( (x-xshift)/xscale ) + (yshift - value)/yscale = 0 
00750 
00751        So   1) find zero point for the polynomial   cpl() + (yshift-value)/yscale
00752        Then 2) shift and rescale the result
00753     */
00754 
00755     power[0] = 0;
00756     check(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
00757         cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
00758       "Error setting coefficient");
00759 
00760     check( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
00761                    &result, multiplicity), "Could not find root");
00762     /* Restore polynomial */
00763     cpl_polynomial_set_coeff(p->pol, power, coeff0);
00764     
00765     /* Shift solution */
00766     result = result * p->scale[1] + p->shift[1];
00767 
00768   cleanup:
00769     return result;
00770 }
00771 
00772 /*----------------------------------------------------------------------------*/
00789 /*----------------------------------------------------------------------------*/
00790 double
00791 uves_polynomial_solve_2d(const polynomial *p, double value, double guess,
00792              int multiplicity, int varno, double x_value)
00793 {
00794     double result = 0;
00795     polynomial *pol_1d = NULL;
00796 
00797     assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00798         "Illegal variable number: %d", varno);
00799 
00800     check( pol_1d = uves_polynomial_collapse(p, varno, x_value),
00801        "Could not collapse polynomial");
00802 
00803     check( result = uves_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
00804        "Could not find root");
00805 
00806   cleanup:
00807     uves_polynomial_delete(&pol_1d);
00808     return result;
00809 }
00810 
00811 /*----------------------------------------------------------------------------*/
00820 /*----------------------------------------------------------------------------*/
00821 double
00822 uves_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
00823 {
00824     double result = 0;
00825     int power[2];
00826 
00827     assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00828         "Illegal variable number (%d)", varno);
00829 
00830     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00831     assure(uves_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
00832        "Polynomial must be 2d. It's %dd", uves_polynomial_get_dimension(p));
00833 
00834     /*  d/dx_i [ p(x) ] =
00835      *  d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
00836      *  [ d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley ]
00837      */
00838 
00839     /* Shift, scale  (x1, x2) */
00840     x1 = (x1 - p->shift[1])/p->scale[1];
00841     x2 = (x2 - p->shift[2])/p->scale[2];
00842  
00843     /* Get derivative of cpl polynomial.
00844      * 
00845      */
00846     {
00847     int degree = cpl_polynomial_get_degree(p->pol);
00848     double yj = 1;  /* y^j */
00849     int i, j;
00850     
00851     result = 0;
00852     for (j = 0, yj = 1;
00853          j <= degree; j++,
00854          yj *= (varno == 1) ? x2 : x1)
00855         {
00856         /*  Proof by example (degree = 3): For each j account for these terms
00857          *  using Horner's rule:
00858          *
00859          * d/dx     y^j * [  c_3j x^3 +  c_2j x^2 +  c_1j x^1 + c_0j ]   =
00860          *
00861          *          y^j * [ 3c_3j x^2 + 2c_2j x^1 + 1c_1j ]     =
00862          *
00863          *          y^j * [ ((3c_3j) x + 2c_2j) x + 1c_1j ]
00864          */
00865 
00866         double sum = 0;
00867         for (i = degree; i >= 1; i--)
00868             {
00869             double c_ij;
00870 
00871             power[0] = (varno == 1) ? i : j;
00872             power[1] = (varno == 1) ? j : i;
00873             
00874             c_ij = cpl_polynomial_get_coeff(p->pol, power);
00875             
00876             sum += (i * c_ij);
00877             if (i >= 2) sum *= (varno == 1) ? x1 : x2;
00878             }
00879 
00880         /* Collect terms */
00881         result += yj * sum;
00882         }
00883     }
00884 
00885     result *= p->scale[0];
00886 
00887 
00888 /* Old code: This method (valid for varno = 2)
00889    of getting the derivative of
00890    the CPL polynomial is slow because of the call to 
00891    uves_polynomial_collapse()
00892 
00893    check( pol_1d = uves_polynomial_collapse(p, 1, x1);
00894    dummy = cpl_polynomial_eval_1d(pol_1d->pol, (x2 - pol_1d->shift[1])/pol_1d->scale[1], &result),
00895    "Error evaluating derivative");
00896 */
00897     
00898   cleanup:
00899     return result;
00900 }
00901 
00902 /*----------------------------------------------------------------------------*/
00909 /*----------------------------------------------------------------------------*/
00910 double
00911 uves_polynomial_derivative_1d(const polynomial *p, double x)
00912 {
00913     double result = 0;
00914     double dummy;
00915 
00916     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00917     assure(uves_polynomial_get_dimension(p) == 1, 
00918        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00919     
00920     check( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
00921        "Error evaluating derivative");
00922     
00923   cleanup:
00924     return result;
00925 }
00926 
00927 /*----------------------------------------------------------------------------*/
00934 /*----------------------------------------------------------------------------*/
00935 polynomial *
00936 uves_polynomial_add_2d(const polynomial *p1, const polynomial *p2)
00937 {
00938     polynomial *result = NULL;
00939     cpl_polynomial *pol = NULL;
00940 
00941     assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00942     assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00943     assure(uves_polynomial_get_dimension(p1) == 2, 
00944        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00945     assure(uves_polynomial_get_dimension(p2) == 2, 
00946        CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00947 
00948     /* cpl_polynomial1((x - shift_x1)/scale_x1) * scale_y1 + shift_y1
00949        +
00950        cpl_polynomial2((x - shift_x2)/scale_x2) * scale_y2 + shift_y2
00951        = ???
00952        Not easy.
00953 
00954        Use brute force:
00955     */
00956     
00957     {
00958         int degree, i, j;
00959 
00960         degree = uves_max_int(uves_polynomial_get_degree(p1),
00961                               uves_polynomial_get_degree(p2));
00962         
00963         pol = cpl_polynomial_new(2);
00964         for (i = 0; i <= degree; i++)
00965             for (j = 0; j <= degree; j++) {
00966                 double coeff1, coeff2;
00967                 int power[2];
00968 
00969                 /* Simple: add coefficients of the same power */
00970                 coeff1 = uves_polynomial_get_coeff_2d(p1, i, j);
00971                 coeff2 = uves_polynomial_get_coeff_2d(p2, i, j);
00972                 
00973                 power[0] = i;
00974                 power[1] = j;
00975                 cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
00976             }
00977     }
00978 
00979     result = uves_polynomial_new(pol);
00980    
00981   cleanup:
00982     uves_free_polynomial(&pol);
00983     return result;
00984 }
00985 
00986 /*----------------------------------------------------------------------------*/
00999 /*----------------------------------------------------------------------------*/
01000 static cpl_error_code
01001 derivative_cpl_polynomial(cpl_polynomial *p, int varno)
01002 {
01003     int dimension, degree;
01004     int i, j;
01005     int power[2];
01006     
01007     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01008     dimension = cpl_polynomial_get_dimension(p);
01009     degree = cpl_polynomial_get_degree(p);
01010     assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT, 
01011         "Illegal dimension: %d", dimension);
01012     assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01013         "Illegal variable number: %d", varno);
01014     
01015     if (dimension == 1)
01016     {
01017         /*  a_i := (i+1) * a_(i+1) */
01018         for(i = 0; i <= degree; i++)
01019         {
01020             double coeff;
01021             power[0] = i+1;
01022             /* power[1] is ignored */
01023             
01024             coeff = cpl_polynomial_get_coeff(p, power);
01025                 
01026             power[0] = i;            
01027             cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01028         }
01029     }
01030     
01031     if (dimension == 2)
01032     {
01033         /*  a_ij := (i+1) * a_{(i+1),j} */
01034         for(i = 0; i <= degree; i++)
01035         {
01036             for(j = 0; i + j <= degree; j++)
01037             {
01038                 double coeff;
01039                 power[varno - 1] = i+1;    /* varno == 1:    0,1  */ 
01040                 power[2 - varno] = j;      /* varno == 2:    1,0  */
01041                 
01042                 coeff = cpl_polynomial_get_coeff(p, power);
01043                 
01044                 power[varno - 1] = i;
01045                 
01046                 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01047             }
01048         }
01049     }
01050 
01051   cleanup:
01052     return cpl_error_get_code();
01053 }
01054 
01055 /*----------------------------------------------------------------------------*/
01065 /*----------------------------------------------------------------------------*/
01066 cpl_error_code
01067 uves_polynomial_derivative(polynomial *p, int varno)
01068 {
01069     int dimension;
01070     
01071     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01072     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01073     assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
01074         "Illegal variable number: %d", varno);
01075 
01076 
01077     /*   d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] = 
01078      *     sum_j d(cpl)/dx_j ( (x - shiftx) / scalex ) * scaley * dx_j/dx_i / scalex_j =
01079      *     d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley/scalex_i,
01080      * 
01081      * so transform :      shifty -> 0
01082      *                     shiftx -> shiftx
01083      *                     scaley -> scaley/scalex_i
01084      *                     scalex -> scalex
01085      *                       cpl  -> d(cpl)/dx_i
01086      */
01087 
01088     p->shift[0] = 0;
01089     p->scale[0] = p->scale[0] / p->scale[varno];
01090 
01091     check( derivative_cpl_polynomial(p->pol, varno),
01092        "Error calculating derivative of CPL-polynomial");
01093     
01094   cleanup:
01095     return cpl_error_get_code();
01096 }
01097 
01098 
01099 /*----------------------------------------------------------------------------*/
01108 /*----------------------------------------------------------------------------*/
01109 double
01110 uves_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
01111 {
01112     polynomial *pp = NULL;
01113     int dimension;
01114     double result = 0;
01115     double factorial;
01116     
01117     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01118     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01119     assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01120     assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
01121     assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);
01122 
01123     /* Calculate the coefficient as
01124      * d^N p / (dx1^degree1 dx2^degree2)  /  (degree1! * degree2!)
01125      * evaluated in (0,0)
01126     */
01127 
01128     pp = uves_polynomial_duplicate(p);
01129 
01130     factorial = 1;
01131     while(degree1 > 0)
01132     {
01133         check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
01134 
01135         factorial *= degree1;
01136         degree1 -= 1;
01137     }
01138 
01139     while(degree2 > 0)
01140     {
01141         check( uves_polynomial_derivative(pp, 2), "Error calculating derivative");
01142 
01143         factorial *= degree2;
01144         degree2 -= 1;
01145     }
01146     
01147     check( result = uves_polynomial_evaluate_2d(pp, 0, 0) / factorial,
01148        "Error evaluating polynomial");
01149     
01150   cleanup:
01151     uves_polynomial_delete(&pp);
01152     return result;
01153 }
01154 /*----------------------------------------------------------------------------*/
01164 /*----------------------------------------------------------------------------*/
01165 double
01166 uves_polynomial_get_coeff_1d(const polynomial *p, int degree)
01167 {
01168     polynomial *pp = NULL;
01169     int dimension;
01170     double result = 0;
01171     double factorial;
01172     
01173     assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01174     check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
01175     assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01176     assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
01177     
01178     /* Calculate the coefficient as
01179      *  d^degree p/dx^degree  /  (degree1! * degree2!)
01180      * evaluated in 0.
01181      */
01182     
01183     pp = uves_polynomial_duplicate(p);
01184     
01185     factorial = 1;
01186     while(degree > 0)
01187     {
01188         check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
01189         
01190         factorial *= degree;
01191         degree -= 1;
01192     }
01193     
01194     check( result = uves_polynomial_evaluate_1d(pp, 0) / factorial,
01195        "Error evaluating polynomial");
01196     
01197   cleanup:
01198     uves_polynomial_delete(&pp);
01199     return result;
01200 }
01201 
01202 
01203 /*----------------------------------------------------------------------------*/
01219 /*----------------------------------------------------------------------------*/
01220 polynomial *
01221 uves_polynomial_collapse(const polynomial *p, int varno, double value)
01222 {
01223     polynomial     *result  = NULL;
01224     cpl_polynomial *pol     = NULL;
01225     int            *power     = NULL;
01226 
01227     int i, j;
01228     int degree, dimension;
01229     
01230     assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01231     dimension = uves_polynomial_get_dimension(p);
01232     assure(dimension  > 0, CPL_ERROR_ILLEGAL_INPUT,
01233        "Polynomial has non-positive dimension: %d", dimension);
01234     assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
01235        "Don't collapse a 1d polynomial. Evaluate it!");
01236 
01237     /* To generalize this function to work with dimensions higher than 2,
01238        also changes needs to be made below (use varno properly). For now,
01239        support only 2d. */
01240     assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
01241     
01242     assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT, 
01243        "Wrong variable number");
01244     value = (value - p->shift[varno]) / p->scale[varno];
01245 
01246     /* Compute new coefficients */
01247     degree = cpl_polynomial_get_degree(p->pol);
01248     pol    = cpl_polynomial_new(dimension - 1);
01249     power = cpl_malloc(sizeof(int) * dimension);
01250     assure_mem( power );
01251     for (i = 0; i <= degree; i++) 
01252     {
01253         double coeff;
01254         
01255         power[2-varno] = i;   /* map 2->0  and 1->1 */
01256         
01257         /* Collect all terms with x^i  (using Horner's rule) */
01258         coeff = 0;
01259         for (j = degree - i; j >= 0; j--) 
01260         {
01261             power[varno-1] = j;  /* map 2->1 and 1->0 */
01262             coeff += cpl_polynomial_get_coeff(p->pol, power);
01263             if (j > 0) coeff *= value;
01264         }
01265         /* Write coefficient in 1d polynomial */
01266         power[0] = i;
01267         cpl_polynomial_set_coeff(pol, power, coeff);
01268     }
01269     
01270     /* Wrap the polynomial */
01271     result = uves_polynomial_new(pol);
01272 
01273     /* Copy the shifts and scales, skip variable number varno */
01274     j = 0;
01275     for(i = 0; i <= dimension - 1; i++) 
01276     {
01277         if (i == varno) 
01278         {
01279             /* Don't copy */
01280             j += 2;
01281             /* For the remainder of this for loop, j = i+1 */
01282         }
01283         else 
01284         {
01285             result->shift[i] = p->shift[j];
01286             result->scale[i] = p->scale[j];
01287             j += 1;
01288         }
01289     }
01290     
01291     assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(), 
01292        "Error collapsing polynomial");
01293     
01294   cleanup:
01295     cpl_free(power); power = NULL;
01296     uves_free_polynomial(&pol);
01297     if (cpl_error_get_code() != CPL_ERROR_NONE)
01298     {
01299         uves_polynomial_delete(&result);
01300     }
01301     return result;
01302 }
01303 
01304 
01305 
01306 /*----------------------------------------------------------------------------*/
01326 /*----------------------------------------------------------------------------*/
01327 polynomial * uves_polynomial_fit_1d(
01328     const cpl_vector    *   x_pos,
01329     const cpl_vector    *   values,
01330     const cpl_vector    *   sigmas,
01331     int                     poly_deg,
01332     double              *   mse)
01333 {
01334     int                 nc ;
01335     int                 np ;
01336     cpl_matrix      *   ma = NULL;
01337     cpl_matrix      *   mb = NULL;
01338     cpl_matrix      *   mx = NULL;
01339     const double    *   x_pos_data ;
01340     const double    *   values_data ;
01341     const double    *   sigmas_data = NULL;
01342     double              mean_x, mean_z;
01343     polynomial      *   result = NULL;
01344     cpl_polynomial  *   out ;
01345     cpl_vector      *   x_val = NULL;
01346     int                 i, j ;
01347     
01348     /* Check entries */
01349     assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
01350     assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT, 
01351         "Polynomial degree is %d. Must be non-negative", poly_deg);
01352     np = cpl_vector_get_size(x_pos) ;
01353     
01354     nc = 1 + poly_deg ;
01355     assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
01356         "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
01357         np, poly_deg, nc);
01358 
01359     /* Fill up look-up table for coefficients to compute */
01360     /* Initialize matrices */
01361     /* ma contains the polynomial terms for each input point. */
01362     /* mb contains the values */
01363     ma = cpl_matrix_new(np, nc) ;
01364     mb = cpl_matrix_new(np, 1) ;
01365 
01366     /* Get mean values */
01367     mean_x = cpl_vector_get_mean(x_pos);
01368     mean_z = cpl_vector_get_mean(values);
01369 
01370     /* Fill up matrices, shift */
01371     x_pos_data = cpl_vector_get_data_const(x_pos) ;
01372     values_data = cpl_vector_get_data_const(values) ;
01373     if (sigmas != NULL)
01374     {
01375         sigmas_data = cpl_vector_get_data_const(sigmas) ;
01376     }
01377 
01378     if (sigmas != NULL)
01379     {
01380         for (i=0 ; i<np ; i++) 
01381         {
01382             /* Catch division by zero */
01383             if (sigmas_data[i] == 0)
01384             {
01385                 uves_free_matrix(&ma) ;
01386                 uves_free_matrix(&mb) ;
01387                 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01388                    "Sigmas must be non-zero");
01389             }
01390             for (j=0 ; j<nc ; j++) 
01391             {
01392                 cpl_matrix_set(ma, i, j,  
01393                        uves_pow_int(x_pos_data[i] - mean_x, j) /
01394                        sigmas_data[i]) ;
01395             }
01396             /* mb contains surface values (z-axis) */
01397             cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
01398         }
01399     }
01400     else  /* Use sigma = 1 */
01401     {
01402         for (i=0 ; i<np ; i++) 
01403         {
01404             for (j=0 ; j<nc ; j++) 
01405             {
01406                 cpl_matrix_set(ma, i, j,  
01407                        uves_pow_int(x_pos_data[i] - mean_x, j) / 1);
01408             }
01409             /* mb contains surface values (z-values) */
01410             cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01411         }
01412     }
01413     
01414     /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
01415     check( mx = cpl_matrix_solve_normal(ma, mb),
01416        "Could not invert matrix");
01417     uves_free_matrix(&ma);
01418     uves_free_matrix(&mb);
01419 
01420     /* Store coefficients for output */
01421     out = cpl_polynomial_new(1) ;
01422 
01423     for (i=0 ; i<nc ; i++) {
01424         cpl_polynomial_set_coeff(out, &i, cpl_matrix_get(mx, i, 0)) ;
01425     }
01426     uves_free_matrix(&mx);
01427 
01428     /* If requested, compute mean squared error */
01429     if (mse != NULL) {
01430         *mse = 0.00 ;
01431         x_val = cpl_vector_new(1) ;
01432         for (i=0 ; i<np ; i++)
01433         {
01434         double residual;
01435         cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
01436         /* Subtract from the true value, square, accumulate */
01437         residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
01438         *mse += residual*residual;
01439         }
01440         uves_free_vector(&x_val) ;
01441         /* Average the error term */
01442         *mse /= (double)np ;
01443     }
01444 
01445     /* Create and shift result */
01446     result = uves_polynomial_new(out);
01447     uves_free_polynomial(&out);
01448 
01449     uves_polynomial_shift(result, 0, mean_z);
01450     uves_polynomial_shift(result, 1, mean_x);
01451 
01452   cleanup:
01453     uves_free_vector(&x_val);
01454     uves_free_matrix(&ma);
01455     uves_free_matrix(&mb);
01456     uves_free_matrix(&mx);
01457     return result;
01458 }
01459 
01460 
01461 /*----------------------------------------------------------------------------*/
01505 /*----------------------------------------------------------------------------*/
01506 polynomial *
01507 uves_polynomial_fit_2d(
01508     const cpl_bivector     *  xy_pos,
01509     const cpl_vector       *  values,
01510     const cpl_vector       *  sigmas,
01511     int                       poly_deg1,
01512     int                       poly_deg2,
01513     double                 *  mse,
01514     double                 *  red_chisq,
01515     polynomial             ** variance)
01516 {
01517     int                 nc ;
01518     int                 degx, degy ;
01519     int             *   degx_tab ;
01520     int             *   degy_tab ;
01521     int                 np ;
01522     cpl_matrix      *   ma ;
01523     cpl_matrix      *   mb ;
01524     cpl_matrix      *   mx ;
01525     cpl_matrix      *   mat;
01526     cpl_matrix      *   mat_ma;
01527     cpl_matrix      *   cov = NULL;
01528     const double    *   xy_pos_data_x ;
01529     const double    *   xy_pos_data_y ;
01530     const double    *   values_data ;
01531     const double    *   sigmas_data = NULL;
01532     const cpl_vector*   xy_pos_x;
01533     const cpl_vector*   xy_pos_y;
01534     double              mean_x, mean_y, mean_z;
01535     cpl_polynomial  *   out ;
01536     cpl_polynomial  *   variance_cpl ;
01537     polynomial      *   result         = NULL;
01538     int             *   powers ;
01539 
01540     /* Check entries */
01541     assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
01542     assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree1 is %d", poly_deg1);
01543     assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree2 is %d", poly_deg2);
01544     np = cpl_bivector_get_size(xy_pos) ;
01545 
01546     /* Can't calculate variance and chi_sq without sigmas */
01547     assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL, 
01548         CPL_ERROR_ILLEGAL_INPUT, 
01549         "Cannot calculate variance or chi_sq without knowing");
01550 
01551     /* Fill up look-up table for coefficients to compute */
01552     nc = (1 + poly_deg1)*(1 + poly_deg2) ;     /* rectangular matrix */
01553     
01554     assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX, "%d coefficients. Only %d points", nc, np);
01555     /* The error code here is set to SINGULAR_MATRIX, in order to allow the caller
01556        to detect when too many coefficients are fitted to too few points */
01557 
01558     /* Need an extra point to calculate reduced chi^2 */
01559     assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT, 
01560        "%d coefficients. %d points. Cannot calculate chi square", nc, np);
01561     
01562     degx_tab = cpl_malloc(nc * sizeof(int)) ;
01563     assure_mem( degx_tab );
01564 
01565     degy_tab = cpl_malloc(nc * sizeof(int)) ;
01566     if (degy_tab == NULL) {
01567     cpl_free(degx_tab);
01568     assure_mem( false );
01569     }
01570 
01571     {
01572         int i=0 ;
01573         for (degy=0 ; degy<=poly_deg2 ; degy++) {     /* rectangular matrix */
01574             for (degx=0 ; degx<=poly_deg1 ; degx++) {
01575                 degx_tab[i] = degx ;
01576                 degy_tab[i] = degy ;
01577                 i++ ;
01578             }
01579         }
01580     }
01581     
01582     /* Initialize matrices */
01583     /* ma contains the polynomial terms in the order described */
01584     /* above in each column, for each input point. */
01585     /* mb contains the values */
01586     ma = cpl_matrix_new(np, nc) ;
01587     mb = cpl_matrix_new(np, 1) ;
01588 
01589     /* Get the mean of each variable */
01590     xy_pos_x = cpl_bivector_get_x_const(xy_pos);
01591     xy_pos_y = cpl_bivector_get_y_const(xy_pos);
01592 
01593     mean_x = cpl_vector_get_mean(xy_pos_x);
01594     mean_y = cpl_vector_get_mean(xy_pos_y);
01595     mean_z = cpl_vector_get_mean(values);
01596 
01597     /* Fill up matrices. At the same time shift the data
01598        so that it is centered around zero */
01599     xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x) ;
01600     xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y) ;
01601     values_data   = cpl_vector_get_data_const(values) ;
01602     if (sigmas != NULL)
01603     {
01604         sigmas_data = cpl_vector_get_data_const(sigmas) ;
01605     }
01606 
01607     if (sigmas != NULL)
01608     {
01609             int i;
01610         for (i=0 ; i<np ; i++) {
01611                 double *ma_data = cpl_matrix_get_data(ma);
01612                 double *mb_data = cpl_matrix_get_data(mb);
01613 
01614                 int j = 0;
01615                 double valy = 1;
01616 
01617         /* Catch division by zero */
01618         if (sigmas_data[i] == 0)
01619             {
01620             uves_free_matrix(&ma) ;
01621             uves_free_matrix(&mb) ;
01622             cpl_free(degx_tab) ;
01623             cpl_free(degy_tab) ;
01624             assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01625                                "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
01626             }
01627 
01628                 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01629                     double valx = 1; 
01630                     for (degx=0 ; degx<=poly_deg1 ; degx++) {
01631                         ma_data[j + i*nc] = valx * valy / sigmas_data[i];
01632                         valx *= (xy_pos_data_x[i] - mean_x);
01633                         j++;
01634                     }
01635                     valy *= (xy_pos_data_y[i] - mean_y);
01636                 }
01637 
01638         /* mb contains surface values (z-axis) */
01639 
01640         mb_data[0 + i*1] = (values_data[i] - mean_z) / sigmas_data[i];
01641         }
01642     }
01643     else  /* Use sigma = 1 */
01644     {
01645             int i;
01646         for (i=0 ; i<np ; i++) {
01647                 double *ma_data = cpl_matrix_get_data(ma);
01648                 double *mb_data = cpl_matrix_get_data(mb);
01649 
01650                 double valy = 1;
01651                 int j = 0;
01652                 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01653                     double valx = 1; 
01654                     for (degx=0 ; degx<=poly_deg1 ; degx++) {
01655                         ma_data[j + i*nc] = valx * valy / 1;
01656                         valx *= (xy_pos_data_x[i] - mean_x);
01657                         j++;
01658                     }
01659                     valy *= (xy_pos_data_y[i] - mean_y);
01660                 }
01661 
01662         /* mb contains surface values (z-axis) */
01663 //        cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01664         mb_data[0 + i*1] = (values_data[i] - mean_z) / 1;
01665         }
01666     }
01667     
01668     /* If variance polynomial is requested, 
01669        compute covariance matrix = (A^T * A)^-1 */
01670     if (variance != NULL)
01671     {
01672         mat    = cpl_matrix_transpose_create(ma);
01673         if (mat != NULL)
01674         {
01675             mat_ma = cpl_matrix_product_create(mat, ma);
01676             if (mat_ma != NULL)
01677             {
01678                 cov          = cpl_matrix_invert_create(mat_ma);
01679                 /* Here, one might do a (paranoia) check that the covariance
01680                    matrix is symmetrical and has positive eigenvalues (so that
01681                    the returned variance polynomial is guaranteed to be positive) */
01682 
01683                 variance_cpl = cpl_polynomial_new(2);
01684             }
01685         }
01686         uves_free_matrix(&mat);
01687         uves_free_matrix(&mat_ma);
01688     }
01689 
01690     /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
01691     mx = cpl_matrix_solve_normal(ma, mb) ;
01692 
01693     uves_free_matrix(&ma) ;
01694     uves_free_matrix(&mb) ;
01695     if (mx == NULL) {
01696         cpl_free(degx_tab) ;
01697         cpl_free(degy_tab) ;
01698     uves_free_matrix(&cov) ;
01699         assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed") ;
01700     }
01701 
01702     /* Store coefficients for output */
01703     out = cpl_polynomial_new(2) ;
01704     powers = cpl_malloc(2 * sizeof(int)) ;
01705     if (powers == NULL) {
01706         cpl_free(degx_tab) ;
01707         cpl_free(degy_tab) ;
01708     uves_free_matrix(&mx) ;
01709     uves_free_matrix(&cov) ;
01710     uves_free_polynomial(&out) ;
01711     assure_mem( false );
01712     }
01713 
01714     {
01715         int i;
01716     for (i = 0 ; i < nc ; i++)
01717     {
01718         powers[0] = degx_tab[i] ;
01719         powers[1] = degy_tab[i] ;
01720         cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0)) ;
01721         
01722         /* Create variance polynomial (if requested) */
01723         if (variance != NULL &&                   /* Requested? */
01724         cov != NULL && variance_cpl != NULL   /* covariance computation succeeded? */
01725         )
01726         {
01727                     int j;
01728             for (j = 0; j < nc; j++)
01729             {
01730                 double coeff;
01731                 /* Add cov_ij to the proper coeff:
01732                    cov_ij * dp/d(ai) * dp/d(aj) =
01733                    cov_ij * (x^degx[i] * y^degy[i]) * (x^degx[i] * y^degy[i]) =
01734                    cov_ij * x^(degx[i]+degx[j]) * y^(degy[i] + degy[j]),
01735                    
01736                    i.e. add cov_ij to coeff (degx[i]+degx[j], degy[i]+degy[j]) */
01737                 powers[0] = degx_tab[i] + degx_tab[j] ;
01738                 powers[1] = degy_tab[i] + degy_tab[j] ;
01739                 
01740                 coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
01741                 cpl_polynomial_set_coeff(variance_cpl, powers, 
01742                              coeff + cpl_matrix_get(cov, i, j)) ;
01743             }
01744         }
01745     }
01746     }
01747     
01748     cpl_free(powers) ;
01749     cpl_free(degx_tab) ;
01750     cpl_free(degy_tab) ;
01751     uves_free_matrix(&cov) ;
01752     uves_free_matrix(&mx) ;
01753     
01754     /* Create and shift result */
01755     result = uves_polynomial_new(out);
01756     uves_free_polynomial(&out);
01757     uves_polynomial_shift(result, 0, mean_z);
01758     uves_polynomial_shift(result, 1, mean_x);
01759     uves_polynomial_shift(result, 2, mean_y);
01760 
01761     /* Wrap up variance polynomial */
01762     if (variance != NULL)
01763     {
01764         *variance = uves_polynomial_new(variance_cpl);
01765         uves_free_polynomial(&variance_cpl);
01766             /* The variance of the fit does not change
01767            when a constant is added to the a_00
01768            coefficient of the polynomial, so don't:
01769            uves_polynomial_shift(*variance, 0, mean_z); */
01770         uves_polynomial_shift(*variance, 1, mean_x);
01771         uves_polynomial_shift(*variance, 2, mean_y);
01772 
01773         /* Maybe here add a consistency check that the variance polynomial is 
01774            positive at all input points */
01775     }  
01776 
01777     /* If requested, compute mean squared error */
01778     if (mse != NULL || red_chisq != NULL) 
01779     {
01780             int i;
01781 
01782         if (mse       != NULL) *mse = 0.00 ;
01783         if (red_chisq != NULL) *red_chisq = 0.00 ;
01784         for (i = 0 ; i < np ; i++) 
01785         {
01786             double regress = uves_polynomial_evaluate_2d(result, 
01787                                  xy_pos_data_x[i],
01788                                  xy_pos_data_y[i]);
01789             /* Subtract from the true value, square, accumulate */
01790             if (mse != NULL)
01791             {
01792                 double residual = values_data[i] - regress;
01793                 *mse += residual*residual;
01794             }
01795             if (red_chisq != NULL)
01796             {
01797                 *red_chisq += uves_pow_int((values_data[i] - regress) /
01798                                sigmas_data[i], 2);
01799             }
01800         }
01801         /* Get average */
01802         if (mse       != NULL)  *mse       /= (double) np ;
01803         
01804         if (red_chisq != NULL)
01805         {
01806             passure( np > nc, "%d %d", np, nc); /* Was already checked */
01807             *red_chisq /= (double) (np - nc) ;
01808         }
01809     }
01810 
01811   cleanup:
01812     return result ;
01813 }
01814 
01815 

Generated on Fri Apr 18 14:11:43 2008 for UVES Pipeline Reference Manual by  doxygen 1.5.1