CR2RE Pipeline Reference Manual 1.6.7
hdrl_correlation.c
1/*
2 * This file is part of the HDRL
3 * Copyright (C) 2017 European Southern Observatory
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
18 */
19
20#ifdef HAVE_CONFIG_H
21#include <config.h>
22#endif
23
24/*-----------------------------------------------------------------------------
25 Includes
26 -----------------------------------------------------------------------------*/
27#include "hdrl_correlation.h"
28
29#include <math.h>
30
31
32/*-----------------------------------------------------------------------------
33 Data structures used internally
34 -----------------------------------------------------------------------------*/
35typedef struct{
36 double mean;
37 double stdev;
38}mean_and_stdev;
39
40/*-----------------------------------------------------------------------------
41 Private Functions
42 -----------------------------------------------------------------------------*/
43
44static inline
45double calculate_xcorr_sample(const cpl_size shift, const cpl_array * arr1,
46 const cpl_array * arr2, const double mean1, const double mean2,
47 const double stdev1, const double stdev2);
48
49static inline
50mean_and_stdev calculate_mean_and_stdev(const cpl_array * arr1);
51
52static inline cpl_error_code
53hdrl_compute_xcorrelation_refine(hdrl_xcorrelation_result* xcorr_res,
54 const double bin, const double wrange);
55
60/*-----------------------------------------------------------------------------
61 Functions
62 -----------------------------------------------------------------------------*/
63
64/* ---------------------------------------------------------------------------*/
76/* ---------------------------------------------------------------------------*/
77
78hdrl_xcorrelation_result *
79hdrl_xcorrelation_result_wrap(cpl_array * x_corr, const cpl_size max_idx,
80 const cpl_size half_window){
81
82 cpl_ensure(x_corr != NULL, CPL_ERROR_NULL_INPUT, NULL);
83 cpl_ensure(max_idx >= 0, CPL_ERROR_ILLEGAL_INPUT, NULL);
84 cpl_ensure(max_idx < cpl_array_get_size(x_corr),
85 CPL_ERROR_ILLEGAL_INPUT, NULL);
86
87 hdrl_xcorrelation_result * to_ret = cpl_calloc(1, sizeof(*to_ret));
88
89 to_ret->xcorr = x_corr;
90 to_ret->pix_peakpos = max_idx;
91 to_ret->half_window = half_window;
92 return to_ret;
93}
94
95/* ---------------------------------------------------------------------------*/
102/* ---------------------------------------------------------------------------*/
103void hdrl_xcorrelation_result_delete(hdrl_xcorrelation_result * self){
104 if(self == NULL) return;
105
106 cpl_array_delete(self->xcorr);
107 cpl_free(self);
108}
109
110/* ---------------------------------------------------------------------------*/
118/* ---------------------------------------------------------------------------*/
119cpl_size
120hdrl_xcorrelation_result_get_peak_pixel(const hdrl_xcorrelation_result * self){
121
122 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
123
124 return self->pix_peakpos;
125}
126
127/* ---------------------------------------------------------------------------*/
137/* ---------------------------------------------------------------------------*/
139 (const hdrl_xcorrelation_result * self){
140
141 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
142
143 return self->peakpos;
144}
145
146/* ---------------------------------------------------------------------------*/
154/* ---------------------------------------------------------------------------*/
156 (const hdrl_xcorrelation_result * self){
157
158 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
159
160 return self->half_window;
161}
162
163/* ---------------------------------------------------------------------------*/
171/* ---------------------------------------------------------------------------*/
172double
173hdrl_xcorrelation_result_get_sigma(const hdrl_xcorrelation_result * self){
174
175 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
176
177 return self->sigma;
178}
179
180/* ---------------------------------------------------------------------------*/
188/* ---------------------------------------------------------------------------*/
189const cpl_array *
190hdrl_xcorrelation_result_get_correlation(const hdrl_xcorrelation_result * self){
191
192 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
193
194 return self->xcorr;
195}
196
197/* ---------------------------------------------------------------------------*/
210/* ---------------------------------------------------------------------------*/
211hdrl_xcorrelation_result * hdrl_compute_xcorrelation(
212 const cpl_array * arr1, const cpl_array * arr2,
213 const cpl_size half_window, const cpl_boolean normalize){
214
215 cpl_ensure(half_window > 1, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
216
217 const mean_and_stdev not_normalized = {0.0, 1.0};
218
219 cpl_ensure(arr1 != NULL && arr2 != NULL, CPL_ERROR_NULL_INPUT, NULL);
220
221 cpl_size xcorr_length = 2 * half_window + 1;
222 cpl_array * corr = cpl_array_new(xcorr_length, HDRL_TYPE_DATA);
223
224 const mean_and_stdev d1 = normalize ?
225 calculate_mean_and_stdev(arr1) : not_normalized;
226 const mean_and_stdev d2 = normalize ?
227 calculate_mean_and_stdev(arr2) : not_normalized;
228
229 cpl_size max_idx = -1;
230 double max_corr = 0.0;
231
232 for(cpl_size i = -half_window; i <= half_window; ++i){
233 const double cr = calculate_xcorr_sample(i, arr1, arr2, d1.mean, d2.mean,
234 d1.stdev, d2.stdev);
235
236 const cpl_size idx = i + half_window;
237 cpl_array_set(corr, idx, cr);
238
239 if(isnan(cr)) continue;
240
241 if(cr >= max_corr || max_idx < 0){
242 max_corr = cr;
243 max_idx = idx;
244 }
245 }
246
247 return hdrl_xcorrelation_result_wrap(corr, max_idx, half_window);
248}
249
250static inline
251cpl_error_code check_if_bad(const hdrl_xcorrelation_result * gfit,
252 const cpl_boolean check_refine){
253
254 cpl_ensure_code(gfit != NULL, CPL_ERROR_ILLEGAL_OUTPUT);
255 cpl_ensure_code(hdrl_xcorrelation_result_get_peak_pixel(gfit) >=0,
256 CPL_ERROR_ILLEGAL_OUTPUT);
257
258 if(check_refine){
259
260 const double px = hdrl_xcorrelation_result_get_peak_subpixel(gfit);
261 cpl_ensure_code(px >= 0.0 && !isnan(px), CPL_ERROR_ILLEGAL_OUTPUT);
262
263 const double sigma = hdrl_xcorrelation_result_get_sigma(gfit);
264 cpl_ensure_code( sigma > 0.0 && !isnan(sigma),CPL_ERROR_ILLEGAL_OUTPUT);
265 }
266
267 return CPL_ERROR_NONE;
268}
269
270static inline
271cpl_boolean check_and_delete_if_bad(hdrl_xcorrelation_result ** gfit_arg,
272 const cpl_boolean check_refine){
273
274 cpl_error_code fail = check_if_bad(*gfit_arg, check_refine);
275
276 if(fail){
278 *gfit_arg = NULL;
279 }
280
281 return fail ? CPL_FALSE : CPL_TRUE;
282}
283
284/* ---------------------------------------------------------------------------*/
298/* ---------------------------------------------------------------------------*/
299hdrl_xcorrelation_result * hdrl_compute_offset_gaussian(
300 const cpl_array * arr1,
301 const cpl_array * arr2,
302 const cpl_size half_win, const cpl_boolean normalize,
303 const double bin, const double wrange){
304
305 cpl_ensure(half_win > 1, CPL_ERROR_ILLEGAL_INPUT, NULL);
306 cpl_ensure(arr1 != NULL, CPL_ERROR_NULL_INPUT, NULL);
307 cpl_ensure(arr2 != NULL, CPL_ERROR_NULL_INPUT, NULL);
308
309 hdrl_xcorrelation_result * gfit =
310 hdrl_compute_offset_gaussian_internal(arr1, arr2, half_win,
311 normalize, bin, wrange);
312
313 cpl_ensure(gfit != NULL, CPL_ERROR_ILLEGAL_OUTPUT, NULL);
314
315 const cpl_size half_win2 =
316 (cpl_size)(3. * CPL_MATH_FWHM_SIG * gfit->sigma / bin);
317
319
320 gfit = hdrl_compute_offset_gaussian_internal(arr1, arr2, half_win2,
321 normalize, bin, wrange);
322
323 return gfit;
324}
325
326/* ---------------------------------------------------------------------------*/
339/* ---------------------------------------------------------------------------*/
340hdrl_xcorrelation_result * hdrl_compute_offset_gaussian_internal(
341 const cpl_array * arr1, const cpl_array * arr2,
342 const cpl_size half_win, const cpl_boolean normalize,
343 const double bin, const double wrange){
344
345 hdrl_xcorrelation_result * res =
346 hdrl_compute_xcorrelation(arr1, arr2, half_win, normalize);
347
348 if(!check_and_delete_if_bad(&res, CPL_FALSE)) return NULL;
349
350 cpl_error_code fail =
351 hdrl_compute_xcorrelation_refine(res, bin, wrange);
352
353 if(fail){
355 return NULL;
356 }
357
358 if(!check_and_delete_if_bad(&res, CPL_TRUE)) return NULL;
359
360 return res;
361}
362
363/* ---------------------------------------------------------------------------*/
373/* ---------------------------------------------------------------------------*/
374static inline cpl_error_code
375hdrl_compute_xcorrelation_refine(hdrl_xcorrelation_result* xcorr_res,
376 const double bin, const double wrange){
377
378 const cpl_array * xcorr = hdrl_xcorrelation_result_get_correlation(xcorr_res);
379 const cpl_size maxpos = hdrl_xcorrelation_result_get_peak_pixel(xcorr_res);
380
381 const cpl_size xcorr_size = cpl_array_get_size(xcorr);
382 const cpl_size pre_idx = CPL_MAX(0, maxpos - 1);
383 const cpl_size post_idx = CPL_MIN(maxpos + 1, xcorr_size - 1);
384
385 const double a = cpl_array_get(xcorr, pre_idx, NULL);
386 const double b = cpl_array_get(xcorr, post_idx, NULL);
387 const double c = cpl_array_get(xcorr, maxpos, NULL);
388 /* Find sub-pixel peak initial estimation. Use parabolic assumption */
389 const double fraction = (b - a) / ( 4. * c - 2. * a - 2. * b );
390 const double subpix_offset = maxpos - fraction;
391
392 xcorr_res->peakpos = subpix_offset * bin;
393 xcorr_res->sigma = bin * 10;
394 xcorr_res->area = 1.0;
395
396 cpl_size num_elems = 0;
397
398 cpl_vector * wavs_windowed = cpl_vector_new(xcorr_size);
399 cpl_vector * corr_windowed = cpl_vector_new(xcorr_size);
400
401 for(cpl_size i = 0; i < xcorr_size; ++i){
402 const double w = i * bin;
403 int rej = 0;
404 const double xcorr_data = cpl_array_get(xcorr, i, &rej);
405
406 if(rej || isnan(xcorr_data)) continue;
407 if(w < xcorr_res->peakpos - wrange
408 || w > xcorr_res->peakpos + wrange) continue;
409
410 cpl_vector_set(corr_windowed, num_elems, xcorr_data);
411 cpl_vector_set(wavs_windowed, num_elems, w);
412
413 num_elems++;
414 }
415
416 if(num_elems <= 0){
417 cpl_vector_delete(wavs_windowed);
418 cpl_vector_delete(corr_windowed);
419 cpl_ensure_code(CPL_FALSE, CPL_ERROR_ILLEGAL_OUTPUT);
420 }
421
422 cpl_vector_set_size(corr_windowed, num_elems);
423 cpl_vector_set_size(wavs_windowed, num_elems);
424
425 cpl_error_code code = cpl_vector_fit_gaussian(wavs_windowed, NULL,
426 corr_windowed, NULL, CPL_FIT_ALL, &xcorr_res->peakpos, &xcorr_res->sigma,
427 &xcorr_res->area, &xcorr_res->offset, &xcorr_res->mse, NULL, NULL);
428
429 /* if the fitting does not converge, CPL_ERROR_CONTINUE is set, the
430 * output parameters are calculated using a best-effort approach.
431 * See the documentation of cpl_vector_fit_gaussian .*/
432 if(code == CPL_ERROR_CONTINUE){
433 cpl_error_reset();
434 }
435
436 cpl_vector_delete(wavs_windowed);
437 cpl_vector_delete(corr_windowed);
438
439 return cpl_error_get_code();
440}
441
442static inline
443double calculate_xcorr_sample(const cpl_size shift,
444 const cpl_array * arr1, const cpl_array * arr2,
445 const double mean1, const double mean2,
446 const double stdev1, const double stdev2){
447
448 double d = 0.0;
449 cpl_size num_el = 0;
450
451 const double norm = 1.00 / sqrt(stdev1 * stdev2);
452
453 const cpl_size l1 = cpl_array_get_size(arr1);
454 const cpl_size l2 = cpl_array_get_size(arr2);
455
456 for(cpl_size i = 0; i < l2; i++){
457 const cpl_size j = i + shift;
458 int rej1 = 0, rej2 = 0;
459 if(j < 0) continue;
460 if(j >= l1) continue;
461
462 const double v1 = cpl_array_get(arr1, j, &rej1);
463 const double v2 = cpl_array_get(arr2, i, &rej2);
464
465 if(rej1 || rej2) continue;
466
467 const double val = norm * (v1 - mean1) * (v2 - mean2);
468
469 d += val;
470 num_el++;
471 }
472 return d / (double)num_el;
473}
474
475static inline
476mean_and_stdev calculate_mean_and_stdev(const cpl_array * arr1){
477
478 const double mean = cpl_array_get_mean(arr1);
479 const double stdev = cpl_array_get_stdev(arr1);
480
481 return (mean_and_stdev){mean, stdev};
482}
void hdrl_xcorrelation_result_delete(hdrl_xcorrelation_result *self)
Destructor for hdrl_xcorrelation_result.
hdrl_xcorrelation_result * hdrl_compute_xcorrelation(const cpl_array *arr1, const cpl_array *arr2, const cpl_size half_window, const cpl_boolean normalize)
Calculate cross-correlation.
const cpl_array * hdrl_xcorrelation_result_get_correlation(const hdrl_xcorrelation_result *self)
Getter for the cross correlation.
cpl_size hdrl_xcorrelation_result_get_peak_pixel(const hdrl_xcorrelation_result *self)
Get the index where the cross correlation reaches its maximum.
cpl_size hdrl_xcorrelation_result_get_half_window(const hdrl_xcorrelation_result *self)
Get the half_window used to calculate the cross-correlation.
hdrl_xcorrelation_result * hdrl_compute_offset_gaussian_internal(const cpl_array *arr1, const cpl_array *arr2, const cpl_size half_win, const cpl_boolean normalize, const double bin, const double wrange)
Calculate gaussian fit on cross-correlation.
hdrl_xcorrelation_result * hdrl_compute_offset_gaussian(const cpl_array *arr1, const cpl_array *arr2, const cpl_size half_win, const cpl_boolean normalize, const double bin, const double wrange)
Calculate gaussian fit on cross-correlation, does a second fitting for refinement.
double hdrl_xcorrelation_result_get_peak_subpixel(const hdrl_xcorrelation_result *self)
Get the index where the cross correlation reaches its maximum, with sub-pixel precision.
double hdrl_xcorrelation_result_get_sigma(const hdrl_xcorrelation_result *self)
Get the estimated standard deviation of the correlation.
hdrl_xcorrelation_result * hdrl_xcorrelation_result_wrap(cpl_array *x_corr, const cpl_size max_idx, const cpl_size half_window)
Constructor for hdrl_xcorrelation_result.
double fraction(double x, double y, double r_out)
Fraction of pixel bounded.