Example #1
0
        /**
         * CheckHomographyHeuristics
         * Check if a homography is valid based on some heuristics.
         * @param i_h_inv
         * inversed homography matrix;
         * @param refWidth
         * @param refHeight
         * @return
         */
        virtual public bool checkHomographyHeuristics(int refWidth, int refHeight)
        {
            NyARDoublePoint2d p0p = new NyARDoublePoint2d();
            NyARDoublePoint2d p1p = new NyARDoublePoint2d();
            NyARDoublePoint2d p2p = new NyARDoublePoint2d();
            NyARDoublePoint2d p3p = new NyARDoublePoint2d();


            this.multiplyPointHomographyInhomogenous(0, 0, p0p);
            this.multiplyPointHomographyInhomogenous(refWidth, 0, p1p);
            this.multiplyPointHomographyInhomogenous(refWidth, refHeight, p2p);
            this.multiplyPointHomographyInhomogenous(0, refHeight, p3p);

            double tr = refWidth * refHeight * 0.0001f;

            if (SmallestTriangleArea(p0p, p1p, p2p, p3p) < tr)
            {
                return(false);
            }

            if (!QuadrilateralConvex(p0p, p1p, p2p, p3p))
            {
                return(false);
            }
            return(true);
        }
Example #2
0
        private void multiplyPointHomographyInhomogenous(double i_x, double i_y, NyARDoublePoint2d i_dest)
        {
            double w = this.m20 * i_x + this.m21 * i_y + this.m22;

            i_dest.x = (this.m00 * i_x + this.m01 * i_y + this.m02) / w; // XP
            i_dest.y = (this.m10 * i_x + this.m11 * i_y + this.m12) / w; // YP
        }
Example #3
0
        /**
         * この関数は、点が矩形の範囲内にあるか判定します。
         * @param i_pos
         * 調査する座標
         * @return
         * 点が矩形の中にあれば、trueを返します。
         */
        public bool isInnerPoint(NyARDoublePoint2d i_pos)
        {
            int x = (int)i_pos.x - this.x;
            int y = (int)i_pos.y - this.y;

            return(0 <= x && x < this.w && 0 <= y && y < this.h);
        }
Example #4
0
        /**
         * Compute the area of a triangle.
         */
        private static double AreaOfTriangle(NyARDoublePoint2d u, NyARDoublePoint2d v)
        {
            //		T a = u[0]*v[1] - u[1]*v[0];
            double a = u.x * v.y - u.y * v.x;

            return((double)(Math.Abs(a) * 0.5));
        }
        private bool solveHomography4PointsInhomogenous(NyARDoubleMatrix33 i_homography_mat,
                                                        NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4,
                                                        NyARDoublePoint2d xp1, NyARDoublePoint2d xp2, NyARDoublePoint2d xp3, NyARDoublePoint2d xp4)
        {
            double[][] _mat_A = ArrayUtils.newDouble2dArray(8, 9);

//		x1.setValue(0, 0);x2.setValue(10, 0);x3.setValue(10, 10);x4.setValue(0, 10);
//		xp1.setValue(10, 10);xp2.setValue(10, 0);xp3.setValue(0, 0);xp4.setValue(0, 10);

            //Homography4PointsInhomogeneousConstraint
            AddHomographyPointContraint(_mat_A, 0, x1, xp1);
            AddHomographyPointContraint(_mat_A, 2, x2, xp2);
            AddHomographyPointContraint(_mat_A, 4, x3, xp3);
            AddHomographyPointContraint(_mat_A, 6, x4, xp4);
            //SolveHomography4PointsInhomogenous
            if (!this.solveNullVector8x9Destructive(i_homography_mat, _mat_A))
            {
                return(false);
            }
            if (Math.Abs(i_homography_mat.determinant()) < 1e-5)
            {
                return(false);
            }
            return(true);
        }
Example #6
0
        /**
         * この関数は、頂点群から最小二乗法を使用して直線を計算します。
         * @param i_points
         * 頂点群を格納した配列。
         * @param i_number_of_data
         * 計算対象の頂点群の数
         * @return
         * 計算に成功すると、trueを返します。
         */
        public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data)
        {
            int    i;
            double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;

            for (i = 0; i < i_number_of_data; i++)
            {
                NyARDoublePoint2d ptr = i_points[i];
                double            xw  = ptr.x;
                sum_xy += xw * ptr.y;
                sum_x  += xw;
                sum_y  += ptr.y;
                sum_x2 += xw * xw;
            }
            double la = -(i_number_of_data * sum_x2 - sum_x * sum_x);
            double lb = -(i_number_of_data * sum_xy - sum_x * sum_y);
            double cc = (sum_x2 * sum_y - sum_xy * sum_x);
            double lc = -(la * sum_x + lb * sum_y) / i_number_of_data;
            //交点を計算
            double w1 = -lb * lb - la * la;

            if (w1 == 0.0)
            {
                return(false);
            }
            this.x  = ((la * lc - lb * cc) / w1);
            this.y  = ((la * cc + lb * lc) / w1);
            this.dy = -lb;
            this.dx = -la;
            return(true);
        }
Example #7
0
        /**
         * 基準点のdpiを推定する。
         * @param i_cptrans
         * 射影変換行列. [cparam]*[trans]
         * @param trans
         * @param pos
         * [2]
         * @param o_dpi
         * x,y方向それぞれの推定dpi
         * @return
         */
        public void ar2GetResolution2d(NyARNftFsetFile.NyAR2FeatureCoord i_pos, NyARDoublePoint2d o_dpi)
        {
            NyARDoubleMatrix44 t = this.ctrans;
            //基点
            double mx0 = i_pos.mx;
            double my0 = i_pos.my;
            double h0  = t.m20 * mx0 + t.m21 * my0 + t.m23;
            double hx0 = t.m00 * mx0 + t.m01 * my0 + t.m03;
            double hy0 = t.m10 * mx0 + t.m11 * my0 + t.m13;
            double x0  = hx0 / h0;
            double y0  = hy0 / h0;

            double h, sx, sy;

            //+X
            h  = h0 + t.m20 * DPI_BOX;
            sx = ((hx0 + DPI_BOX * t.m00) / h) - x0;
            sy = ((hy0 + DPI_BOX * t.m10) / h) - y0;
            //dpi -x
            o_dpi.x = Math.Sqrt(sx * sx + sy * sy) * 2.54;

            //+Y
            h  = h0 + t.m21 * DPI_BOX;
            sx = ((hx0 + DPI_BOX * t.m01) / h) - x0;
            sy = ((hy0 + DPI_BOX * t.m11) / h) - y0;

            //dpi -y
            o_dpi.y = Math.Sqrt(sx * sx + sy * sy) * 2.54;
            return;
        }
Example #8
0
        private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
        {
            double v0, v1, v2, v3, v4, v5, v6, v7;
            v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
            for (int i = 0; i < i_num; i++)
            {
                double wx = worldCoord[i].x;
                double wy = worldCoord[i].y;
                double sx = screenCoord[i].x;
                double sy = screenCoord[i].y;
                v0 += wx * sx;
                v1 += wy * sx;
                v2 += sx;
                v3 += wx * sy;
                v4 += wy * sy;
                v5 += sy;
                v6 += -wx * sx * sx - wx * sy * sy;
                v7 += -wy * sx * sx - wy * sy * sy;
            }
            double[][] t = o_matAtB.getArray();
            t[0][0] = v0;
            t[1][0] = v1;
            t[2][0] = v2;
            t[3][0] = v3;
            t[4][0] = v4;
            t[5][0] = v5;
            t[6][0] = v6;
            t[7][0] = v7;
            return o_matAtB;

        }
Example #9
0
 /**
  * p2-p1ベクトルのsquare normを計算する。
  * @param i_p1
  * @param i_p2
  * @return
  */
 public static double sqNorm(NyARDoublePoint2d i_p1, NyARDoublePoint2d i_p2)
 {
     double x, y;
     x = i_p2.x - i_p1.x;
     y = i_p2.y - i_p1.y;
     return x * x + y * y;
 }
Example #10
0
        /**
         * Multiply an in-homogenous point by a similarity.
         */
        private static void MultiplyPointHomographyInhomogenous(NyARDoublePoint2d xp, NyARDoubleMatrix33 H, NyARDoublePoint2d x)
        {
            double w = H.m20 * x.x + H.m21 * x.y + H.m22;

            xp.x = (H.m00 * x.x + H.m01 * x.y + H.m02) / w;
            xp.y = (H.m10 * x.x + H.m11 * x.y + H.m12) / w;
        }
Example #11
0
        /**
         * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
         * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
         * 線分と直線の類似度を判定する数値になります。
         * @param i_sp1
         * 線分の端点1
         * @param i_sp2
         * 線分の端点2
         * @return
         * 二乗距離値の合計。距離が取れないときは無限大です。
         */
        public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
        {
            double sa, sb, sc;

            sa = this.dy;
            sb = -this.dx;
            sc = (this.dx * this.y - this.dy * this.x);


            double lc;
            double x, y, w1;

            //thisを法線に変換

            //交点を計算
            w1 = sa * (-sa) - sb * sb;
            if (w1 == 0.0)
            {
                return(Double.PositiveInfinity);
            }
            //i_sp1と、i_linerの交点
            lc = -(sb * i_sp1.x - sa * i_sp1.y);
            x  = ((sb * lc + sa * sc) / w1) - i_sp1.x;
            y  = ((sb * sc - sa * lc) / w1) - i_sp1.y;
            double sqdist = x * x + y * y;

            lc = -(sb * i_sp2.x - sa * i_sp2.y);
            x  = ((sb * lc + sa * sc) / w1) - i_sp2.x;
            y  = ((sb * sc - sa * lc) / w1) - i_sp2.y;

            return(sqdist + x * x + y * y);
        }
	    public override void initRotBySquare(NyARLinear[] i_linear,NyARDoublePoint2d[] i_sqvertex)
	    {
		    base.initRotBySquare(i_linear,i_sqvertex);
		    //Matrixからangleをロード
		    this.updateAngleFromMatrix();
		    return;
	    }
        /**
         * Add a point to the homography constraint matrix.
         */
        private static void AddHomographyPointContraint(double[][] A, int A_ptr, NyARDoublePoint2d x, NyARDoublePoint2d xp)
        {
            A[A_ptr][0] = -x.x; //[0];
            A[A_ptr][1] = -x.y; //[1];
            A[A_ptr][2] = -1;
            // ZeroVector3(A+3);
            A[A_ptr][3] = 0;
            A[A_ptr][4] = 0;
            A[A_ptr][5] = 0;

            A[A_ptr][6] = xp.x * x.x; //xp[0] * x[0];
            A[A_ptr][7] = xp.x * x.y; //xp[0] * x[1];
            A[A_ptr][8] = xp.x;       //xp[0];

            // ZeroVector3(A+9);
            A[A_ptr + 1][0] = 0;
            A[A_ptr + 1][1] = 0;
            A[A_ptr + 1][2] = 0;

            A[A_ptr + 1][3] = -x.x;       //-x[0];
            A[A_ptr + 1][4] = -x.y;       //-x[1];
            A[A_ptr + 1][5] = -1;
            A[A_ptr + 1][6] = xp.y * x.x; //xp[1] * x[0];
            A[A_ptr + 1][7] = xp.y * x.y; //xp[1] * x[1];
            A[A_ptr + 1][8] = xp.y;       //xp[1];
        }
Example #14
0
        /**
         * 直線と、i_sp1とi_sp2の作る線分との二乗距離値の合計を返します。計算方法は、線分の2端点を通過する直線の法線上での、2端点と直線の距離の合計です。
         * 線分と直線の類似度を判定する数値になります。
         * @param i_sp1
         * @param i_sp2
         * @param o_point
         * @return
         * 距離が取れないときは無限大です。
         */
        public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
        {
            double la, lb, lc;
            double x, y, w1;

            //thisを法線に変換
            la = this.b;
            lb = -this.a;

            //交点を計算
            w1 = this.a * lb - la * this.b;
            if (w1 == 0.0)
            {
                return(double.PositiveInfinity);
            }
            //i_sp1と、i_linerの交点
            lc = -(la * i_sp1.x + lb * i_sp1.y);
            x  = ((this.b * lc - lb * this.c) / w1) - i_sp1.x;
            y  = ((la * this.c - this.a * lc) / w1) - i_sp1.y;
            double sqdist = x * x + y * y;

            lc = -(la * i_sp2.x + lb * i_sp2.y);
            x  = ((this.b * lc - lb * this.c) / w1) - i_sp2.x;
            y  = ((la * this.c - this.a * lc) / w1) - i_sp2.y;

            return(sqdist + x * x + y * y);
        }
 public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
 {
     int idx = ix + iy * this._stride;
     o_point.x = this._mapx[idx];
     o_point.y = this._mapy[idx];
     return;
 }
Example #16
0
 /**
  * この関数は、頂点集合を包括する矩形を計算して、インスタンスにセットします。
  * @param i_vertex
  * 頂点集合を格納した配列
  * @param i_num_of_vertex
  * 計算対象とする要素の数
  */
 public void setAreaRect(NyARDoublePoint2d[] i_vertex, int i_num_of_vertex)
 {
     //エリアを求める。
     int xmax, xmin, ymax, ymin;
     xmin = xmax = (int)i_vertex[i_num_of_vertex - 1].x;
     ymin = ymax = (int)i_vertex[i_num_of_vertex - 1].y;
     for (int i = i_num_of_vertex - 2; i >= 0; i--)
     {
         if (i_vertex[i].x < xmin)
         {
             xmin = (int)i_vertex[i].x;
         }
         else if (i_vertex[i].x > xmax)
         {
             xmax = (int)i_vertex[i].x;
         }
         if (i_vertex[i].y < ymin)
         {
             ymin = (int)i_vertex[i].y;
         }
         else if (i_vertex[i].y > ymax)
         {
             ymax = (int)i_vertex[i].y;
         }
     }
     this.h = ymax - ymin + 1;
     this.x = xmin;
     this.w = xmax - xmin + 1;
     this.y = ymin;
 }
Example #17
0
 /**
  * この関数は、ラスタドライバから画像を読み出します。
  * @param i_pix_drv
  * @param i_size
  * @param i_vertex
  * @param o_data
  * @param o_param
  * @return
  * @
  */
 public bool pickFromRaster(INyARGsPixelDriver i_pix_drv, NyARDoublePoint2d[] i_vertex, NyIdMarkerPattern o_data, NyIdMarkerParam o_param)
 {
     if (!this._perspective_reader.setSourceSquare(i_vertex))
     {
         return false;
     }
     return this._pickFromRaster(i_pix_drv, o_data, o_param);
 }
Example #18
0
        public void projectionConvert(double i_x, double i_y, double i_z, NyARDoublePoint2d o_2d)
        {
            double w = i_z * this.m22;

            o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) / w;
            o_2d.y = (i_y * this.m11 + i_z * this.m12) / w;
            return;
        }
Example #19
0
        /**
         * 座標値を射影変換します。
         * @param i_3dvertex
         * 変換元の座標値
         * @param o_2d
         * 変換後の座標値を受け取るオブジェクト
         */
        public void project(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d)
        {
            double w = 1 / (i_3dvertex.z * this.m22);

            o_2d.x = (i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w;
            o_2d.y = (i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w;
            return;
        }
Example #20
0
        /**
         * 座標値を射影変換します。
         * @param i_x
         * 変換元の座標値
         * @param i_y
         * 変換元の座標値
         * @param i_z
         * 変換元の座標値
         * @param o_2d
         * 変換後の座標値を受け取るオブジェクト
         */
        public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_2d)
        {
            double w = 1 / (i_z * this.m22);

            o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w;
            o_2d.y = (i_y * this.m11 + i_z * this.m12) * w;
            return;
        }
Example #21
0
 /**
  * Check the geometric consistency between three correspondences.
  */
 private static bool Homography3PointsGeometricallyConsistent(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x1p, NyARDoublePoint2d x2p, NyARDoublePoint2d x3p)
 {
     if (((Geometry.LinePointSide(x1, x2, x3) > 0) ^ (Geometry.LinePointSide(x1p, x2p, x3p) > 0)) == true)
     {
         return(false);
     }
     return(true);
 }
Example #22
0
        public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
        {
            int idx = ix + iy * this._stride;

            o_point.x = this._mapx[idx];
            o_point.y = this._mapy[idx];
            return;
        }
Example #23
0
        /**
         * p2-p1ベクトルのsquare normを計算する。
         * @param i_p1
         * @param i_p2
         * @return
         */
        public static double sqNorm(NyARDoublePoint2d i_p1, NyARDoublePoint2d i_p2)
        {
            double x, y;

            x = i_p2.x - i_p1.x;
            y = i_p2.y - i_p1.y;
            return(x * x + y * y);
        }
Example #24
0
        /**
         * p2-p1間の距離の二乗値を計算します。
         * @param i_p1
         * @param i_p2
         * @return
         */
        public double sqDist(NyARDoublePoint2d i_p1)
        {
            double x, y;

            x = this.x - i_p1.x;
            y = this.y - i_p1.y;
            return(x * x + y * y);
        }
 /**
  * 配列ファクトリ
  * @param i_number
  * @return
  */
 public static NyARDoublePoint2d[] createArray(int i_number)
 {
     NyARDoublePoint2d[] ret = new NyARDoublePoint2d[i_number];
     for (int i = 0; i < i_number; i++)
     {
         ret[i] = new NyARDoublePoint2d();
     }
     return ret;
 }
Example #26
0
 /**
  * 配列ファクトリ
  * @param i_number
  * @return
  */
 public static NyARDoublePoint2d[] createArray(int i_number)
 {
     NyARDoublePoint2d[] ret = new NyARDoublePoint2d[i_number];
     for (int i = 0; i < i_number; i++)
     {
         ret[i] = new NyARDoublePoint2d();
     }
     return(ret);
 }
 //override
 public sealed override bool initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
 {
     bool ret = base.initRotBySquare(i_linear, i_sqvertex);
     if (ret)
     {
         //Matrixからangleをロード
         this.updateAngleFromMatrix();
     }
     return ret;
 }
        public double cauchyProjectiveReprojectionCost(FeaturePairStack.Item i_ptr, double i_one_over_scale2)
        {
            NyARDoublePoint2d ref_ = i_ptr.ref_;
            double            w    = this.m20 * ref_.x + this.m21 * ref_.y + this.m22;
            double            vx   = ((this.m00 * ref_.x + this.m01 * ref_.y + this.m02) / w) - i_ptr.query.x; //XP
            double            vy   = ((this.m10 * ref_.x + this.m11 * ref_.y + this.m12) / w) - i_ptr.query.y; //YP
            double            T    = Math.Log(1 + (vx * vx + vy * vy) * i_one_over_scale2);

            return(T);
        }
Example #29
0
        /**
         * 理想点を計算する。
         * @param i_coord
         * 変換元の座標(理想点)
         */
        public void calculate2dPos(double i_x, double i_y, NyARDoublePoint2d o_idepos)
        {
            NyARDoubleMatrix44 t  = this.ctrans;
            double             h  = (t.m20 * i_x + t.m21 * i_y + t.m23);
            double             hx = (t.m00 * i_x + t.m01 * i_y + t.m03) / h;
            double             hy = (t.m10 * i_x + t.m11 * i_y + t.m13) / h;

            o_idepos.x = hx;
            o_idepos.y = hy;
        }
        /*
         * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *
         * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;
         * 
         * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix
         * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *
         * cosa)*iz+i_trans.z;
         * 
         * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina
         * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa
         * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));
         * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa
         * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *
         * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb
         * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *
         * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *
         * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])
         * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2
         * +2*M*cos(x)*sin(x)
         */
        private double optimizeParamX(double sinb, double cosb, double sinc, double cosc, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle)
        {
            NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
            double L, J, K, M, N, O;
            L = J = K = M = N = O = 0;
            double cp00 = cp.m00;
            double cp01 = cp.m01;
            double cp02 = cp.m02;
            double cp11 = cp.m11;
            double cp12 = cp.m12;

            for (int i = 0; i < i_number_of_vertex; i++)
            {
                double ix, iy, iz;
                ix = i_vertex3d[i].x;
                iy = i_vertex3d[i].y;
                iz = i_vertex3d[i].z;

                double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));
                double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));
                double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));
                double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));
                double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));
                double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));
                double H0 = (iy);
                double H1 = (-sinb * ix + cosb * iz);
                double H2 = i_trans.z;

                double VX = i_vertex2d[i].x;
                double VY = i_vertex2d[i].y;

                double a, b, c, d, e, f;
                a = (VX * H0 - X0);
                b = (VX * H1 - X1);
                c = (VX * H2 - X2);
                d = (VY * H0 - Y0);
                e = (VY * H1 - Y1);
                f = (VY * H2 - Y2);

                L += d * e + a * b;
                N += d * d + a * a;
                J += d * f + a * c;
                M += e * e + b * b;
                K += e * f + b * c;
                O += f * f + c * c;

            }
            L *= 2;
            J *= 2;
            K *= 2;

            return getMinimumErrorAngleFromParam(L, J, K, M, N, O, i_hint_angle);


        }
Example #31
0
        /**
         * Check if four points form a convex quadrilaternal.
         */
        private static bool QuadrilateralConvex(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x4)
        {
            int s;

            s  = Geometry.LinePointSide(x1, x2, x3) > 0 ? 1 : -1;
            s += Geometry.LinePointSide(x2, x3, x4) > 0 ? 1 : -1;
            s += Geometry.LinePointSide(x3, x4, x1) > 0 ? 1 : -1;
            s += Geometry.LinePointSide(x4, x1, x2) > 0 ? 1 : -1;

            return(Math.Abs(s) == 4);
        }
Example #32
0
 private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
 {
     o_matAtA.loadZero();
     double[][] t = o_matAtA.getArray();
     for (int i = 0; i < i_num; i++)
     {
         //0
         double wx = worldCoord[i].x;
         double wy = worldCoord[i].y;
         double sx = screenCoord[i].x;
         double sy = screenCoord[i].y;
         double wxwx = wx * wx;
         double wywy = wy * wy;
         double wxwy = wx * wy;
         t[0][0] += wxwx;
         t[0][1] += wxwy;
         t[0][2] += wx;
         t[1][2] += wy;
         t[0][6] += (-wxwx) * (sx);
         t[0][7] += (-wxwy) * (sx);
         t[1][1] += wywy;
         t[1][7] += (-wywy) * (sx);
         t[2][6] += (-wx) * (sx);
         t[2][7] += (-wy) * (sx);
         t[3][6] += (-wxwx) * (sy);
         t[3][7] += (-wxwy) * (sy);
         t[4][7] += (-wywy) * (sy);
         t[5][6] += (-wx) * (sy);
         t[5][7] += (-wy) * (sy);
         t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
         t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
         t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
     }
     t[1][0] = t[3][4] = t[4][3] = t[0][1];
     t[2][0] = t[3][5] = t[5][3] = t[0][2];
     t[2][1] = t[5][4] = t[4][5] = t[1][2];
     t[7][0] = t[6][1] = t[1][6] = t[0][7];
     t[4][6] = t[6][4] = t[7][3] = t[3][7];
     t[2][2] = t[5][5] = i_num;
     t[3][3] = t[0][0];
     t[4][4] = t[1][1];
     t[6][0] = t[0][6];
     t[6][2] = t[2][6];
     t[6][3] = t[3][6];
     t[6][5] = t[5][6];
     t[7][1] = t[1][7];
     t[7][2] = t[2][7];
     t[7][4] = t[4][7];
     t[7][5] = t[5][7];
     t[7][6] = t[6][7];
     //先頭でゼロクリアしない場合。
     //t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
     return o_matAtA;
 }
Example #33
0
        /**
         * カメラ座標系の点を、スクリーン座標の点へ変換します。
         * @param i_x
         * カメラ座標系の点
         * @param i_y
         * カメラ座標系の点
         * @param i_z
         * カメラ座標系の点
         * @param o_pos2d
         * 結果を受け取るオブジェクトです。
         */
        public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d)
        {
            NyARDoubleMatrix44 m    = this._frustum_rh;
            double             v3_1 = 1 / i_z * m.m32;
            double             w    = this._screen_size.w;
            double             h    = this._screen_size.h;

            o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2;
            o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2;
            return;
        }
 /**
  * 頂点情報を元に、エラー閾値を計算します。
  * @param i_vertex
  */
 private double makeErrThreshold(NyARDoublePoint2d[] i_vertex)
 {
     double a, b, l1, l2;
     a = i_vertex[0].x - i_vertex[2].x;
     b = i_vertex[0].y - i_vertex[2].y;
     l1 = a * a + b * b;
     a = i_vertex[1].x - i_vertex[3].x;
     b = i_vertex[1].y - i_vertex[3].y;
     l2 = a * a + b * b;
     return (Math.Sqrt(l1 > l2 ? l1 : l2)) / 200;
 }
Example #35
0
        /**
         * Use this function to downsample a point to an octave that was found from
         * a bilinear downsampled pyramid.
         *
         * @param[out] xp Downsampled x location
         * @param[out] yp Downsampled y location
         * @param[in] x X location on fine image
         * @param[in] y Y location on fine image
         * @param[in] octave The octave to downsample (x,y) to
         */
        public static void bilinear_downsample_point(NyARDoublePoint2d ret,
                                                     double x,
                                                     double y,
                                                     int octave)
        {
            double a, b;

            a     = 1.0f / (1 << octave);
            b     = 0.5f * a - 0.5f;
            ret.x = x * a + b;
            ret.y = y * a + b;
        }
Example #36
0
        /**
         * 指定したパラメータの式との交点を得る。
         * @param i_a
         * @param i_b
         * @param i_c
         * @param o_point
         * @return
         */
        public bool crossPos(double i_a, double i_b, double i_c, NyARDoublePoint2d o_point)
        {
            double w1 = this.a * i_b - i_a * this.b;

            if (w1 == 0.0)
            {
                return(false);
            }
            o_point.x = (this.b * i_c - i_b * this.c) / w1;
            o_point.y = (i_a * this.c - this.a * i_c) / w1;
            return(true);
        }
Example #37
0
        /**
         * 2直線の交点を計算します。
         * @param l_line_2
         * @param o_point
         * @return
         */
        public bool crossPos(NyARLinear l_line_2, NyARDoublePoint2d o_point)
        {
            double w1 = this.a * l_line_2.b - l_line_2.a * this.b;

            if (w1 == 0.0)
            {
                return(false);
            }
            o_point.x = (this.b * l_line_2.c - l_line_2.b * this.c) / w1;
            o_point.y = (l_line_2.a * this.c - this.a * l_line_2.c) / w1;
            return(true);
        }
Example #38
0
        /**
         * Check the geometric consistency between three correspondences.
         */
        private static bool Homography3PointsGeometricallyConsistent(NyARDoublePoint2d x1, NyARDoublePoint2d x2, NyARDoublePoint2d x3, NyARDoublePoint2d x1p, NyARDoublePoint2d x2p, NyARDoublePoint2d x3p)
        {
            //TODO 1は常に+だからbが+であるかだけをチェックすればいい。
            double a = ((x2.x - x1.x) * (x3.y - x1.y) - (x2.y - x1.y) * (x3.x - x1.x));
            double b = ((x2p.x - x1p.x) * (x3p.y - x1p.y) - (x2p.y - x1p.y) * (x3p.x - x1p.x));

            if ((a > 0) ^ (b > 0) == true)
            {
                return(false);
            }
            return(true);
        }
Example #39
0
        /**
         * 元ar2GenTemplate関数。
         * 与えられた座標を中心に、テンプレート画像を生成する。
         * 座標は観察座標点。
         * @param i_x
         * @param i_y
         * @param i_scale
         * @param o_template
         * @return
         * @throws NyARException
         */
        public void makeFromReferenceImage(double i_x, double i_y, NyARDoubleMatrix44 i_ref_ctrans, INyARCameraDistortionFactor i_ref_dist_factor, NyARNftIsetFile.ReferenceImage i_source)
        {
            int[]             img      = this.img;
            int               img1_ptr = 0;
            int               k        = 0;
            int               sum2     = 0;
            int               sum      = 0;
            NyARDoublePoint2d ideal    = this.__in;

            for (int j = -(this.yts); j <= this.yts; j++)
            {
                for (int i = -(this.xts); i <= this.xts; i++)
                {
                    i_ref_dist_factor.observ2Ideal(i_x + i * AR2_TEMP_SCALE, i_y + j * AR2_TEMP_SCALE, ideal);
                    double ideal_x = ideal.x;
                    double ideal_y = ideal.y;
                    //ar2ScreenCoord2MarkerCoord(in.x,in.y,i_ref_ctrans,in);の展開
                    double c11 = i_ref_ctrans.m20 * ideal_x - i_ref_ctrans.m00;
                    double c12 = i_ref_ctrans.m21 * ideal_x - i_ref_ctrans.m01;
                    double c21 = i_ref_ctrans.m20 * ideal_y - i_ref_ctrans.m10;
                    double c22 = i_ref_ctrans.m21 * ideal_y - i_ref_ctrans.m11;
                    double b1  = i_ref_ctrans.m03 - i_ref_ctrans.m23 * ideal_x;
                    double b2  = i_ref_ctrans.m13 - i_ref_ctrans.m23 * ideal_y;
                    double m   = c11 * c22 - c12 * c21;
                    //public int ar2GetImageValue(double sx, double sy) throws NyARExceptionの展開
                    {
                        int ix = (int)((((c22 * b1 - c12 * b2) / m) * i_source.dpi / 25.4f) + 0.5);
                        int iy = (int)((i_source.height - (((c11 * b2 - c21 * b1) / m) * i_source.dpi) / 25.4f) + 0.5);

                        //座標計算と値取得は分けよう。
                        if (ix < 0 || ix >= i_source.width || iy < 0 || iy >= i_source.height)
                        {
                            img[img1_ptr] = AR2_TEMPLATE_NULL_PIXEL;
                        }
                        else
                        {
                            int ret = img[img1_ptr] = i_source.img[iy * i_source.width + ix];
                            sum2 += ret * ret;
                            sum  += ret;
                            k++;
                        }
                        //byte値はint化
                    }
                    img1_ptr++;
                }
            }
            int vlen = sum2 - sum * sum / k;

            this.vlen         = (int)Math.Sqrt((double)(vlen));
            this.sum_of_img   = sum;
            this.valid_pixels = k;
            return;
        }
Example #40
0
            public bool ar2GetVectorAngle(NyARDoublePoint2d p1, NyARDoublePoint2d p2)
            {
                double l = Math.Sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));

                if (l == 0.0f)
                {
                    return(false);
                }

                this.sin = (p2.y - p1.y) / l;
                this.cos = (p2.x - p1.x) / l;
                return(true);
            }
Example #41
0
 public static NyARDoublePoint2d[][] create2dArray(int i_length_x, int i_length_y)
 {
     NyARDoublePoint2d[][] ret = new NyARDoublePoint2d[i_length_y][];
     for (int i = 0; i < i_length_y; i++)
     {
         ret[i] = new NyARDoublePoint2d[i_length_x];
         for (int i2 = 0; i2 < i_length_x; i2++)
         {
             ret[i][i2] = new NyARDoublePoint2d();
         }
     }
     return(ret);
 }
 /**
  * この関数は、直線との交点を求めます。
  * @param i_vector1
  * 交点を求める直線
  * @param o_point
  * 交点座標を得るオブジェクト。
  * @return
  * 交点が求まると、trueを返します。
  */
 public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point)
 {
     double a1 = i_vector1.dy;
     double b1 = -i_vector1.dx;
     double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x);
     double a2 = this.dy;
     double b2 = -this.dx;
     double c2 = (this.dx * this.y - this.dy * this.x);
     double w1 = a1 * b2 - a2 * b1;
     if (w1 == 0.0)
     {
         return false;
     }
     o_point.x = (b1 * c2 - b2 * c1) / w1;
     o_point.y = (a2 * c1 - a1 * c2) / w1;
     return true;
 }
 public NyARObserv2IdealMap(NyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
 {
     NyARDoublePoint2d opoint = new NyARDoublePoint2d();
     this._mapx = new double[i_screen_size.w * i_screen_size.h];
     this._mapy = new double[i_screen_size.w * i_screen_size.h];
     this._stride = i_screen_size.w;
     int ptr = i_screen_size.h * i_screen_size.w - 1;
     //歪みマップを構築
     for (int i = i_screen_size.h - 1; i >= 0; i--)
     {
         for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
         {
             i_distfactor.observ2Ideal(i2, i, opoint);
             this._mapx[ptr] = opoint.x;
             this._mapy[ptr] = opoint.y;
             ptr--;
         }
     }
     return;
 }
        public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)
	{
		double x, y;
		double d0 = this._factor[0];
		double d1 = this._factor[1];
		double d3 = this._factor[3];
		double d2_w = this._factor[2] / 100000000.0;
		for (int i = 0; i < i_size; i++) {
			x = (i_in[i].x - d0) * d3;
			y = (i_in[i].y - d1) * d3;
			if (x == 0.0 && y == 0.0) {
				o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1);
				o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1);
			} else {
				double d = 1.0 - d2_w * (x * x + y * y);
				o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1);
				o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1);
			}
		}
		return;
	}
 public NyARFixedFloatObserv2IdealMap(NyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
 {
     NyARDoublePoint2d opoint = new NyARDoublePoint2d();
     this._mapx = new int[i_screen_size.w * i_screen_size.h];
     this._mapy = new int[i_screen_size.w * i_screen_size.h];
     this._stride = i_screen_size.w;
     int ptr = i_screen_size.h * i_screen_size.w - 1;
     //歪みマップを構築
     for (int i = i_screen_size.h - 1; i >= 0; i--)
     {
         for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
         {
             i_distfactor.observ2Ideal(i2, i, opoint);
             this._mapx[ptr] = (int)(opoint.x * 65536);
             this._mapy[ptr] = (int)(opoint.y * 65536);
             ptr--;
         }
     }
     i_distfactor.getValue(this._factor);
     return;
 }
        /**
         * この関数は、ARToolKitのcheck_dir関数に相当します。
         * 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整?)
         * @param i_start_vertex
         * 開始位置?
         * @param i_end_vertex
         * 終了位置?
         * @throws NyARException
         */
        public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex)
        {
            double h;
            NyARDoubleMatrix44 inv_cpara = this._inv_cpara;
            //final double[] world = __checkVectorByVertex_world;// [2][3];
            double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0;// mat_a->m[0]*st[0]*10.0+
            double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0;// mat_a->m[3]*st[0]*10.0+
            double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0;// mat_a->m[6]*st[0]*10.0+
            double world3 = world0 + this.v1;
            double world4 = world1 + this.v2;
            double world5 = world2 + this.v3;
            // </Optimize>

            NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
            h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;
            if (h == 0.0)
            {
                return false;
            }
            double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;
            double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;

            //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;
            h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;
            if (h == 0.0)
            {
                return false;
            }
            double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;
            double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;

            double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);
            if (v < 0)
            {
                this.v1 = -this.v1;
                this.v2 = -this.v2;
                this.v3 = -this.v3;
            }
            return true;
        }
	public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex)
	{		
		Debug.Assert(i_number_of_vertex==4);
		double[] cx=this._cx;
		double[] cy=this._cy;
		double cpara02=this._projection_mat.m02;
		double cpara12=this._projection_mat.m12;		
		NyARMat mat_t=this._mat_t;
		double[][] mata = this._mat_a.getArray();
		double[][] matat= this._mat_at.getArray();
		for (int i = 0; i < 4; i++){
			cx[i]=i_ref_vertex_2d[i].x;
			cy[i]=i_ref_vertex_2d[i].y;
			int x2 = i * 2;	
			mata[x2][2] = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];
			mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
		}
		//T(3x3行列)の作成
		mat_t.matrixMul(this._mat_at, this._mat_a);
		mat_t.matrixSelfInv();		
		return;		
	}
        /**
         * 画面上の座標群を指定します。
         * @param i_ref_vertex_2d
         * 歪み矯正済の画面上の頂点座標群への参照値を指定します。
         * @throws NyARException
         * 
         */
        public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d, int i_number_of_vertex)
        {
            //3x2nと2n*3の行列から、最小二乗法計算するために3x3マトリクスを作る。		
            //行列[A]の3列目のキャッシュ
            double[] cx = this._cx;
            double[] cy = this._cy;

            double m22;
            double p00 = this._projection_mat.m00;
            double p01 = this._projection_mat.m01;
            double p11 = this._projection_mat.m11;
            double p12 = this._projection_mat.m12;
            double p02 = this._projection_mat.m02;
            double w1, w2, w3, w4;
            this._a00 = i_number_of_vertex * p00 * p00;
            this._a01_10 = i_number_of_vertex * p00 * p01;
            this._a11 = i_number_of_vertex * (p01 * p01 + p11 * p11);

            m22 = 0;
            w1 = w2 = 0;
            //[A]T*[A]
            for (int i = 0; i < i_number_of_vertex; i++)
            {
                //座標を保存しておく。
                w3 = p02 - (cx[i] = i_ref_vertex_2d[i].x);
                w4 = p12 - (cy[i] = i_ref_vertex_2d[i].y);
                w1 += w3;
                w2 += w4;
                m22 += w3 * w3 + w4 * w4;
            }
            this._a02_20 = w1 * p00;
            this._a12_21 = p01 * w1 + p11 * w2;
            this._a22 = m22;

            this._nmber_of_vertex = i_number_of_vertex;
            return;
        }
        public virtual void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
        {
            NyARRotVector vec1 = this.__initRot_vec1;
            NyARRotVector vec2 = this.__initRot_vec2;

            //向かい合った辺から、2本のベクトルを計算

            //軸1
            vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
            vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);

            //軸2
            vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
            vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);

            //回転の最適化?
            NyARRotVector.checkRotation(vec1, vec2);

            this.m00 = vec1.v1;
            this.m10 = vec1.v2;
            this.m20 = vec1.v3;
            this.m01 = vec2.v1;
            this.m11 = vec2.v2;
            this.m21 = vec2.v3;

            //最後の軸を計算
            double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;
            double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;
            double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;
            double w = Math.Sqrt(w02 * w02 + w12 * w12 + w22 * w22);
            this.m02 = w02 / w;
            this.m12 = w12 / w;
            this.m22 = w22 / w;
            //Matrixからangleをロード
            return;
        }
 /**
  * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数
  *
  * @param i_in
  * @param o_out
  */
 public void ideal2Observ(NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)
 {
     double x = (i_in.x - this._f0) * this._f3;
     double y = (i_in.y - this._f1) * this._f3;
     if (x == 0.0 && y == 0.0)
     {
         o_out.x = this._f0;
         o_out.y = this._f1;
     }
     else
     {
         double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);
         o_out.x = x * d + this._f0;
         o_out.y = y * d + this._f1;
     }
     return;
 }
 /**
  * ideal2Observをまとめて実行します。
  * @param i_in
  * @param o_out
  */
 public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)
 {
     double x, y;
     double d0 = this._f0;
     double d1 = this._f1;
     double d3 = this._f3;
     double d2_w = this._f2 / 100000000.0;
     for (int i = 0; i < i_size; i++)
     {
         x = (i_in[i].x - d0) * d3;
         y = (i_in[i].y - d1) * d3;
         if (x == 0.0 && y == 0.0)
         {
             o_out[i].x = d0;
             o_out[i].y = d1;
         }
         else
         {
             double d = 1.0 - d2_w * (x * x + y * y);
             o_out[i].x = x * d + d0;
             o_out[i].y = y * d + d1;
         }
     }
     return;
 }
 /**
  * この関数は、遠近法のパラメータを計算して、返却します。
  * @param i_size
  * 変換先の矩形のサイズを指定します。
  * @param i_vertex
  * 変換元の頂点を指定します。要素数は4でなければなりません。
  * @param o_param
  * 射影変換パラメータの出力インスタンスを指定します。要素数は8でなければなりません。
  * @return
  * 成功するとtrueを返します。
  * @
  */
 public bool getParam(NyARIntSize i_size, NyARDoublePoint2d[] i_vertex, double[] o_param)
 {
     return this.getParam(i_size.w, i_size.h, i_vertex[0].x, i_vertex[0].y, i_vertex[1].x, i_vertex[1].y, i_vertex[2].x, i_vertex[2].y, i_vertex[3].x, i_vertex[3].y, o_param);
 }
        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;
        }
        /**
         * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
         * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
         * 線分と直線の類似度を判定する数値になります。
         * @param i_sp1
         * 線分の端点1
         * @param i_sp2
         * 線分の端点2
         * @return
         * 二乗距離値の合計。距離が取れないときは無限大です。
         */
        public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
        {
            double sa, sb, sc;
            sa = this.dy;
            sb = -this.dx;
            sc = (this.dx * this.y - this.dy * this.x);

            double lc;
            double x, y, w1;
            //thisを法線に変換

            //交点を計算
            w1 = sa * (-sa) - sb * sb;
            if (w1 == 0.0)
            {
                return Double.PositiveInfinity;
            }
            //i_sp1と、i_linerの交点
            lc = -(sb * i_sp1.x - sa * i_sp1.y);
            x = ((sb * lc + sa * sc) / w1) - i_sp1.x;
            y = ((sb * sc - sa * lc) / w1) - i_sp1.y;
            double sqdist = x * x + y * y;

            lc = -(sb * i_sp2.x - sa * i_sp2.y);
            x = ((sb * lc + sa * sc) / w1) - i_sp2.x;
            y = ((sb * sc - sa * lc) / w1) - i_sp2.y;

            return sqdist + x * x + y * y;
        }
 /**
  * この関数は、正規化したベクトルを出力する、{@link #leastSquares}です。
  * @param i_points
  * 頂点群を格納した配列。
  * @param i_number_of_data
  * 計算対象の頂点群の数
  * @return
  * 計算に成功すると、trueを返します。
  */
 public bool leastSquaresWithNormalize(NyARDoublePoint2d[] i_points, int i_number_of_data)
 {
     bool ret = this.leastSquares(i_points, i_number_of_data);
     double sq = 1 / Math.Sqrt(this.dx * this.dx + this.dy * this.dy);
     this.dx *= sq;
     this.dy *= sq;
     return ret;
 }
 /**
  * この関数は、頂点群から最小二乗法を使用して直線を計算します。
  * @param i_points
  * 頂点群を格納した配列。
  * @param i_number_of_data
  * 計算対象の頂点群の数
  * @return
  * 計算に成功すると、trueを返します。
  */
 public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data)
 {
     int i;
     double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
     for (i = 0; i < i_number_of_data; i++)
     {
         NyARDoublePoint2d ptr = i_points[i];
         double xw = ptr.x;
         sum_xy += xw * ptr.y;
         sum_x += xw;
         sum_y += ptr.y;
         sum_x2 += xw * xw;
     }
     double la = -(i_number_of_data * sum_x2 - sum_x * sum_x);
     double lb = -(i_number_of_data * sum_xy - sum_x * sum_y);
     double cc = (sum_x2 * sum_y - sum_xy * sum_x);
     double lc = -(la * sum_x + lb * sum_y) / i_number_of_data;
     //交点を計算
     double w1 = -lb * lb - la * la;
     if (w1 == 0.0)
     {
         return false;
     }
     this.x = ((la * lc - lb * cc) / w1);
     this.y = ((la * cc + lb * lc) / w1);
     this.dy = -lb;
     this.dx = -la;
     return true;
 }
 /**
  * この関数は、この直線と線分が作るCos値を返します。
  * @param i_pos1
  * 線分の端点1
  * @param i_pos2
  * 線分の端点2
  * @return
  * 2直線のCOS値(radian)
  */
 public double getVecCos(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2)
 {
     return getVecCos(i_pos2.x - i_pos1.x, i_pos2.y - i_pos1.y);
 }
 /**
  * この関数は、遠近法のパラメータを計算して、返却します。
  * @param i_width
  * 変換先の矩形のサイズを指定します。
  * @param i_height
  * 変換先の矩形のサイズを指定します。
  * @param i_vertex
  * 変換元の頂点を指定します。要素数は4でなければなりません。
  * @param o_param
  * 射影変換パラメータの出力インスタンスを指定します。要素数は8でなければなりません。
  * @return
  * 成功するとtrueを返します。
  * @
  */
 public bool getParam(int i_width, int i_height, NyARDoublePoint2d[] i_vertex, double[] o_param)
 {
     return this.getParam(i_width, i_height, i_vertex[0].x, i_vertex[0].y, i_vertex[1].x, i_vertex[1].y, i_vertex[2].x, i_vertex[2].y, i_vertex[3].x, i_vertex[3].y, o_param);
 }
        /**
         * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy );
         *
         * @param ox
         * @param oy
         * @param ix
         * @param iy
         * @return
         */
        public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
        {
            double z02, z0, p, q, z, px, py, opttmp_1;
            double d0 = this._f0;
            double d1 = this._f1;

            px = ix - d0;
            py = iy - d1;
            p = this._f2 / 100000000.0;
            z02 = px * px + py * py;
            q = z0 = Math.Sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);

            for (int i = 1; ; i++)
            {
                if (z0 != 0.0)
                {
                    // Optimize opttmp_1
                    opttmp_1 = p * z02;
                    z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);
                    px = px * z / z0;
                    py = py * z / z0;
                }
                else
                {
                    px = 0.0;
                    py = 0.0;
                    break;
                }
                if (i == PD_LOOP)
                {
                    break;
                }
                z02 = px * px + py * py;
                z0 = Math.Sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);
            }
            o_point.x = px / this._f3 + d0;
            o_point.y = py / this._f3 + d1;
            return;
        }
	    /**
	     * クリッピング付きのライントレーサです。
	     * 
	     * @param i_pos1
	     * @param i_pos2
	     * @param i_edge
	     * @param o_coord
	     * @return
	     * @throws NyARException
	     */
        public bool traceLineWithClip(NyARDoublePoint2d i_pos1,
		    NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
	    {
		    NyARIntSize s=this._ref_base_raster.getSize();
            bool is_p1_inside_area, is_p2_inside_area;

		    NyARIntPoint2d[] pt = this.__pt;
		    // 線分が範囲内にあるかを確認
		    is_p1_inside_area = s.isInnerPoint(i_pos1);
		    is_p2_inside_area = s.isInnerPoint(i_pos2);
		    // 個数で分岐
		    if (is_p1_inside_area && is_p2_inside_area) {
			    // 2ならクリッピング必要なし。
			    if (!this.traceLine(i_pos1, i_pos2, i_edge, o_coord)) {
				    return false;
			    }
			    return true;

		    }
		    // 1,0個の場合は、線分を再定義
		    if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2)) {
			    return false;
		    }
		    if (!this.__temp_l.makeSegmentLine(s.w,s.h,pt)) {
			    return false;
		    }
		    if (is_p1_inside_area != is_p2_inside_area) {
			    // 1ならクリッピング後に、外に出ていた点に近い輪郭交点を得る。

			    if (is_p1_inside_area) {
				    // p2が範囲外
				    pt[(i_pos2.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos1);
			    } else {
				    // p1が範囲外
				    pt[(i_pos1.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos2);
			    }
		    } else {
			    // 0ならクリッピングして得られた2点を使う。
			    if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2)) {
				    return false;
			    }
			    if (!this.__temp_l.makeSegmentLine(s.w,s.h, pt)) {
				    return false;
			    }
		    }
		    if (!this.traceLine(pt[0], pt[1], i_edge, o_coord)) {
			    return false;
		    }

		    return true;
	    }