Example #1
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);
        }
Example #2
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);
        }
        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;
        }
Example #4
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;
        }
Example #5
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;
        }
Example #6
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);
        }