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);
}
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);
}
