/**
* p1->p2とp2->p3の作る角のsin値の絶対値を得ます。
* @param p1
* @param p2
* @param p3
* @return
*/
public static double getAbsSin(NyARDoublePoint2d p1, NyARDoublePoint2d p2, NyARDoublePoint2d p3)
{
double cp = NyARDoublePoint2d.crossProduct3Point(p1, p2, p3);
cp /= (Math.Sqrt(p1.sqDist(p2)) * Math.Sqrt(p2.sqDist(p3)));
return(cp > 0?cp:-cp);
}
public NyARCameraDistortionFactorLT(int i_screen_width, int i_screen_height, int i_offset, INyARCameraDistortionFactor i_base_factor)
{
this._xsize = i_screen_width + i_offset * 2;
this._ysize = i_screen_height + i_offset * 2;
this._xOff = i_offset;
this._yOff = i_offset;
this._i2o = new double[this._xsize * this._ysize * 2];
this._o2i = new double[this._xsize * this._ysize * 2];
this._base_factor = i_base_factor;
NyARDoublePoint2d tmp = new NyARDoublePoint2d();//
for (int j = 0; j < this._ysize; j++)
{
for (int i = 0; i < this._xsize; i++)
{
int ptr = (j * this._xsize + i) * 2;
i_base_factor.ideal2Observ(i - i_offset, j - i_offset, tmp);
this._i2o[ptr + 0] = (double)tmp.x;
this._i2o[ptr + 1] = (double)tmp.y;
i_base_factor.observ2Ideal(i - i_offset, j - i_offset, tmp);
this._o2i[ptr + 0] = (double)tmp.x;
this._o2i[ptr + 1] = (double)tmp.y;
}
}
return;
}
/**
* この関数は、座標を観察座標系から理想座標系へ変換します。
* @param ix
* 変換元の座標
* @param iy
* 変換元の座標
* @param o_point
* 変換後の座標を受け取るオブジェクト
*/
override public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
{
int idx = (int)ix + (int)iy * this._stride;
o_point.x = this._mapx[idx];
o_point.y = this._mapy[idx];
return;
}
/**
* この関数は、インスタンスの座標と、指定点との距離の２乗値を返します。
* @param i_p1
* 点の座標
* @return
* i_p1との距離の二乗値
*/
public double sqDist(NyARDoublePoint2d i_p1)
{
double x, y;
x = this.x - i_p1.x;
y = this.y - i_p1.y;
return(x * x + y * y);
}
/**
* この関数は、観察座標を理想座標へ変換します。
* 入力できる値範囲は、コンストラクタに設定したスクリーンサイズの範囲内です。
* @param ix
* 観察座標の値
* @param iy
* 観察座標の値
* @param o_point
* 理想座標を受け取るオブジェクト。
*/
public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
{
int idx = ix + iy * this._stride;
o_point.x = this._mapx[idx];
o_point.y = this._mapy[idx];
return;
}
/**
* この関数は、オブジェクトの一次元配列を作ります。
* @param i_number
* 作成する配列の長さ
* @return
* 新しい配列。
*/
public static NyARDoublePoint2d[] createArray(int i_length)
{
NyARDoublePoint2d[] ret = new NyARDoublePoint2d[i_length];
for (int i = 0; i < i_length; i++)
{
ret[i] = new NyARDoublePoint2d();
}
return(ret);
}
/**
* この関数は、オブジェクトの一次元配列を作ります。
* @param i_number
* 作成する配列の長さ
* @return
* 新しい配列。
*/
public static NyARDoublePoint2d[] createArray(int i_number)
{
NyARDoublePoint2d[] ret = new NyARDoublePoint2d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoublePoint2d();
}
return(ret);
}
/**
* この関数は、オブジェクトの一次元配列を作ります。
* @param i_number
* 作成する配列の長さ
* @return
* 新しい配列。
*/
public static NyARDoublePoint2d[] createArray(int i_number)
{
NyARDoublePoint2d[] ret = new NyARDoublePoint2d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARDoublePoint2d();
}
return ret;
}
public static void shiftCopy(NyARDoublePoint2d[] i_src, NyARIntPoint2d[] i_dst, int i_shift)
{
int l = i_src.Length;
for (int i = l - 1; i >= 0; i--)
{
int n = (i + i_shift) % l;
i_dst[i].x = (int)i_src[n].x;
i_dst[i].y = (int)i_src[n].y;
}
}
/**
* 2直線の交点を計算します。
* @param l_line_i
* @param l_line_2
* @param o_point
* @return
*/
public static bool crossPos(NyARLinear l_line_i, NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = l_line_2.dy * l_line_i.dx - l_line_i.dy * l_line_2.dx;
if (w1 == 0.0)
{
return false;
}
o_point.x = (l_line_2.dx * l_line_i.c - l_line_i.dx * l_line_2.c) / w1;
o_point.y = (l_line_i.dy * l_line_2.c - l_line_2.dy * l_line_i.c) / w1;
return true;
}
public void getScreenPos(int i_id, double i_x, double i_y, double i_z, ref Vector2 i_out)
{
NyARDoublePoint2d wk_2dpos = this.__wk_2dpos;
NyARDoublePoint3d wk_3dpos = this.__wk_3dpos;
this.getMarkerMatrix(i_id).transform3d(i_x, i_y, i_z, wk_3dpos);
this._frustum.project(wk_3dpos, wk_2dpos);
i_out.X = (float)wk_2dpos.x;
i_out.Y = (float)wk_2dpos.y;
return;
}
/**
* 2直線の交点を計算します。
* @param l_line_i
* @param l_line_2
* @param o_point
* @return
*/
public static bool crossPos(NyARLinear l_line_i, NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = l_line_2.dy * l_line_i.dx - l_line_i.dy * l_line_2.dx;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (l_line_2.dx * l_line_i.c - l_line_i.dx * l_line_2.c) / w1;
o_point.y = (l_line_i.dy * l_line_2.c - l_line_2.dy * l_line_i.c) / w1;
return(true);
}
/**
* 画面上の点が、このターゲットを構成する頂点の内側にあるか判定します。
* (範囲ではなく、頂点の内側であることに注意してください。)
* この関数は、Known/Unknownターゲットに使用できます。
* @param i_x
* @param i_y
* @return
*/
public bool isInnerVertexPoint2d(int i_x, int i_y)
{
Debug.Assert(this._target_type == RT_UNKNOWN || this._target_type == RT_KNOWN);
NyARDoublePoint2d[] vx=((NyARRectTargetStatus)(this._ref_tracktarget._ref_status)).vertex;
for(int i=3;i>=0;i--){
if(NyARDoublePoint2d.crossProduct3Point(vx[i],vx[(i+1)%4],i_x,i_y)<0)
{
return false;
}
}
return true;
}
/**
* この関数は、オブジェクトの二次元配列を作ります。
* @param i_length_x
* 作成する配列の列数
* @param i_length_y
* 作成する配列の行数
* @return
* 新しい配列。
*/
public static NyARDoublePoint2d[][] create2dArray(int i_length_x, int i_length_y)
{
NyARDoublePoint2d[][] ret = new NyARDoublePoint2d[i_length_y][];
for (int i = 0; i < i_length_y; i++)
{
ret[i] = new NyARDoublePoint2d[i_length_x];
for (int i2 = 0; i2 < i_length_x; i2++)
{
ret[i][i2] = new NyARDoublePoint2d();
}
}
return ret;
}
/**
* この関数は、オブジェクトの二次元配列を作ります。
* @param i_length_x
* 作成する配列の列数
* @param i_length_y
* 作成する配列の行数
* @return
* 新しい配列。
*/
public static NyARDoublePoint2d[][] create2dArray(int i_length_x, int i_length_y)
{
NyARDoublePoint2d[][] ret = new NyARDoublePoint2d[i_length_y][];
for (int i = 0; i < i_length_y; i++)
{
ret[i] = new NyARDoublePoint2d[i_length_x];
for (int i2 = 0; i2 < i_length_x; i2++)
{
ret[i][i2] = new NyARDoublePoint2d();
}
}
return(ret);
}
/**
* 4頂点を巡回して、正の外積の個数を数える。
* @param p
* @param order
* @return
*/
private static int countPlusExteriorProduct(NyARDoublePoint2d[] p, int[] order)
{
int ret = 0;
for (int i = 0; i < 4; i++)
{
if (0 < NyARDoublePoint2d.crossProduct3Point(p[order[i + 0]], p[order[(i + 1) % 4]], p[order[(i + 2) % 4]]))
{
ret++;
}
}
return(ret);
}
override public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
{
int px = (int)(ix + 0.5) + this._xOff;
int py = (int)(iy + 0.5) + this._yOff;
if (px < 0 || px >= this._xsize || py < 0 || py >= this._ysize)
{
throw new NyARRuntimeException();
}
int lt = (py * this._xsize + px) * 2;
o_point.x = this._o2i[lt + 0];
o_point.y = this._o2i[lt + 1];
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public override void ideal2Observ(double i_x, double i_y, NyARDoublePoint2d o_out)
{
int px = (int)(i_x + 0.5) + this._xOff;
int py = (int)(i_y + 0.5) + this._yOff;
if (px < 0 || px >= this._xsize || py < 0 || py >= this._ysize)
{
this._base_factor.ideal2Observ(i_x, i_y, o_out);
return;
}
int lt = (py * this._xsize + px) * 2;
o_out.x = this._i2o[lt + 0];
o_out.y = this._i2o[lt + 1];
return;
}
/**
* この関数は、座標を観察座標系から理想座標系へ変換します。
* @param ix
* 変換元の座標
* @param iy
* 変換元の座標
* @param o_point
* 変換後の座標を受け取るオブジェクト
* @todo should optimize!
*/
override public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
{
// OpenCV distortion model, with addition of a scale factor so that
// entire image fits onscreen.
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 px = (ix - x0) / fx;
double py = (iy - y0) / fy;
double x02 = px * px;
double y02 = py * py;
for (int i = 1; ; i++)
{
if (x02 != 0.0 || y02 != 0.0)
{
px = px - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * px + 2.0 * p1 * px * py + p2 * (x02 + y02 + 2.0 * x02) - ((ix - x0) / fx)) / (1.0 + k1 * (3.0 * x02 + y02) + k2 * (5.0 * x02 * x02 + 3.0 * x02 * y02 + y02 * y02) + 2.0 * p1 * py + 6.0 * p2 * px);
py = py - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * py + p1 * (x02 + y02 + 2.0 * y02) + 2.0 * p2 * px * py - ((iy - y0) / fy)) / (1.0 + k1 * (x02 + 3.0 * y02) + k2 * (x02 * x02 + 3.0 * x02 * y02 + 5.0 * y02 * y02) + 6.0 * p1 * py + 2.0 * p2 * px);
}
else
{
px = 0.0;
py = 0.0;
break;
}
if (i == PD_LOOP2)
{
break;
}
x02 = px * px;
y02 = py * py;
}
o_point.x = px * fx / this._s + x0;
o_point.y = py * fy / this._s + y0;
return;
}
/**
* コンストラクタです。
* 入力した{@link NyARCameraDistortionFactor}とそのサイズから、テーブルを作成します。
* ２つのパラメータは整合性が取れていなければなりません。
* (通常は、{@link NyARParam}の{@link NyARParam#getDistortionFactor()},{@link NyARParam#getScreenSize()}から得られた
* パラメータを入力します。)
* @param i_distfactor
* 樽型歪みパラメータのオブジェクト。
* @param i_screen_size
* スクリーンサイズ
*/
public NyARObserv2IdealMap(INyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
{
NyARDoublePoint2d opoint = new NyARDoublePoint2d();
this._mapx = new double[i_screen_size.w * i_screen_size.h];
this._mapy = new double[i_screen_size.w * i_screen_size.h];
this._stride = i_screen_size.w;
int ptr = i_screen_size.h * i_screen_size.w - 1;
//歪みマップを構築
for (int i = i_screen_size.h - 1; i >= 0; i--)
{
for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
{
i_distfactor.observ2Ideal(i2, i, opoint);
this._mapx[ptr] = opoint.x;
this._mapy[ptr] = opoint.y;
ptr--;
}
}
return;
}
public void run()
{
try {
Thread.Sleep(3000);
RawbitSerialIdTable.IdentifyIdResult ret = new RawbitSerialIdTable.IdentifyIdResult();
bool res;
lock (this._parent._mklib){
NyARDoublePoint2d[] vx = NyARDoublePoint2d.createArray(4);
//反時計まわり
vx[0].x = 0; vx[0].y = 0;
vx[1].x = 99; vx[1].y = 0;
vx[2].x = 99; vx[2].y = 99;
vx[3].x = 0; vx[3].y = 99;
res = this._parent._mklib.identifyId(vx, this._source, ret);
}
this._parent.callListener(res, this._serial, ret.artk_direction, ret.marker_width, ret.id);
} catch (Exception e) {
Console.Error.WriteLine(e.StackTrace);
}
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
override public void ideal2Observ(double i_x, double i_y, NyARDoublePoint2d 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 = (x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0;
o_out.y = (y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0;
return;
}
public bool traceLine(NyARDoublePoint2d i_pos1, NyARDoublePoint2d 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 = (int)(i_pos2.x - i_pos1.x);
int dy = (int)(i_pos2.y - i_pos1.y);
int r = base_s.w - s;
int b = base_s.h - s;
// 最大24点を定義して、そのうち両端の2個を除いた点を使用する。
for (int i = 1; i < dist - 1; i++)
{
int x = (int)(i * dx / dist + i_pos1.x - i_edge);
int y = (int)(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;
// 点数は10点程度を得る。
return(traceConture(coord, 1, s, o_coord));
}
/**
* コンストラクタです。
* 入力した{@link INyARCameraDistortionFactor}とそのサイズから、テーブルを作成します。
* ２つのパラメータは整合性が取れていなければなりません。
* (通常は、{@link NyARParam}の{@link NyARParam#getDistortionFactor()},{@link NyARParam#getScreenSize()}から得られた
* パラメータを入力します。)
* @param i_distfactor
* 樽型歪みパラメータのオブジェクト。
* @param i_screen_size
* スクリーンサイズ
*/
public NyARObserv2IdealMap(INyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
{
NyARDoublePoint2d opoint = new NyARDoublePoint2d();
this._mapx = new double[i_screen_size.w * i_screen_size.h];
this._mapy = new double[i_screen_size.w * i_screen_size.h];
this._stride = i_screen_size.w;
int ptr = i_screen_size.h * i_screen_size.w - 1;
//歪みマップを構築
for (int i = i_screen_size.h - 1; i >= 0; i--)
{
for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
{
i_distfactor.observ2Ideal(i2, i, opoint);
this._mapx[ptr] = opoint.x;
this._mapy[ptr] = opoint.y;
ptr--;
}
}
return;
}
/**
* この関数は、射影変換後の２次元頂点座標をセットします。
* i_number_of_vertexは、コンストラクタで指定した最大数以下である必要があります。
*/
public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d, int i_number_of_vertex)
{
//3x2nと2n*3の行列から、最小二乗法計算するために3x3マトリクスを作る。
//行列[A]の3列目のキャッシュ
double[] cx = this._cx;
double[] cy = this._cy;
double m22;
double p00 = this._projection_mat.m00;
double p01 = this._projection_mat.m01;
double p11 = this._projection_mat.m11;
double p12 = this._projection_mat.m12;
double p02 = this._projection_mat.m02;
double w1, w2, w3, w4;
this._a00 = i_number_of_vertex * p00 * p00;
this._a01_10 = i_number_of_vertex * p00 * p01;
this._a11 = i_number_of_vertex * (p01 * p01 + p11 * p11);
//[A]T*[A]の計算
m22 = 0;
w1 = w2 = 0;
for (int i = 0; i < i_number_of_vertex; i++)
{
//座標を保存しておく。
w3 = p02 - (cx[i] = i_ref_vertex_2d[i].x);
w4 = p12 - (cy[i] = i_ref_vertex_2d[i].y);
w1 += w3;
w2 += w4;
m22 += w3 * w3 + w4 * w4;
}
this._a02_20 = w1 * p00;
this._a12_21 = p01 * w1 + p11 * w2;
this._a22 = m22;
this._nmber_of_vertex = i_number_of_vertex;
return;
}
public NyARFixedFloatObserv2IdealMap(NyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
{
NyARDoublePoint2d opoint = new NyARDoublePoint2d();
this._mapx = new int[i_screen_size.w * i_screen_size.h];
this._mapy = new int[i_screen_size.w * i_screen_size.h];
this._stride = i_screen_size.w;
int ptr = i_screen_size.h * i_screen_size.w - 1;
//歪みマップを構築
for (int i = i_screen_size.h - 1; i >= 0; i--)
{
for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
{
i_distfactor.observ2Ideal(i2, i, opoint);
this._mapx[ptr] = (int)(opoint.x * 65536);
this._mapy[ptr] = (int)(opoint.y * 65536);
ptr--;
}
}
i_distfactor.getValue(this._factor);
return;
}
public NyARCameraDistortionFactorMap(int i_screen_width, int i_screen_height, INyARCameraDistortionFactor i_base_factor)
{
this._base_factor = i_base_factor;
NyARDoublePoint2d opoint = new NyARDoublePoint2d();
this._mapx = new double[i_screen_width * i_screen_height];
this._mapy = new double[i_screen_width * i_screen_height];
this._stride = i_screen_width;
int ptr = i_screen_height * i_screen_width - 1;
//歪みマップを構築
for (int i = i_screen_height - 1; i >= 0; i--)
{
for (int i2 = i_screen_width - 1; i2 >= 0; i2--)
{
i_base_factor.observ2Ideal(i2, i, opoint);
this._mapx[ptr] = opoint.x;
this._mapy[ptr] = opoint.y;
ptr--;
}
}
return;
}
/**
* カメラ座標系の4頂点でかこまれる領域から、RGB画像をo_rasterに取得します。
* @param i_vertex
* @param i_matrix
* i_vertexに適応する変換行列。
* ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。
* @param i_resolution
* @param o_raster
* @return
* @throws NyARException
*/
public bool GetRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster)
{
NyARDoublePoint2d[] vx = NyARDoublePoint2d.createArray(4);
if (i_matrix != null)
{
//姿勢変換してから射影変換
NyARDoublePoint3d v3d = new NyARDoublePoint3d();
for (int i = 3; i >= 0; i--)
{
i_matrix.transform3d(i_vertex[i], v3d);
this._ref_prjmat.project(v3d, vx[i]);
}
}
else
{
//射影変換のみ
for (int i = 3; i >= 0; i--)
{
this._ref_prjmat.project(i_vertex[i], vx[i]);
}
}
//パターンの取得
return(i_src.refPerspectiveRasterReader().copyPatt(vx, 0, 0, i_resolution, o_raster));
}
virtual public void extructMatches(HomographyMat H, FeaturePairStack matches)
{
double threshold2 = this._threshold2;
NyARDoublePoint2d xp = new NyARDoublePoint2d();// float xp[2];
//前方詰め
int pos = 0;
for (int i = 0; i < matches.getLength(); i++)
{
H.multiplyPointHomographyInhomogenous(matches.getItem(i).ref_.x, matches.getItem(i).ref_.y, xp);
double t1 = xp.x - matches.getItem(i).query.x;
double t2 = xp.y - matches.getItem(i).query.y;
double d2 = (t1 * t1) + (t2 * t2);
if (d2 <= threshold2)
{
matches.swap(i, pos);
pos++;
}
}
matches.setLength(pos);
return;
}
public bool copyPatt(NyARDoublePoint2d[] i_vertex, int i_edge_x, int i_edge_y, int i_resolution, INyARRgbRaster i_out)
{
return this.copyPatt(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, i_edge_x, i_edge_y, i_resolution, i_out);
}
/**
* この関数は、インスタンスの座標と、指定点との距離の２乗値を返します。
* @param i_p1
* 点の座標
* @return
* i_p1との距離の二乗値
*/
public double sqDist(NyARDoublePoint2d i_p1)
{
double x, y;
x = this.x - i_p1.x;
y = this.y - i_p1.y;
return x * x + y * y;
}
/**
* Find which side of a line a point is on (+,-).
*
* @param[in] A First point on line
* @param[in] B Second point on line
* @param[in] C Arbitrary third point
*/
public static double LinePointSide(NyARDoublePoint2d A, NyARDoublePoint2d B, NyARDoublePoint2d C)
{
return((B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x));
}
/**
* この関数は、観察座標を理想座標へ変換します。
* 入力できる値範囲は、コンストラクタに設定したスクリーンサイズの範囲内です。
* @param ix
* 観察座標の値
* @param iy
* 観察座標の値
* @param o_point
* 理想座標を受け取るオブジェクト。
*/
public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
{
int idx = ix + iy * this._stride;
o_point.x = this._mapx[idx];
o_point.y = this._mapy[idx];
return;
}
/**
* この関数は、頂点集合を包括する矩形のサイズ値（幅、高さ）を計算して、インスタンスにセットします。
* @param i_vertex
* 頂点集合を格納した配列
* @param i_num_of_vertex
* 計算対象とする要素の数
*/
public void setAreaRect(NyARDoublePoint2d[] 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.w = xmax - xmin + 1;
}
/**
* この関数は、座標がサイズの範囲内(0,0-w,hの矩形)にあるかを返します。
* @param i_pos
* 調査する座標点
* @return
* 引数値が範囲内ならば、trueを返します。
* @
*/
public bool isInnerPoint(NyARDoublePoint2d i_pos)
{
return (i_pos.x < this.w && i_pos.y < this.h && 0 <= i_pos.x && 0 <= i_pos.y);
}
/**
* この関数は、頂点集合から、中央値(Σp[n]/n)を求めます。
* @param i_points
* 頂点集合を格納した配列です。
* @param i_number_of_data
* 配列中の有効な頂点数です。
* @param o_out
* 中央値を受け取るオブジェクトです。
*/
public static void makeCenter(NyARDoublePoint2d[] i_points, int i_number_of_data, NyARIntPoint2d o_out)
{
double lx, ly;
lx = ly = 0;
for (int i = i_number_of_data - 1; i >= 0; i--)
{
lx += i_points[i].x;
ly += i_points[i].y;
}
o_out.x = (int)(lx / i_number_of_data);
o_out.y = (int)(ly / i_number_of_data);
}
/**
* コンストラクタです。
* i_srcの値で初期化したインスタンスを生成します。
* @param i_src
* 初期値とするオブジェクト
*/
public NyARDoublePoint2d(NyARDoublePoint2d i_src)
{
this.x = i_src.x;
this.y = i_src.y;
return;
}
/**
* この関数は、３点で定義される直線から、外積を計算します。
* 外積は、p1->p2と、p2->p3で定義する直線の外積です。
* @param p1
* 点１
* @param p2
* 点２
* @param p3
* 点３
* @return
* 外積の値
*/
public static double crossProduct3Point(NyARDoublePoint2d p1, NyARDoublePoint2d p2, NyARDoublePoint2d p3)
{
return (p2.x - p1.x) * (p3.y - p2.y) - (p2.y - p1.y) * (p3.x - p2.x);
}
/**
* この関数は、オブジェクトからインスタンスに値をセットします。
* @param i_source
* コピー元のオブジェクト。
*/
public void setValue(NyARDoublePoint2d i_source)
{
this.x = (int)i_source.x;
this.y = (int)i_source.y;
}
/**
* クリッピング付きのライントレーサです。
*
* @param i_pos1
* @param i_pos2
* @param i_edge
* @param o_coord
* @return
* @throws NyARException
*/
public bool traceLineWithClip(NyARDoublePoint2d i_pos1,
NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntSize s = this._ref_base_raster.getSize();
bool is_p1_inside_area, is_p2_inside_area;
NyARIntPoint2d[] pt = this.__pt;
// 線分が範囲内にあるかを確認
is_p1_inside_area = s.isInnerPoint(i_pos1);
is_p2_inside_area = s.isInnerPoint(i_pos2);
// 個数で分岐
if (is_p1_inside_area && is_p2_inside_area)
{
// 2ならクリッピング必要なし。
if (!this.traceLine(i_pos1, i_pos2, i_edge, o_coord))
{
return(false);
}
return(true);
}
// 1,0個の場合は、線分を再定義
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2))
{
return(false);
}
if (!this.__temp_l.makeSegmentLine(s.w, s.h, pt))
{
return(false);
}
if (is_p1_inside_area != is_p2_inside_area)
{
// 1ならクリッピング後に、外に出ていた点に近い輪郭交点を得る。
if (is_p1_inside_area)
{
// p2が範囲外
pt[(i_pos2.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos1);
}
else
{
// p1が範囲外
pt[(i_pos1.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos2);
}
}
else
{
// 0ならクリッピングして得られた２点を使う。
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2))
{
return(false);
}
if (!this.__temp_l.makeSegmentLine(s.w, s.h, pt))
{
return(false);
}
}
if (!this.traceLine(pt[0], pt[1], i_edge, o_coord))
{
return(false);
}
return(true);
}
/** * ICPアルゴリズムによる姿勢推定を行います。 * * @param screenCoord * @param worldCoord * @param num * @param initMatXw2Xc * @param o_matxw2xc * @param o_result_param * 結果パラメータを受け取るオブジェクト。不要な場合はnullを指定可能。 * @return * @throws NyARException */ public abstract bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc,NyARTransMatResultParam o_result_param);
private double getSizeFactor(double x0, double y0, int xsize, int ysize)
{
double olen, ilen;
double sf1;
double sf = 100.0;
olen = x0;
NyARDoublePoint2d itmp = new NyARDoublePoint2d();
this.observ2Ideal(0, y0, itmp);
ilen = x0 - itmp.x;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
olen = xsize - x0;
this.observ2Ideal(xsize, y0, itmp);
ilen = itmp.x - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
olen = y0;
this.observ2Ideal(x0, 0, itmp);
ilen = y0 - itmp.y;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
olen = ysize - y0;
this.observ2Ideal(x0, ysize, itmp);
ilen = itmp.y - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
this.observ2Ideal(0, 0, itmp);
ilen = x0 - itmp.x;
olen = x0;
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = y0 - itmp.y;
olen = y0;
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
this.observ2Ideal(xsize, 0, itmp);
ilen = itmp.x - x0;
olen = xsize - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = y0 - itmp.y;
olen = y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
this.observ2Ideal(0, ysize, itmp);
ilen = x0 - itmp.x;
olen = x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = itmp.y - y0;
olen = ysize - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
this.observ2Ideal(xsize, ysize, itmp);
ilen = itmp.x - x0;
olen = xsize - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = itmp.y - y0;
olen = ysize - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
if (sf == 100.0)
{
sf = 1.0;
}
return(sf);
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
double err0 = 0, err1;
System.Diagnostics.Debug.Assert(num >= 4);
NyARIcpUtils.DeltaS dS = this.__dS;
NyARIcpUtils.U u = this.__u;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
o_matxw2xc.setValue(initMatXw2Xc);
double breakLoopErrorThresh = this.getBreakLoopErrorThresh();
double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2();
double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh();
double maxLoop = this.getMaxLoop();
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
for (int i = 0; ; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return false;
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
err1 /= num;
if (err1 < breakLoopErrorThresh)
{
break;
}
if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh))
{
break;
}
if (i == maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
// *err = err1;
return true;
}
public override sealed void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
{
this.observ2Ideal((double)ix, (double)iy, o_point);
return;
}
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l,
NyARDoublePoint3d[] worldCoord_l, int num_l,
NyARDoublePoint2d[] screenCoord_r,
NyARDoublePoint3d[] worldCoord_r, int num_r,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc)
{
System.Diagnostics.Debug.Assert(num_l + num_r >= 3);
double err0 = 0, err1;
// 6*2*num?
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num_l + num_r)
{
this.__jus = new NyARIcpUtils.JusStack(num_l + num_r);
this.__du = NyARDoublePoint2d.createArray(num_l + num_r);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
NyARIcpUtils.U u = this.__u;
NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44();
matXw2Xc.setValue(initMatXw2Xc);
matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L);
matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R);
for (int i = 0; ; i++)
{
matXw2Ul.mul(matXc2Ul, matXw2Xc);
matXw2Ur.mul(matXc2Ur, matXw2Xc);
err1 = 0.0;
for (int j = 0; j < num_l; j++)
{
if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j]))
{
return false;
}
double dx = screenCoord_l[j].x - u.x;
double dy = screenCoord_l[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
for (int j = 0; j < num_r; j++)
{
if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j]))
{
return false;
}
double dx = screenCoord_r[j].x - u.x;
double dy = screenCoord_r[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j + num_l].x = dx;
du[j + num_l].y = dy;
}
err1 /= (num_l + num_r);
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2
&& err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this.maxLoop)
{
break;
}
err0 = err1;
for (int j = 0; j < num_l; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0))
{
return false;
}
}
for (int j = 0; j < num_r; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(matXw2Xc);
}
return false;
}
/**
* この関数は、オブジェクトからインスタンスに値をセットします。
* @param i_src
* コピー元のオブジェクト。
*/
public void setValue(NyARDoublePoint2d i_src)
{
this.x = i_src.x;
this.y = i_src.y;
return;
}
public NyARDoublePoint2d getScreenPos(int i_id, double i_x, double i_y, double i_z, NyARDoublePoint2d i_out)
{
NyARDoublePoint3d _wk_3dpos = this._wk_3dpos;
this.getTransformMatrix(i_id).transform3d(i_x, i_y, i_z, _wk_3dpos);
this._view.getFrustum().project(_wk_3dpos, i_out);
return(i_out);
}
/**
* この関数は、オブジェクトからインスタンスに値をセットします。
* @param i_source
* コピー元のオブジェクト。
*/
public void setValue(NyARDoublePoint2d i_source)
{
this.x = (int)i_source.x;
this.y = (int)i_source.y;
}
/**
* この関数は、頂点集合から、中央値(Σp[n]/n)を求めます。
* @param i_points
* 頂点集合を格納した配列です。
* @param i_number_of_data
* 配列中の有効な頂点数です。
* @param o_out
* 中央値を受け取るオブジェクトです。
*/
public static void makeCenter(NyARDoublePoint2d[] i_points, int i_number_of_data, NyARDoublePoint2d o_out)
{
double x, y;
x = y = 0;
for (int i = i_number_of_data - 1; i >= 0; i--)
{
x += i_points[i].x;
y += i_points[i].y;
}
o_out.x = x / i_number_of_data;
o_out.x = y / i_number_of_data;
}
private double optimize(NyARRotMatrix_ARToolKit io_rotmat, NyARDoublePoint3d io_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex)
{
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
double err = -1;
//System.out.println("START");
// ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。
for (int i = 0; ; i++)
{
// <arGetTransMat3>
err = this._mat_optimize.modifyMatrix(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex);
io_rotmat.getPoint3dBatch(i_offset_3d, vertex_3d, 4);
i_solver.solveTransportVector(vertex_3d, io_transvec);
err = this._mat_optimize.modifyMatrix(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex);
//System.out.println("E:"+err*4);
// //</arGetTransMat3>
if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT - 1)
{
break;
}
io_rotmat.getPoint3dBatch(i_offset_3d, vertex_3d, 4);
i_solver.solveTransportVector(vertex_3d, io_transvec);
}
//System.out.println("END");
return err;
}
/**
* この関数は、３点で定義される直線から、外積を計算します。
* 外積は、p1->p2と、p2->p3で定義する直線の外積です。
* @param p1
* 点１
* @param p2
* 点２
* @param p3
* 点３
* @return
* 外積の値
*/
public static double crossProduct3Point(NyARDoublePoint2d p1, NyARDoublePoint2d p2, NyARDoublePoint2d p3)
{
return((p2.x - p1.x) * (p3.y - p2.y) - (p2.y - p1.y) * (p3.x - p2.x));
}
public void getCenter2d(NyARDoublePoint2d o_out)
{
o_out.x = (this.sqvertex[0].x + this.sqvertex[1].x + this.sqvertex[2].x + this.sqvertex[3].x) / 4;
o_out.y = (this.sqvertex[0].y + this.sqvertex[1].y + this.sqvertex[2].y + this.sqvertex[3].y) / 4;
return;
}
