openpilot_comma/selfdrive/camerad/cameras/real_debayer.cl

const __constant float3 color_correction[3] = {
  // Matrix from WBraw -> sRGBD65 (normalized)
  (float3)(1.17898387, -0.19583185, -0.19881648),
  (float3)(-0.03367879,  1.33692858, -0.02475203),
  (float3)(-0.14530508, -0.14109673,  1.22356851),
};
// wait for 24 color calib
//(float3)(0.44832666,  0.0163177 , -0.00434611),
//  (float3)(0.51599514,  1.0778389 , -0.32716517),
//  (float3)(0.0356782 , -0.09415659,  1.33151128),
const __constant float3 df = (float3)(0.5, 0.5, 0.5);

float3 color_correct(float r, float g, float b) {
  float3 ret = (0,0,0);

  // white balance of daylight
  // x /= (float3)(0.4609375, 1.0, 0.546875);
  // x /= (float3)(1.0, 1.0, 1.0);
  //r = max(0.0, min(1.0, x));
  // fix up the colors
  ret += r * color_correction[0];
  ret += g * color_correction[1];
  ret += b * color_correction[2];
  ret = max(0.0, min(1.0, ret));
  return ret;
}

uint int_from_10(const uchar * source, uint start, uint offset) {
  // source: source
  // start: starting address of 0
  // offset: 0 - 3
  uint major = (uint)source[start + offset] << 2;
  uint minor = (source[start + 4] >> (2 * offset)) & 3;
  return major + minor;
}

float to_normal(uint x) {  
  float pv = (float)(x);
  const float black_level = 0;
  pv = max(0.0, pv - black_level);
  pv /= (1024.0f - black_level);
  pv = 1.6*pv / (1.0f + 1.6*pv); // reinhard
  return pv;
}

__kernel void debayer10(const __global uchar * in,
                        __global uchar * out,
                        __local float * cached
                       )
{
  const int x_global = get_global_id(0);
  const int y_global = get_global_id(1);

  // const int globalOffset = ;

  const int localRowLen = 2 + get_local_size(0); // 2 padding
  const int x_local = get_local_id(0);
  const int y_local = get_local_id(1);

  const int localOffset = (y_local + 1) * localRowLen + x_local + 1;

  // cache local pixels first
  // saves memory access and avoids repeated normalization
  uint globalStart_10 = y_global * FRAME_STRIDE + (5 * (x_global / 4));
  uint offset_10 = x_global % 4;
  uint raw_val = int_from_10(in, globalStart_10, offset_10);
  cached[localOffset] = to_normal(raw_val);

  // edges
  if (x_global < 1 || x_global > RGB_WIDTH - 2 || y_global < 1 || y_global > RGB_WIDTH - 2) {
    barrier(CLK_LOCAL_MEM_FENCE);
    return;
  } else {
    int localColOffset = -1;
    int globalColOffset = -1;

    // cache padding
    if (x_local < 1) {
      localColOffset = x_local;
      globalColOffset = -1;
      cached[(y_local + 1) * localRowLen + x_local] = to_normal(int_from_10(in, y_global * FRAME_STRIDE + (5 * ((x_global-1) / 4)), (offset_10 + 3) % 4));
    } else if (x_local >= get_local_size(0) - 1) {
      localColOffset = x_local + 2;
      globalColOffset = 1;
      cached[localOffset + 1] = to_normal(int_from_10(in, y_global * FRAME_STRIDE + (5 * ((x_global+1) / 4)), (offset_10 + 1) % 4));
    }

    if (y_local < 1) {
      cached[y_local * localRowLen + x_local + 1] = to_normal(int_from_10(in, globalStart_10 - FRAME_STRIDE, offset_10));
      if (localColOffset != -1) {
        cached[y_local * localRowLen + localColOffset] = to_normal(int_from_10(in, (y_global-1) * FRAME_STRIDE + (5 * ((x_global+globalColOffset) / 4)), (offset_10+4+globalColOffset) % 4));
      }
    } else if (y_local >= get_local_size(1) - 1) {
      cached[(y_local + 2) * localRowLen + x_local + 1] = to_normal(int_from_10(in, globalStart_10 + FRAME_STRIDE, offset_10));
      if (localColOffset != -1) {
        cached[(y_local + 2) * localRowLen + localColOffset] = to_normal(int_from_10(in, (y_global+1) * FRAME_STRIDE + (5 * ((x_global+globalColOffset) / 4)), (offset_10+4+globalColOffset) % 4));
      }
    }

    // sync
    barrier(CLK_LOCAL_MEM_FENCE);

    // perform debayer
    float r;
    float g;
    float b;

    if (x_global % 2 == 0) {
      if (y_global % 2 == 0) { // G1
        r = (cached[localOffset - 1] + cached[localOffset + 1]) / 2.0f;
        g = (cached[localOffset] + cached[localOffset + localRowLen + 1]) / 2.0f;
        b = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen]) / 2.0f;
      } else { // B
        r = (cached[localOffset - localRowLen - 1] + cached[localOffset - localRowLen + 1] + cached[localOffset + localRowLen - 1] + cached[localOffset + localRowLen + 1]) / 4.0f;
        g = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen] + cached[localOffset - 1] + cached[localOffset + 1]) / 4.0f;
        b = cached[localOffset];
      }
    } else {
      if (y_global % 2 == 0) { // R
        r = cached[localOffset];
        g = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen] + cached[localOffset - 1] + cached[localOffset + 1]) / 4.0f;
        b = (cached[localOffset - localRowLen - 1] + cached[localOffset - localRowLen + 1] + cached[localOffset + localRowLen - 1] + cached[localOffset + localRowLen + 1]) / 4.0f;
      } else { // G2
        r = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen]) / 2.0f;
        g = (cached[localOffset] + cached[localOffset - localRowLen - 1]) / 2.0f;
        b = (cached[localOffset - 1] + cached[localOffset + 1]) / 2.0f;
      }
    }

    float3 rgb = color_correct(r, g, b);
    // rgb = srgb_gamma(rgb); 

    // BGR output
    out[3 * x_global + 3 * y_global * RGB_WIDTH + 0] = (uchar)(255.0f * rgb.z);
    out[3 * x_global + 3 * y_global * RGB_WIDTH + 1] = (uchar)(255.0f * rgb.y);
    out[3 * x_global + 3 * y_global * RGB_WIDTH + 2] = (uchar)(255.0f * rgb.x);
  }
}