Example #1
0
        private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
        {
            NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
            // ARdouble err;
            int i;

            if (matchData.getLength() < 4)
            {
                return(false);
            }
            NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
            NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
            for (i = 0; i < matchData.getLength(); i++)
            {
                sCoord[i].x = matchData.getItem(i).query.x;
                sCoord[i].y = matchData.getItem(i).query.y;

                wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
                wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
                wCoord[i].z = 0.0;
            }


            NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());

            if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
            {
                return(false);
            }

            /*
             * printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
             * initMatXw2Xc[j][i]); printf("\n"); }
             */
            NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());

            icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
            if (i_resultparam.last_error > 10.0f)
            {
                return(false);
            }
            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 #3
0
        /**
         * 4ポイント限定のHomographyPointsGeometricallyConsistent関数
         * @param H
         * @param i_width
         * @param i_height
         * @return
         */
        virtual public bool geometricallyConsistent(HomographyMat H)
        {
            NyARDoublePoint2d[] x = NyARDoublePoint2d.createArray(4);
            x[0].x = 0;
            x[0].y = 0;
            x[1].x = this._w;
            x[1].y = 0;
            x[2].x = this._w;
            x[2].y = this._h;
            x[3].x = 0;
            x[3].y = this._h;
            NyARDoublePoint2d xp1       = new NyARDoublePoint2d();
            NyARDoublePoint2d xp2       = new NyARDoublePoint2d();
            NyARDoublePoint2d xp3       = new NyARDoublePoint2d();
            NyARDoublePoint2d first_xp1 = new NyARDoublePoint2d();
            NyARDoublePoint2d first_xp2 = new NyARDoublePoint2d();


            int x1_ptr = 0;
            //        int x2_ptr = 0+1;
            //        int x3_ptr = 0+2;

            NyARDoublePoint2d xp1_ptr = xp1;
            NyARDoublePoint2d xp2_ptr = xp2;
            NyARDoublePoint2d xp3_ptr = xp3;

            //
            // Check the first 3 points
            //
            MultiplyPointHomographyInhomogenous(xp1, H, x[x1_ptr]);
            MultiplyPointHomographyInhomogenous(xp2, H, x[x1_ptr + 1]);
            MultiplyPointHomographyInhomogenous(xp3, H, x[x1_ptr + 2]);

            first_xp1.setValue(xp1); //indexing.CopyVector2(first_xp1,0, xp1,0);
            first_xp2.setValue(xp2); //indexing.CopyVector2(first_xp2,0, xp2,0);
            //      public boolean Homography4PointsGeometricallyConsistent(float[] x1, float[] x2, float[] x3, float[] x4,float[] x1p,float[] x2p,float[] x3p,float[] x4p) {


            if (!Homography3PointsGeometricallyConsistent(
                    x[x1_ptr], x[x1_ptr + 1], x[x1_ptr + 2],
                    xp1_ptr, xp2_ptr, xp3_ptr))
            {
                return(false);
            }

            //
            // Check the remaining points
            //

            for (int i = 3; i < x.Length; i++)
            {
                x1_ptr += 1;
                MultiplyPointHomographyInhomogenous(xp1_ptr, H, x[x1_ptr + 2]);

                NyARDoublePoint2d tmp_ptr = xp1_ptr;
                xp1_ptr = xp2_ptr;
                xp2_ptr = xp3_ptr;
                xp3_ptr = tmp_ptr;

                if (!Homography3PointsGeometricallyConsistent(
                        x[x1_ptr], x[x1_ptr + 1], x[x1_ptr + 2],
                        xp1_ptr, xp2_ptr, xp3_ptr))
                {
                    return(false);
                }
            }

            //
            // Check the last 3 points
            //

            if (!Homography3PointsGeometricallyConsistent(
                    x[x1_ptr + 1], x[x1_ptr + 2], x[0],
                    xp2_ptr, xp3_ptr, first_xp1))
            {
                return(false);
            }
            if (!Homography3PointsGeometricallyConsistent(
                    x[x1_ptr + 2], x[0], x[2],
                    xp3_ptr, first_xp1, first_xp2))
            {
                return(false);
            }

            return(true);
        }