Add point cloud visualization in long-throw mode

cvg · Nov 17, 2021 · 6df98e9 · 6df98e9
1 parent 8c3e897
commit 6df98e9
Show file tree

Hide file tree

Showing 9 changed files with 488 additions and 141 deletions.
diff --git a/HL2UnityPlugin/HL2ResearchMode.cpp b/HL2UnityPlugin/HL2ResearchMode.cpp
diff --git 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<uint8_t> GetDepthMapTextureBuffer();
         com_array<uint16_t> GetShortAbImageBuffer();
         com_array<uint8_t> GetShortAbImageTextureBuffer();
+        com_array<uint16_t> GetLongAbImageBuffer();
+        com_array<uint8_t> GetLongAbImageTextureBuffer();
         com_array<uint16_t> GetLongDepthMapBuffer();
         com_array<uint8_t> GetLongDepthMapTextureBuffer();
         com_array<uint8_t> GetLFCameraBuffer(int64_t& ts);
@@ -77,17 +81,21 @@ namespace winrt::HL2UnityPlugin::implementation
         com_array<float> GetMagSample();
 
         com_array<float> GetPointCloudBuffer();
+        com_array<float> GetLongThrowPointCloudBuffer();
         com_array<float> GetCenterPoint();
-        com_array<float> 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,24 +131,30 @@ 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;
         std::atomic_bool m_accelSampleUpdated = false;
         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
@@ -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
diff --git a/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd b/UnitySample/Assets/Plugins/WSA/ARM64/HL2UnityPlugin.winmd
diff --git 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
@@ -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)