/**
* この関数は、点が矩形の範囲内にあるか判定します。
* @param i_pos
* 調査する座標
* @return
* 点が矩形の中にあれば、trueを返します。
*/
public bool isInnerPoint(NyARIntPoint2d i_pos)
{
int x = i_pos.x - this.x;
int y = i_pos.y - this.y;
return(0 <= x && x < this.w && 0 <= y && y < this.h);
}
/**
* この関数は、ラスタドライバから画像を読み出します。
* @param i_pix_drv
* @param i_size
* @param i_vertex
* @param o_data
* @param o_param
* @return
* @throws NyARException
*/
public bool getARPlayCardId(INyARGsPixelDriver i_pix_drv, NyARIntPoint2d[] i_vertex, PsArIdParam i_result)
{
if (!this._perspective_reader.setSourceSquare(i_vertex)) {
return false;
}
return this._pickFromRaster(i_pix_drv, i_result);
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w);
o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w);
return;
}
/**
* 座標値を射影変換します。
* @param i_x
* 変換元の座標値
* @param i_y
* 変換元の座標値
* @param i_z
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(double i_x, double i_y, double i_z, NyARIntPoint2d o_2d)
{
double w = 1 / (i_z * this.m22);
o_2d.x = (int)((i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w);
o_2d.y = (int)((i_y * this.m11 + i_z * this.m12) * w);
return;
}
/**
* この関数は、直線の交点を計算します。
* @param i_a
* 交点を求める直線式の係数a
* @param i_b
* 交点を求める直線式の係数b
* @param i_c
* 交点を求める直線式の係数c
* @param o_point
* 交点座標を格納するオブジェクト
* @return
* 交点が求まればtrue
*/
public bool crossPos(double i_a, double i_b, double i_c, NyARIntPoint2d o_point)
{
double w1 = this.a * i_b - i_a * this.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (int)((this.b * i_c - i_b * this.c) / w1);
o_point.y = (int)((i_a * this.c - this.a * i_c) / w1);
return(true);
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_x
* 変換元の座標
* @param i_y
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void ideal2Observ(double i_x, double i_y, NyARIntPoint2d o_out)
{
double x = (i_x - this._f0) * this._f3;
double y = (i_y - this._f1) * this._f3;
if (x == 0.0 && y == 0.0)
{
o_out.x = (int)(this._f0);
o_out.y = (int)(this._f1);
}
else
{
double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);
o_out.x = (int)(x * d + this._f0);
o_out.y = (int)(y * d + this._f1);
}
return;
}
/**
* この関数は、直線を矩形でクリッピングしたときの、端点を計算します。
* @param i_left
* 矩形の左上座標(X)
* @param i_top
* 矩形の左上座標(Y)
* @param i_width
* 矩形の幅
* @param i_height
* 矩形の高さ
* @param o_point
* 端点を返すオブジェクト配列。2要素である必要があります。
* @return
* 端点が求まればtrue
*/
public bool makeSegmentLine(int i_left, int i_top, int i_width, int i_height, NyARIntPoint2d[] o_point)
{
int bottom = i_top + i_height;
int right = i_left + i_width;
int idx = 0;
NyARIntPoint2d ptr = o_point[0];
if (this.crossPos(0, -1, i_top, ptr) && ptr.x >= i_left && ptr.x < right)
{
//y=rect.yの線
idx++;
ptr = o_point[idx];
}
if (this.crossPos(0, -1, bottom - 1, ptr) && ptr.x >= i_left && ptr.x < right)
{
//y=(rect.y+rect.h-1)の線
idx++;
if (idx == 2)
{
return(true);
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, i_left, ptr) && ptr.y >= i_top && ptr.y < bottom)
{
//x=i_leftの線
idx++;
if (idx == 2)
{
return(true);
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, right - 1, ptr) && ptr.y >= i_top && ptr.y < bottom)
{
//x=i_right-1の線
idx++;
if (idx == 2)
{
return(true);
}
}
return(false);
}
public void ideal2Observ(double i_x, double i_y, NyARIntPoint2d o_out)
{
double k1 = this._k1;
double k2 = this._k2;
double p1 = this._p1;
double p2 = this._p2;
double fx = this._fx;
double fy = this._fy;
double x0 = this._x0;
double y0 = this._y0;
double s = this._s;
double x = (i_x - x0) * s / fx;
double y = (i_y - y0) * s / fy;
double l = x * x + y * y;
o_out.x = (int)((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
o_out.y = (int)((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
}
// /**
// * i_x,i_yを通るこの直線の法線上での、この直線とi_linearの距離の二乗値を返します。
// * i_x,i_yに直線上の点を指定すると、この直線の垂線上での、もう一方の直線との距離の二乗値が得られます。
// * @param i_linear
// * @param i_x
// * @param i_y
// * @param o_point
// * @return
// * 交点が無い場合、無限大を返します。
// */
// public const double sqDistWithLinear(NyARLinear i_linear, double i_x,double i_y)
// {
// //thisを法線に変換
// double la=this.b;
// double lb=-this.a;
// double lc=-(la*i_x+lb*i_y);
// //交点を計算
// const double w1 = i_linear.a * lb - la * i_linear.b;
// if (w1 == 0.0) {
// return Double.POSITIVE_INFINITY;
// }
// double x=i_x-((i_linear.b * lc - lb * i_linear.c) / w1);
// double y=i_y-((la * i_linear.c - i_linear.a * lc) / w1);
// return x*x+y*y;
// }
/**
* この関数は、直線を0,0基点(左上)の矩形でクリッピングしたときの、端点を計算します。
* @param i_width
* 矩形の幅
* @param i_height
* 矩形の高さ
* @param o_point
* 端点を返すオブジェクト配列。2要素である必要があります。
* @return
* 端点が求まればtrue
*/
public bool makeSegmentLine(int i_width, int i_height, NyARIntPoint2d[] o_point)
{
int idx = 0;
NyARIntPoint2d ptr = o_point[0];
if (this.crossPos(0, -1, 0, ptr) && ptr.x >= 0 && ptr.x < i_width)
{
//y=rect.yの線
idx++;
ptr = o_point[idx];
}
if (this.crossPos(0, -1, i_height - 1, ptr) && ptr.x >= 0 && ptr.x < i_width)
{
//y=(rect.y+rect.h-1)の線
idx++;
if (idx == 2)
{
return(true);
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, 0, ptr) && ptr.y >= 0 && ptr.y < i_height)
{
//x=i_leftの線
idx++;
if (idx == 2)
{
return(true);
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, i_width - 1, ptr) && ptr.y >= 0 && ptr.y < i_height)
{
//x=i_right-1の線
idx++;
if (idx == 2)
{
return(true);
}
}
return(false);
}
/**
* [readonly]マーカにマッチした{@link NyARMatchPattDeviationColorData}インスタンスを得る。
* @
*/
public NyARMatchPattDeviationColorData getDeviationColorData(ARMarkerList.Item i_marker, INyARPerspectiveCopy i_pix_drv, NyARIntPoint2d[] i_vertex)
{
int mk_edge = i_marker.patt_edge_percentage;
for (int i = this.items.Count - 1; i >= 0; i--)
{
Item ptr = this.items[i];
if (!ptr._patt.getSize().isEqualSize(i_marker.patt_w, i_marker.patt_h) || ptr._patt_edge != mk_edge)
{
//サイズとエッジサイズが合致しない物はスルー
continue;
}
//古かったら更新
i_pix_drv.copyPatt(i_vertex, ptr._patt_edge, ptr._patt_edge, ptr._patt_resolution, ptr._patt);
ptr._patt_d.setRaster(ptr._patt);
return ptr._patt_d;
}
//無い。新しく生成
Item item = new Item(i_marker.patt_w, i_marker.patt_h, mk_edge);
//タイムスタンプの更新とデータの生成
i_pix_drv.copyPatt(i_vertex, item._patt_edge, item._patt_edge, item._patt_resolution, item._patt);
item._patt_d.setRaster(item._patt);
this.items.Add(item);
return item._patt_d;
}
/**
* 画面座標系の4頂点でかこまれる領域から、RGB画像をo_rasterに取得します。
* @param i_vertex
* @param i_resolution
* 1ピクセルあたりのサンプル数です。二乗した値が実際のサンプル数になります。
* @param o_raster
* @return
* @throws NyARException
*/
public bool GetRgbPatt2d(NyARRealitySource i_src, NyARIntPoint2d[] i_vertex, int i_resolution, INyARRgbRaster o_raster)
{
return i_src.refPerspectiveRasterReader().copyPatt(i_vertex, 0, 0, i_resolution, o_raster);
}
/**
* 変換行列と頂点座標から、パラメータを計算
* o_paramの[0..3]にはXのパラメタ、[4..7]にはYのパラメタを格納する。
* @param i_vertex
* @param pa
* @param pb
*/
private void calcPara(NyARIntPoint2d[] i_vertex, double[] o_cparam)
{
NyARDoubleMatrix44 invmat = this._invmat;
double v1, v2, v4;
//変換行列とベクトルの積から、変換パラメタを計算する。
v1 = i_vertex[0].x;
v2 = i_vertex[1].x;
v4 = i_vertex[3].x;
o_cparam[0] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].x + invmat.m03 * v4;
o_cparam[1] = invmat.m10 * v1 + invmat.m11 * v2;//m12,m13は0;
o_cparam[2] = invmat.m20 * v1 + invmat.m23 * v4;//m21,m22は0;
o_cparam[3] = v1;//m30は1.0で、m31,m32,m33は0
v1 = i_vertex[0].y;
v2 = i_vertex[1].y;
v4 = i_vertex[3].y;
o_cparam[4] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].y + invmat.m03 * v4;
o_cparam[5] = invmat.m10 * v1 + invmat.m11 * v2;//m12,m13は0;
o_cparam[6] = invmat.m20 * v1 + invmat.m23 * v4;//m21,m22は0;
o_cparam[7] = v1;//m30は1.0で、m31,m32,m33は0
return;
}
/**
* {@link #getTargetCenter}の出力型違いの関数です。
* @param o_center
*/
public void getTargetCenter(NyARIntPoint2d o_center)
{
Debug.Assert(this._target_type == RT_UNKNOWN || this._target_type == RT_KNOWN);
NyARDoublePoint2d.makeCenter(((NyARRectTargetStatus)(this._ref_tracktarget._ref_status)).vertex,4,o_center);
}
/**
* この関数は、遠近法のパラメータを計算して、返却します。
* @param i_size
* 変換先の矩形のサイズを指定します。
* @param i_vertex
* 変換元の頂点を指定します。要素数は4でなければなりません。
* @param o_param
* 射影変換パラメータの出力インスタンスを指定します。要素数は8でなければなりません。
* @return
* 成功するとtrueを返します。
* @
*/
public bool getParam(NyARIntSize i_size, NyARIntPoint2d[] i_vertex, double[] o_param)
{
Debug.Assert(i_vertex.Length == 4);
return this.getParam(i_size.w, i_size.h, i_vertex[0].x, i_vertex[0].y, i_vertex[1].x, i_vertex[1].y, i_vertex[2].x, i_vertex[2].y, i_vertex[3].x, i_vertex[3].y, o_param);
}
/**
* この関数は、直線の交点を計算します。
* @param i_a
* 交点を求める直線式の係数a
* @param i_b
* 交点を求める直線式の係数b
* @param i_c
* 交点を求める直線式の係数c
* @param o_point
* 交点座標を格納するオブジェクト
* @return
* 交点が求まればtrue
*/
public bool crossPos(double i_a, double i_b, double i_c, NyARIntPoint2d o_point)
{
double w1 = this.a * i_b - i_a * this.b;
if (w1 == 0.0)
{
return false;
}
o_point.x = (int)((this.b * i_c - i_b * this.c) / w1);
o_point.y = (int)((i_a * this.c - this.a * i_c) / w1);
return true;
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w);
o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w);
return;
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void ideal2Observ(NyARDoublePoint2d i_in, NyARIntPoint2d o_out)
{
this.ideal2Observ(i_in.x, i_in.y, o_out);
return;
}
/**
* この関数は、複数の座標点を、一括して理想座標系から観察座標系へ変換します。
* i_inとo_outには、同じインスタンスを指定できます。
*/
public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARIntPoint2d[] o_out, int i_size)
{
double x, y;
double d0 = this._f0;
double d1 = this._f1;
double d3 = this._f3;
double d2_w = this._f2 / 100000000.0;
for (int i = 0; i < i_size; i++)
{
x = (i_in[i].x - d0) * d3;
y = (i_in[i].y - d1) * d3;
if (x == 0.0 && y == 0.0)
{
o_out[i].x = (int)d0;
o_out[i].y = (int)d1;
}
else
{
double d = 1.0 - d2_w * (x * x + y * y);
o_out[i].x = (int)(x * d + d0);
o_out[i].y = (int)(y * d + d1);
}
}
return;
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
*/
public void ideal2Observ(double i_x, double i_y, NyARIntPoint2d o_out)
{
double x = (i_x - this._f0) * this._f3;
double y = (i_y - this._f1) * this._f3;
if (x == 0.0 && y == 0.0)
{
o_out.x = (int)(this._f0);
o_out.y = (int)(this._f1);
}
else
{
double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);
o_out.x = (int)(x * d + this._f0);
o_out.y = (int)(y * d + this._f1);
}
return;
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
*/
public void ideal2Observ(NyARDoublePoint2d i_in, NyARIntPoint2d o_out)
{
this.ideal2Observ(i_in.x, i_in.y, o_out);
return;
}
public void ideal2Observ(double i_x, double i_y, NyARIntPoint2d o_out)
{
double k1 = this._k1;
double k2 = this._k2;
double p1 = this._p1;
double p2 = this._p2;
double fx = this._fx;
double fy = this._fy;
double x0 = this._x0;
double y0 = this._y0;
double s = this._s;
double x = (i_x - x0) * s / fx;
double y = (i_y - y0) * s / fy;
double l = x * x + y * y;
o_out.x = (int)((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
o_out.y = (int)((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
}
/**
* この関数は、射影変換パラメータを計算します。
* @param i_vertex
* 変換元の４角系を定義する頂点配列。４頂点である必要がある。
* @param o_para
* 計算したパラメータの出力先配列
* @return
* 計算に成功するとtrueです。
* @
*/
private bool get_cpara(NyARIntPoint2d[] i_vertex, NyARMat o_para)
{
double[][] world = CPARAM_WORLD;
NyARMat a = new NyARMat(8, 8);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
double[][] a_array = a.getArray();
NyARMat b = new NyARMat(8, 1);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );
double[][] b_array = b.getArray();
double[] a_pt0, a_pt1;
double[] world_pti;
for (int i = 0; i < 4; i++)
{
a_pt0 = a_array[i * 2];
a_pt1 = a_array[i * 2 + 1];
world_pti = world[i];
a_pt0[0] = (double)world_pti[0];// a->m[i*16+0] = world[i][0];
a_pt0[1] = (double)world_pti[1];// a->m[i*16+1] = world[i][1];
a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;
a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;
a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;
a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;
a_pt0[6] = (double)(-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];
a_pt0[7] = (double)(-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];
a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;
a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;
a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;
a_pt1[3] = (double)world_pti[0];// a->m[i*16+11] = world[i][0];
a_pt1[4] = (double)world_pti[1];// a->m[i*16+12] = world[i][1];
a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;
a_pt1[6] = (double)(-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];
a_pt1[7] = (double)(-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];
b_array[i * 2 + 0][0] = (double)i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];
b_array[i * 2 + 1][0] = (double)i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];
}
if (!a.inverse())
{
return false;
}
o_para.mul(a, b);
return true;
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public virtual bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
// パターンの切り出しに失敗することもある。
NyARMat cpara = new NyARMat(8, 1);
if (!get_cpara(i_vertexs, cpara))
{
return false;
}
double[][] para = cpara.getArray();
double para00 = para[0 * 3 + 0][0];
double para01 = para[0 * 3 + 1][0];
double para02 = para[0 * 3 + 2][0];
double para10 = para[1 * 3 + 0][0];
double para11 = para[1 * 3 + 1][0];
double para12 = para[1 * 3 + 2][0];
double para20 = para[2 * 3 + 0][0];
double para21 = para[2 * 3 + 1][0];
double para22 = 1.0;
int lx1 = (int)((i_vertexs[0].x - i_vertexs[1].x) * (i_vertexs[0].x - i_vertexs[1].x) + (i_vertexs[0].y - i_vertexs[1].y) * (i_vertexs[0].y - i_vertexs[1].y));
int lx2 = (int)((i_vertexs[2].x - i_vertexs[3].x) * (i_vertexs[2].x - i_vertexs[3].x) + (i_vertexs[2].y - i_vertexs[3].y) * (i_vertexs[2].y - i_vertexs[3].y));
int ly1 = (int)((i_vertexs[1].x - i_vertexs[2].x) * (i_vertexs[1].x - i_vertexs[2].x) + (i_vertexs[1].y - i_vertexs[2].y) * (i_vertexs[1].y - i_vertexs[2].y));
int ly2 = (int)((i_vertexs[3].x - i_vertexs[0].x) * (i_vertexs[3].x - i_vertexs[0].x) + (i_vertexs[3].y - i_vertexs[0].y) * (i_vertexs[3].y - i_vertexs[0].y));
if (lx2 > lx1)
{
lx1 = lx2;
}
if (ly2 > ly1)
{
ly1 = ly2;
}
int sample_pixel_x = this._size.w;
int sample_pixel_y = this._size.h;
while (sample_pixel_x * sample_pixel_x < lx1 / 4)
{
sample_pixel_x *= 2;
}
while (sample_pixel_y * sample_pixel_y < ly1 / 4)
{
sample_pixel_y *= 2;
}
if (sample_pixel_x > AR_PATT_SAMPLE_NUM)
{
sample_pixel_x = AR_PATT_SAMPLE_NUM;
}
if (sample_pixel_y > AR_PATT_SAMPLE_NUM)
{
sample_pixel_y = AR_PATT_SAMPLE_NUM;
}
int xdiv = sample_pixel_x / this._size.w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;
int ydiv = sample_pixel_y / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;
int img_x = image.getWidth();
int img_y = image.getHeight();
double xdiv2_reciprocal = 1.0 / sample_pixel_x;
double ydiv2_reciprocal = 1.0 / sample_pixel_y;
int r, g, b;
int[] rgb_tmp = new int[3];
//ピクセルリーダーを取得
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
int xdiv_x_ydiv = xdiv * ydiv;
for (int iy = 0; iy < this._size.h; iy++)
{
for (int ix = 0; ix < this._size.w; ix++)
{
r = g = b = 0;
//1ピクセルを作成
for (int j = 0; j < ydiv; j++)
{
double yw = 102.5 + 5.0 * (iy * ydiv + j + 0.5) * ydiv2_reciprocal;
for (int i = 0; i < xdiv; i++)
{
double xw = 102.5 + 5.0 * (ix * xdiv + i + 0.5) * xdiv2_reciprocal;
double d = para20 * xw + para21 * yw + para22;
if (d == 0)
{
throw new NyARException();
}
int xc = (int)((para00 * xw + para01 * yw + para02) / d);
int yc = (int)((para10 * xw + para11 * yw + para12) / d);
if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y)
{
reader.getPixel(xc, yc, rgb_tmp);
r += rgb_tmp[0];// R
g += rgb_tmp[1];// G
b += rgb_tmp[2];// B
// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
}
}
}
this._patdata[iy * this._size.w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
}
}
return true;
}
/**
* この関数は、２つの座標点配列同士の距離値を一括してマップにセットします。
* <p>
* 実装メモ -
* 点のフォーマットが合わない場合は、この関数参考にオーバーロードしてください。
* </p>
* @param i_vertex_r
* 比較する頂点群を格納した配列。
* @param i_row_len
* i_vertex_rの有効な要素数
* @param i_vertex_c
* 基準となる頂点群を格納した配列
* @param i_col_len
* i_vertex_cの有効な要素数
*/
public void setPointDists(NyARIntPoint2d[] i_vertex_r, int i_row_len, NyARIntPoint2d[] i_vertex_c, int i_col_len)
{
DistItem[] map = this._map;
//distortionMapを作成。ついでに最小値のインデクスも取得
int min_index = 0;
int min_dist = int.MaxValue;
int idx = 0;
for (int r = 0; r < i_row_len; r++)
{
for (int c = 0; c < i_col_len; c++)
{
map[idx].col = c;
map[idx].row = r;
int d = i_vertex_r[r].sqDist(i_vertex_c[c]);
map[idx].dist = d;
if (min_dist > d)
{
min_index = idx;
min_dist = d;
}
idx++;
}
}
this._min_dist = min_dist;
this._min_dist_index = min_index;
this._size_col = i_col_len;
this._size_row = i_row_len;
return;
}
/**
* この関数は、直線を矩形でクリッピングしたときの、端点を計算します。
* @param i_left
* 矩形の左上座標(X)
* @param i_top
* 矩形の左上座標(Y)
* @param i_width
* 矩形の幅
* @param i_height
* 矩形の高さ
* @param o_point
* 端点を返すオブジェクト配列。2要素である必要があります。
* @return
* 端点が求まればtrue
*/
public bool makeSegmentLine(int i_left, int i_top, int i_width, int i_height, NyARIntPoint2d[] o_point)
{
int bottom = i_top + i_height;
int right = i_left + i_width;
int idx = 0;
NyARIntPoint2d ptr = o_point[0];
if (this.crossPos(0, -1, i_top, ptr) && ptr.x >= i_left && ptr.x < right)
{
//y=rect.yの線
idx++;
ptr = o_point[idx];
}
if (this.crossPos(0, -1, bottom - 1, ptr) && ptr.x >= i_left && ptr.x < right)
{
//y=(rect.y+rect.h-1)の線
idx++;
if (idx == 2)
{
return true;
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, i_left, ptr) && ptr.y >= i_top && ptr.y < bottom)
{
//x=i_leftの線
idx++;
if (idx == 2)
{
return true;
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, right - 1, ptr) && ptr.y >= i_top && ptr.y < bottom)
{
//x=i_right-1の線
idx++;
if (idx == 2)
{
return true;
}
}
return false;
}
/**
* この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。
* 式の係数値は、正規化されます。
* @param i_point1
* 点１
* @param i_point2
* 点２
* @return
* 直線式が求まれば、true
*/
public bool makeLinearWithNormalize(NyARIntPoint2d i_point1, NyARIntPoint2d i_point2)
{
return(makeLinearWithNormalize(i_point1.x, i_point1.y, i_point2.x, i_point2.y));
}
/**
* この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。
* 式の係数値は、正規化されます。
* @param i_point1
* 点１
* @param i_point2
* 点２
* @return
* 直線式が求まれば、true
*/
public bool makeLinearWithNormalize(NyARIntPoint2d i_point1, NyARIntPoint2d i_point2)
{
return makeLinearWithNormalize(i_point1.x, i_point1.y, i_point2.x, i_point2.y);
}
/**
* この関数は、ラスタドライバから画像を読み出します。
* @param i_pix_drv
* @param i_size
* @param i_vertex
* @param o_data
* @param o_param
* @return
* @
*/
public bool pickFromRaster(INyARGsPixelDriver i_pix_drv, NyARIntPoint2d[] i_vertex, NyIdMarkerPattern o_data, NyIdMarkerParam o_param)
{
if (!this._perspective_reader.setSourceSquare(i_vertex))
{
return false;
}
return this._pickFromRaster(i_pix_drv, o_data, o_param);
}
/**
* 座標値を射影変換します。
* @param i_x
* 変換元の座標値
* @param i_y
* 変換元の座標値
* @param i_z
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(double i_x, double i_y, double i_z, NyARIntPoint2d o_2d)
{
double w = 1 / (i_z * this.m22);
o_2d.x = (int)((i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w);
o_2d.y = (int)((i_y * this.m11 + i_z * this.m12) * w);
return;
}
/**
* 点1と点2の間に線分を定義して、その線分上のベクトルを得ます。点は、画像の内側でなければなりません。 320*240の場合、(x>=0 &&
* x<320 x+w>0 && x+w<320),(y>0 && y<240 y+h>=0 && y+h<=319)となります。
*
* @param i_pos1
* 点1の座標です。
* @param i_pos2
* 点2の座標です。
* @param i_area
* ベクトルを検出するカーネルサイズです。1の場合(n*2-1)^2のカーネルになります。 点2の座標です。
* @param o_coord
* 結果を受け取るオブジェクトです。
* @return
* @throws NyARException
*/
public bool traceLine(NyARIntPoint2d i_pos1, NyARIntPoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
NyARIntSize base_s=this._ref_base_raster.getSize();
// (i_area*2)の矩形が範囲内に収まるように線を引く
// 移動量
// 点間距離を計算
int dist = (int) Math.Sqrt(i_pos1.sqDist(i_pos2));
// 最低AREA*2以上の大きさが無いなら、ラインのトレースは不可能。
if (dist < 4) {
return false;
}
// dist最大数の決定
if (dist > 12) {
dist = 12;
}
// サンプリングサイズを決定(移動速度とサイズから)
int s = i_edge * 2 + 1;
int dx = (i_pos2.x - i_pos1.x);
int dy = (i_pos2.y - i_pos1.y);
int r = base_s.w - s;
int b = base_s.h - s;
// 最大14点を定義して、そのうち両端を除いた点を使用する。
for (int i = 1; i < dist - 1; i++) {
int x = i * dx / dist + i_pos1.x - i_edge;
int y = i * dy / dist + i_pos1.y - i_edge;
// limit
coord.items[i - 1].x = x < 0 ? 0 : (x >= r ? r : x);
coord.items[i - 1].y = y < 0 ? 0 : (y >= b ? b : y);
}
coord.length = dist - 2;
// 点数は20点程度を得る。
return traceConture(coord, 1, s, o_coord);
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
if (this._last_input_raster != image)
{
this._raster_driver = (INyARPerspectiveCopy)image.createInterface(typeof(INyARPerspectiveCopy));
this._last_input_raster = image;
}
//遠近法のパラメータを計算
return this._raster_driver.copyPatt(i_vertexs, this._edge.x, this._edge.y, this._sample_per_pixel, this);
}
/**
* この関数は、点が矩形の範囲内にあるか判定します。
* @param i_pos
* 調査する座標
* @return
* 点が矩形の中にあれば、trueを返します。
*/
public bool isInnerPoint(NyARIntPoint2d i_pos)
{
int x = i_pos.x - this.x;
int y = i_pos.y - this.y;
return (0 <= x && x < this.w && 0 <= y && y < this.h);
}
/**
* この関数は、遠近法のパラメータを計算して、返却します。
* @param i_width
* 変換先の矩形のサイズを指定します。
* @param i_height
* 変換先の矩形のサイズを指定します。
* @param i_vertex
* 変換元の頂点を指定します。要素数は4でなければなりません。
* @param o_param
* 射影変換パラメータの出力インスタンスを指定します。要素数は8でなければなりません。
* @return
* 成功するとtrueを返します。
* @
*/
public bool getParam(int i_width, int i_height, NyARIntPoint2d[] i_vertex, double[] o_param)
{
return this.getParam(i_width, i_height, i_vertex[0].x, i_vertex[0].y, i_vertex[1].x, i_vertex[1].y, i_vertex[2].x, i_vertex[2].y, i_vertex[3].x, i_vertex[3].y, o_param);
}
/**
* この関数は、頂点集合を包括する矩形を計算して、インスタンスにセットします。
* @param i_vertex
* 頂点集合を格納した配列
* @param i_num_of_vertex
* 計算対象とする要素の数
*/
public void setAreaRect(NyARIntPoint2d[] i_vertex, int i_num_of_vertex)
{
//エリアを求める。
int xmax, xmin, ymax, ymin;
xmin = xmax = (int)i_vertex[i_num_of_vertex - 1].x;
ymin = ymax = (int)i_vertex[i_num_of_vertex - 1].y;
for (int i = i_num_of_vertex - 2; i >= 0; i--)
{
if (i_vertex[i].x < xmin)
{
xmin = (int)i_vertex[i].x;
}
else if (i_vertex[i].x > xmax)
{
xmax = (int)i_vertex[i].x;
}
if (i_vertex[i].y < ymin)
{
ymin = (int)i_vertex[i].y;
}
else if (i_vertex[i].y > ymax)
{
ymax = (int)i_vertex[i].y;
}
}
this.h = ymax - ymin + 1;
this.x = xmin;
this.w = xmax - xmin + 1;
this.y = ymin;
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
double[] conv_param = this._convparam;
int rx2, ry2;
rx2 = this._size.w;
ry2 = this._size.h;
int[] rgb_tmp = new int[3];
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
// 変形先領域の頂点を取得
//変換行列から現在の座標系への変換パラメタを作成
calcPara(i_vertexs, conv_param);// 変換パラメータを求める
for (int y = 0; y < ry2; y++)
{
for (int x = 0; x < rx2; x++)
{
int ttx = (int)((conv_param[0] * x * y + conv_param[1] * x + conv_param[2] * y + conv_param[3]) + 0.5);
int tty = (int)((conv_param[4] * x * y + conv_param[5] * x + conv_param[6] * y + conv_param[7]) + 0.5);
reader.getPixel((int)ttx, (int)tty, rgb_tmp);
this._patdata[x + y * rx2] = (rgb_tmp[0] << 16) | (rgb_tmp[1] << 8) | rgb_tmp[2];
}
}
return true;
}
// /**
// * i_x,i_yを通るこの直線の法線上での、この直線とi_linearの距離の二乗値を返します。
// * i_x,i_yに直線上の点を指定すると、この直線の垂線上での、もう一方の直線との距離の二乗値が得られます。
// * @param i_linear
// * @param i_x
// * @param i_y
// * @param o_point
// * @return
// * 交点が無い場合、無限大を返します。
// */
// public const double sqDistWithLinear(NyARLinear i_linear, double i_x,double i_y)
// {
// //thisを法線に変換
// double la=this.b;
// double lb=-this.a;
// double lc=-(la*i_x+lb*i_y);
// //交点を計算
// const double w1 = i_linear.a * lb - la * i_linear.b;
// if (w1 == 0.0) {
// return Double.POSITIVE_INFINITY;
// }
// double x=i_x-((i_linear.b * lc - lb * i_linear.c) / w1);
// double y=i_y-((la * i_linear.c - i_linear.a * lc) / w1);
// return x*x+y*y;
// }
/**
* この関数は、直線を0,0基点(左上)の矩形でクリッピングしたときの、端点を計算します。
* @param i_width
* 矩形の幅
* @param i_height
* 矩形の高さ
* @param o_point
* 端点を返すオブジェクト配列。2要素である必要があります。
* @return
* 端点が求まればtrue
*/
public bool makeSegmentLine(int i_width, int i_height, NyARIntPoint2d[] o_point)
{
int idx = 0;
NyARIntPoint2d ptr = o_point[0];
if (this.crossPos(0, -1, 0, ptr) && ptr.x >= 0 && ptr.x < i_width)
{
//y=rect.yの線
idx++;
ptr = o_point[idx];
}
if (this.crossPos(0, -1, i_height - 1, ptr) && ptr.x >= 0 && ptr.x < i_width)
{
//y=(rect.y+rect.h-1)の線
idx++;
if (idx == 2)
{
return true;
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, 0, ptr) && ptr.y >= 0 && ptr.y < i_height)
{
//x=i_leftの線
idx++;
if (idx == 2)
{
return true;
}
ptr = o_point[idx];
}
if (this.crossPos(-1, 0, i_width - 1, ptr) && ptr.y >= 0 && ptr.y < i_height)
{
//x=i_right-1の線
idx++;
if (idx == 2)
{
return true;
}
}
return false;
}
private bool getHighPixelCenter(int i_st, int[] i_pixels, int i_width, int i_height, int i_th, NyARIntPoint2d o_point)
{
int rp = i_st;
int pos_x = 0;
int pos_y = 0;
int number_of_pos = 0;
for (int i = 0; i < i_height; i++)
{
for (int i2 = 0; i2 < i_width; i2++)
{
if (i_pixels[rp++] > i_th)
{
pos_x += i2;
pos_y += i;
number_of_pos++;
}
}
}
if (number_of_pos > 0)
{
pos_x /= number_of_pos;
pos_y /= number_of_pos;
}
else
{
return false;
}
o_point.x = pos_x;
o_point.y = pos_y;
return true;
}
/**
* この関数は、複数の座標点を、一括して理想座標系から観察座標系へ変換します。
* i_inとo_outには、同じインスタンスを指定できます。
* @param i_in
* 変換元の座標配列
* @param o_out
* 変換後の座標を受け取る配列
* @param i_size
* 変換する座標の個数。
* @todo should optimize!
*/
public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARIntPoint2d[] o_out, int i_size)
{
double k1 = this._k1;
double k2 = this._k2;
double p1 = this._p1;
double p2 = this._p2;
double fx = this._fx;
double fy = this._fy;
double x0 = this._x0;
double y0 = this._y0;
double s = this._s;
for (int i = 0; i < i_size; i++)
{
double x = (i_in[i].x - x0) * s / fx;
double y = (i_in[i].y - y0) * s / fy;
double l = x * x + y * y;
o_out[i].x = (int)((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
o_out[i].y = (int)((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
}
return;
}
/**
* この関数は、マーカ四角形をインスタンスにセットします。
* @param i_vertex
* セットする四角形頂点座標。4要素である必要があります。
* @return
* 成功するとtrueです。
* @
*/
public bool setSourceSquare(NyARIntPoint2d[] i_vertex)
{
return this._param_gen.getParam(READ_RESOLUTION, READ_RESOLUTION, i_vertex, this._cparam);
}
public bool traceConture(int i_th,
NyARIntPoint2d i_entry, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
// Robertsラスタから輪郭抽出
if (!this._cpickup.getContour(this._ref_rob_raster, i_th, i_entry.x, i_entry.y,
coord)) {
// 輪郭線MAXならなにもできないね。
return false;
}
// 輪郭線のベクトル化
return traceConture(coord, this._rob_resolution,
this._rob_resolution * 2, o_coord);
}
