GRAVI Pipeline Reference Manual  1.2.3
gravi_ellipse.c
1 /* $Id: gravi_ellipse.c,v 1.10 2011/05/31 06:10:40 nazouaoui Exp $
2  *
3  * This file is part of the GRAVI Pipeline
4  * Copyright (C) 2002,2003 European Southern Observatory
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
32 /*-----------------------------------------------------------------------------
33  Includes
34  -----------------------------------------------------------------------------*/
35 
36 #ifdef HAVE_CONFIG_H
37 #include <config.h>
38 #endif
39 
40 #include <cpl.h>
41 #include <string.h>
42 #include <stdio.h>
43 #include <math.h>
44 #include <time.h>
45 
46 #include "gravi_data.h"
47 
48 #include "gravi_dfs.h"
49 #include "gravi_pfits.h"
50 #include "gravi_cpl.h"
51 
52 #include "gravi_utils.h"
53 #include "gravi_ellipse.h"
54 
55 /*-----------------------------------------------------------------------------
56  Function code
57  -----------------------------------------------------------------------------*/
58 
59 static int ellipse(const double x_in[], const double v[], double *result) {
60 
61  double x = x_in[0], y = x_in[1], a = v[0], b = v[1], c = v[2], d = v[3],
62  e = v[4];
63  *result = sqrt(pow(a * x + b * y + c, 2) + pow(d * y + e, 2));
64 
65  return (0);
66 }
67 
68 static int dfda_ellipse(const double x_in[], const double v[], double result[]) {
69 
70  double x = x_in[0], y = x_in[1], a = v[0], b = v[1], c = v[2], d = v[3],
71  e = v[4];
72  float inv_sqrtf;
73  inv_sqrtf = 1 / sqrt(pow(a * x + b * y + c, 2) + pow(d * y + e, 2));
74 
75  result[0] = inv_sqrtf * (2 * a * x * x + 2 * x * b * y + 2 * x * c);
76  result[1] = inv_sqrtf * (2 * b * y * y + 2 * a * x * y + 2 * y * c);
77  result[2] = inv_sqrtf * (2 * a * x + 2 * b * y + 2 * c);
78  result[3] = inv_sqrtf * (2 * y * e + 2 * y * y * d);
79  result[4] = inv_sqrtf * (2 * e + 2 * d * y);
80 
81  return (0);
82 }
83 
84 /*---------------------------------------------------------------------------*/
105 /*---------------------------------------------------------------------------*/
106 
107 cpl_vector * gravi_ellipse_phase_create (cpl_vector * vectCA,
108  cpl_vector * vectDB,
109  cpl_vector * envelope)
110 {
111  gravi_msg_function_start(0);
112  cpl_ensure (vectCA, CPL_ERROR_NULL_INPUT, NULL);
113  cpl_ensure (vectDB, CPL_ERROR_NULL_INPUT, NULL);
114 
115  cpl_size size = cpl_vector_get_size (vectCA);
116 
117  /* Linear ellipse fitting */
118  if (USE_LINEAR_ELLIPSE) {
119  cpl_errorstate prestate = cpl_errorstate_get();
120 
121  /* Recenter the vector in-place */
122  cpl_vector_subtract_scalar (vectCA, cpl_vector_get_mean (vectCA));
123  cpl_vector_subtract_scalar (vectDB, cpl_vector_get_mean (vectDB));
124 
125  /* Normalise the vector in-place */
126  cpl_vector_divide_scalar (vectCA, cpl_vector_get_stdev (vectCA));
127  cpl_vector_divide_scalar (vectDB, cpl_vector_get_stdev (vectDB));
128 
129  /* Fill matrix */
130  cpl_matrix * coeff = cpl_matrix_new (size, 5);
131  for (cpl_size v=0; v<size; v++) {
132  double x = cpl_vector_get (vectCA, v);
133  double y = cpl_vector_get (vectDB, v);
134  cpl_matrix_set (coeff, v, 0, x*x);
135  cpl_matrix_set (coeff, v, 1, x*y);
136  cpl_matrix_set (coeff, v, 2, x);
137  cpl_matrix_set (coeff, v, 3, y);
138  cpl_matrix_set (coeff, v, 4, y*y);
139  }
140 
141  /* Fill right-hand-side */
142  cpl_matrix * rhs = cpl_matrix_new (size, 1);
143  for (cpl_size v=0; v<size; v++) {
144  double value = envelope ? cpl_vector_get (envelope, v) : 1.0;
145  cpl_matrix_set (rhs, v, 0, value);
146  }
147 
148  /* Solve */
149  cpl_matrix * res = cpl_matrix_solve_normal (coeff, rhs);
150 
151  /* Delete */
152  FREE (cpl_matrix_delete, coeff);
153  FREE (cpl_matrix_delete, rhs);
154 
155  /* Recover on error but return NULL */
156  if (cpl_error_get_code()) {
157  cpl_errorstate_set (prestate);
158  cpl_msg_warning (cpl_func, "Error during fit of ellipse");
159  return NULL;
160  }
161 
162  /* Convert to Lacour coefficients */
163  double b = cpl_matrix_get (res,4,0);
164  if (b <= 0) {cpl_msg_warning (cpl_func, "Error during fit of ellipse");
165  FREE (cpl_matrix_delete, res); return NULL;}
166 
167  double d = cpl_matrix_get (res,1,0) / (2*b);
168  double a = cpl_matrix_get (res,0,0) - b*d*d;
169  if (a <= 0) {cpl_msg_warning (cpl_func, "Error during fit of ellipse");
170  FREE (cpl_matrix_delete, res); return NULL;}
171 
172  double c2 = cpl_matrix_get (res,3,0) / (2*b);
173  double c1 = (cpl_matrix_get (res,2,0) - 2*b*d*c2) / (2*a);
174 
175  FREE (cpl_matrix_delete, res);
176 
177  /* Replace vectCA and vectDB by their corrected version */
178  a = sqrt(a);
179  b = sqrt(b);
180  for (cpl_size v=0; v<size; v++) {
181  double x = cpl_vector_get (vectCA, v);
182  double y = cpl_vector_get (vectDB, v);
183  cpl_vector_set (vectCA, v, a * (x+c1));
184  cpl_vector_set (vectDB, v, b * (y+d*x+c2));
185  }
186 
187  }
188  /* Non-linear ellipse fitting */
189  else {
190  /* Initialization of th init_val */
191  cpl_vector * init_val = cpl_vector_new(5);
192  cpl_vector_set(init_val, 0, 1.);
193  cpl_vector_set(init_val, 1, 0.);
194  cpl_vector_set(init_val, 2, (-1) * cpl_vector_get_mean(vectCA));
195  cpl_vector_set(init_val, 3, 1);
196  cpl_vector_set(init_val, 4, (-1) * cpl_vector_get_mean(vectDB));
197 
198  /* Construct the matrix_XY and vector_1 */
199  cpl_matrix * matrix_XY = get_matrix_from_vector(vectCA, vectDB);
200  cpl_vector * vector_1 = cpl_vector_new (size);
201  for (cpl_size v=0; v<size; v++) {
202  double value = envelope ? cpl_vector_get (envelope, v) : 1.0;
203  cpl_vector_set (vector_1, v, value);
204  }
205 
206  /* Fit ellipse */
207  cpl_errorstate prestate = cpl_errorstate_get();
208 
209  double mse = 0;
210  int val_to_fit[] = {1,1,1,1,1};
211  cpl_fit_lvmq(matrix_XY, NULL, vector_1, NULL, init_val, val_to_fit,
212  &ellipse, &dfda_ellipse, CPL_FIT_LVMQ_TOLERANCE,
213  CPL_FIT_LVMQ_COUNT, CPL_FIT_LVMQ_MAXITER, &mse, NULL, NULL);
214 
215  /* Recover on error but return NULL */
216  if (cpl_error_get_code()){
217  cpl_vector_delete(vector_1);
218  cpl_vector_delete(init_val);
219  cpl_matrix_delete(matrix_XY);
220  cpl_errorstate_set(prestate);
221  cpl_msg_warning(cpl_func, "Error during fit of ellipse");
222  return NULL;
223  }
224 
225  /* Replace the new vector_X values after the fit where
226  * vector_X = init_val(0) * vector_X + init_val(1) * vector_Y + init_val(2)
227  * vector_Y = init_val(3) * vector_Y + init_val(4) */
228 
229  /* Return the corrected vector */
230  double vector_Yp_i, vector_X_i, vector_Y_i;
231  for (cpl_size i_data = 0; i_data < size; i_data++) {
232 
233  vector_Yp_i = cpl_vector_get(vectDB, i_data) *
234  cpl_vector_get(init_val, 1);
235  vector_X_i = cpl_vector_get(vectCA, i_data) *
236  cpl_vector_get(init_val, 0) + cpl_vector_get(init_val, 2);
237 
238  cpl_vector_set (vectCA, i_data, vector_X_i + vector_Yp_i);
239 
240  vector_Y_i = cpl_vector_get (vectDB, i_data) *
241  cpl_vector_get(init_val, 3) + cpl_vector_get (init_val, 4);
242 
243  cpl_vector_set(vectDB, i_data, vector_Y_i);
244  }
245 
246  cpl_matrix_delete (matrix_XY);
247  cpl_vector_delete (init_val);
248  cpl_vector_delete (vector_1);
249 
250  } /* End non-linear ellipse fitting */
251 
252 
253 
254  /* Initialization of the OPD vector and compute the phase*/
255  cpl_vector * OPD_rad = cpl_vector_new(size);
256  cpl_vector_set (OPD_rad, 0, atan2(cpl_vector_get(vectDB, 0),
257  cpl_vector_get(vectCA, 0)));
258 
259  /* Dewrap opd */
260  double d_opd_i, d_opd_ii, d_opd;
261  for (cpl_size i_data = 1; i_data < size; i_data++){
262 
263  /* Evaluation of the OPD(i_data) and
264  * OPD(i_data-1) */
265  d_opd_i = atan2 (cpl_vector_get(vectDB, i_data),
266  cpl_vector_get(vectCA, i_data));
267 
268  d_opd_ii = atan2 (cpl_vector_get(vectDB, i_data - 1),
269  cpl_vector_get(vectCA, i_data - 1));
270 
271  d_opd = d_opd_i - d_opd_ii;
272 
273  if (d_opd > M_PI) d_opd = d_opd - 2 * M_PI;
274  if (d_opd < - M_PI) d_opd = d_opd + 2 * M_PI;
275 
276  cpl_vector_set (OPD_rad, i_data, cpl_vector_get (OPD_rad, i_data-1) +
277  d_opd);
278  }
279 
280  /* End fit opd */
281  gravi_msg_function_exit(0);
282  return OPD_rad;
283 }
284 
285 /*----------------------------------------------------------------------------*/
286 
287 cpl_vector * gravi_ellipse_phase_create_fast (cpl_vector * vectCA, cpl_vector * vectDB)
288 {
289  gravi_msg_function_start(0);
290  cpl_ensure (vectCA, CPL_ERROR_NULL_INPUT, NULL);
291  cpl_ensure (vectDB, CPL_ERROR_NULL_INPUT, NULL);
292 
293  cpl_size size = cpl_vector_get_size (vectCA);
294 
295  /* Recenter the vector in-place */
296  cpl_vector_subtract_scalar (vectCA, cpl_vector_get_mean (vectCA));
297  cpl_vector_subtract_scalar (vectDB, cpl_vector_get_mean (vectDB));
298 
299  /* Initialization of the OPD vector and compute the phase*/
300  cpl_vector * OPD_rad = cpl_vector_new(size);
301  cpl_vector_set (OPD_rad, 0, atan2(cpl_vector_get(vectDB, 0),
302  cpl_vector_get(vectCA, 0)));
303 
304  /* Dewrap opd */
305  double d_opd_i, d_opd_ii, d_opd;
306  for (cpl_size i_data = 1; i_data < size; i_data++){
307 
308  /* Evaluation of the OPD(i_data) and
309  * OPD(i_data-1) */
310  d_opd_i = atan2 (cpl_vector_get(vectDB, i_data),
311  cpl_vector_get(vectCA, i_data));
312 
313  d_opd_ii = atan2 (cpl_vector_get(vectDB, i_data - 1),
314  cpl_vector_get(vectCA, i_data - 1));
315 
316  d_opd = d_opd_i - d_opd_ii;
317 
318  if (d_opd > M_PI) d_opd = d_opd - 2 * M_PI;
319  if (d_opd < - M_PI) d_opd = d_opd + 2 * M_PI;
320 
321  cpl_vector_set (OPD_rad, i_data, cpl_vector_get (OPD_rad, i_data-1) +
322  d_opd);
323  }
324 
325  /* End fit opd */
326  gravi_msg_function_exit(0);
327  return OPD_rad;
328 }
329 
330 /*----------------------------------------------------------------------------*/
349 /*----------------------------------------------------------------------------*/
350 
351 cpl_vector * gravi_ellipse_meanopd_create (cpl_table * spectrum_table,
352  cpl_table * detector_table,
353  cpl_table ** oiwave_tables,
354  cpl_vector * guess_vector,
355  int base)
356 {
357  gravi_msg_function_start(0);
358  cpl_ensure (spectrum_table, CPL_ERROR_NULL_INPUT, NULL);
359  cpl_ensure (detector_table, CPL_ERROR_NULL_INPUT, NULL);
360  cpl_ensure (oiwave_tables, CPL_ERROR_NULL_INPUT, NULL);
361 
362  /* Get the size of the vectors */
363  cpl_size nwave = gravi_spectrum_get_nwave (spectrum_table);
364  cpl_size nrow = cpl_table_get_nrow (spectrum_table);
365  int npol = (cpl_table_get_nrow (detector_table) > 24 ? 2 : 1);
366  CPLCHECK_NUL ("Cannot get size");
367 
368  /* Check input */
369  for (int pol = 0; pol < npol; pol++)
370  cpl_ensure (oiwave_tables[pol], CPL_ERROR_NULL_INPUT, NULL);
371 
372  /* Spectral band to integrate */
373  int wave_start = nwave > 5 ? nwave/4 : 0;
374  int wave_end = nwave > 5 ? (nwave*3)/4 : nwave-1;
375 
376  /* Init mean_opd to average over channel and polar */
377  cpl_vector * mean_opd = cpl_vector_new (nrow);
378  cpl_vector_fill (mean_opd, 0.0);
379 
380  /* Do the computation twice, first without enveloppe and
381  * then with enveloppe correction */
382  for (int loop = 0; loop < 2; loop++) {
383 
384  cpl_msg_debug (cpl_func, "Now run %s envelope calibration", loop ? "WITH" : "WITHOUT");
385 
386  cpl_vector * opd_vector = cpl_vector_duplicate (mean_opd);
387  cpl_vector_fill (mean_opd, 0.0);
388 
389  /* Save the OPD to compute the enveloppe
390  * accurately in the second loop */
391 
392  /* Loop on polarisations */
393  for (int pol = 0; pol < npol; pol ++) {
394 
395  /* Sign of this baseline */
396  double phi_sign = gravi_region_get_base_sign (detector_table, base);
397 
398  /* Get the region index of this base and pol */
399  int iA = gravi_get_region (detector_table, base, 'A', pol);
400  int iB = gravi_get_region (detector_table, base, 'B', pol);
401  int iC = gravi_get_region (detector_table, base, 'C', pol);
402  int iD = gravi_get_region (detector_table, base, 'D', pol);
403  if (iA<0 || iB<0 || iC<0 || iD<0){
404  cpl_msg_error (cpl_func, "Don't found the A, B, C or D !!!");
405  }
406 
407  /* Init the mean phase over channel for this pol */
408  cpl_vector * pol_opd = cpl_vector_new (nrow);
409  cpl_vector_fill (pol_opd, 0.0);
410 
411  /* Init the mean inter-spectra over channel for this pol */
412  cpl_vector * phase_previous = NULL;
413  cpl_array * is_array = cpl_array_new (nrow, CPL_TYPE_DOUBLE_COMPLEX);
414  cpl_array_fill_window_double_complex (is_array, 0, nrow, 0 + 0.0*I);
415 
416  /* Loop on wave */
417  for (cpl_size wave = wave_start; wave <= wave_end; wave++){
418  cpl_vector * vector_T = NULL;
419 
420  /* Define the vector_X = C - A */
421  cpl_vector * vector_X;
422  vector_X = gravi_table_get_vector (spectrum_table, wave, GRAVI_DATA[iC]);
423  vector_T = gravi_table_get_vector (spectrum_table, wave, GRAVI_DATA[iA]);
424  cpl_vector_subtract (vector_X, vector_T);
425  FREE (cpl_vector_delete, vector_T);
426 
427  /* Define the vector_Y = D - B */
428  cpl_vector * vector_Y;
429  vector_Y = gravi_table_get_vector (spectrum_table, wave, GRAVI_DATA[iD]);
430  vector_T = gravi_table_get_vector (spectrum_table, wave, GRAVI_DATA[iB]);
431  cpl_vector_subtract (vector_Y, vector_T);
432  FREE (cpl_vector_delete, vector_T);
433 
434  CPLCHECK_NUL ("Cannot extract the X and Y vectors");
435 
436  /* Compute envelope from OPD for this channel */
437  cpl_vector * envelope_vector = gravi_compute_envelope (opd_vector, wave, nwave);
438 
439  /* Compute the phase of this channel from ellipse */
440  cpl_vector * phase = gravi_ellipse_phase_create (vector_X, vector_Y, envelope_vector);
441  cpl_vector_delete (vector_X);
442  cpl_vector_delete (vector_Y);
443  cpl_vector_delete (envelope_vector);
444  CPLCHECK_NUL ("Error during the fit of the ellipse");
445 
446  /* Multiply by the sign */
447  cpl_vector_multiply_scalar (phase, phi_sign);
448 
449  /* Wavelength of this channel */
450  double lbd_channel = cpl_table_get (oiwave_tables[pol], "EFF_WAVE", wave, NULL);
451 
452  /* Unwrap if opd_guess is provided */
453  if (guess_vector) {
454  gravi_vector_unwrap_with_guess (phase, guess_vector, CPL_MATH_2PI / lbd_channel);
455  CPLCHECK_NUL ("Error during the unwrap");
456  }
457 
458  /* Accumulate the inter-spectra over the spectral channels */
459  if (wave != wave_start) {
460  cpl_vector_subtract (phase_previous, phase);
461  for (cpl_size row=0; row<nrow; row++) {
462  cpl_array_set_complex (is_array, row,
463  cpl_array_get_complex (is_array, row, NULL) +
464  cexp ( 1.*I*cpl_vector_get (phase_previous, row)));
465  CPLCHECK_NUL ("Cannot accumulate is_array");
466  }
467  FREE (cpl_vector_delete, phase_previous);
468  }
469  if (wave != wave_end)
470  phase_previous = cpl_vector_duplicate (phase);
471 
472  /* Convert phase to opd and accumulate the mean
473  * phase opd over the spectral channels */
474  cpl_vector_multiply_scalar (phase, lbd_channel / CPL_MATH_2PI);
475  cpl_vector_add (pol_opd, phase);
476  CPLCHECK_NUL ("Cannot accumulate phase");
477 
478  FREE (cpl_vector_delete, phase);
479  } /* end loop on wave */
480 
481 
482  /* Compute mean phase over the channels [rad] */
483  cpl_vector_divide_scalar (pol_opd, wave_end - wave_start + 1);
484 
485  /* Compute the group-delay [rad/cannal] */
486  cpl_array_arg (is_array);
487 
488  /* Fit group-delay [rad/cannal] versus opd [um]
489  * with a linear relation */
490  cpl_polynomial * fit_lin = cpl_polynomial_new(1);
491  cpl_matrix * matrix = cpl_matrix_wrap (1, nrow, cpl_array_get_data_double(is_array));
492  const cpl_size mindeg = 0, maxdeg = 1;
493 
494  cpl_polynomial_fit (fit_lin , matrix, NULL, pol_opd, NULL,
495  CPL_FALSE, &mindeg, &maxdeg);
496 
497  /* Get the opd where the group-delay == 0 */
498  double opd0 = cpl_polynomial_get_coeff (fit_lin, &mindeg);
499 
500  cpl_matrix_unwrap (matrix);
501  cpl_array_delete (is_array);
502  cpl_polynomial_delete (fit_lin);
503 
504  CPLCHECK_NUL ("Cannot fit");
505 
506  /* Remove opd0 from the mean phase to ensure
507  * pol_opd = 0 <-> gdelay = 0 */
508  cpl_vector_subtract_scalar (pol_opd, opd0);
509 
510  cpl_msg_info (cpl_func, "opd0 = %g [um] (base %d pol %d %s envelope)",
511  opd0*1e6, base + 1, pol + 1, loop ? "with" : "without");
512 
513  /* Integrathe the two polarisation */
514  cpl_vector_add (mean_opd, pol_opd);
515  cpl_vector_delete (pol_opd);
516 
517  } /* End loop on pol */
518 
519  cpl_vector_divide_scalar (mean_opd, npol);
520 
521  FREE (cpl_vector_delete, opd_vector);
522  } /* End loop on with/without correction */
523 
524  gravi_msg_function_exit(0);
525  return mean_opd;
526 }
527 
int gravi_region_get_base_sign(cpl_table *imaging_detector, int base)
Return the sign of a base by looking at the PORT order.
Definition: gravi_utils.c:399
cpl_vector * gravi_table_get_vector(cpl_table *spectrum_data, cpl_size index, const char *regname)
Create a vector from the row index of the column regname.
Definition: gravi_cpl.c:1870
cpl_vector * gravi_ellipse_phase_create(cpl_vector *vectCA, cpl_vector *vectDB, cpl_vector *envelope)
Compute the phase atan{X&#39;,Y&#39;}, unwraped from first sample.
cpl_vector * gravi_compute_envelope(const cpl_vector *opd, int wave, int nwave)
Compute the envelope value.
Definition: gravi_utils.c:1081
int gravi_get_region(cpl_table *img_det, int base, char phase, int pol)
Find the region matching base, phase and pol.
Definition: gravi_utils.c:584
cpl_vector * gravi_ellipse_meanopd_create(cpl_table *spectrum_table, cpl_table *detector_table, cpl_table **oiwave_tables, cpl_vector *guess_vector, int base)
Compute the OPD modulation of a baseline from spectrum.
cpl_error_code gravi_vector_unwrap_with_guess(cpl_vector *vector, cpl_vector *ref, double ref_to_phase)
Unwrap a phase vector following a guess vector. The difference is actually unwrap and shall thus be s...
Definition: gravi_cpl.c:2284
cpl_matrix * get_matrix_from_vector(cpl_vector *vector1, cpl_vector *vector2)
Copy the content of two vector into a newly allocated 2D matrix.
Definition: gravi_cpl.c:1820