KinectSDK2.0基本Fusion

#include "stdafx.h" #include "included.h" #define TITLE L"Title" #define WNDWIDTH 1024 #define WNDHEIGHT 768 #define lengthof(a) sizeof(a)/sizeof(*a) void SetIdentityMatrix(Matrix4 &mat) { mat.M11 = 1.f; mat.M12 = 0.f; mat.M13 = 0.f; mat.M14 = 0.f; mat.M21 = 0.f; mat.M22 = 1.f; mat.M23 = 0.f; mat.M24 = 0.f; mat.M31 = 0.f; mat.M32 = 0.f; mat.M33 = 1.f; mat.M34 = 0.f; mat.M41 = 0.f; mat.M42 = 0.f; mat.M43 = 0.f; mat.M44 = 1.f; } // 重置 void ThisApp::ResetReconstruction(){ m_pReconstruction->ResetReconstruction(&m_worldToCameraTransform, &m_defaultWorldToVolumeTransform); //m_pReconstruction->ResetReconstruction(nullptr, nullptr); } // ThisApp构造函数 ThisApp::ThisApp(){ // 重建参数 m_reconstructionParams.voxelsPerMeter = 256.f; m_reconstructionParams.voxelCountX = 384; m_reconstructionParams.voxelCountY = 384; m_reconstructionParams.voxelCountZ = 384; // 使用AMP进行运算 m_processorType = NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMP; // 设置为单位矩阵 SetIdentityMatrix(m_worldToCameraTransform); SetIdentityMatrix(m_defaultWorldToVolumeTransform); // 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; } // ThisApp析构函数 ThisApp::~ThisApp(){ // 销毁事件 if (m_hDepthFrameArrived && m_pDepthFrameReader){ m_pDepthFrameReader->UnsubscribeFrameArrived(m_hDepthFrameArrived); m_hDepthFrameArrived = 0; } // 释放DepthFrameReader SafeRelease(m_pDepthFrameReader); // 清理Fusion数据 SafeRelease(m_pReconstruction); SafeRelease(m_pMapper); // 清理Fusion图像帧 SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pSurfaceImageFrame); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pPointCloud); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pSmoothDepthFloatImage); SAFE_FUSION_RELEASE_IMAGE_FRAME(m_pNormalImageFrame); // 清理缓存 SAFE_DELETE_ARRAY(m_pDepthImagePixelBuffer); SAFE_DELETE_ARRAY(m_pDepthDistortionMap); SAFE_DELETE_ARRAY(m_pDepthDistortionLT); // 优雅地关闭Kinect if (m_pKinect){ m_pKinect->Close(); } SafeRelease(m_pKinect); } // 初始化 HRESULT ThisApp::Initialize(HINSTANCE hInstance, int nCmdShow){ HRESULT hr = E_FAIL; if (SUCCEEDED(static_cast<HRESULT>(m_ImagaRenderer))) { //register window class WNDCLASSEX wcex = { sizeof(WNDCLASSEX) }; wcex.style = CS_HREDRAW | CS_VREDRAW; wcex.lpfnWndProc = ThisApp::WndProc; wcex.cbClsExtra = 0; wcex.cbWndExtra = sizeof(LONG_PTR); wcex.hInstance = hInstance; wcex.hCursor = LoadCursorW(nullptr, IDC_ARROW); wcex.hbrBackground = nullptr; wcex.lpszMenuName = nullptr; wcex.lpszClassName = L"Direct2DTemplate"; wcex.hIcon = nullptr; // 注册窗口 RegisterClassEx(&wcex); // 创建窗口 RECT window_rect = { 0, 0, WNDWIDTH, WNDHEIGHT }; DWORD window_style = WS_OVERLAPPEDWINDOW; AdjustWindowRect(&window_rect, window_style, FALSE); AdjustWindowRect(&window_rect, window_style, FALSE); window_rect.right -= window_rect.left; window_rect.bottom -= window_rect.top; window_rect.left = (GetSystemMetrics(SM_CXFULLSCREEN) - window_rect.right) / 2; window_rect.top = (GetSystemMetrics(SM_CYFULLSCREEN) - window_rect.bottom) / 2; m_hwnd = CreateWindowExW(0, wcex.lpszClassName, TITLE, window_style, window_rect.left, window_rect.top, window_rect.right, window_rect.bottom, 0, 0, hInstance, this); hr = m_hwnd ? S_OK : E_FAIL; if (SUCCEEDED(hr)) { // 设置窗口句柄 m_ImagaRenderer.SetHwnd(m_hwnd); // 显示窗口 ShowWindow(m_hwnd, nCmdShow); UpdateWindow(m_hwnd); } } return hr; } // 消息循环 void ThisApp::RunMessageLoop() { MSG msg; HANDLE events[] = { reinterpret_cast<HANDLE>(m_hDepthFrameArrived), reinterpret_cast<HANDLE>(m_coordinateMappingChangedEvent), }; while (true){ // 消息处理 if (PeekMessageW(&msg, nullptr, 0, 0, PM_REMOVE)){ TranslateMessage(&msg); DispatchMessageW(&msg); } // 设置事件 // 事件0: 深度临帧事件 events[0] = reinterpret_cast<HANDLE>(m_hDepthFrameArrived); // 事件1: 坐标映射改变事件 events[1] = reinterpret_cast<HANDLE>(m_coordinateMappingChangedEvent); // 检查事件 switch (MsgWaitForMultipleObjects(lengthof(events), events, FALSE, INFINITE, QS_ALLINPUT)) { // events[0] case WAIT_OBJECT_0 + 0: this->check_depth_frame(); break; // events[1] case WAIT_OBJECT_0 + 1: { int break_point = 9; } break; default: break; } // 退出 if (msg.message == WM_QUIT){ break; } } } // 窗口过程函数 LRESULT CALLBACK ThisApp::WndProc(HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam) { LRESULT result = 0; if (message == WM_CREATE) { LPCREATESTRUCT pcs = (LPCREATESTRUCT)lParam; ThisApp *pOurApp = (ThisApp *)pcs->lpCreateParams; ::SetWindowLongPtrW( hwnd, GWLP_USERDATA, PtrToUlong(pOurApp) ); // 并初始化Kinect if (FAILED(pOurApp->init_kinect())){ ::MessageBoxW(hwnd, L"初始化Kinect v2失败", L"真的很遗憾", MB_ICONERROR); } result = 1; } else { ThisApp *pOurApp = reinterpret_cast<ThisApp *>(static_cast<LONG_PTR>( ::GetWindowLongPtrW( hwnd, GWLP_USERDATA ))); bool wasHandled = false; if (pOurApp) { switch (message) { case WM_DISPLAYCHANGE: InvalidateRect(hwnd, NULL, FALSE); result = 0; wasHandled = true; break; case WM_MOUSEWHEEL: pOurApp->m_ImagaRenderer.matrix._11 += 0.05f * static_cast<float>(static_cast<short>(HIWORD(wParam))) / static_cast<float>(WHEEL_DELTA); pOurApp->m_ImagaRenderer.matrix._22 = pOurApp->m_ImagaRenderer.matrix._11; pOurApp->m_ImagaRenderer.OnRender(); break; case WM_LBUTTONUP: // 重置 pOurApp->ResetReconstruction(); break; case WM_PAINT: pOurApp->m_ImagaRenderer.OnRender(); break; case WM_SIZE: // 改变窗口大小 pOurApp->m_ImagaRenderer.OnSize(LOWORD(lParam), HIWORD(lParam)); break; case WM_CLOSE: // 将收尾操作(如结束全部子线程)放在这里 DestroyWindow(hwnd); result = 1; wasHandled = true; break; case WM_DESTROY: PostQuitMessage(0); result = 1; wasHandled = true; break; } } if (!wasHandled) { result = DefWindowProc(hwnd, message, wParam, lParam); } } return result; } // 初始化Kinect HRESULT ThisApp::init_kinect(){ IDepthFrameSource* pDepthFrameSource = nullptr; // 查找当前默认Kinect HRESU