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