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

Generated on 8 Mar 2011 for UVES Pipeline Reference Manual by  doxygen 1.6.1