あいぼのぺーじ
ソースコード最新のもの
<windows.h>
<dshow.h>
<stdlib.h>
<stdio.h>
<conio.h>
<math.h>
<qedit.h> // SampleGrabber用
"list502.h"
FILE *fp;
FILE *fpa;
FILE *fpb;
FILE *fpla;
FILE *fplb;
FILE *fpch;
void hope(int xzab,int yzab,int countab,int ca,int *absentx,int *absenty){
printf("カメラ%dの重心は(%d,%d)\n",ca,xzab/countab,yzab/countab);
if(ca==1)fprintf(fpa,"カメラ%dの重心は(%d,%d)\n",ca,xzab/countab,yzab/countab);
if(ca==2)fprintf(fpb,"カメラ%dの重心は(%d,%d)\n",ca,xzab/countab,yzab/countab);
*absentx=xzab/countab;
*absenty=yzab/countab;
}
void triangle(int xa, int ya, int xb, int yb){
double xap,yap,xbp,ybp;
double rx,ry,rz;
//カメラから受け取るそれぞれの重心座標
double b=0.25;//2b=カメラ間距離
double f=1;//カメラの焦点距離
double onepixmet=0.00028;//1ピクセルは0.028cm、0.00028mとする
xap=xa*onepixmet;
yap=ya*onepixmet;
xbp=xb*onepixmet;
ybp=yb*onepixmet;
//3D座標データをセットする
rx=b*(xap+xbp)/(xap-xbp);
ry=b*2*yap/(xap-xbp);
rz=b*2*f/(xap-xbp);
printf("Turn Triangle(x,y,z)=(%lf,%lf,%lf) (m)\n",rx,ry,rz);
fprintf(fp,"Turn Triangle(x,y,z)=(%lf,%lf,%lf) (m)\n",rx,ry,rz);
}
void lease (){
//カメラ間の距離(m)
int b=0.25;
//カメラの焦点距離(m)
int f=2;
//カメラの重心格納
int asentx=0;//x座標の重心
int asenty=0;//y座標の重心
int bsentx=0;
int bsenty=0;
IMG0 img00[10];
IMG0 img01[10];
int i,j;
long n,n2;
BYTE *buffer;
BYTE *buf2;
BYTE *buf3;
BYTE *buffer2;
BYTE *buf22;
BYTE *buf32;
ULONG cFetched;
AM_MEDIA_TYPE amt;
CoInitialize(NULL); // COMの初期化
// ---- キャプチャフィルタの準備 ----
// キャプチャデバイスを探す
ICreateDevEnum * pDevEnum = NULL;
IEnumMoniker * pClassEnum = NULL;
IBaseFilter * pbf[10];
HRESULT hr;
IMoniker * pMoniker[10]; // 最大カメラ数10
// デバイス列挙子を作成
CoCreateInstance( CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC,
IID_ICreateDevEnum, (void ** ) &pDevEnum);
// ビデオキャプチャデバイス列挙子を作成
pDevEnum -> CreateClassEnumerator( CLSID_VideoInputDeviceCategory,
&pClassEnum, 0);
if ( pClassEnum == NULL ){
printf("ビデオキャプチャデバイスは存在しません\n");
pDevEnum -> Release();
CoUninitialize();
return ;
}
// ビデオキャプチャデバイスのオブジェクトの
// インタフェースを最大10個得る
pClassEnum -> Next(10, pMoniker, &cFetched);
printf("カメラ数 %d 個\n", cFetched );
for( i = 0; i < cFetched ; i++){
pMoniker[ i ] -> BindToObject( 0, 0, IID_IBaseFilter, (void**)&pbf[i] );
pMoniker[ i ] -> Release();
printf( "%d 個目のIBaseFilter取得 \n", i+1 );
}
pDevEnum -> Release();
pClassEnum -> Release();
// ---- フィルタグラフの準備 ----
// フィルタグラフを作り、インターフェースを得る
// 複数のカメラを一括制御ならフィルタグラフは１つでいい
IGraphBuilder * pGraph = NULL;
IMediaControl * pMC = NULL;
CoCreateInstance( CLSID_FilterGraph, NULL, CLSCTX_INPROC,
IID_IGraphBuilder, (void **) &pGraph);
pGraph -> QueryInterface( IID_IMediaControl, (LPVOID *) &pMC );
// カメラ数分のキャプチャフィルタをフィルタグラフに追加
for( i = 0 ; i < cFetched ; i++){
pGraph -> AddFilter( pbf[ i ], L"Video Device"); // 名前は任意
printf( "%d 個目のAddFilter完了 \n", i+1 );
}
// ---- グラバフィルタの準備 ----
// グラバフィルタを作る、カメラの数だけ必要
IBaseFilter * pF[ 10 ];
ISampleGrabber * pGrab[ 10 ]; // これらは後で解放すること。
ZeroMemory(&amt, sizeof(AM_MEDIA_TYPE));
amt.majortype = MEDIATYPE_Video;
amt.subtype = MEDIASUBTYPE_RGB24;
amt.formattype = FORMAT_VideoInfo;
for( i = 0 ; i < cFetched ; i++){
CoCreateInstance( CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER,
IID_IBaseFilter, (LPVOID *)& pF[ i ]);
pF[ i ] -> QueryInterface( IID_ISampleGrabber, (void **)&pGrab[ i ] );
// グラバフィルタの挿入場所の特定のための設定
pGrab[ i ] -> SetMediaType( &amt );
// グラバフィルタをフィルタグラフに追加
pGraph -> AddFilter(pF[ i ] , L"SampGrab");
}
// ---- キャプチャグラフの準備 ----
// キャプチャグラフを作る、カメラの数だけ必要
ICaptureGraphBuilder2 * pCapture[10]; // これらは後で解放すること。
for( i =0 ; i < cFetched ; i++){ // 最大カメラ数10
CoCreateInstance( CLSID_CaptureGraphBuilder2 , NULL, CLSCTX_INPROC,
IID_ICaptureGraphBuilder2, (void **) &pCapture[ i ] );
// フィルタグラフをキャプチャグラフに組み込む
pCapture[ i ] -> SetFiltergraph( pGraph );
// キャプチャグラフの設定、グラバをレンダリング出力に設定
pCapture[ i ] -> RenderStream (NULL, NULL,
pbf[ i ] , NULL, pF[ i ]);
printf( "%d 個目のキャプチャグラフ完了 \n", i+1 );
}
// ---- 表示ウィンドウの準備 ----
//IMG0 img00[10]; // 最大10ウインドウ
VIDEOINFOHEADER * pVideoHeader;
char cbuf1[200];
char cbuf2[200];
gr_reg();
// 表示ウィンドウの定義
for( i =0 ; i < cFetched ; i++){
// ビットマップ情報の取得
pGrab[ i ] -> GetConnectedMediaType( &amt );
// ビデオ ヘッダーへのポインタを獲得する。
printf( "カメラ%d amt.lSampleSize = %d (byte)\n", i+1, amt.lSampleSize );
pVideoHeader = (VIDEOINFOHEADER * )amt.pbFormat;
// ビデオ ヘッダーには、ビットマップ情報が含まれる。
// ビットマップ情報を BITMAPINFO 構造体にコピーする。
img00[ i ].bih = pVideoHeader->bmiHeader;
//後で修正する
if(i==0){
n = img00[i].bih.biSizeImage;
printf("long n=%d\n",n);
buffer = (BYTE *)malloc( n );
buf2 = (BYTE *)malloc( n );
buf3 = (BYTE *)malloc( n );
}
if(i==1){
n2 = img00[i].bih.biSizeImage;
printf("long n2=%d\n",n2);
buffer2 = (BYTE *)malloc( n2 );
buf22 = (BYTE *)malloc( n2 );
buf32 = (BYTE *)malloc( n2 );
}
printf( "width = %d , height %d , color %d \n",
img00[ i ].bih.biWidth,
img00[ i ].bih.biHeight,
img00[ i ].bih.biBitCount );
img00[ i ].lpBmpData = (BYTE *)malloc( img00[ i ].bih.biSizeImage );
//-------------------------------------------------
img00[ i ].hi = (HINSTANCE)GetWindowLong( HWND_DESKTOP, GWL_HINSTANCE );
img00[ i ].x = i*400; img00[ i ].y = 0;
img01[ i ].bih = pVideoHeader->bmiHeader;
printf( "width = %d , height %d , color %d \n",
img01[ i ].bih.biWidth,
img01[ i ].bih.biHeight,
img01[ i ].bih.biBitCount );
img01[ i ].lpBmpData = (BYTE *)malloc( img01[ i ].bih.biSizeImage );
img01[ i ].hi = (HINSTANCE)GetWindowLong( HWND_DESKTOP, GWL_HINSTANCE );
img01[ i ].x = i*400; img01[ i ].y = 300;
gr_init( &img00[ i ] ); // 表示ウィンドウを作る
gr_init( &img01[ i ] );
wsprintf( cbuf1, "カメラ %d / %d",i + 1 , cFetched );
SetWindowText( img00[ i ].hwnd, cbuf1 );
wsprintf( cbuf2, "カメラsa %d / %d",i + 1 , cFetched );
SetWindowText( img01[ i ].hwnd, cbuf2 );
}
img00[0].lpBmpData=buffer;
img00[1].lpBmpData=buffer2;
img01[0].lpBmpData=buf2;
img01[1].lpBmpData=buf22;
// ----レンダリング開始,終了 ----
pMC -> Run();// レンダリング開始
double s;
for( i =0 ; i < cFetched ; i++)
pGrab[ i ] -> SetBufferSamples(TRUE); // グラブ開始
int nn = 0; // グラブ回数のカウント、意味はない
int xz;//画面のx座標
int yz;//画面のy座標
int rgb;
int xza,yza;//カメラAの重心の座標
int xzb,yzb;//カメラBの重心の座標
int counta,countb;
double siki,siki2=1;
printf("開始？");
getch();
printf("\n");
while(1){ // 0.5秒に１回グラブ
for( i =0 ; i < cFetched ; i++){
if(i==0) hr = pGrab[ i ] -> GetCurrentBuffer((long *)&(img00[ i ].bih.biSizeImage), (long *)buffer);
if(i==1) hr = pGrab[ i ] -> GetCurrentBuffer((long *)&(img00[ i ].bih.biSizeImage), (long *)buffer2);
}
//差分処理の追加はここ
//ここからカメラAの処理
s=0.0;
xz=0;
yz=0;
xza=0;
yza=0;
rgb=0;
counta=0;
for(j=0;j<(int)n;j++){
buf2[j]=fabs((double)(buffer[j]-buf3[j]));
siki=buf2[j];
//fprintf(fpch,"カメラA座標チェック%d(x,y)=(%d,%d)\n",j,xz,yz);
if(siki>200) {
fprintf(fpla,"カメラA座標(x,y)=(%d,%d) RGB=%dの差分%lf\n",xz,yz,rgb,siki);
xza=xza+xz-160;
yza=yza+120-yz;
counta++;
}
//ここで重心を計算
if(yz==240 && rgb==2){xz++; yz=0;}
switch(rgb){
case 0:
rgb++;
break;
case 1:
rgb++;
break;
case 2:
rgb=0;
yz++;
break;
}
s=s+buf2[j];
}
printf("カメラAの差分値%lf\n",s/n);
if(counta!=0) {
//dfx,dfy,counta,ca
hope(xza,yza,counta,1,&asentx,&asenty);
}
//ここからカメラBの処理
s=0.0;
xz=0;
yz=0;
xzb=0;
yzb=0;
rgb=0;
countb=0;
for(j=0;j<(int)n2;j++){
buf22[j]=fabs((double)(buffer2[j]-buf32[j]));
siki=buf22[j];
//fprintf(fpch,"カメラB座標チェック%d(x,y)=(%d,%d)\n",j,xz,yz);
if(siki>200){
fprintf(fplb,"カメラB座標(x,y)=(%d,%d) RGB=%dの差分%lf\n",xz,yz,rgb,siki);
xzb=xzb+xz-160;
yzb=yzb+120-yz;
countb++;
}
//ここで重心を計算
if(yz==240 && rgb==2){xz++; yz=0;}
switch(rgb){
case 0:
rgb++;
break;
case 1:
rgb++;
break;
case 2:
rgb=0;
yz++;
break;
}
s=s+buf22[j];
}
printf("カメラBの差分値%lf\n",s/n2);
if(countb!=0){
hope(xzb,yzb,countb,2,&bsentx,&bsenty);
}
//asentx=0;//強制テスト
if(asentx!=0 && asenty!=0 && bsentx!=0 && bsenty!=0){
fprintf(fpch,"送信座標チェックA(x,y)=(%d,%d) B(x,y)=(%d,%d)\n",asentx,asenty,bsentx,bsenty);
triangle(asentx,asenty,bsentx,bsenty);
}
//開放処理
for(i =0 ; i < cFetched ; i++){
InvalidateRect( img00[ i ].hwnd, NULL, FALSE);
InvalidateRect( img01[ i ].hwnd, NULL, FALSE);
}
printf("グラブ n = %d\n", nn++ );
if( kbhit() ){
getch(); // kbhitで取得したキーの破棄
pMC -> Pause();
printf( "グラブ停止：終了 -- q 、再開 -- その他のキー\n" );
if(getch() == 'q')
break;
pMC -> Run();
}
CopyMemory( buf3, buffer ,n);
CopyMemory( buf32,buffer2,n2);
//Sleep(100);
}
// ---- 終了処理 ----
// インターフェースのリリース
for( i =0; i < cFetched ; i++){
pGrab[ i ] -> Release();
pF[ i ] -> Release();
pCapture[ i ] -> Release(); // キャプチャグラフ用
pbf[ i ] -> Release(); // キャプチャフィルタ用
free( img00[ i ].lpBmpData );
free( img01[ i ].lpBmpData );
}
pMC -> Release(); // フィルタグラフ用
pGraph -> Release();
// COMのリリース
CoUninitialize();
}
void main()
{
fp=fopen("triangle.txt","w");
fpa=fopen("acamera.txt","w");
fpb=fopen("bcamera.txt","w");
fpla=fopen("iventloga.txt","w");
fplb=fopen("iventlogb.txt","w");
fpch=fopen("allcheck.txt","w");
printf("接続されているカメラが２台であることを確認してください(qで終了)\n");
if(getch()=='q') return;
lease();
fclose(fp);
fclose(fpa);
fclose(fpb);
fclose(fpla);
fclose(fplb);
fclose(fpch);
return;
}
最終更新：2009年11月14日 12:01
