Exemple #1
0
        /**
         * 変換行列と頂点座標から、パラメータを計算
         * 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;
        }
Exemple #2
0
        /**
         * この関数は、行列同士の掛け算をして、インスタンスに格納します。
         * 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;
        }
Exemple #3
0
        /**
         * コンストラクタです。
         * @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;
        }
Exemple #4
0
        /**
         * {@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);
        }
Exemple #5
0
        /**
         * {@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);
        }
Exemple #6
0
 /**
  * この関数は、オブジェクトの配列を生成して返します。
  * @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;
 }
Exemple #7
0
        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);
        }
Exemple #8
0
        /**
         * カメラ座標系の点を、スクリーン座標の点へ変換します。
         * @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;
 }
Exemple #10
0
        /**
         * 透視変換パラメータを行列から復元します。
         * @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);
        }
Exemple #11
0
            /**
             * 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);
            }
Exemple #12
0
        /**
         * この関数は、スクリーン座標を撮像点座標に変換します。
         * 撮像点の座標系は、カメラ座標系になります。
         * <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;
        }
Exemple #13
0
        /**
         * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
         * 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);
        }
Exemple #14
0
        /**
         * この関数は、{@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;
        }
Exemple #15
0
        /**
         * この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
         * この座標は、カメラ座標系です。
         * @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;
        }
Exemple #16
0
        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;
        }
 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;
 }
Exemple #18
0
            /**
             * 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;
            }
Exemple #19
0
        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;
        }
Exemple #20
0
        /**
         * この関数は、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);
        }
Exemple #21
0
        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;
        }
Exemple #22
0
 /**
  * この関数は、オブジェクトの内容をインスタンスにコピーします。
  * @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;
 }
Exemple #23
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 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);
        }
Exemple #24
0
            /**
             * 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);
            }
Exemple #25
0
 /**
  * {@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;
 }
Exemple #26
0
 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;
        }
Exemple #29
0
 /**
  * この関数は、視錐台行列をインスタンスにセットします。
  * @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);
 }
Exemple #30
0
 /**
  * この関数は、カメラパラメータから右手系の視錐台を作ります。
  * <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;
 }
 /**
  * この関数は、オブジェクトの内容をインスタンスにコピーします。
  * @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;
 }
Exemple #32
0
        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;
 }
Exemple #34
0
        /**
         *
         * @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;
        }
Exemple #35
0
 public NyARIcpPlane(NyARParam i_param)
 {
     this._cparam = i_param.getPerspectiveProjectionMatrix();
 }
Exemple #36
0
 /**
  * この関数は、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;
 }
Exemple #37
0
 /**
  * この関数は、スクリーン上の点と原点を結ぶ直線と、任意姿勢の平面の交差点を、カメラの座標系で取得します。
  * この座標は、カメラ座標系です。
  * @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;
 }
Exemple #38
0
 /**
  * この関数は、視錐台行列をインスタンスにセットします。
  * @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);
 }
Exemple #39
0
        /**
         *
         * @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);
        }
Exemple #40
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 計算に過去の履歴を使う点が、{@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);
Exemple #42
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 計算に過去の履歴を使う点が、{@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);
 }
Exemple #44
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 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;
 }
Exemple #46
0
        /**
         * この関数は、逆行列を計算して、インスタンスにセットします。
         * @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;
        }
Exemple #47
0
        /**
         * この関数は、{@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;
        }
        /**
         * 右手系の視錐台を作ります。
         * この視錐台は、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;
        }
        /**
         * この関数は、行列同士の掛け算をして、インスタンスに格納します。
         * 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;
        }
Exemple #50
0
        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_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;
 }
Exemple #52
0
        /**
         * 右手系の視錐台を作ります。
         * この視錐台は、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;
        }
Exemple #53
0
        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);
        }
Exemple #54
0
 /**
  * {@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;
 }
Exemple #55
0
 /**
  * 画面上の点と原点を結ぶ直線と任意姿勢の平面の交差点を、平面の座標系で取得します。
  * 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;
 }
Exemple #56
0
        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;
        }
Exemple #57
0
            /**
             *
             * @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);
            }
Exemple #58
0
 /**
  * カメラ座標系の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);
 }