예제 #1
0
        /**
         * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
         *
         * @param i_square
         * 計算対象のNyARSquareオブジェクト
         * @param i_direction
         * @param i_width
         * @return
         * @throws NyARException
         */
        public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;

            double err_threshold = makeErrThreshold(i_square.sqvertex);

            //平行移動量計算機に、2D座標系をセット
            NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

            //回転後の3D座標系から、平行移動量を計算
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //計算結果の最適化(平行移動量と回転行列の最適化)
            o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);

            // マトリクスの保存
            this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            return;
        }
예제 #2
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * ARToolKitのarGetTransMatに該当します。
         * @see INyARTransMat#transMatContinue
         */
        public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans         = this.__transMat_trans;
            double            err_threshold = makeErrThreshold(i_square.sqvertex);

            NyARDoublePoint2d[] vertex_2d;
            if (this._ref_dist_factor != null)
            {
                //歪み復元必要
                vertex_2d = this.__transMat_vertex_2d;
                this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            }
            else
            {
                //歪み復元は不要
                vertex_2d = i_square.sqvertex;
            }
            //平行移動量計算機に、2D座標系をセット
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

            //回転後の3D座標系から、平行移動量を計算
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //計算結果の最適化(平行移動量と回転行列の最適化)
            this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv);
            return(true);
        }
        /**
         * This function retrieves bitmap from the area defined by RECT(i_l,i_t,i_r,i_b) above transform matrix i_base_mat. 
         * ----
         * この関数は、basementで示される平面のAで定義される領域から、ビットマップを読み出します。
         * 例えば、8cmマーカでRECT(i_l,i_t,i_r,i_b)に-40,0,0,-40.0を指定すると、マーカの左下部分の画像を抽出します。
         * 
         * マーカから離れた場所になるほど、また、マーカの鉛直方向から外れるほど誤差が大きくなります。
         * @param i_src_imege
         * 詠み出し元の画像を指定します。
         * @param i_l
         * 基準点からの左上の相対座標(x)を指定します。
         * @param i_t
         * 基準点からの左上の相対座標(y)を指定します。
         * @param i_r
         * 基準点からの右下の相対座標(x)を指定します。
         * @param i_b
         * 基準点からの右下の相対座標(y)を指定します。
         * @param i_base_mat
         * @return 画像の取得の成否を返す。
         */
        public bool pickupImage2d(INyARRgbRaster i_src_imege, double i_l, double i_t, double i_r, double i_b, NyARTransMatResult i_base_mat)
        {
            double cp00, cp01, cp02, cp11, cp12;
            cp00 = this._ref_perspective.m00;
            cp01 = this._ref_perspective.m01;
            cp02 = this._ref_perspective.m02;
            cp11 = this._ref_perspective.m11;
            cp12 = this._ref_perspective.m12;
            //マーカと同一平面上にある矩形の4個の頂点を座標変換して、射影変換して画面上の
            //頂点を計算する。
            //[hX,hY,h]=[P][RT][x,y,z]

            //出力先
            NyARIntPoint2d[] poinsts = this._work_points;

            double yt0, yt1, yt2;
            double x3, y3, z3;

            double m00 = i_base_mat.m00;
            double m10 = i_base_mat.m10;
            double m20 = i_base_mat.m20;

            //yとtの要素を先に計算
            yt0 = i_base_mat.m01 * i_t + i_base_mat.m03;
            yt1 = i_base_mat.m11 * i_t + i_base_mat.m13;
            yt2 = i_base_mat.m21 * i_t + i_base_mat.m23;
            // l,t
            x3 = m00 * i_l + yt0;
            y3 = m10 * i_l + yt1;
            z3 = m20 * i_l + yt2;
            poinsts[0].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
            poinsts[0].y = (int)((y3 * cp11 + z3 * cp12) / z3);
            // r,t
            x3 = m00 * i_r + yt0;
            y3 = m10 * i_r + yt1;
            z3 = m20 * i_r + yt2;
            poinsts[1].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
            poinsts[1].y = (int)((y3 * cp11 + z3 * cp12) / z3);

            //yとtの要素を先に計算
            yt0 = i_base_mat.m01 * i_b + i_base_mat.m03;
            yt1 = i_base_mat.m11 * i_b + i_base_mat.m13;
            yt2 = i_base_mat.m21 * i_b + i_base_mat.m23;

            // r,b
            x3 = m00 * i_r + yt0;
            y3 = m10 * i_r + yt1;
            z3 = m20 * i_r + yt2;
            poinsts[2].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
            poinsts[2].y = (int)((y3 * cp11 + z3 * cp12) / z3);
            // l,b
            x3 = m00 * i_l + yt0;
            y3 = m10 * i_l + yt1;
            z3 = m20 * i_l + yt2;
            poinsts[3].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
            poinsts[3].y = (int)((y3 * cp11 + z3 * cp12) / z3);
            return this.pickFromRaster(i_src_imege, poinsts);
        }
예제 #4
0
        /**
         * NyARTransMatResultの内容からNyARRotMatrixを復元します。
         * @param i_prev_result
         */
        public virtual void initRotByPrevResult(NyARTransMatResult 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;
        }
예제 #5
0
        /**
         * NyARTransMatResultの内容からNyARRotMatrixを復元します。
         * @param i_prev_result
         */
        public virtual void initRotByPrevResult(NyARTransMatResult 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;
        }
예제 #6
0
        public void Test_arDetectMarkerLite()
        {
            Assembly assembly = Assembly.GetExecutingAssembly();
            
            //AR用カメラパラメタファイルをロード
            NyARParam ap = new NyARParam();
            ap.loadARParam(assembly.GetManifestResourceStream(RES_CAMERA));
            ap.changeScreenSize(320, 240);

            //AR用のパターンコードを読み出し	
            NyARCode code = new NyARCode(16, 16);
            Stream sr1=assembly.GetManifestResourceStream(RES_PATT);
            code.loadARPatt(new StreamReader(sr1));

            //試験イメージの読み出し(320x240 BGRAのRAWデータ)
            StreamReader sr = new StreamReader(assembly.GetManifestResourceStream(RES_DATA));
            BinaryReader bs = new BinaryReader(sr.BaseStream);
            byte[] raw = bs.ReadBytes(320 * 240 * 4);
            NyARRgbRaster_BGRA ra = new NyARRgbRaster_BGRA(320, 240,false);
            ra.wrapBuffer(raw);
            //		Blank_Raster ra=new Blank_Raster(320, 240);

            //1パターンのみを追跡するクラスを作成
//            NyARSingleDetectMarker_Quad ar = new NyARSingleDetectMarker_Quad(ap, code, 80.0);
            NyARSingleDetectMarker ar = new NyARSingleDetectMarker(ap, code, 80.0,ra.getBufferType());
            NyARTransMatResult result_mat = new NyARTransMatResult();
            ar.setContinueMode(false);
            ar.detectMarkerLite(ra, 100);
            ar.getTransmationMatrix(result_mat);

            //マーカーを検出
            for (int i3 = 0; i3 < 10; i3++)
            {
                Stopwatch sw = new Stopwatch();
                sw.Start();
                for (int i = 0; i < 10; i++)
                {
                    //変換行列を取得
                    ar.detectMarkerLite(ra, 100);
                    ar.getTransmationMatrix(result_mat);
                }
                sw.Stop();
                Debug.WriteLine(sw.ElapsedMilliseconds + "[ms]");
            }
            return;
        }
 /**
  * この関数は、検出したマーカーの変換行列を計算して、o_resultへ値を返します。
  * 直前に実行した{@link #detectMarkerLite}が成功していないと使えません。
  * @param o_result
  * 変換行列を受け取るオブジェクト。
  * @
  */
 public void getTransmat(NyARTransMatResult o_result)
 {
     // 一番一致したマーカーの位置とかその辺を計算
     if (this._is_continue)
     {
         this._transmat.transMatContinue(this._square, this._offset, o_result, o_result);
     }
     else
     {
         this._transmat.transMat(this._square, this._offset, o_result);
     }
     return;
 }
      /// <summary>
      /// Listener method called when something was detected.
      /// </summary>
      /// <param name="callingDetector">The detector that called the method.</param>
      /// <param name="coordsX">The four x coordinates of the detected marker square.</param>
      /// <param name="coordsY">The four y coordinates of the detected marker square.</param>
      /// <param name="coordCount">The number of coordinates.</param>
      /// <param name="coordIndices">The indices of the coordiantes in the coords array.</param>
      public void onSquareDetect(NyARSquareContourDetector callingDetector, int[] coordsX, int[] coordsY, int coordCount, int[] coordIndices)
      {
         // Init variables            
         points[0].x = coordsX[coordIndices[0]];
         points[0].y = coordsY[coordIndices[0]];
         points[1].x = coordsX[coordIndices[1]];
         points[1].y = coordsY[coordIndices[1]];
         points[2].x = coordsX[coordIndices[2]];
         points[2].y = coordsY[coordIndices[2]];
         points[3].x = coordsX[coordIndices[3]];
         points[3].y = coordsY[coordIndices[3]];

         // Evaluate and find best match
         if (this.colorPattern.pickFromRaster(this.Buffer, points))
         {
            // Find best matching marker
            this.patternMatchDeviationData.setRaster(this.colorPattern);
            Marker foundMarker = null;
            int foundDirection = NyARMatchPattResult.DIRECTION_UNKNOWN;
            double bestConfidence = 0;

            foreach (var patMat in patternMatchers)
            {
               // Evaluate
               patMat.evaluate(this.patternMatchDeviationData, evaluationResult);

               // Best match?
               if (evaluationResult.confidence > bestConfidence)
               {
                  foundMarker = patMat.Marker;
                  foundDirection = evaluationResult.direction;
                  bestConfidence = evaluationResult.confidence;
               }
            }

            // Calculate found marker square
            var square = new NyARSquare();
            var len = coordIndices.Length;
            for (int i = 0; i < len; i++)
            {
               int idx = (i + len - foundDirection) % len;
               this.coordinationMapper.coord2Line(coordIndices[idx], coordIndices[(idx + 1) % len], coordsX, coordsY, coordCount, square.line[i]);
            }
            // Calculate normal
            for (int i = 0; i < len; i++)
            {
               NyARLinear.crossPos(square.line[i], square.line[(i + 3) % len], square.sqvertex[i]);
            }

            // Calculate matrix using continued mode
            if (foundMarker != null)
            {
               var nymat = new NyARTransMatResult();
               matrixCalculator.transMatContinue(square, foundMarker.RectOffset, nymat);

               // Create and add result to collection
               var v = square.sqvertex;
               var resultSquare = new Square(v[0].x, v[0].y, v[1].x, v[1].y, v[2].x, v[2].y, v[3].x, v[3].y);
               Results.Add(new DetectionResult(foundMarker, bestConfidence, nymat.ToMatrix3D(), resultSquare));
            }
         }
      }
예제 #9
0
        /**
         * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
         * 
         * @param i_square
         * 計算対象のNyARSquareオブジェクト
         * @param i_direction
         * @param i_width
         * @return
         * @throws NyARException
         */
        public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;

            double err_threshold = makeErrThreshold(i_square.sqvertex);

            //平行移動量計算機に、2D座標系をセット
            NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

            //回転後の3D座標系から、平行移動量を計算
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //計算結果の最適化(平行移動量と回転行列の最適化)
            o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);

            // マトリクスの保存
            this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            return;
        }
예제 #10
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 void optimize(NyARRotMatrix iw_rotmat, NyARDoublePoint3d iw_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold, NyARTransMatResult 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, min_err);

            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, err);
                min_err = err;
            }
            //System.out.println("END");
            return;
        }
예제 #11
0
        /**
         * パラメータで変換行列を更新します。
         *
         * @param i_rot
         * @param i_off
         * @param i_trans
         */
        public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
        {
            o_result.m00 = i_rot.m00;
            o_result.m01 = i_rot.m01;
            o_result.m02 = i_rot.m02;
            o_result.m03 = i_trans.x;

            o_result.m10 = i_rot.m10;
            o_result.m11 = i_rot.m11;
            o_result.m12 = i_rot.m12;
            o_result.m13 = i_trans.y;

            o_result.m20 = i_rot.m20;
            o_result.m21 = i_rot.m21;
            o_result.m22 = i_rot.m22;
            o_result.m23 = i_trans.z;

            o_result.has_value = true;
            return;
        }
예제 #12
0
        /*
         * (non-Javadoc)
         * @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)
         */
        public void transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;

            // io_result_convが初期値なら、transMatで計算する。
            if (!o_result_conv.has_value)
            {
                this.transMat(i_square, i_offset, o_result_conv);
                return;
            }

            //最適化計算の閾値を決定
            double err_threshold = makeErrThreshold(i_square.sqvertex);


            //平行移動量計算機に、2D座標系をセット
            NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotByPrevResult(o_result_conv);

            //回転後の3D座標系から、平行移動量を計算
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //現在のエラーレートを計算しておく
            double             min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
            NyARDoubleMatrix33 rot     = this.__rot;

            //エラーレートが前回のエラー値より閾値分大きかったらアゲイン
            if (min_err < o_result_conv.error + err_threshold)
            {
                rot.setValue(this._rotmatrix);
                //最適化してみる。
                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);
                    this._rotmatrix.setValue(rot);
                    min_err = err;
                }
                this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            }
            else
            {
                //回転行列を計算
                this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

                //回転後の3D座標系から、平行移動量を計算
                this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
                this._transsolver.solveTransportVector(vertex_3d, trans);

                //計算結果の最適化(平行移動量と回転行列の最適化)
                min_err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
                this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            }
            o_result_conv.error = min_err;
            return;
        }
예제 #13
0
        /**
         * パラメータで変換行列を更新します。
         * 
         * @param i_rot
         * @param i_off
         * @param i_trans
         */
        public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
        {
            o_result.m00 = i_rot.m00;
            o_result.m01 = i_rot.m01;
            o_result.m02 = i_rot.m02;
            o_result.m03 = i_trans.x;

            o_result.m10 = i_rot.m10;
            o_result.m11 = i_rot.m11;
            o_result.m12 = i_rot.m12;
            o_result.m13 = i_trans.y;

            o_result.m20 = i_rot.m20;
            o_result.m21 = i_rot.m21;
            o_result.m22 = i_rot.m22;
            o_result.m23 = i_trans.z;

            o_result.has_value = true;
            return;
        }
예제 #14
0
        /*
         * (non-Javadoc)
         * @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)
         */
        public void transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;

            // io_result_convが初期値なら、transMatで計算する。
            if (!o_result_conv.has_value)
            {
                this.transMat(i_square, i_offset, o_result_conv);
                return;
            }

            //最適化計算の閾値を決定
            double err_threshold = makeErrThreshold(i_square.sqvertex);


            //平行移動量計算機に、2D座標系をセット
            NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotByPrevResult(o_result_conv);

            //回転後の3D座標系から、平行移動量を計算
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //現在のエラーレートを計算しておく
            double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
            NyARDoubleMatrix33 rot = this.__rot;
            //エラーレートが前回のエラー値より閾値分大きかったらアゲイン
            if (min_err < o_result_conv.error + err_threshold)
            {
                rot.setValue(this._rotmatrix);
                //最適化してみる。
                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);
                    this._rotmatrix.setValue(rot);
                    min_err = err;
                }
                this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            }
            else
            {
                //回転行列を計算
                this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

                //回転後の3D座標系から、平行移動量を計算
                this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
                this._transsolver.solveTransportVector(vertex_3d, trans);

                //計算結果の最適化(平行移動量と回転行列の最適化)
                min_err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
                this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
            }
            o_result_conv.error = min_err;
            return;
        }
 /**
  * @deprecated
  * {@link #getTransmat}
  */
 public void getTransmationMatrix(NyARTransMatResult o_result)
 {
     this.getTransmat(o_result);
     return;
 }
 protected abstract void onUpdateHandler(NyARSquare i_square, NyARTransMatResult result);
예제 #17
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * ARToolKitのarGetTransMatに該当します。
         * @see INyARTransMat#transMatContinue
         */
        public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;
            double err_threshold = makeErrThreshold(i_square.sqvertex);

            NyARDoublePoint2d[] vertex_2d;
            if (this._ref_dist_factor != null)
            {
                //歪み復元必要
                vertex_2d = this.__transMat_vertex_2d;
                this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
            }
            else
            {
                //歪み復元は不要
                vertex_2d = i_square.sqvertex;
            }
            //平行移動量計算機に、2D座標系をセット
            this._transsolver.set2dVertex(vertex_2d, 4);

            //回転行列を計算
            this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);

            //回転後の3D座標系から、平行移動量を計算
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
            this._transsolver.solveTransportVector(vertex_3d, trans);

            //計算結果の最適化(平行移動量と回転行列の最適化)
            this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv);
            return true;
        }
예제 #18
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
         * @see INyARTransMat#transMatContinue
         */
        public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult i_prev_result, NyARTransMatResult o_result)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;

            // io_result_convが初期値なら、transMatで計算する。
            if (!i_prev_result.has_value)
            {
                this.transMat(i_square, i_offset, o_result);
                return(true);
            }
            //過去のエラーレートを記録(ここれやるのは、i_prev_resultとo_resultに同じインスタンスを指定できるようにするため)
            double last_error = i_prev_result.last_error;

            //最適化計算の閾値を決定
            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(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);

            //結果をストア
            o_result.setValue(rot, trans, min_err);
            //エラーレートの判定
            if (min_err < last_error + err_threshold)
            {
                //			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, err);
                    min_err = err;
                }
            }
            else
            {
                //			System.out.println("TR:again");
                //回転行列を計算
                rot.initRotBySquare(i_square.line, i_square.sqvertex);

                //回転後の3D座標系から、平行移動量を計算
                rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
                this._transsolver.solveTransportVector(vertex_3d, trans);

                //計算結果の最適化(平行移動量と回転行列の最適化)
                this.optimize(rot, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
            }
            return(true);
        }
예제 #19
0
        /**
         * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
         * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
         * @see INyARTransMat#transMatContinue
         */
        public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult i_prev_result, NyARTransMatResult o_result)
        {
            NyARDoublePoint3d trans = this.__transMat_trans;
            // io_result_convが初期値なら、transMatで計算する。
            if (!i_prev_result.has_value)
            {
                this.transMat(i_square, i_offset, o_result);
                return true;
            }
            //過去のエラーレートを記録(ここれやるのは、i_prev_resultとo_resultに同じインスタンスを指定できるようにするため)
            double last_error = i_prev_result.last_error;

            //最適化計算の閾値を決定
            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(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
            //結果をストア
            o_result.setValue(rot, trans, min_err);
            //エラーレートの判定
            if (min_err < last_error + err_threshold)
            {
                //			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, err);
                    min_err = err;
                }
            }
            else
            {
                //			System.out.println("TR:again");
                //回転行列を計算
                rot.initRotBySquare(i_square.line, i_square.sqvertex);

                //回転後の3D座標系から、平行移動量を計算
                rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
                this._transsolver.solveTransportVector(vertex_3d, trans);

                //計算結果の最適化(平行移動量と回転行列の最適化)
                this.optimize(rot, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
            }
            return true;
        }
예제 #20
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 void optimize(NyARRotMatrix iw_rotmat, NyARDoublePoint3d iw_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold, NyARTransMatResult 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, min_err);

            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, err);
                min_err = err;
            }
            //System.out.println("END");
            return;
        }
예제 #21
0
        private void UpdateMarkerTransforms()
        {
            // Reset all marker patterns to not found
            foreach (MarkerInfo markerInfo in markerInfoList)
            {
                markerInfo.IsFound = false;
                foreach (PatternInfo patternInfo in markerInfo.PatternInfos.Values)
                    patternInfo.IsFound = false;
            }

            int numMarkerFound = multiDetector.detectMarkerLite(raster, threshold);
            NyARTransMatResult result = new NyARTransMatResult();

            for (int i = 0; i < numMarkerFound; ++i)
            {
                int id = multiDetector.getARCodeIndex(i);

                MarkerInfo markerInfo = markerInfoMap[id];

                if (multiDetector.getConfidence(i) >= markerInfo.PatternInfos[id].ConfidenceThreshold)
                {
                    try
                    {
                        markerInfo.IsFound = true;
                        markerInfo.PatternInfos[id].IsFound = true;
                        markerInfo.PatternInfos[id].Confidence = (float)multiDetector.getConfidence(i);
                        multiDetector.getTransmationMatrix(i, result);
                        markerInfo.PatternInfos[id].Transform = (new Matrix(
                            (float)result.m00, (float)result.m10, (float)result.m20, (float)result.m30,
                            (float)result.m01, (float)result.m11, (float)result.m21, (float)result.m31,
                            (float)result.m02, (float)result.m12, (float)result.m22, (float)result.m32,
                            (float)result.m03, (float)result.m13, (float)result.m23, (float)result.m33)) *
                             Matrix.CreateFromAxisAngle(Vector3.UnitX, MathHelper.Pi);
                    }
                    catch (NyARException exp)
                    {
                        Log.Write(exp.Message);

                        markerInfo.PatternInfos[id].IsFound = false;
                        markerInfo.PatternInfos[id].Confidence = 0;
                    }
                }
            }

            // Compute the final transformation of each marker info
            foreach (MarkerInfo markerInfo in markerInfoList)
            {
                if (markerInfo.IsFound)
                {
                    Matrix resultMat = MatrixHelper.Empty;

                    List<Matrix> transforms = null;
                    List<float> weights = null;

                    switch (markerInfo.Method)
                    {
                        case ComputationMethod.Average:
                            int count = 0;
                            foreach (int id in markerInfo.PatternInfos.Keys)
                            {
                                PatternInfo patternInfo = markerInfo.PatternInfos[id];
                                if (patternInfo.IsFound)
                                {
                                    tmpMat1 = markerInfo.RelativeTransforms[id];
                                    Matrix.Multiply(ref tmpMat1, ref patternInfo.Transform, out tmpMat2);
                                    Matrix.Add(ref resultMat, ref tmpMat2, out resultMat);
                                    count++;
                                }
                            }

                            resultMat /= count;
                            break;
                        case ComputationMethod.BestConfidence:
                            float bestConfidence = 0;
                            foreach (int id in markerInfo.PatternInfos.Keys)
                            {
                                PatternInfo patternInfo = markerInfo.PatternInfos[id];
                                if (patternInfo.IsFound && patternInfo.Confidence > bestConfidence)
                                {
                                    tmpMat1 = markerInfo.RelativeTransforms[id];
                                    Matrix.Multiply(ref tmpMat1, ref patternInfo.Transform, out resultMat);
                                    bestConfidence = patternInfo.Confidence;
                                }
                            }
                            break;
                        case ComputationMethod.WeightedAverage:
                            transforms = new List<Matrix>();
                            weights = new List<float>();
                            float weightSum = 0;

                            foreach (int id in markerInfo.PatternInfos.Keys)
                            {
                                PatternInfo patternInfo = markerInfo.PatternInfos[id];
                                if (patternInfo.IsFound)
                                {
                                    Matrix transform = markerInfo.RelativeTransforms[id];
                                    Matrix.Multiply(ref transform, ref patternInfo.Transform, out transform);
                                    transforms.Add(transform);
                                    weights.Add(patternInfo.Confidence);
                                    weightSum += patternInfo.Confidence;
                                }
                            }

                            for (int i = 0; i < weights.Count; ++i)
                                weights[i] /= weightSum;

                            for (int i = 0; i < transforms.Count; ++i)
                            {
                                tmpMat1 = transforms[i];
                                Matrix.Multiply(ref tmpMat1, weights[i], out tmpMat2);
                                Matrix.Add(ref resultMat, ref tmpMat2, out resultMat);
                            }

                            break;
                        case ComputationMethod.Custom:
                            if (ComputeTransform == null)
                                throw new MarkerException("For ComputationMethod.Custom method, you must set " +
                                    "the ComputeTransform property");

                            transforms = new List<Matrix>();
                            weights = new List<float>();

                            foreach (int id in markerInfo.PatternInfos.Keys)
                            {
                                PatternInfo patternInfo = markerInfo.PatternInfos[id];
                                if (patternInfo.IsFound)
                                {
                                    Matrix transform = markerInfo.RelativeTransforms[id];
                                    Matrix.Multiply(ref transform, ref patternInfo.Transform, out transform);
                                    transforms.Add(transform);
                                    weights.Add(patternInfo.Confidence);
                                }
                            }

                            ComputeTransform(transforms, weights, out resultMat);

                            break;
                    }

                    markerInfo.Transform = resultMat;
                }
            }
        }