lgmm.cpp

动态场景中运动目标检测提取与跟踪 对新手很有用

#include "stdafx.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include "lgmm.h"
#include "lgraph.h"
#include "DlgParameters.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif



UINT	ThreadMain(LPVOID pParam);
CCameraVideo* PCCamvideoDoc;

//2维数组的相关模板
template <class T> T** make2DArray(int a, int b)
{
T** data = new T*[a];
for(int i=0;i<a;i++) data[i]=new T[b];
return data;
}

template <class T> void init2DArray(T** data, int a, int b, T val)
{
int i, j;
if(sizeof(T)==1)
{
  for(i=0;i<a;i++) memset(data[i],val,b);
}
else
{
  for(i=0;i<a;i++)
  for(j=0;j<b;j++)
    data[i][j] = val;
}
}

template <class T> void init2DArray(T** data, int x1, int x2, int y1, int y2, T val)
{
int i, j;
if(sizeof(T)==1)
{
  for(i=x1;i<=x2;i++) memset(&data[i][y1],val,y2-y1+1);
}
else
{
  for(i=x1;i<=x2;i++)
  for(j=y1;j<=y2;j++)
    data[i][j] = val;
}
}

template <class T> void delete2DArray(T** data, int a)
{
for(int i=0;i<a;i++) delete[] data[i];
delete[] data;
}


float rhmax(float x, float y)
{ return((x)>(y) ? (x) : (y));
}

float rhmax3(float x, float y,float z)
{ return(rhmax(rhmax(x,y), z));
}

float rhmin(float x, float y)
{ return((x)<(y) ? (x) : (y));
}

float rhmin3(float x, float y,float z)
{ return(rhmin(rhmin(x,y), z));
}


void RGB2HSV( float r, float g, float b, float *h, float *s, float *v )
{
// r,g,b values are from 0 to 255
// h = [0,360], s = [0,255], v = [0,255]

	float min, max, delta;

	r=r/255;
	g=g/255;
	b=b/255;

	min = rhmin3( r, g, b );
	max = rhmax3( r, g, b );
	*v = max;				// v
	delta = max - min;
	if( delta != 0 )
//		*s = delta / max;		// s
		*s = delta;		
	else {
		// r = g = b			// s = 0, h is undefined
		*s = 0;
		//*h = -1;
		*h = 0;
		*v *= 255;
		return;
	}
	if( r == max )
		*h = ( g - b ) / delta;		// between yellow & magenta
	else if( g == max )
		*h = 2 + ( b - r ) / delta;	// between cyan & yellow
	else
		*h = 4 + ( r - g ) / delta;	// between magenta & cyan
	*h *= 60;				// degrees
	if( *h < 0 )
		*h += 360;

	*s *= 255;
	*v *= 255;
}

float Hsub(float h1,float h2)//h1:mean h2:current
{
//Hsub=h2-h1
	float ftemp = (float)fabs(h2-h1);
	if(ftemp<180)
	{		
		return(h2-h1);
	}
	else if(ftemp>180)
	{
		if(h1<h2)//angle>180,return negative (h1<h2,ftemp>0)
			return(h2-h1-360);
		else//angle>180,return positive (h1>h2,ftemp<0)
			return(h2-h1+360);
	}
	else return 0;//180,-180
}

void Hnorm(float& val)
{
	val = (float)((360+(int)val)%360);
}

float Habs(float h1,float h2)
{
//Habs=|h1-h2|
	float ftemp;
	ftemp=(float)fabs(h1-h2);
	if(ftemp<=180)
		return(ftemp);
	else
		return(360-ftemp);
}

void HSV2RGB( float *r, float *g, float *b, float h, float s, float v )
{
	s /= 255;
	v /= 255;
	int i;
	float f, p, q, t;
	if( s == 0 ) {
		// achromatic (grey)
		*r = *g = *b = v * 255;
		return;
	}
	h /= 60;			// sector 0 to 5
	i = (int)floor( h );
	f = h - i;			// factorial part of h
	p = v * ( 1 - s );
	q = v * ( 1 - s * f );
	t = v * ( 1 - s * ( 1 - f ) );
	switch( i ) {
		case 0:
			*r = v;
			*g = t;
			*b = p;
			break;
		case 1:
			*r = q;
			*g = v;
			*b = p;
			break;
		case 2:
			*r = p;
			*g = v;
			*b = t;
			break;
		case 3:
			*r = p;
			*g = q;
			*b = v;
			break;
		case 4:
			*r = t;
			*g = p;
			*b = v;
			break;
		default:		// case 5:
			*r = v;
			*g = p;
			*b = q;
			break;
	}
	*r *= 255;
	*g *= 255;
	*b *= 255;
}



LGM::LGM()
{
	m_mean = NULL;
	m_variance = NULL;
}

LGM::~LGM()
{
	if(m_mean != NULL)
	{
		delete []m_mean;
		delete []m_variance;
	}
}

void LGM::Init(unsigned char size, float *mean)//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
{
	m_size = size;
	m_mean = new float[size];
	//ASSERT(m_mean != NULL);
	memcpy(m_mean, mean, size * sizeof(float));
	m_variance = new float[size];
	for(int i=0;i<size;i++) m_variance[i] = 10;
//	if(mean[0] > 0.0) m_weight = 1.0;
	if((mean[0] >= 0.0)&&(mean[1] >= 0.0)&&(mean[2] >= 0.0)) m_weight = 1.0;
	else m_weight = 0.0;	//数据错误
	m_time &= 0;
}

void LGM::Init(float *mean, float weight)
{
	memcpy(m_mean, mean, m_size * sizeof(float));
	for(int i=0;i<m_size;i++) m_variance[i] = 10;
	m_weight = weight;
	m_background = false;
	m_time &= 0;
}

bool LGM::IsValid(float *value, float* distance)//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
{
	distance[1] = value[1] - m_mean[1];
	if(fabs(distance[1])>2.5*m_variance[1]) return false;
	distance[2] = value[2] - m_mean[2];
	if(fabs(distance[2])>2.5*m_variance[2]) return false;
	distance[0] = Hsub(m_mean[0],value[0]) * rhmin(value[1],m_mean[1]) / 255;
	if(fabs(distance[0])>2.5*m_variance[0]) return false;
	distance[3] = distance[0]*distance[0]+distance[1]*distance[1]+distance[2]*distance[2];
	return true;
/*
	distance[3] = 0.0;
	for(int i=0; i<m_size; i++)
	{
		distance[i] = fabs(value[i] - m_mean[i]);
		if(distance[i] > 2.5 * m_variance[i])
		{
			return false;
		}
		distance[i] = distance[i]*distance[i];
		distance[3] += distance[i];
	}
	return true;
*/
}

void LGM::Update(void)
{
	m_weight *= (float)(1 - LALPHA);
}

void LGM::Update(float *value, float &distance)
{
/*
	m_weight = m_weight * (1 - ALPHA) + ALPHA;

	float ratio = ALPHA / m_weight;
	for(int i=0; i<m_size; i++)
	{
		m_mean[i] = m_mean[i] * (1 - ratio) + value[i] * ratio;
	}
	m_variance = sqrt(m_variance * m_variance * (1 - ratio) + distance * ratio);
*/
}

void LGM::Init_Update(float *distance, float sum)//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
{
	float weight = (float)((1.0 - m_weight / sum) * LALPHA);
	for(int i=0; i<m_size; i++)
	{
		m_mean[i] += distance[i] * weight;
		m_variance[i] = m_variance[i] * m_variance[i];
		m_variance[i] += (distance[i] * distance[i] - m_variance[i]) * weight;
		m_variance[i] = (float)sqrt(m_variance[i]);
	}
	Hnorm(m_mean[0]);
	m_weight = (float)(m_weight + 1.0);
/*
	m_weight = m_weight + 1.0;
	float weight = 1 / m_weight;
	for(int i=0; i<m_size; i++)
	{
		m_mean[i] += distance[i] * weight;
		m_variance[i] = m_variance[i] * m_variance[i];
		m_variance[i] += (distance[i] * distance[i] - m_variance[i]) * weight;
		m_variance[i] = sqrt(m_variance[i]);
	}
	Hnorm(m_mean[0]);
*/
}



LGMM::LGMM()
{
	m_ptr = NULL;
}

void LGMM::Init(unsigned char num, unsigned char size, float *mean)
{
	m_num = num;
	m_ptr = new LGM*[num];
	//ASSERT(m_ptr != NULL);
	if(m_ptr != NULL)
	{
		m_ptr[0] = new LGM;//不再初始化为相同分布
		//ASSERT(m_ptr[0] != NULL);
		m_ptr[0]->Init(size, mean);
		for(int i=0; i<size; i++)
		{
			mean[i] = -50.0;
//			mean[i] = 0.0;
		}
		for(i=1; i<num; i++)
		{
			m_ptr[i] = new LGM;
			//ASSERT(m_ptr[i] != NULL);
			m_ptr[i]->Init(size, mean);
		}
	}
}

LGMM::~LGMM()
{
	if(m_ptr != NULL)
	{
		for(int i=0; i<m_num; i++)
		{
			delete m_ptr[i];
		}
		delete []m_ptr;
	}
}

bool LGMM::Process(float *value)
{
	//对当前像素进行一系列处理,是运动目标像素返回true,否则返回false
	//确定是否有匹配
	//有:  是否背景,生成运动目标图像
	//	   更新所有的weight和匹配模型的mean,variance
    //	   对模型排序,确定新的背景模型
	//没有:更新最后一个模型
/*
	int i, match = -1;
	float dist, minDist = 1000000.0;
	for(i=0; i<m_num; i++)
	{
		if(m_ptr[i]->IsValid(value, dist) && dist < minDist)
		{
			minDist = dist;
			match = i;
			//break;
		}
	}
	if(match == -1)//没有匹配
	{
		dist = (m_ptr[m_num-1]->m_weight - 0.1) / (m_num - 1);
		m_ptr[m_num-1]->Init(value, 0.1);
		for(i=0; i<m_num-1; i++)
		{
			m_ptr[i]->m_weight += dist;
		}
		return true;
	}
	else//有匹配
	{
		//更新模型参数
		for(i=0; i<m_num; i++)
		{
			if(i == match)//匹配的模型
			{
				m_ptr[i]->Update(value, minDist);
			}
			else//其它模型
			{
				m_ptr[i]->Update();
			}
		}
		//模型按weight/variance排序
		GM *temp;
		for(i=match; i>0; i--)
		{
			if(m_ptr[i]->m_weight/m_ptr[i]->m_variance > m_ptr[i-1]->m_weight/m_ptr[i-1]->m_variance)
			{
				temp = m_ptr[i];
				m_ptr[i] = m_ptr[i-1];
				m_ptr[i-1] = temp;
				match --;//注意match的变化
			}
			else
			{
				break;//其它顺序肯定不变,可跳出
			}
		}
		//确定新的背景模型
		float wsum=0.0;
		for(i=0; i<m_num; i++)
		{
			wsum += m_ptr[i]->m_weight;