FCam: src/processing/Demosaic.cpp Source File

00001 #include <cmath>
00002 #include <iostream>
00003 #include <algorithm>
00004 #include <map>
00005 #include <vector>
00006 #include <string.h>
00007 #ifdef FCAM_ARCH_ARM
00008 #include "Demosaic_ARM.h"
00009 #endif
00010 
00011 #include <FCam/processing/Demosaic.h>
00012 #include <FCam/Sensor.h>
00013 #include <FCam/Time.h>
00014 
00015 
00016 namespace FCam {
00017 
00018     // Make a linear luminance -> pixel value lookup table
00019     void makeLUT(const Frame &f, float contrast, int blackLevel, float gamma, unsigned char *lut) {
00020         unsigned short minRaw = f.minRawValue()+blackLevel;
00021         unsigned short maxRaw = f.maxRawValue();
00022         
00023         for (int i = 0; i <= minRaw; i++) {
00024             lut[i] = 0;
00025         }
00026         
00027         float invRange = 1.0f/(maxRaw - minRaw);
00028         float b = 2 - powf(2.0f, contrast/100.0f);
00029         float a = 2 - 2*b; 
00030         for (int i = minRaw+1; i <= maxRaw; i++) {
00031             // Get a linear luminance in the range 0-1
00032             float y = (i-minRaw)*invRange;
00033             // Gamma correct it
00034             y = powf(y, 1.0f/gamma);
00035             // Apply a piecewise quadratic contrast curve
00036             if (y > 0.5) {
00037                 y = 1-y;
00038                 y = a*y*y + b*y;
00039                 y = 1-y;
00040             } else {
00041                 y = a*y*y + b*y;
00042             }
00043             // Convert to 8 bit and save
00044             y = std::floor(y * 255 + 0.5f);
00045             if (y < 0) y = 0;
00046             if (y > 255) y = 255;
00047             lut[i] = (unsigned char)y;
00048         }
00049         
00050         // add a guard band
00051         for (int i = maxRaw+1; i < 4096; i++) {
00052             lut[i] = 255;
00053         }
00054     }
00055 
00056     // Some functions used by demosaic
00057     inline short max(short a, short b) {return a>b ? a : b;}
00058     inline short max(short a, short b, short c, short d) {return max(max(a, b), max(c, d));}
00059     inline short min(short a, short b) {return a<b ? a : b;}
00060 
00061     Image demosaic(Frame src, float contrast, bool denoise, int blackLevel, float gamma) {
00062         if (!src.image().valid()) {
00063             error(Event::DemosaicError, "Cannot demosaic an invalid image");
00064             return Image();
00065         }
00066         if (src.image().bytesPerRow() % 2 == 1) {
00067             error(Event::DemosaicError, "Cannot demosaic an image with bytesPerRow not divisible by 2");
00068             return Image();
00069         }
00070        
00071         // We've vectorized this code for arm
00072         #ifdef FCAM_ARCH_ARM
00073         return demosaic_ARM(src, contrast, denoise, blackLevel, gamma);
00074         #endif
00075 
00076         Image input = src.image();
00077 
00078         // First check we're the right bayer pattern. If not crop and continue.
00079         switch((int)src.bayerPattern()) {
00080         case GRBG:
00081             break;
00082         case RGGB:
00083             input = input.subImage(1, 0, Size(input.width()-2, input.height()));
00084             break;
00085         case BGGR:
00086             input = input.subImage(0, 1, Size(input.width(), input.height()-2));
00087             break;
00088         case GBRG:
00089             input = input.subImage(1, 1, Size(input.width()-2, input.height()-2));
00090         default:
00091             error(Event::DemosaicError, "Can't demosaic from a non-bayer sensor\n");
00092             return Image();
00093         }
00094 
00095         const int BLOCK_WIDTH = 40;
00096         const int BLOCK_HEIGHT = 24;
00097         const int G = 0, GR = 0, R = 1, B = 2, GB = 3;        
00098 
00099         int rawWidth = input.width();
00100         int rawHeight = input.height();
00101         int outWidth = rawWidth-8;
00102         int outHeight = rawHeight-8;
00103         outWidth /= BLOCK_WIDTH;
00104         outWidth *= BLOCK_WIDTH;
00105         outHeight /= BLOCK_HEIGHT;
00106         outHeight *= BLOCK_HEIGHT;
00107 
00108         Image out(outWidth, outHeight, RGB24);               
00109 
00110         // Check we're the right size, if not, crop center
00111         if (((input.width() - 8) != (unsigned)outWidth) ||
00112             ((input.height() - 8) != (unsigned)outHeight)) { 
00113             int offX = (input.width() - 8 - outWidth)/2;
00114             int offY = (input.height() - 8 - outHeight)/2;
00115             offX -= offX&1;
00116             offY -= offY&1;
00117             
00118             if (offX || offY) {
00119                 input = input.subImage(offX, offY, Size(outWidth+8, outHeight+8));
00120             }
00121         }           
00122 
00123         // Prepare the lookup table
00124         unsigned char lut[4096];
00125         makeLUT(src, contrast, blackLevel, gamma, lut);
00126 
00127         // Grab the color matrix
00128         float colorMatrix[12];
00129         src.rawToRGBColorMatrix(colorMatrix);
00130 
00131         for (int by = 0; by < rawHeight-8-BLOCK_HEIGHT+1; by += BLOCK_HEIGHT) {
00132             for (int bx = 0; bx < rawWidth-8-BLOCK_WIDTH+1; bx += BLOCK_WIDTH) {
00133                 /*
00134                   Stage 1: Load a block of input, treat it as 4-channel gr, r, b, gb
00135                 */
00136                 short inBlock[4][BLOCK_HEIGHT/2+4][BLOCK_WIDTH/2+4];
00137 
00138                 for (int y = 0; y < BLOCK_HEIGHT/2+4; y++) {
00139                     for (int x = 0; x < BLOCK_WIDTH/2+4; x++) {
00140                         inBlock[GR][y][x] = ((short *)input(bx + 2*x, by + 2*y))[0];
00141                         inBlock[R][y][x] = ((short *)input(bx + 2*x+1, by + 2*y))[0];
00142                         inBlock[B][y][x] = ((short *)input(bx + 2*x, by + 2*y+1))[0];
00143                         inBlock[GB][y][x] = ((short *)input(bx + 2*x+1, by + 2*y+1))[0];
00144                     }
00145                 }
00146 
00147                 // linear luminance outputs
00148                 short linear[3][4][BLOCK_HEIGHT/2+4][BLOCK_WIDTH/2+4];
00149 
00150                 /*                  
00151 
00152                 Stage 1.5: Suppress hot pixels
00153 
00154                 gr[HERE] = min(gr[HERE], max(gr[UP], gr[LEFT], gr[RIGHT], gr[DOWN]));
00155                 r[HERE]  = min(r[HERE], max(r[UP], r[LEFT], r[RIGHT], r[DOWN]));
00156                 b[HERE]  = min(b[HERE], max(b[UP], b[LEFT], b[RIGHT], b[DOWN]));
00157                 gb[HERE] = min(gb[HERE], max(gb[UP], gb[LEFT], gb[RIGHT], gb[DOWN]));
00158  
00159                 */
00160 
00161                 if (denoise) {
00162                     for (int y = 1; y < BLOCK_HEIGHT/2+3; y++) {
00163                         for (int x = 1; x < BLOCK_WIDTH/2+3; x++) {
00164                             linear[G][GR][y][x] = min(inBlock[GR][y][x],
00165                                                       max(inBlock[GR][y-1][x],
00166                                                           inBlock[GR][y+1][x],
00167                                                           inBlock[GR][y][x+1],
00168                                                           inBlock[GR][y][x-1]));
00169                             linear[R][R][y][x] = min(inBlock[R][y][x],
00170                                                      max(inBlock[R][y-1][x],
00171                                                          inBlock[R][y+1][x],
00172                                                          inBlock[R][y][x+1],
00173                                                          inBlock[R][y][x-1]));
00174                             linear[B][B][y][x] = min(inBlock[B][y][x],
00175                                                      max(inBlock[B][y-1][x],
00176                                                          inBlock[B][y+1][x],
00177                                                          inBlock[B][y][x+1],
00178                                                          inBlock[B][y][x-1]));
00179                             linear[G][GB][y][x] = min(inBlock[GB][y][x],
00180                                                       max(inBlock[GB][y-1][x],
00181                                                           inBlock[GB][y+1][x],
00182                                                           inBlock[GB][y][x+1],
00183                                                           inBlock[GB][y][x-1]));
00184                         }
00185                     }
00186                 } else {
00187                     for (int y = 1; y < BLOCK_HEIGHT/2+3; y++) {
00188                         for (int x = 1; x < BLOCK_WIDTH/2+3; x++) {
00189                             linear[G][GR][y][x] = inBlock[GR][y][x];
00190                             linear[R][R][y][x] = inBlock[R][y][x];
00191                             linear[B][B][y][x] = inBlock[B][y][x];
00192                             linear[G][GB][y][x] = inBlock[GB][y][x];
00193                         }
00194                     }                    
00195                 }
00196                 
00197 
00198                 /*
00199                   2: Interpolate g at r 
00200                   
00201                   gv_r = (gb[UP] + gb[HERE])/2;
00202                   gvd_r = |gb[UP] - gb[HERE]|;
00203                   
00204                   gh_r = (gr[HERE] + gr[RIGHT])/2;
00205                   ghd_r = |gr[HERE] - gr[RIGHT]|;
00206                   
00207                   g_r = ghd_r < gvd_r ? gh_r : gv_r;
00208                   
00209                   3: Interpolate g at b
00210                   
00211                   gv_b = (gr[DOWN] + gr[HERE])/2;
00212                   gvd_b = |gr[DOWN] - gr[HERE]|;
00213                   
00214                   gh_b = (gb[LEFT] + gb[HERE])/2;
00215                   ghd_b = |gb[LEFT] - gb[HERE]|;
00216                   
00217                   g_b = ghd_b < gvd_b ? gh_b : gv_b;
00218 
00219                 */
00220 
00221                 for (int y = 1; y < BLOCK_HEIGHT/2+3; y++) {
00222                     for (int x = 1; x < BLOCK_WIDTH/2+3; x++) {
00223                         short gv_r = (linear[G][GB][y-1][x] + linear[G][GB][y][x])/2;
00224                         short gvd_r = abs(linear[G][GB][y-1][x] - linear[G][GB][y][x]);
00225                         short gh_r = (linear[G][GR][y][x] + linear[G][GR][y][x+1])/2;
00226                         short ghd_r = abs(linear[G][GR][y][x] - linear[G][GR][y][x+1]);
00227                         linear[G][R][y][x] = ghd_r < gvd_r ? gh_r : gv_r;
00228 
00229                         short gv_b = (linear[G][GR][y+1][x] + linear[G][GR][y][x])/2;
00230                         short gvd_b = abs(linear[G][GR][y+1][x] - linear[G][GR][y][x]);
00231                         short gh_b = (linear[G][GB][y][x] + linear[G][GB][y][x-1])/2;
00232                         short ghd_b = abs(linear[G][GB][y][x] - linear[G][GB][y][x-1]);
00233                         linear[G][B][y][x] = ghd_b < gvd_b ? gh_b : gv_b;                        
00234                     }
00235                 }
00236 
00237                 /*
00238                   4: Interpolate r at gr
00239                   
00240                   r_gr = (r[LEFT] + r[HERE])/2 + gr[HERE] - (g_r[LEFT] + g_r[HERE])/2;
00241                   
00242                   5: Interpolate b at gr
00243                   
00244                   b_gr = (b[UP] + b[HERE])/2 + gr[HERE] - (g_b[UP] + g_b[HERE])/2;
00245                   
00246                   6: Interpolate r at gb
00247                   
00248                   r_gb = (r[HERE] + r[DOWN])/2 + gb[HERE] - (g_r[HERE] + g_r[DOWN])/2;
00249                   
00250                   7: Interpolate b at gb
00251                   
00252                   b_gb = (b[HERE] + b[RIGHT])/2 + gb[HERE] - (g_b[HERE] + g_b[RIGHT])/2;
00253                 */
00254                 for (int y = 1; y < BLOCK_HEIGHT/2+3; y++) {
00255                     for (int x = 1; x < BLOCK_WIDTH/2+3; x++) {
00256                         linear[R][GR][y][x] = ((linear[R][R][y][x-1] + linear[R][R][y][x])/2 +
00257                                                linear[G][GR][y][x] - 
00258                                                (linear[G][R][y][x-1] + linear[G][R][y][x])/2);
00259 
00260                         linear[B][GR][y][x] = ((linear[B][B][y-1][x] + linear[B][B][y][x])/2 +
00261                                                linear[G][GR][y][x] - 
00262                                                (linear[G][B][y-1][x] + linear[G][B][y][x])/2);
00263 
00264                         linear[R][GB][y][x] = ((linear[R][R][y][x] + linear[R][R][y+1][x])/2 +
00265                                                linear[G][GB][y][x] - 
00266                                                (linear[G][R][y][x] + linear[G][R][y+1][x])/2);
00267 
00268                         linear[B][GB][y][x] = ((linear[B][B][y][x] + linear[B][B][y][x+1])/2 +
00269                                                linear[G][GB][y][x] - 
00270                                                (linear[G][B][y][x] + linear[G][B][y][x+1])/2);
00271 
00272                     }
00273                 }       
00274 
00275 
00276                 /*
00277                   
00278                 8: Interpolate r at b
00279                 
00280                 rp_b = (r[DOWNLEFT] + r[HERE])/2 + g_b[HERE] - (g_r[DOWNLEFT] + g_r[HERE])/2;
00281                 rn_b = (r[LEFT] + r[DOWN])/2 + g_b[HERE] - (g_r[LEFT] + g_r[DOWN])/2;
00282                 rpd_b = (r[DOWNLEFT] - r[HERE]);
00283                 rnd_b = (r[LEFT] - r[DOWN]);    
00284                 
00285                 r_b = rpd_b < rnd_b ? rp_b : rn_b;
00286                 
00287                 9: Interpolate b at r
00288                 
00289                 bp_r = (b[UPRIGHT] + b[HERE])/2 + g_r[HERE] - (g_b[UPRIGHT] + g_b[HERE])/2;
00290                 bn_r = (b[RIGHT] + b[UP])/2 + g_r[HERE] - (g_b[RIGHT] + g_b[UP])/2;     
00291                 bpd_r = |b[UPRIGHT] - b[HERE]|;
00292                 bnd_r = |b[RIGHT] - b[UP]|;     
00293                 
00294                 b_r = bpd_r < bnd_r ? bp_r : bn_r;             
00295                 
00296                 */
00297                 for (int y = 1; y < BLOCK_HEIGHT/2+3; y++) {
00298                     for (int x = 1; x < BLOCK_WIDTH/2+3; x++) {
00299                         short rp_b = ((linear[R][R][y+1][x-1] + linear[R][R][y][x])/2 +
00300                                       linear[G][B][y][x] - 
00301                                       (linear[G][R][y+1][x-1] + linear[G][R][y][x])/2);
00302                         short rpd_b = abs(linear[R][R][y+1][x-1] - linear[R][R][y][x]);
00303                         
00304                         short rn_b = ((linear[R][R][y][x-1] + linear[R][R][y+1][x])/2 +
00305                                       linear[G][B][y][x] - 
00306                                       (linear[G][R][y][x-1] + linear[G][R][y+1][x])/2);
00307                         short rnd_b = abs(linear[R][R][y][x-1] - linear[R][R][y+1][x]);
00308                         
00309                         linear[R][B][y][x] = rpd_b < rnd_b ? rp_b : rn_b;
00310 
00311                         short bp_r = ((linear[B][B][y-1][x+1] + linear[B][B][y][x])/2 +
00312                                       linear[G][R][y][x] - 
00313                                       (linear[G][B][y-1][x+1] + linear[G][B][y][x])/2);
00314                         short bpd_r = abs(linear[B][B][y-1][x+1] - linear[B][B][y][x]);
00315                         
00316                         short bn_r = ((linear[B][B][y][x+1] + linear[B][B][y-1][x])/2 +
00317                                       linear[G][R][y][x] - 
00318                                       (linear[G][B][y][x+1] + linear[G][B][y-1][x])/2);
00319                         short bnd_r = abs(linear[B][B][y][x+1] - linear[B][B][y-1][x]);
00320                         
00321                         linear[B][R][y][x] = bpd_r < bnd_r ? bp_r : bn_r;                       
00322                     }
00323                 }
00324 
00325                 /*
00326                   10: Color matrix
00327                     
00328                   11: Gamma correct
00329            
00330                 */
00331 
00332                 float r, g, b;
00333                 unsigned short ri, gi, bi;
00334                 for (int y = 2; y < BLOCK_HEIGHT/2+2; y++) {
00335                     for (int x = 2; x < BLOCK_WIDTH/2+2; x++) {
00336 
00337                         // Convert from sensor rgb to srgb
00338                         r = colorMatrix[0]*linear[R][GR][y][x] +
00339                             colorMatrix[1]*linear[G][GR][y][x] +
00340                             colorMatrix[2]*linear[B][GR][y][x] +
00341                             colorMatrix[3];
00342 
00343                         g = colorMatrix[4]*linear[R][GR][y][x] +
00344                             colorMatrix[5]*linear[G][GR][y][x] +
00345                             colorMatrix[6]*linear[B][GR][y][x] +
00346                             colorMatrix[7];
00347 
00348                         b = colorMatrix[8]*linear[R][GR][y][x] +
00349                             colorMatrix[9]*linear[G][GR][y][x] +
00350                             colorMatrix[10]*linear[B][GR][y][x] +
00351                             colorMatrix[11];
00352 
00353                         // Clamp
00354                         ri = r < 0 ? 0 : (r > 1023 ? 1023 : (unsigned short)(r+0.5f));
00355                         gi = g < 0 ? 0 : (g > 1023 ? 1023 : (unsigned short)(g+0.5f));
00356                         bi = b < 0 ? 0 : (b > 1023 ? 1023 : (unsigned short)(b+0.5f));
00357                        
00358                         // Gamma correct and store
00359                         out(bx+(x-2)*2, by+(y-2)*2)[0] = lut[ri];
00360                         out(bx+(x-2)*2, by+(y-2)*2)[1] = lut[gi];
00361                         out(bx+(x-2)*2, by+(y-2)*2)[2] = lut[bi];
00362 
00363                         // Convert from sensor rgb to srgb
00364                         r = colorMatrix[0]*linear[R][R][y][x] +
00365                             colorMatrix[1]*linear[G][R][y][x] +
00366                             colorMatrix[2]*linear[B][R][y][x] +
00367                             colorMatrix[3];
00368 
00369                         g = colorMatrix[4]*linear[R][R][y][x] +
00370                             colorMatrix[5]*linear[G][R][y][x] +
00371                             colorMatrix[6]*linear[B][R][y][x] +
00372                             colorMatrix[7];
00373 
00374                         b = colorMatrix[8]*linear[R][R][y][x] +
00375                             colorMatrix[9]*linear[G][R][y][x] +
00376                             colorMatrix[10]*linear[B][R][y][x] +
00377                             colorMatrix[11];
00378 
00379                         // Clamp
00380                         ri = r < 0 ? 0 : (r > 1023 ? 1023 : (unsigned short)(r+0.5f));
00381                         gi = g < 0 ? 0 : (g > 1023 ? 1023 : (unsigned short)(g+0.5f));
00382                         bi = b < 0 ? 0 : (b > 1023 ? 1023 : (unsigned short)(b+0.5f));
00383                         
00384                         // Gamma correct and store
00385                         out(bx+(x-2)*2+1, by+(y-2)*2)[0] = lut[ri];
00386                         out(bx+(x-2)*2+1, by+(y-2)*2)[1] = lut[gi];
00387                         out(bx+(x-2)*2+1, by+(y-2)*2)[2] = lut[bi];
00388                         
00389                         // Convert from sensor rgb to srgb
00390                         r = colorMatrix[0]*linear[R][B][y][x] +
00391                             colorMatrix[1]*linear[G][B][y][x] +
00392                             colorMatrix[2]*linear[B][B][y][x] +
00393                             colorMatrix[3];
00394 
00395                         g = colorMatrix[4]*linear[R][B][y][x] +
00396                             colorMatrix[5]*linear[G][B][y][x] +
00397                             colorMatrix[6]*linear[B][B][y][x] +
00398                             colorMatrix[7];
00399 
00400                         b = colorMatrix[8]*linear[R][B][y][x] +
00401                             colorMatrix[9]*linear[G][B][y][x] +
00402                             colorMatrix[10]*linear[B][B][y][x] +
00403                             colorMatrix[11];
00404 
00405                         // Clamp
00406                         ri = r < 0 ? 0 : (r > 1023 ? 1023 : (unsigned short)(r+0.5f));
00407                         gi = g < 0 ? 0 : (g > 1023 ? 1023 : (unsigned short)(g+0.5f));
00408                         bi = b < 0 ? 0 : (b > 1023 ? 1023 : (unsigned short)(b+0.5f));
00409                        
00410                         // Gamma correct and store
00411                         out(bx+(x-2)*2, by+(y-2)*2+1)[0] = lut[ri];
00412                         out(bx+(x-2)*2, by+(y-2)*2+1)[1] = lut[gi];
00413                         out(bx+(x-2)*2, by+(y-2)*2+1)[2] = lut[bi];
00414                         
00415                         // Convert from sensor rgb to srgb
00416                         r = colorMatrix[0]*linear[R][GB][y][x] +
00417                             colorMatrix[1]*linear[G][GB][y][x] +
00418                             colorMatrix[2]*linear[B][GB][y][x] +
00419                             colorMatrix[3];
00420 
00421                         g = colorMatrix[4]*linear[R][GB][y][x] +
00422                             colorMatrix[5]*linear[G][GB][y][x] +
00423                             colorMatrix[6]*linear[B][GB][y][x] +
00424                             colorMatrix[7];
00425 
00426                         b = colorMatrix[8]*linear[R][GB][y][x] +
00427                             colorMatrix[9]*linear[G][GB][y][x] +
00428                             colorMatrix[10]*linear[B][GB][y][x] +
00429                             colorMatrix[11];
00430 
00431                         // Clamp
00432                         ri = r < 0 ? 0 : (r > 1023 ? 1023 : (unsigned short)(r+0.5f));
00433                         gi = g < 0 ? 0 : (g > 1023 ? 1023 : (unsigned short)(g+0.5f));
00434                         bi = b < 0 ? 0 : (b > 1023 ? 1023 : (unsigned short)(b+0.5f));
00435                        
00436                         // Gamma correct and store
00437                         out(bx+(x-2)*2+1, by+(y-2)*2+1)[0] = lut[ri];
00438                         out(bx+(x-2)*2+1, by+(y-2)*2+1)[1] = lut[gi];
00439                         out(bx+(x-2)*2+1, by+(y-2)*2+1)[2] = lut[bi];
00440                         
00441                     }
00442                 }                
00443             }
00444         }
00445 
00446         return out;
00447     }
00448 
00449     // Generic RAW to thumbnail converter
00450     Image makeThumbnailRAW(Frame src, const Size &thumbSize, float contrast, int blackLevel, float gamma) {
00451         
00452         // Special-case the N900's common case for speed (5 MP GRGB -> 640x480 RGB24 on ARM)
00453         #ifdef FCAM_ARCH_ARM
00454         if (src.image().width() == 2592 &&
00455             src.image().height() == 1968 &&
00456             thumbSize.width == 640 &&
00457             thumbSize.height == 480 &&
00458             src.bayerPattern() == GRBG) {
00459             return makeThumbnailRAW_ARM(src, contrast, blackLevel, gamma);
00460         }
00461         #endif
00462 
00463         // Make the response curve
00464         unsigned char lut[4096];
00465         makeLUT(src, contrast, blackLevel, gamma, lut);
00466 
00467         Image thumb;
00469         bool redRowEven, blueRowGreenPixelEven;
00470         switch (src.bayerPattern()) {
00471         case RGGB:
00472             redRowEven = true;
00473             blueRowGreenPixelEven = true;
00474             break;
00475         case BGGR:
00476             redRowEven = false;
00477             blueRowGreenPixelEven = false;
00478             break;
00479         case GRBG:
00480             redRowEven = true;
00481             blueRowGreenPixelEven = false;
00482             break;
00483         case GBRG:
00484             redRowEven = false;
00485             blueRowGreenPixelEven = true;
00486             break;
00487         default:
00488             return thumb;
00489             break;
00490         }
00491 
00492         Time startTime = Time::now();
00493 
00494         thumb = Image(thumbSize, RGB24);
00495 
00496         unsigned int w = src.image().width();
00497         unsigned int h = src.image().height();
00498         unsigned int tw = thumbSize.width;
00499         unsigned int th = thumbSize.height;
00500         short maxPixel = 1023;
00501         short minPixel = 0;
00502         unsigned int scaleX = (int)std::floor((float)w / tw);
00503         unsigned int scaleY = (int)std::floor((float)h / th);
00504         unsigned int scale = std::min(scaleX, scaleY); // Maintain aspect ratio
00505         
00506         int cropX = (w-scale*tw)/2;
00507         if (cropX % 2 == 1) cropX--; // Ensure we're at start of 2x2 block
00508         int cropY = (h-scale*th)/2;
00509         if (cropY % 2 == 1) cropY--; // Ensure we're at start of 2x2 block
00510 
00511         float colorMatrix[12];
00512         src.rawToRGBColorMatrix(colorMatrix);
00513 
00514         /* A fast downsampling/demosaicing - average down color
00515            channels over the whole source pixel block under a single
00516            thumbnail pixel, and just use those colors directly as the
00517            pixel colors. */        
00518 
00519         for (unsigned int ty=0; ty < th; ty++) {
00520             unsigned char *tpix = thumb(0,ty); // Get a pointer to the beginning of the row
00521             for (unsigned int tx=0; tx < tw; tx++) {
00522                 unsigned int r=0,g=0,b=0;
00523                 unsigned int rc=0,gc=0,bc=0;
00524 
00525                 unsigned char *rowStart = src.image()(cropX + tx*scale, cropY + ty*scale);
00526 
00527                 bool isRedRow = ((ty*scale % 2 == 0) == redRowEven);
00528 
00529                 for(unsigned int i=0; i<scale; i++, rowStart+=src.image().bytesPerRow(), isRedRow = !isRedRow) {                    
00530                     unsigned short *px = (unsigned short *)rowStart;
00531                     if (isRedRow) {
00532                         bool isRed=((tx*scale)%2==0) == blueRowGreenPixelEven;
00533                         for (unsigned int j=0; j < scale; j++, isRed=!isRed) {
00534                             if (isRed) {
00535                                 r += *(px++);
00536                                 rc++;
00537                             } else {
00538                                 g += *(px++);
00539                                 gc++;
00540                             }
00541                         }
00542                     } else {
00543                         bool isGreen=((tx*scale)%2==0) == blueRowGreenPixelEven;
00544                         for (unsigned int j=0; j < scale; j++, isGreen=!isGreen) {
00545                             if (isGreen) {
00546                                 g += *(px++);
00547                                 gc++;
00548                             } else {
00549                                 b += *(px++);
00550                                 bc++;
00551                             }
00552                             
00553                         }
00554                     }
00555                 }
00556                 float r_sensor = ((float)r / rc);
00557                 float g_sensor = ((float)g / gc);
00558                 float b_sensor = ((float)b / bc);
00559                 float r_srgb = (r_sensor * colorMatrix[0] +
00560                                 g_sensor * colorMatrix[1] +
00561                                 b_sensor * colorMatrix[2] +
00562                                 colorMatrix[3]);
00563                 float g_srgb = (r_sensor * colorMatrix[4] +
00564                                 g_sensor * colorMatrix[5] +
00565                                 b_sensor * colorMatrix[6] +
00566                                 colorMatrix[7]);
00567                 float b_srgb = (r_sensor * colorMatrix[8] +
00568                                 g_sensor * colorMatrix[9] +
00569                                 b_sensor * colorMatrix[10] +
00570                                 colorMatrix[11]);
00571                 unsigned short r_linear = std::min(maxPixel,std::max(minPixel,(short int)std::floor(r_srgb+0.5)));
00572                 unsigned short g_linear = std::min(maxPixel,std::max(minPixel,(short int)std::floor(g_srgb+0.5)));
00573                 unsigned short b_linear = std::min(maxPixel,std::max(minPixel,(short int)std::floor(b_srgb+0.5)));
00574                 *(tpix++) = lut[r_linear];
00575                 *(tpix++) = lut[g_linear];
00576                 *(tpix++) = lut[b_linear];
00577             }
00578         }
00579 
00580         //std::cout << "Done creating thumbnail. time = " << ((Time::now()-startTime)/1000) << std::endl;
00581 
00582         return thumb;
00583 
00584     }
00585 
00586     Image makeThumbnail(Frame src, const Size &thumbSize, float contrast, int blackLevel, float gamma) {
00587         Image thumb;
00588 
00589         // Sanity checks
00590         if (not src.image().valid()) return thumb;
00591         if (thumbSize.width == 0 or thumbSize.height == 0) return thumb;
00592 
00593         switch (src.image().type()) {
00594         case RAW:
00595             thumb = makeThumbnailRAW(src, thumbSize, contrast, blackLevel, gamma);
00596             break;
00597         case RGB24:
00598         case RGB16:
00599         case UYVY:
00600         case YUV24:
00601         case UNKNOWN:
00602         default:
00604             // These formats can't be thumbnailed yet
00605             break;
00606         }
00607         return thumb;
00608     }
00609 
00610 }