kinectfusion程序

//------------------------------------------------------------------------------ // <copyright file="KinectFusionProcessor.cpp" company="Microsoft"> // Copyright (c) Microsoft Corporation. All rights reserved. // </copyright> //------------------------------------------------------------------------------ // System includes #include "stdafx.h" #pragma warning(push) #pragma warning(disable:6255) #pragma warning(disable:6263) #pragma warning(disable:4995) #include "ppl.h" #pragma warning(pop) // Project includes #include "KinectFusionProcessor.h" #include "KinectFusionHelper.h" #include "resource.h" #define AssertOwnThread() \ _ASSERT_EXPR(GetCurrentThreadId() == m_threadId, __FUNCTIONW__ L" called on wrong thread!"); #define AssertOtherThread() \ _ASSERT_EXPR(GetCurrentThreadId() != m_threadId, __FUNCTIONW__ L" called on wrong thread!"); HRESULT KinectFusionProcessor::CopyDepth( IDepthFrame* pDepthFrame ) { // Check the frame pointer if (NULL == pDepthFrame) { return E_INVALIDARG; } UINT nBufferSize = 0; UINT16 *pBuffer = NULL; HRESULT hr = pDepthFrame->AccessUnderlyingBuffer(&nBufferSize, &pBuffer); if (FAILED(hr)) { return hr; } //copy and remap depth const UINT bufferLength = NUI_DEPTH_RAW_HEIGHT * NUI_DEPTH_RAW_WIDTH; UINT16 * pDepth = m_pDepthUndistortedPixelBuffer; UINT16 * pRawDepth = m_pDepthRawPixelBuffer; for(UINT i = 0; i < bufferLength; i++, pDepth++, pRawDepth++) { const UINT id = m_pDepthDistortionLT[i]; *pDepth = id < bufferLength? pBuffer[id] : 0; *pRawDepth = pBuffer[i]; } return S_OK; } /// <summary> /// Constructor /// </summary> KinectFusionProcessor::KinectFusionProcessor() : m_hWnd(nullptr), m_msgFrameReady(WM_NULL), m_msgUpdateSensorStatus(WM_NULL), m_hThread(nullptr), m_threadId(0), m_pVolume(nullptr), m_hrRecreateVolume(S_OK), m_pNuiSensor(nullptr), m_cLostFrameCounter(0), m_bTrackingFailed(false), m_cFrameCounter(0), m_cFPSFrameCounter(0), m_fFrameCounterStartTime(0), m_cLastDepthFrameTimeStamp(0), m_cLastColorFrameTimeStamp(0), m_fMostRecentRaycastTime(0), m_pDepthUndistortedPixelBuffer(nullptr), m_pDepthRawPixelBuffer(nullptr), m_pColorCoordinates(nullptr), m_pDepthVisibilityTestMap(nullptr), m_pDepthDistortionMap(nullptr), m_pDepthDistortionLT(nullptr), m_pMapper(nullptr), m_pDepthFloatImage(nullptr), m_pColorImage(nullptr), m_pResampledColorImage(nullptr), m_pResampledColorImageDepthAligned(nullptr), m_pSmoothDepthFloatImage(nullptr), m_pDepthPointCloud(nullptr), m_pRaycastPointCloud(nullptr), m_pRaycastDepthFloatImage(nullptr), m_pShadedSurface(nullptr), m_pShadedSurfaceNormals(nullptr), m_pCapturedSurfaceColor(nullptr), m_pFloatDeltaFromReference(nullptr), m_pShadedDeltaFromReference(nullptr), m_bKinectFusionInitialized(false), m_bResetReconstruction(false), m_bResolveSensorConflict(false), m_hStopProcessingEvent(INVALID_HANDLE_VALUE), m_pCameraPoseFinder(nullptr), m_bTrackingHasFailedPreviously(false), m_pDownsampledDepthFloatImage(nullptr), m_pDownsampledSmoothDepthFloatImage(nullptr), m_pDownsampledDepthPointCloud(nullptr), m_pDownsampledShadedDeltaFromReference(nullptr), m_pDownsampledRaycastPointCloud(nullptr), m_bCalculateDeltaFrame(false), m_coordinateMappingChangedEvent(NULL), m_bHaveValidCameraParameters(false) { // Initialize synchronization objects InitializeCriticalSection(&m_lockParams); InitializeCriticalSection(&m_lockFrame); InitializeCriticalSection(&m_lockVolume); m_hStopProcessingEvent = CreateEvent( nullptr, TRUE, /* bManualReset */ FALSE, /* bInitialState */ nullptr ); SetIdentityMatrix(m_worldToCameraTransform); SetIdentityMatrix(m_defaultWorldToVolumeTransform); // We don't know these at object creation time, so we use nominal values. // These will later be updated in response to the CoordinateMappingChanged event. m_cameraParameters.focalLengthX = NUI_KINECT_DEPTH_NORM_FOCAL_LENGTH_X; m_cameraParameters.focalLengthY = NUI_KINECT_DEPTH_NORM_FOCAL_LENGTH_Y; m_cameraParameters.principalPointX = NUI_KINECT_DEPTH_NORM_PRINCIPAL_POINT_X; m_cameraParameters.principalPointY = NUI_KINECT_DEPTH_NORM_PRINCIPAL_POINT_Y; } /// <summary> /// Destructor /// </summary> KinectFusionProcessor::~KinectFusionProcessor() { AssertOtherThread(); // Shutdown the sensor StopProcessing(); // Clean up Kinect Fusion SafeRelease(m_pVolume); if (nullptr != m_pMapper) m_pMapper->UnsubscribeCoordinateMappingChanged(m_coordinateMappingChangedEvent); SafeRelease(m_pMapper); // Clean up Kinect Fusion Camera Pose Finder SafeRelease(m_pCameraPoseFinder); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pColorImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pResampledColorImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pResampledColorImageDepthAligned); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pRaycastPointCloud); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pRaycastDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pShadedSurface); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pShadedSurfaceNormals); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pCapturedSurfaceColor); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pFloatDeltaFromReference); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pShadedDeltaFromReference); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pSmoothDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDepthPointCloud); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDownsampledDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDownsampledSmoothDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDownsampledDepthPointCloud); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDownsampledShadedDeltaFromReference); // Clean up the depth pixel array SAFE_DELETE_ARRAY(m_pDepthUndistortedPixelBuffer); SAFE_DELETE_ARRAY(m_pDepthRawPixelBuffer); // Clean up the color coordinate array SAFE_DELETE_ARRAY(m_pColorCoordinates); SAFE_DELETE_ARRAY(m_pDepthVisibilityTestMap); SAFE_DELETE_ARRAY(m_pDepthDistortionMap); SAFE_DELETE_ARRAY(m_pDepthDistortionLT); // Clean up synchronization object if (m_hStopProcessingEvent != INVALID_HANDLE_VALUE) { CloseHandle(m_hStopProcessingEvent); } // done with depth frame reader SafeRelease(m_pDepthFrameReader); SafeRelease(m_pColorFrameReader); DeleteCriticalSection(&m_lockParams); DeleteCriticalSection(&m_lockFrame); DeleteCriticalSection(&m_lockVolume); } /// <summary> /// Shuts down the sensor /// </summary> void KinectFusionProcessor::ShutdownSensor() { AssertOwnThread(); // Clean up Kinect if (m_pNuiSensor != nullptr) { m_pNuiSensor->Close(); SafeRelease(m_pNuiSensor); } } /// <summary> /// Starts Kinect Fusion processing. /// </summary> /// <param name="phThread">returns the new processing thread's handle</param> HRESULT KinectFusionProcessor::StartProcessing() { AssertOtherThread(); if (m_hThread == nullptr) { m_hThread = CreateThread(nullptr, 0, ThreadProc, this, 0, &m_threadId); } return (m_hThread != nullptr) ? S_OK : HRESULT_FROM_WIN32(GetLastError()); } /// <summary> /// Stops Kinect Fusion processing. /// </summary> HRESULT KinectFusionProcessor::StopProcessing() { AssertOtherThread(); if (m_hThread != nullptr)