再帰相関法によるオプティカルフロー

こちらにある
http://www.jsk.t.u-tokyo.ac.jp/~k-okada/paper/1999_cvim_okada_flow.pdf
再帰相関法によるオプティカルフローを実装してみました。
オプティカルフローとは
動画において画素がどのように動いているかを推定するというものです。

解像度を下げたり(160x120)端っこを処理しないとか、
計算コストが下がるようなことをいろいろやっています。
とりあえず20fpsぐらいで動くようです。

こちらのClickOnceのテストプログラムに追加して試せるようにしてみました。
http://www.rmake.net/dycoon/tmp/clickoncetest3/publish.htm

Vistaとかで実行できないので実行ファイルを圧縮したものを以下に置きます。

http://www.rmake.net/dycoon/files/DycV3Demo.zip

メニューのTests->DShowTest->OpticalFlowで実行することができます。
実行には当然ながらカメラが必要です。
DirectShowのあたりの処理がいい加減とか、
Direct3Dの頂点のインデックスに32bit使っているとかが原因で
動かないかもしれませんが、試してみていただければと思います。

こっそりソースコードを乗っけておきます。
完全ではないので適当に推測して呼んでいただければと思います。

	//<Dyc Open types>
	struct TOptcalFlowVec;
	typedef TOptcalFlowVec * POptcalFlowVec;
	//</Dyc Open types>

	//<Dyc Open SL Struct>
	struct TOptcalFlowVec 
	{
		//<Dyc Save Data>
		Bool flg;
		DWord c;
		Long x;
		Long y;
		//</Dyc Save Data>

	};
	//</Dyc Open SL Struct>

	void DycSLWrite(PDycSLStream S, TOptcalFlowVec * Val);
	void DycSLRead(PDycSLStream S, TOptcalFlowVec * Val);

	Bool DycSLWriteOlds(PDycSLStream S, TOptcalFlowVec * Val);
	Bool DycSLReadOlds(PDycSLStream S, TOptcalFlowVec * Val);

	TDycSLStruct_Defines(TOptcalFlowVec);


	
	//<Dyc Open types>
	class TOpticalFlowRC;
	typedef TOpticalFlowRC * POpticalFlowRC;
	//</Dyc Open types>

	//<Dyc Open SL Class>
	class TOpticalFlowRC : public TDycSL
	{
		public:
			//<Dyc Save Data>

			TSize ImageWidth;
			TSize ImageHeight;
			TSize SearchRWidth;
			TSize SearchRHeight;
			TSize WindowWidth;
			TSize WindowHeight;

			TAlgnVec<Byte> P;
			TAlgnVec<DWord> Q0;
			TAlgnVec<DWord> Q1;
			TAlgnVec<DWord> N;
			TAlgnVec<TOptcalFlowVec> V;

			//</Dyc Save Data>

			TOpticalFlowRC();
			virtual ~TOpticalFlowRC();

			virtual PDycSLInfo GetSLInfo();

			virtual void OnSave(PDycSLStream S);
			virtual void OnLoad(PDycSLStream S);

			TAlgnVec<TOptcalFlowVec> & GetV();
	
			Bool Init(TSize _ImageWidth, TSize _ImageHeight,
					TSize _SearchRWidth, TSize _SearchRHeight,
					TSize _WindowWidth, TSize _WindowHeight);

			Bool Do(TBitmap * prevbmp, TBitmap * bmp);
	};
	//</Dyc Open SL Class>

	TDycSLRoot_Defines_Itfc(TOpticalFlowRC);

	void DycSLWrite(PDycSLStream S, TOptcalFlowVec * Val)
	{
		
	}

	void DycSLRead(PDycSLStream S, TOptcalFlowVec * Val)
	{
		
	}

	Bool DycSLWriteOlds(PDycSLStream S, TOptcalFlowVec * Val)
	{
		return False;
	}

	Bool DycSLReadOlds(PDycSLStream S, TOptcalFlowVec * Val)
	{
		return False;
	}

	
	TDycSLRoot_Defines_Impl(TOpticalFlowRC, TDycSL);

	TOpticalFlowRC::TOpticalFlowRC()
	{
		
	}
	
	TOpticalFlowRC::~TOpticalFlowRC()
	{
		
	}

	void TOpticalFlowRC::OnSave(PDycSLStream S)
	{
		
	}

	void TOpticalFlowRC::OnLoad(PDycSLStream S)
	{
		
	}

	TAlgnVec<TOptcalFlowVec> & TOpticalFlowRC::GetV()
	{
		return V;
	}

	Bool TOpticalFlowRC::Init(TSize _ImageWidth, TSize _ImageHeight,
					TSize _SearchRWidth, TSize _SearchRHeight,
					TSize _WindowWidth, TSize _WindowHeight)
	{
		ImageWidth = _ImageWidth;
		ImageHeight = _ImageHeight;
		SearchRWidth = _SearchRWidth;
		SearchRHeight = _SearchRHeight;
		WindowWidth = _WindowWidth;
		WindowHeight = _WindowHeight;

		if(ImageWidth <= 0 || ImageHeight <= 0) return False;
		if(SearchRWidth <= 0 || SearchRHeight <= 0) return False;
		if(WindowWidth <= 0 || WindowHeight <= 0) return False;
	
		V.resize(ImageHeight * ImageWidth);

		P.resize(ImageHeight * ImageWidth * (_SearchRHeight * 2 + 1) * (_SearchRWidth * 2 + 1));
		Q0.resize(P.size());
		Q1.resize(P.size());

		N.resize(P.size());

		TSize i, isz;
		isz = N.size();
		for(i = 0 ; i < isz ; i++)
		{
			N[i] = 0;
		}


		return True;
	}

	Bool TOpticalFlowRC::Do(TBitmap * prevbmp, TBitmap * bmp)
	{
		if(prevbmp->GetWidth() != bmp->GetWidth()) return False;
		if(prevbmp->GetHeight() != bmp->GetHeight()) return False;
		if(prevbmp->GetBitSize() != bmp->GetBitSize()) return False;
		if(prevbmp->GetBitSize() != 32) return False;
		if(prevbmp->GetWidth() != ImageWidth) return False;
		if(prevbmp->GetHeight() != ImageHeight) return False;

		Long i, isz, j, jsz, k, ksz, l, lsz;
		Long sx, sy, sxa, sya;
		Long dsx, dsy, dsxa, dsya;
		Long wh, ww;
		Long ix, iy;
		//Long jx, jy;
		Long srh, srw;
		Long vx;
		Byte * bp0;
		Byte * bp1;
		DWord * lp0;
		DWord * lp1;
		DWord lp0a;
		Byte * pa = &P[0];
		DWord * q0a = &Q0[0];
		DWord * q1a = &Q1[0];
		DWord * na = &N[0];
		TOptcalFlowVec * va = &V[0];
		Long scln = prevbmp->GetScanlineSize();

		bp0 = (Byte *)prevbmp->Lock();
		bp1 = (Byte *)bmp->Lock();

		wh = (Long)WindowHeight;
		ww = (Long)WindowWidth;
		isz = (Long)ImageHeight;
		jsz = (Long)ImageWidth;
		ksz = (Long)SearchRHeight * 2 + 1;
		lsz = (Long)SearchRWidth * 2 + 1;

		srh = (Long)SearchRHeight;
		srw = (Long)SearchRWidth;

		dsy = jsz * ksz * lsz;
		dsx = ksz * lsz;
		dsya = lsz;
		dsxa = 1;

		//*
		for(i = 0 ; i < isz ; i++)
		{
			for(j = 0 ; j < jsz ; j++)
			{
				TOptcalFlowVec & vx = va[i * jsz + j];
				vx.flg = false;
				vx.c = 0x7fffffff;
				vx.x = 0;
				vx.y = 0;
			}
		}
		//*/

		sy = 0;

		for(i = 0 ; i < isz ; i++)
		{
			sx = sy;
			
			for(j = 0 ; j < jsz ; j++)
			{
				sya = sx;

				lp0 = (DWord *)(bp0 + (i) * scln + (j) * 4);
				lp0a = (*lp0) & 0x000000ff;

				for(k = 0 ; k < ksz ; k++)
				{
					sxa = sya;

					iy = i + k - srh;

					for(l = 0 ; l < lsz ; l++)
					{
						ix = j + l - srw;
						
						if(iy >= 0 && iy < isz &&
							ix >= 0 && ix < jsz)
						{
							lp1 = (DWord *)(bp1 + iy * scln + ix * 4);

							pa[sxa] = (Byte)abs((Long)(lp0a - ((*lp1) & 0x000000ff)));
						}
						else
						{
							pa[sxa] = 255;
						}

						sxa += dsxa;
					}

					sya += dsya;
				}
			
				sx += dsx;
			}
			
			sy += dsy;
		}

		{
			for(j = 0 ; j < jsz ; j++)
			for(k = 0 ; k < ksz ; k++)
			for(l = 0 ; l < lsz ; l++)
			{
				vx = j * dsx + k * dsya + l * dsxa;
				q0a[vx] = 0;
				for(i = 0 ; i < wh ; i++)
				{
					q0a[vx] += pa[vx + i * dsy];
				}
			}

			sy = dsy;

			for(i = 1 ; i < isz - wh ; i++)
			{
				sx = sy;

				for(j = 0 ; j < jsz ; j++)
				{
					sya = sx;

					for(k = 0 ; k < ksz ; k++)
					{
						sxa = sya;

						for(l = 0 ; l < lsz ; l++)
						{
							q0a[sxa] = q0a[sxa - dsy] + pa[sxa + dsy * (wh - 1)] - pa[sxa - dsy];

							sxa += dsxa;
						}

						sya += dsya;
					}

					sx += dsx;
				}

				sy += dsy;
			}

		}

		{
			for(i = 0 ; i < isz ; i++)
			for(k = 0 ; k < ksz ; k++)
			for(l = 0 ; l < lsz ; l++)
			{
				vx = i * dsy + k * dsya + l * dsxa;
				q1a[vx] = 0;
				for(j = 0 ; j < ww ; j++)
				{
					q1a[vx] += pa[vx + j * dsx];
				}
			}

			sy = 0;

			for(i = 0 ; i < isz ; i++)
			{
				sx = sy + dsx;

				for(j = 1 ; j < jsz - ww ; j++)
				{
					sya = sx;

					for(k = 0 ; k < ksz ; k++)
					{
						sxa = sya;

						for(l = 0 ; l < lsz ; l++)
						{
							q1a[sxa] = q1a[sxa - dsx] + pa[sxa + dsx * (ww - 1)] - pa[sxa - dsx];

							sxa += dsxa;
						}

						sya += dsya;
					}

					sx += dsx;
				}

				sy += dsy;
			}

		}

		{
			for(k = 0 ; k < ksz ; k++)
			for(l = 0 ; l < lsz ; l++)
			{
				vx = k * dsya + l * dsxa;
				na[vx] = 0;
				for(i = 0 ; i < wh ; i++)
				{
					na[vx] += q1a[vx + i * dsy];
				}
			}

			sy = dsy;

			for(i = 1 ; i < isz - wh ; i++)
			{
				sx = sy;

				for(j = 0 ; j < 1 ; j++)
				{
					sya = sx;

					for(k = 0; k < ksz ; k++)
					{
						sxa = sya;

						for(l = 0 ; l < lsz ; l++)
						{
							na[sxa] = na[sxa - dsy] + q1a[sxa + dsy * (wh - 1)] - q1a[sxa - dsy];

							sxa += dsxa;
						}

						sya += dsya;
					}

					sx += dsx;
				}

				sy += dsy;
			}

			sy = 0;

			for(i = 0 ; i < isz - wh ; i++)
			{
				sx = sy + dsx;

				for(j = 1 ; j < jsz - ww ; j++)
				{
					sya = sx;

					for(k = 0 ; k < ksz ; k++)
					{
						sxa = sya;

						for(l = 0 ; l < lsz ; l++)
						{
							na[sxa] = na[sxa - dsx] + q0a[sxa + dsx * (ww - 1)] - q0a[sxa - dsx];

							if(k == 2 && l == 0 && na[sxa] == 0)
							{
								static int a;
								a++;
							}

							sxa += dsxa;
						}

						sya += dsya;
					}

					sx += dsx;
				}

				sy += dsy;
			}
			
		}

		{
			sy = 0;

			for(i = 0 ; i < isz ; i++)
			{
				sx = sy;

				for(j = 0 ; j < jsz ; j++)
				{
					sya = sx;

					TOptcalFlowVec & vx = va[i * jsz + j];
					vx.flg = False;
					vx.c = 0x7fffffff;
					vx.x = 0;
					vx.y = 0;

					for(k = 0 ; k < ksz ; k++)
					{
						sxa = sya;

						for(l = 0 ; l < lsz ; l++)
						{
							if(na[sxa] < vx.c)
							{
								vx.c = na[sxa];
								vx.flg = True;
								vx.x = l - srw;
								vx.y = k - srh;
							}

							sxa += dsxa;
						}

						sya += dsya;
					}

					sx += dsx;
				}
			
				sy += dsy;
			}

		}

		return True;
	}