コード例 #1
0
        private double optimize(NyARRotMatrix io_rotmat, NyARDoublePoint3d io_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold)
        {
            //System.out.println("START");
            NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
            //初期のエラー値を計算
            double             min_err = errRate(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
            NyARDoubleMatrix33 rot     = this.__rot;

            rot.setValue(io_rotmat);
            for (int i = 0; i < 5; i++)
            {
                //変換行列の最適化
                this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);
                double err = errRate(rot, io_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, io_transvec);
                io_rotmat.setValue(rot);
                min_err = err;
            }
            //System.out.println("END");
            return(min_err);
        }
コード例 #2
0
 /**
  * この関数は、コンストラクタから呼び出してください。
  * @param i_distfactor
  * 歪みの逆矯正に使うオブジェクト。
  * @param i_projmat
  * @
  */
 private void initInstance(INyARCameraDistortionFactor i_distfactor, NyARPerspectiveProjectionMatrix i_projmat)
 {
     this._transsolver = new NyARTransportVectorSolver(i_projmat, 4);
     //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
     //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
     this._rotmatrix          = new NyARRotMatrix(i_projmat);
     this._mat_optimize       = new NyARPartialDifferentiationOptimize(i_projmat);
     this._ref_dist_factor    = i_distfactor;
     this._ref_projection_mat = i_projmat;
     return;
 }
コード例 #3
0
 /**
  * この関数は、コンストラクタから呼び出してください。
  * @param i_distfactor
  * 歪みの逆矯正に使うオブジェクト。
  * @param i_projmat
  * @
  */
 private void initInstance(INyARCameraDistortionFactor i_distfactor, NyARPerspectiveProjectionMatrix i_projmat)
 {
     this._transsolver = new NyARTransportVectorSolver(i_projmat, 4);
     //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
     //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
     this._rotmatrix = new NyARRotMatrix(i_projmat);
     this._mat_optimize = new NyARPartialDifferentiationOptimize(i_projmat);
     this._ref_dist_factor = i_distfactor;
     this._ref_projection_mat = i_projmat;
     return;
 }
コード例 #4
0
 public NyARTransMat(NyARParam i_param)
 {
     NyARCameraDistortionFactor dist = i_param.getDistortionFactor();
     NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix();
     this._transsolver = new NyARTransportVectorSolver(pmat, 4);
     //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
     //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
     this._rotmatrix = new NyARRotMatrix(pmat);
     this._mat_optimize = new NyARPartialDifferentiationOptimize(pmat);
     this._ref_dist_factor = dist;
     this._projection_mat_ref = pmat;
 }
コード例 #5
0
        public NyARTransMat(NyARParam i_param)
        {
            NyARCameraDistortionFactor      dist = i_param.getDistortionFactor();
            NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix();

            this._transsolver = new NyARTransportVectorSolver(pmat, 4);
            //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
            //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
            this._rotmatrix          = new NyARRotMatrix(pmat);
            this._mat_optimize       = new NyARPartialDifferentiationOptimize(pmat);
            this._ref_dist_factor    = dist;
            this._projection_mat_ref = pmat;
        }
コード例 #6
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;
        }
コード例 #7
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);
        }
コード例 #8
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);
        }
コード例 #9
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;
        }
コード例 #10
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;
        }