private void ProcessFrame(byte bodyIndex) { try { RemoveNonBodyPixels(bodyIndex); reconstruction.DepthToDepthFloatFrame(rawDepthData, floatDepthFrame, MIN_DEPTH, MAX_DEPTH, false); var aligned = reconstruction.ProcessFrame( floatDepthFrame, FusionDepthProcessor.DefaultAlignIterationCount, FusionDepthProcessor.DefaultIntegrationWeight, out alignmentEnergy, worldToCameraTransform); if (aligned) { syncContext.Post(() => FrameAligned?.Invoke(this, EventArgs.Empty)); worldToCameraTransform = reconstruction.GetCurrentWorldToCameraTransform(); } } catch (InvalidOperationException) { } try { reconstruction.CalculatePointCloud(pointCloudFrame, worldToCameraTransform); FusionDepthProcessor.ShadePointCloud(pointCloudFrame, worldToCameraTransform, surfaceFrame, null); SurfaceBitmap.Access(data => surfaceFrame.CopyPixelDataTo(data)); syncContext.Post(() => SurfaceBitmapUpdated?.Invoke(this, EventArgs.Empty)); } catch (InvalidOperationException) { } }
/// <summary> /// Up sample color delta from reference frame with nearest neighbor - replicates pixels /// </summary> /// <param name="factor">The up sample factor (2=x*2,y*2, 4=x*4,y*4, 8=x*8,y*8, 16=x*16,y*16).</param> private unsafe void UpsampleColorDeltasFrameNearestNeighbor(int factor) { #region ErrorHandling if (null == downsampledDeltaFromReferenceFrameColorFrame || null == downsampledDeltaFromReferenceColorPixels || null == deltaFromReferenceFramePixelsArgb) { throw new ArgumentException("inputs null"); } if (false == (2 == factor || 4 == factor || 8 == factor || 16 == factor)) { throw new ArgumentException("factor != 2, 4, 8 or 16"); } int upsampleWidth = downsampledWidth * factor; int upsampleHeight = downsampledHeight * factor; if (depthWidth != upsampleWidth || depthHeight != upsampleHeight) { throw new ArgumentException("upsampled image size != depth size"); } #endregion downsampledDeltaFromReferenceFrameColorFrame.CopyPixelDataTo(downsampledDeltaFromReferenceColorPixels); // Here we make use of unsafe code to just copy the whole pixel as an int for performance reasons, as we do // not need access to the individual rgba components. fixed(int *rawColorPixelPtr = downsampledDeltaFromReferenceColorPixels) { int *rawColorPixels = (int *)rawColorPixelPtr; // Note we run this only for the source image height pixels to sparsely fill the destination with rows Parallel.For( 0, downsampledHeight, y => { int destIndex = y * upsampleWidth * factor; int sourceColorIndex = y * downsampledWidth; for (int x = 0; x < downsampledWidth; ++x, ++sourceColorIndex) { int color = rawColorPixels[sourceColorIndex]; // Replicate pixels horizontally for (int colFactorIndex = 0; colFactorIndex < factor; ++colFactorIndex, ++destIndex) { // Replicate pixels vertically for (int rowFactorIndex = 0; rowFactorIndex < factor; ++rowFactorIndex) { // Copy color pixel deltaFromReferenceFramePixelsArgb[destIndex + (rowFactorIndex * upsampleWidth)] = color; } } } }); } int sizeOfInt = sizeof(int); int rowByteSize = downsampledHeight * sizeOfInt; // Duplicate the remaining rows with memcpy for (int y = 0; y < downsampledHeight; ++y) { // iterate only for the smaller number of rows int srcRowIndex = upsampleWidth * factor * y; // Duplicate lines for (int r = 1; r < factor; ++r) { int index = upsampleWidth * ((y * factor) + r); System.Buffer.BlockCopy( deltaFromReferenceFramePixelsArgb, srcRowIndex * sizeOfInt, deltaFromReferenceFramePixelsArgb, index * sizeOfInt, rowByteSize); } } }
void DrawFusionFrame() { int[] surfaceImagePixels = new int[depthWidth * depthHeight]; surfaceImageFrame.CopyPixelDataTo(surfaceImagePixels); drawBitmap.WritePixels(drawRect, surfaceImagePixels, drawStride, 0); }
private void ProcessDepthData(DepthImagePixel[] depthPixels) { try { if (processingSaveFile) { return; } // DepthImagePixel から DepthFloatFrame に変換する FusionDepthProcessor.DepthToDepthFloatFrame( depthPixels, DepthWidth, DepthHeight, depthFloatBuffer, FusionDepthProcessor.DefaultMinimumDepth, FusionDepthProcessor.DefaultMaximumDepth, false); // フレームを処理する bool trackingSucceeded = volume.ProcessFrame( depthFloatBuffer, FusionDepthProcessor.DefaultAlignIterationCount, FusionDepthProcessor.DefaultIntegrationWeight, volume.GetCurrentWorldToCameraTransform()); if (!trackingSucceeded) { // 一定数エラーになったらリセット // Kinectまたは対象を素早く動かしすぎ などの場合 trackingErrorCount++; if (trackingErrorCount >= 100) { Trace.WriteLine(@"tracking error."); trackingErrorCount = 0; volume.ResetReconstruction(Matrix4.Identity); } return; } // PointCloudを取得する volume.CalculatePointCloud( pointCloudBuffer, volume.GetCurrentWorldToCameraTransform()); // PointCloudを2次元のデータに描画する FusionDepthProcessor.ShadePointCloud( pointCloudBuffer, volume.GetCurrentWorldToCameraTransform(), shadedSurfaceColorFrame, null); // 2次元のデータをBitmapに書きだす var colorPixels = new int[depthPixels.Length]; shadedSurfaceColorFrame.CopyPixelDataTo(colorPixels); ImageKinectFusion.Source = BitmapSource.Create(DepthWidth, DepthHeight, 96, 96, PixelFormats.Bgr32, null, colorPixels, DepthWidth * 4); } catch (Exception ex) { Trace.WriteLine(ex.Message); } finally { processingFrame = false; } }
/// <summary> /// processes the depth data package into the kinect fusion volume /// </summary> /// <param name="pKdp">the data package</param> void processDepthData(KinectDataPackage pKdp, System.Threading.CancellationToken pCancelToken) { lock (canWorkLock) { Log.LogManager.updateAlgorithmStatus("Kinect Fusion integration"); this.volume.ResetReconstruction(Matrix4.Identity); int picturesIntegrated = 0; foreach (ushort[] pDepth in pKdp.rawDepthData) { pCancelToken.ThrowIfCancellationRequested(); WriteableBitmap bitmap = new WriteableBitmap(this.depthFloatFrame.Width, this.depthFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null); FusionFloatImageFrame depthFloatBuffer = new FusionFloatImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height); FusionPointCloudImageFrame pointCloudBuffer = new FusionPointCloudImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height); FusionColorImageFrame shadedSurfaceColorFrame = new FusionColorImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height); int[] voxelPixels = new int[this.depthFloatFrame.Width * this.depthFloatFrame.Height]; this.volume.DepthToDepthFloatFrame( pDepth, depthFloatBuffer, Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.minDepthClip, Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.maxDepthClip, false); float alignmentValue; bool trackingSucceeded = this.volume.ProcessFrame(depthFloatBuffer, Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.iterationCount, Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.integrationWeight, out alignmentValue, volume.GetCurrentWorldToCameraTransform()); // If camera tracking failed, no data integration or raycast for reference // point cloud will have taken place, and the internal camera pose // will be unchanged. if (!trackingSucceeded) { trackingErrorCount++; } else { Matrix4 calculatedCameraPose = volume.GetCurrentWorldToCameraTransform(); // Set the camera pose and reset tracking errors worldToCameraTransform = calculatedCameraPose; trackingErrorCount = 0; } // Calculate the point cloud volume.CalculatePointCloud(pointCloudBuffer, worldToCameraTransform); // Shade point cloud and render FusionDepthProcessor.ShadePointCloud( pointCloudBuffer, worldToCameraTransform, null, shadedSurfaceColorFrame ); shadedSurfaceColorFrame.CopyPixelDataTo(voxelPixels); bitmap.WritePixels( new Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight), voxelPixels, bitmap.PixelWidth * sizeof(int), 0); bitmap.Freeze(); OnNewFusionPictureEvent.BeginInvoke(pKdp.usedConfig.ID, bitmap, null, null); picturesIntegrated++; Log.LogManager.writeLogDebug("[DataIntegration:Reconstruction] " + picturesIntegrated + " of " + pKdp.rawDepthData.Count + " Pictures integrated"); } //if request was calibration request, export meshes if (pKdp.usedConfig.clientRequestObject.requestType == ClientConfigObject.RequestType.calibration) { exportMesh(volume, pKdp, false); Log.LogManager.writeLog("[DataIntegration:Reconstruction] Mesh of " + pKdp.usedConfig.name + " exported."); return; } //broadcast new point cloud PointCloud p = new PointCloud(volume); p.ConfigObject = pKdp.usedConfig; OnNewPointCloudEvent.BeginInvoke(p, null, null); Log.LogManager.writeLog("[DataIntegration:Reconstruction] All pictures of " + pKdp.usedConfig.name + " integrated"); Log.LogManager.updateAlgorithmStatus("Done"); } }