color_histogram.cpp

人头跟踪算法

#include "stdafx.h"	// TRACE(), AfxMessageBox(), max(), min()
#include "assert.h"

#include "image.h" // Image8
#include "base.h" // ISIZE[XY]
#include "color_histogram.h"		// ColorHistogram

void ComputeColorHistogramOfWholeImage(Image8 *color1,
									   Image8 *color2,
									   Image8 *color3,
									   ColorHistogram *ch)
{
	unsigned char *ptr1, *ptr2, *ptr3;
	int col1, col2, col3;
	int indx;
	int i;

	assert(256%N_BINS_COLOR1==0 && 256%N_BINS_COLOR2==0 && 256%N_BINS_COLOR3==0);
	memset(ch->val, 0, N_BINS_TOT * sizeof(int)); 
	ptr1 = color1->GetImagePtr();
	ptr2 = color2->GetImagePtr();
	ptr3 = color3->GetImagePtr();
	for (i=0 ; i<ISIZEX*ISIZEY ; i++) {
		col1 = *ptr1/BINWIDTH_COLOR1;
		col2 = *ptr2/BINWIDTH_COLOR2;
		col3 = *ptr3/BINWIDTH_COLOR3;
		indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
		assert(indx >= 0 && indx < N_BINS_TOT);
		(ch->val[indx])++;
		ptr1++;
		ptr2++;
		ptr3++;
	}
}

void ComputeColorHistogram(Image8 *color1,
						   Image8 *color2,
						   Image8 *color3,
						   int xcen,
						   int ycen,
						   OUTLINE *outline,
						   ColorHistogram *ch)
{
	int hx=outline->halfsize_x, hy=outline->halfsize_y;
	unsigned char *ptr1, *ptr2, *ptr3;
	unsigned char *ptro = outline->mask;
	int col1, col2, col3;
	int indx;
	int x, y;

	assert(256%N_BINS_COLOR1==0 && 256%N_BINS_COLOR2==0 && 256%N_BINS_COLOR3==0);
	memset(ch->val, 0, N_BINS_TOT * sizeof(int)); 
	ptr1 = color1->GetImagePtr() + (ycen-hy)*ISIZEX + (xcen-hx);
	ptr2 = color2->GetImagePtr() + (ycen-hy)*ISIZEX + (xcen-hx);
	ptr3 = color3->GetImagePtr() + (ycen-hy)*ISIZEX + (xcen-hx);
	for (y=ycen-hy ; y<=ycen+hy ; y++) {
		for (x=xcen-hx ; x<=xcen+hx ; x++) {
			if (*ptro++) {
				col1 = *ptr1/BINWIDTH_COLOR1;
				col2 = *ptr2/BINWIDTH_COLOR2;
				col3 = *ptr3/BINWIDTH_COLOR3;
				indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
				assert(indx >= 0 && indx < N_BINS_TOT);
				(ch->val[indx])++;
			}
			ptr1++;
			ptr2++;
			ptr3++;
		}
		ptr1 += ISIZEX - (2*hx+1);
		ptr2 += ISIZEX - (2*hx+1);
		ptr3 += ISIZEX - (2*hx+1);
	}
}

void UpdateColorHistogram(Image8 *color1,
						  Image8 *color2,
						  Image8 *color3,
						  int xcen,
						  int ycen,
						  PixelList *addlist,
						  PixelList *sublist,
						  ColorHistogram *ch)
{
	unsigned char *ptr1, *ptr2, *ptr3;
	pixelListType *ptra, *ptrs;
	int col1, col2, col3;
	int x, y;
	int indx;
	int i;
//	pixelListType tmpadd[31], tmpsub[37];

//	memcpy(tmpadd, addlist->pixels, 31*sizeof(pixelListType));
//	memcpy(tmpsub, sublist->pixels, 37*sizeof(pixelListType));

	assert(256%N_BINS_COLOR1==0 && 256%N_BINS_COLOR2==0 && 256%N_BINS_COLOR3==0);
	ptr1 = color1->GetImagePtr();
	ptr2 = color2->GetImagePtr();
	ptr3 = color3->GetImagePtr();

	// add new pixels
	ptra = addlist->pixels;
	for (i = addlist->n_pixels ; i>0 ; i--) {
		x = *ptra++ + xcen;
		y = *ptra++ + ycen;
		assert(x>=0 && x<ISIZEX && y>=0 && y<ISIZEY);
		col1 = *(ptr1+y*ISIZEX+x)/BINWIDTH_COLOR1;
		col2 = *(ptr2+y*ISIZEX+x)/BINWIDTH_COLOR2;
		col3 = *(ptr3+y*ISIZEX+x)/BINWIDTH_COLOR3;
		indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
		assert(indx >= 0 && indx < N_BINS_TOT);
		(ch->val[indx])++;
	}

	// subtract old pixels
	ptrs = sublist->pixels;
	for (i = sublist->n_pixels ; i>0 ; i--) {
		x = *ptrs++ + xcen;
		y = *ptrs++ + ycen;
		assert(x>=0 && x<ISIZEX && y>=0 && y<ISIZEY);
		col1 = *(ptr1+y*ISIZEX+x)/BINWIDTH_COLOR1;
		col2 = *(ptr2+y*ISIZEX+x)/BINWIDTH_COLOR2;
		col3 = *(ptr3+y*ISIZEX+x)/BINWIDTH_COLOR3;
		indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
		assert(indx >= 0 && indx < N_BINS_TOT);
		(ch->val[indx])--;
	}
}

void UpdateColorHistogramAndIntersection(Image8 *color1,
										 Image8 *color2,
										 Image8 *color3,
										 int xcen,
										 int ycen,
										 PixelList *addlist,
										 PixelList *sublist,
										 ColorHistogram *ch_model,
										 ColorHistogram *ch_img,
										 int *score)
{
	unsigned char *ptr1, *ptr2, *ptr3;
	pixelListType *ptra, *ptrs;
	int col1, col2, col3;
	int x, y;
	int indx;
	int i;
//	pixelListType tmpadd[31], tmpsub[37];

//	memcpy(tmpadd, addlist->pixels, 31*sizeof(pixelListType));
//	memcpy(tmpsub, sublist->pixels, 37*sizeof(pixelListType));

	assert(256%N_BINS_COLOR1==0 && 256%N_BINS_COLOR2==0 && 256%N_BINS_COLOR3==0);
	ptr1 = color1->GetImagePtr();
	ptr2 = color2->GetImagePtr();
	ptr3 = color3->GetImagePtr();

	// add new pixels
	ptra = addlist->pixels;
	for (i = addlist->n_pixels ; i>0 ; i--) {
		x = *ptra++ + xcen;
		y = *ptra++ + ycen;
		assert(x>=0 && x<ISIZEX && y>=0 && y<ISIZEY);
		col1 = *(ptr1+y*ISIZEX+x)/BINWIDTH_COLOR1;
		col2 = *(ptr2+y*ISIZEX+x)/BINWIDTH_COLOR2;
		col3 = *(ptr3+y*ISIZEX+x)/BINWIDTH_COLOR3;
		indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
		assert(indx >= 0 && indx < N_BINS_TOT);
		(ch_img->val[indx])++;
		if (ch_img->val[indx] <= ch_model->val[indx]) (*score)++;
	}

	// subtract old pixels
	ptrs = sublist->pixels;
	for (i = sublist->n_pixels ; i>0 ; i--) {
		x = *ptrs++ + xcen;
		y = *ptrs++ + ycen;
		assert(x>=0 && x<ISIZEX && y>=0 && y<ISIZEY);
		col1 = *(ptr1+y*ISIZEX+x)/BINWIDTH_COLOR1;
		col2 = *(ptr2+y*ISIZEX+x)/BINWIDTH_COLOR2;
		col3 = *(ptr3+y*ISIZEX+x)/BINWIDTH_COLOR3;
		indx = col3*N_BINS_COLOR1*N_BINS_COLOR2+col2*N_BINS_COLOR1 + col1;
		assert(indx >= 0 && indx < N_BINS_TOT);
		(ch_img->val[indx])--;
		if (ch_img->val[indx] < ch_model->val[indx]) (*score)--;
	}
}


int ColorHistogramIntersection(ColorHistogram *ch1,
							   ColorHistogram *ch2)
{
	int *ptr1, *ptr2;
	int sum=0;
	int i;

	ptr1 = ch1->val;
	ptr2 = ch2->val;
	for (i=0 ; i<N_BINS_TOT ; i++) {
		sum += min(*ptr1, *ptr2);
		ptr1++;  ptr2++;
	}

	return sum;
}

// add histograms, save the result in ch1
void AddColorHistograms(ColorHistogram *ch1, 
						ColorHistogram *ch2)
{
	int *ptr1, *ptr2;
	int i;

	ptr1 = ch1->val;
	ptr2 = ch2->val;
	for (i=0 ; i<N_BINS_TOT ; i++) {
		*ptr1 += *ptr2;
		ptr1++;  ptr2++;
	}
}

void ExtractColorSpace(ImageBGR24 *img, 
					   Image8 *img_color1, 
					   Image8 *img_color2,
					   Image8 *img_color3)
{
	unsigned char *ptri = img->GetImagePtr();
	unsigned char *ptr1 = img_color1->GetImagePtr();
	unsigned char *ptr2 = img_color2->GetImagePtr();
	unsigned char *ptr3 = img_color3->GetImagePtr();
	int b, g ,r;
	int i;

	for (i=0 ; i<ISIZEX*ISIZEY ; i++) {
		b = *ptri++;
		g = *ptri++;
		r = *ptri++;
		// NOTE: These next three lines must be manually maintained to remain 
		// consistent with the three lines in the function below.
		*ptr1++ = (unsigned char) max(0, min(255, (b-g)*10+128));
		*ptr2++ = (unsigned char) max(0, min(255, (g-r)*10+128));
		*ptr3++ = (unsigned char) max(0, min(255, (b+g+r)/3));
	}
}

void BGRToColorBins(unsigned char b,
					unsigned char g,
					unsigned char r,
					int *col1, 
					int *col2, 
					int *col3)
{
	// NOTE: These next three lines must be manually maintained to remain
	// consistent with the three lines in the function above.
	*col1 = (unsigned char) max(0, min(255, (b-g)*10+128));
	*col2 = (unsigned char) max(0, min(255, (g-r)*10+128));
	*col3 = (unsigned char) max(0, min(255, (b+g+r)/3));
	*col1 /= BINWIDTH_COLOR1;
	*col2 /= BINWIDTH_COLOR2;
	*col3 /= BINWIDTH_COLOR3;
}

// used only for displaying
void QuantizeColorSpace(Image8 *img_color_in1, 
						Image8 *img_color_in2,
						Image8 *img_color_in3,
						Image8 *img_color_out1,
						Image8 *img_color_out2,
						Image8 *img_color_out3)
{
	unsigned char *ptri1 = img_color_in1->GetImagePtr();
	unsigned char *ptri2 = img_color_in2->GetImagePtr();
	unsigned char *ptri3 = img_color_in3->GetImagePtr();
	unsigned char *ptro1 = img_color_out1->GetImagePtr();
	unsigned char *ptro2 = img_color_out2->GetImagePtr();
	unsigned char *ptro3 = img_color_out3->GetImagePtr();
	int i;

//	assert(BINWIDTH_COLOR1 == 8);
//	assert(BINWIDTH_COLOR2 == 8);
//	assert(BINWIDTH_COLOR3 == 8);
	for (i=0 ; i<ISIZEX*ISIZEY ; i++) {
		*ptro1++ = (*ptri1++ / BINWIDTH_COLOR1) * BINWIDTH_COLOR1;
		*ptro2++ = (*ptri2++ / BINWIDTH_COLOR2) * BINWIDTH_COLOR2;
		*ptro3++ = (*ptri3++ / BINWIDTH_COLOR3) * BINWIDTH_COLOR3;
	}
}

// ch_in and ch_out are allowed to be the same
void NormalizeColorHistogram(ColorHistogram *ch_in,
							 int numerator,
							 int denominator,
							 ColorHistogram *ch_out) 
{
	int *ptri, *ptro;
	int i;

	if (denominator == 0) 
		AfxMessageBox("Warning: divide by zero", MB_OK|MB_ICONWARNING, 0);
	else {
		ptri = ch_in->val;
		ptro = ch_out->val;
		for (i=0 ; i<N_BINS_TOT ; i++) {
			*ptro++ = (*ptri++ * numerator) / denominator;
		}
	}
}

#define COLORHISTOGRAM_HEADER_LENGTH 3
char colorhistogram_header[COLORHISTOGRAM_HEADER_LENGTH+1] = "CH1";

void WriteColorHistogram(char *fname,
						 ColorHistogram *ch) 
{
	FILE *fp;
	int tmp;

	// Open file for writing
	fp = fopen(fname, "wb");
	if (fp == NULL) {
		AfxMessageBox("Error: could not open file for writing color histogram!!", MB_OK|MB_ICONSTOP, 0);
		return;
	}

	// Write header
	fwrite(colorhistogram_header, sizeof(char), COLORHISTOGRAM_HEADER_LENGTH, fp);
	tmp = N_BINS_COLOR1;  fwrite(&tmp, sizeof(int), 1, fp);
	tmp = N_BINS_COLOR2;  fwrite(&tmp, sizeof(int), 1, fp);
	tmp = N_BINS_COLOR3;  fwrite(&tmp, sizeof(int), 1, fp);
	tmp = CH_MODEL_SZ;    fwrite(&tmp, sizeof(int), 1, fp);

	// Write color histogram model
	if (fwrite(ch->val, sizeof(int), N_BINS_TOT, fp) != N_BINS_TOT) 
		AfxMessageBox("Error writing color histogram!!", MB_OK|MB_ICONSTOP, 0);
	fclose(fp);
}

void ReadColorHistogram(char *fname,
						ColorHistogram *ch) 
{
	char strtmp[COLORHISTOGRAM_HEADER_LENGTH+1];
	int tmp;
	FILE *fp;

	// Open file for reading
	fp = fopen(fname, "rb");
	if (fp == NULL) {
		AfxMessageBox("Error: could not open file for reading color histogram!!", MB_OK|MB_ICONSTOP, 0);
		return;
	}

	// Read header
	fread(strtmp, sizeof(char), COLORHISTOGRAM_HEADER_LENGTH, fp);
	strtmp[COLORHISTOGRAM_HEADER_LENGTH] = '\0';
	if (strcmp(strtmp, colorhistogram_header) != 0)
		AfxMessageBox("Error: color histogram file has invalid header!!", MB_OK|MB_ICONSTOP, 0);
	fread(&tmp, sizeof(int), 1, fp);
	if (tmp != N_BINS_COLOR1)
		AfxMessageBox("Error: color histogram file contains wrong # of color1 bins!!", MB_OK|MB_ICONSTOP, 0);
	fread(&tmp, sizeof(int), 1, fp);
	if (tmp != N_BINS_COLOR2)
		AfxMessageBox("Error: color histogram file contains wrong # of color2 bins!!", MB_OK|MB_ICONSTOP, 0);
	fread(&tmp, sizeof(int), 1, fp);
	if (tmp != N_BINS_COLOR3)
		AfxMessageBox("Error: color histogram file contains wrong # of color3 bins!!", MB_OK|MB_ICONSTOP, 0);
	fread(&tmp, sizeof(int), 1, fp);
	if (tmp != CH_MODEL_SZ)
		AfxMessageBox("Error: color histogram file contains wrong ellipse size!!", MB_OK|MB_ICONSTOP, 0);

	// Read color histogram model
	if (fread(ch->val, sizeof(int), N_BINS_TOT, fp) != N_BINS_TOT) 
		AfxMessageBox("Error reading color histogram!!", MB_OK|MB_ICONSTOP, 0);
	fclose(fp);
}
#undef COLORHISTOGRAM_HEADER_LENGTH

/*
void WriteColorHistogram(char *fname,
						 ColorHistogram *ch) 
{
	FILE *fp;
	int tmp;

	fp = fopen(fname, "wb");
	if (fp == NULL)
		AfxMessageBox("Error: could not open file for writing color histogram!!", MB_OK|MB_ICONSTOP, 0);
	tmp = fwrite(ch->val, sizeof(int), N_BINS_TOT, fp);	
	if (tmp != N_BINS_TOT) 
		AfxMessageBox("Error writing color histogram!!", MB_OK|MB_ICONSTOP, 0);
	fclose(fp);
}

void ReadColorHistogram(char *fname,
						ColorHistogram *ch) 
{
	FILE *fp;
	int tmp;

	fp = fopen(fname, "rb");
	if (fp == NULL)
		AfxMessageBox("Error: could not open file for reading color histogram!!", MB_OK|MB_ICONSTOP, 0);
	else {
		tmp = fread(ch->val, sizeof(int), N_BINS_TOT, fp);	
		if (tmp != N_BINS_TOT) 
			AfxMessageBox("Error reading color histogram!!", MB_OK|MB_ICONSTOP, 0);
		fclose(fp);
	}
}
*/

// ch_in and ch_out are allowed to be the same
int SizeOfColorHistogram(ColorHistogram *ch)
{
	int *ptr;
	int i;
	int count = 0;

	ptr = ch->val;
	for (i=0 ; i<N_BINS_TOT ; i++) {
		count += *ptr++;
	}
	return count;
}

BOOL AreColorHistogramsEqual(ColorHistogram *ch1,
							 ColorHistogram *ch2)
{
	int *ptr1, *ptr2;
	BOOL equal = TRUE;
	int i;

	ptr1 = ch1->val;
	ptr2 = ch2->val;
	for (i=0 ; i<N_BINS_TOT ; i++) {
		if (*ptr1++ != *ptr2++) equal = FALSE;
	}
	return equal;
}