"Fossies" - the Fresh Open Source Software Archive 
Member "pfstools-2.2.0/src/camera/robertson02.cpp" (12 Aug 2021, 17927 Bytes) of package /linux/privat/pfstools-2.2.0.tgz:
As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style:
standard) with prefixed line numbers and
code folding option.
Alternatively you can here
view or
download the uninterpreted source code file.
1 /**
2 * @brief Robertson02 algorithm for automatic self-calibration.
3 *
4 *
5 * This file is a part of PFS CALIBRATION package.
6 * ----------------------------------------------------------------------
7 * Copyright (C) 2004 Grzegorz Krawczyk
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 * ----------------------------------------------------------------------
23 *
24 * @author Grzegorz Krawczyk, <gkrawczyk@users.sourceforge.net>
25 * Ivo Ihrke , <ihrke@mmci.uni-saarland.de>
26 *
27 * $Id: robertson02.cpp,v 1.11 2011/02/25 13:45:14 ihrke Exp $
28 */
29
30
31 #include <config.h>
32
33 #include <iostream>
34 #include <vector>
35 #include <cstdlib>
36
37 #include <math.h>
38
39 #include <responses.h>
40 #include <robertson02.h>
41
42 #include <iostream>
43 #include <cstring>
44
45 using namespace std;
46
47
48 #define PROG_NAME "robertson02"
49
50 // maximum iterations after algorithm accepts local minima
51 #define MAXIT 100
52
53 // maximum accepted error
54 #define MAX_DELTA 1e-5f
55
56 extern bool verbose; /* verbose should be declared for each standalone code */
57
58 float normalize_rcurve( float *rcurve, int M );
59
60 inline float max3( float a[3])
61 {
62 float max = (a[0]>a[1]) ? a[0] : a[1];
63 return (a[2]>max) ? a[2] : max;
64 }
65
66 float get_exposure_compensation( const Exposure &ex )
67 {
68 return ex.exposure_time * ex.aperture*ex.aperture * ex.iso/3.125f / (1.0592f * 11.4f / 3.125f);
69 }
70
71 int robertson02_applyResponseRGB( pfs::Array2D *rgb_out[],
72 const ExposureList *imgs[],
73 const float *resp_curve[],
74 const float *weights,
75 int M,
76 const noise_parameters *camera,
77 const float deghosting_threshold)
78 {
79 // number of exposures
80 int N = imgs[0]->size();
81
82 // frame size
83 int width = rgb_out[0] -> getCols( );
84 int height = rgb_out[0] -> getRows( );
85
86 // number of saturated pixels
87 int saturated_pixels = 0;
88
89 float mmax[3]= {1e-30, 1e-30, 1e-30};
90
91 /*
92 float pw[M];
93 for( int i = 0 ; i < M ; i++ ) {
94 pw[i] = weights[i];
95 }
96 for( int i = 1; i < 10; i++ )
97 pw[M-i] = 0;
98
99 float ew[N]; // Per-exposure weight to account for noise charactetistic
100
101 // For each exposure
102 for( int i = 0 ; i < N ; i++ ) {
103 const Exposure &ex = (*imgs[0])[i];
104
105 // TODO: Estimate read-out noise and use for exposure weighting
106 // TODO: Better blending if difference is weights is high
107
108 // This is a noise-optimial reconstruction assuming that the input is affected only
109 // by the photon noise (no read-out noise), which is most often the case
110 ew[i] = ex.exposure_time;
111
112 VERBOSE_STR << "Ex: " << i << " exp. time: " << ex.exposure_time << "\t iso: " << ex.iso
113 << "\t aperture: " << ex.aperture << "\t weight: " << ew[i] << std::endl;
114 }
115 */
116
117 bool photon_noise_only = false;
118 if( strcmp( camera->camera_name, "Empty" ) == 0 )
119 {
120 photon_noise_only = true;
121 VERBOSE_STR << "Merging using the Poisson Photon Noise Estimator" << endl;
122 }
123 else
124 VERBOSE_STR << "Merging using the Variance Weighted Estimator" << endl;
125
126 // For each pixel
127 int skipped_deghost = 0;
128 for( int j = 0; j < width * height; j++ )
129 {
130 bool saturatedRGB[3] = { false, false, false };
131
132 // First compute scene radiances, weights and saturated exposures
133 float X[3][N];
134 float w[3][N];
135 bool saturated_exp[N];
136 fill(saturated_exp, saturated_exp + N, false);
137 bool skip_exp[N];
138 fill(skip_exp, skip_exp + N, false);
139 float t[N];
140 float g[N];
141
142 for( int i = 0; i < N; i++ )
143 for( int cc = 0; cc < 3; cc++ )
144 {
145 const Exposure &ex = (*imgs[cc])[i];
146 t[i] = ex.exposure_time;
147 g[i] = ex.iso / 100;
148
149 int m = (int) (*ex.yi)( j );
150 X[cc][i] = resp_curve[cc][ m ] / ( t[i] * g[i] * camera->color_coefficients[cc] );
151 if( photon_noise_only )
152 w[cc][i] = t[i];
153 else
154 {
155 float var = resp_curve[cc][ m ] * g[i] * camera->color_coefficients[cc]
156 + powf(camera->std_readout * g[i] * camera->color_coefficients[cc], 2)
157 + powf(camera->std_adc * camera->color_coefficients[cc], 2);
158 w[cc][i] = powf( t[i] * g[i] * camera->color_coefficients[cc], 2 ) / var;
159 }
160
161 // Test for overexposed pixels
162 saturated_exp[i] |= resp_curve[cc][m] >= resp_curve[cc][M-1];
163 }
164
165 // Deghosting - find the exposure that could be used as a reference
166 // Currently using middle exposure
167 // TODO: Perform histogram analysis to pick best reference
168 int ref = int(N/2);
169 if( deghosting_threshold != -1 )
170 {
171 // Compute the number of pixels to be skipped due to deghosting
172 for( int i = 0; i < N; i++ )
173 if(i != ref && !saturated_exp[ref] && !saturated_exp[i])
174 {
175 // First compute the Pearson correlation coefficient wrt reference
176 float ref_mean = 0, ref_var = 1e-5, cur_mean = 0, cur_var = 1e-5, cov = 0;
177 for( int cc = 0; cc < 3; cc++ )
178 {
179 ref_mean += X[cc][ref];
180 cur_mean += X[cc][i];
181 }
182 ref_mean /= 3;
183 cur_mean /= 3;
184 for( int cc = 0; cc < 3; cc++ )
185 {
186 ref_var += powf(X[cc][ref] - ref_mean, 2);
187 cur_var += powf(X[cc][i] - cur_mean, 2);
188 cov += (X[cc][ref] - ref_mean)*(X[cc][i] - cur_mean);
189 }
190
191 // Next check whether total deviation from the reference is acceptable
192 float std_sum = 0;
193 float deviation_from_ref = 0;
194 for( int cc = 0; cc < 3; cc++ )
195 {
196 // Reciprocal of the weight is the exposure compensated variance
197 // Square root of this is thus the exposure compensated standard deviation
198 // Assuming a gaussian distribution, 95% of the population should lie 2 stds away from mean
199 std_sum += deghosting_threshold * sqrt(1/w[cc][ref] + 1/w[cc][i]);
200 deviation_from_ref += fabs( (float)(X[cc][ref] - X[cc][i]) );
201 }
202
203 // Skipping the Pearson coefficient test as reults are not satisfactory
204 // Fixing a better reference might solve this
205 // float rho = cov / sqrt(ref_var/2 * cur_var/2);
206 // skip_exp[i] = rho < 0.9 || deviation_from_ref > std_sum;
207 skip_exp[i] = deviation_from_ref > std_sum;
208 if( skip_exp[i] )
209 skipped_deghost++;
210 }
211 }
212
213 /* // Deghosting
214 if( ref != -1 && !saturated_exp[ref] ) {
215
216 const Exposure &ex = (*imgs[0])[ref];
217 float ti = get_exposure_compensation( ex );
218
219 float Y_ref = (X[ref*3] + X[ref*3+1] + X[ref*3+2])/3.f;
220 const float noise_level = 0.01;
221 float var_ref = (noise_level*noise_level)*Y_ref/ti; // variance of the photon noise
222
223 for( int i = 0 ; i < N ; i++ )
224 {
225
226 const Exposure &ex = (*imgs[0])[i];
227 float ti = get_exposure_compensation( ex );
228
229 if( saturated_exp[i] ) {
230 // skip_exp[i] = true;
231 continue;
232 }
233
234 //continue; // Skip deghostig for now
235
236 float rho = corr_vec3( &X[i*3], &X[ref*3] );
237 // float chroma = std::max( std::max( X[i*3], X[i*3+1] ), X[i*3+2] ) - std::min( std::min( X[i*3], X[i*3+1] ), X[i*3+2] );
238 // cerr << rho << " - " << chroma << ", ";
239 // cerr << i << "(" << saturated_exp[i] << "): " << rho << endl;
240
241 float Y_exp = (X[i*3] + X[i*3+1] + X[i*3+2])/3.f;
242 float var_exp = (noise_level*noise_level)*Y_exp / ti;
243
244 float ci = 1.96 * sqrt(var_ref+var_exp);
245
246 // Skip a frame if color correlation too low or difference to reference too high
247 skip_exp[i] = ( rho < 0.9 ) || fabs( Y_exp - Y_ref ) > ci;
248 if( skip_exp[i] )
249 skipped_deghost++;
250 }
251 }
252
253
254 int skip_bits = 0;
255
256 for( int i = 0 ; i < N ; i++ ) {
257
258 if( !skip_exp[i] )
259 skip_bits++; // |= (1<<i);
260 }
261
262
263
264 for( int i = 0 ; i < N ; i++ ) {
265 skip_exp[i] = false;
266 }
267 */
268
269
270 // cerr << endl;
271
272 // For each color channels
273 for( int cc=0; cc < 3; cc++ )
274 {
275
276 // all exposures for each pixel
277 float sum = 0.0f;
278 float div = 0.0f;
279
280 // For each exposure
281 for( int i = 0 ; i < N ; i++ )
282 {
283 if( saturated_exp[i] || (deghosting_threshold != -1 && skip_exp[i]) )
284 continue;
285
286 sum += X[cc][i] * w[cc][i];
287 div += w[cc][i];
288
289 // if( j < 10 && cc == 1 )
290 // std::cerr << "j = " << j << " ;i = " << i << " ;w = " << w[cc][i] << std::endl;
291 }
292
293 if( div >= 1e-15 )
294 {
295 (*rgb_out[cc])(j) = sum / div;
296 mmax[cc] = ( mmax[cc] > (*rgb_out[cc])(j) ) ? mmax[cc] : (*rgb_out[cc])(j);
297 }
298 else
299 {
300 (*rgb_out[cc])(j) = -1;
301 saturatedRGB[cc] = true;
302 }
303 }
304
305
306 if( saturatedRGB[0] || saturatedRGB[1] || saturatedRGB[2] ) {
307 saturated_pixels++;
308 }
309
310 /* for( int cc=0; cc < 3; cc++ ) {
311 // TODO: Some fancy stuff to deal with distorted colors
312
313 if( saturatedRGB[cc] ) {
314 // If none of the exposures can give actual pixel value, make
315 // the best (clipped) estimate using the longest or the
316 // shortest exposure;
317
318 const Exposure &ex = (*imgs[cc])[0];
319 // If pixel value > gray level, use the shortest exposure;
320 float short_long = ( (*ex.yi)(j) > M/2 ) ? 1.f : -1.f;
321
322 float best_ti = 1e10f * short_long;
323 int best_v = short_long == 1.f ? M-1 : 0;
324 for( int i = 0 ; i < N ; i++ )
325 {
326 if( deghosting && skip_exp[i] )
327 continue;
328
329 const Exposure &ex = (*imgs[cc])[i];
330 int m = (int) (*ex.yi)( j );
331 float ti = get_exposure_compensation( ex );;
332 if( ti*short_long < best_ti*short_long ) {
333 best_ti = ti;
334 best_v = (int)(*ex.yi)(j);
335 }
336 }
337 (*rgb_out[cc])(j) = 1/best_ti * resp_curve[cc][best_v];
338 }
339
340 }*/
341
342
343 }
344
345 // Fill in nan values and normalise
346 for( int j = 0; j < width * height; j++ )
347 for( int cc=0; cc < 3; cc++ )
348 {
349 if( (*rgb_out[cc])(j) == -1)
350 (*rgb_out[cc])(j) = mmax[cc];
351 (*rgb_out[cc])(j) /= max3(mmax);
352 }
353
354 VERBOSE_STR << "Exposure pixels skipped due to deghosting: " <<
355 (float)skipped_deghost*100.f/(float)(width*height*N) << "%" << std::endl;
356
357 return saturated_pixels;
358
359 }
360
361
362
363 int robertson02_applyResponse( pfs::Array2D *xj,
364 const ExposureList &imgs,
365 const float *rcurve,
366 const float *weights,
367 int M )
368 {
369
370 // number of exposures
371 int N = imgs.size();
372
373 // frame size
374 int width = xj -> getCols( );
375 int height = xj -> getRows( );
376
377 // number of saturated pixels
378 int saturated_pixels = 0;
379
380 // cerr << "M = " << M << endl;
381 // cerr << "W[0] = " << weights[0] << endl;
382 // cerr << "W[end] = " << weights[M-1] << endl;
383
384 // all pixels
385 for( int j = 0; j < width * height; j++ )
386 {
387 // all exposures for each pixel
388 float sum = 0.0f;
389 float div = 0.0f;
390
391 for( int i = 0 ; i < N ; i++ )
392 {
393 int m = (int) (*imgs[ i ].yi)( j );
394 float ti = imgs[ i ].ti;
395
396 sum += weights [ m ] / ti * rcurve [ m ];
397 div += weights [ m ];
398 }
399
400
401 /* if( div != 0.0f )
402 (*xj)( j ) = sum / div;
403 else
404 (*xj)( j ) = 0.0f;*/
405
406 if( div >= 0.01f )
407 (*xj)( j ) = sum / div;
408 else
409 (*xj)( j ) = 0;
410
411 /*
412 {
413 float best_ti = 1e10;
414 int best_v = M-1;
415 for( int i = 0 ; i < N ; i++ )
416 {
417 int m = (int) (*imgs[ i ].yi)( j );
418 float ti = imgs[ i ].ti;
419 if( ti < best_ti ) {
420 best_ti = ti;
421 best_v = (int)(*imgs[i].yi)(j);
422 }
423 }
424 (*xj)( j ) = (M-1) / best_ti * rcurve [best_v];
425
426 }*/
427
428
429
430 }
431
432 return saturated_pixels;
433
434 }
435
436
437 int robertson02_getResponse( pfs::Array2D *xj,
438 const ExposureList &imgs,
439 float *rcurve,
440 const float *weights,
441 int M )
442 {
443
444 // number of exposures
445 int N = imgs.size();
446
447 // frame size
448 int width = imgs[0].yi -> getCols( );
449 int height = imgs[0].yi -> getRows( );
450
451 // number of saturated pixels
452 int saturated_pixels = 0;
453
454 // indices
455 int i, j, m;
456
457 float* rcurve_prev = new float[ M ]; // previous response
458
459 if( rcurve_prev == NULL )
460 {
461 std::cerr << "robertson02: could not allocate memory for camera response" << std::endl;
462 exit(1);
463 }
464
465 // 0. Initialization
466
467 normalize_rcurve( rcurve, M );
468
469 for( m = 0 ; m < M ; m++ ) {
470 // cerr << "m = " << m << " rc = " << rcurve [ m ] << endl;
471 rcurve_prev [ m ] = rcurve [ m ];
472 }
473
474 robertson02_applyResponse( xj, imgs, rcurve, weights, M );
475
476 // Optimization process
477 bool converged = false;
478 long *cardEm = new long [ M ];
479 float *sum = new float[ M ];
480
481 if( sum == NULL ||
482 cardEm == NULL )
483 {
484 std::cerr << "robertson02: could not allocate memory for optimization process" << std::endl;
485 exit(1);
486 }
487
488 int cur_it = 0;
489 float pdelta = 0.0f;
490
491 while( !converged )
492 {
493
494 // Display response curve - for debugging purposes
495 /* for( m = 0 ; m < M ; m+=32 ) {
496 cerr << "m = " << m << " rc = " << rcurve [ m ] << endl;
497 }*/
498
499 // 1. Minimize with respect to rcurve
500 for( m = 0 ; m < M ; m++ )
501 {
502 cardEm [ m ] = 0;
503 sum[ m ] = 0.0f;
504 }
505
506 // For each exposure
507 for( i = 0 ; i < N ; i++ )
508 {
509
510 pfs::Array2D* yi = imgs[i].yi;
511 float ti = imgs[ i ].ti;
512
513 for( j = 0 ; j < width * height ; j++ )
514 {
515 m = (int) (*yi)( j );
516
517 if( m < M && m >= 0 )
518 {
519 sum[ m ] += ti * (*xj)( j );
520 cardEm[ m ] ++;
521 }
522 else
523 std::cerr << "robertson02: m out of range: " << m << std::endl;
524 }
525 }
526
527
528 for( m = 0; m < M ; m++ )
529 {
530 if( cardEm[ m ] != 0 )
531 rcurve [ m ] = sum [ m ] / cardEm [ m ];
532 else
533 rcurve [ m ] = 0.0f;
534 }
535
536 // 2. Normalize rcurve
537 float middle_response = normalize_rcurve( rcurve, M );
538
539 // 3. Apply new response
540 saturated_pixels = robertson02_applyResponse( xj, imgs, rcurve, weights, M );
541
542 // 4. Check stopping condition
543 float delta = 0.0f;
544 int hits = 0;
545
546 for( m = 0 ; m < M ; m++ )
547 {
548 if( rcurve[ m ] != 0.0f )
549 {
550 float diff = rcurve [ m ] - rcurve_prev [ m ];
551
552 delta += diff * diff;
553
554 rcurve_prev [ m ] = rcurve[ m ];
555
556 hits++;
557 }
558 }
559
560 delta /= hits;
561
562 VERBOSE_STR << " #" << cur_it
563 << " delta=" << delta
564 << " (coverage: " << 100*hits/M << "%)\n";
565
566 if( delta < MAX_DELTA )
567 converged = true;
568 else if( isnan(delta) || cur_it > MAXIT )
569 {
570 VERBOSE_STR << "algorithm failed to converge, too noisy data in range\n";
571 break;
572 }
573
574 pdelta = delta;
575 cur_it++;
576 }
577
578 if( converged )
579 VERBOSE_STR << " #" << cur_it
580 << " delta=" << pdelta << " <- converged\n";
581
582 delete[] rcurve_prev;
583 delete[] cardEm;
584 delete[] sum;
585
586 return saturated_pixels;
587 }
588
589
590 //----------------------------------------------------------
591 // private part
592
593
594 int comp_floats( const void *a, const void *b )
595 {
596 return ( (*((float*)a))< (*((float*)b)) ) ;
597 }
598
599 float normalize_rcurve( float* rcurve, int M )
600 {
601 int FILTER_SIZE = M / 256;
602 float mean;
603 float rcurve_filt [ M ];
604 float to_sort [ 2 * FILTER_SIZE + 1 ];
605
606 mean = 0.f;
607 for ( int i = 0; i < M; i++ )
608 {
609 mean += rcurve [ i ];
610 }
611 mean /= M;
612
613 if( mean != 0.0f )
614 for( int m = 0 ; m < M ; m++ )
615 {
616 rcurve [ m ] /= mean;
617
618 /* filtered curve - initialization */
619 rcurve_filt [ m ] = 0.0f;
620 }
621
622 /* median filter response curve */
623 for ( int m = FILTER_SIZE ; m < M - FILTER_SIZE; m++ )
624 {
625 for ( int i = -FILTER_SIZE; i <= FILTER_SIZE; i++ )
626 to_sort [ i + FILTER_SIZE ] = rcurve[ m + i ];
627
628 qsort ( to_sort, 2 * FILTER_SIZE + 1 , sizeof(float), comp_floats );
629
630 rcurve_filt [ m ] = to_sort [ FILTER_SIZE ];
631
632 }
633
634 /* boundaries */
635 for( int m = 0 ; m < FILTER_SIZE ; m++ )
636 {
637 rcurve_filt [ m ] = rcurve_filt [ FILTER_SIZE ];
638 rcurve_filt [ M - m - 1 ] = rcurve_filt [ M - FILTER_SIZE - 1 ];
639 }
640
641 /* copy curve */
642 for( int m = 0 ; m < M ; m++ )
643 {
644 rcurve [ m ] = rcurve_filt [ m ];
645 }
646
647 return mean;
648 }
649
650
651 /*float normalize_rcurve_old( float* rcurve, int M )
652 {
653
654 int Mmin, Mmax;
655 // find min max
656 for( Mmin=0 ; Mmin<M && rcurve[Mmin]==0 ; Mmin++ );
657 for( Mmax=M-1 ; Mmax>0 && rcurve[Mmax]==0 ; Mmax-- );
658
659 int Mmid = Mmin+(Mmax-Mmin)/2;
660 float mid = rcurve[Mmid];
661
662 // std::cerr << "robertson02: middle response, mid=" << mid
663 // << " [" << Mmid << "]"
664 // << " " << Mmin << ".." << Mmax << std::endl;
665
666 if( mid==0.0f )
667 {
668 // find first non-zero middle response
669 while( Mmid<Mmax && rcurve[Mmid]==0.0f )
670 Mmid++;
671 mid = rcurve[Mmid];
672 }
673
674 if( mid!=0.0f )
675 for( int m=0 ; m<M ; m++ )
676 rcurve[m] /= mid;
677 return mid;
678 }
679 */