/** Copy part of the U anv V planes of a YUV422planar image to another
*/
void
yuv422planar_copy_uv(const unsigned char *src, unsigned char *dst,
unsigned int width, unsigned int height,
unsigned int x, unsigned int y,
unsigned int copy_width, unsigned int copy_height)
{
const unsigned char *sup = YUV422_PLANAR_U_PLANE(src, width, height) + (x / 2);
const unsigned char *svp = YUV422_PLANAR_V_PLANE(src, width, height) + (x / 2);
unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, width, height) + (x / 2);
unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, width, height) + (x / 2);
unsigned int w;
unsigned int h;
unsigned const char *lsup = sup, *lsvp = svp, *ldup = dup, *ldvp = dvp;
for (h = 0; h < copy_height; ++h) {
for ( w = 0; w < copy_width; w += 2 ) {
*dup++ = *sup++;
*dvp++ = *svp++;
}
lsup += width / 2;
lsvp += width / 2;
ldup += width / 2;
ldvp += width / 2;
}
}
void
LossyScaler::scale()
{
if ( orig_width == 0 ) return;
if ( orig_height == 0 ) return;
if ( scal_width == 0 ) return;
if ( scal_height == 0 ) return;
if ( orig_buffer == NULL ) return;
if ( scal_buffer == NULL ) return;
if ( scal_width < needed_scaled_width() ) return;
if ( scal_height < needed_scaled_height() ) return;
float skip = 1 / scale_factor;
unsigned char *oyp = orig_buffer;
unsigned char *oup = YUV422_PLANAR_U_PLANE( orig_buffer, orig_width, orig_height );
unsigned char *ovp = YUV422_PLANAR_V_PLANE( orig_buffer, orig_width, orig_height );
unsigned char *syp = scal_buffer;
unsigned char *sup = YUV422_PLANAR_U_PLANE( scal_buffer, scal_width, scal_height );
unsigned char *svp = YUV422_PLANAR_V_PLANE( scal_buffer, scal_width, scal_height );
memset( syp, 0, scal_width * scal_height );
memset( sup, 128, scal_width * scal_height );
float oh_float = 0.0;
float ow_float = 0.0;
unsigned int oh_pixel;
unsigned int ow_pixel;
unsigned int ow_pixel_next;
for (unsigned int h = 0; h < scal_height; ++h) {
oh_pixel = (unsigned int) rint(oh_float);
ow_float = 0.0;
if (oh_pixel >= orig_height) {
oh_pixel = orig_height - 1;
}
for (unsigned int w = 0; w < scal_width; w += 2) {
ow_pixel = (unsigned int) rint(ow_float);
ow_pixel_next = (unsigned int) rint( ow_float + skip);
if (ow_pixel >= orig_width) {
ow_pixel = orig_width - 1;
}
if (ow_pixel_next >= orig_width) {
ow_pixel_next = orig_width - 1;
}
syp[ h * scal_width + w ] = oyp[ oh_pixel * orig_width + ow_pixel ];
syp[ h * scal_width + w + 1 ] = oyp[ oh_pixel * orig_width + ow_pixel_next ];
sup[ (h * scal_width + w) / 2 ] = (oup[ (oh_pixel * orig_width + ow_pixel) / 2 ] + oup[ (oh_pixel * orig_width + ow_pixel_next) / 2 ]) / 2;
svp[ (h * scal_width + w) / 2 ] = (ovp[ (oh_pixel * orig_width + ow_pixel) / 2 ] + ovp[ (oh_pixel * orig_width + ow_pixel_next) / 2 ]) / 2;
ow_float += 2 * skip;
}
oh_float += skip;
}
}
/** Copy part of the U anv V planes of a YUV422planar image to another
*/
void
yuv420planar_to_yuv422planar(const unsigned char *src, unsigned char *dst,
unsigned int width, unsigned int height)
{
const unsigned char *sup = YUV420_PLANAR_U_PLANE(src, width, height);
const unsigned char *svp = YUV420_PLANAR_V_PLANE(src, width, height);
unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, width, height);
unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, width, height);
unsigned int h;
// cp Y plane
memcpy(dst, src, width * height);
for (h = 0; h < height / 2; ++h) {
// cp U line twice!
memcpy(dup, sup, width / 2);
dup += width / 2;
memcpy(dup, sup, width / 2);
dup += width / 2;
sup += width / 2;
// cp V line twice!
memcpy(dvp, svp, width / 2);
dvp += width / 2;
memcpy(dvp, svp, width / 2);
dvp += width / 2;
svp += width / 2;
}
}
/** Get mass point of primary color.
* @param roi ROI to consider
* @param massPoint contains mass point upon return
*/
void
SimpleColorClassifier::get_mass_point_of_color( ROI *roi,
fawkes::upoint_t *massPoint )
{
unsigned int nrOfOrangePixels;
nrOfOrangePixels = 0;
massPoint->x = 0;
massPoint->y = 0;
// for accessing ROI pixels
register unsigned int h = 0;
register unsigned int w = 0;
// planes
register unsigned char *yp = _src + (roi->start.y * roi->line_step) + (roi->start.x * roi->pixel_step);
register unsigned char *up = YUV422_PLANAR_U_PLANE(_src, roi->image_width, roi->image_height)
+ ((roi->start.y * roi->line_step) / 2 + (roi->start.x * roi->pixel_step) / 2) ;
register unsigned char *vp = YUV422_PLANAR_V_PLANE(_src, roi->image_width, roi->image_height)
+ ((roi->start.y * roi->line_step) / 2 + (roi->start.x * roi->pixel_step) / 2);
// line starts
unsigned char *lyp = yp;
unsigned char *lup = up;
unsigned char *lvp = vp;
color_t dcolor;
// consider each ROI pixel
for (h = 0; h < roi->height; ++h) {
for (w = 0; w < roi->width; w += 2) {
// classify its color
dcolor = color_model->determine(*yp++, *up++, *vp++);
yp++;
// ball pixel?
if (color == dcolor) {
// take into account its coordinates
massPoint->x += w;
massPoint->y += h;
nrOfOrangePixels++;
}
}
// next line
lyp += roi->line_step;
lup += roi->line_step / 2;
lvp += roi->line_step / 2;
yp = lyp;
up = lup;
vp = lvp;
}
// to obtain mass point, divide by number of pixels that were added up
massPoint->x = (unsigned int) (float(massPoint->x) / float(nrOfOrangePixels));
massPoint->y = (unsigned int) (float(massPoint->y) / float(nrOfOrangePixels));
/* shift mass point
such that it is relative to image
(not relative to ROI) */
massPoint->x += roi->start.x;
massPoint->y += roi->start.y;
}
/** 8-Bit gray to YUV422_PLANAR
*/
void
gray8_to_yuv422planar_plainc(const unsigned char *src, unsigned char *dst,
unsigned int width, unsigned int height)
{
// copy Y plane
memcpy(dst, src, width * height);
// set U and V plane
memset(YUV422_PLANAR_U_PLANE(dst, width, height), 128, width * height);
}
/* Convert quarter YUV422 planar buffer to plain YUV422 planar.
* @param quarter input buffer in YUV422_PLANAR_QUARTER
* @param output buffer in YUV422_PLANAR
* @param width width of the image (width of YUV422_PLANAR image)
* @param height height of the image (height of YUV422_PLANAR image)
*/
void
yuv422planar_quarter_to_yuv422planar(const unsigned char *quarter,
unsigned char *planar,
const unsigned int width,
const unsigned int height)
{
volatile const unsigned char *y, *u, *v;
unsigned int w, h;
const unsigned int w_h_4 = (width * height) / 4;
const unsigned int w_h_8 = (width * height) / 8;
//const unsigned int w_t_2 = width * 2;
const unsigned int w_b_2 = width / 2;
const unsigned int w_b_4 = width / 4;
unsigned char *yp, *up, *vp, t;
yp = planar;
up = YUV422_PLANAR_U_PLANE(planar, width, height);
vp = YUV422_PLANAR_V_PLANE(planar, width, height);
for (h = 0; h < height / 2; ++h) {
y = quarter + (h * w_b_2);
u = quarter + w_h_4 + (h * w_b_4);
v = quarter + w_h_4 + w_h_8 + (h * w_b_4);
for (w = 0; w < w_b_4; ++w) {
t = *y++;
*yp++ = t;
*yp++ = t;
t = *y++;
*yp++ = t;
*yp++ = t;
t = *u++;
*up++ = t;
*up++ = t;
t = *v++;
*vp++ = t;
*vp++ = t;
}
memcpy(yp, yp - width, width);
memcpy(up, up - w_b_2, w_b_2);
memcpy(vp, vp - w_b_2, w_b_2);
yp += width;
up += w_b_2;
vp += w_b_2;
}
}
void
bayerGBRG_to_yuv422planar_nearest_neighbour(const unsigned char *bayer, unsigned char *yuv,
unsigned int width, unsigned int height)
{
unsigned char *y = yuv;
unsigned char *u = YUV422_PLANAR_U_PLANE(yuv, width, height);
unsigned char *v = YUV422_PLANAR_V_PLANE(yuv, width, height);
const unsigned char *b = bayer;
int y1, u1, v1, y2, u2, v2;
int t1, t2;
for ( unsigned int h = 0; h < height; h += 2) {
// g b ... line
for (unsigned int w = 0; w < width; w += 2) {
t1 = b[width];
t2 = b[1];
RGB2YUV(t1, *b, t2, y1, u1, v1);
++b;
t1 = b[width - 1];
t2 = b[-1];
RGB2YUV(t1, t2, *b, y2, u2, v2);
++b;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// r g ... line
for (unsigned int w = 0; w < width; w += 2) {
t1 = b[1];
t2 = b[-width+1];
RGB2YUV(*b, t1, t2, y1, u1, v1);
++b;
t1 = b[-1];
t2 = b[-width];
RGB2YUV(t1, *b, t2, y2, u2, v2);
++b;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
}
}
/** Create image from LUT.
* Create image from LUT, useful for debugging and analysing.
* This method produces a representation of the given level
* (Y range with 0 <= level < depth) for visual inspection of the colormap.
* The dimensions of the resulting image are 512x512 pixels. It uses standard strong
* colors for the different standard color classes. C_UNKNOWN is grey, C_BACKGROUND
* is black (like C_BLACK).
* If the standard method does not suit your needs you can override this method.
* @param yuv422_planar_buffer contains the image upon return, must be initialized
* with the appropriate memory size before calling, dimensions are 512x512 pixels.
* @param level the level to draw, it's a range in the Y direction and is in the
* range 0 <= level < depth.
*/
void
Colormap::to_image(unsigned char *yuv422_planar_buffer, unsigned int level)
{
unsigned int iwidth = image_width() / 2;
unsigned int iheight = image_height() / 2;
unsigned int lwidth = width();
unsigned int lheight = height();
unsigned int pixel_per_step = iheight / lheight;
unsigned int lines_per_step = iwidth / lwidth;
unsigned char *yp = yuv422_planar_buffer;
unsigned char *up = YUV422_PLANAR_U_PLANE(yuv422_planar_buffer, iwidth * 2, iheight * 2);
unsigned char *vp = YUV422_PLANAR_V_PLANE(yuv422_planar_buffer, iwidth * 2, iheight * 2);
unsigned int y = level * deepness() / depth();
YUV_t c;
for (unsigned int v = lwidth; v > 0 ; --v) {
unsigned int v_index = (v - 1) * deepness() / lwidth;
for (unsigned int u = 0; u < lheight; ++u) {
unsigned int u_index = u * deepness() / lheight;
c = ColorObjectMap::get_color(determine(y, u_index, v_index));
for (unsigned int p = 0; p < pixel_per_step; ++p) {
*yp++ = c.Y;
*yp++ = c.Y;
*up++ = c.U;
*vp++ = c.V;
}
}
// Double line
unsigned int lines = (2 * (lines_per_step - 1)) + 1;
memcpy(yp, (yp - iwidth * 2), (iwidth * 2) * lines);
yp += (iwidth * 2) * lines;
memcpy(up, (up - iwidth), iwidth * lines);
memcpy(vp, (vp - iwidth), iwidth * lines);
up += iwidth * lines;
vp += iwidth * lines;
}
}
void
bayerRGGB_to_yuv422planar_nearest_neighbour(const unsigned char *bayer,
unsigned char *yuv,
unsigned int width,
unsigned int height)
{
unsigned char *y = yuv;
unsigned char *u = YUV422_PLANAR_U_PLANE(yuv, width, height);
unsigned char *v = YUV422_PLANAR_V_PLANE(yuv, width, height);
const unsigned char *b = bayer;
int y1, u1, v1, y2, u2, v2;
for ( unsigned int h = 0; h < height; h += 2) {
// r g ... line
for (unsigned int w = 0; w < width; w += 2) {
RGB2YUV(*b, b[1], b[width+1], y1, u1, v1);
++b;
RGB2YUV(b[-1], *b, b[width], y2, u2, v2);
++b;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// g b ... line
for (unsigned int w = 0; w < width; w += 2) {
RGB2YUV(*(b-width), *b, b[1], y1, u1, v1);
++b;
RGB2YUV(*(b-width-1), b[-1], *b, y2, u2, v2);
++b;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
}
}
void
FilterSum::apply()
{
if ( src[0] == NULL ) return;
if ( src[1] == NULL ) return;
if ( src_roi[0] == NULL ) return;
if ( src_roi[1] == NULL ) return;
unsigned int h = 0;
unsigned int w = 0;
// y-plane
unsigned char *byp = src[0] + (src_roi[0]->start.y * src_roi[0]->line_step) + (src_roi[0]->start.x * src_roi[0]->pixel_step);
// u-plane
unsigned char *bup = YUV422_PLANAR_U_PLANE(src[0], src_roi[0]->image_width, src_roi[0]->image_height)
+ ((src_roi[0]->start.y * src_roi[0]->line_step) / 2 + (src_roi[0]->start.x * src_roi[0]->pixel_step) / 2) ;
// v-plane
unsigned char *bvp = YUV422_PLANAR_V_PLANE(src[0], src_roi[0]->image_width, src_roi[0]->image_height)
+ ((src_roi[0]->start.y * src_roi[0]->line_step) / 2 + (src_roi[0]->start.x * src_roi[0]->pixel_step) / 2);
// y-plane
unsigned char *fyp = src[1] + (src_roi[1]->start.y * src_roi[1]->line_step) + (src_roi[1]->start.x * src_roi[1]->pixel_step);
// u-plane
unsigned char *fup = YUV422_PLANAR_U_PLANE(src[1], src_roi[1]->image_width, src_roi[1]->image_height)
+ ((src_roi[1]->start.y * src_roi[1]->line_step) / 2 + (src_roi[1]->start.x * src_roi[1]->pixel_step) / 2) ;
// v-plane
unsigned char *fvp = YUV422_PLANAR_V_PLANE(src[1], src_roi[1]->image_width, src_roi[1]->image_height)
+ ((src_roi[1]->start.y * src_roi[1]->line_step) / 2 + (src_roi[1]->start.x * src_roi[1]->pixel_step) / 2);
// destination y-plane
unsigned char *dyp = dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step);
// destination u-plane
unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
+ ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2) ;
// destination v-plane
unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
+ ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
// line starts
unsigned char *lbyp = byp; // y-plane
unsigned char *lbup = fup; // u-plane
unsigned char *lbvp = fvp; // v-plane
unsigned char *lfyp = fyp; // y-plane
unsigned char *lfup = fup; // u-plane
unsigned char *lfvp = fvp; // v-plane
unsigned char *ldyp = dyp; // destination y-plane
unsigned char *ldup = dup; // destination u-plane
unsigned char *ldvp = dvp; // destination v-plane
for (h = 0; (h < src_roi[1]->height) && (h < dst_roi->height); ++h) {
for (w = 0; (w < src_roi[1]->width) && (w < dst_roi->width); w += 2) {
*dyp++ = ((*byp + *fyp) > 255) ? 255 : (*byp + *fyp);
++byp; ++fyp;
*dyp++ = ((*byp + *fyp) > 255) ? 255 : (*byp + *fyp);
++byp; ++fyp;
*dup++ = (*fup++ + *bup++) / 2;
*dvp++ = (*fvp++ + *bvp++) / 2;
}
lbyp += src_roi[0]->line_step;
lbup += src_roi[0]->line_step / 2;
lbvp += src_roi[0]->line_step / 2;
lfyp += src_roi[1]->line_step;
lfup += src_roi[1]->line_step / 2;
lfvp += src_roi[1]->line_step / 2;
ldyp += dst_roi->line_step;
ldup += dst_roi->line_step / 2;
ldvp += dst_roi->line_step / 2;
byp = lbyp;
bup = lbup;
bvp = lbvp;
fyp = lfyp;
fup = lfup;
fvp = lfvp;
dyp = ldyp;
dup = ldup;
dvp = ldvp;
}
}
void
bayerGBRG_to_yuv422planar_bilinear(const unsigned char *bayer, unsigned char *yuv,
unsigned int width, unsigned int height)
{
unsigned char *y = yuv;
unsigned char *u = YUV422_PLANAR_U_PLANE(yuv, width, height);
unsigned char *v = YUV422_PLANAR_V_PLANE(yuv, width, height);
const unsigned char *bf = bayer;
int y1, u1, v1, y2, u2, v2;
int r, g, b;
// first line is special
// g b ... line
// not full data in first columns
RGB2YUV(bf[width], *bf, bf[1], y1, u1, v1);
++bf;
r = (bf[width - 1] + bf[width + 1]) >> 1;
// correct:
// g = (bf[-1] + bf[width] + bf[1]) / 3;
// faster:
g = (bf[-1] + bf[1]) >> 1;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
// rest of first line
for (unsigned int w = 2; w < width - 2; w += 2) {
b = (bf[-1] + bf[1]) >> 1;
RGB2YUV(bf[width], *bf, b, y1, u1, v1);
++bf;
r = (bf[width - 1] + bf[width + 1]) >> 1;
// correct:
// g = (bf[-1] + bf[width] + bf[1]) / 3;
// faster:
g = (bf[-1] + bf[1]) >> 1;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// not full data in last columns
b = (bf[-1] + bf[1]) >> 1;
RGB2YUV(bf[width], *bf, b, y1, u1, v1);
++bf;
g = (bf[-1] + bf[width]) >> 1;
RGB2YUV(bf[width - 1], g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
for ( unsigned int h = 1; h < height - 1; h += 2) {
// r g ... line
// correct: g = (bf[-width] + bf[1] + bf[width]) / 3;
// faster:
g = (bf[-width] + bf[1]) >> 1;
b = (bf[width-1] + bf[width+1]) >> 1;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
r = (bf[-1] + bf[1]) >> 1;
b = (bf[-width] + bf[width]) >> 1;
RGB2YUV(r, *bf, b, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
for (unsigned int w = 2; w < width - 2; w += 2) {
g = (bf[-width] + bf[1] + bf[width] + bf[-1]) >> 2;
b = (bf[-width-1] + bf[-width+1] + bf[width-1] + bf[width+1]) >> 2;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
r = (bf[-1] + bf[1]) >> 1;
b = (bf[-width] + bf[width]) >> 1;
RGB2YUV(r, *bf, b, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
g = (bf[-width] + bf[1] + bf[width] + bf[-1]) >> 2;
b = (bf[-width-1] + bf[-width+1] + bf[width-1] + bf[width+1]) >> 2;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
b = (bf[-width] + bf[width]) >> 1;
RGB2YUV(bf[-1], *bf, g, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
// g b ... line
r = (bf[width] + bf[-width]) >> 1;
RGB2YUV(r, *bf, bf[1], y1, u1, v1);
++bf;
r = (bf[-width-1] + bf[-width+1] + bf[width - 1] + bf[width + 1]) >> 2;
g = (bf[-width] + bf[1] + bf[width] + bf[-1]) >> 2;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
for (unsigned int w = 2; w < width - 2; w += 2) {
r = (bf[width] + bf[-width]) >> 1;
b = (bf[-1] + bf[1]) >> 1;
RGB2YUV(r, *bf, b, y1, u1, v1);
++bf;
r = (bf[-width-1] + bf[-width+1] + bf[width-1] + bf[width+1]) >> 2;
g = (bf[-width] + bf[1] + bf[width] + bf[-1]) >> 2;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
r = (bf[width] + bf[-width]) >> 1;
b = (bf[-1] + bf[1]) >> 1;
RGB2YUV(r, *bf, b, y1, u1, v1);
++bf;
r = (bf[-width-1] + bf[width-1]) >> 1;
// correct: g = (bf[-width] + bf[width] + bf[-1]) / 3;
// faster:
g = (bf[-width] + bf[-1]) >> 1;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// last r g ... line
// correct: g = (bf[-width] + bf[1] + bf[width]) / 3;
// faster:
g = (bf[-width] + bf[1]) >> 1;
b = bf[-width+1];
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
r = (bf[-1] + bf[1]) >> 1;
b = bf[-width];
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
for (unsigned int w = 2; w < width - 2; w += 2) {
// correct: g = (bf[-width] + bf[1] + bf[-1]) / 3
// faster:
g = (bf[-width] + bf[-1]) >> 1;
b = (bf[-width-1] + bf[-width+1]) >> 1;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
r = (bf[-1] + bf[1]) >> 1;
b = bf[-width];
RGB2YUV(r, *bf, b, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// correct: g = (bf[-width] + bf[1] + bf[-1]) / 3;
// faster:
g = (bf[-width] + bf[-1]) >> 1;
b = (bf[-width-1] + bf[-width+1]) >> 1;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
b = bf[-width];
RGB2YUV(bf[-1], *bf, b, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
void
bayerGBRG_to_yuv422planar_bilinear2(const unsigned char *bayer, unsigned char *yuv,
unsigned int width, unsigned int height)
{
unsigned char *y = yuv;
unsigned char *u = YUV422_PLANAR_U_PLANE(yuv, width, height);
unsigned char *v = YUV422_PLANAR_V_PLANE(yuv, width, height);
const unsigned char *bf = bayer;
int y1, u1, v1, y2, u2, v2;
int r, g, b;
// ignore first g b ... line
bf += width;
y += width;
u += width >> 1;
v += width >> 1;
for ( unsigned int h = 1; h < height - 1; h += 2) {
// r g ... line
// ignore first two columns
++bf; ++bf;
++y; ++y;
++u; ++v;
for (unsigned int w = 2; w < width - 2; w += 2) {
g = (bf[1] + bf[-1]) >> 1;
b = (bf[width-1] + bf[width+1]) >> 1;
RGB2YUV(*bf, g, b, y1, u1, v1);
++bf;
r = (bf[-1] + bf[1]) >> 1;
b = (bf[-width] + bf[width]) >> 1;
RGB2YUV(r, *bf, b, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// ignore last two columns
++bf; ++bf;
++y; ++y;
++u; ++v;
// g b ... line
// ignore first two columns
++bf; ++bf;
++y; ++y;
++u; ++v;
for (unsigned int w = 2; w < width - 2; w += 2) {
r = (bf[width] + bf[-width]) >> 1;
b = (bf[-1] + bf[1]) >> 1;
RGB2YUV(r, *bf, b, y1, u1, v1);
++bf;
r = (bf[width-1] + bf[width+1]) >> 1;
g = (bf[1] + bf[width]) >> 1;
RGB2YUV(r, g, *bf, y2, u2, v2);
++bf;
assign(y, u, v, y1, u1, v1, y2, u2, v2);
}
// ignore last two columns
++bf; ++bf;
++y; ++y;
++u; ++v;
}
// ignore last r g ... line
}
void
FilterUnwarp::apply()
{
// destination y-plane
unsigned char *dyp =
dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step);
// destination u-plane
unsigned char *dup =
YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
+ ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
// v-plane
unsigned char *dvp =
YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
+ ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
// line starts
unsigned char *ldyp = dyp; // destination y-plane
unsigned char *ldup = dup; // u-plane
unsigned char *ldvp = dvp; // v-plane
unsigned int warp1_x = 0, warp1_y = 0, warp2_x = 0, warp2_y = 0;
unsigned char py1 = 0, py2 = 0, pu1 = 0, pu2 = 0, pv1 = 0, pv2 = 0;
for (unsigned int h = 0; h < dst_roi->height; ++h) {
for (unsigned int w = 0; w < dst_roi->width; w += 2) {
mm->unwarp2warp(dst_roi->start.x + w, dst_roi->start.y + h, &warp1_x, &warp1_y);
mm->unwarp2warp(dst_roi->start.x + w + 1, dst_roi->start.y + h + 1, &warp2_x, &warp2_y);
if ((warp1_x < src_roi[0]->image_width) && (warp1_y < src_roi[0]->image_height)) {
// Src pixel is in original image
YUV422_PLANAR_YUV(src[0],
src_roi[0]->image_width,
src_roi[0]->image_height,
warp1_x,
warp1_y,
py1,
pu1,
pv1);
*dyp++ = py1;
*dup = pu1;
*dvp = pv1;
if ((warp2_x < src_roi[0]->image_width) && (warp2_y < src_roi[0]->image_height)) {
YUV422_PLANAR_YUV(src[0],
src_roi[0]->image_width,
src_roi[0]->image_height,
warp2_x,
warp2_y,
py2,
pu2,
pv2);
*dyp++ = py2;
*dup = (*dup + pu2) / 2;
*dvp = (*dvp + pv2) / 2;
} else {
*dyp++ = 0;
}
dup++;
dvp++;
} else {
*dyp++ = 0;
*dup = 0;
*dvp = 0;
if ((warp2_x < src_roi[0]->image_width) && (warp2_y < src_roi[0]->image_height)) {
YUV422_PLANAR_YUV(src[0],
src_roi[0]->image_width,
src_roi[0]->image_height,
warp2_x,
warp2_y,
py2,
pu2,
pv2);
*dyp++ = py2;
*dup = pu2;
*dvp = pv2;
} else {
*dyp++ = 0;
}
dup++;
dvp++;
}
}
ldyp += dst_roi->line_step;
ldup += dst_roi->line_step;
ldup += dst_roi->line_step;
dyp = ldyp;
dup = ldup;
dvp = ldvp;
}
}