public void transformVertex(double i_x, double i_y, double i_z, NyARDoublePoint3d o_out) { o_out.x = this.m00 * i_x + this.m01 * i_y + this.m02 * i_z + this.m03; o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z + this.m13; o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z + this.m23; return; }
/** * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] ) * * @param i_square * 計算対象のNyARSquareオブジェクト * @param i_direction * @param i_width * @return * @throws NyARException */ public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv) { NyARDoublePoint3d trans = this.__transMat_trans; double err_threshold = makeErrThreshold(i_square.sqvertex); //平行移動量計算機に、2D座標系をセット NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d; NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); this._transsolver.set2dVertex(vertex_2d, 4); //回転行列を計算 this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex); //回転後の3D座標系から、平行移動量を計算 this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold); // マトリクスの保存 this.updateMatrixValue(this._rotmatrix, trans, o_result_conv); return; }
/** * この関数は、3次元座標を座標変換します。 * 4列目は1と仮定します。 * @param i_x * 変換する三次元座標(X) * @param i_y * 変換する三次元座標(Y) * @param i_z * 変換する三次元座標(Z) * @param o_out * 変換後の座標を受け取るオブジェクト */ public void transform3d(double i_x, double i_y, double i_z, NyARDoublePoint3d o_out) { o_out.x = this.m00 * i_x + this.m01 * i_y + this.m02 * i_z + this.m03; o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z + this.m13; o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z + this.m23; return; }
/** * * @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); }
/** * 画面座標群と3次元座標群から、平行移動量を計算します。 * 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。 */ public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer) { double[][] matc = this._mat_c.getArray(); double cpara00 = this._projection_mat.m00; double cpara01 = this._projection_mat.m01; double cpara02 = this._projection_mat.m02; double cpara11 = this._projection_mat.m11; double cpara12 = this._projection_mat.m12; double[] cx = this._cx; double[] cy = this._cy; //(3D座標?)を一括請求 for (int i = 0; i < 4; i++) { int x2 = i + i; NyARDoublePoint3d point3d_ptr = i_vertex3d[i]; //透視変換? matc[x2][0] = point3d_ptr.z * cx[i] - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z; // mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz; matc[x2 + 1][0] = point3d_ptr.z * cy[i] - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z; // mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz; } this._mat_e.mul(this._mat_at, this._mat_c); this._mat_f.mul(this._mat_t, this._mat_e); double[][] matf = this._mat_f.getArray(); o_transfer.x = matf[0][0]; // trans[0] = mat_f->m[0]; o_transfer.y = matf[1][0]; o_transfer.z = matf[2][0]; // trans[2] = mat_f->m[2]; return; }
private void rotation2Sincos_ZXY(NyARDoubleMatrix33 i_rot_matrix, TSinCosValue[] o_out, NyARDoublePoint3d o_ang) { double x, y, z; double sina = i_rot_matrix.m21; if (sina >= 1.0) { x = Math.PI / 2; y = 0; z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00); } else if (sina <= -1.0) { x = -Math.PI / 2; y = 0; z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00); } else { x = Math.Asin(sina); y = Math.Atan2(-i_rot_matrix.m20, i_rot_matrix.m22); z = Math.Atan2(-i_rot_matrix.m01, i_rot_matrix.m11); } o_ang.x = x; o_ang.y = y; o_ang.z = z; o_out[0].sin_val = Math.Sin(x); o_out[0].cos_val = Math.Cos(x); o_out[1].sin_val = Math.Sin(y); o_out[1].cos_val = Math.Cos(y); o_out[2].sin_val = Math.Sin(z); o_out[2].cos_val = Math.Cos(z); 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; }
public void setValue(NyARDoublePoint3d i_in) { this.x = i_in.x; this.y = i_in.y; this.z = i_in.z; return; }
public void setValue(NyARDoublePoint3d i_in) { this.x=i_in.x; this.y=i_in.y; this.z=i_in.z; return; }
/** * この関数は、姿勢行列のエラーレートを計算します。 * エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。 * 通常、ユーザが使うことはありません。 * @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); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * ARToolKitのarGetTransMatに該当します。 * @see INyARTransMat#transMatContinue */ public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv) { NyARDoublePoint3d trans = this.__transMat_trans; double err_threshold = makeErrThreshold(i_square.sqvertex); NyARDoublePoint2d[] vertex_2d; if (this._ref_dist_factor != null) { //歪み復元必要 vertex_2d = this.__transMat_vertex_2d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); } else { //歪み復元は不要 vertex_2d = i_square.sqvertex; } //平行移動量計算機に、2D座標系をセット this._transsolver.set2dVertex(vertex_2d, 4); //回転行列を計算 this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex); //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv); return(true); }
public void getMarkerPlanePos(int id,int i_x,int i_y,ref Vector3 i_out) { NyARDoublePoint3d p=new NyARDoublePoint3d(); this.getMarkerPlanePos(id,i_x,i_y,p); i_out.x=-(float)p.x; i_out.y=(float)p.y; i_out.z=(float)p.z; }
/** * p2-p1ベクトルのsquare normを計算する。 * @param i_p1 * @param i_p2 * @return */ public static double sqNorm(NyARDoublePoint3d i_p1, NyARDoublePoint3d i_p2) { double x, y, z; x = i_p2.x - i_p1.x; y = i_p2.y - i_p1.y; z = i_p2.z - i_p1.z; return x * x + y * y + z * z; }
/** * i_pointとのベクトルから距離を計算します。 * @return */ public double dist(NyARDoublePoint3d i_point) { double x, y, z; x = this.x - i_point.x; y = this.y - i_point.y; z = this.z - i_point.z; return Math.Sqrt(x * x + y * y + z * z); }
/** * 座標値を射影変換します。 * @param i_3dvertex * 変換元の座標値 * @param o_2d * 変換後の座標値を受け取るオブジェクト */ public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d) { double w = 1 / (i_3dvertex.z * this.m22); o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w); o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w); return; }
/** * 現在の行列で3次元座標を射影変換します。 * @param i_3dvertex * @param o_2d */ public void projectionConvert(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d) { double w = 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_number * @return */ public static NyARDoublePoint3d[] createArray(int i_number) { NyARDoublePoint3d[] ret = new NyARDoublePoint3d[i_number]; for (int i = 0; i < i_number; i++) { ret[i] = new NyARDoublePoint3d(); } return ret; }
/** * 配列ファクトリ * @param i_number * @return */ public static NyARDoublePoint3d[] createArray(int i_number) { NyARDoublePoint3d[] ret = new NyARDoublePoint3d[i_number]; for (int i = 0; i < i_number; i++) { ret[i] = new NyARDoublePoint3d(); } return(ret); }
/** * i_pointとのベクトルから距離を計算します。 * @return */ public double dist(NyARDoublePoint3d i_point) { double x, y, z; x = this.x - i_point.x; y = this.y - i_point.y; z = this.z - i_point.z; return(Math.Sqrt(x * x + y * y + z * z)); }
/** * p2-p1ベクトルのsquare normを計算する。 * @param i_p1 * @param i_p2 * @return */ public static double sqNorm(NyARDoublePoint3d i_p1, NyARDoublePoint3d i_p2) { double x, y, z; x = i_p2.x - i_p1.x; y = i_p2.y - i_p1.y; z = i_p2.z - i_p1.z; return(x * x + y * y + z * z); }
/** * p2-p1間の距離の二乗値を計算します。 * @param i_p1 * @return */ public double sqDist(NyARDoublePoint3d i_p1) { double x, y, z; x = this.x - i_p1.x; y = this.y - i_p1.y; z = this.z - i_p1.z; return(x * x + y * y + z * z); }
/** * この関数は、入力した3次元頂点を回転して返します。 * @param i_in_point * 回転する三次元座標 * @param i_out_point * 回転した三次元座標 */ public void getPoint3d(NyARDoublePoint3d i_in_point, NyARDoublePoint3d i_out_point) { double x = i_in_point.x; double y = i_in_point.y; double z = i_in_point.z; i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z; i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z; i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z; return; }
/** * この関数は、回転行列を最適化します。 * i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した2次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。 * @param io_rot * 調整する回転行列 * @param i_trans * 平行移動量 * @param i_vertex3d * 三次元オフセット座標 * @param i_vertex2d * 理想座標系の頂点座標 * @param i_number_of_vertex * 頂点数 * @ */ public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) { NyARDoublePoint3d ang = this.__ang; // ZXY系のsin/cos値を抽出 io_rot.getZXYAngle(ang); modifyMatrix(ang, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang); io_rot.setZXYAngle(ang.x, ang.y, ang.z); return; }
/* * 射影変換式 基本式 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); }
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; }
/** * i_in_pointを変換行列で座標変換する。 * @param i_in_point * @param i_out_point */ public void getPoint3d(NyARDoublePoint3d i_in_point, NyARDoublePoint3d i_out_point) { double x = i_in_point.x; double y = i_in_point.y; double z = i_in_point.z; i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z; i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z; i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z; return; }
/** * 先にセットした2次元座標群と3次元座標群から、平行移動量を計算します。 * 2d座標系は、直前に実行したset2dVertexのものを使用します。 * @param i_vertex3d * 3次元空間の座標群を設定します。頂点の順番は、画面座標群と同じ順序で格納してください。 * @param o_transfer * @throws NyARException */ public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer) { int number_of_vertex = this._nmber_of_vertex; double p00 = this._projection_mat.m00; double p01 = this._projection_mat.m01; double p02 = this._projection_mat.m02; double p11 = this._projection_mat.m11; double p12 = this._projection_mat.m12; //行列[A]の3列目のキャッシュ double[] cx = this._cx; double[] cy = this._cy; //回転行列を元座標の頂点群に適応 //[A]T*[b]を計算 double b1 = 0, b2 = 0, b3 = 0; for (int i = 0; i < number_of_vertex; i++) { double w1 = i_vertex3d[i].z * cx[i] - p00 * i_vertex3d[i].x - p01 * i_vertex3d[i].y - p02 * i_vertex3d[i].z; double w2 = i_vertex3d[i].z * cy[i] - p11 * i_vertex3d[i].y - p12 * i_vertex3d[i].z; b1 += w1; b2 += w2; b3 += cx[i] * w1 + cy[i] * w2; } //[A]T*[b]を計算 b3 = p02 * b1 + p12 * b2 - b3;//順番変えたらダメよ b2 = p01 * b1 + p11 * b2; b1 = p00 * b1; //([A]T*[A])*[T]=[A]T*[b]を方程式で解く。 //a01とa10を0と仮定しても良いんじゃないかな? double a00 = this._a00; double a01 = this._a01_10; double a02 = this._a02_20; double a11 = this._a11; double a12 = this._a12_21; double a22 = this._a22; double t1 = a22 * b2 - a12 * b3; double t2 = a12 * b2 - a11 * b3; double t3 = a01 * b3 - a02 * b2; double t4 = a12 * a12 - a11 * a22; double t5 = a02 * a12 - a01 * a22; double t6 = a02 * a11 - a01 * a12; double det = a00 * t4 - a01 * t5 + a02 * t6; o_transfer.x = (a01 * t1 - a02 * t2 + b1 * t4) / det; o_transfer.y = -(a00 * t1 + a02 * t3 + b1 * t5) / det; o_transfer.z = (a00 * t2 + a01 * t3 + b1 * t6) / det; return; }
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) { TSinCosValue[] angles_in = this.__angles_in;// x,y,z NyARDoublePoint3d ang = this.__ang; // ZXY系のsin/cos値を抽出 rotation2Sincos_ZXY(io_rot, angles_in, ang); ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x); ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y); ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z); io_rot.setZXYAngle(ang.x, ang.y, ang.z); return; }
/** * 複数の頂点を一括して変換する * @param i_in_point * @param i_out_point * @param i_number_of_vertex */ public void getPoint3dBatch(NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex) { for (int i = i_number_of_vertex - 1; i >= 0; i--) { NyARDoublePoint3d out_ptr = i_out_point[i]; NyARDoublePoint3d in_ptr = i_in_point[i]; double x = in_ptr.x; double y = in_ptr.y; double z = in_ptr.z; out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z; out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z; out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z; } }
/** * この関数は、スクリーン座標を撮像点座標に変換します。 * 撮像点の座標系は、カメラ座標系になります。 * <p>公式 - * この関数は、gluUnprojectのビューポートとモデルビュー行列を固定したものです。 * 公式は、以下の物使用しました。 * http://www.opengl.org/sdk/docs/man/xhtml/gluUnProject.xml * ARToolKitの座標系に合せて計算するため、OpenGLのunProjectとはix,iyの与え方が違います。画面上の座標をそのまま与えてください。 * </p> * @param ix * スクリーン上の座標 * @param iy * 画像上の座標 * @param o_point_on_screen * 撮像点座標 */ public void unProject(double ix, double iy, NyARDoublePoint3d o_point_on_screen) { double n = (this._frustum_rh.m23 / (this._frustum_rh.m22 - 1)); NyARDoubleMatrix44 m44 = this._inv_frustum_rh; double v1 = (this._screen_size.w - ix - 1) * 2 / this._screen_size.w - 1.0;//ARToolKitのFrustramに合せてる。 double v2 = (this._screen_size.h - iy - 1) * 2 / this._screen_size.h - 1.0; double v3 = 2 * n - 1.0; double b = 1 / (m44.m30 * v1 + m44.m31 * v2 + m44.m32 * v3 + m44.m33); o_point_on_screen.x = (m44.m00 * v1 + m44.m01 * v2 + m44.m02 * v3 + m44.m03) * b; o_point_on_screen.y = (m44.m10 * v1 + m44.m11 * v2 + m44.m12 * v3 + m44.m13) * b; o_point_on_screen.z = (m44.m20 * v1 + m44.m21 * v2 + m44.m22 * v3 + m44.m23) * b; return; }
/** * icpGetU_from_X_by_MatX2U関数 * @param matX2U * @param coord3d * @return */ public bool setXbyMatX2U(NyARDoubleMatrix44 matX2U, NyARDoublePoint3d coord3d) { double hx = matX2U.m00 * coord3d.x + matX2U.m01 * coord3d.y + matX2U.m02 * coord3d.z + matX2U.m03; double hy = matX2U.m10 * coord3d.x + matX2U.m11 * coord3d.y + matX2U.m12 * coord3d.z + matX2U.m13; double h = matX2U.m20 * coord3d.x + matX2U.m21 * coord3d.y + matX2U.m22 * coord3d.z + matX2U.m23; if (h == 0.0) { return(false); } this.x = hx / h; this.y = hy / h; return(true); }
/** * この関数は、回転角を最適化します。 * i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した2次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。 * @param i_angle * 回転角 * @param i_trans * 平行移動量 * @param i_vertex3d * 三次元オフセット座標 * @param i_vertex2d * 理想座標系の頂点座標 * @param i_number_of_vertex * 頂点数 * @param o_angle * 調整した回転角を受け取る配列 * @ */ public void modifyMatrix(NyARDoublePoint3d i_angle, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d o_angle) { // ZXY系のsin/cos値を抽出 double sinx = Math.Sin(i_angle.x); double cosx = Math.Cos(i_angle.x); double siny = Math.Sin(i_angle.y); double cosy = Math.Cos(i_angle.y); double sinz = Math.Sin(i_angle.z); double cosz = Math.Cos(i_angle.z); o_angle.x = i_angle.x + optimizeParamX(siny, cosy, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x); o_angle.y = i_angle.y + optimizeParamY(sinx, cosx, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y); o_angle.z = i_angle.z + optimizeParamZ(sinx, cosx, siny, cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z); return; }
private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam) { NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44(); // ARdouble err; int i; if (matchData.getLength() < 4) { return(false); } NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength()); NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength()); for (i = 0; i < matchData.getLength(); i++) { sCoord[i].x = matchData.getItem(i).query.x; sCoord[i].y = matchData.getItem(i).query.y; wCoord[i].x = matchData.getItem(i).ref_.pos3d.x; wCoord[i].y = matchData.getItem(i).ref_.pos3d.y; wCoord[i].z = 0.0; } NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix()); if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc)) { return(false); } /* * printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f", * initMatXw2Xc[j][i]); printf("\n"); } */ NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix()); icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam); if (i_resultparam.last_error > 10.0f) { return(false); } return(true); }
/** * この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。 * 拡大率は1倍にセットします。 * @param i_rot * 設定する回転行列 * @param i_trans * 設定する平行移動量 */ public void setValue(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans) { this.m00=i_rot.m00; this.m01=i_rot.m01; this.m02=i_rot.m02; this.m03=i_trans.x; this.m10 =i_rot.m10; this.m11 =i_rot.m11; this.m12 =i_rot.m12; this.m13 =i_trans.y; this.m20 = i_rot.m20; this.m21 = i_rot.m21; this.m22 = i_rot.m22; this.m23 = i_trans.z; this.m30=this.m31=this.m32=0; this.m33=1.0; return; }
/** * パラメータで変換行列を更新します。 * * @param i_rot * @param i_off * @param i_trans */ public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result) { o_result.m00 = i_rot.m00; o_result.m01 = i_rot.m01; o_result.m02 = i_rot.m02; o_result.m03 = i_trans.x; o_result.m10 = i_rot.m10; o_result.m11 = i_rot.m11; o_result.m12 = i_rot.m12; o_result.m13 = i_trans.y; o_result.m20 = i_rot.m20; o_result.m21 = i_rot.m21; o_result.m22 = i_rot.m22; o_result.m23 = i_trans.z; o_result.has_value = true; return; }
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; }
/** * この関数は、行列の回転成分から、ZXY系の角度値を計算します。 * @param o_out * 角度値を受け取るオブジェクトです。 * 角度値の範囲は、0-PI(要確認)です。 */ public void getZXYAngle(NyARDoublePoint3d o_out) { double sina = this.m21; if (sina >= 1.0) { o_out.x = Math.PI / 2; o_out.y = 0; o_out.z = Math.Atan2(-this.m10, this.m00); } else if (sina <= -1.0) { o_out.x = -Math.PI / 2; o_out.y = 0; o_out.z = Math.Atan2(-this.m10, this.m00); } else { o_out.x = Math.Asin(sina); o_out.z = Math.Atan2(-this.m01, this.m11); o_out.y = Math.Atan2(-this.m20, this.m22); } }
/** * この関数は、0-PIの間で値を返します。 * @param o_out */ public void getZXYAngle(NyARDoublePoint3d o_out) { double sina = this.m21; if (sina >= 1.0) { o_out.x = Math.PI / 2; o_out.y = 0; o_out.z = Math.Atan2(-this.m10, this.m00); } else if (sina <= -1.0) { o_out.x = -Math.PI / 2; o_out.y = 0; o_out.z = Math.Atan2(-this.m10, this.m00); } else { o_out.x = Math.Asin(sina); o_out.z = Math.Atan2(-this.m01, this.m11); o_out.y = Math.Atan2(-this.m20, this.m22); } }
/** * 平行移動量と回転行列をセットします。この関数は、INyARTransmatインタフェイスのクラスが結果を保存するために使います。 * @param i_rot * @param i_trans */ public void setValue(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, double i_error) { this.m00 = i_rot.m00; this.m01 = i_rot.m01; this.m02 = i_rot.m02; this.m03 = i_trans.x; this.m10 = i_rot.m10; this.m11 = i_rot.m11; this.m12 = i_rot.m12; this.m13 = i_trans.y; this.m20 = i_rot.m20; this.m21 = i_rot.m21; this.m22 = i_rot.m22; this.m23 = i_trans.z; this.m30 = this.m31 = this.m32 = 0; this.m33 = 1.0; this.has_value = true; this.last_error = i_error; return; }
/** * この関数は、回転行列を最適化します。 * i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した2次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。 * @param io_rot * 調整する回転行列 * @param i_trans * 平行移動量 * @param i_vertex3d * 三次元オフセット座標 * @param i_vertex2d * 理想座標系の頂点座標 * @param i_number_of_vertex * 頂点数 * @ */ public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) { NyARDoublePoint3d ang = this.__ang; // ZXY系のsin/cos値を抽出 io_rot.getZXYAngle(ang); modifyMatrix(ang, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang); io_rot.setZXYAngle(ang.x, ang.y, ang.z); return; }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * ARToolKitの本P175周辺の実装と同じです。 * <p> * このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。 * </p> * @param ix * スクリーン上の座標 * @param iy * スクリーン上の座標 * @param i_mat * 平面の姿勢行列です。 * @param o_pos * 結果を受け取るオブジェクトです。 * @return * 計算に成功すると、trueを返します。 */ public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos) { //交点をカメラ座標系で計算 unProjectOnCamera(ix, iy, i_mat, o_pos); //座標系の変換 NyARDoubleMatrix44 m = new NyARDoubleMatrix44(); if (!m.inverse(i_mat)) { return(false); } m.transform3d(o_pos, o_pos); 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); }
/** * この関数は、行列を回転行列として、ZXY系の角度値をセットします。 * @param i_angle * セットする角度値です。 */ public void setZXYAngle(NyARDoublePoint3d i_angle) { setZXYAngle(i_angle.x, i_angle.y, i_angle.z); return; }
/** * 画面座標群と3次元座標群から、平行移動量を計算します。 * 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。 */ public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint3d o_transfer) { double[][] matc = this._mat_c.getArray(); double cpara00 = this._projection_mat.m00; double cpara01 = this._projection_mat.m01; double cpara02 = this._projection_mat.m02; double cpara11 = this._projection_mat.m11; double cpara12 = this._projection_mat.m12; double[] cx = this._cx; double[] cy = this._cy; //(3D座標?)を一括請求 for (int i = 0; i < 4; i++) { int x2 = i + i; NyARDoublePoint3d point3d_ptr = i_vertex3d[i]; //透視変換? matc[x2][0] = point3d_ptr.z * cx[i] - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z;// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz; matc[x2 + 1][0] = point3d_ptr.z * cy[i] - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz; } this._mat_e.mul(this._mat_at, this._mat_c); this._mat_f.mul(this._mat_t, this._mat_e); double[][] matf = this._mat_f.getArray(); o_transfer.x = matf[0][0];// trans[0] = mat_f->m[0]; o_transfer.y = matf[1][0]; o_transfer.z = matf[2][0];// trans[2] = mat_f->m[2]; return; }
/** * この関数は、3次元座標を座標変換します。 * @param i_position * 変換する三次元座標 * @param o_out * 変換後の座標を受け取るオブジェクト */ public void transformVertex(NyARDoublePoint3d i_position, NyARDoublePoint3d o_out) { transformVertex(i_position.x, i_position.y, i_position.z, o_out); return; }
/** * コンストラクタです。 * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。 * @param i_matrix * 参照する射影変換オブジェクト * @ */ public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix) : base(i_matrix) { this._angle = new NyARDoublePoint3d(); return; }
public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) { TSinCosValue[] angles_in = this.__angles_in;// x,y,z NyARDoublePoint3d ang = this.__ang; // ZXY系のsin/cos値を抽出 rotation2Sincos_ZXY(io_rot, angles_in, ang); ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x); ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y); ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z); io_rot.setZXYAngle(ang.x, ang.y, ang.z); return; }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * ARToolKitの本P175周辺の実装と同じです。 * <p> * このAPIは繰り返し使用には最適化されていません。同一なi_matに繰り返しアクセスするときは、展開してください。 * </p> * @param ix * スクリーン上の座標 * @param iy * スクリーン上の座標 * @param i_mat * 平面の姿勢行列です。 * @param o_pos * 結果を受け取るオブジェクトです。 * @return * 計算に成功すると、trueを返します。 */ public bool unProjectOnMatrix(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos) { //交点をカメラ座標系で計算 unProjectOnCamera(ix, iy, i_mat, o_pos); //座標系の変換 NyARDoubleMatrix44 m = new NyARDoubleMatrix44(); if (!m.inverse(i_mat)) { return false; } m.transform3d(o_pos, o_pos); return true; }
/** * この関数は、3次元座標を座標変換します。 * 4列目は1と仮定します。 * @param i_in * 返還前する座標値 * @param o_out * 変換後の座標を受け取るオブジェクト */ public void transform3d(NyARDoublePoint3d i_in, NyARDoublePoint3d o_out) { transform3d(i_in.x, i_in.y, i_in.z, o_out); }
/** * この関数は、入力した複数の3次元頂点を回転して返します。 * @param i_in_point * 回転する三次元座標の配列 * @param i_out_point * 回転した三次元座標の配列 * @param i_number_of_vertex * 回転する座標の個数 */ public void getPoint3dBatch(NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex) { for (int i = i_number_of_vertex - 1; i >= 0; i--) { NyARDoublePoint3d out_ptr = i_out_point[i]; NyARDoublePoint3d in_ptr = i_in_point[i]; double x = in_ptr.x; double y = in_ptr.y; double z = in_ptr.z; out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z; out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z; out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z; } return; }
/** * この関数は、回転角を最適化します。 * i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した2次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。 * @param i_angle * 回転角 * @param i_trans * 平行移動量 * @param i_vertex3d * 三次元オフセット座標 * @param i_vertex2d * 理想座標系の頂点座標 * @param i_number_of_vertex * 頂点数 * @param o_angle * 調整した回転角を受け取る配列 * @ */ public void modifyMatrix(NyARDoublePoint3d i_angle, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d o_angle) { // ZXY系のsin/cos値を抽出 double sinx = Math.Sin(i_angle.x); double cosx = Math.Cos(i_angle.x); double siny = Math.Sin(i_angle.y); double cosy = Math.Cos(i_angle.y); double sinz = Math.Sin(i_angle.z); double cosz = Math.Cos(i_angle.z); o_angle.x = i_angle.x + optimizeParamX(siny, cosy, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x); o_angle.y = i_angle.y + optimizeParamY(sinx, cosx, sinz, cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y); o_angle.z = i_angle.z + optimizeParamZ(sinx, cosx, siny, cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z); return; }
/** * ターゲット座標系の4頂点でかこまれる領域を射影した平面から、RGB画像をo_rasterに取得します。 * @param i_vertex * ターゲットのオフセットを基準値とした、頂点座標。要素数は4であること。(mm単位) * @param i_matrix * i_vertexに適応する変換行列。 * ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。 * @param i_resolution * 1ピクセルあたりのサンプリング値(n^2表現) * @param o_raster * 出力ラスタ * @return * @throws NyARException * <p>メモ:この関数にはnewが残ってるので注意</p> */ public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster) { Debug.Assert(this._target_type==RT_KNOWN); NyARDoublePoint2d[] da4=this._ref_pool._wk_da2_4; NyARDoublePoint3d v3d=new NyARDoublePoint3d(); if(i_matrix!=null){ //姿勢変換してから射影変換 for(int i=3;i>=0;i--){ //姿勢を変更して射影変換 i_matrix.transform3d(i_vertex[i],v3d); this._transform_matrix.transform3d(v3d,v3d); this._ref_pool._ref_prj_mat.project(v3d,da4[i]); } }else{ //射影変換のみ for(int i=3;i>=0;i--){ //姿勢を変更して射影変換 this._transform_matrix.transform3d(i_vertex[i],v3d); this._ref_pool._ref_prj_mat.project(v3d,da4[i]); } } //パターンの取得 return i_src.refPerspectiveRasterReader().copyPatt(da4,0,0,i_resolution, o_raster); }
/** * i_bufに{@link #getAngle()}の結果をコピーして返します。 * @return * 複製した{@link #getAngle()}の値 */ public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf) { i_buf.setValue(this._angle); return i_buf; }
/** * この関数は、回転行列を最適化します。 * ARToolKitのarGetRotに相当します。 */ public double modifyMatrix(NyARRotMatrix_ARToolKit io_rot, NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) { double factor; double a2, b2, c2; double h, x, y; double err, minerr = 0; int t1, t2, t3; int best_idx = 0; factor = 10.0 * Math.PI / 180.0; double rot0, rot1, rot2; double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23; double combo02_2, combo02_5, combo02_8, combo02_11; double combo22_2, combo22_5, combo22_8, combo22_11; double combo12_2, combo12_5, combo12_8, combo12_11; // vertex展開 double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32; VX00 = i_vertex3d[0].x; VX01 = i_vertex3d[0].y; VX02 = i_vertex3d[0].z; VX10 = i_vertex3d[1].x; VX11 = i_vertex3d[1].y; VX12 = i_vertex3d[1].z; VX20 = i_vertex3d[2].x; VX21 = i_vertex3d[2].y; VX22 = i_vertex3d[2].z; VX30 = i_vertex3d[3].x; VX31 = i_vertex3d[3].y; VX32 = i_vertex3d[3].z; double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31; P2D00 = i_vertex2d[0].x; P2D01 = i_vertex2d[0].y; P2D10 = i_vertex2d[1].x; P2D11 = i_vertex2d[1].y; P2D20 = i_vertex2d[2].x; P2D21 = i_vertex2d[2].y; P2D30 = i_vertex2d[3].x; P2D31 = i_vertex2d[3].y; NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref; double CP0 = prjmat.m00, CP1 = prjmat.m01, CP2 = prjmat.m02, CP4 = prjmat.m10, CP5 = prjmat.m11, CP6 = prjmat.m12, CP8 = prjmat.m20, CP9 = prjmat.m21, CP10 = prjmat.m22; combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03; combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13; combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23; double CACA, SASA, SACA, CA, SA; double CACACB, SACACB, SASACB, CASB, SASB; double SACASC, SACACBSC, SACACBCC, SACACC; double[][] double1D = this.__modifyMatrix_double1D; double[] a_factor = double1D[1]; double[] sinb = double1D[2]; double[] cosb = double1D[3]; double[] b_factor = double1D[4]; double[] sinc = double1D[5]; double[] cosc = double1D[6]; double[] c_factor = double1D[7]; double w, w2; double wsin, wcos; NyARDoublePoint3d angle = io_rot.getAngle(); a2 = angle.x; b2 = angle.y; c2 = angle.z; // comboの3行目を先に計算 for (int i = 0; i < 10; i++) { minerr = 1000000000.0; // sin-cosテーブルを計算(これが外に出せるとは…。) for (int j = 0; j < 3; j++) { w2 = factor * (j - 1); w = a2 + w2; a_factor[j] = w; w = b2 + w2; b_factor[j] = w; sinb[j] = Math.Sin(w); cosb[j] = Math.Cos(w); w = c2 + w2; c_factor[j] = w; sinc[j] = Math.Sin(w); cosc[j] = Math.Cos(w); } // for (t1 = 0; t1 < 3; t1++) { SA = Math.Sin(a_factor[t1]); CA = Math.Cos(a_factor[t1]); // Optimize CACA = CA * CA; SASA = SA * SA; SACA = SA * CA; for (t2 = 0; t2 < 3; t2++) { wsin = sinb[t2]; wcos = cosb[t2]; CACACB = CACA * wcos; SACACB = SACA * wcos; SASACB = SASA * wcos; CASB = CA * wsin; SASB = SA * wsin; // comboの計算1 combo02 = CP0 * CASB + CP1 * SASB + CP2 * wcos; combo12 = CP4 * CASB + CP5 * SASB + CP6 * wcos; combo22 = CP8 * CASB + CP9 * SASB + CP10 * wcos; combo02_2 = combo02 * VX02 + combo03; combo02_5 = combo02 * VX12 + combo03; combo02_8 = combo02 * VX22 + combo03; combo02_11 = combo02 * VX32 + combo03; combo12_2 = combo12 * VX02 + combo13; combo12_5 = combo12 * VX12 + combo13; combo12_8 = combo12 * VX22 + combo13; combo12_11 = combo12 * VX32 + combo13; combo22_2 = combo22 * VX02 + combo23; combo22_5 = combo22 * VX12 + combo23; combo22_8 = combo22 * VX22 + combo23; combo22_11 = combo22 * VX32 + combo23; for (t3 = 0; t3 < 3; t3++) { wsin = sinc[t3]; wcos = cosc[t3]; SACASC = SACA * wsin; SACACC = SACA * wcos; SACACBSC = SACACB * wsin; SACACBCC = SACACB * wcos; rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC; rot1 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin; rot2 = -CASB * wcos - SASB * wsin; combo00 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2; combo10 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2; combo20 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2; rot0 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC; rot1 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos; rot2 = CASB * wsin - SASB * wcos; combo01 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2; combo11 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2; combo21 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2; // err = 0.0; h = combo20 * VX00 + combo21 * VX01 + combo22_2; x = P2D00 - (combo00 * VX00 + combo01 * VX01 + combo02_2) / h; y = P2D01 - (combo10 * VX00 + combo11 * VX01 + combo12_2) / h; err += x * x + y * y; h = combo20 * VX10 + combo21 * VX11 + combo22_5; x = P2D10 - (combo00 * VX10 + combo01 * VX11 + combo02_5) / h; y = P2D11 - (combo10 * VX10 + combo11 * VX11 + combo12_5) / h; err += x * x + y * y; h = combo20 * VX20 + combo21 * VX21 + combo22_8; x = P2D20 - (combo00 * VX20 + combo01 * VX21 + combo02_8) / h; y = P2D21 - (combo10 * VX20 + combo11 * VX21 + combo12_8) / h; err += x * x + y * y; h = combo20 * VX30 + combo21 * VX31 + combo22_11; x = P2D30 - (combo00 * VX30 + combo01 * VX31 + combo02_11) / h; y = P2D31 - (combo10 * VX30 + combo11 * VX31 + combo12_11) / h; err += x * x + y * y; if (err < minerr) { minerr = err; a2 = a_factor[t1]; b2 = b_factor[t2]; c2 = c_factor[t3]; best_idx = t1 + t2 * 3 + t3 * 9; } } } } if (best_idx == (1 + 3 + 9)) { factor *= 0.5; } } io_rot.setAngle(a2, b2, c2); /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ return minerr / 4; }
/** * 座標値を射影変換します。 * @param i_3dvertex * 変換元の座標値 * @param o_2d * 変換後の座標値を受け取るオブジェクト */ public void project(NyARDoublePoint3d i_3dvertex, NyARIntPoint2d o_2d) { double w = 1 / (i_3dvertex.z * this.m22); o_2d.x = (int)((i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w); o_2d.y = (int)((i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w); return; }
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; }
/** * この関数は、ZXY系の角度値を、回転行列にセットします。 * @param i_angle * ZXY系のラジアン値 */ public void initRotByAngle(NyARDoublePoint3d i_angle) { this.setZXYAngle(i_angle); }
public void transformVertex(NyARDoublePoint3d i_in,NyARDoublePoint3d o_out) { transformVertex(i_in.x,i_in.y,i_in.z,o_out); }
/** * カメラ座標系の4頂点でかこまれる領域から、RGB画像をo_rasterに取得します。 * @param i_vertex * @param i_matrix * i_vertexに適応する変換行列。 * ターゲットの姿勢行列を指定すると、ターゲット座標系になります。不要ならばnullを設定してください。 * @param i_resolution * @param o_raster * @return * @throws NyARException */ public bool getRgbPatt3d(NyARRealitySource i_src, NyARDoublePoint3d[] i_vertex, NyARDoubleMatrix44 i_matrix, int i_resolution, INyARRgbRaster o_raster) { NyARDoublePoint2d[] vx=NyARDoublePoint2d.createArray(4); if(i_matrix!=null){ //姿勢変換してから射影変換 NyARDoublePoint3d v3d=new NyARDoublePoint3d(); for(int i=3;i>=0;i--){ i_matrix.transform3d(i_vertex[i],v3d); this._ref_prjmat.project(v3d,vx[i]); } }else{ //射影変換のみ for(int i=3;i>=0;i--){ this._ref_prjmat.project(i_vertex[i],vx[i]); } } //パターンの取得 return i_src.refPerspectiveRasterReader().copyPatt(vx, 0, 0, i_resolution, o_raster); }