Beispiel #1
0
            /**
             * icpGetDeltaS関数です。
             * @param dU
             * @param J_U_S
             * @param n
             * @return
             * @throws NyARException
             */
            public bool setJusArray(NyARIcpUtils.JusStack i_jus)
            {
                i_jus.makeJtJ(this.__matJtJ);
                double[] JtU = i_jus.makeJtU(this.__JtU);

                if (!this.__matJtJ.inverse())
                {
                    return(false);
                }
                double[][] matJtJb = this.__matJtJ.getArray();
                this.s0 = matJtJb[0][0] * JtU[0] + matJtJb[0][1] * JtU[1] + matJtJb[0][2] * JtU[2] + matJtJb[0][3] * JtU[3] + matJtJb[0][4] * JtU[4] + matJtJb[0][5] * JtU[5];
                this.s1 = matJtJb[1][0] * JtU[0] + matJtJb[1][1] * JtU[1] + matJtJb[1][2] * JtU[2] + matJtJb[1][3] * JtU[3] + matJtJb[1][4] * JtU[4] + matJtJb[1][5] * JtU[5];
                this.s2 = matJtJb[2][0] * JtU[0] + matJtJb[2][1] * JtU[1] + matJtJb[2][2] * JtU[2] + matJtJb[2][3] * JtU[3] + matJtJb[2][4] * JtU[4] + matJtJb[2][5] * JtU[5];
                this.s3 = matJtJb[3][0] * JtU[0] + matJtJb[3][1] * JtU[1] + matJtJb[3][2] * JtU[2] + matJtJb[3][3] * JtU[3] + matJtJb[3][4] * JtU[4] + matJtJb[3][5] * JtU[5];
                this.s4 = matJtJb[4][0] * JtU[0] + matJtJb[4][1] * JtU[1] + matJtJb[4][2] * JtU[2] + matJtJb[4][3] * JtU[3] + matJtJb[4][4] * JtU[4] + matJtJb[4][5] * JtU[5];
                this.s5 = matJtJb[5][0] * JtU[0] + matJtJb[5][1] * JtU[1] + matJtJb[5][2] * JtU[2] + matJtJb[5][3] * JtU[3] + matJtJb[5][4] * JtU[4] + matJtJb[5][5] * JtU[5];
                return(true);
            }
Beispiel #2
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);
        }