/** * CheckHomographyHeuristics * Check if a homography is valid based on some heuristics. * @param i_h_inv * inversed homography matrix; * @param refWidth * @param refHeight * @return */ virtual public bool checkHomographyHeuristics(int refWidth, int refHeight) { NyARDoublePoint2d p0p = new NyARDoublePoint2d(); NyARDoublePoint2d p1p = new NyARDoublePoint2d(); NyARDoublePoint2d p2p = new NyARDoublePoint2d(); NyARDoublePoint2d p3p = new NyARDoublePoint2d(); this.multiplyPointHomographyInhomogenous(0, 0, p0p); this.multiplyPointHomographyInhomogenous(refWidth, 0, p1p); this.multiplyPointHomographyInhomogenous(refWidth, refHeight, p2p); this.multiplyPointHomographyInhomogenous(0, refHeight, p3p); double tr = refWidth * refHeight * 0.0001f; if (SmallestTriangleArea(p0p, p1p, p2p, p3p) < tr) { return(false); } if (!QuadrilateralConvex(p0p, p1p, p2p, p3p)) { return(false); } return(true); }
private void multiplyPointHomographyInhomogenous(double i_x, double i_y, NyARDoublePoint2d i_dest) { double w = this.m20 * i_x + this.m21 * i_y + this.m22; i_dest.x = (this.m00 * i_x + this.m01 * i_y + this.m02) / w; // XP i_dest.y = (this.m10 * i_x + this.m11 * i_y + this.m12) / w; // YP }
/** * この関数は、点が矩形の範囲内にあるか判定します。 * @param i_pos * 調査する座標 * @return * 点が矩形の中にあれば、trueを返します。 */ public bool isInnerPoint(NyARDoublePoint2d i_pos) { int x = (int)i_pos.x - this.x; int y = (int)i_pos.y - this.y; return(0 <= x && x < this.w && 0 <= y && y < this.h); }
/** * Compute the area of a triangle. */ private static double AreaOfTriangle(NyARDoublePoint2d u, NyARDoublePoint2d v) { // T a = u[0]*v[1] - u[1]*v[0]; double a = u.x * v.y - u.y * v.x; return((double)(Math.Abs(a) * 0.5)); }
private bool solveHomography4PointsInhomogenous(NyARDoubleMatrix33 i_homography_mat, NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4, NyARDoublePoint2d xp1, NyARDoublePoint2d xp2, NyARDoublePoint2d xp3, NyARDoublePoint2d xp4) { double[][] _mat_A = ArrayUtils.newDouble2dArray(8, 9); // x1.setValue(0, 0);x2.setValue(10, 0);x3.setValue(10, 10);x4.setValue(0, 10); // xp1.setValue(10, 10);xp2.setValue(10, 0);xp3.setValue(0, 0);xp4.setValue(0, 10); //Homography4PointsInhomogeneousConstraint AddHomographyPointContraint(_mat_A, 0, x1, xp1); AddHomographyPointContraint(_mat_A, 2, x2, xp2); AddHomographyPointContraint(_mat_A, 4, x3, xp3); AddHomographyPointContraint(_mat_A, 6, x4, xp4); //SolveHomography4PointsInhomogenous if (!this.solveNullVector8x9Destructive(i_homography_mat, _mat_A)) { return(false); } if (Math.Abs(i_homography_mat.determinant()) < 1e-5) { return(false); } return(true); }
/** * この関数は、頂点群から最小二乗法を使用して直線を計算します。 * @param i_points * 頂点群を格納した配列。 * @param i_number_of_data * 計算対象の頂点群の数 * @return * 計算に成功すると、trueを返します。 */ public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data) { int i; double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0; for (i = 0; i < i_number_of_data; i++) { NyARDoublePoint2d ptr = i_points[i]; double xw = ptr.x; sum_xy += xw * ptr.y; sum_x += xw; sum_y += ptr.y; sum_x2 += xw * xw; } double la = -(i_number_of_data * sum_x2 - sum_x * sum_x); double lb = -(i_number_of_data * sum_xy - sum_x * sum_y); double cc = (sum_x2 * sum_y - sum_xy * sum_x); double lc = -(la * sum_x + lb * sum_y) / i_number_of_data; //交点を計算 double w1 = -lb * lb - la * la; if (w1 == 0.0) { return(false); } this.x = ((la * lc - lb * cc) / w1); this.y = ((la * cc + lb * lc) / w1); this.dy = -lb; this.dx = -la; return(true); }
/** * 基準点のdpiを推定する。 * @param i_cptrans * 射影変換行列. [cparam]*[trans] * @param trans * @param pos * [2] * @param o_dpi * x,y方向それぞれの推定dpi * @return */ public void ar2GetResolution2d(NyARNftFsetFile.NyAR2FeatureCoord i_pos, NyARDoublePoint2d o_dpi) { NyARDoubleMatrix44 t = this.ctrans; //基点 double mx0 = i_pos.mx; double my0 = i_pos.my; double h0 = t.m20 * mx0 + t.m21 * my0 + t.m23; double hx0 = t.m00 * mx0 + t.m01 * my0 + t.m03; double hy0 = t.m10 * mx0 + t.m11 * my0 + t.m13; double x0 = hx0 / h0; double y0 = hy0 / h0; double h, sx, sy; //+X h = h0 + t.m20 * DPI_BOX; sx = ((hx0 + DPI_BOX * t.m00) / h) - x0; sy = ((hy0 + DPI_BOX * t.m10) / h) - y0; //dpi -x o_dpi.x = Math.Sqrt(sx * sx + sy * sy) * 2.54; //+Y h = h0 + t.m21 * DPI_BOX; sx = ((hx0 + DPI_BOX * t.m01) / h) - x0; sy = ((hy0 + DPI_BOX * t.m11) / h) - y0; //dpi -y o_dpi.y = Math.Sqrt(sx * sx + sy * sy) * 2.54; return; }
private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB) { double v0, v1, v2, v3, v4, v5, v6, v7; v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0; for (int i = 0; i < i_num; i++) { double wx = worldCoord[i].x; double wy = worldCoord[i].y; double sx = screenCoord[i].x; double sy = screenCoord[i].y; v0 += wx * sx; v1 += wy * sx; v2 += sx; v3 += wx * sy; v4 += wy * sy; v5 += sy; v6 += -wx * sx * sx - wx * sy * sy; v7 += -wy * sx * sx - wy * sy * sy; } double[][] t = o_matAtB.getArray(); t[0][0] = v0; t[1][0] = v1; t[2][0] = v2; t[3][0] = v3; t[4][0] = v4; t[5][0] = v5; t[6][0] = v6; t[7][0] = v7; return o_matAtB; }
/** * p2-p1ベクトルのsquare normを計算する。 * @param i_p1 * @param i_p2 * @return */ public static double sqNorm(NyARDoublePoint2d i_p1, NyARDoublePoint2d i_p2) { double x, y; x = i_p2.x - i_p1.x; y = i_p2.y - i_p1.y; return x * x + y * y; }
/** * Multiply an in-homogenous point by a similarity. */ private static void MultiplyPointHomographyInhomogenous(NyARDoublePoint2d xp, NyARDoubleMatrix33 H, NyARDoublePoint2d x) { double w = H.m20 * x.x + H.m21 * x.y + H.m22; xp.x = (H.m00 * x.x + H.m01 * x.y + H.m02) / w; xp.y = (H.m10 * x.x + H.m11 * x.y + H.m12) / w; }
/** * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。 * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。 * 線分と直線の類似度を判定する数値になります。 * @param i_sp1 * 線分の端点1 * @param i_sp2 * 線分の端点2 * @return * 二乗距離値の合計。距離が取れないときは無限大です。 */ public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2) { double sa, sb, sc; sa = this.dy; sb = -this.dx; sc = (this.dx * this.y - this.dy * this.x); double lc; double x, y, w1; //thisを法線に変換 //交点を計算 w1 = sa * (-sa) - sb * sb; if (w1 == 0.0) { return(Double.PositiveInfinity); } //i_sp1と、i_linerの交点 lc = -(sb * i_sp1.x - sa * i_sp1.y); x = ((sb * lc + sa * sc) / w1) - i_sp1.x; y = ((sb * sc - sa * lc) / w1) - i_sp1.y; double sqdist = x * x + y * y; lc = -(sb * i_sp2.x - sa * i_sp2.y); x = ((sb * lc + sa * sc) / w1) - i_sp2.x; y = ((sb * sc - sa * lc) / w1) - i_sp2.y; return(sqdist + x * x + y * y); }
public override void initRotBySquare(NyARLinear[] i_linear,NyARDoublePoint2d[] i_sqvertex) { base.initRotBySquare(i_linear,i_sqvertex); //Matrixからangleをロード this.updateAngleFromMatrix(); return; }
/** * Add a point to the homography constraint matrix. */ private static void AddHomographyPointContraint(double[][] A, int A_ptr, NyARDoublePoint2d x, NyARDoublePoint2d xp) { A[A_ptr][0] = -x.x; //[0]; A[A_ptr][1] = -x.y; //[1]; A[A_ptr][2] = -1; // ZeroVector3(A+3); A[A_ptr][3] = 0; A[A_ptr][4] = 0; A[A_ptr][5] = 0; A[A_ptr][6] = xp.x * x.x; //xp[0] * x[0]; A[A_ptr][7] = xp.x * x.y; //xp[0] * x[1]; A[A_ptr][8] = xp.x; //xp[0]; // ZeroVector3(A+9); A[A_ptr + 1][0] = 0; A[A_ptr + 1][1] = 0; A[A_ptr + 1][2] = 0; A[A_ptr + 1][3] = -x.x; //-x[0]; A[A_ptr + 1][4] = -x.y; //-x[1]; A[A_ptr + 1][5] = -1; A[A_ptr + 1][6] = xp.y * x.x; //xp[1] * x[0]; A[A_ptr + 1][7] = xp.y * x.y; //xp[1] * x[1]; A[A_ptr + 1][8] = xp.y; //xp[1]; }
/** * 直線と、i_sp1とi_sp2の作る線分との二乗距離値の合計を返します。計算方法は、線分の2端点を通過する直線の法線上での、2端点と直線の距離の合計です。 * 線分と直線の類似度を判定する数値になります。 * @param i_sp1 * @param i_sp2 * @param o_point * @return * 距離が取れないときは無限大です。 */ public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2) { double la, lb, lc; double x, y, w1; //thisを法線に変換 la = this.b; lb = -this.a; //交点を計算 w1 = this.a * lb - la * this.b; if (w1 == 0.0) { return(double.PositiveInfinity); } //i_sp1と、i_linerの交点 lc = -(la * i_sp1.x + lb * i_sp1.y); x = ((this.b * lc - lb * this.c) / w1) - i_sp1.x; y = ((la * this.c - this.a * lc) / w1) - i_sp1.y; double sqdist = x * x + y * y; lc = -(la * i_sp2.x + lb * i_sp2.y); x = ((this.b * lc - lb * this.c) / w1) - i_sp2.x; y = ((la * this.c - this.a * lc) / w1) - i_sp2.y; return(sqdist + x * x + y * y); }
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.x = xmin; this.w = xmax - xmin + 1; this.y = ymin; }
/** * この関数は、ラスタドライバから画像を読み出します。 * @param i_pix_drv * @param i_size * @param i_vertex * @param o_data * @param o_param * @return * @ */ public bool pickFromRaster(INyARGsPixelDriver i_pix_drv, NyARDoublePoint2d[] 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); }
public void projectionConvert(double i_x, double i_y, double i_z, NyARDoublePoint2d o_2d) { double w = i_z * this.m22; o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) / w; o_2d.y = (i_y * this.m11 + i_z * this.m12) / w; return; }
/** * 座標値を射影変換します。 * @param i_3dvertex * 変換元の座標値 * @param o_2d * 変換後の座標値を受け取るオブジェクト */ public void project(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d) { double w = 1 / (i_3dvertex.z * this.m22); o_2d.x = (i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w; o_2d.y = (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, NyARDoublePoint2d o_2d) { double w = 1 / (i_z * this.m22); o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w; o_2d.y = (i_y * this.m11 + i_z * this.m12) * w; return; }
/** * Check the geometric consistency between three correspondences. */ private static bool Homography3PointsGeometricallyConsistent(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x1p, NyARDoublePoint2d x2p, NyARDoublePoint2d x3p) { if (((Geometry.LinePointSide(x1, x2, x3) > 0) ^ (Geometry.LinePointSide(x1p, x2p, x3p) > 0)) == true) { return(false); } return(true); }
/** * p2-p1ベクトルのsquare normを計算する。 * @param i_p1 * @param i_p2 * @return */ public static double sqNorm(NyARDoublePoint2d i_p1, NyARDoublePoint2d i_p2) { double x, y; x = i_p2.x - i_p1.x; y = i_p2.y - i_p1.y; return(x * x + y * y); }
/** * p2-p1間の距離の二乗値を計算します。 * @param i_p1 * @param i_p2 * @return */ 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 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); }
//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; }
public double cauchyProjectiveReprojectionCost(FeaturePairStack.Item i_ptr, double i_one_over_scale2) { NyARDoublePoint2d ref_ = i_ptr.ref_; double w = this.m20 * ref_.x + this.m21 * ref_.y + this.m22; double vx = ((this.m00 * ref_.x + this.m01 * ref_.y + this.m02) / w) - i_ptr.query.x; //XP double vy = ((this.m10 * ref_.x + this.m11 * ref_.y + this.m12) / w) - i_ptr.query.y; //YP double T = Math.Log(1 + (vx * vx + vy * vy) * i_one_over_scale2); return(T); }
/** * 理想点を計算する。 * @param i_coord * 変換元の座標(理想点) */ public void calculate2dPos(double i_x, double i_y, NyARDoublePoint2d o_idepos) { NyARDoubleMatrix44 t = this.ctrans; double h = (t.m20 * i_x + t.m21 * i_y + t.m23); double hx = (t.m00 * i_x + t.m01 * i_y + t.m03) / h; double hy = (t.m10 * i_x + t.m11 * i_y + t.m13) / h; o_idepos.x = hx; o_idepos.y = hy; }
/* * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina * * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z; * * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * * cosa)*iz+i_trans.z; * * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z)); * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc * * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc * * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc * * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n]) * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2 * +2*M*cos(x)*sin(x) */ private double optimizeParamX(double sinb, double cosb, double sinc, double cosc, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) { NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref; double L, J, K, M, N, O; L = J = K = M = N = O = 0; double cp00 = cp.m00; double cp01 = cp.m01; double cp02 = cp.m02; double cp11 = cp.m11; double cp12 = cp.m12; for (int i = 0; i < i_number_of_vertex; i++) { double ix, iy, iz; ix = i_vertex3d[i].x; iy = i_vertex3d[i].y; iz = i_vertex3d[i].z; double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy)); double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz)); double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z)); double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy)); double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz)); double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z)); double H0 = (iy); double H1 = (-sinb * ix + cosb * iz); double H2 = i_trans.z; double VX = i_vertex2d[i].x; double VY = i_vertex2d[i].y; double a, b, c, d, e, f; a = (VX * H0 - X0); b = (VX * H1 - X1); c = (VX * H2 - X2); d = (VY * H0 - Y0); e = (VY * H1 - Y1); f = (VY * H2 - Y2); L += d * e + a * b; N += d * d + a * a; J += d * f + a * c; M += e * e + b * b; K += e * f + b * c; O += f * f + c * c; } L *= 2; J *= 2; K *= 2; return getMinimumErrorAngleFromParam(L, J, K, M, N, O, i_hint_angle); }
/** * Check if four points form a convex quadrilaternal. */ private static bool QuadrilateralConvex(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4) { int s; s = Geometry.LinePointSide(x1, x2, x3) > 0 ? 1 : -1; s += Geometry.LinePointSide(x2, x3, x4) > 0 ? 1 : -1; s += Geometry.LinePointSide(x3, x4, x1) > 0 ? 1 : -1; s += Geometry.LinePointSide(x4, x1, x2) > 0 ? 1 : -1; return(Math.Abs(s) == 4); }
private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA) { o_matAtA.loadZero(); double[][] t = o_matAtA.getArray(); for (int i = 0; i < i_num; i++) { //0 double wx = worldCoord[i].x; double wy = worldCoord[i].y; double sx = screenCoord[i].x; double sy = screenCoord[i].y; double wxwx = wx * wx; double wywy = wy * wy; double wxwy = wx * wy; t[0][0] += wxwx; t[0][1] += wxwy; t[0][2] += wx; t[1][2] += wy; t[0][6] += (-wxwx) * (sx); t[0][7] += (-wxwy) * (sx); t[1][1] += wywy; t[1][7] += (-wywy) * (sx); t[2][6] += (-wx) * (sx); t[2][7] += (-wy) * (sx); t[3][6] += (-wxwx) * (sy); t[3][7] += (-wxwy) * (sy); t[4][7] += (-wywy) * (sy); t[5][6] += (-wx) * (sy); t[5][7] += (-wy) * (sy); t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy); t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy); t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy); } t[1][0] = t[3][4] = t[4][3] = t[0][1]; t[2][0] = t[3][5] = t[5][3] = t[0][2]; t[2][1] = t[5][4] = t[4][5] = t[1][2]; t[7][0] = t[6][1] = t[1][6] = t[0][7]; t[4][6] = t[6][4] = t[7][3] = t[3][7]; t[2][2] = t[5][5] = i_num; t[3][3] = t[0][0]; t[4][4] = t[1][1]; t[6][0] = t[0][6]; t[6][2] = t[2][6]; t[6][3] = t[3][6]; t[6][5] = t[5][6]; t[7][1] = t[1][7]; t[7][2] = t[2][7]; t[7][4] = t[4][7]; t[7][5] = t[5][7]; t[7][6] = t[6][7]; //先頭でゼロクリアしない場合。 //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0; return o_matAtA; }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @param i_x * カメラ座標系の点 * @param i_y * カメラ座標系の点 * @param i_z * カメラ座標系の点 * @param o_pos2d * 結果を受け取るオブジェクトです。 */ public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d) { NyARDoubleMatrix44 m = this._frustum_rh; double v3_1 = 1 / i_z * m.m32; double w = this._screen_size.w; double h = this._screen_size.h; o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2; o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2; return; }
/** * 頂点情報を元に、エラー閾値を計算します。 * @param i_vertex */ private double makeErrThreshold(NyARDoublePoint2d[] i_vertex) { double a, b, l1, l2; a = i_vertex[0].x - i_vertex[2].x; b = i_vertex[0].y - i_vertex[2].y; l1 = a * a + b * b; a = i_vertex[1].x - i_vertex[3].x; b = i_vertex[1].y - i_vertex[3].y; l2 = a * a + b * b; return (Math.Sqrt(l1 > l2 ? l1 : l2)) / 200; }
/** * Use this function to downsample a point to an octave that was found from * a bilinear downsampled pyramid. * * @param[out] xp Downsampled x location * @param[out] yp Downsampled y location * @param[in] x X location on fine image * @param[in] y Y location on fine image * @param[in] octave The octave to downsample (x,y) to */ public static void bilinear_downsample_point(NyARDoublePoint2d ret, double x, double y, int octave) { double a, b; a = 1.0f / (1 << octave); b = 0.5f * a - 0.5f; ret.x = x * a + b; ret.y = y * a + b; }
/** * 指定したパラメータの式との交点を得る。 * @param i_a * @param i_b * @param i_c * @param o_point * @return */ public bool crossPos(double i_a, double i_b, double i_c, NyARDoublePoint2d o_point) { double w1 = this.a * i_b - i_a * this.b; if (w1 == 0.0) { return(false); } o_point.x = (this.b * i_c - i_b * this.c) / w1; o_point.y = (i_a * this.c - this.a * i_c) / w1; return(true); }
/** * 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); }
/** * Check the geometric consistency between three correspondences. */ private static bool Homography3PointsGeometricallyConsistent(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x1p, NyARDoublePoint2d x2p, NyARDoublePoint2d x3p) { //TODO 1は常に+だからbが+であるかだけをチェックすればいい。 double a = ((x2.x - x1.x) * (x3.y - x1.y) - (x2.y - x1.y) * (x3.x - x1.x)); double b = ((x2p.x - x1p.x) * (x3p.y - x1p.y) - (x2p.y - x1p.y) * (x3p.x - x1p.x)); if ((a > 0) ^ (b > 0) == true) { return(false); } return(true); }
/** * 元ar2GenTemplate関数。 * 与えられた座標を中心に、テンプレート画像を生成する。 * 座標は観察座標点。 * @param i_x * @param i_y * @param i_scale * @param o_template * @return * @throws NyARException */ public void makeFromReferenceImage(double i_x, double i_y, NyARDoubleMatrix44 i_ref_ctrans, INyARCameraDistortionFactor i_ref_dist_factor, NyARNftIsetFile.ReferenceImage i_source) { int[] img = this.img; int img1_ptr = 0; int k = 0; int sum2 = 0; int sum = 0; NyARDoublePoint2d ideal = this.__in; for (int j = -(this.yts); j <= this.yts; j++) { for (int i = -(this.xts); i <= this.xts; i++) { i_ref_dist_factor.observ2Ideal(i_x + i * AR2_TEMP_SCALE, i_y + j * AR2_TEMP_SCALE, ideal); double ideal_x = ideal.x; double ideal_y = ideal.y; //ar2ScreenCoord2MarkerCoord(in.x,in.y,i_ref_ctrans,in);の展開 double c11 = i_ref_ctrans.m20 * ideal_x - i_ref_ctrans.m00; double c12 = i_ref_ctrans.m21 * ideal_x - i_ref_ctrans.m01; double c21 = i_ref_ctrans.m20 * ideal_y - i_ref_ctrans.m10; double c22 = i_ref_ctrans.m21 * ideal_y - i_ref_ctrans.m11; double b1 = i_ref_ctrans.m03 - i_ref_ctrans.m23 * ideal_x; double b2 = i_ref_ctrans.m13 - i_ref_ctrans.m23 * ideal_y; double m = c11 * c22 - c12 * c21; //public int ar2GetImageValue(double sx, double sy) throws NyARExceptionの展開 { int ix = (int)((((c22 * b1 - c12 * b2) / m) * i_source.dpi / 25.4f) + 0.5); int iy = (int)((i_source.height - (((c11 * b2 - c21 * b1) / m) * i_source.dpi) / 25.4f) + 0.5); //座標計算と値取得は分けよう。 if (ix < 0 || ix >= i_source.width || iy < 0 || iy >= i_source.height) { img[img1_ptr] = AR2_TEMPLATE_NULL_PIXEL; } else { int ret = img[img1_ptr] = i_source.img[iy * i_source.width + ix]; sum2 += ret * ret; sum += ret; k++; } //byte値はint化 } img1_ptr++; } } int vlen = sum2 - sum * sum / k; this.vlen = (int)Math.Sqrt((double)(vlen)); this.sum_of_img = sum; this.valid_pixels = k; return; }
public bool ar2GetVectorAngle(NyARDoublePoint2d p1, NyARDoublePoint2d p2) { double l = Math.Sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); if (l == 0.0f) { return(false); } this.sin = (p2.y - p1.y) / l; this.cos = (p2.x - p1.x) / l; return(true); }
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_vector1 * 交点を求める直線 * @param o_point * 交点座標を得るオブジェクト。 * @return * 交点が求まると、trueを返します。 */ public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point) { double a1 = i_vector1.dy; double b1 = -i_vector1.dx; double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x); double a2 = this.dy; double b2 = -this.dx; double c2 = (this.dx * this.y - this.dy * this.x); double w1 = a1 * b2 - a2 * b1; if (w1 == 0.0) { return false; } o_point.x = (b1 * c2 - b2 * c1) / w1; o_point.y = (a2 * c1 - a1 * c2) / w1; return true; }
public NyARObserv2IdealMap(NyARCameraDistortionFactor 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 ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size) { double x, y; double d0 = this._factor[0]; double d1 = this._factor[1]; double d3 = this._factor[3]; double d2_w = this._factor[2] / 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 = (long)(d0*NyMath.FIXEDFLOAT16_1); o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1); } else { double d = 1.0 - d2_w * (x * x + y * y); o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1); o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1); } } 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; }
/** * この関数は、ARToolKitのcheck_dir関数に相当します。 * 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整?) * @param i_start_vertex * 開始位置? * @param i_end_vertex * 終了位置? * @throws NyARException */ public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex) { double h; NyARDoubleMatrix44 inv_cpara = this._inv_cpara; //final double[] world = __checkVectorByVertex_world;// [2][3]; double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0;// mat_a->m[0]*st[0]*10.0+ double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0;// mat_a->m[3]*st[0]*10.0+ double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0;// mat_a->m[6]*st[0]*10.0+ double world3 = world0 + this.v1; double world4 = world1 + this.v2; double world5 = world2 + this.v3; // </Optimize> NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref; h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2; if (h == 0.0) { return false; } double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h; double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h; //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5; h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5; if (h == 0.0) { return false; } double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h; double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h; double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1); if (v < 0) { this.v1 = -this.v1; this.v2 = -this.v2; this.v3 = -this.v3; } return true; }
public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex) { Debug.Assert(i_number_of_vertex==4); double[] cx=this._cx; double[] cy=this._cy; double cpara02=this._projection_mat.m02; double cpara12=this._projection_mat.m12; NyARMat mat_t=this._mat_t; double[][] mata = this._mat_a.getArray(); double[][] matat= this._mat_at.getArray(); for (int i = 0; i < 4; i++){ cx[i]=i_ref_vertex_2d[i].x; cy[i]=i_ref_vertex_2d[i].y; int x2 = i * 2; mata[x2][2] = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0]; mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1]; } //T(3x3行列)の作成 mat_t.matrixMul(this._mat_at, this._mat_a); mat_t.matrixSelfInv(); return; }
/** * 画面上の座標群を指定します。 * @param i_ref_vertex_2d * 歪み矯正済の画面上の頂点座標群への参照値を指定します。 * @throws NyARException * */ 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); m22 = 0; w1 = w2 = 0; //[A]T*[A] 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 virtual void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex) { NyARRotVector vec1 = this.__initRot_vec1; NyARRotVector vec2 = this.__initRot_vec2; //向かい合った辺から、2本のベクトルを計算 //軸1 vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]); vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]); //軸2 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; }
/** * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数 * * @param i_in * @param o_out */ public void ideal2Observ(NyARDoublePoint2d i_in, NyARDoublePoint2d o_out) { double x = (i_in.x - this._f0) * this._f3; double y = (i_in.y - this._f1) * this._f3; if (x == 0.0 && y == 0.0) { o_out.x = this._f0; o_out.y = this._f1; } else { double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y); o_out.x = x * d + this._f0; o_out.y = y * d + this._f1; } return; }
/** * ideal2Observをまとめて実行します。 * @param i_in * @param o_out */ public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] 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 = d0; o_out[i].y = d1; } else { double d = 1.0 - d2_w * (x * x + y * y); o_out[i].x = x * d + d0; o_out[i].y = y * d + d1; } } return; }
/** * この関数は、遠近法のパラメータを計算して、返却します。 * @param i_size * 変換先の矩形のサイズを指定します。 * @param i_vertex * 変換元の頂点を指定します。要素数は4でなければなりません。 * @param o_param * 射影変換パラメータの出力インスタンスを指定します。要素数は8でなければなりません。 * @return * 成功するとtrueを返します。 * @ */ public bool getParam(NyARIntSize i_size, NyARDoublePoint2d[] i_vertex, double[] o_param) { 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); }
public override bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param) { Debug.Assert(num >= 4); double err0 = 0, err1; NyARIcpUtils.U u = this.__u; NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__E = new double[num]; this.__E2 = new double[num]; this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; double[] E = this.__E; double[] E2 = this.__E2; NyARDoublePoint2d[] du = this.__du; NyARDoubleMatrix44 matXw2U = this.__matXw2U; int inlierNum = (int)(num * this.getInlierProbability()) - 1; if (inlierNum < 3) { inlierNum = 3; } o_matxw2xc.setValue(initMatXw2Xc); for (int i = 0;; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); 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; du[j].x = dx; du[j].y = dy; E[j] = E2[j] = dx * dx + dy * dy; } Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE); double K2 = E2[inlierNum] * K2_FACTOR; if (K2 < 16.0) { K2 = 16.0; } err1 = 0.0; for (int j = 0; j < num; j++) { if (E2[j] > K2) { err1 += K2 / 6.0; } else { err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2)); } } err1 /= num; if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this._maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if (E[j] <= K2) { double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2); if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W)) { return false; } } } if (jus.getLength() < 3) { return false; } if (!dS.setJusArray(jus)) { return false; } dS.makeMat(o_matxw2xc); } if(o_result_param!=null){ o_result_param.last_error=err1; } return true; }
/** * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。 * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。 * 線分と直線の類似度を判定する数値になります。 * @param i_sp1 * 線分の端点1 * @param i_sp2 * 線分の端点2 * @return * 二乗距離値の合計。距離が取れないときは無限大です。 */ public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2) { double sa, sb, sc; sa = this.dy; sb = -this.dx; sc = (this.dx * this.y - this.dy * this.x); double lc; double x, y, w1; //thisを法線に変換 //交点を計算 w1 = sa * (-sa) - sb * sb; if (w1 == 0.0) { return Double.PositiveInfinity; } //i_sp1と、i_linerの交点 lc = -(sb * i_sp1.x - sa * i_sp1.y); x = ((sb * lc + sa * sc) / w1) - i_sp1.x; y = ((sb * sc - sa * lc) / w1) - i_sp1.y; double sqdist = x * x + y * y; lc = -(sb * i_sp2.x - sa * i_sp2.y); x = ((sb * lc + sa * sc) / w1) - i_sp2.x; y = ((sb * sc - sa * lc) / w1) - i_sp2.y; return sqdist + x * x + y * y; }
/** * この関数は、正規化したベクトルを出力する、{@link #leastSquares}です。 * @param i_points * 頂点群を格納した配列。 * @param i_number_of_data * 計算対象の頂点群の数 * @return * 計算に成功すると、trueを返します。 */ public bool leastSquaresWithNormalize(NyARDoublePoint2d[] i_points, int i_number_of_data) { bool ret = this.leastSquares(i_points, i_number_of_data); double sq = 1 / Math.Sqrt(this.dx * this.dx + this.dy * this.dy); this.dx *= sq; this.dy *= sq; return ret; }
/** * この関数は、頂点群から最小二乗法を使用して直線を計算します。 * @param i_points * 頂点群を格納した配列。 * @param i_number_of_data * 計算対象の頂点群の数 * @return * 計算に成功すると、trueを返します。 */ public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data) { int i; double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0; for (i = 0; i < i_number_of_data; i++) { NyARDoublePoint2d ptr = i_points[i]; double xw = ptr.x; sum_xy += xw * ptr.y; sum_x += xw; sum_y += ptr.y; sum_x2 += xw * xw; } double la = -(i_number_of_data * sum_x2 - sum_x * sum_x); double lb = -(i_number_of_data * sum_xy - sum_x * sum_y); double cc = (sum_x2 * sum_y - sum_xy * sum_x); double lc = -(la * sum_x + lb * sum_y) / i_number_of_data; //交点を計算 double w1 = -lb * lb - la * la; if (w1 == 0.0) { return false; } this.x = ((la * lc - lb * cc) / w1); this.y = ((la * cc + lb * lc) / w1); this.dy = -lb; this.dx = -la; return true; }
/** * この関数は、この直線と線分が作るCos値を返します。 * @param i_pos1 * 線分の端点1 * @param i_pos2 * 線分の端点2 * @return * 2直線のCOS値(radian) */ public double getVecCos(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2) { return getVecCos(i_pos2.x - i_pos1.x, i_pos2.y - i_pos1.y); }
/** * この関数は、遠近法のパラメータを計算して、返却します。 * @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, NyARDoublePoint2d[] 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); }
/** * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy ); * * @param ox * @param oy * @param ix * @param iy * @return */ public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point) { double z02, z0, p, q, z, px, py, opttmp_1; double d0 = this._f0; double d1 = this._f1; px = ix - d0; py = iy - d1; p = this._f2 / 100000000.0; z02 = px * px + py * py; q = z0 = Math.Sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py); for (int i = 1; ; i++) { if (z0 != 0.0) { // Optimize opttmp_1 opttmp_1 = p * z02; z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1); px = px * z / z0; py = py * z / z0; } else { px = 0.0; py = 0.0; break; } if (i == PD_LOOP) { break; } z02 = px * px + py * py; z0 = Math.Sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py); } o_point.x = px / this._f3 + d0; o_point.y = py / this._f3 + d1; return; }
/** * クリッピング付きのライントレーサです。 * * @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ならクリッピングして得られた2点を使う。 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; }