/** * 変換行列と頂点座標から、パラメータを計算 * o_paramの[0..3]にはXのパラメタ、[4..7]にはYのパラメタを格納する。 * @param i_vertex * @param pa * @param pb */ private void calcPara(NyARIntPoint2d[] i_vertex, double[] o_cparam) { NyARDoubleMatrix44 invmat = this._invmat; double v1, v2, v4; //変換行列とベクトルの積から、変換パラメタを計算する。 v1 = i_vertex[0].x; v2 = i_vertex[1].x; v4 = i_vertex[3].x; o_cparam[0] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].x + invmat.m03 * v4; o_cparam[1] = invmat.m10 * v1 + invmat.m11 * v2; //m12,m13は0; o_cparam[2] = invmat.m20 * v1 + invmat.m23 * v4; //m21,m22は0; o_cparam[3] = v1; //m30は1.0で、m31,m32,m33は0 v1 = i_vertex[0].y; v2 = i_vertex[1].y; v4 = i_vertex[3].y; o_cparam[4] = invmat.m00 * v1 + invmat.m01 * v2 + invmat.m02 * i_vertex[2].y + invmat.m03 * v4; o_cparam[5] = invmat.m10 * v1 + invmat.m11 * v2; //m12,m13は0; o_cparam[6] = invmat.m20 * v1 + invmat.m23 * v4; //m21,m22は0; o_cparam[7] = v1; //m30は1.0で、m31,m32,m33は0 return; }
/** * この関数は、行列同士の掛け算をして、インスタンスに格納します。 * i_mat_lとi_mat_rには、thisを指定しないでください。 * @param i_mat_l * 左成分の行列 * @param i_mat_r * 右成分の行列 */ public void mul(NyARDoubleMatrix44 i_mat_l, NyARDoubleMatrix44 i_mat_r) { Debug.Assert(this != i_mat_l); Debug.Assert(this != i_mat_r); this.m00 = i_mat_l.m00 * i_mat_r.m00 + i_mat_l.m01 * i_mat_r.m10 + i_mat_l.m02 * i_mat_r.m20 + i_mat_l.m03 * i_mat_r.m30; this.m01 = i_mat_l.m00 * i_mat_r.m01 + i_mat_l.m01 * i_mat_r.m11 + i_mat_l.m02 * i_mat_r.m21 + i_mat_l.m03 * i_mat_r.m31; this.m02 = i_mat_l.m00 * i_mat_r.m02 + i_mat_l.m01 * i_mat_r.m12 + i_mat_l.m02 * i_mat_r.m22 + i_mat_l.m03 * i_mat_r.m32; this.m03 = i_mat_l.m00 * i_mat_r.m03 + i_mat_l.m01 * i_mat_r.m13 + i_mat_l.m02 * i_mat_r.m23 + i_mat_l.m03 * i_mat_r.m33; this.m10 = i_mat_l.m10 * i_mat_r.m00 + i_mat_l.m11 * i_mat_r.m10 + i_mat_l.m12 * i_mat_r.m20 + i_mat_l.m13 * i_mat_r.m30; this.m11 = i_mat_l.m10 * i_mat_r.m01 + i_mat_l.m11 * i_mat_r.m11 + i_mat_l.m12 * i_mat_r.m21 + i_mat_l.m13 * i_mat_r.m31; this.m12 = i_mat_l.m10 * i_mat_r.m02 + i_mat_l.m11 * i_mat_r.m12 + i_mat_l.m12 * i_mat_r.m22 + i_mat_l.m13 * i_mat_r.m32; this.m13 = i_mat_l.m10 * i_mat_r.m03 + i_mat_l.m11 * i_mat_r.m13 + i_mat_l.m12 * i_mat_r.m23 + i_mat_l.m13 * i_mat_r.m33; this.m20 = i_mat_l.m20 * i_mat_r.m00 + i_mat_l.m21 * i_mat_r.m10 + i_mat_l.m22 * i_mat_r.m20 + i_mat_l.m23 * i_mat_r.m30; this.m21 = i_mat_l.m20 * i_mat_r.m01 + i_mat_l.m21 * i_mat_r.m11 + i_mat_l.m22 * i_mat_r.m21 + i_mat_l.m23 * i_mat_r.m31; this.m22 = i_mat_l.m20 * i_mat_r.m02 + i_mat_l.m21 * i_mat_r.m12 + i_mat_l.m22 * i_mat_r.m22 + i_mat_l.m23 * i_mat_r.m32; this.m23 = i_mat_l.m20 * i_mat_r.m03 + i_mat_l.m21 * i_mat_r.m13 + i_mat_l.m22 * i_mat_r.m23 + i_mat_l.m23 * i_mat_r.m33; this.m30 = i_mat_l.m30 * i_mat_r.m00 + i_mat_l.m31 * i_mat_r.m10 + i_mat_l.m32 * i_mat_r.m20 + i_mat_l.m33 * i_mat_r.m30; this.m31 = i_mat_l.m30 * i_mat_r.m01 + i_mat_l.m31 * i_mat_r.m11 + i_mat_l.m32 * i_mat_r.m21 + i_mat_l.m33 * i_mat_r.m31; this.m32 = i_mat_l.m30 * i_mat_r.m02 + i_mat_l.m31 * i_mat_r.m12 + i_mat_l.m32 * i_mat_r.m22 + i_mat_l.m33 * i_mat_r.m32; this.m33 = i_mat_l.m30 * i_mat_r.m03 + i_mat_l.m31 * i_mat_r.m13 + i_mat_l.m32 * i_mat_r.m23 + i_mat_l.m33 * i_mat_r.m33; return; }
/** * コンストラクタです。 * @param i_width * このラスタの幅 * @param i_height * このラスタの高さ * @ */ public NyARColorPatt_PseudoAffine(int i_width, int i_height) { this._size = new NyARIntSize(i_width, i_height); this._patdata = new int[i_height * i_width]; this._pixelreader = NyARRgbPixelDriverFactory.createDriver(this); //疑似アフィン変換のパラメタマトリクスを計算します。 //長方形から計算すると、有効要素がm00,m01,m02,m03,m10,m11,m20,m23,m30になります。 NyARDoubleMatrix44 mat = this._invmat; mat.m00 = 0; mat.m01 = 0; mat.m02 = 0; mat.m03 = 1.0; mat.m10 = 0; mat.m11 = i_width - 1; mat.m12 = 0; mat.m13 = 1.0; mat.m20 = (i_width - 1) * (i_height - 1); mat.m21 = i_width - 1; mat.m22 = i_height - 1; mat.m23 = 1.0; mat.m30 = 0; mat.m31 = 0; mat.m32 = i_height - 1; mat.m33 = 1.0; mat.inverse(mat); return; }
/** * {@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); }
/** * {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。 */ private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat) { double m02 = 0; double m12 = 0; double m20 = 0; double m21 = 0; double m22 = 0; for (int i = 0; i < i_num; i++) { double cx = i_cp.m02 - pos2d[i].x; double cy = i_cp.m12 - pos2d[i].y; m02 += (cx) * i_cp.m00; m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11; m20 += i_cp.m00 * (cx); m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy); m22 += (cx) * (cx) + (cy) * (cy); } o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num; o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num; o_mat.m02 = m02; o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num; o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num; o_mat.m12 = m12; o_mat.m20 = m20; o_mat.m21 = m21; o_mat.m22 = m22; o_mat.inverse(o_mat); return(o_mat); }
/** * この関数は、オブジェクトの配列を生成して返します。 * @param i_number * 配列の長さ * @return * 新しいオブジェクト配列 */ public static NyARDoubleMatrix44[] createArray(int i_number) { NyARDoubleMatrix44[] ret = new NyARDoubleMatrix44[i_number]; for (int i = 0; i < i_number; i++) { ret[i] = new NyARDoubleMatrix44(); } return ret; }
public PerspectiveParam getPerspectiveParam(PerspectiveParam o_value) { NyARDoubleMatrix44 mat = this._frustum_rh; o_value.far = mat.m23 / (mat.m22 + 1); o_value.near = mat.m23 / (mat.m22 - 1); o_value.aspect = mat.m11 / mat.m00; o_value.fovy = 2 * Math.Atan(1 / (mat.m00 * o_value.aspect)); return(o_value); }
/** * カメラ座標系の点を、スクリーン座標の点へ変換します。 * @param i_x * カメラ座標系の点 * @param i_y * カメラ座標系の点 * @param i_z * カメラ座標系の点 * @param o_pos2d * 結果を受け取るオブジェクトです。 */ public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d) { NyARDoubleMatrix44 m = this._frustum_rh; double v3_1 = 1 / i_z * m.m32; double w = this._screen_size.w; double h = this._screen_size.h; o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2; o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2; return; }
/// <summary> /// 行列をRotationとVectorへ分解します。 /// </summary> /// <param name='mat'> /// Mat. /// </param> /// <param name='i_scale'> /// I_scale. /// </param> /// <param name='o_pos'> /// O_pos. /// </param> /// <param name='o_rot'> /// O_rot. /// </param> public static void Mat2UnityVecRot(NyARDoubleMatrix44 mat, double i_scale, ref Vector3 o_pos, ref Quaternion o_rot) { Mat2Rot( mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22, ref o_rot); double scale = 1 / i_scale; o_pos.x = (float)(mat.m03 * scale); o_pos.y = (float)(mat.m13 * scale); o_pos.z = (float)(mat.m23 * scale); return; }
/** * 透視変換パラメータを行列から復元します。 * @param o_value * @return * 値をセットしたo_valueを返します。 */ public FrustumParam getFrustumParam(FrustumParam o_value) { double near; NyARDoubleMatrix44 mat = this._frustum_rh; o_value.far = mat.m23 / (mat.m22 + 1); o_value.near = near = mat.m23 / (mat.m22 - 1); o_value.left = (mat.m02 - 1) * near / mat.m00; o_value.right = (mat.m02 + 1) * near / mat.m00; o_value.bottom = (mat.m12 - 1) * near / mat.m11; o_value.top = (mat.m12 + 1) * near / mat.m11; return(o_value); }
/** * 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; }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * 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); }
/** * この関数は、{@link NyARTransMatResult}の内容を、回転行列にセットします。 * @param i_prev_result * セットする姿勢変換行列。 */ public virtual void initRotByPrevResult(NyARDoubleMatrix44 i_prev_result) { this.m00 = i_prev_result.m00; this.m01 = i_prev_result.m01; this.m02 = i_prev_result.m02; this.m10 = i_prev_result.m10; this.m11 = i_prev_result.m11; this.m12 = i_prev_result.m12; this.m20 = i_prev_result.m20; this.m21 = i_prev_result.m21; this.m22 = i_prev_result.m22; 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; }
public void setFromMatrix(NyARDoubleMatrix44 i_mat) { // 最大成分を検索 double elem0 = i_mat.m00 - i_mat.m11 - i_mat.m22 + 1.0; double elem1 = -i_mat.m00 + i_mat.m11 - i_mat.m22 + 1.0; double elem2 = -i_mat.m00 - i_mat.m11 + i_mat.m22 + 1.0; double elem3 = i_mat.m00 + i_mat.m11 + i_mat.m22 + 1.0; if (elem0 > elem1 && elem0 > elem2 && elem0 > elem3) { double v = Math.Sqrt(elem0) * 0.5; double mult = 0.25 / v; this.x = v; this.y = ((i_mat.m10 + i_mat.m01) * mult); this.z = ((i_mat.m02 + i_mat.m20) * mult); this.w = ((i_mat.m21 - i_mat.m12) * mult); } else if (elem1 > elem2 && elem1 > elem3) { double v = Math.Sqrt(elem1) * 0.5; double mult = 0.25 / v; this.x = ((i_mat.m10 + i_mat.m01) * mult); this.y = (v); this.z = ((i_mat.m21 + i_mat.m12) * mult); this.w = ((i_mat.m02 - i_mat.m20) * mult); } else if (elem2 > elem3) { double v = Math.Sqrt(elem2) * 0.5; double mult = 0.25 / v; this.x = ((i_mat.m02 + i_mat.m20) * mult); this.y = ((i_mat.m21 + i_mat.m12) * mult); this.z = (v); this.w = ((i_mat.m10 - i_mat.m01) * mult); } else { double v = Math.Sqrt(elem3) * 0.5; double mult = 0.25 / v; this.x = ((i_mat.m21 - i_mat.m12) * mult); this.y = ((i_mat.m02 - i_mat.m20) * mult); this.z = ((i_mat.m10 - i_mat.m01) * mult); this.w = v; } return; }
/** * icpUpdateMat関数です。matXw2Xcへ値を出力します。 * @param matXw2Xc * @param dS * double[6] * @return */ public void makeMat(NyARDoubleMatrix44 matXw2Xc) { IcpMat44 mat = this.__mat; mat.setDeltaS(this); double w0, w1, w2, w3; w0 = matXw2Xc.m00; w1 = matXw2Xc.m01; w2 = matXw2Xc.m02; w3 = matXw2Xc.m03; //ji matXw2Xc.m00 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20; matXw2Xc.m01 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21; matXw2Xc.m02 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22; matXw2Xc.m03 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3; w0 = matXw2Xc.m10; w1 = matXw2Xc.m11; w2 = matXw2Xc.m12; w3 = matXw2Xc.m13; matXw2Xc.m10 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20; matXw2Xc.m11 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21; matXw2Xc.m12 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22; matXw2Xc.m13 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3; w0 = matXw2Xc.m20; w1 = matXw2Xc.m21; w2 = matXw2Xc.m22; w3 = matXw2Xc.m23; matXw2Xc.m20 = w0 * mat.m00 + w1 * mat.m10 + w2 * mat.m20; matXw2Xc.m21 = w0 * mat.m01 + w1 * mat.m11 + w2 * mat.m21; matXw2Xc.m22 = w0 * mat.m02 + w1 * mat.m12 + w2 * mat.m22; matXw2Xc.m23 = w0 * mat.m03 + w1 * mat.m13 + w2 * mat.m23 + w3; matXw2Xc.m30 = 0; matXw2Xc.m31 = 0; matXw2Xc.m32 = 0; matXw2Xc.m33 = 1; 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; }
/** * この関数は、ARToolKitのcheck_dir関数に相当します。 * 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整?) * @param i_start_vertex * 開始位置? * @param i_end_vertex * 終了位置? * @throws NyARException */ public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex) { double h; NyARDoubleMatrix44 inv_cpara = this._inv_cpara; //final double[] world = __checkVectorByVertex_world;// [2][3]; double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0; // mat_a->m[0]*st[0]*10.0+ double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0; // mat_a->m[3]*st[0]*10.0+ double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0; // mat_a->m[6]*st[0]*10.0+ double world3 = world0 + this.v1; double world4 = world1 + this.v2; double world5 = world2 + this.v3; // </Optimize> NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref; h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2; if (h == 0.0) { return(false); } double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h; double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h; //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5; h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5; if (h == 0.0) { return(false); } double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h; double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h; double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1); if (v < 0) { this.v1 = -this.v1; this.v2 = -this.v2; this.v3 = -this.v3; } return(true); }
/** * この関数は、オブジェクトの内容をインスタンスにコピーします。 * @param i_value * コピー元のオブジェクト */ public void setValue(NyARDoubleMatrix44 i_value) { this.m00 = i_value.m00; this.m01 = i_value.m01; this.m02 = i_value.m02; this.m03 = i_value.m03; this.m10 = i_value.m10; this.m11 = i_value.m11; this.m12 = i_value.m12; this.m13 = i_value.m13; this.m20 = i_value.m20; this.m21 = i_value.m21; this.m22 = i_value.m22; this.m23 = i_value.m23; this.m30 = i_value.m30; this.m31 = i_value.m31; this.m32 = i_value.m32; this.m33 = i_value.m33; 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); }
/** * icpGetJ_U_Xc * @param J_U_Xc * @param matXc2U * @param cameraCoord * @return */ public bool setXc2UXc(NyARDoubleMatrix44 matXc2U, double Xcx, double Xcy, double Xcz) { double w1, w2, w3, w3_w3; w1 = matXc2U.m00 * Xcx + matXc2U.m01 * Xcy + matXc2U.m02 * Xcz + matXc2U.m03; w2 = matXc2U.m10 * Xcx + matXc2U.m11 * Xcy + matXc2U.m12 * Xcz + matXc2U.m13; w3 = matXc2U.m20 * Xcx + matXc2U.m21 * Xcy + matXc2U.m22 * Xcz + matXc2U.m23; if (w3 == 0.0) { return(false); } w3_w3 = w3 * w3; this.m00 = (matXc2U.m00 * w3 - matXc2U.m20 * w1) / w3_w3; this.m01 = (matXc2U.m01 * w3 - matXc2U.m21 * w1) / w3_w3; this.m02 = (matXc2U.m02 * w3 - matXc2U.m22 * w1) / w3_w3; this.m10 = (matXc2U.m10 * w3 - matXc2U.m20 * w2) / w3_w3; this.m11 = (matXc2U.m11 * w3 - matXc2U.m21 * w2) / w3_w3; this.m12 = (matXc2U.m12 * w3 - matXc2U.m22 * w2) / w3_w3; return(true); }
/** * {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。 */ private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val) { double v0 = 0; double v1 = 0; double v2 = 0; for (int j = 0; j < i_num; j++) { double p3x = pos3d[j].x + offset.x; double p3y = pos3d[j].y + offset.y; double p3z = pos3d[j].z + offset.z; double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z; double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z; double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z; double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz; double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz; v0 += i_cp.m00 * c1; v1 += i_cp.m01 * c1 + i_cp.m11 * c2; v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2; } o_val[0] = v0; o_val[1] = v1; o_val[2] = v2; return o_val; }
public NyARIcpPlane(NyARParam i_param) { this._cparam = i_param.getPerspectiveProjectionMatrix(); }
public override bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param) { Debug.Assert(num >= 4); double err0 = 0, err1; NyARIcpUtils.U u = this.__u; NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__E = new double[num]; this.__E2 = new double[num]; this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; double[] E = this.__E; double[] E2 = this.__E2; NyARDoublePoint2d[] du = this.__du; NyARDoubleMatrix44 matXw2U = this.__matXw2U; int inlierNum = (int)(num * this.getInlierProbability()) - 1; if (inlierNum < 3) { inlierNum = 3; } o_matxw2xc.setValue(initMatXw2Xc); for (int i = 0;; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); for (int j = 0; j < num; j++) { if (!u.setXbyMatX2U(matXw2U, worldCoord[j])) { return false; } double dx = screenCoord[j].x - u.x; double dy = screenCoord[j].y - u.y; du[j].x = dx; du[j].y = dy; E[j] = E2[j] = dx * dx + dy * dy; } Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE); double K2 = E2[inlierNum] * K2_FACTOR; if (K2 < 16.0) { K2 = 16.0; } err1 = 0.0; for (int j = 0; j < num; j++) { if (E2[j] > K2) { err1 += K2 / 6.0; } else { err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2)); } } err1 /= num; if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this._maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if (E[j] <= K2) { double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2); if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W)) { return false; } } } if (jus.getLength() < 3) { return false; } if (!dS.setJusArray(jus)) { return false; } dS.makeMat(o_matxw2xc); } if(o_result_param!=null){ o_result_param.last_error=err1; } return true; }
/** * この関数は、逆行列を計算して、インスタンスにセットします。 * @param i_src * 逆行列を計算するオブジェクト。thisを指定できます。 * @return * 逆行列を得られると、trueを返します。 */ public bool inverse(NyARDoubleMatrix44 i_src) { double a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34, a41, a42, a43, a44; double b11, b12, b13, b14, b21, b22, b23, b24, b31, b32, b33, b34, b41, b42, b43, b44; double t1, t2, t3, t4, t5, t6; a11 = i_src.m00; a12 = i_src.m01; a13 = i_src.m02; a14 = i_src.m03; a21 = i_src.m10; a22 = i_src.m11; a23 = i_src.m12; a24 = i_src.m13; a31 = i_src.m20; a32 = i_src.m21; a33 = i_src.m22; a34 = i_src.m23; a41 = i_src.m30; a42 = i_src.m31; a43 = i_src.m32; a44 = i_src.m33; t1 = a33 * a44 - a34 * a43; t2 = a34 * a42 - a32 * a44; t3 = a32 * a43 - a33 * a42; t4 = a34 * a41 - a31 * a44; t5 = a31 * a43 - a33 * a41; t6 = a31 * a42 - a32 * a41; b11 = a22 * t1 + a23 * t2 + a24 * t3; b21 = -(a23 * t4 + a24 * t5 + a21 * t1); b31 = a24 * t6 - a21 * t2 + a22 * t4; b41 = -(a21 * t3 - a22 * t5 + a23 * t6); t1 = a43 * a14 - a44 * a13; t2 = a44 * a12 - a42 * a14; t3 = a42 * a13 - a43 * a12; t4 = a44 * a11 - a41 * a14; t5 = a41 * a13 - a43 * a11; t6 = a41 * a12 - a42 * a11; b12 = -(a32 * t1 + a33 * t2 + a34 * t3); b22 = a33 * t4 + a34 * t5 + a31 * t1; b32 = -(a34 * t6 - a31 * t2 + a32 * t4); b42 = a31 * t3 - a32 * t5 + a33 * t6; t1 = a13 * a24 - a14 * a23; t2 = a14 * a22 - a12 * a24; t3 = a12 * a23 - a13 * a22; t4 = a14 * a21 - a11 * a24; t5 = a11 * a23 - a13 * a21; t6 = a11 * a22 - a12 * a21; b13 = a42 * t1 + a43 * t2 + a44 * t3; b23 = -(a43 * t4 + a44 * t5 + a41 * t1); b33 = a44 * t6 - a41 * t2 + a42 * t4; b43 = -(a41 * t3 - a42 * t5 + a43 * t6); t1 = a23 * a34 - a24 * a33; t2 = a24 * a32 - a22 * a34; t3 = a22 * a33 - a23 * a32; t4 = a24 * a31 - a21 * a34; t5 = a21 * a33 - a23 * a31; t6 = a21 * a32 - a22 * a31; b14 = -(a12 * t1 + a13 * t2 + a14 * t3); b24 = a13 * t4 + a14 * t5 + a11 * t1; b34 = -(a14 * t6 - a11 * t2 + a12 * t4); b44 = a11 * t3 - a12 * t5 + a13 * t6; double det_1 = (a11 * b11 + a21 * b12 + a31 * b13 + a41 * b14); if (det_1 == 0) { return false; } det_1 = 1 / det_1; this.m00 = b11 * det_1; this.m01 = b12 * det_1; this.m02 = b13 * det_1; this.m03 = b14 * det_1; this.m10 = b21 * det_1; this.m11 = b22 * det_1; this.m12 = b23 * det_1; this.m13 = b24 * det_1; this.m20 = b31 * det_1; this.m21 = b32 * det_1; this.m22 = b33 * det_1; this.m23 = b34 * det_1; this.m30 = b41 * det_1; this.m31 = b42 * det_1; this.m32 = b43 * det_1; this.m33 = b44 * det_1; return true; }
/** * この関数は、視錐台行列をインスタンスにセットします。 * @param i_projection * ARToolKitスタイルの射影変換行列 * @param i_width * スクリーンサイズです。 * @param i_height * スクリーンサイズです。 */ public void setValue(NyARDoubleMatrix44 i_projection_mat, int i_width, int i_height) { this._frustum_rh.setValue(i_projection_mat); this._inv_frustum_rh.inverse(this._frustum_rh); this._screen_size.setValue(i_width, i_height); }
/** * この関数は、カメラパラメータから右手系の視錐台を作ります。 * <p>注意 - * この処理は低速です。繰り返しの使用はできるだけ避けてください。 * </p> * @param i_dist_min * 視錐台のnear point(mm指定) * @param i_dist_max * 視錐台のfar point(mm指定) * @param o_frustum * 視錐台を受け取る配列。 * @see NyARPerspectiveProjectionMatrix#makeCameraFrustumRH */ public void makeCameraFrustumRH(double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum) { this._projection_matrix.makeCameraFrustumRH(this._screen_size.w, this._screen_size.h, i_dist_min, i_dist_max, o_frustum); return; }
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; }
/** * この関数は、検出したマーカーの変換行列を計算して、o_resultへ値を返します。 * 直前に実行した{@link #detectMarkerLite}が成功していないと使えません。 * @param o_result * 変換行列を受け取るオブジェクト。 * @ */ public void getTransmat(NyARDoubleMatrix44 o_result) { // 一番一致したマーカーの位置とかその辺を計算 if (this._is_continue) { //履歴が使えそうか判定 if (this._last_input_mat == o_result) { if (this._transmat.transMatContinue(this._square, this._offset, o_result, this._last_result_param.last_error, o_result, this._last_result_param)) { return; } } } //履歴使えないor継続認識失敗 this._transmat.transMat(this._square, this._offset, o_result, this._last_result_param); this._last_input_mat = o_result; 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; }
/** * この関数は、ARToolKitスタイルのProjectionMatrixから、 CameraFrustamを計算します。 * @param i_promat * @param i_size * スクリーンサイズを指定します。 * @param i_scale * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param i_near * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param i_far * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param o_gl_projection * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 */ public static void ToCameraFrustumRH(NyARPerspectiveProjectionMatrix i_promat, NyARIntSize i_size, double i_scale, double i_near, double i_far, ref Matrix4x4 o_mat) { NyARDoubleMatrix44 m = new NyARDoubleMatrix44(); i_promat.makeCameraFrustumRH(i_size.w, i_size.h, i_near * i_scale, i_far * i_scale, m); o_mat.m00 = (float)m.m00; o_mat.m01 = (float)m.m01; o_mat.m02 = (float)m.m02; o_mat.m03 = (float)m.m03; o_mat.m10 = (float)m.m10; o_mat.m11 = (float)m.m11; o_mat.m12 = (float)m.m12; o_mat.m13 = (float)m.m13; o_mat.m20 = (float)m.m20; o_mat.m21 = (float)m.m21; o_mat.m22 = (float)m.m22; o_mat.m23 = (float)m.m23; o_mat.m30 = (float)m.m30; o_mat.m31 = (float)m.m31; o_mat.m32 = (float)m.m32; o_mat.m33 = (float)m.m33; 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); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@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); }
/** * 自己コールバック関数です。 * 継承したクラスで、マーカ更新時の処理を実装してください。 * 引数の値の有効期間は、関数が終了するまでです。 * @param i_square * 現在のマーカ検出位置です。 * @param o_result * 現在の姿勢変換行列です。 */ protected abstract void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 o_result);
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@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; }
/** * ターゲット座標系の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); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 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; }
/** * @deprecated * {@link #getTransmat} */ public void getTransmationMatrix(NyARDoubleMatrix44 o_result) { this.getTransmat(o_result); return; }
/** * 右手系の視錐台を作ります。 * この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。 * @param i_screen_width * スクリーンサイズを指定します。 * @param i_screen_height * スクリーンサイズを指定します。 * @param i_dist_min * near pointを指定します(mm単位) * @param i_dist_max * far pointを指定します(mm単位) * @param o_frustum * 視錐台の格納先オブジェクトを指定します。 */ public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum) { NyARMat trans_mat = new NyARMat(3, 4); NyARMat icpara_mat = new NyARMat(3, 4); double[][] p = ArrayUtils.newDouble2dArray(3, 3); int i; this.decompMat(icpara_mat, trans_mat); double[][] icpara = icpara_mat.getArray(); double[][] trans = trans_mat.getArray(); for (i = 0; i < 4; i++) { icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i]; } p[0][0] = icpara[0][0] / icpara[2][2]; p[0][1] = icpara[0][1] / icpara[2][2]; p[0][2] = icpara[0][2] / icpara[2][2]; p[1][0] = icpara[1][0] / icpara[2][2]; p[1][1] = icpara[1][1] / icpara[2][2]; p[1][2] = icpara[1][2] / icpara[2][2]; p[2][0] = icpara[2][0] / icpara[2][2]; p[2][1] = icpara[2][1] / icpara[2][2]; p[2][2] = icpara[2][2] / icpara[2][2]; double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33; //視錐台への変換 q00 = (2.0 * p[0][0] / (i_screen_width - 1)); q01 = (2.0 * p[0][1] / (i_screen_width - 1)); q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0); q03 = 0.0; o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0]; o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1]; o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2]; o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03; q10 = 0.0; q11 = -(2.0 * p[1][1] / (i_screen_height - 1)); q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0); q13 = 0.0; o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0]; o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1]; o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2]; o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13; q20 = 0.0; q21 = 0.0; q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max); q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max); o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0]; o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1]; o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2]; o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23; q30 = 0.0; q31 = 0.0; q32 = -1.0; q33 = 0.0; o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0]; o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1]; o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2]; o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33; 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 bool icpGetInitXw2Xc_from_PlanarData( NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARDoubleMatrix44 initMatXw2Xc) { if (i_num < 4) { throw new NyARException(); } // nを元に配列の準備 NyARMat matAtA = this.__matAtA; NyARMat matAtB = this.__matAtB; NyARMat matC = this.__matC; makeMatAtA(screenCoord, worldCoord, i_num, matAtA); makeMatAtB(screenCoord, worldCoord, i_num, matAtB); if (!matAtA.inverse()) { return(false); } matC.mul(matAtA, matAtB); double[][] bufc = matC.getArray(); double t0, t1, t2; NyARRotVector vec0 = this.__vec0; NyARRotVector vec1 = this.__vec1; NyARDoubleMatrix44 matxc = this._cparam; vec0.v3 = (bufc[6][0]); vec0.v2 = (bufc[3][0] - matxc.m12 * vec0.v3) / matxc.m11; vec0.v1 = (bufc[0][0] - matxc.m02 * vec0.v3 - matxc.m01 * vec0.v2) / matxc.m00; vec1.v3 = (bufc[7][0]); vec1.v2 = (bufc[4][0] - matxc.m12 * vec1.v3) / matxc.m11; vec1.v1 = (bufc[1][0] - matxc.m02 * vec1.v3 - matxc.m01 * vec1.v2) / matxc.m00; t2 = 1.0; t1 = (bufc[5][0] - matxc.m12 * t2) / matxc.m11; t0 = (bufc[2][0] - matxc.m02 * t2 - matxc.m01 * t1) / matxc.m00; double l1 = Math.Sqrt(vec0.v1 * vec0.v1 + vec0.v2 * vec0.v2 + vec0.v3 * vec0.v3); double l2 = Math.Sqrt(vec1.v1 * vec1.v1 + vec1.v2 * vec1.v2 + vec1.v3 * vec1.v3); vec0.v1 /= l1; vec0.v2 /= l1; vec0.v3 /= l1; vec1.v1 /= l2; vec1.v2 /= l2; vec1.v3 /= l2; t0 /= (l1 + l2) / 2.0; t1 /= (l1 + l2) / 2.0; t2 /= (l1 + l2) / 2.0; if (t2 < 0.0) { vec0.v1 = -vec0.v1; vec0.v2 = -vec0.v2; vec0.v3 = -vec0.v3; vec1.v1 = -vec1.v1; vec1.v2 = -vec1.v2; vec1.v3 = -vec1.v3; t0 = -t0; t1 = -t1; t1 = -t2; } // ここまで if (!NyARRotVector.checkRotation(vec0, vec1)) { return(false); } double v20 = vec0.v2 * vec1.v3 - vec0.v3 * vec1.v2; double v21 = vec0.v3 * vec1.v1 - vec0.v1 * vec1.v3; double v22 = vec0.v1 * vec1.v2 - vec0.v2 * vec1.v1; l1 = Math.Sqrt(v20 * v20 + v21 * v21 + v22 * v22); v20 /= l1; v21 /= l1; v22 /= l1; initMatXw2Xc.m00 = vec0.v1; initMatXw2Xc.m10 = vec0.v2; initMatXw2Xc.m20 = vec0.v3; initMatXw2Xc.m01 = vec1.v1; initMatXw2Xc.m11 = vec1.v2; initMatXw2Xc.m21 = vec1.v3; initMatXw2Xc.m02 = v20; initMatXw2Xc.m12 = v21; initMatXw2Xc.m22 = v22; initMatXw2Xc.m03 = t0; initMatXw2Xc.m13 = t1; initMatXw2Xc.m23 = t2; initMatXw2Xc.m30 = initMatXw2Xc.m31 = initMatXw2Xc.m32 = 0; initMatXw2Xc.m33 = 1; icpGetInitXw2XcSub(initMatXw2Xc, screenCoord, worldCoord, i_num, initMatXw2Xc); return(true); }
/** * {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。 */ private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat) { double m02 = 0; double m12 = 0; double m20 = 0; double m21 = 0; double m22 = 0; for (int i = 0; i < i_num; i++) { double cx = i_cp.m02 - pos2d[i].x; double cy = i_cp.m12 - pos2d[i].y; m02 += (cx) * i_cp.m00; m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11; m20 += i_cp.m00 * (cx); m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy); m22 += (cx) * (cx) + (cy) * (cy); } o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num; o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num; o_mat.m02 = m02; o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num; o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num; o_mat.m12 = m12; o_mat.m20 = m20; o_mat.m21 = m21; o_mat.m22 = m22; o_mat.inverse(o_mat); return o_mat; }
/** * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。 * 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; }
public static void ToCameraViewRH(NyARDoubleMatrix44 mat, double i_scale, ref NyARDoubleMatrix44 o_mat) { o_mat.m00 = (float)-mat.m00; o_mat.m01 = (float)mat.m01; o_mat.m02 = (float)mat.m02; o_mat.m10 = (float)mat.m10; o_mat.m11 = (float)-mat.m11; o_mat.m12 = (float)-mat.m12; o_mat.m20 = (float)-mat.m20; o_mat.m21 = (float)mat.m21; o_mat.m22 = (float)mat.m22; o_mat.m30 = (float)0.0; o_mat.m31 = (float)0.0; o_mat.m32 = (float)0.0; double scale = 1 / i_scale; o_mat.m03 = (float)(mat.m03 * scale); o_mat.m13 = -(float)(mat.m13 * scale); o_mat.m23 = (float)(mat.m23 * scale); o_mat.m33 = (float)1.0; return; }
/** * * @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); }
/** * カメラ座標系の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); }