public override void initRotBySquare(NyARLinear[] i_linear,NyARDoublePoint2d[] i_sqvertex)
{
base.initRotBySquare(i_linear,i_sqvertex);
//Matrixからangleをロード
this.updateAngleFromMatrix();
return;
}
/**
* この関数は、引数値からパラメータをインスタンスへコピーします。
* @param i_source
* コピー元のオブジェクト
*/
public void copyFrom(NyARLinear i_source)
{
this.b = i_source.b;
this.a = i_source.a;
this.c = i_source.c;
return;
}
public double c; //切片
public static NyARLinear[] createArray(int i_number)
{
NyARLinear[] ret = new NyARLinear[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARLinear();
}
return(ret);
}
/**
* この関数は、指定サイズのオブジェクト配列を作ります。
* @param i_number
* 作成する配列の長さ
* @return
* 新しい配列。
*/
public static NyARLinear[] createArray(int i_number)
{
NyARLinear[] ret = new NyARLinear[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARLinear();
}
return ret;
}
/**
* i_x,i_yを通過する、i_linearの法線を計算して、格納します。
*/
public void normalLine(double i_x, double i_y, NyARLinear i_linear)
{
double la = i_linear.a;
double lb = i_linear.b;
this.a = lb;
this.b = -la;
this.c = -(lb * i_x - la * i_y);
}
//override
public sealed override bool initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
{
bool ret = base.initRotBySquare(i_linear, i_sqvertex);
if (ret)
{
//Matrixからangleをロード
this.updateAngleFromMatrix();
}
return ret;
}
/**
* 2直線の交点を計算します。
* @param l_line_2
* @param o_point
* @return
*/
public bool crossPos(NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = this.a * l_line_2.b - l_line_2.a * this.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (this.b * l_line_2.c - l_line_2.b * this.c) / w1;
o_point.y = (l_line_2.a * this.c - this.a * l_line_2.c) / w1;
return(true);
}
/**
* 輪郭点集合からay+bx+c=0の直線式を計算します。
* @param i_st
* @param i_ed
* @param i_xcoord
* @param i_ycoord
* @param i_cood_num
* @param o_line
* @return
* @throws NyARException
*/
public bool coord2Line(int i_st,int i_ed,int[] i_xcoord, int[] i_ycoord,int i_cood_num, NyARLinear o_line)
{
//頂点を取得
int n,st,ed;
double w1;
//探索区間の決定
if(i_ed>=i_st){
//頂点[i]から頂点[i+1]までの輪郭が、1区間にあるとき
w1 = (double) (i_ed - i_st + 1) * 0.05 + 0.5;
//探索区間の決定
st = (int) (i_st+w1);
ed = (int) (i_ed - w1);
}else{
//頂点[i]から頂点[i+1]までの輪郭が、2区間に分かれているとき
w1 = (double) ((i_ed+i_cood_num-i_st+1)%i_cood_num) * 0.05 + 0.5;
//探索区間の決定
st = ((int) (i_st+w1))%i_cood_num;
ed = ((int) (i_ed+i_cood_num-w1))%i_cood_num;
}
//探索区間数を確認
if(st<=ed){
//探索区間は1区間
n = ed - st + 1;
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n,this._xpos,this._ypos,0);
}else{
//探索区間は2区間
n=ed+1+i_cood_num-st;
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st,i_cood_num-st,this._xpos,this._ypos,0);
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, 0,ed+1,this._xpos,this._ypos,i_cood_num-st);
}
//要素数の確認
if (n < 2) {
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return false;
}
//主成分分析する。
NyARDoubleMatrix22 evec=this.__getSquareLine_evec;
double[] mean=this.__getSquareLine_mean;
this._pca.pca(this._xpos,this._ypos,n,evec, this.__getSquareLine_ev,mean);
o_line.dy = evec.m01;// line[i][0] = evec->m[1];
o_line.dx = -evec.m00;// line[i][1] = -evec->m[0];
o_line.c = -(o_line.dy * mean[0] + o_line.dx * mean[1]);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
return true;
}
/**
* ２直線に直交するベクトルを計算する・・・だと思う。
* @param i_linear1
* @param i_linear2
*/
public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)
{
//1行目
NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
double w1 = i_linear1.dy * i_linear2.dx - i_linear2.dy * i_linear1.dx;
double w2 = i_linear1.dx * i_linear2.c - i_linear2.dx * i_linear1.c;
double w3 = i_linear1.c * i_linear2.dy - i_linear2.c * i_linear1.dy;
double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];
double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];
double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];
double w = Math.Sqrt(m0 * m0 + m1 * m1 + m2 * m2);
this.v1 = m0 / w;
this.v2 = m1 / w;
this.v3 = m2 / w;
return;
}
/**
* i_x,i_yを通るこの直線の法線と、i_linearが交わる点を返します。
* @param i_x
* @param i_y
* @param i_linear
* @param o_point
* @return
*/
public bool normalLineCrossPos(double i_x, double i_y, NyARLinear i_linear, NyARDoublePoint2d o_point)
{
//thisを法線に変換
double la = this.b;
double lb = -this.a;
double lc = -(la * i_x + lb * i_y);
//交点を計算
double w1 = i_linear.a * lb - la * i_linear.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = ((i_linear.b * lc - lb * i_linear.c) / w1);
o_point.y = ((la * i_linear.c - i_linear.a * lc) / w1);
return(true);
}
/**
* この関数は、２直線に直交するベクトルを計算して、その３次元ベクトルをインスタンスに格納します。
* （多分）
* @param i_linear1
* 直線１
* @param i_linear2
* 直線２
*/
public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)
{
//1行目
NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
double w1 = i_linear1.a * i_linear2.b - i_linear2.a * i_linear1.b;
double w2 = i_linear1.b * i_linear2.c - i_linear2.b * i_linear1.c;
double w3 = i_linear1.c * i_linear2.a - i_linear2.c * i_linear1.a;
double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01; //w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];
double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00; //-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];
double m2 = w1 * cmat.m00 * cmat.m11; //w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];
double w = Math.Sqrt(m0 * m0 + m1 * m1 + m2 * m2);
this.v1 = m0 / w;
this.v2 = m1 / w;
this.v3 = m2 / w;
return;
}
/**
* この関数は、i_lineの直線を、インスタンスにセットします。
* {@link #x},{@link #y}の値は、(i_x,i_y)を通過するi_lineの法線とi_lineの交点をセットします。
* @param i_line
* セットする直線式
* @param i_x
* {@link #x},{@link #y}を確定するための、法線の通過点
* @param i_y
* {@link #x},{@link #y}を確定するための、法線の通過点
* @return
* セットに成功すると、trueを返します。
*/
public bool setLinear(NyARLinear i_line, double i_x, double i_y)
{
double la = i_line.b;
double lb = -i_line.a;
double lc = -(la * i_x + lb * i_y);
//交点を計算
double w1 = -lb * lb - la * la;
if (w1 == 0.0)
{
return(false);
}
this.x = ((la * lc - lb * i_line.c) / w1);
this.y = ((la * i_line.c + lb * lc) / w1);
this.dy = -lb;
this.dx = -la;
return(true);
}
public virtual void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
{
NyARRotVector vec1 = this.__initRot_vec1;
NyARRotVector vec2 = this.__initRot_vec2;
//向かい合った辺から、２本のベクトルを計算
//軸１
vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);
//軸２
vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);
//回転の最適化？
NyARRotVector.checkRotation(vec1, vec2);
this.m00 = vec1.v1;
this.m10 = vec1.v2;
this.m20 = vec1.v3;
this.m01 = vec2.v1;
this.m11 = vec2.v2;
this.m21 = vec2.v3;
//最後の軸を計算
double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;
double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;
double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;
double w = Math.Sqrt(w02 * w02 + w12 * w12 + w22 * w22);
this.m02 = w02 / w;
this.m12 = w12 / w;
this.m22 = w22 / w;
//Matrixからangleをロード
return;
}
/**
* 輪郭点集合からay+bx+c=0の直線式を計算します。
* @param i_st
* @param i_ed
* @param i_xcoord
* @param i_ycoord
* @param i_cood_num
* @param o_line
* @return
* @throws NyARException
*/
public bool coord2Line(int i_st, int i_ed, int[] i_xcoord, int[] i_ycoord, int i_cood_num, NyARLinear o_line)
{
//頂点を取得
int n, st, ed;
double w1;
//探索区間の決定
if (i_ed >= i_st)
{
//頂点[i]から頂点[i+1]までの輪郭が、1区間にあるとき
w1 = (double)(i_ed - i_st + 1) * 0.05 + 0.5;
//探索区間の決定
st = (int)(i_st + w1);
ed = (int)(i_ed - w1);
}
else
{
//頂点[i]から頂点[i+1]までの輪郭が、2区間に分かれているとき
w1 = (double)((i_ed + i_cood_num - i_st + 1) % i_cood_num) * 0.05 + 0.5;
//探索区間の決定
st = ((int)(i_st + w1)) % i_cood_num;
ed = ((int)(i_ed + i_cood_num - w1)) % i_cood_num;
}
//探索区間数を確認
if (st <= ed)
{
//探索区間は1区間
n = ed - st + 1;
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n, this._xpos, this._ypos, 0);
}
else
{
//探索区間は2区間
n = ed + 1 + i_cood_num - st;
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, i_cood_num - st, this._xpos, this._ypos, 0);
this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, 0, ed + 1, this._xpos, this._ypos, i_cood_num - st);
}
//要素数の確認
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return(false);
}
//主成分分析する。
NyARDoubleMatrix22 evec = this.__getSquareLine_evec;
double[] mean = this.__getSquareLine_mean;
this._pca.pca(this._xpos, this._ypos, n, evec, this.__getSquareLine_ev, mean);
o_line.dy = evec.m01; // line[i][0] = evec->m[1];
o_line.dx = -evec.m00; // line[i][1] = -evec->m[0];
o_line.c = -(o_line.dy * mean[0] + o_line.dx * mean[1]); // line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
return(true);
}
/**
* この関数は、i_x,i_yを通るこの直線の法線と、i_linearが交わる点を返します。
* @param i_x
* 法線が通過する点X
* @param i_y
* 法線が通過する点Y
* @param i_linear
* 交点を計算する直線式
* @param o_point
* 交点を返却するオブジェクト
* @return
* 交点が求まれば、trueを返します。
*/
public bool normalLineCrossPos(double i_x, double i_y, NyARLinear i_linear, NyARDoublePoint2d o_point)
{
//thisを法線に変換
double la = this.b;
double lb = -this.a;
double lc = -(la * i_x + lb * i_y);
//交点を計算
double w1 = i_linear.a * lb - la * i_linear.b;
if (w1 == 0.0)
{
return false;
}
o_point.x = ((i_linear.b * lc - lb * i_linear.c) / w1);
o_point.y = ((la * i_linear.c - i_linear.a * lc) / w1);
return true;
}
/**
* この関数は、i_x,i_yを通過する、i_linearの法線を計算して、インスタンスへ格納します。
* @param i_x
* 通過点X
* @param i_y
* 通過点Y
* @param i_linear
* 法線を計算する直線式(この引数にはthisを指定できます。)
*/
public void normalLine(double i_x, double i_y, NyARLinear i_linear)
{
double la = i_linear.a;
double lb = i_linear.b;
this.a = lb;
this.b = -la;
this.c = -(lb * i_x - la * i_y);
}
/**
* この関数は、2直線が交差しているかを返します。
* @param l_line_2
* 交差しているか確認するオブジェクト
* @return
* 交差していればtrue
*/
public bool isCross(NyARLinear l_line_2)
{
double w1 = this.a * l_line_2.b - l_line_2.a * this.b;
return (w1 == 0.0) ? false : true;
}
/**
* この関数は、直線の交点を計算します。
* @param l_line_2
* 交点を計算する直線式
* @param o_point
* 交点座標を格納するオブジェクト
* @return
* 交点が求まればtrue
*/
public bool crossPos(NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = this.a * l_line_2.b - l_line_2.a * this.b;
if (w1 == 0.0)
{
return false;
}
o_point.x = (this.b * l_line_2.c - l_line_2.b * this.c) / w1;
o_point.y = (l_line_2.a * this.c - this.a * l_line_2.c) / w1;
return true;
}
/**
* 2直線が交差しているかを返します。
* @param l_line_2
* @return
*/
public bool isCross(NyARLinear l_line_2)
{
double w1 = this.a * l_line_2.b - l_line_2.a * this.b;
return((w1 == 0.0)?false:true);
}
/**
* この関数は、i_lineの直線を、インスタンスにセットします。
* {@link #x},{@link #y}の値は、(i_x,i_y)を通過するi_lineの法線とi_lineの交点をセットします。
* @param i_line
* セットする直線式
* @param i_x
* {@link #x},{@link #y}を確定するための、法線の通過点
* @param i_y
* {@link #x},{@link #y}を確定するための、法線の通過点
* @return
* セットに成功すると、trueを返します。
*/
public bool setLinear(NyARLinear i_line, double i_x, double i_y)
{
double la = i_line.b;
double lb = -i_line.a;
double lc = -(la * i_x + lb * i_y);
//交点を計算
double w1 = -lb * lb - la * la;
if (w1 == 0.0)
{
return false;
}
this.x = ((la * lc - lb * i_line.c) / w1);
this.y = ((la * i_line.c + lb * lc) / w1);
this.dy = -lb;
this.dx = -la;
return true;
}
/**
* 予想位置を基準に四角形をトレースして、一定の基準をクリアするかを評価します。
* @param i_reader
* @param i_edge_size
* @param i_prevsq
* @return
* @throws NyARException
*/
private bool traceSquareLine(INyARVectorReader i_reader, int i_edge_size, NyARRectTargetStatus i_prevsq, NyARLinear[] o_line)
{
NyARDoublePoint2d p1,p2;
VecLinearCoordinates vecpos=this._ref_my_pool._vecpos;
//NyARIntRect i_rect
p1=i_prevsq.estimate_vertex[0];
int dist_limit=i_edge_size*i_edge_size;
//強度敷居値(セルサイズ-1)
// int min_th=i_edge_size*2+1;
// min_th=(min_th*min_th);
for(int i=0;i<4;i++)
{
p2=i_prevsq.estimate_vertex[(i+1)%4];
//クリップ付きで予想位置周辺の直線のトレース
i_reader.traceLineWithClip(p1,p2,i_edge_size,vecpos);
//クラスタリングして、傾きの近いベクトルを探す。(限界は10度)
this._ref_my_pool._vecpos_op.margeResembleCoords(vecpos);
//基本的には1番でかいベクトルだよね。だって、直線状に取るんだもの。
int vid=vecpos.getMaxCoordIndex();
//データ品質規制(強度が多少強くないと。)
// if(vecpos.items[vid].sq_dist<(min_th)){
// return false;
// }
//@todo:パラメタ調整
//角度規制(元の線分との角度を確認)
if(vecpos.items[vid].getAbsVecCos(i_prevsq.vertex[i],i_prevsq.vertex[(i+1)%4])<NyARMath.COS_DEG_5){
//System.out.println("CODE1");
return false;
}
//@todo:パラメタ調整
//予想点からさほど外れていない点であるか。(検出点の移動距離を計算する。)
double dist;
dist=vecpos.items[vid].sqDistBySegmentLineEdge(i_prevsq.vertex[i],i_prevsq.vertex[i]);
if(dist<dist_limit){
o_line[i].setVectorWithNormalize(vecpos.items[vid]);
}else{
//System.out.println("CODE2:"+dist+","+dist_limit);
return false;
}
//頂点ポインタの移動
p1=p2;
}
return true;
}