コード例 #1
0
    public void Caculate1()
    {
        Debug.Log(leftCamera.name + ":\n" + leftCamera.worldToCameraMatrix);
        Debug.Log(rightCamera.name + ":\n" + rightCamera.worldToCameraMatrix);

        Vector3[]   points     = GetFeaturePoints();
        PointData[] pointDatas = PreparePointData(points);

        Matrix4x4 ML = Matrix4x4.zero;
        Matrix4x4 MR = Matrix4x4.zero;
        int       m, n; float[] M, N, X;

        /*prepare data for N = [lm00 lm01 lm02 lm03
         *              lm10 lm11 lm12 lm13
         *              rm00 rm01 rm02 rm03
         *              rm10 rm11 rm12 rm13] */
        PrepareData0(out m, out n, out M, out N, out X, pointDatas, 0.5f);
        EigenSharp.cal_line_equation(ref X, m, n, M, N);
        ML[0, 0] = X[0]; ML[0, 1] = X[1]; ML[0, 2] = X[2]; ML[0, 3] = X[3];
        MR[0, 0] = X[8]; MR[0, 1] = X[9]; MR[0, 2] = X[10]; MR[0, 3] = X[11];

        PrepareData1(out m, out n, out M, out N, out X, pointDatas, ML, MR);
        EigenSharp.cal_line_equation(ref X, m, n, M, N);
        ML[2, 0] = X[0]; ML[2, 1] = X[1]; ML[2, 2] = X[2]; ML[2, 3] = X[3];
        MR[2, 0] = X[4]; MR[2, 1] = X[5]; MR[2, 2] = X[6]; MR[2, 3] = X[7];

        Debug.Log("ML:\n" + ML);
        Debug.Log("MR:\n" + MR);
    }
コード例 #2
0
    public void Caculate()
    {
        Debug.Log(leftCamera.name + ":\n" + leftCamera.worldToCameraMatrix);
        Debug.Log(rightCamera.name + ":\n" + rightCamera.worldToCameraMatrix);

        Vector3[] points = new Vector3[features.childCount];
        for (int i = 0; i < features.childCount; i++)
        {
            points[i] = features.GetChild(i).position;
        }

        int m, n; float[] M, N, X;

        PrepareData(out m, out n, out M, out N, out X, points);
        EigenSharp.cal_line_equation(ref X, m, n, M, N);


        Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", X[0], X[1], X[2], X[3]));
        Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", X[4], X[5], X[6], X[7]));
        Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", X[8], X[9], X[10], X[11]));
        Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", X[12], X[13], X[14], X[15]));
        //   Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", x[16], x[17], x[18], x[19]));
//        Debug.Log(string.Format("{0:F5}:{1:F5}:{2:F5}:{3:F5}:", x[20], x[21], x[22], x[23]));

        Vector3 RL0 = new Vector3(X[0], X[1], X[2]);
        Vector3 RL1 = new Vector3(X[4], X[5], X[6]);
        Vector3 RL2 = -Vector3.Cross(RL0, RL1);

        Vector3 RR0 = new Vector3(X[8], X[9], X[10]);
        Vector3 RR1 = new Vector3(X[12], X[13], X[14]);
        Vector3 RR2 = -Vector3.Cross(RR0, RR1);

        Matrix4x4 ML = Matrix4x4.zero;

        ML[0, 0] = RL0.x; ML[0, 1] = RL0.y; ML[0, 2] = RL0.z; ML[0, 3] = X[3];
        ML[1, 0] = RL1.x; ML[1, 1] = RL1.y; ML[1, 2] = RL1.z; ML[1, 3] = X[7];
        ML[2, 0] = RL2.x; ML[2, 1] = RL2.y; ML[2, 2] = RL2.z; ML[2, 3] = 0;
        ML[3, 0] = 0;     ML[3, 1] = 0;     ML[3, 2] = 0;     ML[3, 3] = 1;

        Matrix4x4 MR = Matrix4x4.zero;

        MR[0, 0] = RR0.x; MR[0, 1] = RR0.y; MR[0, 2] = RR0.z; MR[0, 3] = X[11];
        MR[1, 0] = RR1.x; MR[1, 1] = RR1.y; MR[1, 2] = RR1.z; MR[1, 3] = X[15];
        MR[2, 0] = RR2.x; MR[2, 1] = RR2.y; MR[2, 2] = RR2.z; MR[2, 3] = 0;
        MR[3, 0] = 0;     MR[3, 1] = 0;     MR[3, 2] = 0;     MR[3, 3] = 1;



        Vector3 pw  = points[9];
        float   xxl = ML[0, 0] * pw.x + ML[0, 1] * pw.y + ML[0, 2] * pw.z + ML[0, 3];
        //Debug.Log(string.Format("x':{0:F5}", xxl));
        float xxxl = Mathf.Tan(30 * Mathf.Deg2Rad) * WorldToClip(pw, leftCamera).x;
        //Debug.Log(string.Format("x'':{0:F5}", xxxl));
        float tmp = ML[2, 0] * pw.x + ML[2, 1] * pw.y + ML[2, 2] * pw.z;
        //Debug.Log(string.Format("tmp'':{0:F5}", tmp));
        float ml23 = -xxl / xxxl - tmp;



        float xxr  = MR[0, 0] * pw.x + MR[0, 1] * pw.y + MR[0, 2] * pw.z + MR[0, 3];
        float xxxr = Mathf.Tan(30 * Mathf.Deg2Rad) * WorldToClip(pw, rightCamera).x;
        float mr23 = -xxr / xxxr - (MR[2, 0] * pw.x + MR[2, 1] * pw.y + MR[2, 2] * pw.z);

        ML[2, 3] = ml23;
        MR[2, 3] = mr23;



        Debug.Log("ML:\n" + ML);
        Debug.Log("MR:\n" + MR);
    }