public void RunAlgorithm()
{
Matrix4x4 T = new Matrix4x4(); // transformation matrix
System.Random random = new System.Random();
IList <int> indexes = new List <int>();
// handle inside points
for (int i = 0; i < AlltPts[0].Count; i++)
{
indexes.Add(i);
}
while (!lessthanMin)
{
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = 0f;// (float)(random.NextDouble() * 0.001) + additionError; // 0~3 mm
matrixP.SetColumn(column, new Vector4(AlltPts[1][index].x + randomDouble, AlltPts[1][index].y + randomDouble, AlltPts[1][index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(AlltPts[0][index].x, AlltPts[0][index].y, AlltPts[0][index].z, 1));
column++;
}
Debug.Log(matrixP);
Debug.Log(_matrixP);
T_temp = _matrixP * matrixP.inverse;
float errorsum = 0;
for (int index = 0; index < AlltPts[0].Count; index++)
{
Vector3 p = T_temp.MultiplyPoint(AlltPts[1][index]);
errorsum += Pow2(p.x - AlltPts[0][index].x) + Pow2(p.y - AlltPts[0][index].y) + Pow2(p.z - AlltPts[0][index].z);
}
float error = errorsum / AlltPts[0].Count;
//Debug.Log(String.Format("error: {0}", error));
if (error < min)
{
lessthanMin = true;
min = error;
T = T_temp;
}
}
}
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
string asdesigned = "";
string aftercal = "";
for (int index = 0; index < StartPts[0].Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", StartPts[0][index].x, StartPts[0][index].y, StartPts[0][index].z) + "\n";
}
for (int index = 0; index < EndPts[0].Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", EndPts[0][index].x, EndPts[0][index].y, EndPts[0][index].z) + "\n";
}
for (int index = 0; index < StartPts[1].Count; index++)
{
Vector3 afterCal = T.MultiplyPoint(StartPts[1][index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
for (int index = 0; index < EndPts[1].Count; index++)
{
Vector3 afterCal = T.MultiplyPoint(EndPts[1][index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
Debug.Log(asdesigned);
Debug.Log(aftercal);
float origin_error = 0;
float modified_error = 0;
float origin_d_error = 0;
float modified_d_error = 0;
for (int i = 0; i < StartPts[0].Count; i++)
{
if ((i + 1) == 5 || (i + 1) == 12)
{
//DrawLine(StartPts[0][i], EndPts[0][i], Color.green);
//DrawLine(StartPts[1][i], EndPts[1][i], Color.red);
//DrawLine(T.MultiplyPoint(StartPts[1][i]), T.MultiplyPoint(EndPts[1][i]), Color.cyan);
CreateCylinderBetweenPoints(StartPts[0][i], EndPts[0][i], 14f, Color.green);
//CreateCylinderBetweenPoints(StartPts[1][i], EndPts[1][i], 14f, Color.red);
CreateCylinderBetweenPoints(T.MultiplyPoint(StartPts[1][i]), T.MultiplyPoint(EndPts[1][i]), 14f, Color.cyan);
float error_d_b = 0;
error_d_b += Pow2(StartPts[0][i].x - StartPts[1][i].x) + Pow2(StartPts[0][i].y - StartPts[1][i].y) + Pow2(StartPts[0][i].z - StartPts[1][i].z);
error_d_b += Pow2(EndPts[0][i].x - EndPts[1][i].x) + Pow2(EndPts[0][i].y - EndPts[1][i].y) + Pow2(EndPts[0][i].z - EndPts[1][i].z);
Debug.Log("error_d_b");
Debug.Log(error_d_b / 2);
origin_d_error += error_d_b / 2;
float error_d_mb = 0;
error_d_mb += Pow2(StartPts[0][i].x - T.MultiplyPoint(StartPts[1][i]).x) + Pow2(StartPts[0][i].y - T.MultiplyPoint(StartPts[1][i]).y) + Pow2(StartPts[0][i].z - T.MultiplyPoint(StartPts[1][i]).z);
error_d_mb += Pow2(EndPts[0][i].x - T.MultiplyPoint(EndPts[1][i]).x) + Pow2(EndPts[0][i].y - T.MultiplyPoint(EndPts[1][i]).y) + Pow2(EndPts[0][i].z - T.MultiplyPoint(EndPts[1][i]).z);
Debug.Log("error_d_mb");
Debug.Log(error_d_mb / 2);
modified_d_error += error_d_mb / 2;
}
else
{
//DrawLine(StartPts[0][i], EndPts[0][i], Color.black);
//DrawLine(StartPts[1][i], EndPts[1][i], Color.magenta);
//DrawLine(T.MultiplyPoint(StartPts[1][i]), T.MultiplyPoint(EndPts[1][i]), Color.yellow);
CreateCylinderBetweenPoints(StartPts[0][i], EndPts[0][i], 14f, Color.black);
//CreateCylinderBetweenPoints(StartPts[1][i], EndPts[1][i], 14f, Color.grey);
CreateCylinderBetweenPoints(T.MultiplyPoint(StartPts[1][i]), T.MultiplyPoint(EndPts[1][i]), 14f, Color.yellow);
float error_b = 0;
error_b += Pow2(StartPts[0][i].x - StartPts[1][i].x) + Pow2(StartPts[0][i].y - StartPts[1][i].y) + Pow2(StartPts[0][i].z - StartPts[1][i].z);
error_b += Pow2(EndPts[0][i].x - EndPts[1][i].x) + Pow2(EndPts[0][i].y - EndPts[1][i].y) + Pow2(EndPts[0][i].z - EndPts[1][i].z);
origin_error += error_b / 2;
float error_mb = 0;
error_mb += Pow2(StartPts[0][i].x - T.MultiplyPoint(StartPts[1][i]).x) + Pow2(StartPts[0][i].y - T.MultiplyPoint(StartPts[1][i]).y) + Pow2(StartPts[0][i].z - T.MultiplyPoint(StartPts[1][i]).z);
error_mb += Pow2(EndPts[0][i].x - T.MultiplyPoint(EndPts[1][i]).x) + Pow2(EndPts[0][i].y - T.MultiplyPoint(EndPts[1][i]).y) + Pow2(EndPts[0][i].z - T.MultiplyPoint(EndPts[1][i]).z);
modified_error += error_mb / 2;
}
}
Debug.Log(String.Format("origin_error: {0}", origin_error));
Debug.Log(String.Format("modified_error: {0}", modified_error));
Debug.Log(String.Format("origin_d_error: {0}", origin_d_error));
Debug.Log(String.Format("modified_d_error: {0}", modified_d_error));
//Vector3 N4_s = new Vector3(-49.42f, 475.02f, 207f);
//Vector3 N4_e = new Vector3(133.04f, -437.67f, 205f);
//DrawLine(T.MultiplyPoint(N4_s), T.MultiplyPoint(N4_e), Color.red);
//DrawLine(N4_s, N4_e, Color.cyan);
//Vector3 N4o_s = new Vector3(-152.15f, 469.42f, 199f);
//Vector3 N4o_e = new Vector3(20.86f, -458.59f, 199f);
//DrawLine(N4o_s, N4o_e, Color.green);
//float error_N4 = 0;
//Vector3 p_N4 = T.MultiplyPoint(N4_s);
//Vector3 p1_N4 = T.MultiplyPoint(N4_e);
//error_N4 += Pow2(p_N4.x - N4_s.x) + Pow2(p_N4.y - N4_s.y) + Pow2(p_N4.z - N4_s.z);
//error_N4 += Pow2(p1_N4.x - N4_e.x) + Pow2(p1_N4.y - N4_e.y) + Pow2(p1_N4.z - N4_e.z);
//Debug.Log(error_N4 / 2);
}
public void RunAlgorithm()
{
Matrix4x4 T = new Matrix4x4(); // transformation matrix
IList <int> indexes = new List <int>();
for (int i = 0; i < 8; i++)
{
indexes.Add(i);
}
while (!lessthanMin)
{
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = (float)(random.NextDouble() * 0.001) + additionError; // 0~3 mm
matrixP.SetColumn(column, new Vector4(insidePts2[index].x + randomDouble, insidePts2[index].y + randomDouble, insidePts2[index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(insidePts1[index].x, insidePts1[index].y, insidePts1[index].z, 1));
column++;
}
T_temp = _matrixP * matrixP.inverse;
float errorsum = 0;
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 p = T_temp.MultiplyPoint(insidePts2[index]);
errorsum += Pow2(p.x - insidePts1[index].x) + Pow2(p.y - insidePts1[index].y) + Pow2(p.z - insidePts1[index].z);
}
float error = errorsum / insidePts2.Count;
if (error < min)
{
lessthanMin = true;
min = error;
T = T_temp;
}
}
}
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
string asdesigned = "";
string aftercal = "";
for (int index = 0; index < insidePts1.Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", insidePts1[index].x, insidePts1[index].y, insidePts1[index].z) + "\n";
}
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 afterCal = T.MultiplyPoint(insidePts2[index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
Debug.Log(asdesigned);
Debug.Log(aftercal);
for (int i = 0; i < 4; i++)
{
if (i != 3)
{
DrawLine(insidePts1[i], insidePts1[i + 1], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[i]), T.MultiplyPoint(insidePts2[i + 1]), Color.yellow);
}
else
{
DrawLine(insidePts1[3], insidePts1[0], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[3]), T.MultiplyPoint(insidePts2[0]), Color.yellow);
}
}
for (int i = 4; i < 8; i++)
{
if (i != 7)
{
DrawLine(insidePts1[i], insidePts1[i + 1], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[i]), T.MultiplyPoint(insidePts2[i + 1]), Color.yellow);
}
else
{
DrawLine(insidePts1[7], insidePts1[4], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[7]), T.MultiplyPoint(insidePts2[4]), Color.yellow);
}
}
}
// Start is called before the first frame update
void Start()
{
// handle inside points
IList <int> indexes = new List <int>();
int i = 0;
foreach (Vector3 vec in combinations)
{
indexes.Add(i);
i++;
insidePts1.Add(formwork1.transform.TransformPoint(Vector3.Scale(formwork1.transform.localScale / 2, vec)));
insidePts2.Add(formwork2.transform.TransformPoint(Vector3.Scale(formwork2.transform.localScale / 2, vec)));
}
for (int j = 0; j < insidePts1.Count; j++)
{
Debug.Log(insidePts1[j]);
Debug.Log(insidePts2[j]);
}
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = 0; //= (float)(random.NextDouble() * 0.001) * 3; // 0~3 mm
matrixP.SetColumn(column, new Vector4(insidePts2[index].x, insidePts2[index].y + randomDouble, insidePts2[index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(insidePts1[index].x, insidePts1[index].y, insidePts1[index].z, 1));
column++;
}
Debug.Log(matrixP);
Debug.Log(matrixP.inverse);
T_temp = _matrixP * matrixP.inverse;
float errorsum = 0;
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 p = T_temp.MultiplyPoint(insidePts2[index]);
errorsum += Pow2(p.x - insidePts1[index].x) + Pow2(p.y - insidePts1[index].y) + Pow2(p.z - insidePts1[index].z);
}
float error = errorsum / insidePts2.Count;
Debug.Log(String.Format("error: {0}", error));
if (error < min)
{
min = error;
T = T_temp;
}
}
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
for (int index = 0; index < insidePts2.Count; index++)
{
Debug.Log(String.Format("after cal P{0}", index));
Vector3 afterCal = T.MultiplyPoint(insidePts2[index]);
Debug.Log(String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z));
Debug.Log(String.Format("as-designed P{0}", index));
Debug.Log(String.Format("x: {0}, y: {1}, z: {2}", insidePts1[index].x, insidePts1[index].y, insidePts1[index].z));
}
}
public void RunAlgorithm()
{
List <Vector3> insidePts1 = new List <Vector3>(); // as-designed
List <Vector3> insidePts2 = new List <Vector3>(); // as-built
Matrix4x4 T = new Matrix4x4(); // transformation matrix
System.Random random = new System.Random();
IList <int> indexes = new List <int>();
int pointsNum = 10;
// handle inside points
for (int i = 0; i <= pointsNum; i++)
{
indexes.Add(i);
insidePts1.Add(startPt_asdesigned + (endPt_asdesigned - startPt_asdesigned) * i / pointsNum);
insidePts2.Add(startPt_asbuilt + (endPt_asbuilt - startPt_asbuilt) * i / pointsNum);
}
while (!lessthanMin)
{
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
Matrix4x4 after_T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = (float)(random.NextDouble() * 0.001) + additionError; // 0~3 mm
matrixP.SetColumn(column, new Vector4(insidePts2[index].x + randomDouble, insidePts2[index].y + randomDouble, insidePts2[index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(insidePts1[index].x, insidePts1[index].y, insidePts1[index].z, 1));
column++;
}
T_temp = _matrixP * matrixP.inverse;
//Matrix4x4 scaleMatrix = new Matrix4x4();
//float sx = Convert.ToSingle(Math.Sqrt(Convert.ToDouble(Pow2(T_temp.GetColumn(0).x) + Pow2(T_temp.GetColumn(0).y) + Pow2(T_temp.GetColumn(0).z))));
//float sy = Convert.ToSingle(Math.Sqrt(Convert.ToDouble(Pow2(T_temp.GetColumn(1).x) + Pow2(T_temp.GetColumn(1).y) + Pow2(T_temp.GetColumn(1).z))));
//float sz = Convert.ToSingle(Math.Sqrt(Convert.ToDouble(Pow2(T_temp.GetColumn(2).x) + Pow2(T_temp.GetColumn(2).y) + Pow2(T_temp.GetColumn(2).z))));
//scaleMatrix.SetColumn(0, new Vector4(sx, 0, 0, 0));
//scaleMatrix.SetColumn(1, new Vector4(0, sy, 0, 0));
//scaleMatrix.SetColumn(2, new Vector4(0, 0, sz, 0));
//scaleMatrix.SetColumn(3, new Vector4(0, 0, 0, 1));
//after_T_temp = T_temp * scaleMatrix.inverse;
float errorsum = 0;
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 p = T_temp.MultiplyPoint(insidePts2[index]);
errorsum += Pow2(p.x - insidePts1[index].x) + Pow2(p.y - insidePts1[index].y) + Pow2(p.z - insidePts1[index].z);
}
float error = errorsum / insidePts2.Count;
//Debug.Log(String.Format("error: {0}", error));
if (error < min)
{
lessthanMin = true;
min = error;
T = T_temp;
}
}
}
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
string asdesigned = "";
string aftercal = "";
for (int index = 0; index < insidePts2.Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", insidePts1[index].x, insidePts1[index].y, insidePts1[index].z) + "\n";
}
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 afterCal = T.MultiplyPoint(insidePts2[index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
Debug.Log(asdesigned);
Debug.Log(aftercal);
DrawLine(insidePts1[0], insidePts1[insidePts1.Count - 1], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[0]), T.MultiplyPoint(insidePts2[insidePts2.Count - 1]), Color.yellow);
}
public void RunAlgorithm()
{
List <Vector3> insidePts1 = this.Pt_asdesigned; // as-designed
List <Vector3> insidePts2 = this.Pt_asbuilt; // as-built
Matrix4x4 T = new Matrix4x4(); // transformation matrix
System.Random random = new System.Random();
IList <int> indexes = new List <int>();
// handle inside points
for (int i = 0; i < Pt_asdesigned.Count; i++)
{
indexes.Add(i);
}
while (!lessthanMin)
{
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = 0f;// (float)(random.NextDouble() * 0.001) + additionError; // 0~3 mm
matrixP.SetColumn(column, new Vector4(insidePts2[index].x + randomDouble, insidePts2[index].y + randomDouble, insidePts2[index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(insidePts1[index].x, insidePts1[index].y, insidePts1[index].z, 1));
column++;
}
//Debug.Log(matrixP);
//Debug.Log(matrixP.inverse);
T_temp = _matrixP * matrixP.inverse;
float errorsum = 0;
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 p = T_temp.MultiplyPoint(insidePts2[index]);
errorsum += Pow2(p.x - insidePts1[index].x) + Pow2(p.y - insidePts1[index].y) + Pow2(p.z - insidePts1[index].z);
}
float error = errorsum / insidePts2.Count;
//Debug.Log(String.Format("error: {0}", error));
if (error < min)
{
lessthanMin = true;
min = error;
T = T_temp;
}
}
}
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
string asdesigned = "";
string aftercal = "";
for (int index = 0; index < insidePts2.Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", insidePts1[index].x, insidePts1[index].y, insidePts1[index].z) + "\n";
}
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 afterCal = T.MultiplyPoint(insidePts2[index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
Debug.Log(asdesigned);
Debug.Log(aftercal);
for (int i = 0; i < insidePts1.Count; i += 2)
{
DrawLine(insidePts1[i], insidePts1[i + 1], Color.black);
DrawLine(T.MultiplyPoint(insidePts2[i]), T.MultiplyPoint(insidePts2[i + 1]), Color.yellow);
}
Vector3 N4_s = new Vector3(-49.42f, 475.02f, 207f);
Vector3 N4_e = new Vector3(133.04f, -437.67f, 205f);
DrawLine(T.MultiplyPoint(N4_s), T.MultiplyPoint(N4_e), Color.red);
DrawLine(N4_s, N4_e, Color.cyan);
Vector3 N4o_s = new Vector3(-152.15f, 469.42f, 199f);
Vector3 N4o_e = new Vector3(20.86f, -458.59f, 199f);
DrawLine(N4o_s, N4o_e, Color.green);
float error_N4 = 0;
Vector3 p_N4 = T.MultiplyPoint(N4_s);
Vector3 p1_N4 = T.MultiplyPoint(N4_e);
error_N4 += Pow2(p_N4.x - N4_s.x) + Pow2(p_N4.y - N4_s.y) + Pow2(p_N4.z - N4_s.z);
error_N4 += Pow2(p1_N4.x - N4_e.x) + Pow2(p1_N4.y - N4_e.y) + Pow2(p1_N4.z - N4_e.z);
Debug.Log(error_N4 / 2);
}
// Start is called before the first frame update
void Start()
{
while (length < 13f)
{
Vector3 cylinder1_sp;
Vector3 cylinder1_ep;
Vector3 cylinder2_sp;
Vector3 cylinder2_ep;
List <Vector3> insidePts1 = new List <Vector3>(); // as-designed
List <Vector3> insidePts2 = new List <Vector3>(); // as-built
Matrix4x4 T = new Matrix4x4(); // transformation matrix
float min = 0.1f; // Max error
bool lessthanMin = false;
Debug.Log(String.Format("Length: {0}m", length.ToString()));
Vector3 localscale1 = cylinder1.transform.localScale;
cylinder1.transform.localScale = new Vector3(localscale1.x, length / 2, localscale1.z);
Vector3 localscale2 = cylinder1.transform.localScale;
cylinder2.transform.localScale = new Vector3(localscale2.x, length / 2, localscale2.z);
cylinder1_sp = cylinder1.transform.position - cylinder1.transform.up * cylinder1.transform.localScale.y;
cylinder1_ep = cylinder1.transform.position + cylinder1.transform.up * cylinder1.transform.localScale.y;
cylinder2_sp = cylinder2.transform.position - cylinder2.transform.up * cylinder2.transform.localScale.y;
cylinder2_ep = cylinder2.transform.position + cylinder2.transform.up * cylinder2.transform.localScale.y;
IList <int> indexes = new List <int>();
int pointsNum = 10;
// handle inside points
for (int i = 0; i < pointsNum + 1; i++)
{
indexes.Add(i);
insidePts1.Add(cylinder1_sp + cylinder1.transform.up * cylinder1.transform.localScale.y * 2 * i / pointsNum);
insidePts2.Add(cylinder2_sp + cylinder2.transform.up * cylinder2.transform.localScale.y * 2 * i / pointsNum);
}
// algorithm
while (!lessthanMin)
{
foreach (IEnumerable <int> combination in Combinations(indexes, 4))
{
Matrix4x4 matrixP = new Matrix4x4(); // P as-built
Matrix4x4 _matrixP = new Matrix4x4(); // P' as-designed
Matrix4x4 T_temp; // transformation matrix temp
int column = 0;
foreach (int index in combination)
{
// Generate a random number to reset y and z of every as-built point
float randomDouble = (float)(random.NextDouble() * 0.001) + additionError; // 0~3 mm
matrixP.SetColumn(column, new Vector4(insidePts2[index].x + randomDouble, insidePts2[index].y + randomDouble, insidePts2[index].z + randomDouble, 1));
_matrixP.SetColumn(column, new Vector4(insidePts1[index].x, insidePts1[index].y, insidePts1[index].z, 1));
column++;
}
//Debug.Log(matrixP);
//Debug.Log(matrixP.inverse);
T_temp = _matrixP * matrixP.inverse;
float errorsum = 0;
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 p = T_temp.MultiplyPoint3x4(insidePts2[index]);
errorsum += Pow2(p.x - insidePts1[index].x) + Pow2(p.y - insidePts1[index].y) + Pow2(p.z - insidePts1[index].z);
}
float error = errorsum / insidePts2.Count;
//Debug.Log(String.Format("error: {0}", error));
if (error < min)
{
lessthanMin = true;
min = error;
T = T_temp;
}
}
}
string asdesigned = "";
string asbuilt = "";
string aftercal = "";
for (int index = 0; index < insidePts1.Count; index++)
{
asdesigned += String.Format("x: {0}, y: {1}, z: {2}", insidePts1[index].x, insidePts1[index].y, insidePts1[index].z) + "\n";
}
for (int index = 0; index < insidePts2.Count; index++)
{
asbuilt += String.Format("x: {0}, y: {1}, z: {2}", insidePts2[index].x, insidePts2[index].y, insidePts2[index].z) + "\n";
}
for (int index = 0; index < insidePts2.Count; index++)
{
Vector3 afterCal = T.MultiplyPoint3x4(insidePts2[index]);
aftercal += String.Format("x: {0}, y: {1}, z: {2}", afterCal.x, afterCal.y, afterCal.z) + "\n";
}
Debug.Log(asdesigned);
Debug.Log(asbuilt);
Debug.Log(aftercal);
Debug.Log(String.Format("final error: {0}", min));
Debug.Log(T);
length += 1f;
}
}
