public Vector2 Predict(PoseAndEyeAndFace data)
{
var camera = new CoordinateSystem();
var displayNormRealign = UnitVector3D.YAxis;
var displayXPositiveRealign = UnitVector3D.XAxis;
var refHeadPositionRaw = Point3D.Centroid(Samples.Select(r => r.HeadPose.Position).Select(r => r.ToMathNetPoint3D()));
var refHeadPosition = ConvertPointFromOpenFaceSpaceToMathNetSpace(refHeadPositionRaw);
var refHeadRotationRaw = Point3D.Centroid(Samples.Select(r => r.HeadPose.Angle).Select(r => r.ToMathNetPoint3D()));
var refHeadRotation = ConvertAngelFromOpenFaceSpaceToMathNetSpace(refHeadRotationRaw);
var refHeadRotationCoord = camera
.RotateCoordSysAroundVector(UnitVector3D.XAxis, Angle.FromRadians(refHeadRotation.X))
.RotateCoordSysAroundVector(UnitVector3D.ZAxis, Angle.FromRadians(refHeadRotation.Z))
.RotateCoordSysAroundVector(UnitVector3D.YAxis, Angle.FromRadians(refHeadRotation.Y));
var refHeadRotationAlign = CoordinateSystem.CreateMappingCoordinateSystem(camera, refHeadRotationCoord);
var refHeadRotationUnalign = CoordinateSystem.CreateMappingCoordinateSystem(refHeadRotationCoord, camera);
var displayWidth = HeadDisplayDistance * Tan(FieldOfViewWidthSingleSideAngle) * 2;
var displayHeight = displayWidth / DisplayAspectRatio;
var displaySize = new Vector2D(displayWidth, displayHeight);
var refDisplayNorm = displayNormRealign.TransformBy(refHeadRotationUnalign).Normalize();
var refDisplayXPositive = displayXPositiveRealign.TransformBy(refHeadRotationUnalign).Normalize();
var refDisplayCentroid = refHeadPosition + HeadDisplayDistance * refDisplayNorm;
var refDisplayPlane = new MathNet.Spatial.Euclidean.Plane(refDisplayCentroid, refDisplayNorm);
var headPosition = ConvertPointFromOpenFaceSpaceToMathNetSpace(data.Pose.Position.ToMathNetPoint3D());
var headRotation = ConvertAngelFromOpenFaceSpaceToMathNetSpace(data.Pose.Angle.ToMathNetPoint3D());
var headRotationCoord = camera
.RotateCoordSysAroundVector(UnitVector3D.XAxis, Angle.FromRadians(headRotation.X))
.RotateCoordSysAroundVector(UnitVector3D.ZAxis, Angle.FromRadians(headRotation.Z))
.RotateCoordSysAroundVector(UnitVector3D.YAxis, Angle.FromRadians(headRotation.Y));
var headRotationAlign = CoordinateSystem.CreateMappingCoordinateSystem(camera, headRotationCoord);
var headRotationUnalign = CoordinateSystem.CreateMappingCoordinateSystem(headRotationCoord, camera);
var displayNorm = displayNormRealign.TransformBy(headRotationUnalign).Normalize();
var displayXPositive = displayXPositiveRealign.TransformBy(headRotationUnalign).Normalize();
var displayCentroid = headPosition + HeadDisplayDistance * displayNorm;
var displayPlane = new MathNet.Spatial.Euclidean.Plane(displayCentroid, displayNorm);
var calibPoints = Samples.Select(r => CalibPointOnDisplay(displayCentroid, displayNorm, displayXPositive, displaySize, r.Display.ToMathNetPoint2D())).ToArray();
var calibRays = calibPoints.Select(p => new Ray3D(headPosition, p - headPosition)).ToArray();
var transformedCalibPoints = calibRays.Select(r => (Point3D)r.IntersectionWith(refDisplayPlane)).ToArray();
var transformedRelatives = transformedCalibPoints.Select(p => RelativeToDisplay(refDisplayCentroid, refDisplayNorm, refDisplayXPositive, displaySize, p)).ToArray();
var pupilDistances = Samples.Select(r => PupilCornerDistance(r.Gaze, r.HeadPose)).ToArray();
var inputX = pupilDistances.Select(d => d.X).ToArray();
var outputX = transformedRelatives.Select(p => p.X).ToArray();
var polynomialX = Polynomial.Fit(inputX, outputX, Order);
var inputY = pupilDistances.Select(d => d.Y).ToArray();
var outputY = transformedRelatives.Select(p => p.Y).ToArray();
var polynomialY = Polynomial.Fit(inputY, outputY, Order);
var distance = PupilCornerDistance(data.Eye, data.Pose);
var x = polynomialX.Evaluate(distance.X);
var y = polynomialY.Evaluate(distance.Y);
return(new Vector2((float)x, (float)y));
}
private void MakeMathPlane()
{
UnitVector3D normal = new UnitVector3D(
Math.Sin(this.Angle * Math.PI / 180) * Math.Sin(this.Dip * Math.PI / 180),
Math.Sin(this.Angle * Math.PI / 180) * Math.Cos(this.Dip * Math.PI / 180),
Math.Cos(this.Angle * Math.PI / 180));
Point3D rootPoint = new Point3D(RootPoint.X, RootPoint.Y, RootPoint.Z);
this.mathPlane = new MathNet.Spatial.Euclidean.Plane(rootPoint, normal);
}
/// <summary>
/// 生成曲面
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void buttonXBuild_Click(object sender, EventArgs e)
{
double attitudeL = this.doubleInputAttitudeLength.Value;
double shapeR = this.doubleInputShapeRadium.Value;
// 生成产状数据集
double nx = 0.0, ny = 0.0, nz = 0.0;
List <Point> sourceAttitudePoints = new List <Point>(this.targetMarkersList.Count * 4);
foreach (var marker in this.targetMarkersList)
{
nx += Math.Sin(marker.MyDip * Math.PI / 180) * Math.Sin(marker.MyAngle * Math.PI / 180)
/ this.targetMarkersList.Count;
ny += Math.Cos(marker.MyDip * Math.PI / 180) * Math.Sin(marker.MyAngle * Math.PI / 180)
/ this.targetMarkersList.Count;
nz += Math.Cos(marker.MyAngle * Math.PI / 180) / this.targetMarkersList.Count;
double dx = -Math.Tan(marker.MyAngle * Math.PI / 180) * Math.Cos(marker.MyDip * Math.PI / 180);
double dy = -Math.Tan(marker.MyAngle * Math.PI / 180) * Math.Sin(marker.MyDip * Math.PI / 180);
Point northPoint = new Point(
marker.X,
marker.Y + attitudeL,
marker.Z + dy * attitudeL);
Point southPoint = new Point(
marker.X,
marker.Y - attitudeL,
marker.Z - dy * attitudeL);
Point eastPoint = new Point(
marker.X + attitudeL,
marker.Y,
marker.Z + dx * attitudeL);
Point westPoint = new Point(
marker.X - attitudeL,
marker.Y,
marker.Z - dx * attitudeL);
sourceAttitudePoints.AddRange(new[] { northPoint, southPoint, eastPoint, westPoint });
}
Point middlePoint = CurveAlgorithm.MiddlePointOfPoints(this.targetMarkersList);
// 平均产状平面
Vector3D n = new Vector3D(nx, ny, nz);
var plane = new MathNet.Spatial.Euclidean.Plane(
new Point3D(middlePoint.X, middlePoint.Y, middlePoint.Z),
n.Normalize());
double a0 = -plane.D / plane.C;
double a1 = -plane.A / plane.C;
double a2 = -plane.B / plane.C;
// 插值点数据集
List <Point> sourcePoints = new List <Point>(this.targetMarkersList);
sourcePoints.AddRange(sourceAttitudePoints);
SurfaceEquation surfaceEquation = new SurfaceEquation(a0, a1, a2, sourcePoints, shapeR);
// 确定曲面区域
List <Point> edgePoints = CurveAlgorithm.GetEdgePoints(sourcePoints, this.GridEdgeLength);
// 区域内插加密点 GeoHelper.InsertPointsInPolygon
List <Point> pointsList = GeoHelper.InsertPointsInPolygon(edgePoints, this.GridEdgeLength);
// 生成网格 Triangulations
Triangulations tris = new Triangulations(pointsList, new List <Point>());
// 计算插值 SurfaceMesh
tris.MeshSurface(surfaceEquation);
// 绘制曲面
IColor66 fillColor = this.sgworld.Creator.CreateColor(128, 128, 128, 128);
IColor66 lineColor = this.sgworld.Creator.CreateColor(255, 255, 255, 0);
var parentGid = GeoHelper.CreateGroup("产状地质曲面", ref this.sgworld);
Facet facet = new Facet(ref this.sgworld, tris.TsData, "Test", parentGid, lineColor, fillColor);
// facet.DrawFacet();
// 保存三角网结果
TsFile ts = new TsFile(
tris.TsData,
"TSurf",
"M",
"JGM",
"Name",
new List <string>());
ts.WriteTsFile();
ts.UpdateTsFile(ref this.db);
ToastNotification.Show(this, "曲面模型已保存为模型部件", 2500, eToastPosition.MiddleCenter);
}