/** * * @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; }
/** * この関数は、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 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_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; }
/** * この関数は、回転行列を最適化します。 * 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; }
/** * この関数は、入力した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; }
/** * この関数は、入力した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; }
/** * 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); }
/** * この関数は、スクリーン座標を撮像点座標に変換します。 * 撮像点の座標系は、カメラ座標系になります。 * <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; }
/** * この関数は、回転角を最適化します。 * 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; }
/** * この関数は、入力した複数の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; }
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; }
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; }
/** * この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。 * 拡大率は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; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * ARToolKitのarGetTransMatに該当します。 * @see INyARTransMat#transMatContinue */ public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { 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); //回転行列を計算 if (!this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex)) { return(false); } //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result); //必要なら計算パラメータを返却 if (o_param != null) { o_param.last_error = err; } return(true); }
/** * この関数は、行列の回転成分から、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); } }
/** * {@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 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; }
/** * この関数は、行列を回転行列として、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); } }
/** * この関数は、回転行列を最適化します。 * 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); }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * 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; }
/** * この関数は、行列を回転行列として、ZXY系の角度値をセットします。 * @param i_angle * セットする角度値です。 */ public void setZXYAngle(NyARDoublePoint3d i_angle) { setZXYAngle(i_angle.x, i_angle.y, i_angle.z); return; }
/** * この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。 * この座標は、カメラ座標系です。 * @param ix * スクリーン上の座標 * @param iy * スクリーン上の座標 * @param i_mat * 平面の姿勢行列です。 * @param o_pos * 結果を受け取るオブジェクトです。 */ public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos) { //画面→撮像点 this.unProject(ix, iy, o_pos); //撮像点→カメラ座標系 double nx = i_mat.m02; double ny = i_mat.m12; double nz = i_mat.m22; double mx = i_mat.m03; double my = i_mat.m13; double mz = i_mat.m23; double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z); o_pos.x = t * o_pos.x; o_pos.y = t * o_pos.y; o_pos.z = t * o_pos.z; }
/** * * @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_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_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_x * 変換する三次元座標(X) * @param i_y * 変換する三次元座標(Y) * @param i_z * 変換する三次元座標(Z) * @param o_out * 変換後の座標を受け取るオブジェクト */ 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; o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z; o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z; 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; }
/** * この関数は、行列を回転行列として、ZXY系の角度値をセットします。 * @param i_angle * セットする角度値です。 */ public void setZXYAngle(NyARDoublePoint3d i_angle) { setZXYAngle(i_angle.x, i_angle.y, i_angle.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); }
/** * コンストラクタです。 * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。 * @param i_matrix * 参照する射影変換オブジェクト * @ */ public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix) : base(i_matrix) { this._angle = new NyARDoublePoint3d(); return; }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @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); }
/** * i_bufに{@link #getAngle()}の結果をコピーして返します。 * @return * 複製した{@link #getAngle()}の値 */ public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf) { i_buf.setValue(this._angle); return i_buf; }
/** * この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。 * この座標は、カメラ座標系です。 * @param ix * スクリーン上の座標 * @param iy * スクリーン上の座標 * @param i_mat * 平面の姿勢行列です。 * @param o_pos * 結果を受け取るオブジェクトです。 */ public void unProjectOnCamera(double ix, double iy, NyARDoubleMatrix44 i_mat, NyARDoublePoint3d o_pos) { //画面→撮像点 this.unProject(ix, iy, o_pos); //撮像点→カメラ座標系 double nx = i_mat.m02; double ny = i_mat.m12; double nz = i_mat.m22; double mx = i_mat.m03; double my = i_mat.m13; double mz = i_mat.m23; double t = (nx * mx + ny * my + nz * mz) / (nx * o_pos.x + ny * o_pos.y + nz * o_pos.z); o_pos.x = t * o_pos.x; o_pos.y = t * o_pos.y; o_pos.z = t * o_pos.z; }
/** * この関数は、ZXY系の角度値を、回転行列にセットします。 * @param i_angle * ZXY系のラジアン値 */ public void initRotByAngle(NyARDoublePoint3d i_angle) { this.setZXYAngle(i_angle); }
/** * * @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); }
/** * この関数は、回転行列を最適化します。 * 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; }
/** * 画面座標群と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; }
/** * この関数は、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); }
/** * {@link #icpGetInitXw2XcSub}関数のオフセットを計算します。 */ private static NyARDoublePoint3d makeOffset(NyARDoublePoint3d[] ppos3d, int num, NyARDoublePoint3d o_off) { double minx, miny, minz; double maxx, maxy, maxz; minx = miny = minz = Double.PositiveInfinity; maxx = maxy = maxz = Double.NegativeInfinity; for (int i = 0; i < num; i++) { if (ppos3d[i].x > maxx) { maxx = ppos3d[i].x; } if (ppos3d[i].x < minx) { minx = ppos3d[i].x; } if (ppos3d[i].y > maxy) { maxy = ppos3d[i].y; } if (ppos3d[i].y < miny) { miny = ppos3d[i].y; } if (ppos3d[i].z > maxz) { maxz = ppos3d[i].z; } if (ppos3d[i].z < minz) { minz = ppos3d[i].z; } } o_off.x = -(maxx + minx) / 2.0; o_off.y = -(maxy + miny) / 2.0; o_off.z = -(maxz + minz) / 2.0; return(o_off); }
private double optimizeParamZ(double sina, double cosa, double sinb, double cosb, 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 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz)); double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz)); double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z); double Y0 = cp11 * (ix * cosb + sinb * iz); double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb); double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z)); double H0 = 0; double H1 = 0; double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + 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); }
/** * この関数は、ZXY系の角度値を、回転行列にセットします。 * @param i_angle * ZXY系のラジアン値 */ public void initRotByAngle(NyARDoublePoint3d i_angle) { this.setZXYAngle(i_angle); }
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; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。 * @see INyARTransMat#transMatContinue */ public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { NyARDoublePoint3d trans = this.__transMat_trans; //最適化計算の閾値を決定 double err_threshold = makeErrThreshold(i_square.sqvertex); //平行移動量計算機に、2D座標系をセット 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; } this._transsolver.set2dVertex(vertex_2d, 4); //回転行列を計算 NyARRotMatrix rot = this._rotmatrix; rot.initRotByPrevResult(i_prev_result); //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //現在のエラーレートを計算 double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //エラーレートの判定 if (min_err < i_prev_err + err_threshold) { //save initial result o_result.setValue(rot, trans); // System.out.println("TR:ok"); //最適化してみる。 for (int i = 0; i < 5; i++) { //変換行列の最適化 this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4); double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //System.out.println("E:"+err); if (min_err - err < err_threshold / 2) { //System.out.println("BREAK"); break; } this._transsolver.solveTransportVector(vertex_3d, trans); o_result.setValue(rot, trans); min_err = err; } //継続計算成功 if (o_param != null) { o_param.last_error = min_err; } return(true); } //継続計算失敗 return(false); }
/** * {@link #icpGetInitXw2XcSub}関数のオフセットを計算します。 */ private static NyARDoublePoint3d makeOffset(NyARDoublePoint3d[] ppos3d, int num, NyARDoublePoint3d o_off) { double minx, miny, minz; double maxx, maxy, maxz; minx = miny = minz = Double.PositiveInfinity; maxx = maxy = maxz = Double.NegativeInfinity; for (int i = 0; i < num; i++) { if (ppos3d[i].x > maxx) { maxx = ppos3d[i].x; } if (ppos3d[i].x < minx) { minx = ppos3d[i].x; } if (ppos3d[i].y > maxy) { maxy = ppos3d[i].y; } if (ppos3d[i].y < miny) { miny = ppos3d[i].y; } if (ppos3d[i].z > maxz) { maxz = ppos3d[i].z; } if (ppos3d[i].z < minz) { minz = ppos3d[i].z; } } o_off.x = -(maxx + minx) / 2.0; o_off.y = -(maxy + miny) / 2.0; o_off.z = -(maxz + minz) / 2.0; return o_off; }
/* * 射影変換式 基本式 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)); }
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; }
/** * この関数は、回転行列を最適化します。 * 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; }
/** * この関数は、スクリーン座標点をマーカ平面の点に変換します。 * {@link #isExistMarker(int)}がtrueの時にだけ使用できます。 * @param i_id * マーカID(ハンドル)値。 * @param i_x * 変換元のスクリーン座標 * @param i_y * 変換元のスクリーン座標 * @param i_out * 結果を格納するオブジェクト * @return * 結果を格納したi_outに設定したオブジェクト */ public NyARDoublePoint3d getMarkerPlanePos(int i_id, int i_x, int i_y, NyARDoublePoint3d i_out) { this._frustum.unProjectOnMatrix(i_x, i_y, this.getMarkerMatrix(i_id), i_out); return i_out; }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @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); }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * 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); }
/** * この関数は、スクリーン座標を撮像点座標に変換します。 * 撮像点の座標系は、カメラ座標系になります。 * <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; }
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 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; }
/** * {@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); }
/** * i_bufに{@link #getAngle()}の結果をコピーして返します。 * @return * 複製した{@link #getAngle()}の値 */ public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf) { i_buf.setValue(this._angle); return(i_buf); }