nyarsquaredetector_quad.java

java 版的 ARToolkit

/* 
 * PROJECT: NyARToolkit
 * --------------------------------------------------------------------------------
 * This work is based on the original ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 *
 * The NyARToolkit is Java version ARToolkit class library.
 * Copyright (C)2008 R.Iizuka
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp>
 * 
 */
package jp.nyatla.nyartoolkit.sandbox.quadx2;
import jp.nyatla.nyartoolkit.NyARException;
import jp.nyatla.nyartoolkit.core.labeling.*;
import jp.nyatla.nyartoolkit.core.raster.*;
import jp.nyatla.nyartoolkit.core.types.*;
import jp.nyatla.nyartoolkit.core.param.*;


import jp.nyatla.nyartoolkit.core2.types.NyARI64Linear;
import jp.nyatla.nyartoolkit.core2.types.NyARI64Point2d;
import jp.nyatla.nyartoolkit.core2.types.matrix.NyARI64Matrix22;
import jp.nyatla.nyartoolkit.core.*;
import jp.nyatla.nyartoolkit.sandbox.x2.*;


/**
 * 1/4に解像度を落して解析するNyARSquareDetector_X2
 * 与えるBinRasterが既に1/4のサイズになっていないといけないことに注意
 */
public class NyARSquareDetector_Quad implements INyARSquareDetector
{
    private static int PCA_LENGTH = 20;
    private static double VERTEX_FACTOR = 1.0;// 線検出のファクタ

    private static int AR_AREA_MAX = 25000;// #define AR_AREA_MAX 100000

    private static int AR_AREA_MIN = 20;// #define AR_AREA_MIN 70
    private int _width;
    private int _height;

    private INyARLabeling _labeling;

    private NyARLabelingImage _limage;

    private OverlapChecker _overlap_checker = new OverlapChecker();
    private NyARFixedFloatObserv2IdealMap _dist_factor;
    /**
     * 最大i_squre_max個のマーカーを検出するクラスを作成する。
     * 
     * @param i_param
     */
    public NyARSquareDetector_Quad(NyARCameraDistortionFactor i_dist_factor_ref, NyARIntSize i_size) throws NyARException
    {
        this._width = i_size.w / 2;
        this._height = i_size.h / 2;
        this._labeling = new NyARLabeling_ARToolKit_X2();
        this._limage = new NyARLabelingImage(this._width, this._height);
        this._labeling.attachDestination(this._limage);

        // 輪郭の最大長は画面に映りうる最大の長方形サイズ。
        int number_of_coord = (this._width + this._height) * 2;

        // 輪郭バッファは頂点変換をするので、輪郭バッファの２倍取る。
        this._max_coord = number_of_coord;
        this._xcoord = new int[number_of_coord * 2];
        this._ycoord = new int[number_of_coord * 2];

        //1/4サイズの歪みマップを作る
        NyARCameraDistortionFactor quadfactor = new NyARCameraDistortionFactor();
        quadfactor.copyFrom(i_dist_factor_ref);
        quadfactor.changeScale(0.5);
        this._dist_factor = new NyARFixedFloatObserv2IdealMap(quadfactor, i_size);
        //PCA
        this._pca = new NyARFixedFloatPca2d();
        this._xpos = new int[PCA_LENGTH];//最大辺長はthis._width+this._height
        this._ypos = new int[PCA_LENGTH];//最大辺長はthis._width+this._height

    }

    private int _max_coord;
    private int[] _xcoord;
    private int[] _ycoord;

    private void normalizeCoord(int[] i_coord_x, int[] i_coord_y, int i_index, int i_coord_num)
    {
        // vertex1を境界にして、後方に配列を連結
        System.arraycopy(i_coord_x, 1, i_coord_x, i_coord_num, i_index);
        System.arraycopy(i_coord_y, 1, i_coord_y, i_coord_num, i_index);
    }

    private int[] __detectMarker_mkvertex = new int[5];

    /**
     * arDetectMarker2を基にした関数
     * この関数はNyARSquare要素のうち、directionを除くパラメータを取得して返します。
     * directionの確定は行いません。
     * @param i_raster
     * 解析する２値ラスタイメージを指定します。
     * @param o_square_stack
     * 抽出した正方形候補を格納するリスト
     * @throws NyARException
     */
    public void detectMarker(NyARBinRaster i_raster, NyARSquareStack o_square_stack) throws NyARException
    {
        INyARLabeling labeling_proc = this._labeling;
        NyARLabelingImage limage = this._limage;

        // 初期化

        // マーカーホルダをリセット
        o_square_stack.clear();

        // ラベリング
        labeling_proc.labeling(i_raster);

        // ラベル数が0ならここまで
        int label_num = limage.getLabelStack().getLength();
        if (label_num < 1)
        {
            return;
        }

        NyARLabelingLabelStack stack = limage.getLabelStack();
        NyARLabelingLabel[] labels = (NyARLabelingLabel[])stack.getArray();


        // ラベルを大きい順に整列
        stack.sortByArea();

        // デカいラベルを読み飛ばし
        int i;
        for (i = 0; i < label_num; i++)
        {
            // 検査対象内のラベルサイズになるまで無視
            if (labels[i].area <= AR_AREA_MAX)
            {
                break;
            }
        }

        int xsize = this._width;
        int ysize = this._height;
        int[] xcoord = this._xcoord;
        int[] ycoord = this._ycoord;
        int coord_max = this._max_coord;
        int[] mkvertex = this.__detectMarker_mkvertex;
        OverlapChecker overlap = this._overlap_checker;
        int coord_num;
        int label_area;
        NyARLabelingLabel label_pt;

        //重なりチェッカの最大数を設定
        overlap.reset(label_num);

        for (; i < label_num; i++)
        {
            label_pt = labels[i];
            label_area = label_pt.area;
            // 検査対象サイズよりも小さくなったら終了
            if (label_area < AR_AREA_MIN)
            {
                break;
            }
            // クリップ領域が画面の枠に接していれば除外
            if (label_pt.clip_l == 1 || label_pt.clip_r == xsize - 2)
            {// if(wclip[i*4+0] == 1 || wclip[i*4+1] ==xsize-2){
                continue;
            }
            if (label_pt.clip_t == 1 || label_pt.clip_b == ysize - 2)
            {// if( wclip[i*4+2] == 1 || wclip[i*4+3] ==ysize-2){
                continue;
            }
            // 既に検出された矩形との重なりを確認
            if (!overlap.check(label_pt))
            {
                // 重なっているようだ。
                continue;
            }

            // 輪郭を取得
            coord_num = limage.getContour(i, coord_max, xcoord, ycoord);
            if (coord_num == coord_max)
            {
                // 輪郭が大きすぎる。
                continue;
            }
            //頂点候補のインデクスを取得
            int vertex1 = scanVertex(xcoord, ycoord, coord_num);

            // 頂点候補(vertex1)を先頭に並べなおした配列を作成する。
            normalizeCoord(xcoord, ycoord, vertex1, coord_num);

            // 領域を準備する。
            NyARSquare square_ptr = (NyARSquare)o_square_stack.prePush();

            // 頂点情報を取得
            if (!getSquareVertex(xcoord, ycoord, vertex1, coord_num, label_area, mkvertex))
            {
                o_square_stack.pop();// 頂点の取得が出来なかったので破棄
                continue;
            }
            // マーカーを検出
            if (!getSquareLine(mkvertex, xcoord, ycoord, square_ptr))
            {
                // 矩形が成立しなかった。
                o_square_stack.pop();
                continue;
            }
            // 検出済の矩形の属したラベルを重なりチェックに追加する。
            overlap.push(label_pt);
        }
        return;
    }

    /**
     * 辺からの対角線が最長になる点を対角線候補として返す。
     * 
     * @param i_xcoord
     * @param i_ycoord
     * @param i_coord_num
     * @return
     */
    private int scanVertex(int[] i_xcoord, int[] i_ycoord, int i_coord_num)
    {
        int sx = i_xcoord[0];
        int sy = i_ycoord[0];
        int d = 0;
        int w, x, y;
        int ret = 0;
        for (int i = 1; i < i_coord_num; i++)
        {
            x = i_xcoord[i] - sx;
            y = i_ycoord[i] - sy;
            w = x * x + y * y;
            if (w > d)
            {