x/PointCloudWeb
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

correct ScanConverterService with new calculating method

Browse Source
This commit is contained in:
Tim Wundenberg
2021-07-16 21:46:14 +02:00
parent 4f2aba9e6d
commit 90c3ab4a51
2 changed files with 55 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
PointCloudWeb.Server/PointCloudWeb.Server.Tests/Services/ScanConverterServiceTest.cs
View File

@@ -10,34 +10,44 @@ namespace PointCloudWeb.Server.Tests.Services
[Theory]
[InlineData(1, 1, 1)]
//[InlineData(-1, 1, 1)]
//[InlineData(1, -1, 1)]
[InlineData(2, 2, 2)]
[InlineData(2, 1, 1)]
[InlineData(1, 2, 1)]
[InlineData(1, 1, 2)]
[InlineData(10, 15, 32)]
[InlineData(5, 12, 7)]
[InlineData(-1, 1, 1)]
[InlineData(1, -1, 1)]
[InlineData(-1, -1, 1)]
[InlineData(-5, 12, 7)]
[InlineData(-5, -12, 7)]
[InlineData(-22, 13, 11)]
//[InlineData(1, 1, -1)]
//[InlineData(-1, -1, 1)]
//[InlineData(-1, 1, -1)]
//[InlineData(1, -1, -1)]
//[InlineData(10, 15, 32)]
//[InlineData(5, 12, 7)]
//[InlineData(-5, 12, 7)]
//[InlineData(5, -12, 7)]
//[InlineData(5, 12, -7)]
//[InlineData(-5, -12, 7)]
//[InlineData(-5, 12, -7)]
//[InlineData(-5, -12, -7)]
//[InlineData(-22, 13, 11)]
public static void ScanConverterTest(int x, int y, int z)
{
var scan = new ScanDataPoint
{
RAX = 45,//(Math.Acos(y / Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2))) * 180 / Math.PI),
RAY = 45,//(Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI),
//RAY = 360 - (Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI),
//RAY = 180 - (Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI),
RAX = Math.Acos(y / Math.Sqrt(Math.Pow(y, 2) + Math.Pow(z, 2))),
RAY = Math.Acos(x / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))),
DistanceMM = (float)Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2))
};
//from rad to degree
scan.RAX = scan.RAX * 180 / Math.PI;
scan.RAY = scan.RAY * 180 / Math.PI;
var expected = new Point(x, y, z);
var service = new ScanConverterService();
var point = service.Transform(scan);
Assert.Equal(expected, point);
//if (scan.RAX >= 0 && scan.RAX < 90)
// scan.RAY = 360 - scan.RAY;
//else if (scan.RAX >= 90 && scan.RAX < 180)
@@ -55,18 +65,6 @@ namespace PointCloudWeb.Server.Tests.Services
// sin *= -1;
//}
var expected = new Point(x, y, z);
var service = new ScanConverterService();
var point = service.Transform(scan);
//Assert.Equal(expected, point);
Assert.True(
point.X >= expected.X - 1 && point.X <= expected.X + 1
&& point.Y >= expected.Y - 1 && point.Y <= expected.Y + 1
&& point.Z >= expected.Z - 1 && point.Z <= expected.Z + 1,
"expected: " + expected.ToString() + " \nactual: " + point.ToString() +
"\nAX: " + scan.RAX + "\n AY: " + scan.RAY
);
}
}
}

98
PointCloudWeb.Server/PointCloudWeb.Server/Services/ScanConverterService.cs
View File

@@ -6,7 +6,6 @@ namespace PointCloudWeb.Server.Services
{
public class ScanConverterService
{
private enum RotationAxis { X, Y, Z };
private static void CorrectQuadrants(double angle, ref float sin, ref float cos)
{
@@ -27,79 +26,42 @@ namespace PointCloudWeb.Server.Services
}
}
private static Matrix4x4 GetTransformationMatrix(ScanDataPoint scan, RotationAxis type)
{
double angle = type switch
{
RotationAxis.X => scan.RAX,//360 - 90 + scan.RAX,
RotationAxis.Y => -scan.RAY,//360 - scan.RAY,
RotationAxis.Z => 0,//360 - scan.RAY,
_ => throw new NotImplementedException(),
};
if (angle == 0)
return new Matrix4x4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
angle %= 360;
if (angle < 0)
angle += 360;
var angleInR = angle * (Math.PI / 180.0);
var sin = (float)Math.Sin(angleInR);
var cos = (float)Math.Cos(angleInR);
//CorrectQuadrants(angle, ref sin, ref cos);
return type switch
{
RotationAxis.X => new Matrix4x4(
1, 0, 0, 0,
0, cos, -sin, 0,
0, sin, cos, 0,
0, 0, 0, 1
),
RotationAxis.Y => new Matrix4x4(
cos, 0, sin, 0,
0, 1, 0, 0,
-sin, 0, cos, 0,
0, 0, 0, 1
),
RotationAxis.Z => new Matrix4x4(
cos, -sin, 0, 0,
sin, cos, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
),
_ => throw new NotImplementedException(),
};
}
private int Round(double value) => (int)Math.Round(value, 0, MidpointRounding.AwayFromZero);
public Point Transform(ScanDataPoint scan)
{
Vector3 v = new Vector3(0, 0, 0);
var degreeXA = scan.RAX;
var degreeXB = 180 - 90 - degreeXA;
var degreeYA = scan.RAY;
var degreeYB = 180 - 90 - degreeYA;
v.Z = scan.DistanceMM;
var radXA = degreeXA * Math.PI / 180;
var radXB = degreeXB * Math.PI / 180;
var radYA = degreeYA * Math.PI / 180;
var radYB = degreeYB * Math.PI / 180;
var matrixX = GetTransformationMatrix(scan, RotationAxis.X);
var matrixY = GetTransformationMatrix(scan, RotationAxis.Y);
var matrixZ = GetTransformationMatrix(scan, RotationAxis.Z);
double sinXA = Math.Sin(radXA);
double sinXB = Math.Sin(radXB);
double sinYA = Math.Sin(radYA);
double sinYB = Math.Sin(radYB);
v = Vector3.Transform(v, matrixX);
v = Vector3.Transform(v, matrixY);
v = Vector3.Transform(v, matrixZ);
return new Point(
(int)Math.Round(v.X, 0, MidpointRounding.AwayFromZero),
(int)Math.Round(v.Y, 0, MidpointRounding.AwayFromZero),
(int)Math.Round(v.Z, 0, MidpointRounding.AwayFromZero)
var z = Math.Sqrt(
Math.Pow(
Math.Pow(sinXB, 2) / Math.Pow(sinXA, 2)
+ Math.Pow(sinYB, 2) / Math.Pow(sinYA, 2)
+ 1
, -1)
* Math.Pow(scan.DistanceMM, 2)
);
var p = new Point()
{
X = Round(z * sinYB / sinYA),
Y = Round(z * sinXB / sinXA),
Z = Round(z)
};
return p;
}
}
}