マルチカメラ応用位置決め
[ビジョンツール]

説明

ライブラリの説明
本ライブラリは、ＸＹθ移動可能なステージを用いてアライメントを行うことを目的とした計算ライブラリです。 つまり画像処理を行うライブラリではなく、画像処理で得られた結果に対して処理を行うライブラリです。 特に複数の狭視野のカメラを用いて高精度にアライメントを行う用途を想定しています。 用途は特に限定されるものではありませんが、液晶ガラスの張り合わせや、 プリント基板印刷の位置決めといったような用途で用いられます。
xyqn_purpose.png

Fig1. ライブラリの目的

使用の際の前提条件
ＸＹθ移動ステージと連動するライブラリなので、ステージと連携を取ることが必須になります。 つまり、次に挙げるような環境が揃っていることが前提となります。 また、カメラが移動する場合には対応せず、基本的にカメラが固定された環境下で使用します。
  • ステージに対して任意のＸＹθ位置に移動する命令を与えることが可能である
  • ステージの現在位置を取得することが可能である
  • ステージ移動正方向が次の条件を満たしていること
    • ステージＸ軸正方向をステージＹ軸正方向に回転させた場合の方向(時計/反時計回り)と、それを画像に映して見る方向との関係に変化がないこと。
    • また、その回転方向が、ステージθ正方向と同じであること
      xyqn_system.png

      Fig2. 使用するシステム

特長
液晶ガラスの張り合わせなどを例に取ると、たたみ１畳以上の大きさのものをμmレベルの精度でアライメントしたいという要求になることがあります。 その際、位置決めに使用する基準マークは、一般的に複数個用意され、ガラス基板の端に非常に小さく配置されます。 このような状況では、１カメラで全てのアライメントマークを撮像しようとしても、視野が広すぎて小さなアライメントマークを計測することが困難です。 そのため、複数のカメラを用いて個別にアライメントマークを撮像することになりますが、 カメラとステージの位置関係、カメラの撮像倍率が既知でなければ正確なアライメントができません。 さらにそれがカメラの個数分必要になり、その調整は非常に煩わしいものになります。 このような状況で本ライブラリを用いることで、ステージ移動とマーク計測を繰り返すことによってカメラとステージの位置関係を半自動で求め、 手間を掛けずにアライメントまで行うことが可能になります。
xyqn_example.png

Fig3. アライメントの例（パネルの張り合わせ）

サンプルコード
マルチカメラ応用位置決めライブラリを使用して効果的にアライメントを行うためには、ある程度決まった手順でライブラリをコールする必要があります。 本サンプルコードを参照して、ステージ位置情報やマーク計測等のデータ生成、ライブラリのコール手順等を参考にしてください。
また、ステージの現在位置の取得やステージに対しての移動指令を行いながら使用するため、通常はサンプルコードだけでは実現不可能です。 そこで、本サンプルコードではステージの移動やモニタに映るマークの計測値は、計算によるシミュレーションデータを利用して実現しています。
なお、本シミュレーションでは画像の計測誤差、レンズ歪み、機械の移動誤差等の悪条件はまったく加味しておりません。 よって、ここで得られたキャリブレーション、及びアライメントの収束回数及び精度は、実際にアライメントを行う場合の精度を保証するものではないことを予めご了承の上でご利用下さい。

※本サンプルコードが正常終了した場合には「XYQPARAM.DAT」というファイルを作成します。
/* マルチカメラ応用位置決めライブラリサンプルコード(シミュレータ付属) */
/* エラー処理を省略している箇所がありますので注意して下さい。*/
#include    <math.h>
#include    <stdio.h>
#include    "fie.h"

#define     DATA_FILE       "XYQPARAM.DAT"  /* データセーブ用 */
#define     CHAN_N          2               /* カメラ数       */

/* 計算用の諸定義 */
#define     PI_DEG          180.0               /* π(度)  */
#define     PI_RAD          3.14159265358979    /* π(rad) */

/**********************************
  カメラパラメータ
 **********************************/
#define     CAM1_X      ( - 24.0 )                          /* 固定カメラ１取り付け位置Ｘ                  */
#define     CAM1_Y      ( - 24.0 )                          /* 固定カメラ１取り付け位置Ｙ                  */
#define     CAM1_Q      (   30.0 * PI_RAD / PI_DEG )        /* 固定カメラ１取り付け位置θ(rad)              */
#define     CAM1_RES    (    1.0 )                          /* 固定カメラ１スケール値 1(画素) ＝ dRes(mm) */
#define     CAM2_X      ( -  6.0 )                          /* 固定カメラ２取り付け位置Ｘ                  */
#define     CAM2_Y      ( - 18.0 )                          /* 固定カメラ２取り付け位置Ｙ                  */
#define     CAM2_Q      (    0.0 * PI_RAD / PI_DEG )        /* 固定カメラ２取り付け位置θ(rad)              */
#define     CAM2_RES    (    1.0 )                          /* 固定カメラ２スケール値 1(画素) ＝ dRes(mm) */

/**********************************
  キャリブレーションパラメータ
 **********************************/
#define     CLB_MARK1_X     ( -  6.0 )                      /* ステージ上のキャリブレーションマーク１Ｘ座標   */
#define     CLB_MARK1_Y     ( -  6.0 )                      /* ステージ上のキャリブレーションマーク１Ｙ座標   */
#define     CLB_MARK2_X     (    6.0 )                      /* ステージ上のキャリブレーションマーク２Ｘ座標   */
#define     CLB_MARK2_Y     ( -  6.0 )                      /* ステージ上のキャリブレーションマーク２Ｙ座標   */

#define     SFT_X           (    6.0 )                      /* キャリブレーションＸ方向シフト量(mm)     */
#define     SFT_Y           ( -  6.0 )                      /* キャリブレーションＹ方向シフト量(mm)     */
#define     ROT_Q           (   60.0 * PI_RAD / PI_DEG )    /* キャリブレーション総回転量(rad)
                                                                ±x度ならばこの値は２・x度になる      */
#define     CORREC_COUNT    3                               /* 補正計算回数                           */

/**********************************
    アライメントパラメータ
 **********************************/
#define     ATGT_MARK1_X    ( -  6.0 )      /* ステージ上のターゲットマーク１Ｘ座標       */
#define     ATGT_MARK1_Y    ( -  6.0 )      /* ステージ上のターゲットマーク１Ｙ座標       */
#define     ATGT_MARK2_X    (    6.0 )      /* ステージ上のターゲットマーク２Ｘ座標       */
#define     ATGT_MARK2_Y    ( -  6.0 )      /* ステージ上のターゲットマーク２Ｙ座標       */

#define     AOBJ_MARK1_X    ( -  6.0 )      /* ステージ上のオブジェクトマーク１Ｘ座標    */
#define     AOBJ_MARK1_Y    ( - 12.0 )      /* ステージ上のオブジェクトマーク１Ｙ座標    */
#define     AOBJ_MARK2_X    (    6.0 )      /* ステージ上のオブジェクトマーク２Ｘ座標    */
#define     AOBJ_MARK2_Y    ( - 12.0 )      /* ステージ上のオブジェクトマーク２Ｙ座標    */

#define     LIMIT           0.01            /* アライメント精度（mm）             */
#define     RETRY_N         3               /* 精度が未達の場合の最大繰り返し回数  */


/* 内部で定義した関数  */
static  VOID    calc_correct_calib_pos( DOUBLE, DOUBLE, DOUBLE, DOUBLE *, DOUBLE *, DOUBLE * );
static  INT     calibration( DOUBLE, DOUBLE, DOUBLE, char[] );
static  INT     alignment( char[] );

/* シミュレーションデータ作成用関数 */
static  INT     simu_vTt( DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE, DOUBLE *, DOUBLE * );


/************
 メイン関数
 ************/
VOID main( VOID )
{
    // FIEライブラリの使用前に必ずコールする必要があります。
    fnFIE_setup();

    /* キャリブレーション実行 */
    if( F_ERR_NONE == calibration( SFT_X, SFT_Y, ROT_Q, DATA_FILE ) ){
        /* アライメント実行 */
        alignment( DATA_FILE );
    }

    // 終了処理
    fnFIE_teardown();
}

/* ファイル書き込み用関数 */
static INT write_data( VOID *data, INT sz, VOID* strm ){
    return (INT)fwrite( data, 1, sz, (FILE*)strm );
}

/* ファイル読込用関数 */
static INT read_data( VOID *data, INT sz, VOID* strm ){
    return (INT)fread( data, 1, sz, (FILE*)strm );
}


/* カメラとＸＹθステージのキャリブレーション */
static  INT calibration( 
DOUBLE shift_x,         /* 入力：キャリブレーションステージ平行Ｘ移動量 */
DOUBLE shift_y,         /* 入力：キャリブレーションステージ平行Ｙ移動量 */
DOUBLE rot_q,           /* 入力：キャリブレーションステージ回転θ移動量(rad) */
char    *file_name      /* 入力：パラメータを保存するファイルの名称 */ ){
    INT     ch;
    INT     status = -1;
    INT     loop;
    DPNT_T  posit[CHAN_N][4];
    DPNT_T  correc[CHAN_N][2], center[CHAN_N];
    DOUBLE  x_cor, y_cor, q_cor;
    DOUBLE  d_tXm[CHAN_N],d_tYm[CHAN_N];
    DOUBLE  now_x, now_y,now_q;
    FHANDLE hxyqn;
    const DOUBLE cam_x[CHAN_N]   = { CAM1_X, CAM2_X };
    const DOUBLE cam_y[CHAN_N]   = { CAM1_Y, CAM2_Y };
    const DOUBLE cam_q[CHAN_N]   = { CAM1_Q, CAM2_Q };
    const DOUBLE cam_res[CHAN_N] = { CAM1_RES, CAM2_RES };
    
    fvstream    strm;   // データストリーム
    
    /*  キャリブレーションシステムのオープン  */
    if( NULL == ( hxyqn = fnFIE_xyqn_open() ) ) goto exit;
    
    /* キャリブレーションマーク */
    d_tXm[0] = CLB_MARK1_X; d_tYm[0] = CLB_MARK1_Y;
    d_tXm[1] = CLB_MARK2_X; d_tYm[1] = CLB_MARK2_Y;

    for( ch=0; ch<CHAN_N; ch++ ){
        /* 「カメラ座標系上の基準点」の座標取得 */
        now_x  = 0.0;       now_y  = 0.0;       now_q  = 0.0;
        simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch],
                  now_x, now_y, now_q,
                  d_tXm[ch], d_tYm[ch],
                  &posit[ch][0].x, &posit[ch][0].y );

        /* 「カメラ座標系上のシフト参照点」の座標取得 */
        now_x  = shift_x;       now_y = shift_y;        now_q  = 0.0;
        simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch],
                  now_x, now_y, now_q,
                  d_tXm[ch], d_tYm[ch],
                  &posit[ch][1].x, &posit[ch][1].y );

        /* 「カメラ座標系上の＋側回転参照点」の座標取得 */
        now_x  = 0.0;       now_y = 0.0;        now_q  = rot_q / 2.0;
        simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch],
                  now_x, now_y, now_q,
                  d_tXm[ch], d_tYm[ch],
                  &posit[ch][2].x, &posit[ch][2].y );

        /* 「カメラ座標系上の−側回転参照点」の座標取得 */
        now_x  = 0.0;       now_y = 0.0;        now_q  = - rot_q / 2.0;
        simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch],
                  now_x, now_y, now_q,
                  d_tXm[ch], d_tYm[ch],
                  &posit[ch][3].x, &posit[ch][3].y );
    }

    printf( "-- CALIBRATION START --\n\r" );
    printf( "OPEN DATA\n\r" );
    printf( "MRK1 BASE (%8.3f, %8.3f)(画素,画素)\n\r", posit[0][0].x, posit[0][0].y );
    printf( "MRK1 SFT  (%8.3f, %8.3f)(画素,画素)\n\r", posit[0][1].x, posit[0][1].y );
    printf( "MRK1 ROT+ (%8.3f, %8.3f)(画素,画素)\n\r", posit[0][2].x, posit[0][2].y );
    printf( "MRK1 ROT- (%8.3f, %8.3f)(画素,画素)\n\r", posit[0][3].x, posit[0][3].y );
    printf( "MRK2 BASE (%8.3f, %8.3f)(画素,画素)\n\r", posit[1][0].x, posit[1][0].y );
    printf( "MRK2 SFT  (%8.3f, %8.3f)(画素,画素)\n\r", posit[1][1].x, posit[1][1].y );
    printf( "MRK2 ROT+ (%8.3f, %8.3f)(画素,画素)\n\r", posit[1][2].x, posit[1][2].y );
    printf( "MRK2 ROT- (%8.3f, %8.3f)(画素,画素)\n\r", posit[1][3].x, posit[1][3].y );

    /*****************************
     キャリブレーションのオープン
     *****************************/
    if( F_ERR_NONE != fnFIE_xyqn_calc_table( hxyqn, shift_x, shift_y, rot_q, CHAN_N, (const DPNT_T *)posit ) )
        goto exit;

    /* 補正を繰り返すことでキャリブレーションの精度を高めることが可能です。 */
    for( loop = 0; loop < CORREC_COUNT; loop++ ){
        /*******************************************
        「ＸＹθステージ座標系上の基準点」座標の取得
         *******************************************/
        fnFIE_xyqn_get_center( hxyqn, center );
        /* 誤差が全く無い場合には、ここで求まる center の値 CLB_MARKn_X(Y)と同じ値になります */
        printf( "CENTER DATA:%d\n\r", loop + 1 );
        printf( "MRK1 CENT (%8.3f, %8.3f)(mm,mm)\n\r", center[0].x, center[0].y );
        printf( "MRK2 CENT (%8.3f, %8.3f)(mm,mm)\n\r", center[1].x, center[1].y );

        for( ch=0; ch<CHAN_N; ch++ ){
            /* 「ＸＹθステージ座標系上の基準点」を中心として＋Δθ移動した時の座標取得 */
            calc_correct_calib_pos( center[ch].x, center[ch].y,   rot_q / 2.0, &x_cor, &y_cor, &q_cor );
            now_x  = x_cor;     now_y = y_cor;      now_q  = q_cor;
            simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch], 
                      now_x, now_y, now_q,
                      d_tXm[ch], d_tYm[ch],
                      &correc[ch][0].x, &correc[ch][0].y );

            /* 「ＸＹθステージ座標系上の基準点」を中心として−Δθ移動した時の座標取得 */
            calc_correct_calib_pos( center[ch].x, center[ch].y, - rot_q / 2.0, &x_cor, &y_cor, &q_cor );
            now_x  = x_cor;     now_y = y_cor;      now_q  = q_cor;
            simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch], 
                      now_x, now_y, now_q,
                      d_tXm[ch], d_tYm[ch],
                      &correc[ch][1].x, &correc[ch][1].y );
        }

        /*******************************************
        「ＸＹθステージ座標系上の基準点」の座標補正
         *******************************************/
        fnFIE_xyqn_correction( hxyqn, (const DPNT_T *)correc );
    }
    /* 誤差が全く無い場合には、ここで求まる correc はBASEと同じ値である
      （ステージは動いているが、画面上に写る(計測される)マーク位置は移動しない）*/
    printf( "CORREC DATA:%d\n\r", loop + 1 );
    printf( "MRK1 CROT+(%8.3f, %8.3f)(画素,画素)\n\r", correc[0][0].x, correc[0][0].y );
    printf( "MRK1 CROT-(%8.3f, %8.3f)(画素,画素)\n\r", correc[0][1].x, correc[0][1].y );
    printf( "MRK2 CROT+(%8.3f, %8.3f)(画素,画素)\n\r", correc[1][0].x, correc[1][0].y );
    printf( "MRK2 CROT-(%8.3f, %8.3f)(画素,画素)\n\r", correc[1][1].x, correc[1][1].y );

    // ストリームの準備
    strm.read   = read_data;
    strm.write  = write_data;
    strm.stream = NULL;
    /************************************
     パラメータステージをファイルに保管 
     ************************************/
    strm.stream = fopen( file_name, "wb" );
    fnFIE_xyqn_save( hxyqn, &strm );
    fclose( (FILE*)strm.stream );
    strm.stream = NULL;
    
    status = 0;
exit:
    fnFIE_free_object( hxyqn );
    
    return( status );
}


/* 位置合わせ */
static  INT alignment( char * file_name /* 入力：使用するパラメータファイルの名称 */ ){
    INT     ch;
    INT     status = -1;
    INT     retry, err_cnt;
    DOUBLE  x, y, q;
    DOUBLE  now_x, now_y, now_q;
    DPNT_T  base_on_cam[CHAN_N], mark_on_cam[CHAN_N];
    DPNT_T  base_on_xyq[CHAN_N], mark_on_xyq[CHAN_N];
    DPNT_T  residual[CHAN_N];
    FHANDLE hxyqn;
    
    const DOUBLE cam_x[CHAN_N]   = { CAM1_X, CAM2_X };
    const DOUBLE cam_y[CHAN_N]   = { CAM1_Y, CAM2_Y };
    const DOUBLE cam_q[CHAN_N]   = { CAM1_Q, CAM2_Q };
    const DOUBLE cam_res[CHAN_N] = { CAM1_RES, CAM2_RES };

    DOUBLE  d_tXtm[CHAN_N], d_tYtm[CHAN_N];
    DOUBLE  d_tXom[CHAN_N], d_tYom[CHAN_N];
 
    fvstream    strm;   // データストリーム

    /*  キャリブレーションシステムのオープン  */
    if( NULL == ( hxyqn = fnFIE_xyqn_open() ) ) goto exit;

    /* アライメントターゲットマークステージ上の位置 */
    d_tXtm[0] = ATGT_MARK1_X;           d_tYtm[0] = ATGT_MARK1_Y;
    d_tXtm[1] = ATGT_MARK2_X;           d_tYtm[1] = ATGT_MARK2_Y;

    /* アライメントオブジェクトマークステージ上の位置 */
    d_tXom[0] = AOBJ_MARK1_X;           d_tYom[0] = AOBJ_MARK1_Y;
    d_tXom[1] = AOBJ_MARK2_X;           d_tYom[1] = AOBJ_MARK2_Y;

    // ストリームの準備
    strm.read   = read_data;
    strm.write  = write_data;
    strm.stream = NULL;
    
    /***************************************************
                アライメントデータのオープン
     ***************************************************/
    strm.stream = fopen( file_name, "rb" );
    if( F_ERR_NONE != fnFIE_xyqn_load( hxyqn, &strm ) ) goto exit;
    fclose( (FILE*)strm.stream );

    /* ステージ初期位置は原点 */
    now_x = 0.0;    now_y = 0.0;    now_q = 0.0;
    
    /* 「カメラ座標系上の到達点（ターゲット位置）」の座標取得 */
    for( ch=0; ch<CHAN_N; ch++ ){
        simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch],
                  now_x, now_y, now_q,
                  d_tXtm[ch], d_tYtm[ch],
                  &base_on_cam[ch].x, &base_on_cam[ch].y );
    }
    
    /************************************************
      「ｘｙθステージ座標系上の到達点」の座標算出
     ************************************************/
    fnFIE_xyqn_trans_coord( hxyqn, base_on_cam, base_on_xyq );

    for( retry = 0; retry < RETRY_N; retry++ ){
        /* 「カメラ座標系上の対象点（オブジェクト位置）」の座標取得 */
        for( ch=0; ch<CHAN_N; ch++ ){
            simu_vTt( cam_x[ch], cam_y[ch], cam_q[ch], cam_res[ch], 
                      now_x, now_y, now_q,
                      d_tXom[ch], d_tYom[ch],
                      &mark_on_cam[ch].x, &mark_on_cam[ch].y );
        }
        
        /************************************************
          「ｘｙθステージ座標系上の対象点」の座標算出 
         ************************************************/
        fnFIE_xyqn_trans_coord( hxyqn, mark_on_cam, mark_on_xyq );

        /* 到達点と対象点の距離を計算 */
        /* 現在のステージ上のマーク位置のずれをチェック */
        for( ch = 0; ch < CHAN_N; ch++ ){
            residual[ch].x = base_on_xyq[ch].x - mark_on_xyq[ch].x;
            residual[ch].y = base_on_xyq[ch].y - mark_on_xyq[ch].y;

            /* 状況報告 */
            printf( "MRK%2d DIFF(%8.3f, %8.3f)(mm,mm)\n\r", ch, residual[ch].x, residual[ch].y );
        }
        
        err_cnt = 0;
        
        /* マーク間距離の検証 */
        for( ch = 0; ch < CHAN_N; ch++ ){
            /* アライメントの規格として、マーク間距離を判断 */
            if( ( LIMIT<fabs( residual[ch].x ) ) || ( LIMIT<fabs( residual[ch].y )) ){
                err_cnt++;
                break;
            }
        }

        if( err_cnt ){
            /****************************
                   アライメント実行
             ****************************/
            if( F_ERR_NONE != fnFIE_xyqn_trans_execute( hxyqn, base_on_cam, mark_on_cam, now_x, now_y, now_q, &x, &y, &q ) )
                goto exit;
            
            printf( "ALIGN:%d\n\r", retry + 1 );
            printf( "PRE TPOS  (%8.3f, %8.3f, %8.3f)(mm,mm,度)\n\r", now_x, now_y, now_q * PI_DEG / PI_RAD );
            printf( "NXT TPOS  (%8.3f, %8.3f, %8.3f)(mm,mm,度)\n\r", x, y, q * PI_DEG / PI_RAD );
            /* 現在のＸＹθステージ位置・姿勢を更新する  */
            now_x = x;      now_y = y;      now_q = q;
        }else{
            /* アライメントが規格内に収まった */
            break;
        }
    }
    
    //アライメント結果表示（正常終了時にはステージを移動させた回数も）
    if( err_cnt ) printf( "ALIGNMENT FAILED !!\n\r" );
    else          printf( "ALIGNMENT SUCCESS !!(%d)\n\r", retry );

    status = 0;
exit:
    fnFIE_free_object( hxyqn );

    return( status );
}



/****************************************************************
    fnFIE_xyqn_correction();に使用するステージ位置の計算
    下記パラメータ(xc,xc)を中心にステージをqc回転させる時のステージの移動量を求めている。
 ****************************************************************/
static  VOID    calc_correct_calib_pos(
DOUBLE  xc,         /* 入力：ＸＹθステージ座標系上の基準点Ｘ座標                       */
DOUBLE  yc,         /* 入力：ＸＹθステージ座標系上の基準点Ｙ座標                       */
DOUBLE  qc,         /* 入力：ＸＹθステージ座標系上の基準点を中心としてqc分回転させる  */
DOUBLE  *x_cor,     /* 出力：ＸＹθステージのＸ移動量                                 */
DOUBLE  *y_cor,     /* 出力：ＸＹθステージのＹ移動量                                 */
DOUBLE  *q_cor      /* 出力：ＸＹθステージのθ移動量                                  */ ){
    DOUBLE  ksin, kcos;
    DPNT_T  p_new, p_old;

    p_old.x = xc;   p_old.y = yc;

    ksin = sin( qc );   kcos = cos( qc );

    p_new.x = p_old.x * kcos - p_old.y * ksin;
    p_new.y = p_old.x * ksin + p_old.y * kcos;

    *x_cor = p_old.x - p_new.x;
    *y_cor = p_old.y - p_new.y;
    *q_cor = qc;
}



/******************************************
   カメラ シミュレーションデータの生成  ステージ上のマーク位置をビデオ座標上の計測位置にする。
 ******************************************/
static  INT simu_vTt(
DOUBLE  dX,             /* 入力：カメラ取り付け位置Ｘ              */
DOUBLE  dY,             /* 入力：カメラ取り付け位置Ｙ              */
DOUBLE  dQ,             /* 入力：カメラ取り付け位置θ(rad)          */
DOUBLE  dRes,           /* 入力：スケール値 1(画素) ＝ dRes(mm)      */
DOUBLE  dTblX,          /* 入力：ステージ位置Ｘ                       */
DOUBLE  dTblY,          /* 入力：ステージ位置Ｙ                       */
DOUBLE  dTblQ,          /* 入力：ステージ位置θ(rad)               */
DOUBLE  d_tXm,          /* 入力：ステージ上のマーク位置Ｘ            */
DOUBLE  d_tYm,          /* 入力：ステージ上のマーク位置Ｙ            */
DOUBLE  *dp_vXm,        /* 出力：画面に映った上のマークの計測位置Ｘ */
DOUBLE  *dp_vYm         /* 出力：画面に映った上のマークの計測位置Ｙ */ ){
    INT     status = -1;
    DOUBLE  rad_q;
    FMATRIX *mat_res = NULL, *imat_cam = NULL, *mat_tbl = NULL, *mat_wrk = NULL;
    FMATRIX *mat_vTt = NULL;
    FVECTOR *vect_v  = NULL, *vect_t = NULL;

    if ( NULL == ( mat_res  = fnFIE_mat_aalloc( 3, 3 ) ) ) goto exit;
    if ( NULL == ( imat_cam = fnFIE_mat_aalloc( 3, 3 ) ) ) goto exit;
    if ( NULL == ( mat_tbl  = fnFIE_mat_aalloc( 3, 3 ) ) ) goto exit;
    if ( NULL == ( mat_vTt  = fnFIE_mat_aalloc( 3, 3 ) ) ) goto exit;
    if ( NULL == ( mat_wrk  = fnFIE_mat_aalloc( 3, 3 ) ) ) goto exit;
    if ( NULL == ( vect_v   = fnFIE_mat_valloc( 3 ) ) ) goto exit;
    if ( NULL == ( vect_t   = fnFIE_mat_valloc( 3 ) ) ) goto exit;
    
    //スケール
    mat_res->m[0][0] = cos(0.0)/dRes; mat_res->m[0][1] = -sin(0.0)/dRes; mat_res->m[0][2] = 0.0;
    mat_res->m[1][0] = sin(0.0)/dRes; mat_res->m[1][1] =  cos(0.0)/dRes; mat_res->m[1][2] = 0.0;
    mat_res->m[2][0] = 0.0;           mat_res->m[2][1] = 0.0;            mat_res->m[2][2] = 1.0;
    
    //カメラ位置
    rad_q = dQ;
    imat_cam->m[0][0] = cos(rad_q); imat_cam->m[0][1] = -sin(rad_q); imat_cam->m[0][2] = dX;
    imat_cam->m[1][0] = sin(rad_q); imat_cam->m[1][1] =  cos(rad_q); imat_cam->m[1][2] = dY;
    imat_cam->m[2][0] = 0.0;        imat_cam->m[2][1] = 0.0;         imat_cam->m[2][2] = 1.0;
    fnFIE_mat_inverse( imat_cam );
    
    //ステージ
    rad_q = dTblQ;
    mat_tbl->m[0][0] = cos(rad_q); mat_tbl->m[0][1] = -sin(rad_q); mat_tbl->m[0][2] = dTblX;
    mat_tbl->m[1][0] = sin(rad_q); mat_tbl->m[1][1] =  cos(rad_q); mat_tbl->m[1][2] = dTblY;
    mat_tbl->m[2][0] = 0.0;        mat_tbl->m[2][1] = 0.0;         mat_tbl->m[2][2] = 1.0;
    
    //ステージ上の座標->(ワールド座標->)カメラ座標変換行列を生成
    fnFIE_mat_mul_aa( mat_res, imat_cam, mat_wrk );
    fnFIE_mat_mul_aa( mat_wrk, mat_tbl, mat_vTt );

    //座標変換
    vect_t->v[0] = d_tXm;
    vect_t->v[1] = d_tYm;
    vect_t->v[2] = 1.0;
    fnFIE_mat_mul_av( mat_vTt, vect_t, vect_v );

    *dp_vXm = vect_v->v[0];
    *dp_vYm = vect_v->v[1];
    status = 0;
exit:
    fnFIE_mat_afree( imat_cam );
    fnFIE_mat_afree( mat_res );
    fnFIE_mat_afree( mat_tbl );
    fnFIE_mat_afree( mat_vTt );
    fnFIE_mat_afree( mat_wrk );
    fnFIE_mat_vfree( vect_t );
    fnFIE_mat_vfree( vect_v );
    
    return( status );
}


関数

FHANDLE FVALGAPI fnFIE_xyqn_open (VOID)
 キャリブレーションシステムのオープン
INT FVALGAPI fnFIE_xyqn_calc_table (FHANDLE hcalib, DOUBLE delt_x, DOUBLE delt_y, DOUBLE delt_q, INT channels, const DPNT_T *pos_tbl)
 キャリブレーションテーブルの計算
INT FVALGAPI fnFIE_xyqn_get_channels (FHANDLE hcalib, INT *channels)
 カメラチャネル数の取得
INT FVALGAPI fnFIE_xyqn_get_center (FHANDLE hcalib, DPNT_T *ch_center)
 補正計算用回転中心の取得
INT FVALGAPI fnFIE_xyqn_correction (FHANDLE hcalib, const DPNT_T *pos_tbl)
 基準点座標の補正計算
INT FVALGAPI fnFIE_xyqn_trans_coord (FHANDLE hcalib, const DPNT_T *inp, DPNT_T *out)
 カメラ座標からＸＹθテーブル座標へ変換
INT FVALGAPI fnFIE_xyqn_trans_execute (FHANDLE hcalib, const DPNT_T *base_on_cam, const DPNT_T *mark_on_cam, DOUBLE xo, DOUBLE yo, DOUBLE qo, DOUBLE *x, DOUBLE *y, DOUBLE *q)
 位置あわせをする際の位置・姿勢算出
INT FVALGAPI fnFIE_xyqn_load (FHANDLE hcalib, fvstream *strm)
 キャリブレーションデータのストリーム読み込み
INT FVALGAPI fnFIE_xyqn_save (FHANDLE hcalib, fvstream *strm)
 キャリブレーションデータのストリーム書き込み
INT FVALGAPI fnFIE_xyqn_get_status (FHANDLE hcalib, INT *status)
 キャリブレーション状態の確認

関数

FHANDLE FVALGAPI fnFIE_xyqn_open ( VOID   ) 

キャリブレーションシステムのオープン

キャリブレーションシステムをオープンします。 本関数でオープンしたオブジェクトは不要になったら fnFIE_free_object() にて解放してください。

戻り値:
正常に終了した場合は、キャリブレーションシステムのハンドルを返します。 異常終了した場合は NULL を返します。

INT FVALGAPI fnFIE_xyqn_calc_table ( FHANDLE  hcalib,
DOUBLE  delt_x,
DOUBLE  delt_y,
DOUBLE  delt_q,
INT  channels,
const DPNT_T *  pos_tbl 
)

キャリブレーションテーブルの計算

与えられたシフト量・回転量及び座標値群から ｘｙθテーブルによる位置合わせに必要なパラメータテーブルを生成します。

最終目標であるアライメントを行うために、ステージとカメラの位置関係を求める必要があります。 その際に、ステージを指定通りに移動させ、『ステージの移動量』と『それに追従して移動するマークの計測位置』 の関係を用いることでステージとカメラの位置関係を計算します。 そのために本ライブラリでは、ステージに対して「ＸＹの平行移動」、「＋回転」、「−回転」への指定量の移動が必要になります。

pos_tbl は、テーブルを平行移動(移動量：delt_x , delt_y )、＋回転(移動量：delt_q / 2.0 )、−回転(移動量：- delt_q / 2.0 )を行ったときのマーク位置配列で、 次のような配置で、 チャネル( channels )数分だけ配置します。

		pos_tbl[0] ch0 の基準マーク位置座標
		pos_tbl[1] ch0 の平行移動時マーク位置座標
		pos_tbl[2] ch0 の＋回転時マーク位置座標
		pos_tbl[3] ch0 の−回転時マーク位置座標
		pos_tbl[4] ch1 の基準マーク位置座標
		pos_tbl[5] ch1 の平行移動時マーク位置座標
		pos_tbl[6] ch1 の＋回転時マーク位置座標
		pos_tbl[7] ch1 の−回転時マーク位置座標
		pos_tbl[8] ch2 の基準マーク位置座標
		・・・
		・・・
		・・・
		pos_tbl[ ( channels - 2 ) * 4 + 3 ] ch( channels -2) の−回転時マーク位置座標
		pos_tbl[ ( channels - 1 ) * 4     ] ch( channels -1) の基準マーク位置座標
		pos_tbl[ ( channels - 1 ) * 4 + 1 ] ch( channels -1) の平行移動時マーク位置座標
		pos_tbl[ ( channels - 1 ) * 4 + 2 ] ch( channels -1) の＋回転時マーク位置座標
		pos_tbl[ ( channels - 1 ) * 4 + 3 ] ch( channels -1) の−回転時マーク位置座標

xyqn_calib.png

fig.キャリブレーションに使用するデータ（チャネル( channels )数は 2）

引数:
[in,out] hcalib キャリブレーションシステムのハンドル
[in] delt_x 平行移動キャリブレーション時のＸ方向シフト量Δｘ(mm):符号付きの数値で指定
[in] delt_y 平行移動キャリブレーション次のＹ方向シフト量Δｙ(mm):符号付きの数値で指定
[in] delt_q 回転量Δθ(rad):符号無しの数値で指定
[in] channels カメラチャネル数(２以上)
[in] pos_tbl キャリブレーションデータテーブル
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NOMEMORY メモリ不足
F_ERR_CALC_IMPOSSIBLE 与えられた座標で計算不能
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー
注意:
ステージ移動後にマークを計測する関係上、移動量はマークが視野の外に出ない程度の範囲で移動量を指定してください(なるべく大きな値を指定した方が精度には有利です)。 本関数だけでもキャリブレーションは終了しますが、一般的に精度があまり良くありません。更なる精度向上のために fnFIE_xyqn_correction() を使用してください。

INT FVALGAPI fnFIE_xyqn_get_channels ( FHANDLE  hcalib,
INT *  channels 
)

カメラチャネル数の取得

fnFIE_xyqn_calc_table() 実行時に指定されたカメラチャネル数を取得します。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[out] channels カメラチャネル数
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

INT FVALGAPI fnFIE_xyqn_get_center ( FHANDLE  hcalib,
DPNT_T *  ch_center 
)

補正計算用回転中心の取得

ｘｙθテーブル座標系上の各々の基準点座標を返します。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[out] ch_center 各チャネルの基準点座標配列
fnFIE_xyqn_calc_table() 実行時に指定したチャネル数分の領域が必要です。
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

INT FVALGAPI fnFIE_xyqn_correction ( FHANDLE  hcalib,
const DPNT_T *  pos_tbl 
)

基準点座標の補正計算

fnFIE_xyqn_calc_table() で計算された基準点座標を補正します。

弊社では、本関数の使用により、一般的に考えられるキャリブレーション手法に一工夫加えることで補正を行い、より精度の良いキャリブレーションを実現しております。
なお、補正計算処理には適切なデータを入力する必要がありますので、次の手順で使用してください。

xyqn_correc.png

fig.基準点座標補正の手順

※ステージ補正移動の計算について
補正計算にも、ステージ移動とマーク計測を利用します。そのため、所定の位置にステージを移動させてマークの計測を行う必要があります。
補正のためのステージ位置 (pos_x,pos_y) の計算方法については、次の計算式を用いてください。
fnFIE_xyqn_get_center() で得られた基準点座標を(x,y)，θ移動量を rev_q とすると、
  • pos_x = x - ( x・cos(rev_q) - y・sin(rev_q) )
  • pos_y = y - ( x・sin(rev_q) + y・cos(rev_q) )

で表され、実際に回転補正を行うための rev_q には fnFIE_xyqn_calc_table()で使用した回転量である delt_q/2.0, -delt_q/2.0 を指定してください。 補正移動のイメージとしては、キャリブレーション時に使用したマークに対してθ回転を行い、その回転によって移動した量をＸＹ移動で元に戻すような移動です。 キャリブレーション時の精度が良い場合には、±θ補正移動をした後のマークの位置が基準時のマーク位置の近くへと戻ります。

xyqn_correc_mov.png

fig.キャリブレーション補正時のステージ及び、画像上のマーク移動

pos_tbl は、 fnFIE_xyqn_get_center() で得られた回転中心を中心として、 テーブルを＋回転補正移動、−回転補正移動を行ったときのマーク位置配列で、 次のような配置で、 fnFIE_xyqn_calc_table() 実行時に指定したチャネル数分だけ配置します。

		pos_tbl[0] ch0 の＋回転補正時マーク位置座標
		pos_tbl[1] ch0 の−回転補正時マーク位置座標
		pos_tbl[2] ch1 の＋回転補正時マーク位置座標
		pos_tbl[3] ch1 の−回転補正時マーク位置座標
		pos_tbl[4] ch2 の＋回転補正時マーク位置座標
		・・・
		・・・
		・・・
		pos_tbl[ ( channels - 2 ) * 2 + 1 ] ch( channels -2) の−回転補正時マーク位置座標
		pos_tbl[ ( channels - 1 ) * 2 + 0 ] ch( channels -1) の＋回転補正時マーク位置座標
		pos_tbl[ ( channels - 1 ) * 2 + 1 ] ch( channels -1) の−回転補正時マーク位置座標

引数:
[in,out] hcalib キャリブレーションシステムのハンドル
[in] pos_tbl 補正計算用座標値群を格納したテーブル
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NOMEMORY メモリ不足
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー
注意:
補正処理を何度も繰り返すことでキャリブレーションの精度を上げていくことが可能です。終了する目安は、±θ補正移動時の画像上のマーク位置が安定してきたことで判断可能です。 本処理を繰り返すうちにマークが視野外に出てしまう場合には、再度 fnFIE_xyqn_calc_table() からやり直すか、ステージの構成を見直す必要があります。

INT FVALGAPI fnFIE_xyqn_trans_coord ( FHANDLE  hcalib,
const DPNT_T *  inp,
DPNT_T *  out 
)

カメラ座標からＸＹθテーブル座標へ変換

カメラ座標系上の座標値をｘｙθテーブル座標系上の座標値に変換します。

inp と out はカメラ座標の配列です。 要素数は共に fnFIE_xyqn_calc_table() 実行時に指定したチャネル数となります。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[in] inp 各カメラ座標配列（カメラ座標系）
[out] out 各カメラ座標配列（ＸＹθ座標系）
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

INT FVALGAPI fnFIE_xyqn_trans_execute ( FHANDLE  hcalib,
const DPNT_T *  base_on_cam,
const DPNT_T *  mark_on_cam,
DOUBLE  xo,
DOUBLE  yo,
DOUBLE  qo,
DOUBLE *  x,
DOUBLE *  y,
DOUBLE *  q 
)

位置あわせをする際の位置・姿勢算出

与えられた座標値( base_on_cam[] , mark_on_cam[] )を利用してｘｙθテーブルの移動量を決定します。 base_on_cam と mark_on_cam は座標の配列です。 要素数は共に fnFIE_xyqn_calc_table() 実行時に指定したチャネル数となります。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[in] base_on_cam カメラ座標系上のｎチャネル分の基準座標配列
[in] mark_on_cam カメラ座標系上のｎチャネル分のマーク座標配列
[in] xo テーブル現在位置Ｘ座標(mm)
[in] yo テーブル現在位置Ｙ座標(mm)
[in] qo テーブル現在位置θ座標(rad)
[out] x テーブル移動後Ｘ座標(mm)
[out] y テーブル移動後Ｙ座標(mm)
[out] q テーブル移動後θ座標(rad)
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NOMEMORY メモリ不足
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー
注意:
キャリブレーションの精度があまり良くない、または要求精度が厳しい場合等、本処理一回でアライメントの精度が出ないことがあります。 そのような場合は再度ステージを移動した後にマークを撮像し、繰り返しアライメントを行ってください。

INT FVALGAPI fnFIE_xyqn_load ( FHANDLE  hcalib,
fvstream *  strm 
)

キャリブレーションデータのストリーム読み込み

キャリブレーションデータを、指定のデータストリームから読み込みます。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[out] strm データストリーム
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

INT FVALGAPI fnFIE_xyqn_save ( FHANDLE  hcalib,
fvstream *  strm 
)

キャリブレーションデータのストリーム書き込み

キャリブレーションデータを、指定のデータストリームに書き込みます。

使用例
fnFIE_xyqn_load() の例を参照してください。
引数:
[in] hcalib キャリブレーションシステムのハンドル
[in] strm データストリーム
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

INT FVALGAPI fnFIE_xyqn_get_status ( FHANDLE  hcalib,
INT *  status 
)

キャリブレーション状態の確認

オープン後、キャリブレーションデータが作成されているかどうか、 つまり、 fnFIE_xyqn_calc_table() もしくは fnFIE_xyqn_load() が実行されたかを取得します。

引数:
[in] hcalib キャリブレーションシステムのハンドル
[out] status キャリブレーションデータの状態
  • TRUE キャリブレーション済み
  • FALSE 未キャリブレーション
戻り値:
F_ERR_NONE 正常終了
F_ERR_INVALID_OBJECT 不正なオブジェクトが渡された
F_ERR_INVALID_PARAM 不正なパラメータが渡された
F_ERR_NO_LICENCE ライセンスエラー、または未初期化エラー

Documentation copyright © 2009-2024 FAST Corporation.
Generated on Fri Aug 9 16:38:48 2024 for FIEライブラリ by doxygen 1.5.6-FASTSP-p2