nyarvertexdetector.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.vertexdetect;
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.core.*;
import jp.nyatla.nyartoolkit.sandbox.x2.*;
/**
 * PCAではなく、頂点座標そのものからSquare位置を計算するクラス
 *
 */
public class NyARVertexDetector implements INyARSquareDetector
{
	private static final double VERTEX_FACTOR = 1.0;// 線検出のファクタ

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

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

	private final NyARLabeling_ARToolKit_X2 _labeling;

	private final NyARLabelingImage _limage;

	private final OverlapChecker _overlap_checker = new OverlapChecker();
	private final NyARObserv2IdealMap _dist_factor_ref;

	/**
	 * 最大i_squre_max個のマーカーを検出するクラスを作成する。
	 * 
	 * @param i_param
	 */
	public NyARVertexDetector(NyARObserv2IdealMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException
	{
		this._width = i_size.w;
		this._height = i_size.h;
		this._dist_factor_ref = i_dist_factor_ref;
		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];
	}

	private final int _max_coord;
	private final int[] _xcoord;
	private final 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 final int[] __detectMarker_mkvertex = new int[5];

	/**
	 * ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,int *warea, double *wpos, int *wclip,int area_max, int area_min, double
	 * factor, int *marker_num ) 関数の代替品 ラベリング情報からマーカー一覧を作成してo_marker_listを更新します。 関数はo_marker_listに重なりを除外したマーカーリストを作成します。
	 * 
	 * @param i_raster
	 * 解析する２値ラスタイメージを指定します。
	 * @param o_square_stack
	 * 抽出した正方形候補を格納するリスト
	 * @throws NyARException
	 */
	public final void detectMarker(NyARBinRaster i_raster, NyARSquareStack o_square_stack) throws NyARException
	{
		final INyARLabeling labeling_proc = this._labeling;
		final NyARLabelingImage limage = this._limage;

		// 初期化

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

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

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

		final NyARLabelingLabelStack stack = limage.getLabelStack();
		final NyARLabelingLabel[] labels = (NyARLabelingLabel[])stack.getArray();
		
		
		// ラベルを大きい順に整列
		stack.sortByArea();

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

		final int xsize = this._width;
		final int ysize = this._height;
		final int[] xcoord = this._xcoord;
		final int[] ycoord = this._ycoord;
		final int coord_max = this._max_coord;
		final int[] mkvertex = this.__detectMarker_mkvertex;
		final 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;
			}
			//頂点候補のインデクスを取得
			final 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;
			}
			//頂点情報からライン情報を作っちゃう
			getSquare(mkvertex, xcoord, ycoord, square_ptr);
			
			// 検出済の矩形の属したラベルを重なりチェックに追加する。
			overlap.push(label_pt);
		}	
		return;
	}
	/**
	 * ２つの頂点座標を結ぶ直線から、NyARLinearを計算する。
	 * @param i_v1
	 * @param i_v2
	 * @param o_line
	 */
	final private void getLine(NyARDoublePoint2d i_v1,NyARDoublePoint2d i_v2,NyARLinear o_line)
	{
		final double x=i_v1.x-i_v2.x;
		final double y=i_v1.y-i_v2.y;
		final double x2=x*x;
		final double y2=y*y;
		final double rise_=Math.sqrt(x2/(x2+y2));
		o_line.rise=rise_;
		o_line.run=Math.sqrt(y2/(x2+y2));
		if(x<0){
			if(y<0){
				o_line.rise=-o_line.rise;
			}else{
				o_line.rise=-o_line.rise;
				o_line.run=-o_line.run;
			}
		}else{
			if(y<0){
				o_line.rise=-o_line.rise;
				o_line.run=-o_line.run;
			}else{
				o_line.rise=-o_line.rise;
			}			
		}
		o_line.intercept=(i_v1.y+(o_line.run/o_line.rise)*(i_v1.x))*rise_;
		
	}

	private void getSquare(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square)
	{
		final NyARObserv2IdealMap dist_factor=this._dist_factor_ref;		
		final NyARDoublePoint2d[] vertex=o_square.sqvertex;
		//歪み補正
		for(int i=0;i<4;i++)
		{
			final int idx=i_mkvertex[i];
			o_square.imvertex[i].x=i_xcoord[idx];
      		o_square.imvertex[i].y=i_ycoord[idx];
			dist_factor.observ2Ideal(i_xcoord[idx], i_ycoord[idx],vertex[i]);
		}
		//ライン計算
		getLine(vertex[1],vertex[0],o_square.line[0]);
		getLine(vertex[2],vertex[1],o_square.line[1]);