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] [Theory]
[InlineData(1, 1, 1)] [InlineData(1, 1, 1)]
//[InlineData(-1, 1, 1)] [InlineData(2, 2, 2)]
//[InlineData(1, -1, 1)] [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(-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(-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) public static void ScanConverterTest(int x, int y, int z)
{ {
var scan = new ScanDataPoint 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), RAX = Math.Acos(y / Math.Sqrt(Math.Pow(y, 2) + Math.Pow(z, 2))),
RAY = 45,//(Math.Acos(z / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))) * 180 / Math.PI), RAY = Math.Acos(x / Math.Sqrt(Math.Pow(z, 2) + Math.Pow(x, 2))),
//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),
DistanceMM = (float)Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 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) //if (scan.RAX >= 0 && scan.RAX < 90)
// scan.RAY = 360 - scan.RAY; // scan.RAY = 360 - scan.RAY;
//else if (scan.RAX >= 90 && scan.RAX < 180) //else if (scan.RAX >= 90 && scan.RAX < 180)
@@ -55,18 +65,6 @@ namespace PointCloudWeb.Server.Tests.Services
// sin *= -1; // 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 public class ScanConverterService
{ {
private enum RotationAxis { X, Y, Z };
private static void CorrectQuadrants(double angle, ref float sin, ref float cos) 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) private int Round(double value) => (int)Math.Round(value, 0, MidpointRounding.AwayFromZero);
{
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(),
};
}
public Point Transform(ScanDataPoint scan) 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); double sinXA = Math.Sin(radXA);
var matrixY = GetTransformationMatrix(scan, RotationAxis.Y); double sinXB = Math.Sin(radXB);
var matrixZ = GetTransformationMatrix(scan, RotationAxis.Z); double sinYA = Math.Sin(radYA);
double sinYB = Math.Sin(radYB);
v = Vector3.Transform(v, matrixX); var z = Math.Sqrt(
v = Vector3.Transform(v, matrixY); Math.Pow(
v = Vector3.Transform(v, matrixZ); Math.Pow(sinXB, 2) / Math.Pow(sinXA, 2)
+ Math.Pow(sinYB, 2) / Math.Pow(sinYA, 2)
return new Point( + 1
(int)Math.Round(v.X, 0, MidpointRounding.AwayFromZero), , -1)
(int)Math.Round(v.Y, 0, MidpointRounding.AwayFromZero), * Math.Pow(scan.DistanceMM, 2)
(int)Math.Round(v.Z, 0, MidpointRounding.AwayFromZero)
); );
var p = new Point()
{
X = Round(z * sinYB / sinYA),
Y = Round(z * sinXB / sinXA),
Z = Round(z)
};
return p;
} }
} }
} }