point size change, point color change, all only for landmarks, also p…

…rinting the extrinsics for Odometry
cvg · Mar 15, 2023 · d9df67d · d9df67d
1 parent 4cb8997
commit d9df67d
Show file tree

Hide file tree

Showing 14 changed files with 135 additions and 90 deletions.
diff --git a/HL2UnityPlugin/HL2ResearchMode.cpp b/HL2UnityPlugin/HL2ResearchMode.cpp
@@ -1190,45 +1190,53 @@ namespace winrt::HL2UnityPlugin::implementation
         return winrt::to_hstring(msg);
     }
 
-    // hstring HL2ResearchMode::PrintRigNodeInCoordSystem(SpatialCoordinateSystem coordSys)
-    // {
-    //     if (coordSys == nullptr)
-    //     {
-    //         return nullptr;
-    //     }
+    hstring HL2ResearchMode::PrintRigNodeInCoordSystem(SpatialCoordinateSystem coordSys)
+    {
+        if (coordSys == nullptr)
+        {
+            return winrt::to_hstring("coordSys not valid");
+        }
 
-    //     if (m_locator == 0)
-    //     {
-    //         std::cout << "m_locator does not exist yet!" << std::endl;
-    //         return nullptr;
-    //     }
-
-    //     ResearchModeSensorTimestamp timestamp;
-    //     m_LFSensor->GetTimeStamp(&timestamp);
-    //     SpatialLocation location = m_locator.TryLocateAtTimestamp(timestamp, coordSys);
-
-    //     // Printing the location rot and pos separately
-    //     std::cout << "Odometry Extrinsics separate: \n" << std::endl;
-    //     auto rot = location.Orientation();
-    //     auto quatInDx = XMFLOAT4(rot.x, rot.y, rot.z, rot.w);
-    //     auto rotMat = XMMatrixRotationQuaternion(XMLoadFloat4(&quatInDx));
-    //     std::cout << rotMat << std::endl; 
-    //     auto pos = location.Position();
-    //     auto posMat = XMMatrixTranslation(pos.x, pos.y, pos.z);
-    //     std::cout << posMat << std::endl;
-
-    //     // Printing the entire tranformation matrix, this should be rigNode location in coordSys
-    //     XMMATRIX locationMat = SpatialLocationToDxMatrix(location);
-    //     XMFLOAT4X4 locationMatFloat4X4;
-    //     XMStoreFloat4x4(&locationMatFloat4X4, locationMat);
-
-    //     std::stringstream ss;
-    //     ss << "Odometry Extrinsics: \n" << MatrixToString(locationMatFloat4X4);
-    //     std::string msg = ss.str();
-    //     std::wstring widemsg = std::wstring(msg.begin(), msg.end());
-    //     OutputDebugString(widemsg.c_str());
-    //     return winrt::to_hstring(msg);
-    // }
+        if (m_locator == 0)
+        {
+            std::cout << "m_locator does not exist yet!" << std::endl;
+            return winrt::to_hstring("m_locator does not exist yet");
+        }
+
+
+        IResearchModeSensorFrame* pLFCameraFrame = nullptr;
+        m_LFSensor->GetNextBuffer(&pLFCameraFrame);
+
+        IResearchModeSensorVLCFrame* pLFFrame = nullptr;
+        winrt::check_hresult(pLFCameraFrame->QueryInterface(IID_PPV_ARGS(&pLFFrame)));
+
+        ResearchModeSensorTimestamp timestamp;
+        pLFCameraFrame->GetTimeStamp(&timestamp);
+        auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks)));
+        SpatialLocation location = m_locator.TryLocateAtTimestamp(ts, coordSys);
+
+        // // Printing the location rot and pos separately
+        // std::cout << "Odometry Extrinsics separate: \n" << std::endl;
+        // auto rot = location.Orientation();
+        // auto quatInDx = XMFLOAT4(rot.x, rot.y, rot.z, rot.w);
+        // auto rotMat = XMMatrixRotationQuaternion(XMLoadFloat4(&quatInDx));
+        // std::cout << rotMat << std::endl; 
+        // auto pos = location.Position();
+        // auto posMat = XMMatrixTranslation(pos.x, pos.y, pos.z);
+        // std::cout << posMat << std::endl;
+
+        // Printing the entire tranformation matrix, this should be rigNode location in coordSys
+        XMMATRIX locationMat = SpatialLocationToDxMatrix(location);
+        XMFLOAT4X4 locationMatFloat4X4;
+        XMStoreFloat4x4(&locationMatFloat4X4, locationMat);
+
+        std::stringstream ss;
+        ss << "Odometry Extrinsics: \n" << MatrixToString(locationMatFloat4X4);
+        std::string msg = ss.str();
+        std::wstring widemsg = std::wstring(msg.begin(), msg.end());
+        OutputDebugString(widemsg.c_str());
+        return winrt::to_hstring(msg);
+    }
 
     std::string HL2ResearchMode::MatrixToString(DirectX::XMFLOAT4X4 mat)
     {

diff --git a/HL2UnityPlugin/HL2ResearchMode.h b/HL2UnityPlugin/HL2ResearchMode.h
@@ -39,6 +39,9 @@ namespace winrt::HL2UnityPlugin::implementation
         hstring PrintAccelExtrinsics();
         hstring PrintGyroExtrinsics();
 
+        hstring PrintRigNodeInCoordSystem(Windows::Perception::Spatial::SpatialCoordinateSystem coordSys);
+
+
         void InitializeDepthSensor();
         void InitializeLongDepthSensor();
         void InitializeSpatialCamerasFront();

diff --git a/HL2UnityPlugin/HL2ResearchMode.idl b/HL2UnityPlugin/HL2ResearchMode.idl
@@ -44,6 +44,8 @@ namespace HL2UnityPlugin
         String PrintAccelExtrinsics();
         String PrintGyroExtrinsics();
 
+        String PrintRigNodeInCoordSystem(Windows.Perception.Spatial.SpatialCoordinateSystem coordSys);
+
         Boolean DepthMapTextureUpdated();
         Boolean ShortAbImageTextureUpdated();
         Boolean LongAbImageTextureUpdated();

diff --git a/UnitySample/Assets/Materials/PointCloudMaterial.mat b/UnitySample/Assets/Materials/PointCloudMaterial.mat
@@ -75,7 +75,7 @@ Material:
     - _Mode: 0
     - _OcclusionStrength: 1
     - _Parallax: 0.02
-    - _PointSize: 0.001
+    - _PointSize: 0.01
     - _Size: 0.005
     - _SmoothnessTextureChannel: 0
     - _SpecularHighlights: 1
@@ -87,3 +87,4 @@ Material:
     - _Color: {r: 1, g: 1, b: 1, a: 1}
     - _EmissionColor: {r: 0, g: 0, b: 0, a: 1}
     - _SpecColor: {r: 0.2, g: 0.2, b: 0.2, a: 1}
+  m_BuildTextureStacks: []
diff --git a/...ets/MixedRealityToolkit.Generated/CustomProfiles/New MixedRealityDiagnosticsProfile.asset b/...ets/MixedRealityToolkit.Generated/CustomProfiles/New MixedRealityDiagnosticsProfile.asset
@@ -0,0 +1,27 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!114 &11400000
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 8c2d00f2d26cc124caed106ffbfe3f06, type: 3}
+  m_Name: New MixedRealityDiagnosticsProfile
+  m_EditorClassIdentifier: 
+  isCustomProfile: 1
+  showDiagnostics: 1
+  showProfiler: 0
+  showFrameInfo: 1
+  showMemoryStats: 1
+  frameSampleRate: 0.1
+  windowAnchor: 7
+  windowOffset: {x: 0.1, y: 0.1}
+  windowScale: 1
+  windowFollowSpeed: 5
+  defaultInstancedMaterial: {fileID: 2100000, guid: 5b6fc83077d74fd4ca4c675623942a3e,
+    type: 2}
+  showProfilerDuringMRC: 0
diff --git a/...alityToolkit.Generated/CustomProfiles/New MixedRealityToolkitConfigurationProfile 2.asset b/...alityToolkit.Generated/CustomProfiles/New MixedRealityToolkitConfigurationProfile 2.asset
@@ -43,7 +43,7 @@ MonoBehaviour:
       Microsoft.MixedReality.Toolkit.Services.SpatialAwarenessSystem
   spatialAwarenessSystemProfile: {fileID: 11400000, guid: 81601bd5c5dab1d43a3edb0ede45c046,
     type: 2}
-  diagnosticsSystemProfile: {fileID: 11400000, guid: 478436bd1083882479a52d067e98e537,
+  diagnosticsSystemProfile: {fileID: 11400000, guid: 0f4379f4e6d743445a81914f95e3c6fd,
     type: 2}
   enableDiagnosticsSystem: 1
   diagnosticsSystemType:

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/Resources/Default Point.mat b/UnitySample/Assets/Resources/Default Point.mat
@@ -50,7 +50,7 @@ Material:
     - _FlipbookMode: 0
     - _LightingEnabled: 0
     - _Mode: 0
-    - _PointSize: 0.0025000002
+    - _PointSize: 0.01
     - _SoftParticlesEnabled: 0
     - _SoftParticlesFarFadeDistance: 1
     - _SoftParticlesNearFadeDistance: 0

diff --git a/UnitySample/Assets/Resources/ROSConnectionPrefab.prefab b/UnitySample/Assets/Resources/ROSConnectionPrefab.prefab
@@ -43,7 +43,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 7acef0b79454c9b4dae3f8139bc4ba77, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  m_RosIPAddress: 192.168.1.180
+  m_RosIPAddress: 129.132.105.150
   m_RosPort: 9090
   m_ConnectOnStart: 0
   m_KeepaliveTime: 2

diff --git a/UnitySample/Assets/Scenes/PointCloudSample.unity b/UnitySample/Assets/Scenes/PointCloudSample.unity
@@ -2859,7 +2859,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 06473f65f83aaa845b717448f572d7a4, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  hostIPAddress: 192.168.50.179
+  hostIPAddress: 129.132.105.150
   port: 9090
   ConnectionStatusLED: {fileID: 1648274831}
 --- !u!114 &891080483
@@ -5192,7 +5192,7 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   FinishedReader: 0
-  ip: 192.168.1.180
+  ip: 192.168.1.24
   port: 9090
 --- !u!4 &1119771596
 Transform:
@@ -5237,7 +5237,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 7acef0b79454c9b4dae3f8139bc4ba77, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  m_RosIPAddress: 127.0.0.1
+  m_RosIPAddress: 129.132.105.150
   m_RosPort: 10000
   m_ConnectOnStart: 0
   m_KeepaliveTime: 1

diff --git a/UnitySample/Assets/Scipts/ConfigReader.cs b/UnitySample/Assets/Scipts/ConfigReader.cs
@@ -14,8 +14,8 @@ public class ConfigReader : MonoBehaviour
 {
     public bool FinishedReader = false;
 
-    public string ip = "192.168.50.238";
-    public int port = 9091;
+    public string ip = "129.132.105.150";
+    public int port = 9090;
 
     private ROSConnection ros;
 

diff --git a/UnitySample/Assets/Scipts/OdomPublisher.cs b/UnitySample/Assets/Scipts/OdomPublisher.cs
@@ -77,6 +77,8 @@ IEnumerator Start()
         previousTimestamp = GetCurrentTimestamp();
 #endif
 
+
+
     }
 
     void FixedUpdate()
@@ -117,50 +119,50 @@ private void InitializeMessage()
         };
     }
 
-    private void UpdateMessage()
-    {
-        // Add check to see if odometry never changed (aka lose tracking)
-
-        // Update header
-        message.header.seq++;
-        // timer.Now(message.header.stamp);
-        // // Get the midpoint time
-        float deltaTime = (float)(Time.realtimeSinceStartup - previousRealTime);
-        double midTime = previousRealTime + ((Time.realtimeSinceStartup - previousRealTime) / 2.0);
-        message.header.stamp.sec = (uint)midTime;
-        message.header.stamp.nanosec = (uint)((midTime - (int)message.header.stamp.sec) * 1e9);
+    // private void UpdateMessage()
+    // {
+    //     // Add check to see if odometry never changed (aka lose tracking)
+
+    //     // Update header
+    //     message.header.seq++;
+    //     // timer.Now(message.header.stamp);
+    //     // // Get the midpoint time
+    //     float deltaTime = (float)(Time.realtimeSinceStartup - previousRealTime);
+    //     double midTime = previousRealTime + ((Time.realtimeSinceStartup - previousRealTime) / 2.0);
+    //     message.header.stamp.sec = (uint)midTime;
+    //     message.header.stamp.nanosec = (uint)((midTime - (int)message.header.stamp.sec) * 1e9);
 
-        // Adding the Pose
-        Debug.Log("Adding the pose to message");
-        var relativePosition = World.InverseTransformPoint(Device.position);
-        Quaternion relativeRotation = Quaternion.Inverse(World.rotation) * Device.rotation;
-        Debug.Log("relativePosition: " + relativePosition);
-        Debug.Log("relativeRotation: " + relativeRotation);
-        Debug.Log("Device.rotation: " + Device.rotation);
-
-        // Turning the Pose to ROS messages
-        GetGeometryPoint(relativePosition.Unity2Ros(), message.pose.pose.position);
-        GetGeometryQuaternion(relativeRotation.Unity2Ros(), message.pose.pose.orientation);
-
-        // Adding the Twist
-        Debug.Log("Adding twist to message");
-        Vector3 linearVelocity = (relativePosition - previousPosition)/deltaTime;
-        linearVelocity = relativeRotation * linearVelocity;
-
-        Vector3 angularVelocity = (relativeRotation.eulerAngles - previousRotation.eulerAngles)/deltaTime;
-
-        message.twist.twist.linear = GetGeometryVector3(linearVelocity.Unity2Ros());
-        message.twist.twist.angular = GetGeometryVector3(angularVelocity.Unity2Ros());
-
-        previousRealTime = Time.realtimeSinceStartup;
-        previousPosition = relativePosition;
-        previousRotation = relativeRotation;
-
-        // Publish Odometry Message
-        Debug.Log("Publishing OdometryMsg");
-        ros.Publish(OdomTopicName, message);
-        Debug.Log("Published OdometryMsg");
-    }
+    //     // Adding the Pose
+    //     Debug.Log("Adding the pose to message");
+    //     var relativePosition = World.InverseTransformPoint(Device.position); // Device position in the world
+    //     Quaternion relativeRotation = Quaternion.Inverse(World.rotation) * Device.rotation;
+    //     Debug.Log("relativePosition: " + relativePosition);
+    //     Debug.Log("relativeRotation: " + relativeRotation);
+    //     Debug.Log("Device.rotation: " + Device.rotation);
+
+    //     // Turning the Pose to ROS messages
+    //     GetGeometryPoint(relativePosition.Unity2Ros(), message.pose.pose.position);
+    //     GetGeometryQuaternion(relativeRotation.Unity2Ros(), message.pose.pose.orientation);
+
+    //     // Adding the Twist
+    //     Debug.Log("Adding twist to message");
+    //     Vector3 linearVelocity = (relativePosition - previousPosition)/deltaTime;
+    //     linearVelocity = relativeRotation * linearVelocity;
+
+    //     Vector3 angularVelocity = (relativeRotation.eulerAngles - previousRotation.eulerAngles)/deltaTime;
+
+    //     message.twist.twist.linear = GetGeometryVector3(linearVelocity.Unity2Ros());
+    //     message.twist.twist.angular = GetGeometryVector3(angularVelocity.Unity2Ros());
+
+    //     previousRealTime = Time.realtimeSinceStartup;
+    //     previousPosition = relativePosition;
+    //     previousRotation = relativeRotation;
+
+    //     // Publish Odometry Message
+    //     Debug.Log("Publishing OdometryMsg");
+    //     ros.Publish(OdomTopicName, message);
+    //     Debug.Log("Published OdometryMsg");
+    // }
 
     private void UpdateMessageUsingWindowsSpatialLocator()
     {

diff --git a/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs b/UnitySample/Assets/Scipts/ResearchModeVideoStream.cs
@@ -231,6 +231,8 @@ IEnumerator Start()
         researchMode.PrintRFExtrinsics();
         researchMode.PrintLLExtrinsics();
         researchMode.PrintRRExtrinsics();
+
+        researchMode.PrintRigNodeInCoordSystem(unityWorldOrigin);
 #endif
     }