逆行列を使わないパースペクティブ補正付きのパターン取得クラス

逆行列を使わずに馬鹿正直に方程式を解いて、パースペクティブ補正をしながらパターンを取得するクラスができました。

まだ高速化していないので速度的にはそんなに早くないですが、読みやすく書いてあります。

get_cparaのところにちょっと細工があって、8ｘ8方程式を4x5方程式２個に分割して解いてます。定数項があるので、計算量はもっと減るはずですけどね。

/**
 * 遠近法を使ったパースペクティブ補正付きのINyARColorPatt
 *
 */
public class NyARColorPatt_Perspective implements INyARColorPatt
{
	private int[] _patdata;
	private NyARBufferReader _buf_reader;
	private NyARRgbPixelReader_INT1D_X8R8G8B8_32 _pixelreader;
	private NyARIntSize _size;
	
	private static final int LOCAL_LT=100;
	private NyARIntPoint2d _pickup_lt=new NyARIntPoint2d();
	private NyARIntSize _pickup_size=new NyARIntSize();

	/**
	 * 例えば、64
	 * @param i_width
	 * 取得画像の解像度幅
	 * @param i_height
	 * 取得画像の解像度高さ
	 */
	public NyARColorPatt_Perspective(int i_width, int i_height)
	{
		//入力制限
		assert i_width>2 && i_height>2;
		
		this._size=new NyARIntSize(i_width,i_height);
		this._patdata = new int[i_height*i_width];
		this._buf_reader=new NyARBufferReader(this._patdata,NyARBufferReader.BUFFERFORMAT_INT1D_X8R8G8B8_32);
		this._pixelreader=new NyARRgbPixelReader_INT1D_X8R8G8B8_32(this._patdata,this._size);
		setEdgeSize(0,0);
		return;
	}
	/**
	 * 矩形領域のエッジサイズを指定します。
	 * エッジの計算方法は以下の通りです。
	 * 1.マーカ全体を(i_x_edge*2+width)x(i_y_edge*2+height)の解像度でパラメタを計算します。
	 * 2.ピクセルの取得開始位置を(i_x_edge/2,i_y_edge/2)へ移動します。
	 * 3.開始位置から、width x height個のピクセルを取得します。
	 * 
	 * ARToolKit標準マーカの場合は、width/2,height/2を指定してください。
	 * @param i_x_edge
	 * @param i_y_edge
	 */
	public void setEdgeSize(int i_x_edge,int i_y_edge)
	{
		assert(i_x_edge>=0);
		assert(i_y_edge>=0);
		//ピックアップサイズの決定
		this._pickup_lt.x=i_x_edge+LOCAL_LT;
		this._pickup_lt.y=i_y_edge+LOCAL_LT;
		//ピックアップ開始位置を決定
		this._pickup_size.w=i_x_edge*2+this._size.w;
		this._pickup_size.h=i_y_edge*2+this._size.h;
		return;
	}
	public void setEdgeSizeByPercent(int i_x_percent,int i_y_percent)
	{
		assert(i_x_percent>=0);
		assert(i_y_percent>=0);
		setEdgeSize(this._size.w*i_x_percent/50,this._size.h*i_y_percent/50);
		return;
	}

	
	public final int getWidth()
	{
		return this._size.w;
	}
	public final int getHeight()
	{
		return this._size.h;
	}
	public final NyARIntSize getSize()
	{
		return 	this._size;
	}
	public final INyARBufferReader getBufferReader()
	{
		return this._buf_reader;
	}
	public final INyARRgbPixelReader getRgbPixelReader()
	{
		return this._pixelreader;
	}
	final protected boolean get_cpara(final NyARIntPoint2d[] i_vertex,double[] o_param)throws NyARException
	{
		double[][] la1,la2;
		double[] ra1,ra2;
		double ltx=LOCAL_LT;
		double lty=LOCAL_LT;
		double rbx=LOCAL_LT+this._pickup_size.w;
		double rby=LOCAL_LT+this._pickup_size.h;
		la1=new double[4][5];
		la2=new double[4][5];
		ra1=new double[4];
		ra2=new double[4];
		//A,B,C,(GH)の方程式
		la1[0][0]=ltx;	la1[0][1]=lty;	la1[0][2]=1;	la1[0][3]=-ltx*i_vertex[0].x;	la1[0][4]=-lty*i_vertex[0].x;
		la1[1][0]=rbx;	la1[1][1]=lty;	la1[1][2]=1;	la1[1][3]=-rbx*i_vertex[1].x;	la1[1][4]=-lty*i_vertex[1].x;
		la1[2][0]=rbx;	la1[2][1]=rby;	la1[2][2]=1;	la1[2][3]=-rbx*i_vertex[2].x;	la1[2][4]=-rby*i_vertex[2].x;
		la1[3][0]=ltx;	la1[3][1]=rby;	la1[3][2]=1;	la1[3][3]=-ltx*i_vertex[3].x;	la1[3][4]=-rby*i_vertex[3].x;
		ra1[0]=i_vertex[0].x;ra1[1]=i_vertex[1].x;ra1[2]=i_vertex[2].x;ra1[3]=i_vertex[3].x;
		NyARSystemOfLinearEquationsProcessor.doGaussianElimination(la1,ra1,5,4);

		//D,E,F,(GH)の方程式
		la2[0][0]=ltx;	la2[0][1]=lty;	la2[0][2]=1;	la2[0][3]=-ltx*i_vertex[0].y;	la2[0][4]=-lty*i_vertex[0].y;
		la2[1][0]=rbx;	la2[1][1]=lty;	la2[1][2]=1;	la2[1][3]=-rbx*i_vertex[1].y;	la2[1][4]=-lty*i_vertex[1].y;
		la2[2][0]=rbx;	la2[2][1]=rby;	la2[2][2]=1;	la2[2][3]=-rbx*i_vertex[2].y;	la2[2][4]=-rby*i_vertex[2].y;
		la2[3][0]=ltx;	la2[3][1]=rby;	la2[3][2]=1;	la2[3][3]=-ltx*i_vertex[3].y;	la2[3][4]=-rby*i_vertex[3].y;
		ra2[0]=i_vertex[0].y;ra2[1]=i_vertex[1].y;ra2[2]=i_vertex[2].y;ra2[3]=i_vertex[3].y;
		NyARSystemOfLinearEquationsProcessor.doGaussianElimination(la2,ra2,5,4);
		//GHを計算
		double A,B,C,D,E,F,G,H;
		H=(ra2[3]-ra1[3])/(la2[3][4]-la1[3][4]);
		G=ra2[3]-la2[3][4]*H;
		//残りを計算
		F=ra2[2]-H*la2[2][4]-G*la2[2][3];
		E=ra2[1]-H*la2[1][4]-G*la2[1][3]-F*la2[1][2];
		D=ra2[0]-H*la2[0][4]-G*la2[0][3]-F*la2[0][2]-E*la2[0][1];
		C=ra1[2]-H*la1[2][4]-G*la1[2][3];
		B=ra1[1]-H*la1[1][4]-G*la1[1][3]-C*la1[1][2];
		A=ra1[0]-H*la1[0][4]-G*la1[0][3]-C*la1[0][2]-B*la1[0][1];
		o_param[0]=A;
		o_param[1]=B;
		o_param[2]=C;
		o_param[3]=D;
		o_param[4]=E;
		o_param[5]=F;
		o_param[6]=G;
		o_param[7]=H;
		return true;
	}	
	private final int[] __pickFromRaster_rgb_tmp = new int[3];
	/**
	 * 
	 * @param image
	 * @param i_marker
	 * @return 切り出しに失敗した
	 * @throws Exception
	 */
	public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException
	{
		//遠近法のパラメータを計算
		double[] cpara = new double[8];
		if (!get_cpara(i_square.imvertex, cpara)) {
			return false;
		}
		
		int img_x = image.getWidth();
		int img_y = image.getHeight();

		int[] rgb_tmp = __pickFromRaster_rgb_tmp;

		//ピクセルリーダーを取得
		INyARRgbPixelReader reader=image.getRgbPixelReader();

		for(int iy=0;iy<this._size.h;iy++){
			for(int ix=0;ix<this._size.w;ix++){
				//1ピクセルを作成
				int cx=this._pickup_lt.x+ix;
				int cy=this._pickup_lt.y+iy;
				final double d=cpara[6]*cx+cpara[7]*cy+1.0;
				final int x=(int)((cpara[0]*cx+cpara[1]*cy+cpara[2])/d);
				final int y=(int)((cpara[3]*cx+cpara[4]*cy+cpara[5])/d);
				if (x >= 0 && x < img_x && y >= 0 && y < img_y) {
					reader.getPixel(x, y, rgb_tmp);
					this._patdata[iy*this._size.w+ix]=(((rgb_tmp[0])&0xff)<<16)|(((rgb_tmp[1])&0xff)<<8)|(((rgb_tmp[2])&0xff));
				}
			}
		}
		return true;
	}	
}