Make tracking states available, reset configs

KinectToVR · Jun 3, 2024 · 41d561d · 41d561d
1 parent c0710dd
commit 41d561d
Show file tree

Hide file tree

Showing 14 changed files with 216 additions and 220 deletions.
diff --git a/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj b/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj
@@ -7,13 +7,13 @@
     <GeneratePackageOnBuild>True</GeneratePackageOnBuild>
     <Title>Amethyst Device Plugin API (Contract)</Title>
     <FileVersion></FileVersion>
-    <Version>0.2.11</Version>
+    <Version>0.2.13</Version>
     <Platforms>x64</Platforms>
   </PropertyGroup>
 
   <PropertyGroup>
     <PackageId>Amethyst.Plugins.Contract</PackageId>
-    <Version>0.2.22</Version>
+    <Version>0.2.23</Version>
     <Authors>K2VR</Authors>
     <Company>K2VR</Company>
     <PackageTags>Amethyst;K2VR;KinectToVR;VR;OpenVR;Fullbody Tracking</PackageTags>

diff --git a/Amethyst.Plugins.Contract/Classes.cs b/Amethyst.Plugins.Contract/Classes.cs
@@ -121,6 +121,12 @@ public TrackerBase()
     [DataMember(Name = "ConnectionState")]
     public bool ConnectionState { get; set; } = false;
 
+    /// <summary>
+    ///     Tracking state: tracked/occluded/not tracked
+    /// </summary>
+    [DataMember(Name = "TrackingState")]
+    public TrackedJointState TrackingState { get; set; } = TrackedJointState.StateNotTracked;
+
     /// <summary>
     ///     Serial number, or name, identifies a tracker
     /// </summary>

diff --git a/Amethyst.Support/Amethyst.Support.vcxproj b/Amethyst.Support/Amethyst.Support.vcxproj
@@ -129,7 +129,6 @@
     </Link>
   </ItemDefinitionGroup>
   <ItemGroup>
-    <ClInclude Include="KalmanFilter.h" />
     <ClInclude Include="pch.h" />
     <ClInclude Include="Support.h">
       <DependentUpon>Support.idl</DependentUpon>

diff --git a/Amethyst.Support/KalmanFilter.h b/Amethyst.Support/KalmanFilter.h
diff --git a/Amethyst.Support/Support.cpp b/Amethyst.Support/Support.cpp
@@ -3,9 +3,8 @@
 
 #include "Support.g.cpp"       // NOLINT(bugprone-suspicious-include)
 #include "LowPassFilter.g.cpp" // NOLINT(bugprone-suspicious-include)
-#include "KalmanFilter.g.cpp"  // NOLINT(bugprone-suspicious-include)
 
-#include "KalmanFilter.h"
+#include <Eigen/Dense>
 #include <numbers>
 
 // The upper bound of the fog volume along the y-axis (height)
@@ -344,39 +343,4 @@ namespace winrt::AmethystSupport::implementation
 
         return output_; // Update and return
     }
-
-    KalmanFilter::KalmanFilter()
-    {
-        for (auto& filter : filters_)
-            filter.init(); // Setup!
-    }
-
-    SVector3 KalmanFilter::Update(const SVector3& input)
-    {
-        Eigen::VectorXf u(1);
-        Eigen::VectorXf y[3] =
-        {
-            Eigen::VectorXf(1),
-            Eigen::VectorXf(1),
-            Eigen::VectorXf(1)
-        };
-
-        y[0] << input.X;
-        y[1] << input.Y;
-        y[2] << input.Z;
-        u << 0; // zero control input
-
-        for (auto& filter : filters_)
-            filter.predict(u);
-
-        filters_[0].update(y[0]);
-        filters_[1].update(y[1]);
-        filters_[2].update(y[2]);
-
-        return SVector3{
-            .X = filters_[0].state().x(),
-            .Y = filters_[1].state().x(),
-            .Z = filters_[2].state().x()
-        };
-    }
 }
diff --git a/Amethyst.Support/Support.h b/Amethyst.Support/Support.h
@@ -1,7 +1,5 @@
 #pragma once
 #include "Support.g.h"
-#include "KalmanFilter.g.h"
-#include "KalmanFilter.h"
 #include "LowPassFilter.g.h"
 
 namespace winrt::AmethystSupport::implementation
@@ -29,16 +27,6 @@ namespace winrt::AmethystSupport::implementation
         SVector3 output_{0, 0, 0};
         float e_pow_; // Init-only
     };
-
-    struct KalmanFilter : KalmanFilterT<KalmanFilter>
-    {
-        KalmanFilter();
-
-        SVector3 Update(const SVector3& input);
-
-    private:
-        std::array<CKalmanFilter, 3> filters_;
-    };
 }
 
 namespace winrt::AmethystSupport::factory_implementation
@@ -50,8 +38,4 @@ namespace winrt::AmethystSupport::factory_implementation
     struct LowPassFilter : LowPassFilterT<LowPassFilter, implementation::LowPassFilter>
     {
     };
-
-    struct KalmanFilter : KalmanFilterT<KalmanFilter, implementation::KalmanFilter>
-    {
-    };
 }
diff --git a/Amethyst.Support/Support.idl b/Amethyst.Support/Support.idl
@@ -40,11 +40,4 @@ namespace AmethystSupport
 		LowPassFilter(Single cutOff, Single deltaTime);
 		SVector3 Update(SVector3 input); // Recalculate
     }
-
-    [default_interface]
-	runtimeclass KalmanFilter
-	{
-		KalmanFilter(); // No explc parameters required
-		SVector3 Update(SVector3 input); // Recalculate
-    }
 }
diff --git a/Amethyst.sln b/Amethyst.sln
@@ -10,60 +10,50 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Amethyst.Support", "Amethys
 EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
-		Debug|ARM = Debug|ARM
 		Debug|ARM64 = Debug|ARM64
 		Debug|x64 = Debug|x64
 		Debug|x86 = Debug|x86
-		Release|ARM = Release|ARM
 		Release|ARM64 = Release|ARM64
 		Release|x64 = Release|x64
 		Release|x86 = Release|x86
 	EndGlobalSection
 	GlobalSection(ProjectConfigurationPlatforms) = postSolution
-		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM.ActiveCfg = Debug|x64
-		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM.Build.0 = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.ActiveCfg = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.Build.0 = Debug|x64
+		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.Deploy.0 = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.ActiveCfg = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.Build.0 = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.Deploy.0 = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.ActiveCfg = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.Build.0 = Debug|x64
-		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM.ActiveCfg = Release|x64
-		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM.Build.0 = Release|x64
+		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.Deploy.0 = Debug|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.ActiveCfg = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.Build.0 = Release|x64
+		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.Deploy.0 = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.ActiveCfg = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.Build.0 = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.Deploy.0 = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.ActiveCfg = Release|x64
 		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.Build.0 = Release|x64
-		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM.ActiveCfg = Debug|x64
-		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM.Build.0 = Debug|x64
+		{4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.Deploy.0 = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM64.ActiveCfg = Debug|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM64.Build.0 = Debug|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x64.ActiveCfg = Debug|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x64.Build.0 = Debug|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x86.ActiveCfg = Debug|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x86.Build.0 = Debug|x64
-		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM.ActiveCfg = Release|x64
-		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM.Build.0 = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM64.ActiveCfg = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM64.Build.0 = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x64.ActiveCfg = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x64.Build.0 = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x86.ActiveCfg = Release|x64
 		{8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x86.Build.0 = Release|x64
-		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM.ActiveCfg = Debug|ARM
-		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM.Build.0 = Debug|ARM
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM64.ActiveCfg = Debug|ARM64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM64.Build.0 = Debug|ARM64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x64.ActiveCfg = Debug|x64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x64.Build.0 = Debug|x64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x86.ActiveCfg = Debug|Win32
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x86.Build.0 = Debug|Win32
-		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM.ActiveCfg = Release|ARM
-		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM.Build.0 = Release|ARM
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM64.ActiveCfg = Release|ARM64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM64.Build.0 = Release|ARM64
 		{FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|x64.ActiveCfg = Release|x64

diff --git a/Amethyst/Amethyst.csproj b/Amethyst/Amethyst.csproj
@@ -55,6 +55,7 @@
         <PackageReference Include="CommunityToolkit.WinUI.UI.Controls" Version="7.1.2" />
         <PackageReference Include="CommunityToolkit.WinUI.UI.Controls.Markdown" Version="7.1.2" />
         <PackageReference Include="CommunityToolkit.WinUI.UI.Media" Version="7.1.2" />
+        <PackageReference Include="MathNet.Numerics" Version="5.0.0" />
         <PackageReference Include="Microsoft-WindowsAPICodePack-Shell" Version="1.1.5" />
         <PackageReference Include="Microsoft.AppCenter.Analytics" Version="5.0.3" />
         <PackageReference Include="Microsoft.AppCenter.Crashes" Version="5.0.3" />

diff --git a/Amethyst/Classes/AppSettings.cs b/Amethyst/Classes/AppSettings.cs
@@ -543,8 +543,8 @@ public static bool? ExtraTrackers
 
     public static bool SetupFinished
     {
-        get => Windows.Storage.ApplicationData.Current.LocalSettings.Values["SetupFinished"] as bool? ?? false;
-        set => Windows.Storage.ApplicationData.Current.LocalSettings.Values["SetupFinished"] = value;
+        get => Windows.Storage.ApplicationData.Current.LocalSettings.Values["FirstSetupFinished"] as bool? ?? false;
+        set => Windows.Storage.ApplicationData.Current.LocalSettings.Values["FirstSetupFinished"] = value;
     }
 }
 

diff --git a/Amethyst/Classes/AppTracker.cs b/Amethyst/Classes/AppTracker.cs
@@ -80,6 +80,7 @@ public class AppTracker : INotifyPropertyChanged
 
     [JsonIgnore] public long PoseTimestamp { get; set; } = 0;
     [JsonIgnore] public long PreviousPoseTimestamp { get; set; } = 0;
+    [JsonIgnore] public TrackedJointState TrackingState { get; set; } = TrackedJointState.StateNotTracked;
 
     // Is this joint overridden?
     public bool IsPositionOverridden
@@ -626,6 +627,7 @@ public TrackerBase GetTrackerBase(
         var trackerBase = new TrackerBase
         {
             ConnectionState = IsActive,
+            TrackingState = TrackingState,
             Role = Role,
             Serial = Serial,
 
@@ -683,7 +685,7 @@ public void UpdateFilters()
     {
         // Update LowPass and Kalman filters
         _lowPassPosition = _lowPassFilter.Update(Position.Projected()).V();
-        _kalmanPosition = _kalmanFilter.Update(Position.Projected()).V();
+        _kalmanPosition = _kalmanFilter.Update(Position);
 
         // Update the LERP (mix) filter
         _lerpPosition = Vector3.Lerp(_lastLerpPosition, Position, 0.31f);

diff --git a/Amethyst/Classes/Main.cs b/Amethyst/Classes/Main.cs
@@ -402,6 +402,7 @@ private static void UpdateAppTrackers()
 
                 // Push raw positions and timestamps
                 tracker.Position = joint.Position;
+                tracker.TrackingState = joint.TrackingState;
                 tracker.PreviousPosition = joint.PreviousPosition;
 
                 tracker.PoseTimestamp = joint.PoseTimestamp;
@@ -496,6 +497,7 @@ private static void UpdateAppTrackers()
                 {
                     // Push raw positions and timestamps
                     tracker.Position = joint.Position;
+                    tracker.TrackingState = joint.TrackingState;
                     tracker.PreviousPosition = joint.PreviousPosition;
 
                     tracker.PoseTimestamp = joint.PoseTimestamp;