/** * この関数は、点が矩形の範囲内にあるか判定します。 * @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); }
/** * この関数は、頂点群から最小二乗法を使用して直線を計算します。 * @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); }
/** * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。 * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。 * 線分と直線の類似度を判定する数値になります。 * @param i_sp1 * 線分の端点1 * @param i_sp2 * 線分の端点2 * @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); }
/** * この関数は、この直線と、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); }
/** * 座標値を射影変換します。 * @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; }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @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 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); }
/** * この関数は、直線の交点を計算します。 * @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, 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); }
/** * この関数は、座標を観察座標系から理想座標系へ変換します。 * @param ix * 変換元の座標 * @param iy * 変換元の座標 * @param o_point * 変換後の座標を受け取るオブジェクト * @todo should optimize! */ 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; }
/** * この関数は、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); }
/** * この関数は、座標点を理想座標系から観察座標系へ変換します。 * @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; }
/** * この関数は、直線との交点を求めます。 * @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); }
/** * この関数は、座標点を理想座標系から観察座標系へ変換します。 * @param i_in * 変換元の座標 * @param o_out * 変換後の座標を受け取るオブジェクト */ 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; }
public void icpGetInitXw2XcSub(NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] ppos3d, int num, NyARDoubleMatrix44 conv) { NyARDoublePoint3d off = makeOffset(ppos3d, num, this.__off); NyARDoubleMatrix33 matd = makeMatD(this._cparam, pos2d, num, this.__matd); double[] mate = makeMatE(this._cparam, rot, pos2d, ppos3d, off, num, this.__mate); conv.setValue(rot); conv.m03 = matd.m00 * mate[0] + matd.m01 * mate[1] + matd.m02 * mate[2]; conv.m13 = matd.m10 * mate[0] + matd.m11 * mate[1] + matd.m12 * mate[2]; conv.m23 = matd.m20 * mate[0] + matd.m21 * mate[1] + matd.m22 * mate[2]; conv.m03 = conv.m00 * off.x + conv.m01 * off.y + conv.m02 * off.z + conv.m03; conv.m13 = conv.m10 * off.x + conv.m11 * off.y + conv.m12 * off.z + conv.m13; conv.m23 = conv.m20 * off.x + conv.m21 * off.y + conv.m22 * off.z + conv.m23; 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); }
/** * この関数は、頂点群から最小二乗法を使用して直線を計算します。 * @param i_points * 頂点群を格納した配列。 * @param i_number_of_data * 計算対象の頂点群の数 * @return * 計算に成功すると、trueを返します。 */ public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data) { Debug.Assert(i_number_of_data > 1); 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; } this.b = -(i_number_of_data * sum_x2 - sum_x * sum_x); this.a = (i_number_of_data * sum_xy - sum_x * sum_y); this.c = (sum_x2 * sum_y - sum_xy * sum_x); return(true); }
/** * この関数は、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; }
/** * この関数は、座標を観察座標系から理想座標系へ変換します。 * @param ix * 変換元の座標 * @param iy * 変換元の座標 * @param o_point * 変換後の座標を受け取るオブジェクト */ 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; }
/** * この関数は、この直線と、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; }
/** * この関数は、頂点群から最小二乗法を使用して直線を計算します。 * @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; }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @param i_pos * カメラ座標系の点 * @param o_pos2d * 結果を受け取るオブジェクトです。 */ public void project(NyARDoublePoint3d i_pos, NyARDoublePoint2d o_pos2d) { this.project(i_pos.x, i_pos.y, i_pos.z, o_pos2d); }
/** * 対象矩形の頂点配列をコピーして返します。 * 樽型歪みの逆矯正は行いません。 * @param o_vertex */ public void getTargetVertex(NyARDoublePoint2d[] o_vertex) { Debug.Assert(this._target_type == RT_UNKNOWN || this._target_type == RT_KNOWN); NyARDoublePoint2d[] v=((NyARRectTargetStatus)(this._ref_tracktarget._ref_status)).vertex; for(int i=3;i>=0;i--){ o_vertex[i].setValue(v[i]); } }
public bool icpGetInitXw2Xc_from_PlanarData( NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARDoubleMatrix44 initMatXw2Xc) { if (i_num < 4) { throw new NyARException(); } // nを元に配列の準備 NyARMat matAtA = this.__matAtA; NyARMat matAtB = this.__matAtB; NyARMat matC = this.__matC; makeMatAtA(screenCoord, worldCoord, i_num, matAtA); makeMatAtB(screenCoord, worldCoord, i_num, matAtB); if (!matAtA.inverse()) { return false; } matC.mul(matAtA, matAtB); double[][] bufc = matC.getArray(); double t0, t1, t2; NyARRotVector vec0 = this.__vec0; NyARRotVector vec1 = this.__vec1; NyARDoubleMatrix44 matxc = this._cparam; vec0.v3 = (bufc[6][0]); vec0.v2 = (bufc[3][0] - matxc.m12 * vec0.v3) / matxc.m11; vec0.v1 = (bufc[0][0] - matxc.m02 * vec0.v3 - matxc.m01 * vec0.v2) / matxc.m00; vec1.v3 = (bufc[7][0]); vec1.v2 = (bufc[4][0] - matxc.m12 * vec1.v3) / matxc.m11; vec1.v1 = (bufc[1][0] - matxc.m02 * vec1.v3 - matxc.m01 * vec1.v2) / matxc.m00; t2 = 1.0; t1 = (bufc[5][0] - matxc.m12 * t2) / matxc.m11; t0 = (bufc[2][0] - matxc.m02 * t2 - matxc.m01 * t1) / matxc.m00; double l1 = Math.Sqrt(vec0.v1 * vec0.v1 + vec0.v2 * vec0.v2 + vec0.v3 * vec0.v3); double l2 = Math.Sqrt(vec1.v1 * vec1.v1 + vec1.v2 * vec1.v2 + vec1.v3 * vec1.v3); vec0.v1 /= l1; vec0.v2 /= l1; vec0.v3 /= l1; vec1.v1 /= l2; vec1.v2 /= l2; vec1.v3 /= l2; t0 /= (l1 + l2) / 2.0; t1 /= (l1 + l2) / 2.0; t2 /= (l1 + l2) / 2.0; if (t2 < 0.0) { vec0.v1 = -vec0.v1; vec0.v2 = -vec0.v2; vec0.v3 = -vec0.v3; vec1.v1 = -vec1.v1; vec1.v2 = -vec1.v2; vec1.v3 = -vec1.v3; t0 = -t0; t1 = -t1; t1 = -t2; } // ここまで if (!NyARRotVector.checkRotation(vec0, vec1)) { return false; } double v20 = vec0.v2 * vec1.v3 - vec0.v3 * vec1.v2; double v21 = vec0.v3 * vec1.v1 - vec0.v1 * vec1.v3; double v22 = vec0.v1 * vec1.v2 - vec0.v2 * vec1.v1; l1 = Math.Sqrt(v20 * v20 + v21 * v21 + v22 * v22); v20 /= l1; v21 /= l1; v22 /= l1; initMatXw2Xc.m00 = vec0.v1; initMatXw2Xc.m10 = vec0.v2; initMatXw2Xc.m20 = vec0.v3; initMatXw2Xc.m01 = vec1.v1; initMatXw2Xc.m11 = vec1.v2; initMatXw2Xc.m21 = vec1.v3; initMatXw2Xc.m02 = v20; initMatXw2Xc.m12 = v21; initMatXw2Xc.m22 = v22; initMatXw2Xc.m03 = t0; initMatXw2Xc.m13 = t1; initMatXw2Xc.m23 = t2; initMatXw2Xc.m30 = initMatXw2Xc.m31 = initMatXw2Xc.m32 = 0; initMatXw2Xc.m33 = 1; icpGetInitXw2XcSub(initMatXw2Xc, screenCoord, worldCoord, i_num, initMatXw2Xc); return true; }
/** * {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。 */ private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val) { double v0 = 0; double v1 = 0; double v2 = 0; for (int j = 0; j < i_num; j++) { double p3x = pos3d[j].x + offset.x; double p3y = pos3d[j].y + offset.y; double p3z = pos3d[j].z + offset.z; double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z; double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z; double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z; double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz; double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz; v0 += i_cp.m00 * c1; v1 += i_cp.m01 * c1 + i_cp.m11 * c2; v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2; } o_val[0] = v0; o_val[1] = v1; o_val[2] = v2; return o_val; }
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; }
//override public override sealed bool initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex) { bool ret = base.initRotBySquare(i_linear, i_sqvertex); if (ret) { //Matrixからangleをロード this.updateAngleFromMatrix(); } return ret; }
/** * 頂点を左回転して、矩形を回転させます。 * @param i_shift */ private static void rotateVertexL(NyARDoublePoint2d[] i_vertex,int i_shift) { Debug.Assert(i_shift<4); NyARDoublePoint2d vertext; if(i_shift==0){ return; } int t1,t2; int d, i, j, mk; int ll=4-i_shift; d = _gcd_table4[ll];//NyMath.gcn(4,ll); mk = (4-ll) % 4; for (i = 0; i < d; i++) { vertext=i_vertex[i]; for (j = 1; j < 4/d; j++) { t1=(i + (j-1)*mk) % 4; t2=(i + j*mk) % 4; i_vertex[t1]=i_vertex[t2]; } t1=(i + ll) % 4; i_vertex[t1]=vertext; } }
/** * 頂点同士の距離から、頂点のシフト量を返します。この関数は、よく似た2つの矩形の頂点同士の対応を取るために使用します。 * @param i_square * 比較対象の矩形 * @return * シフト量を数値で返します。 * シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。 */ private static int checkVertexShiftValue(NyARDoublePoint2d[] i_vertex1,NyARDoublePoint2d[] i_vertex2) { Debug.Assert(i_vertex1.Length==4 && i_vertex2.Length==4); //3-0番目 int min_dist=int.MaxValue; int min_index=0; int xd,yd; for(int i=3;i>=0;i--){ int d=0; for(int i2=3;i2>=0;i2--){ xd= (int)(i_vertex1[i2].x-i_vertex2[(i2+i)%4].x); yd= (int)(i_vertex1[i2].y-i_vertex2[(i2+i)%4].y); d+=xd*xd+yd*yd; } if(min_dist>d){ min_dist=d; min_index=i; } } return min_index; }
/** * * @param iw_rotmat * @param iw_transvec * @param i_solver * @param i_offset_3d * @param i_2d_vertex * @param i_err_threshold * @param o_result * @return * @ */ private double optimize(NyARRotMatrix iw_rotmat,NyARDoublePoint3d iw_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex,double i_err_threshold,NyARDoubleMatrix44 o_result) { //System.out.println("START"); NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; //初期のエラー値を計算 double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d); o_result.setValue(iw_rotmat, iw_transvec); for (int i = 0; i < 5; i++) { //変換行列の最適化 this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4); double err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d); //System.out.println("E:"+err); if (min_err - err < i_err_threshold) { //System.out.println("BREAK"); break; } i_solver.solveTransportVector(vertex_3d, iw_transvec); o_result.setValue(iw_rotmat, iw_transvec); min_err = err; } //System.out.println("END"); return min_err; }
/** * 頂点情報を元に、エラー閾値を計算します。 * @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; }
/** * この関数は、姿勢行列のエラーレートを計算します。 * エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。 * 通常、ユーザが使うことはありません。 * @param i_rot * 回転行列 * @param i_trans * 平行移動量 * @param i_vertex3d * オフセット位置 * @param i_vertex2d * 理想座標 * @param i_number_of_vertex * 評価する頂点数 * @param o_rot_vertex * 計算過程で得られた、各頂点の三次元座標 * @return * エラーレート(Σ(理想座標と計算座標の距離[n]^2)) * @ */ public double errRate(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d[] o_rot_vertex) { NyARPerspectiveProjectionMatrix cp = this._ref_projection_mat; double cp00 = cp.m00; double cp01 = cp.m01; double cp02 = cp.m02; double cp11 = cp.m11; double cp12 = cp.m12; double err = 0; for (int i = 0; i < i_number_of_vertex; i++) { double x3d, y3d, z3d; o_rot_vertex[i].x = x3d = i_rot.m00 * i_vertex3d[i].x + i_rot.m01 * i_vertex3d[i].y + i_rot.m02 * i_vertex3d[i].z; o_rot_vertex[i].y = y3d = i_rot.m10 * i_vertex3d[i].x + i_rot.m11 * i_vertex3d[i].y + i_rot.m12 * i_vertex3d[i].z; o_rot_vertex[i].z = z3d = i_rot.m20 * i_vertex3d[i].x + i_rot.m21 * i_vertex3d[i].y + i_rot.m22 * i_vertex3d[i].z; x3d += i_trans.x; y3d += i_trans.y; z3d += i_trans.z; //射影変換 double x2d = x3d * cp00 + y3d * cp01 + z3d * cp02; double y2d = y3d * cp11 + z3d * cp12; double h2d = z3d; //エラーレート計算 double t1 = i_vertex2d[i].x - x2d / h2d; double t2 = i_vertex2d[i].y - y2d / h2d; err += t1 * t1 + t2 * t2; } return err / i_number_of_vertex; }
/** * この関数は、マーカ四角形をインスタンスにセットします。 * @param i_vertex * セットする四角形頂点座標。4要素である必要があります。 * @return * 成功するとtrueです。 * @ */ public bool setSourceSquare(NyARDoublePoint2d[] i_vertex) { return this._param_gen.getParam(READ_RESOLUTION, READ_RESOLUTION, i_vertex, this._cparam); }
/** * {@link #observ2Ideal(double, double, NyARDoublePoint2d)}のラッパーです。 */ public void observ2Ideal(NyARDoublePoint2d i_in, NyARDoublePoint2d o_point) { this.observ2Ideal(i_in.x, i_in.y, o_point); }
/** * {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。 */ private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat) { double m02 = 0; double m12 = 0; double m20 = 0; double m21 = 0; double m22 = 0; for (int i = 0; i < i_num; i++) { double cx = i_cp.m02 - pos2d[i].x; double cy = i_cp.m12 - pos2d[i].y; m02 += (cx) * i_cp.m00; m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11; m20 += i_cp.m00 * (cx); m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy); m22 += (cx) * (cx) + (cy) * (cy); } o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num; o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num; o_mat.m02 = m02; o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num; o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num; o_mat.m12 = m12; o_mat.m20 = m20; o_mat.m21 = m21; o_mat.m22 = m22; o_mat.inverse(o_mat); return o_mat; }
/** * この関数は、座標点を理想座標系から観察座標系へ変換します。 * @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; }
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); }
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; }
/** * 対象矩形の中央点を返します。 * 樽型歪みの逆矯正は行いません。 * @param o_center */ public void getTargetCenter(NyARDoublePoint2d 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); }
private double getSizeFactor(double x0, double y0, int xsize, int ysize) { double ox, oy; double olen, ilen; double sf1; double sf = 100.0; ox = 0.0; oy = y0; olen = x0; NyARDoublePoint2d itmp = new NyARDoublePoint2d(); this.observ2Ideal(ox, oy, 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; } } ox = xsize; oy = y0; olen = xsize - x0; this.observ2Ideal(ox, oy, 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; } } ox = x0; oy = 0.0; olen = y0; this.observ2Ideal(ox, oy, 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; } } ox = x0; oy = ysize; olen = ysize - y0; this.observ2Ideal(ox, oy, 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; } } ox = 0.0; oy = 0.0; this.observ2Ideal(ox, oy, 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; } } ox = xsize; oy = 0.0; this.observ2Ideal(ox, oy, 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; } } ox = 0.0; oy = ysize; this.observ2Ideal(ox, oy, 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; } } ox = xsize; oy = ysize; this.observ2Ideal(ox, oy, 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); }
/** * この関数は、この直線と線分が作る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 J_U_S * double[2][6] * @return */ public bool push(NyARDoubleMatrix44 matXc2U, NyARDoubleMatrix44 matXw2Xc, NyARDoublePoint3d worldCoord, NyARDoublePoint2d du, double W) { double[][] J_Xc_S = this.__J_Xc_S; J_U_Xc juxc = this.__juxc; double wx = worldCoord.x; double wy = worldCoord.y; double wz = worldCoord.z; //icpGetJ_Xc_S1 if (!juxc.setXc2UXc( matXc2U, matXw2Xc.m00 * wx + matXw2Xc.m01 * wy + matXw2Xc.m02 * wz + matXw2Xc.m03, matXw2Xc.m10 * wx + matXw2Xc.m11 * wy + matXw2Xc.m12 * wz + matXw2Xc.m13, matXw2Xc.m20 * wx + matXw2Xc.m21 * wy + matXw2Xc.m22 * wz + matXw2Xc.m23)) { return(false); } //icpGetJ_Xc_S2 J_Xc_S[0][0] = (-matXw2Xc.m01 * wz) + (matXw2Xc.m02 * wy); J_Xc_S[0][1] = (matXw2Xc.m00 * wz) + (-matXw2Xc.m02 * wx); J_Xc_S[0][2] = (-matXw2Xc.m00 * wy) + (matXw2Xc.m01 * wx); J_Xc_S[0][3] = (matXw2Xc.m00); J_Xc_S[0][4] = (matXw2Xc.m01); J_Xc_S[0][5] = (matXw2Xc.m02); J_Xc_S[1][0] = (-matXw2Xc.m11 * wz) + (matXw2Xc.m12 * wy); J_Xc_S[1][1] = (matXw2Xc.m10 * wz) + (-matXw2Xc.m12 * wx); J_Xc_S[1][2] = (-matXw2Xc.m10 * wy) + (matXw2Xc.m11 * wx); J_Xc_S[1][3] = (matXw2Xc.m10); J_Xc_S[1][4] = (matXw2Xc.m11); J_Xc_S[1][5] = (matXw2Xc.m12); J_Xc_S[2][0] = (-matXw2Xc.m21 * wz) + (matXw2Xc.m22 * wy); J_Xc_S[2][1] = (matXw2Xc.m20 * wz) + (-matXw2Xc.m22 * wx); J_Xc_S[2][2] = (-matXw2Xc.m20 * wy) + (matXw2Xc.m21 * wx); J_Xc_S[2][3] = (matXw2Xc.m20); J_Xc_S[2][4] = (matXw2Xc.m21); J_Xc_S[2][5] = (matXw2Xc.m22); Item item = this.prePush(); if (item == null) { return(false); } item.m00 = ((juxc.m00 * J_Xc_S[0][0]) + (juxc.m01 * J_Xc_S[1][0]) + (juxc.m02 * J_Xc_S[2][0])) * W; item.m01 = ((juxc.m00 * J_Xc_S[0][1]) + (juxc.m01 * J_Xc_S[1][1]) + (juxc.m02 * J_Xc_S[2][1])) * W; item.m02 = ((juxc.m00 * J_Xc_S[0][2]) + (juxc.m01 * J_Xc_S[1][2]) + (juxc.m02 * J_Xc_S[2][2])) * W; item.m03 = ((juxc.m00 * J_Xc_S[0][3]) + (juxc.m01 * J_Xc_S[1][3]) + (juxc.m02 * J_Xc_S[2][3])) * W; item.m04 = ((juxc.m00 * J_Xc_S[0][4]) + (juxc.m01 * J_Xc_S[1][4]) + (juxc.m02 * J_Xc_S[2][4])) * W; item.m05 = ((juxc.m00 * J_Xc_S[0][5]) + (juxc.m01 * J_Xc_S[1][5]) + (juxc.m02 * J_Xc_S[2][5])) * W; item.m10 = ((juxc.m10 * J_Xc_S[0][0]) + (juxc.m11 * J_Xc_S[1][0]) + (juxc.m12 * J_Xc_S[2][0])) * W; item.m11 = ((juxc.m10 * J_Xc_S[0][1]) + (juxc.m11 * J_Xc_S[1][1]) + (juxc.m12 * J_Xc_S[2][1])) * W; item.m12 = ((juxc.m10 * J_Xc_S[0][2]) + (juxc.m11 * J_Xc_S[1][2]) + (juxc.m12 * J_Xc_S[2][2])) * W; item.m13 = ((juxc.m10 * J_Xc_S[0][3]) + (juxc.m11 * J_Xc_S[1][3]) + (juxc.m12 * J_Xc_S[2][3])) * W; item.m14 = ((juxc.m10 * J_Xc_S[0][4]) + (juxc.m11 * J_Xc_S[1][4]) + (juxc.m12 * J_Xc_S[2][4])) * W; item.m15 = ((juxc.m10 * J_Xc_S[0][5]) + (juxc.m11 * J_Xc_S[1][5]) + (juxc.m12 * J_Xc_S[2][5])) * W; item.ux = du.x * W; item.uy = du.y * W; return(true); }
/** * この関数は、正規化したベクトルを出力する、{@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; }
/** * この関数は、この直線と線分が作るCos値の絶対値を返します。 * @param i_pos1 * 線分の端点1 * @param i_pos2 * 線分の端点2 * @return * 2直線のCOS値の絶対値(radian) */ public double getAbsVecCos(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2) { return(getAbsVecCos(i_pos2.x - i_pos1.x, i_pos2.y - i_pos1.y)); }
/** * この関数は、直線との交点を求めます。 * @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; }
/** * 適当に与えられた4線分から、四角形の頂点を計算する。 * @param i_line * 4線分を格納した配列 * @param o_point * 検出した4頂点 * @return * 四角形を検出したらtrue * @throws NyARException */ public bool line2SquareVertex(VecLinearCoordinates.VecLinearCoordinatePoint[] i_line, NyARDoublePoint2d[] o_point) { NyARDoublePoint2d[] v = this.__wk_v; int number_of_vertex = 0; int non_vertexid = 0; int ptr = 0; for (int i = 0; i < 3; i++) { for (int i2 = i + 1; i2 < 4; i2++) { if (i_line[i].crossPos(i_line[i2], v[ptr])) { number_of_vertex++; } else { non_vertexid = ptr; } ptr++; } } int num_of_plus = -1; int[] target_order; switch (number_of_vertex) { case 4: case 5: //正の外積の数を得る。0,4ならば、目的の図形 num_of_plus = countPlusExteriorProduct(v, _45vertextable[non_vertexid]); target_order = _45vertextable[non_vertexid]; break; case 6: //(0-5),(1-4),(2-3)の頂点ペアの組合せを試す。頂点の検索順は、(0,1,5,4),(0,2,5,3),(1,2,4,3) //3パターンについて、正の外積の数を得る。0,4のものがあればOK int order_id = -1; num_of_plus = -1; for (int i = 0; i < 3; i++) { num_of_plus = countPlusExteriorProduct(v, _order_table[i]); if (num_of_plus % 4 == 0) { order_id = i; break; } } if (order_id == -1) { return false; } target_order = _order_table[order_id]; break; default: //他の頂点数の時はNG return false; } //回転方向の正規化(ここパラメータ化しようよ) switch (num_of_plus) { case 0: //逆回転で検出した場合 for (int i = 0; i < 4; i++) { o_point[i].setValue(v[target_order[3 - i]]); } break; case 4: //正回転で検出した場合 for (int i = 0; i < 4; i++) { o_point[i].setValue(v[target_order[i]]); } break; default: return false; } return true; }
/** * この関数は、ラスタドライバから画像を読み出します。 * @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); }
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; }
/** * この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。 * 式の係数値は、正規化されます。 * @param i_point1 * 点1 * @param i_point2 * 点2 * @return * 直線式が求るとtrueを返します。 */ public bool makeLinearWithNormalize(NyARDoublePoint2d i_point1, NyARDoublePoint2d i_point2) { return(makeLinearWithNormalize(i_point1.x, i_point1.y, i_point2.x, i_point2.y)); }
/** * 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; }
/** * 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; }
/** * この関数は、矩形の中心点を計算します。 * @param o_out * 結果を格納するバッファ。 */ 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; }