//---------------------------------------------------------
// 概要      : 動的な背景の更新を含む背景差分法
// File Name : bbs3.cpp
// Library   : OpenCV for MS-Windows 1.0
//---------------------------------------------------------

#include <stdio.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

#define	MASK_THRESHOLD		1	//	backgroundMaskImageやstillObjectMaskImageを生成するための閾値
#define THRESHOLD_MAX_VALUE	255	//	2値化の際に使用する最大値

#define	BACKGROUND_ALPHA				0.01	//	背景更新の際の混合比率
#define	BACKGROUND_INITIAL_THRESHOLD	20		//	背景の初期閾値

#define	STILL_OBJECT_ALPHA				0.1		//	静止物体更新の際の混合比率
#define	STILL_OBJECT_INITIAL_THRESHOLD	255		//	静止物体の初期閾値

#define	THRESHOLD_COEFFICIENT	5.0	//	閾値の値を引く際の使用する閾値にかける数

#define	NOT_STILL_DEC_STEP			10	//	動いた場合のカウンタ減算量
#define	STILL_OBJECT_TO_BACKGROUND	100	//	背景として採用する基準

//
//	画像を上下反転して表示する
//
//	引数:
//		windowName : 画像を表示するウィンドウの名前
//		image      : 上下反転して表示する画像用IplImage
//
void showFlipImage( char *windowName, IplImage *image ) {
	if ( image->origin == 0 ) {
		cvFlip( image, image, 0 );
		cvShowImage( windowName, image );
		cvFlip( image, image, 0 );
	}
}

int main( int argc, char **argv ) { 
	int key;					//	キー入力用の変数
	CvCapture *capture = NULL;	//	カメラキャプチャ用の構造体

	char *windowNameCapture = "Capture";					//	キャプチャした画像を表示するウィンドウの名前
	char *windowNameForeground  = "Foreground Mask";		//	前景マスク画像を表示するウィンドウの名前
	char *windowNameStillObjectMask  = "Still Object Mask";	//	静止物体マスク画像を表示するウィンドウの名前
	char *windowNameBackground = "Background";				//	背景画像を表示するウィンドウの名前
	char *windowNameStillObject  = "Still Object";			//	静止物体を表示するウィンドウの名前
	char *windowNameCounter = "Counter";					//	カウンタを表示するウィンドウの名前

	//	カメラを初期化する
	if ( ( capture = cvCreateCameraCapture( -1 ) ) == NULL ) {
		//	カメラが見つからなかった場合
		printf( "カメラが見つかりません\n" );
		return -1;
	}
	
	//	ウィンドウを生成する
	cvNamedWindow( windowNameCapture, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( windowNameForeground, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( windowNameStillObjectMask, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( windowNameBackground, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( windowNameStillObject, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( windowNameCounter, CV_WINDOW_AUTOSIZE );

	IplImage *frameImage = cvQueryFrame( capture );

	//	画像サイズを保存
	CvSize imageSize = cvGetSize( frameImage );

	//	画像を生成する
	IplImage *backgroundAverageImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );		//	背景の平均値保存用IplImage
	IplImage *backgroundThresholdImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );		//	背景の閾値保存用IplImage
	IplImage *stillObjectAverageImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );		//	静止物体の平均値保存用IplImage
	IplImage *stillObjectThresholdImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );		//	静止物体の閾値保存用IplImage
	IplImage *stillObjectCounterImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );		//	静止物体のカウンタ用IplImage
	IplImage *backgroundDifferenceImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );		//	背景差分画像用IplImage
	IplImage *stillObjectDifferenceImage = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );	//	静止物体差分画像用IplIMage
	IplImage *thresholdImage32 = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );				//	32bitの閾値画像用IplImage
	IplImage *thresholdImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 3 );					//	閾値画像用IplImage
	IplImage *resultImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );					//	結果画像用IplImage
	IplImage *backgroundMaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );			//	背景マスク画像用IplImage
	IplImage *foregroundMaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );			//	前景マスク用IplImage
	IplImage *stillObjectMaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );			//	静止物体マスク用IplImage
	IplImage *movingObjectMask = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );				//	動物体マスク用IplImage
	IplImage *backgroundCopyMaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );		//	背景にコピーする際に使用するマスク用IplImage
	IplImage *tmpMaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );					//	テンポラリ用IplImage
	IplImage *tmp2MaskImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 1 );					//	テンポラリ用IplImage(その2)
	IplImage *frameImage32 = cvCreateImage( imageSize, IPL_DEPTH_32F, 3 );					//	32bitのキャプチャした画像用IplImage
	IplImage *backgroundImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 3 );				//	背景画像用IplImage
	IplImage *stillObjectImage = cvCreateImage( imageSize, IPL_DEPTH_8U, 3 );				//	静止物体画像用IplImage

	//	初期化する
	cvConvert( frameImage, backgroundAverageImage );
	cvSet( backgroundThresholdImage, cvScalarAll( BACKGROUND_INITIAL_THRESHOLD ) );
	cvSetZero( stillObjectAverageImage );
	cvSetZero( stillObjectThresholdImage );
	cvSetZero( stillObjectCounterImage );
								 
	while ( 1 ) {
		frameImage = cvQueryFrame( capture );

		//	float 32bitに変換する
		cvConvert( frameImage, frameImage32 );

		//	背景との差 /////////////////////////////////////////////////////////

		//	現在の背景との差の絶対値を成分ごとに取る
		cvAbsDiff( frameImage32, backgroundAverageImage, backgroundDifferenceImage );

		//	閾値の値を引く
		cvAddWeighted( backgroundDifferenceImage, 1.0, backgroundThresholdImage, -THRESHOLD_COEFFICIENT, 0.0, thresholdImage32 );

		// thresholdImage の要素が1つでも0以上だったら背景ではない
		cvConvert( thresholdImage32, thresholdImage );
		cvCvtColor( thresholdImage, resultImage, CV_BGR2GRAY );
        cvThreshold( resultImage, backgroundMaskImage, MASK_THRESHOLD, THRESHOLD_MAX_VALUE, CV_THRESH_BINARY_INV );

		//	背景候補との差 /////////////////////////////////////////////////////

		//	現在の背景候補との差の絶対値を成分ごとに取る
		cvAbsDiff( frameImage32, stillObjectAverageImage, stillObjectDifferenceImage );

		//	閾値の値を引く
		cvAddWeighted( stillObjectDifferenceImage, 1.0, stillObjectThresholdImage, -THRESHOLD_COEFFICIENT, 0.0, thresholdImage32 );

		//	thresholdImage の要素が1つでも0以上だったら背景候補ではない
		cvConvert( thresholdImage32, thresholdImage );
		cvCvtColor( thresholdImage, resultImage, CV_BGR2GRAY );
        cvThreshold( resultImage, stillObjectMaskImage, MASK_THRESHOLD, THRESHOLD_MAX_VALUE, CV_THRESH_BINARY_INV );
		
		//	ここまでで、
		//	backgroundDifferenceImage, backgroundMaskImage
		//	stillObjectDifferenceImage, stillObjectMaskImage
		//	に意味のある値が入る。

		//	各種情報を更新する /////////////////////////////////////////////////

		//	背景に同化する場合 (backgroundMaskImage=1の場合)
		cvRunningAvg( frameImage32, backgroundAverageImage, BACKGROUND_ALPHA, backgroundMaskImage );
		cvRunningAvg( backgroundDifferenceImage, backgroundThresholdImage, BACKGROUND_ALPHA, backgroundMaskImage );

		//	背景候補に同化する場合 (backgroundMaskImage=0 && stillObjectMaskImage=1)
		cvNot( backgroundMaskImage, foregroundMaskImage );
		cvAnd( foregroundMaskImage, stillObjectMaskImage, tmpMaskImage );	//	背景候補
		
		cvRunningAvg( frameImage32, stillObjectAverageImage, STILL_OBJECT_ALPHA, tmpMaskImage );
		cvRunningAvg( stillObjectDifferenceImage, stillObjectThresholdImage, STILL_OBJECT_ALPHA, tmpMaskImage );
					  
		//	背景候補カウンタを増やす
		cvAddS( stillObjectCounterImage, cvScalar( 1 ), stillObjectCounterImage, tmpMaskImage );
		
		//	カウンタが閾値以上になったら、背景候補を背景として採用する
		cvThreshold( stillObjectCounterImage, tmp2MaskImage, STILL_OBJECT_TO_BACKGROUND, THRESHOLD_MAX_VALUE, CV_THRESH_BINARY );
		cvAnd( tmpMaskImage, tmp2MaskImage, backgroundCopyMaskImage );
		cvCopy( stillObjectAverageImage, backgroundAverageImage, backgroundCopyMaskImage );
		cvCopy( stillObjectThresholdImage, backgroundThresholdImage, backgroundCopyMaskImage );

		//	この backgroundCopyMaskImage は、後で背景候補を0に初期化する際に使用
		cvSet( stillObjectCounterImage, cvScalarAll( 0 ), backgroundCopyMaskImage );

		//	背景候補でもなく、背景でもない場合
		//	(foregroundMaskImage = 1 && stillObjectMaskImage = 0)
		cvNot( stillObjectMaskImage, movingObjectMask );
		
		//	カウンタを減らす(短時,間ノイズ対応)
		//	これは、背景に分類されたピクセルに対しても行う。すなわち、
		//	movingObjectMask == 1 || backgroundMaskImage == 1
		cvOr( backgroundMaskImage, movingObjectMask, tmpMaskImage );
		cvSubS( stillObjectCounterImage, cvScalarAll( NOT_STILL_DEC_STEP ), stillObjectCounterImage, tmpMaskImage );
		
		//	カウンタが0になったら背景候補を初期化する
		cvNot( stillObjectCounterImage, tmp2MaskImage );	// tmp2 = 1 なら初期化

		//	初期化する値の候補は2種類ある。
		//	(1)現在の画像で初期化 --- 背景でもなく背景候補でもない場合
		//	(2)登録なし状態で初期化 --- 背景もしくは背景候補をコピーした場合
		//	ここでは(1)で初期化しておく
		cvOr( tmpMaskImage, backgroundCopyMaskImage, tmpMaskImage );
		cvAnd( tmpMaskImage, tmp2MaskImage, tmpMaskImage );

		cvCopy( frameImage32, stillObjectAverageImage, tmpMaskImage );
		cvSet( stillObjectThresholdImage, cvScalarAll( STILL_OBJECT_INITIAL_THRESHOLD ), tmpMaskImage );
		
		//	ノイズを除去する
		cvSmooth( foregroundMaskImage, foregroundMaskImage, CV_MEDIAN );

		cvConvert( backgroundAverageImage, backgroundImage );
		cvConvert( stillObjectAverageImage, stillObjectImage );

		//	画像を表示する
		cvShowImage( windowNameCapture, frameImage );
		showFlipImage( windowNameForeground, foregroundMaskImage );
		showFlipImage( windowNameStillObjectMask, stillObjectMaskImage );
		showFlipImage( windowNameBackground, backgroundImage );
		showFlipImage( windowNameStillObject, stillObjectImage );
		showFlipImage( windowNameCounter, stillObjectCounterImage );

		//	キー入力判定
		key = cvWaitKey( 1 );
		if ( key == 'q' ) {
			//	'q'キーが押されたらループを抜ける
			break;
		}
	}

	//	キャプチャを解放する
	cvReleaseCapture( &capture );
	//	メモリを解放する
	cvReleaseImage( &backgroundAverageImage );
	cvReleaseImage( &backgroundThresholdImage);
	cvReleaseImage( &stillObjectAverageImage );
	cvReleaseImage( &stillObjectThresholdImage );
	cvReleaseImage( &stillObjectCounterImage );
	cvReleaseImage( &backgroundDifferenceImage );
	cvReleaseImage( &stillObjectDifferenceImage );
	cvReleaseImage( &thresholdImage32 );
	cvReleaseImage( &thresholdImage );
	cvReleaseImage( &resultImage );
	cvReleaseImage( &backgroundMaskImage );
	cvReleaseImage( &foregroundMaskImage );
	cvReleaseImage( &stillObjectMaskImage );
	cvReleaseImage( &movingObjectMask );
	cvReleaseImage( &backgroundCopyMaskImage );
	cvReleaseImage( &tmpMaskImage );
	cvReleaseImage( &tmp2MaskImage );
	cvReleaseImage( &frameImage32 );
	cvReleaseImage( &backgroundImage );
	cvReleaseImage( &stillObjectImage );
	//	ウィンドウを破棄する
	cvDestroyWindow( windowNameCapture );
	cvDestroyWindow( windowNameForeground );
	cvDestroyWindow( windowNameStillObjectMask );
	cvDestroyWindow( windowNameBackground );
	cvDestroyWindow( windowNameStillObject );
	cvDestroyWindow( windowNameCounter );

	return 0;
}
