/// <summary> /// Update frustum graphics transform /// </summary> /// <param name="worldToCameraMatrix4">The transformation.</param> public void UpdateFrustumTransformMatrix4(Matrix4 worldToCameraMatrix4) { // Update the frustum graphics if we have created them if (null != this.CameraFrustum) { Matrix3D transform = KinectFusionHelper.ConvertMatrix4ToMatrix3D(worldToCameraMatrix4); Quaternion q = KinectFusionHelper.Matrix3DToQuaternion(transform); double angle = q.Angle; Vector3D axis = q.Axis; axis.X = -axis.X; // negate x rotation Quaternion newq = new Quaternion(axis, angle); Transform3DGroup cumulativeFrustumViewTransform = new Transform3DGroup(); cumulativeFrustumViewTransform.Children.Add(new TranslateTransform3D(new Vector3D(-transform.OffsetX, transform.OffsetY, transform.OffsetZ))); cumulativeFrustumViewTransform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(axis, angle))); this.cameraFrustumTransform3D.Matrix = cumulativeFrustumViewTransform.Value; // Set the camera transform to the frustum graphics } }
/// <summary> /// Camera frustum 3D graphics /// </summary> /// <param name="viewport">The viewport.</param> /// <param name="near">The near plane of the frustum.</param> /// <param name="far">The far plane of the frustum.</param> /// <param name="depthWidth">The width of the depth image.</param> /// <param name="depthHeight">The height of the depth image.</param> /// <param name="color">The color to draw the frustum.</param> /// <param name="thickness">The line thickness to use when drawing the frustum.</param> public void CreateFrustum3DGraphics(Viewport3D viewport, float near, float far, float depthWidth, float depthHeight, System.Windows.Media.Color color, int thickness) { if (null == viewport) { return; } this.graphicsViewport = viewport; // De-normalize default camera params float px = camParams.PrincipalPointX * depthWidth; float py = camParams.PrincipalPointY * depthHeight; float fx = camParams.FocalLengthX * depthWidth; float fy = camParams.FocalLengthY * depthHeight; float iflx = 1.0f / fx; float ifly = 1.0f / fy; this.CameraFrustum = new ScreenSpaceLines3D(); this.CameraFrustum.Points = new Point3DCollection(); Point3DCollection pts = this.CameraFrustum.Points; // Near plane rectangle pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly)); // Far plane rectangle pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly)); // Connecting lines pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly)); pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly)); this.CameraFrustum.Thickness = thickness; this.CameraFrustum.Color = color; // Add a fixed rotation around the Y axis to look back down +Z towards origin Matrix3D fixedRotY180 = new Matrix3D(); fixedRotY180.Rotate(new Quaternion(new Vector3D(0, 1, 0), 180)); this.cumulativeCameraFrustumTransform.Children.Add(new MatrixTransform3D(fixedRotY180)); this.cumulativeCameraFrustumTransform.Children.Add(this.cameraFrustumTransform3D); this.CameraFrustum.Transform = this.cumulativeCameraFrustumTransform; }