processing/Demosaic.cpp

#include <math.h>
#include <vector>
#include <iostream>
using namespace std;

#include "Demosaic.h"
#include "../Sensor.h"


namespace FCam {
    Image demosaic(Frame::Ptr frame, float gamma) {
    if (!frame || !frame->image.valid()) {
        cerr << "Cannot demosaic an invalid image" << endl;
        Image nullImage;
        return nullImage;
    }
    
    cout << "Demosaicking" << endl;
    // use bilinear demosaicking for now
    int rawWidth = frame->image.size.width;
    int rawHeight = frame->image.size.height;
    Image out(rawWidth-4, rawHeight-4, RGB24);

    int minVal = frame->sensor->minRawValue();
    int maxVal = frame->sensor->maxRawValue();

    // make the 8 bit LUT 
    vector<unsigned char> to8bit(maxVal+100);
    for (int i = 0; i < maxVal; i++) {
        float val = (i-minVal);
        if (val < 0) to8bit[i] = 0;
        else {
        val /= maxVal;
        if (val >= 1) to8bit[i] = 255;
        else {
            val = powf(val, 1.0f/gamma);
            to8bit[i] = (unsigned char)(val * 256 + 0.5);
        }
        }
    }

    // what's the address of the top left red pixel?
    int redX, redY;
    switch(frame->sensor->getBayerPattern()) {
    case Sensor::RGGB:
        redX = 0; redY = 0; break;
    case Sensor::BGGR:
        redX = 1; redY = 1; break;
    case Sensor::GBRG:
        redX = 0; redY = 1; break;
    case Sensor::GRBG:
        redX = 1; redY = 0; break;
    default:
        cerr << "Can't demosaic data from a non-bayer sensor." << endl;
        return out;
    }

    unsigned char *outPtr = out.data;
    for (int y = 0; y < out.size.height; y++) {
        unsigned short *inPtr = ((unsigned short *)frame->image.data) + (y+2)*rawWidth + 2;
        for (int x = 0; x < out.size.width; x++) {

        if ((y & 1) == redY) {
            if ((x & 1) == redX) {
            // red pixel
            *outPtr++ = to8bit[inPtr[0]];
            *outPtr++ = to8bit[(inPtr[-1] + 
                         inPtr[1] + 
                         inPtr[-rawWidth] + 
                         inPtr[rawWidth])>>2];
            *outPtr++ = to8bit[(inPtr[-1-rawWidth] +
                         inPtr[1+rawWidth] + 
                         inPtr[1-rawWidth] +
                         inPtr[-1+rawWidth])>>2];
            } else {
            // green pixel in red row
            *outPtr++ = to8bit[(inPtr[-1] + inPtr[1])>>1];
            *outPtr++ = to8bit[inPtr[0]];
            *outPtr++ = to8bit[(inPtr[-rawWidth] + inPtr[rawWidth])>>1];
            }
        } else {
            if ((x & 1) == redX) {
            // green pixel in blue row
            *outPtr++ = to8bit[(inPtr[-rawWidth] + inPtr[rawWidth])>>1];
            *outPtr++ = to8bit[inPtr[0]];
            *outPtr++ = to8bit[(inPtr[-1] + inPtr[1])>>1];
            } else {
            // blue pixel
            *outPtr++ = to8bit[(inPtr[-1-rawWidth] +
                         inPtr[1+rawWidth] + 
                         inPtr[1-rawWidth] +
                         inPtr[-1+rawWidth])>>2];
            *outPtr++ = to8bit[(inPtr[-1] + 
                         inPtr[1] + 
                         inPtr[-rawWidth] + 
                         inPtr[rawWidth])>>2];
            *outPtr++ = to8bit[inPtr[0]];
            }
        }

        inPtr++;
        }
    }

    cout << "Done demosaicking" << endl;

    return out;
    }
}

---