Пример #1
0
        /// <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
            }
        }
Пример #2
0
        /// <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;
        }