"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.
For more information about "robertson02.cpp" see the
Fossies "Dox" file reference documentation and the latest
Fossies "Diffs" side-by-side code changes report:
2.1.0_vs_2.2.0.
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 */