マーカー認識について

[mitsu]

コード:

#include "ardrone/ardrone.h"

#ifdef _WIN32
#include <windows.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#ifndef __APPLE__
#include <GL/gl.h>
#include <GL/glut.h>
#else
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#endif
#include <AR/gsub.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/ar.h>

/*
#include <AR/config.h>
#include <AR/video.h>
#include <AR/param.h>			// arParamDisp()
#include <AR/ar.h>
#include <AR/gsub_lite.h>*/
#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)
// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
char *vconf_name = "Data/WDM_camera_flipV.xml";
int xsize;
int ysize;
int thresh = 100;
int count  = 0;
void arToolKit( IplImage* cvImage );
int              j, k;


char *cparam_name = "Data/camera_para.dat";         // カメラパラメータファイル 
ARParam cparam;  


/* パターンファイル */
int		flag = 0;
char   *patt_name   = "Data/patt.hiro";				// パターンファイル
int    patt_id;										// パターンのID
double patt_trans[3][4];							// 座標変換行列
double patt_center[2] = { 0.0, 0.0 };				// パターンの中心座標
double patt_width     = 80.0;					// パターンのサイズ（単位：ｍｍ）


// 関数宣言
 void Init( void );


int main(int argc,  char **argv)
{
	// GLUTの初期化
	glutInit( &argc, argv );

	// ARアプリケーションの初期化
	Init();

	// カメラからのビデオキャプチャを初期化する
    CvCapture *videoCapture = cvCreateCameraCapture( 1 );
    if( videoCapture == NULL )
    {
        return -1;
	}
	
	// AR.Drone
	ARDrone ardrone;
	Init();
	// 初期化
	if (!ardrone.open()) {
		printf("ARDroneの初期化に失敗しました\n");
		return -1;
	}
	// メインループ
	while (!GetAsyncKeyState(VK_ESCAPE)) {
		// AR.Droneの更新
		if (!ardrone.update()) break;
		// 画像の取得
		IplImage *image = ardrone.getImage();
		
		//OpenCV→ARToolkit
	//	argDrawMode2D();
	//	argDispImage( dataPtr, 0,0 );
		// ナビゲーションデータの取得
		double roll = ardrone.getRoll();
		double pitch = ardrone.getPitch();
		double yaw = ardrone.getYaw();
		printf("ardrone.roll = %3.2f [deg]\n", roll * RAD_TO_DEG);
		printf("ardrone.pitch = %3.2f [deg]\n", pitch * RAD_TO_DEG);
		printf("ardrone.yaw = %3.2f [deg]\n", yaw * RAD_TO_DEG);
		// 高度
		double altitude = ardrone.getAltitude();
		printf("ardrone.altitude = %3.2f [m]\n", altitude);
		// 速度
		double vx, vy, vz;
		double velocity = ardrone.getVelocity(&vx, &vy, &vz);
		printf("ardrone.vx = %3.2f [m/s]\n", vx);
		printf("ardrone.vy = %3.2f [m/s]\n", vy);
		printf("ardrone.vz = %3.2f [m/s]\n", vz);
		// バッテリ残量
		int battery = ardrone.getBatteryPercentage();
		printf("ardrone.battery = %d [％] (残り約%d分)\n", battery, 12*battery/100);
		// 離陸・着陸
		if (KEY_PUSH(VK_SPACE)) {
			if (ardrone.onGround()) ardrone.takeoff();
			else ardrone.landing();
		}
		// ARDroneが飛行状態
		if (!ardrone.onGround()) {
			// 速度指令
			double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
			if (KEY_DOWN(VK_UP)) x = 0.5;
			if (KEY_DOWN(VK_DOWN)) x = -0.5;
			if (KEY_DOWN(VK_LEFT)) r = 0.5;
			if (KEY_DOWN(VK_RIGHT)) r = -0.5;
			if (KEY_DOWN('Q')) z = 0.5;
			if (KEY_DOWN('A')) z = -0.5;
			ardrone.move3D(x, y, z, r);
		}
		// カメラ切り替え
		static int mode = 0;
		if (KEY_PUSH('C')) ardrone.setCamera(++mode%4);
		
		 // 表示
		cvShowImage("camera", image);
		
		
		
		// ARMarker処理
		arToolKit(image);
		
		cvWaitKey(1);
	
	}
	// さようなら
	ardrone.close();
	
	// ビデオキャプチャを解放する
    cvReleaseCapture( &videoCapture );

	
	
	return 0;
}
void Init(void)
{
	 // パターンファイルのロード 
    if( (patt_id = arLoadPatt("Data/patt.hiro")) < 0){ 
		printf("パターンファイルの読み込みに失敗しました\n"); 
        exit(0); 
	}

	printf("マーカ読み込み成功\n");
	flag = 1;

}

 void cleanup(void)
{
    arVideoCapStop();
    arVideoClose();
    argCleanup();
}

/*
 * ARToolKit用関数
 * メインループから呼び出される
 * 参考：http://www.cg-ya.net/imedia/ar/artoolkit_base_program/
 */
void arToolKit( IplImage* cvImage ) 
{
/*	CvSize cvSize = { cvImage->width, cvImage->height };
	IplImage *dst = cvCreateImage( cvSize, cvImage->depth, 4 );
	cvCvtColor( cvImage, dst, CV_BGR2BGRA );
	ARUint8* arImage = reinterpret_cast<ARUint8*>( dst->imageData );
*/
	ARUint8* arImage = (ARUint8*)(cvImage->imageData);
	ARMarkerInfo* marker_info;
	int marker_num;     //マーカーらしきもの

	printf("%d\n", flag );
	// マーカが画像内にあるか
	if( arDetectMarker( arImage, thresh, &marker_info, &marker_num ) < 0 ) {
		printf("マーカなし\n");
		return;
	}	

	// 登録したマーカの一致度比較
	int k = - 1;

	for(  j = 0; j < marker_num; j++ )
	{
		if( patt_id == marker_info[j].id  )
		{
			if( k == -1 ) 
				k = j;
			else if( marker_info[k].cf < marker_info[j].cf )
			printf("登録マーカあり\n");
		}
	}
	
	/*検出したマーカー位置に緑色の四角形を描画*/
	glColor3f(0.0, 1.0, 0.0);    //緑色
	glLineWidth(5.0);
	argDrawSquare(marker_info[k].vertex,0,0); //四角形を描画
	

	// 登録したマーカが存在しない場合
	if( k == -1 ) 
	{
		printf("登録マーカなし\n");
		return;
	}
	flag++;
	// マーカの位置・姿勢
	arGetTransMat( &marker_info[k], patt_center, patt_width, patt_trans );

	// マーカの情報表示
	printf( "信頼度：%f\n", marker_info->cf);	
	printf( "マーカの中心座標　x:%5f　y:%5f\nn", marker_info->pos[0], marker_info->pos[1] );
}

上記の "if( arDetectMarker( arImage, thresh, &marker_info, &marker_num ) < 0 )"
で　変数に数値が入らず "一致度比較処理で "marker_num" が0より高い数値にならず、登録とロードができたはずなのに"Data/patt.hiro"のマーカーを "ar drone"のカメラに向けてもマーカーが見つからず困っています
アドバイスをいただきたいです。