diff --git a/HL2UnityPlugin/HL2ResearchMode.cpp b/HL2UnityPlugin/HL2ResearchMode.cpp index f9eaa47..719cab9 100644 --- a/HL2UnityPlugin/HL2ResearchMode.cpp +++ b/HL2UnityPlugin/HL2ResearchMode.cpp @@ -66,7 +66,6 @@ namespace winrt::HL2UnityPlugin::implementation void HL2ResearchMode::InitializeDepthSensor() { - for (auto sensorDescriptor : m_sensorDescriptors) { if (sensorDescriptor.sensorType == DEPTH_AHAT) @@ -149,13 +148,14 @@ namespace winrt::HL2UnityPlugin::implementation } } - void HL2ResearchMode::StartDepthSensorLoop() + void HL2ResearchMode::StartDepthSensorLoop(bool reconstructPointCloud) { //std::thread th1([this] {this->DepthSensorLoopTest(); }); - if (m_refFrame == nullptr) + if (reconstructPointCloud && m_refFrame == nullptr) { m_refFrame = m_locator.GetDefault().CreateStationaryFrameOfReferenceAtCurrentLocation().CoordinateSystem(); } + m_reconstructShortThrowPointCloud = reconstructPointCloud; m_pDepthUpdateThread = new std::thread(HL2ResearchMode::DepthSensorLoop, this); } @@ -167,14 +167,13 @@ namespace winrt::HL2UnityPlugin::implementation { pHL2ResearchMode->m_depthSensorLoopStarted = true; } - else { - return; - } + else return; pHL2ResearchMode->m_depthSensor->OpenStream(); try { + UINT64 lastTs = 0; while (pHL2ResearchMode->m_depthSensorLoopStarted) { IResearchModeSensorFrame* pDepthSensorFrame = nullptr; @@ -200,26 +199,33 @@ namespace winrt::HL2UnityPlugin::implementation auto pAbTexture = std::make_unique(outAbBufferCount); std::vector pointCloud; - // get tracking transform ResearchModeSensorTimestamp timestamp; pDepthSensorFrame->GetTimeStamp(×tamp); - auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks))); - auto transToWorld = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts, pHL2ResearchMode->m_refFrame); - if (transToWorld == nullptr) + if (timestamp.HostTicks == lastTs) continue; + lastTs = timestamp.HostTicks; + + // get tracking transform + Windows::Perception::Spatial::SpatialLocation transToWorld = nullptr; + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud) { - continue; + auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks))); + transToWorld = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts, pHL2ResearchMode->m_refFrame); + if (transToWorld == nullptr) continue; } - auto depthToWorld = pHL2ResearchMode->m_depthCameraPoseInvMatrix * SpatialLocationToDxMatrix(transToWorld); + + XMMATRIX depthToWorld = XMMatrixIdentity(); + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud) + depthToWorld = pHL2ResearchMode->m_depthCameraPoseInvMatrix * SpatialLocationToDxMatrix(transToWorld); pHL2ResearchMode->mu.lock(); auto roiCenterFloat = XMFLOAT3(pHL2ResearchMode->m_roiCenter[0], pHL2ResearchMode->m_roiCenter[1], pHL2ResearchMode->m_roiCenter[2]); auto roiBoundFloat = XMFLOAT3(pHL2ResearchMode->m_roiBound[0], pHL2ResearchMode->m_roiBound[1], pHL2ResearchMode->m_roiBound[2]); pHL2ResearchMode->mu.unlock(); + XMVECTOR roiCenter = XMLoadFloat3(&roiCenterFloat); XMVECTOR roiBound = XMLoadFloat3(&roiBoundFloat); - UINT16 maxAbValue = 0; for (UINT i = 0; i < resolution.Height; i++) { for (UINT j = 0; j < resolution.Width; j++) @@ -228,26 +234,29 @@ namespace winrt::HL2UnityPlugin::implementation UINT16 depth = pDepth[idx]; depth = (depth > 4090) ? 0 : depth - pHL2ResearchMode->m_depthOffset; - // back-project point cloud within Roi - if (i > pHL2ResearchMode->depthCamRoi.kRowLower*resolution.Height&& i < pHL2ResearchMode->depthCamRoi.kRowUpper * resolution.Height && - j > pHL2ResearchMode->depthCamRoi.kColLower* resolution.Width&& j < pHL2ResearchMode->depthCamRoi.kColUpper * resolution.Width && - depth > pHL2ResearchMode->depthCamRoi.depthNearClip && depth < pHL2ResearchMode->depthCamRoi.depthFarClip) + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud) { - float xy[2] = { 0, 0 }; - float uv[2] = { j, i }; - pHL2ResearchMode->m_pDepthCameraSensor->MapImagePointToCameraUnitPlane(uv, xy); - auto pointOnUnitPlane = XMFLOAT3(xy[0], xy[1], 1); - auto tempPoint = (float)depth / 1000 * XMVector3Normalize(XMLoadFloat3(&pointOnUnitPlane)); - // apply transformation - auto pointInWorld = XMVector3Transform(tempPoint, depthToWorld); - - // filter point cloud based on region of interest - if (!pHL2ResearchMode->m_useRoiFilter || - (pHL2ResearchMode->m_useRoiFilter && XMVector3InBounds(pointInWorld - roiCenter, roiBound))) + // back-project point cloud within Roi + if (i > pHL2ResearchMode->depthCamRoi.kRowLower * resolution.Height && i < pHL2ResearchMode->depthCamRoi.kRowUpper * resolution.Height && + j > pHL2ResearchMode->depthCamRoi.kColLower * resolution.Width && j < pHL2ResearchMode->depthCamRoi.kColUpper * resolution.Width && + depth > pHL2ResearchMode->depthCamRoi.depthNearClip && depth < pHL2ResearchMode->depthCamRoi.depthFarClip) { - pointCloud.push_back(XMVectorGetX(pointInWorld)); - pointCloud.push_back(XMVectorGetY(pointInWorld)); - pointCloud.push_back(-XMVectorGetZ(pointInWorld)); + float xy[2] = { 0, 0 }; + float uv[2] = { j, i }; + pHL2ResearchMode->m_pDepthCameraSensor->MapImagePointToCameraUnitPlane(uv, xy); + auto pointOnUnitPlane = XMFLOAT3(xy[0], xy[1], 1); + auto tempPoint = (float)depth / 1000 * XMVector3Normalize(XMLoadFloat3(&pointOnUnitPlane)); + // apply transformation + auto pointInWorld = XMVector3Transform(tempPoint, depthToWorld); + + // filter point cloud based on region of interest + if (!pHL2ResearchMode->m_useRoiFilter || + (pHL2ResearchMode->m_useRoiFilter && XMVector3InBounds(pointInWorld - roiCenter, roiBound))) + { + pointCloud.push_back(XMVectorGetX(pointInWorld)); + pointCloud.push_back(XMVectorGetY(pointInWorld)); + pointCloud.push_back(-XMVectorGetZ(pointInWorld)); + } } } @@ -261,19 +270,11 @@ namespace winrt::HL2UnityPlugin::implementation if (abValue > 1000) { processedAbValue = 0xFF; } else { processedAbValue = (uint8_t)((float)abValue / 1000 * 255); } - /*if (abValue > maxAbValue) - { - maxAbValue = abValue; - std::stringstream ss; - ss << "Non zero valule. Idx: " << idx << " Raw Value: " << abValue << "Processed: " << (int)processedAbValue << "\n"; - std::string msg = ss.str(); - std::wstring widemsg = std::wstring(msg.begin(), msg.end()); - OutputDebugString(widemsg.c_str()); - }*/ pAbTexture.get()[idx] = processedAbValue; // save the depth of center pixel - if (i == (UINT)(0.35 * resolution.Height) && j == (UINT)(0.5 * resolution.Width) + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud && + i == (UINT)(0.35 * resolution.Height) && j == (UINT)(0.5 * resolution.Width) && pointCloud.size()>=3) { pHL2ResearchMode->m_centerDepth = depth; @@ -293,14 +294,18 @@ namespace winrt::HL2UnityPlugin::implementation std::lock_guard l(pHL2ResearchMode->mu); // save point cloud - if (!pHL2ResearchMode->m_pointCloud) + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud) { - OutputDebugString(L"Create Space for point cloud...\n"); - pHL2ResearchMode->m_pointCloud = new float[outBufferCount * 3]; - } + if (!pHL2ResearchMode->m_pointCloud) + { + OutputDebugString(L"Create Space for point cloud...\n"); + pHL2ResearchMode->m_pointCloud = new float[outBufferCount * 3]; + } - memcpy(pHL2ResearchMode->m_pointCloud, pointCloud.data(), pointCloud.size() * sizeof(float)); - pHL2ResearchMode->m_pointcloudLength = pointCloud.size(); + memcpy(pHL2ResearchMode->m_pointCloud, pointCloud.data(), pointCloud.size() * sizeof(float)); + pHL2ResearchMode->m_pointcloudLength = pointCloud.size(); + } + // save raw depth map if (!pHL2ResearchMode->m_depthMap) @@ -336,12 +341,13 @@ namespace winrt::HL2UnityPlugin::implementation } pHL2ResearchMode->m_shortAbImageTextureUpdated = true; pHL2ResearchMode->m_depthMapTextureUpdated = true; - pHL2ResearchMode->m_pointCloudUpdated = true; + if (pHL2ResearchMode->m_reconstructShortThrowPointCloud) pHL2ResearchMode->m_pointCloudUpdated = true; pDepthTexture.reset(); // release space - if (pDepthFrame) { + if (pDepthFrame) + { pDepthFrame->Release(); } if (pDepthSensorFrame) @@ -358,12 +364,13 @@ namespace winrt::HL2UnityPlugin::implementation } - void HL2ResearchMode::StartLongDepthSensorLoop() + void HL2ResearchMode::StartLongDepthSensorLoop(bool reconstructPointCloud) { - if (m_refFrame == nullptr) + if (reconstructPointCloud && m_refFrame == nullptr) { m_refFrame = m_locator.GetDefault().CreateStationaryFrameOfReferenceAtCurrentLocation().CoordinateSystem(); } + m_reconstructLongThrowPointCloud = reconstructPointCloud; m_pLongDepthUpdateThread = new std::thread(HL2ResearchMode::LongDepthSensorLoop, this); } @@ -383,6 +390,7 @@ namespace winrt::HL2UnityPlugin::implementation try { + UINT64 lastTs = 0; bool printedResolution = false; while (pHL2ResearchMode->m_longDepthSensorLoopStarted) { IResearchModeSensorFrame* pDepthSensorFrame = nullptr; @@ -398,24 +406,44 @@ namespace winrt::HL2UnityPlugin::implementation size_t outBufferCount = 0; const UINT16* pDepth = nullptr; - const BYTE* pSigma = nullptr; pDepthFrame->GetSigmaBuffer(&pSigma, &outBufferCount); pDepthFrame->GetBuffer(&pDepth, &outBufferCount); + + const UINT16* pAbImage = nullptr; + pDepthFrame->GetAbDepthBuffer(&pAbImage, &outBufferCount); + pHL2ResearchMode->m_longDepthBufferSize = outBufferCount; auto pDepthTexture = std::make_unique(outBufferCount); + auto pAbTexture = std::make_unique(outBufferCount); + std::vector pointCloud; - // get tracking transform ResearchModeSensorTimestamp timestamp; pDepthSensorFrame->GetTimeStamp(×tamp); - auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks))); - auto transToWorld = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts, pHL2ResearchMode->m_refFrame); - if (transToWorld == nullptr) + if (timestamp.HostTicks == lastTs) continue; + lastTs = timestamp.HostTicks; + + // get tracking transform + Windows::Perception::Spatial::SpatialLocation transToWorld = nullptr; + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud) { - continue; + auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks))); + transToWorld = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts, pHL2ResearchMode->m_refFrame); + if (transToWorld == nullptr) continue; } + XMMATRIX depthToWorld = XMMatrixIdentity(); + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud) + depthToWorld = pHL2ResearchMode->m_longDepthCameraPoseInvMatrix * SpatialLocationToDxMatrix(transToWorld); + + pHL2ResearchMode->mu.lock(); + auto roiCenterFloat = XMFLOAT3(pHL2ResearchMode->m_roiCenter[0], pHL2ResearchMode->m_roiCenter[1], pHL2ResearchMode->m_roiCenter[2]); + auto roiBoundFloat = XMFLOAT3(pHL2ResearchMode->m_roiBound[0], pHL2ResearchMode->m_roiBound[1], pHL2ResearchMode->m_roiBound[2]); + pHL2ResearchMode->mu.unlock(); + + XMVECTOR roiCenter = XMLoadFloat3(&roiCenterFloat); + XMVECTOR roiBound = XMLoadFloat3(&roiBoundFloat); for (UINT i = 0; i < resolution.Height; i++) { @@ -425,9 +453,58 @@ namespace winrt::HL2UnityPlugin::implementation UINT16 depth = pDepth[idx]; depth = (pSigma[idx] & 0x80) ? 0 : depth - pHL2ResearchMode->m_depthOffset; + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud) + { + // back-project point cloud within Roi + if (i > pHL2ResearchMode->depthCamRoi.kRowLower * resolution.Height && i < pHL2ResearchMode->depthCamRoi.kRowUpper * resolution.Height && + j > pHL2ResearchMode->depthCamRoi.kColLower * resolution.Width && j < pHL2ResearchMode->depthCamRoi.kColUpper * resolution.Width && + depth > 200) + { + float xy[2] = { 0, 0 }; + float uv[2] = { j, i }; + pHL2ResearchMode->m_pLongDepthCameraSensor->MapImagePointToCameraUnitPlane(uv, xy); + auto pointOnUnitPlane = XMFLOAT3(xy[0], xy[1], 1); + auto tempPoint = (float)depth / 1000 * XMVector3Normalize(XMLoadFloat3(&pointOnUnitPlane)); + // apply transformation + auto pointInWorld = XMVector3Transform(tempPoint, depthToWorld); + + // filter point cloud based on region of interest + if (!pHL2ResearchMode->m_useRoiFilter || + (pHL2ResearchMode->m_useRoiFilter && XMVector3InBounds(pointInWorld - roiCenter, roiBound))) + { + pointCloud.push_back(XMVectorGetX(pointInWorld)); + pointCloud.push_back(XMVectorGetY(pointInWorld)); + pointCloud.push_back(-XMVectorGetZ(pointInWorld)); + } + } + } + // save as grayscale texture pixel into temp buffer if (depth == 0) { pDepthTexture.get()[idx] = 0; } else { pDepthTexture.get()[idx] = (uint8_t)((float)depth / 4000 * 255); } + + // save AbImage as grayscale texture pixel into temp buffer + UINT16 abValue = pAbImage[idx]; + uint8_t processedAbValue = 0; + if (abValue > 2000) { processedAbValue = 0xFF; } + else { processedAbValue = (uint8_t)((float)abValue / 2000 * 255); } + + pAbTexture.get()[idx] = processedAbValue; + + // save the depth of center pixel + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud && + i == (UINT)(0.35 * resolution.Height) && j == (UINT)(0.5 * resolution.Width) + && pointCloud.size() >= 3) + { + pHL2ResearchMode->m_centerDepth = depth; + if (depth > pHL2ResearchMode->depthCamRoi.depthNearClip && depth < pHL2ResearchMode->depthCamRoi.depthFarClip) + { + std::lock_guard l(pHL2ResearchMode->mu); + pHL2ResearchMode->m_centerPoint[0] = *(pointCloud.end() - 3); + pHL2ResearchMode->m_centerPoint[1] = *(pointCloud.end() - 2); + pHL2ResearchMode->m_centerPoint[2] = *(pointCloud.end() - 1); + } + } } } @@ -435,10 +512,23 @@ namespace winrt::HL2UnityPlugin::implementation { std::lock_guard l(pHL2ResearchMode->mu); + // save point cloud + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud) + { + if (!pHL2ResearchMode->m_longThrowPointCloud) + { + OutputDebugString(L"Create Space for point cloud (long throw)...\n"); + pHL2ResearchMode->m_longThrowPointCloud = new float[outBufferCount * 3]; + } + + memcpy(pHL2ResearchMode->m_longThrowPointCloud, pointCloud.data(), pointCloud.size() * sizeof(float)); + pHL2ResearchMode->m_longThrowPointcloudLength = pointCloud.size(); + } + // save raw depth map if (!pHL2ResearchMode->m_longDepthMap) { - OutputDebugString(L"Create Space for depth map...\n"); + OutputDebugString(L"Create Space for long throw depth map...\n"); pHL2ResearchMode->m_longDepthMap = new UINT16[outBufferCount]; } memcpy(pHL2ResearchMode->m_longDepthMap, pDepth, outBufferCount * sizeof(UINT16)); @@ -446,25 +536,42 @@ namespace winrt::HL2UnityPlugin::implementation // save pre-processed depth map texture (for visualization) if (!pHL2ResearchMode->m_longDepthMapTexture) { - OutputDebugString(L"Create Space for depth map texture...\n"); + OutputDebugString(L"Create Space for long throw depth map texture...\n"); pHL2ResearchMode->m_longDepthMapTexture = new UINT8[outBufferCount]; } memcpy(pHL2ResearchMode->m_longDepthMapTexture, pDepthTexture.get(), outBufferCount * sizeof(UINT8)); - } + // save raw AbImage + if (!pHL2ResearchMode->m_longAbImage) + { + OutputDebugString(L"Create Space for long AbImage...\n"); + pHL2ResearchMode->m_longAbImage = new UINT16[outBufferCount]; + } + memcpy(pHL2ResearchMode->m_longAbImage, pAbImage, outBufferCount * sizeof(UINT16)); + + // save pre-processed AbImage texture (for visualization) + if (!pHL2ResearchMode->m_longAbImageTexture) + { + OutputDebugString(L"Create Space for long AbImage texture...\n"); + pHL2ResearchMode->m_longAbImageTexture = new UINT8[outBufferCount]; + } + memcpy(pHL2ResearchMode->m_longAbImageTexture, pAbTexture.get(), outBufferCount * sizeof(UINT8)); + } + pHL2ResearchMode->m_longAbImageTextureUpdated = true; pHL2ResearchMode->m_longDepthMapTextureUpdated = true; + if (pHL2ResearchMode->m_reconstructLongThrowPointCloud) pHL2ResearchMode->m_longThrowPointCloudUpdated = true; pDepthTexture.reset(); // release space - if (pDepthFrame) { + if (pDepthFrame) + { pDepthFrame->Release(); } if (pDepthSensorFrame) { pDepthSensorFrame->Release(); } - } } catch (...) {} @@ -536,7 +643,9 @@ namespace winrt::HL2UnityPlugin::implementation auto ts_left = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp_left.HostTicks))); auto ts_right = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp_right.HostTicks))); - auto rigToWorld_l = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts_left, pHL2ResearchMode->m_refFrame); + + // uncomment the block below if their transform is needed + /*auto rigToWorld_l = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts_left, pHL2ResearchMode->m_refFrame); auto rigToWorld_r = rigToWorld_l; if (ts_left.TargetTime() != ts_right.TargetTime()) { rigToWorld_r = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts_right, pHL2ResearchMode->m_refFrame); @@ -548,7 +657,7 @@ namespace winrt::HL2UnityPlugin::implementation } auto LfToWorld = pHL2ResearchMode->m_LFCameraPoseInvMatrix * SpatialLocationToDxMatrix(rigToWorld_l); - auto RfToWorld = pHL2ResearchMode->m_RFCameraPoseInvMatrix * SpatialLocationToDxMatrix(rigToWorld_r); + auto RfToWorld = pHL2ResearchMode->m_RFCameraPoseInvMatrix * SpatialLocationToDxMatrix(rigToWorld_r);*/ // save data { @@ -557,12 +666,6 @@ namespace winrt::HL2UnityPlugin::implementation pHL2ResearchMode->m_lastSpatialFrame.LFFrame.timestamp = timestamp_left.HostTicks; pHL2ResearchMode->m_lastSpatialFrame.RFFrame.timestamp = timestamp_right.HostTicks; - /*auto ts_ft_l = pHL2ResearchMode->m_converter.RelativeTicksToAbsoluteTicks(HundredsOfNanoseconds((long long)timestamp_left.HostTicks)); - auto ts_ft_r = pHL2ResearchMode->m_converter.RelativeTicksToAbsoluteTicks(HundredsOfNanoseconds((long long)timestamp_right.HostTicks)); - - pHL2ResearchMode->m_lastSpatialFrame.LFFrame.timestamp_ft = ts_ft_l.count(); - pHL2ResearchMode->m_lastSpatialFrame.RFFrame.timestamp_ft = ts_ft_r.count();*/ - pHL2ResearchMode->m_lastSpatialFrame.LFFrame.timestamp_ft = ts_left.TargetTime().time_since_epoch().count(); pHL2ResearchMode->m_lastSpatialFrame.RFFrame.timestamp_ft = ts_right.TargetTime().time_since_epoch().count(); @@ -831,8 +934,12 @@ namespace winrt::HL2UnityPlugin::implementation inline bool HL2ResearchMode::ShortAbImageTextureUpdated() { return m_shortAbImageTextureUpdated; } + inline bool HL2ResearchMode::LongAbImageTextureUpdated() { return m_longAbImageTextureUpdated; } + inline bool HL2ResearchMode::PointCloudUpdated() { return m_pointCloudUpdated; } + inline bool HL2ResearchMode::LongThrowPointCloudUpdated() { return m_longThrowPointCloudUpdated; } + inline int HL2ResearchMode::GetLongDepthBufferSize() { return m_longDepthBufferSize; } inline bool HL2ResearchMode::LongDepthMapTextureUpdated() { return m_longDepthMapTextureUpdated; } @@ -941,6 +1048,16 @@ namespace winrt::HL2UnityPlugin::implementation delete[] m_shortAbImageTexture; m_shortAbImageTexture = nullptr; } + if (m_longAbImage) + { + delete[] m_longAbImage; + m_longAbImage = nullptr; + } + if (m_longAbImageTexture) + { + delete[] m_longAbImageTexture; + m_longAbImageTexture = nullptr; + } if (m_lastSpatialFrame.LFFrame.image) { @@ -1054,6 +1171,19 @@ namespace winrt::HL2UnityPlugin::implementation return tempBuffer; } + + com_array HL2ResearchMode::GetLongAbImageBuffer() + { + std::lock_guard l(mu); + if (!m_longAbImage) + { + return com_array(); + } + com_array tempBuffer = com_array(m_longAbImage, m_longAbImage + m_longDepthBufferSize); + + return tempBuffer; + } + com_array HL2ResearchMode::GetLongDepthMapTextureBuffer() { std::lock_guard l(mu); @@ -1067,6 +1197,20 @@ namespace winrt::HL2UnityPlugin::implementation return tempBuffer; } + // Get depth map texture buffer. (For visualization purpose) + com_array HL2ResearchMode::GetLongAbImageTextureBuffer() + { + std::lock_guard l(mu); + if (!m_longAbImageTexture) + { + return com_array(); + } + com_array tempBuffer = com_array(std::move_iterator(m_longAbImageTexture), std::move_iterator(m_longAbImageTexture + m_longDepthBufferSize)); + + m_longAbImageTextureUpdated = false; + return tempBuffer; + } + com_array HL2ResearchMode::GetLFCameraBuffer(int64_t& ts) { std::lock_guard l(mu); @@ -1164,7 +1308,7 @@ namespace winrt::HL2UnityPlugin::implementation com_array HL2ResearchMode::GetPointCloudBuffer() { std::lock_guard l(mu); - if (m_pointcloudLength == 0) + if (!m_reconstructShortThrowPointCloud || m_pointcloudLength == 0) { return com_array(); } @@ -1173,21 +1317,35 @@ namespace winrt::HL2UnityPlugin::implementation return tempBuffer; } - // Get the 3D point (float[3]) of center point in depth map. Can be used to render depth cursor. - com_array HL2ResearchMode::GetCenterPoint() + // Get the buffer for point cloud in the form of float array. + // There will be 3n elements in the array where the 3i, 3i+1, 3i+2 element correspond to x, y, z component of the i'th point. (i->[0,n-1]) + com_array HL2ResearchMode::GetLongThrowPointCloudBuffer() { std::lock_guard l(mu); - com_array centerPoint = com_array(std::move_iterator(m_centerPoint), std::move_iterator(m_centerPoint + 3)); - - return centerPoint; + { + std::stringstream ss; + ss << "m_reconstructLongThrowPointCloud: " << m_reconstructLongThrowPointCloud << "\n"; + ss << "m_pointcloudLength: " << m_longThrowPointcloudLength << "\n"; + std::string msg = ss.str(); + std::wstring widemsg = std::wstring(msg.begin(), msg.end()); + OutputDebugString(widemsg.c_str()); + } + if (!m_reconstructLongThrowPointCloud || m_longThrowPointcloudLength == 0) + { + return com_array(); + }; + com_array tempBuffer = com_array(std::move_iterator(m_longThrowPointCloud), std::move_iterator(m_longThrowPointCloud + m_longThrowPointcloudLength)); + m_longThrowPointCloudUpdated = false; + return tempBuffer; } - com_array HL2ResearchMode::GetDepthSensorPosition() + // Get the 3D point (float[3]) of center point in depth map. Can be used to render depth cursor. + com_array HL2ResearchMode::GetCenterPoint() { std::lock_guard l(mu); - com_array depthSensorPos = com_array(std::move_iterator(m_depthSensorPosition), std::move_iterator(m_depthSensorPosition + 3)); + com_array centerPoint = com_array(std::move_iterator(m_centerPoint), std::move_iterator(m_centerPoint + 3)); - return depthSensorPos; + return centerPoint; } // Set the reference coordinate system. Need to be set before the sensor loop starts; otherwise, default coordinate will be used. diff --git a/HL2UnityPlugin/HL2ResearchMode.h b/HL2UnityPlugin/HL2ResearchMode.h index d508e69..207a4fb 100644 --- a/HL2UnityPlugin/HL2ResearchMode.h +++ b/HL2UnityPlugin/HL2ResearchMode.h @@ -39,8 +39,8 @@ namespace winrt::HL2UnityPlugin::implementation void InitializeGyroSensor(); void InitializeMagSensor(); - void StartDepthSensorLoop(); - void StartLongDepthSensorLoop(); + void StartDepthSensorLoop(bool reconstructPointCloud = true); + void StartLongDepthSensorLoop(bool reconstructPointCloud = true); void StartSpatialCamerasFrontLoop(); void StartAccelSensorLoop(); void StartGyroSensorLoop(); @@ -50,7 +50,9 @@ namespace winrt::HL2UnityPlugin::implementation bool DepthMapTextureUpdated(); bool ShortAbImageTextureUpdated(); + bool LongAbImageTextureUpdated(); bool PointCloudUpdated(); + bool LongThrowPointCloudUpdated(); bool LongDepthMapTextureUpdated(); bool LFImageUpdated(); bool RFImageUpdated(); @@ -66,6 +68,8 @@ namespace winrt::HL2UnityPlugin::implementation com_array GetDepthMapTextureBuffer(); com_array GetShortAbImageBuffer(); com_array GetShortAbImageTextureBuffer(); + com_array GetLongAbImageBuffer(); + com_array GetLongAbImageTextureBuffer(); com_array GetLongDepthMapBuffer(); com_array GetLongDepthMapTextureBuffer(); com_array GetLFCameraBuffer(int64_t& ts); @@ -77,17 +81,21 @@ namespace winrt::HL2UnityPlugin::implementation com_array GetMagSample(); com_array GetPointCloudBuffer(); + com_array GetLongThrowPointCloudBuffer(); com_array GetCenterPoint(); - com_array GetDepthSensorPosition(); std::mutex mu; private: float* m_pointCloud = nullptr; int m_pointcloudLength = 0; + float* m_longThrowPointCloud = nullptr; + int m_longThrowPointcloudLength = 0; UINT16* m_depthMap = nullptr; UINT8* m_depthMapTexture = nullptr; UINT16* m_shortAbImage = nullptr; UINT8* m_shortAbImageTexture = nullptr; + UINT16* m_longAbImage = nullptr; + UINT8* m_longAbImageTexture = nullptr; UINT16* m_longDepthMap = nullptr; UINT8* m_longDepthMapTexture = nullptr; @@ -123,17 +131,20 @@ namespace winrt::HL2UnityPlugin::implementation std::atomic_int m_RFbufferSize = 0; std::atomic_uint16_t m_centerDepth = 0; float m_centerPoint[3]{ 0,0,0 }; - float m_depthSensorPosition[3]{ 0,0,0 }; + std::atomic_bool m_depthSensorLoopStarted = false; std::atomic_bool m_longDepthSensorLoopStarted = false; std::atomic_bool m_spatialCamerasFrontLoopStarted = false; std::atomic_bool m_accelSensorLoopStarted = false; std::atomic_bool m_gyroSensorLoopStarted = false; std::atomic_bool m_magSensorLoopStarted = false; + std::atomic_bool m_depthMapTextureUpdated = false; std::atomic_bool m_shortAbImageTextureUpdated = false; + std::atomic_bool m_longAbImageTextureUpdated = false; std::atomic_bool m_longDepthMapTextureUpdated = false; std::atomic_bool m_pointCloudUpdated = false; + std::atomic_bool m_longThrowPointCloudUpdated = false; std::atomic_bool m_useRoiFilter = false; std::atomic_bool m_LFImageUpdated = false; std::atomic_bool m_RFImageUpdated = false; @@ -141,6 +152,9 @@ namespace winrt::HL2UnityPlugin::implementation std::atomic_bool m_gyroSampleUpdated = false; std::atomic_bool m_magSampleUpdated = false; + std::atomic_bool m_reconstructShortThrowPointCloud = false; + std::atomic_bool m_reconstructLongThrowPointCloud = false; + float m_roiBound[3]{ 0,0,0 }; float m_roiCenter[3]{ 0,0,0 }; static void DepthSensorLoop(HL2ResearchMode* pHL2ResearchMode); @@ -170,7 +184,7 @@ namespace winrt::HL2UnityPlugin::implementation static DirectX::XMMATRIX HL2ResearchMode::SpatialLocationToDxMatrix(Windows::Perception::Spatial::SpatialLocation location); struct DepthCamRoi { float kRowLower = 0.2; - float kRowUpper = 0.5; + float kRowUpper = 0.55; float kColLower = 0.3; float kColUpper = 0.7; UINT16 depthNearClip = 200; // Unit: mm diff --git a/HL2UnityPlugin/HL2ResearchMode.idl b/HL2UnityPlugin/HL2ResearchMode.idl index 240d018..11a0d0e 100644 --- a/HL2UnityPlugin/HL2ResearchMode.idl +++ b/HL2UnityPlugin/HL2ResearchMode.idl @@ -9,10 +9,13 @@ namespace HL2UnityPlugin UInt8[] GetDepthMapTextureBuffer(); UInt16[] GetShortAbImageBuffer(); UInt8[] GetShortAbImageTextureBuffer(); + UInt16[] GetLongAbImageBuffer(); + UInt8[] GetLongAbImageTextureBuffer(); Single[] GetPointCloudBuffer(); UInt16[] GetLongDepthMapBuffer(); UInt8[] GetLongDepthMapTextureBuffer(); + Single[] GetLongThrowPointCloudBuffer(); UInt8[] GetLFCameraBuffer(out Int64 ts); UInt8[] GetRFCameraBuffer(out Int64 ts); @@ -23,7 +26,6 @@ namespace HL2UnityPlugin Single[] GetMagSample(); Single[] GetCenterPoint(); - Single[] GetDepthSensorPosition(); Int32 GetDepthBufferSize(); Int32 GetLongDepthBufferSize(); String PrintDepthResolution(); @@ -35,10 +37,12 @@ namespace HL2UnityPlugin Boolean DepthMapTextureUpdated(); Boolean ShortAbImageTextureUpdated(); + Boolean LongAbImageTextureUpdated(); Boolean PointCloudUpdated(); Boolean LongDepthMapTextureUpdated(); - Boolean LFImageUpdated(); + Boolean LongThrowPointCloudUpdated(); + Boolean LFImageUpdated(); Boolean RFImageUpdated(); Boolean AccelSampleUpdated(); Boolean GyroSampleUpdated(); @@ -53,7 +57,9 @@ namespace HL2UnityPlugin void InitializeMagSensor(); void StartDepthSensorLoop(); + void StartDepthSensorLoop(Boolean reconstructPointCloud); void StartLongDepthSensorLoop(); + void StartLongDepthSensorLoop(Boolean reconstructPointCloud); void StartSpatialCamerasFrontLoop(); void StartAccelSensorLoop(); void StartGyroSensorLoop(); diff --git a/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.dll b/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.dll index 9dc3747..26c7bf0 100644 Binary files a/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.dll and b/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.dll differ diff --git a/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd b/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd index 70f1bb8..687cd2e 100644 Binary files a/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd and b/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd differ diff --git a/UnitySample/Assets/Scenes/PointCloudSample.unity b/UnitySample/Assets/Scenes/PointCloudSample.unity index 6045c7e..b5eb913 100644 --- a/UnitySample/Assets/Scenes/PointCloudSample.unity +++ b/UnitySample/Assets/Scenes/PointCloudSample.unity @@ -155,6 +155,100 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 1 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &119160456 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 119160457} + - component: {fileID: 119160460} + - component: {fileID: 119160459} + - component: {fileID: 119160458} + m_Layer: 0 + m_Name: PreviewPlaneLongAbImage + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &119160457 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 119160456} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0.07, y: 0.166, z: 1} + m_LocalScale: {x: 0.096, y: -0.0864, z: 1} + m_Children: [] + m_Father: {fileID: 1517424694} + m_RootOrder: 3 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!64 &119160458 +MeshCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 119160456} + m_Material: {fileID: 0} + m_IsTrigger: 0 + m_Enabled: 1 + serializedVersion: 4 + m_Convex: 0 + m_CookingOptions: 30 + m_Mesh: {fileID: 10210, guid: 0000000000000000e000000000000000, type: 0} +--- !u!23 &119160459 +MeshRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 119160456} + m_Enabled: 1 + m_CastShadows: 1 + m_ReceiveShadows: 1 + m_DynamicOccludee: 1 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 2 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 2100000, guid: e69f67baa4177ba41b2fae2b673b369d, type: 2} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 3 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 +--- !u!33 &119160460 +MeshFilter: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 119160456} + m_Mesh: {fileID: 10210, guid: 0000000000000000e000000000000000, type: 0} --- !u!1 &151586573 GameObject: m_ObjectHideFlags: 0 @@ -970,7 +1064,7 @@ Transform: m_LocalScale: {x: 0.128, y: -0.096, z: 1} m_Children: [] m_Father: {fileID: 1517424694} - m_RootOrder: 3 + m_RootOrder: 4 m_LocalEulerAnglesHint: {x: 0, y: 0, z: -90} --- !u!1 &456762697 GameObject: @@ -1019,7 +1113,7 @@ GameObject: - component: {fileID: 456786862} - component: {fileID: 456786861} m_Layer: 0 - m_Name: PreviewPlaneDepth + m_Name: PreviewPlaneShortDepth m_TagString: Untagged m_Icon: {fileID: 0} m_NavMeshLayer: 0 @@ -1672,9 +1766,12 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: fa057c5b267aac542bd34a1c0d8e2a50, type: 3} m_Name: m_EditorClassIdentifier: + depthSensorMode: 1 + enablePointCloud: 1 depthPreviewPlane: {fileID: 456786859} shortAbImagePreviewPlane: {fileID: 349520856} longDepthPreviewPlane: {fileID: 891683166} + longAbImagePreviewPlane: {fileID: 119160456} LFPreviewPlane: {fileID: 446940270} RFPreviewPlane: {fileID: 1942013569} text: {fileID: 180299606} @@ -1739,7 +1836,7 @@ GameObject: m_Icon: {fileID: 0} m_NavMeshLayer: 0 m_StaticEditorFlags: 0 - m_IsActive: 0 + m_IsActive: 1 --- !u!64 &891683167 MeshCollider: m_ObjectHideFlags: 0 @@ -1809,7 +1906,7 @@ Transform: m_PrefabAsset: {fileID: 0} m_GameObject: {fileID: 891683166} m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} - m_LocalPosition: {x: 0.028, y: 0.05, z: 1} + m_LocalPosition: {x: -0.06, y: 0.166, z: 1} m_LocalScale: {x: 0.096, y: -0.0864, z: 1} m_Children: [] m_Father: {fileID: 1517424694} @@ -3449,6 +3546,7 @@ Transform: - {fileID: 456786860} - {fileID: 349520857} - {fileID: 891683170} + - {fileID: 119160457} - {fileID: 446940274} - {fileID: 1942013573} - {fileID: 2067370469} @@ -4884,7 +4982,7 @@ Transform: m_LocalScale: {x: 0.128, y: -0.096, z: 1} m_Children: [] m_Father: {fileID: 1517424694} - m_RootOrder: 4 + m_RootOrder: 5 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 90} --- !u!1 &2067370468 GameObject: @@ -4918,7 +5016,7 @@ RectTransform: m_Children: - {fileID: 180299605} m_Father: {fileID: 1517424694} - m_RootOrder: 5 + m_RootOrder: 6 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} m_AnchorMin: {x: 0, y: 0} m_AnchorMax: {x: 0, y: 0} diff --git a/UnitySample/Assets/Scipts/ResearchModeImuStream.cs b/UnitySample/Assets/Scipts/ResearchModeImuStream.cs index 3f9858a..c6abb42 100644 --- a/UnitySample/Assets/Scipts/ResearchModeImuStream.cs +++ b/UnitySample/Assets/Scipts/ResearchModeImuStream.cs @@ -41,8 +41,6 @@ void Start() researchMode.StartMagSensorLoop(); #endif } - - bool startRealtimePreview = true; void LateUpdate() { #if ENABLE_WINMD_SUPPORT @@ -126,7 +124,6 @@ public void StopSensorsEvent() #if ENABLE_WINMD_SUPPORT researchMode.StopAllSensorDevice(); #endif - startRealtimePreview = false; } private void OnApplicationFocus(bool focus) diff --git a/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs b/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs index 3222788..fa0b16d 100644 --- a/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs +++ b/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs @@ -14,6 +14,15 @@ public class ResearchModeVideoStream : MonoBehaviour HL2ResearchMode researchMode; #endif + enum DepthSensorMode + { + ShortThrow, + LongThrow, + None + }; + [SerializeField] DepthSensorMode depthSensorMode = DepthSensorMode.ShortThrow; + [SerializeField] bool enablePointCloud = true; + TCPClient tcpClient; public GameObject depthPreviewPlane = null; @@ -31,6 +40,11 @@ public class ResearchModeVideoStream : MonoBehaviour private Texture2D longDepthMediaTexture = null; private byte[] longDepthFrameData = null; + public GameObject longAbImagePreviewPlane = null; + private Material longAbImageMediaMaterial = null; + private Texture2D longAbImageMediaTexture = null; + private byte[] longAbImageFrameData = null; + public GameObject LFPreviewPlane = null; private Material LFMediaMaterial = null; private Texture2D LFMediaTexture = null; @@ -53,7 +67,7 @@ public class ResearchModeVideoStream : MonoBehaviour private void Awake() { #if ENABLE_WINMD_SUPPORT -#if UNITY_2020_1_OR_NEWER +#if UNITY_2020_1_OR_NEWER // note: Unity 2021.2 and later not supported IntPtr WorldOriginPtr = UnityEngine.XR.WindowsMR.WindowsMREnvironment.OriginSpatialCoordinateSystem; unityWorldOrigin = Marshal.GetObjectForIUnknown(WorldOriginPtr) as Windows.Perception.Spatial.SpatialCoordinateSystem; //unityWorldOrigin = Windows.Perception.Spatial.SpatialLocator.GetDefault().CreateStationaryFrameOfReferenceAtCurrentLocation().CoordinateSystem; @@ -65,26 +79,44 @@ private void Awake() } void Start() { - if (depthPreviewPlane != null) + if (depthSensorMode == DepthSensorMode.ShortThrow) { - depthMediaMaterial = depthPreviewPlane.GetComponent().material; - depthMediaTexture = new Texture2D(512, 512, TextureFormat.Alpha8, false); - depthMediaMaterial.mainTexture = depthMediaTexture; - } + if (depthPreviewPlane != null) + { + depthMediaMaterial = depthPreviewPlane.GetComponent().material; + depthMediaTexture = new Texture2D(512, 512, TextureFormat.Alpha8, false); + depthMediaMaterial.mainTexture = depthMediaTexture; + } - if (shortAbImagePreviewPlane != null) - { - shortAbImageMediaMaterial = shortAbImagePreviewPlane.GetComponent().material; - shortAbImageMediaTexture = new Texture2D(512, 512, TextureFormat.Alpha8, false); - shortAbImageMediaMaterial.mainTexture = shortAbImageMediaTexture; + if (shortAbImagePreviewPlane != null) + { + shortAbImageMediaMaterial = shortAbImagePreviewPlane.GetComponent().material; + shortAbImageMediaTexture = new Texture2D(512, 512, TextureFormat.Alpha8, false); + shortAbImageMediaMaterial.mainTexture = shortAbImageMediaTexture; + } + longDepthPreviewPlane.SetActive(false); + longAbImagePreviewPlane.SetActive(false); } - - if (longDepthPreviewPlane != null) + + if (depthSensorMode == DepthSensorMode.LongThrow) { - longDepthMediaMaterial = longDepthPreviewPlane.GetComponent().material; - longDepthMediaTexture = new Texture2D(320, 288, TextureFormat.Alpha8, false); - longDepthMediaMaterial.mainTexture = longDepthMediaTexture; + if (longDepthPreviewPlane != null) + { + longDepthMediaMaterial = longDepthPreviewPlane.GetComponent().material; + longDepthMediaTexture = new Texture2D(320, 288, TextureFormat.Alpha8, false); + longDepthMediaMaterial.mainTexture = longDepthMediaTexture; + } + + if (longAbImagePreviewPlane != null) + { + longAbImageMediaMaterial = longAbImagePreviewPlane.GetComponent().material; + longAbImageMediaTexture = new Texture2D(320, 288, TextureFormat.Alpha8, false); + longAbImageMediaMaterial.mainTexture = longAbImageMediaTexture; + } + depthPreviewPlane.SetActive(false); + shortAbImagePreviewPlane.SetActive(false); } + if (LFPreviewPlane != null) { @@ -109,15 +141,18 @@ void Start() #if ENABLE_WINMD_SUPPORT researchMode = new HL2ResearchMode(); - researchMode.InitializeDepthSensor(); - //researchMode.InitializeLongDepthSensor(); + + // Depth sensor should be initialized in only one mode + if (depthSensorMode == DepthSensorMode.LongThrow) researchMode.InitializeLongDepthSensor(); + else if (depthSensorMode == DepthSensorMode.ShortThrow) researchMode.InitializeDepthSensor(); + researchMode.InitializeSpatialCamerasFront(); researchMode.SetReferenceCoordinateSystem(unityWorldOrigin); researchMode.SetPointCloudDepthOffset(0); // Depth sensor should be initialized in only one mode - researchMode.StartDepthSensorLoop(); - //researchMode.StartLongDepthSensorLoop(); + if (depthSensorMode == DepthSensorMode.LongThrow) researchMode.StartLongDepthSensorLoop(enablePointCloud); + else if (depthSensorMode == DepthSensorMode.ShortThrow) researchMode.StartDepthSensorLoop(enablePointCloud); researchMode.StartSpatialCamerasFrontLoop(); #endif @@ -128,7 +163,8 @@ void LateUpdate() { #if ENABLE_WINMD_SUPPORT // update depth map texture - if (startRealtimePreview && depthPreviewPlane != null && researchMode.DepthMapTextureUpdated()) + if (depthSensorMode == DepthSensorMode.ShortThrow && startRealtimePreview && + depthPreviewPlane != null && researchMode.DepthMapTextureUpdated()) { byte[] frameTexture = researchMode.GetDepthMapTextureBuffer(); if (frameTexture.Length > 0) @@ -146,8 +182,10 @@ void LateUpdate() depthMediaTexture.Apply(); } } + // update short-throw AbImage texture - if (startRealtimePreview && shortAbImagePreviewPlane != null && researchMode.ShortAbImageTextureUpdated()) + if (depthSensorMode == DepthSensorMode.ShortThrow && startRealtimePreview && + shortAbImagePreviewPlane != null && researchMode.ShortAbImageTextureUpdated()) { byte[] frameTexture = researchMode.GetShortAbImageTextureBuffer(); if (frameTexture.Length > 0) @@ -165,25 +203,48 @@ void LateUpdate() shortAbImageMediaTexture.Apply(); } } + // update long depth map texture - //if (startRealtimePreview && longDepthPreviewPlane != null && researchMode.LongDepthMapTextureUpdated()) - //{ - // byte[] frameTexture = researchMode.GetLongDepthMapTextureBuffer(); - // if (frameTexture.Length > 0) - // { - // if (longDepthFrameData == null) - // { - // longDepthFrameData = frameTexture; - // } - // else - // { - // System.Buffer.BlockCopy(frameTexture, 0, longDepthFrameData, 0, longDepthFrameData.Length); - // } - - // longDepthMediaTexture.LoadRawTextureData(longDepthFrameData); - // longDepthMediaTexture.Apply(); - // } - //} + if (depthSensorMode == DepthSensorMode.LongThrow && startRealtimePreview && + longDepthPreviewPlane != null && researchMode.LongDepthMapTextureUpdated()) + { + byte[] frameTexture = researchMode.GetLongDepthMapTextureBuffer(); + if (frameTexture.Length > 0) + { + if (longDepthFrameData == null) + { + longDepthFrameData = frameTexture; + } + else + { + System.Buffer.BlockCopy(frameTexture, 0, longDepthFrameData, 0, longDepthFrameData.Length); + } + + longDepthMediaTexture.LoadRawTextureData(longDepthFrameData); + longDepthMediaTexture.Apply(); + } + } + + // update long-throw AbImage texture + if (depthSensorMode == DepthSensorMode.LongThrow && startRealtimePreview && + longAbImagePreviewPlane != null && researchMode.LongAbImageTextureUpdated()) + { + byte[] frameTexture = researchMode.GetLongAbImageTextureBuffer(); + if (frameTexture.Length > 0) + { + if (longAbImageFrameData == null) + { + longAbImageFrameData = frameTexture; + } + else + { + System.Buffer.BlockCopy(frameTexture, 0, longAbImageFrameData, 0, longAbImageFrameData.Length); + } + + longAbImageMediaTexture.LoadRawTextureData(longAbImageFrameData); + longAbImageMediaTexture.Apply(); + } + } // update LF camera texture if (startRealtimePreview && LFPreviewPlane != null && researchMode.LFImageUpdated()) @@ -227,9 +288,22 @@ void LateUpdate() } // Update point cloud - if (renderPointCloud && pointCloudRendererGo != null) + UpdatePointCloud(); +#endif + } + +#if ENABLE_WINMD_SUPPORT + private void UpdatePointCloud() + { + if (enablePointCloud && renderPointCloud && pointCloudRendererGo != null) { - float[] pointCloud = researchMode.GetPointCloudBuffer(); + if ((depthSensorMode == DepthSensorMode.LongThrow && !researchMode.LongThrowPointCloudUpdated()) || + (depthSensorMode == DepthSensorMode.ShortThrow && !researchMode.PointCloudUpdated())) return; + + float[] pointCloud = new float[] { }; + if (depthSensorMode == DepthSensorMode.LongThrow) pointCloud = researchMode.GetLongThrowPointCloudBuffer(); + else if (depthSensorMode == DepthSensorMode.ShortThrow) pointCloud = researchMode.GetPointCloudBuffer(); + if (pointCloud.Length > 0) { int pointCloudLength = pointCloud.Length / 3; @@ -238,13 +312,12 @@ void LateUpdate() { pointCloudVector3[i] = new Vector3(pointCloud[3 * i], pointCloud[3 * i + 1], pointCloud[3 * i + 2]); } - //Debug.LogError("Point Cloud Size: " + pointCloudVector3.Length.ToString()); + text.text = "Point Cloud Length: " + pointCloudVector3.Length.ToString(); pointCloudRenderer.Render(pointCloudVector3, pointColor); - } } -#endif } +#endif #region Button Event Functions diff --git a/UnitySample/README.md b/UnitySample/README.md index 0e5352a..b667ed9 100644 --- a/UnitySample/README.md +++ b/UnitySample/README.md @@ -10,9 +10,10 @@ This Unity project shows - Unity 2019.4* - Visual Studio 2019 -\* To use it in Unity 2020 or later, +\* To use it in Unity 2020.1 - 2021.1, - Open Unity project and install XRSDK (Project Settings-XR Plugin Management-install, then tick "Windows Mixed Reality") - Select MixedRealityToolkit Gameobject in the Hierarchy. In the Inspector, change the mixed reality configuration profile to `New XRSDKConfigurationProfile` (or `DefaultXRSDKConfigurationProfile`). +- Point cloud sample not supported in Unity 2021.2 or later since OpenXR becomes the only supported pipeline with different way of obtaining the Unity world coordiante frame. Other functions shouldn't be influenced. # Build 1. Open this folder in Unity.