46 #include "gravi_data.h" 48 #include "gravi_dfs.h" 49 #include "gravi_pfits.h" 50 #include "gravi_cpl.h" 52 #include "gravi_utils.h" 53 #include "gravi_ellipse.h" 59 static int ellipse(
const double x_in[],
const double v[],
double *result) {
61 double x = x_in[0], y = x_in[1], a = v[0], b = v[1], c = v[2], d = v[3],
63 *result = sqrt(pow(a * x + b * y + c, 2) + pow(d * y + e, 2));
68 static int dfda_ellipse(
const double x_in[],
const double v[],
double result[]) {
70 double x = x_in[0], y = x_in[1], a = v[0], b = v[1], c = v[2], d = v[3],
73 inv_sqrtf = 1 / sqrt(pow(a * x + b * y + c, 2) + pow(d * y + e, 2));
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);
109 cpl_vector * envelope)
111 gravi_msg_function_start(0);
112 cpl_ensure (vectCA, CPL_ERROR_NULL_INPUT, NULL);
113 cpl_ensure (vectDB, CPL_ERROR_NULL_INPUT, NULL);
115 cpl_size size = cpl_vector_get_size (vectCA);
118 if (USE_LINEAR_ELLIPSE) {
119 cpl_errorstate prestate = cpl_errorstate_get();
122 cpl_vector_subtract_scalar (vectCA, cpl_vector_get_mean (vectCA));
123 cpl_vector_subtract_scalar (vectDB, cpl_vector_get_mean (vectDB));
126 cpl_vector_divide_scalar (vectCA, cpl_vector_get_stdev (vectCA));
127 cpl_vector_divide_scalar (vectDB, cpl_vector_get_stdev (vectDB));
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);
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);
149 cpl_matrix * res = cpl_matrix_solve_normal (coeff, rhs);
152 FREE (cpl_matrix_delete, coeff);
153 FREE (cpl_matrix_delete, rhs);
156 if (cpl_error_get_code()) {
157 cpl_errorstate_set (prestate);
158 cpl_msg_warning (cpl_func,
"Error during fit of ellipse");
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;}
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;}
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);
175 FREE (cpl_matrix_delete, res);
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));
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));
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);
207 cpl_errorstate prestate = cpl_errorstate_get();
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);
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");
230 double vector_Yp_i, vector_X_i, vector_Y_i;
231 for (cpl_size i_data = 0; i_data < size; i_data++) {
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);
238 cpl_vector_set (vectCA, i_data, vector_X_i + vector_Yp_i);
240 vector_Y_i = cpl_vector_get (vectDB, i_data) *
241 cpl_vector_get(init_val, 3) + cpl_vector_get (init_val, 4);
243 cpl_vector_set(vectDB, i_data, vector_Y_i);
246 cpl_matrix_delete (matrix_XY);
247 cpl_vector_delete (init_val);
248 cpl_vector_delete (vector_1);
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)));
260 double d_opd_i, d_opd_ii, d_opd;
261 for (cpl_size i_data = 1; i_data < size; i_data++){
265 d_opd_i = atan2 (cpl_vector_get(vectDB, i_data),
266 cpl_vector_get(vectCA, i_data));
268 d_opd_ii = atan2 (cpl_vector_get(vectDB, i_data - 1),
269 cpl_vector_get(vectCA, i_data - 1));
271 d_opd = d_opd_i - d_opd_ii;
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;
276 cpl_vector_set (OPD_rad, i_data, cpl_vector_get (OPD_rad, i_data-1) +
281 gravi_msg_function_exit(0);
287 cpl_vector * gravi_ellipse_phase_create_fast (cpl_vector * vectCA, cpl_vector * vectDB)
289 gravi_msg_function_start(0);
290 cpl_ensure (vectCA, CPL_ERROR_NULL_INPUT, NULL);
291 cpl_ensure (vectDB, CPL_ERROR_NULL_INPUT, NULL);
293 cpl_size size = cpl_vector_get_size (vectCA);
296 cpl_vector_subtract_scalar (vectCA, cpl_vector_get_mean (vectCA));
297 cpl_vector_subtract_scalar (vectDB, cpl_vector_get_mean (vectDB));
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)));
305 double d_opd_i, d_opd_ii, d_opd;
306 for (cpl_size i_data = 1; i_data < size; i_data++){
310 d_opd_i = atan2 (cpl_vector_get(vectDB, i_data),
311 cpl_vector_get(vectCA, i_data));
313 d_opd_ii = atan2 (cpl_vector_get(vectDB, i_data - 1),
314 cpl_vector_get(vectCA, i_data - 1));
316 d_opd = d_opd_i - d_opd_ii;
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;
321 cpl_vector_set (OPD_rad, i_data, cpl_vector_get (OPD_rad, i_data-1) +
326 gravi_msg_function_exit(0);
352 cpl_table * detector_table,
353 cpl_table ** oiwave_tables,
354 cpl_vector * guess_vector,
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);
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");
369 for (
int pol = 0; pol < npol; pol++)
370 cpl_ensure (oiwave_tables[pol], CPL_ERROR_NULL_INPUT, NULL);
373 int wave_start = nwave > 5 ? nwave/4 : 0;
374 int wave_end = nwave > 5 ? (nwave*3)/4 : nwave-1;
377 cpl_vector * mean_opd = cpl_vector_new (nrow);
378 cpl_vector_fill (mean_opd, 0.0);
382 for (
int loop = 0; loop < 2; loop++) {
384 cpl_msg_debug (cpl_func,
"Now run %s envelope calibration", loop ?
"WITH" :
"WITHOUT");
386 cpl_vector * opd_vector = cpl_vector_duplicate (mean_opd);
387 cpl_vector_fill (mean_opd, 0.0);
393 for (
int pol = 0; pol < npol; 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 !!!");
408 cpl_vector * pol_opd = cpl_vector_new (nrow);
409 cpl_vector_fill (pol_opd, 0.0);
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);
417 for (cpl_size wave = wave_start; wave <= wave_end; wave++){
418 cpl_vector * vector_T = NULL;
421 cpl_vector * vector_X;
424 cpl_vector_subtract (vector_X, vector_T);
425 FREE (cpl_vector_delete, vector_T);
428 cpl_vector * vector_Y;
431 cpl_vector_subtract (vector_Y, vector_T);
432 FREE (cpl_vector_delete, vector_T);
434 CPLCHECK_NUL (
"Cannot extract the X and Y vectors");
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");
447 cpl_vector_multiply_scalar (phase, phi_sign);
450 double lbd_channel = cpl_table_get (oiwave_tables[pol],
"EFF_WAVE", wave, NULL);
455 CPLCHECK_NUL (
"Error during the unwrap");
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");
467 FREE (cpl_vector_delete, phase_previous);
469 if (wave != wave_end)
470 phase_previous = cpl_vector_duplicate (phase);
474 cpl_vector_multiply_scalar (phase, lbd_channel / CPL_MATH_2PI);
475 cpl_vector_add (pol_opd, phase);
476 CPLCHECK_NUL (
"Cannot accumulate phase");
478 FREE (cpl_vector_delete, phase);
483 cpl_vector_divide_scalar (pol_opd, wave_end - wave_start + 1);
486 cpl_array_arg (is_array);
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;
494 cpl_polynomial_fit (fit_lin , matrix, NULL, pol_opd, NULL,
495 CPL_FALSE, &mindeg, &maxdeg);
498 double opd0 = cpl_polynomial_get_coeff (fit_lin, &mindeg);
500 cpl_matrix_unwrap (matrix);
501 cpl_array_delete (is_array);
502 cpl_polynomial_delete (fit_lin);
504 CPLCHECK_NUL (
"Cannot fit");
508 cpl_vector_subtract_scalar (pol_opd, opd0);
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");
514 cpl_vector_add (mean_opd, pol_opd);
515 cpl_vector_delete (pol_opd);
519 cpl_vector_divide_scalar (mean_opd, npol);
521 FREE (cpl_vector_delete, opd_vector);
524 gravi_msg_function_exit(0);
int gravi_region_get_base_sign(cpl_table *imaging_detector, int base)
Return the sign of a base by looking at the PORT order.
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.
cpl_vector * gravi_ellipse_phase_create(cpl_vector *vectCA, cpl_vector *vectDB, cpl_vector *envelope)
Compute the phase atan{X',Y'}, unwraped from first sample.
cpl_vector * gravi_compute_envelope(const cpl_vector *opd, int wave, int nwave)
Compute the envelope value.
int gravi_get_region(cpl_table *img_det, int base, char phase, int pol)
Find the region matching base, phase and pol.
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...
cpl_matrix * get_matrix_from_vector(cpl_vector *vector1, cpl_vector *vector2)
Copy the content of two vector into a newly allocated 2D matrix.