bayer.cpp

机器人SLAM方面的

/*
  ktracker (c) 2006 Kris Beevers

  This file is part of ktracker.

  ktracker is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  ktracker is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with ktracker; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

  $Id: bayer.cpp,v 1.1.1.1 2006/10/10 20:41:29 beevek Exp $
*/

// decode a Bayer image with pattern RGGB to BGR24.  this is a
// simplified version of coriander's edge sensing interpolation
// method, from the following reference:
// Edge Sensing Interpolation II from http://www-ise.stanford.edu/~tingchen/
//   (Laroche,Claude A.  "Apparatus and method for adaptively
//   interpolating a full color image utilizing chrominance gradients"
//   U.S. Patent 5,373,322)

// the source was modified from:
// http://libdc1394.cvs.sourceforge.net/libdc1394/libdc1394/libdc1394/dc1394_bayer.c?hideattic=0&revision=1.1.2.10&view=markup

#include <inttypes.h>
#include <stdlib.h>

#define CLIP(in, out)\
   in = in < 0 ? 0 : in;\
   in = in > 255 ? 255 : in;\
   out=in;

void clear_borders(uint8_t *bgr, int sx, int sy, int w)
{
  int i, j;
  // black edges are added with a width w:
  i = 3 * sx * w - 1;
  j = 3 * sx * sy - 1;
  while (i >= 0) {
    bgr[i--] = 0;
    bgr[j--] = 0;
  }
  i = sx * (sy - 1) * 3 - 1 + w * 3;
  while (i > sx) {
    j = 6 * w;
    while (j > 0) {
      bgr[i--] = 0;
      j--;
    }
    i -= (sx - 2 * w) * 3;
  }
}

void dragonfly_bayer2bgr(const uint8_t *bayer, uint8_t *bgr, uint32_t sx, uint32_t sy)
{
  uint8_t *outR, *outG, *outB;
  register uint32_t i3, j3, base;
  uint32_t i, j;
  int dh, dv;
  int tmp;
  uint32_t sx3=sx*3;

  outR = &bgr[0];
  outG = &bgr[1];
  outB = &bgr[2];

  // copy original RGB data to output images
  for (i = 0, i3=0; i < sy*sx; i += (sx<<1), i3 += (sx3<<1)) {
    for (j = 0, j3=0; j < sx; j += 2, j3+=6) {
      base=i3+j3;
      outB[base] = bayer[i + j];
      outR[base + sx3 + 3] = bayer[i + sx + (j + 1)];
      outG[base + 3] = bayer[i + j + 1];
      outG[base + sx3] = bayer[i + sx + j];
    }
  }

  // process GREEN channel
  for (i3=2*sx3; i3 < (sy - 2)*sx3; i3 += (sx3<<1)) {
    for (j3=6; j3 < sx3 - 9; j3+=6) {
      base=i3+j3;
      dh = abs(((outB[base - 6] +
		 outB[base + 6]) >> 1) -
	       outB[base]);
      dv = abs(((outB[base - (sx3<<1)] +
		 outB[base + (sx3<<1)]) >> 1) -
	       outB[base]);
      tmp = (((outG[base - 3]   + outG[base + 3]) >> 1) * (dh<=dv) +
	     ((outG[base - sx3] + outG[base + sx3]) >> 1) * (dh>dv));
      //tmp = (dh==dv) ? tmp>>1 : tmp;
      CLIP(tmp, outG[base]);
    }
  }
  for (i3=3*sx3; i3 < (sy - 3)*sx3; i3 += (sx3<<1)) {
    for (j3=9; j3 < sx3 - 6; j3+=6) {
      base=i3+j3;
      dh = abs(((outR[base - 6] +
		 outR[base + 6]) >> 1) -
	       outR[base]);
      dv = abs(((outR[base - (sx3<<1)] +
		 outR[base + (sx3<<1)]) >> 1) -
	       outR[base]);
      tmp = (((outG[base - 3]   + outG[base + 3]) >> 1) * (dh<=dv) +
	     ((outG[base - sx3] + outG[base + sx3]) >> 1) * (dh>dv));
      //tmp = (dh==dv) ? tmp>>1 : tmp;
      CLIP(tmp, outG[base]);
    }
  }
  // process RED channel
  for (i3=sx3; i3 < (sy - 1)*sx3; i3 += (sx3<<1)) {	// G-points (1/2)
    for (j3=6; j3 < sx3 - 3; j3+=6) {
      base=i3+j3;
      tmp = outG[base] +
	((outR[base - 3] -
	  outG[base - 3] +
	  outR[base + 3] -
	  outG[base + 3]) >>1);
      CLIP(tmp, outR[base]);
    }
  }
  for (i3=2*sx3; i3 < (sy - 2)*sx3; i3 += (sx3<<1)) {
    for (j3=3; j3 < sx3; j3+=6) {	// G-points (2/2)
      base=i3+j3;
      tmp = outG[base] +
	((outR[base - sx3] -
	  outG[base - sx3] +
	  outR[base + sx3] -
	  outG[base + sx3]) >> 1);
      CLIP(tmp, outR[base]);
    }
    for (j3=6; j3 < sx3 - 3; j3+=6) {	// B-points
      base=i3+j3;
      tmp = outG[base] +
	((outR[base - sx3 - 3] -
	  outG[base - sx3 - 3] +
	  outR[base - sx3 + 3] -
	  outG[base - sx3 + 3] +
	  outR[base + sx3 - 3] -
	  outG[base + sx3 - 3] +
	  outR[base + sx3 + 3] -
	  outG[base + sx3 + 3]) >> 2);
      CLIP(tmp, outR[base]);
    }
  }
  
  // process BLUE channel
  for (i = 0,i3=0; i < sy*sx; i += (sx<<1), i3 += (sx3<<1)) {
    for (j = 1, j3=3; j < sx - 2; j += 2, j3+=6) {
      base=i3+j3;
      tmp = outG[base] +
	((outB[base - 3] -
	  outG[base - 3] +
	  outB[base + 3] -
	  outG[base + 3]) >> 1);
      CLIP(tmp, outB[base]);
    }
  }
  for (i3=sx3; i3 < (sy - 1)*sx3; i3 += (sx3<<1)) {
    for (j3=0; j3 < sx3 - 3; j3+=6) {
      base=i3+j3;
      tmp = outG[base] +
	((outB[base - sx3] -
	  outG[base - sx3] +
	  outB[base + sx3] -
	  outG[base + sx3]) >> 1);
      CLIP(tmp, outB[base]);
    }
    for (j3=3; j3 < sx3 - 6; j3+=6) {
      base=i3+j3;
      tmp = outG[base] +
	((outB[base - sx3 - 3] -
	  outG[base - sx3 - 3] +
	  outB[base - sx3 + 3] -
	  outG[base - sx3 + 3] +
	  outB[base + sx3 - 3] -
	  outG[base + sx3 - 3] +
	  outB[base + sx3 + 3] -
	  outG[base + sx3 + 3]) >> 2);
      CLIP(tmp, outB[base]);
    }
  }
  
  clear_borders(bgr, sx, sy, 3);
}