27#include "hdrl_correlation.h"
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);
50mean_and_stdev calculate_mean_and_stdev(
const cpl_array * arr1);
52static inline cpl_error_code
53hdrl_compute_xcorrelation_refine(hdrl_xcorrelation_result* xcorr_res,
54 const double bin,
const double wrange);
78hdrl_xcorrelation_result *
80 const cpl_size half_window){
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);
87 hdrl_xcorrelation_result * to_ret = cpl_calloc(1,
sizeof(*to_ret));
89 to_ret->xcorr = x_corr;
90 to_ret->pix_peakpos = max_idx;
91 to_ret->half_window = half_window;
104 if(self == NULL)
return;
106 cpl_array_delete(self->xcorr);
122 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
124 return self->pix_peakpos;
139 (
const hdrl_xcorrelation_result * self){
141 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
143 return self->peakpos;
156 (
const hdrl_xcorrelation_result * self){
158 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
160 return self->half_window;
175 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, 0);
192 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
212 const cpl_array * arr1,
const cpl_array * arr2,
213 const cpl_size half_window,
const cpl_boolean normalize){
215 cpl_ensure(half_window > 1, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);
217 const mean_and_stdev not_normalized = {0.0, 1.0};
219 cpl_ensure(arr1 != NULL && arr2 != NULL, CPL_ERROR_NULL_INPUT, NULL);
221 cpl_size xcorr_length = 2 * half_window + 1;
222 cpl_array * corr = cpl_array_new(xcorr_length, HDRL_TYPE_DATA);
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;
229 cpl_size max_idx = -1;
230 double max_corr = 0.0;
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,
236 const cpl_size idx = i + half_window;
237 cpl_array_set(corr, idx, cr);
239 if(isnan(cr))
continue;
241 if(cr >= max_corr || max_idx < 0){
251cpl_error_code check_if_bad(
const hdrl_xcorrelation_result * gfit,
252 const cpl_boolean check_refine){
254 cpl_ensure_code(gfit != NULL, CPL_ERROR_ILLEGAL_OUTPUT);
256 CPL_ERROR_ILLEGAL_OUTPUT);
261 cpl_ensure_code(px >= 0.0 && !isnan(px), CPL_ERROR_ILLEGAL_OUTPUT);
264 cpl_ensure_code( sigma > 0.0 && !isnan(sigma),CPL_ERROR_ILLEGAL_OUTPUT);
267 return CPL_ERROR_NONE;
271cpl_boolean check_and_delete_if_bad(hdrl_xcorrelation_result ** gfit_arg,
272 const cpl_boolean check_refine){
274 cpl_error_code fail = check_if_bad(*gfit_arg, check_refine);
281 return fail ? CPL_FALSE : CPL_TRUE;
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){
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);
309 hdrl_xcorrelation_result * gfit =
311 normalize, bin, wrange);
313 cpl_ensure(gfit != NULL, CPL_ERROR_ILLEGAL_OUTPUT, NULL);
315 const cpl_size half_win2 =
316 (cpl_size)(3. * CPL_MATH_FWHM_SIG * gfit->sigma / bin);
321 normalize, bin, wrange);
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){
345 hdrl_xcorrelation_result * res =
348 if(!check_and_delete_if_bad(&res, CPL_FALSE))
return NULL;
350 cpl_error_code fail =
351 hdrl_compute_xcorrelation_refine(res, bin, wrange);
358 if(!check_and_delete_if_bad(&res, CPL_TRUE))
return NULL;
374static inline cpl_error_code
375hdrl_compute_xcorrelation_refine(hdrl_xcorrelation_result* xcorr_res,
376 const double bin,
const double wrange){
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);
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);
389 const double fraction = (b - a) / ( 4. * c - 2. * a - 2. * b );
390 const double subpix_offset = maxpos -
fraction;
392 xcorr_res->peakpos = subpix_offset * bin;
393 xcorr_res->sigma = bin * 10;
394 xcorr_res->area = 1.0;
396 cpl_size num_elems = 0;
398 cpl_vector * wavs_windowed = cpl_vector_new(xcorr_size);
399 cpl_vector * corr_windowed = cpl_vector_new(xcorr_size);
401 for(cpl_size i = 0; i < xcorr_size; ++i){
402 const double w = i * bin;
404 const double xcorr_data = cpl_array_get(xcorr, i, &rej);
406 if(rej || isnan(xcorr_data))
continue;
407 if(w < xcorr_res->peakpos - wrange
408 || w > xcorr_res->peakpos + wrange)
continue;
410 cpl_vector_set(corr_windowed, num_elems, xcorr_data);
411 cpl_vector_set(wavs_windowed, num_elems, w);
417 cpl_vector_delete(wavs_windowed);
418 cpl_vector_delete(corr_windowed);
419 cpl_ensure_code(CPL_FALSE, CPL_ERROR_ILLEGAL_OUTPUT);
422 cpl_vector_set_size(corr_windowed, num_elems);
423 cpl_vector_set_size(wavs_windowed, num_elems);
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);
432 if(code == CPL_ERROR_CONTINUE){
436 cpl_vector_delete(wavs_windowed);
437 cpl_vector_delete(corr_windowed);
439 return cpl_error_get_code();
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){
451 const double norm = 1.00 / sqrt(stdev1 * stdev2);
453 const cpl_size l1 = cpl_array_get_size(arr1);
454 const cpl_size l2 = cpl_array_get_size(arr2);
456 for(cpl_size i = 0; i < l2; i++){
457 const cpl_size j = i + shift;
458 int rej1 = 0, rej2 = 0;
460 if(j >= l1)
continue;
462 const double v1 = cpl_array_get(arr1, j, &rej1);
463 const double v2 = cpl_array_get(arr2, i, &rej2);
465 if(rej1 || rej2)
continue;
467 const double val = norm * (v1 - mean1) * (v2 - mean2);
472 return d / (double)num_el;
476mean_and_stdev calculate_mean_and_stdev(
const cpl_array * arr1){
478 const double mean = cpl_array_get_mean(arr1);
479 const double stdev = cpl_array_get_stdev(arr1);
481 return (mean_and_stdev){mean, stdev};
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.