diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
new file mode 100644
index 0000000..64a0af0
--- /dev/null
+++ b/.github/workflows/build.yml
@@ -0,0 +1,163 @@
+name: Build
+
+on:
+  push:
+    branches: [main, dev]
+  pull_request:
+    branches: [main, dev]
+  workflow_dispatch:
+
+# Cancel an in-flight run when a newer commit lands on the same ref.
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  build:
+    name: ${{ matrix.label }}
+    runs-on: ${{ matrix.os }}
+    timeout-minutes: 60
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: ubuntu-22.04
+            label: linux-x86_64
+          # macos-13 (Intel) is in GitHub's deprecation window — jobs sit in
+          # the queue for hours. Apple Silicon only for now; x86_64 Mac users
+          # build from source.
+          - os: macos-14
+            label: macos-arm64
+          - os: windows-2022
+            label: windows-x86_64
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      # -------------------------------------------------------------------
+      # Linux: Intel apt repo for librealsense, plus OpenCV + GLFW system deps.
+      # -------------------------------------------------------------------
+      - name: Install Linux dependencies
+        if: runner.os == 'Linux'
+        run: |
+          set -euxo pipefail
+          sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            ca-certificates curl gnupg lsb-release \
+            build-essential cmake ninja-build pkg-config \
+            libopencv-dev \
+            libgl1-mesa-dev libglu1-mesa-dev \
+            libxinerama-dev libxcursor-dev libxi-dev libxrandr-dev \
+            libwayland-dev libxkbcommon-dev \
+            libusb-1.0-0-dev libudev-dev \
+            libgtk-3-dev
+
+          # Intel RealSense apt repo. The .pgp file Intel hosts on their CDN
+          # is currently stale relative to the key that actually signs the apt
+          # InRelease metadata (apt logs "NO_PUBKEY FB0B24895113F120"), so we
+          # fetch the signing key directly from Ubuntu's keyserver over HTTP
+          # and dearmor it. (HTTP avoids needing dirmngr + /root/.gnupg, both
+          # of which were missing on the runner.)
+          # If this ever stops working, run `apt-get update` against the repo
+          # locally to find the current key ID and update the fingerprint below.
+          sudo install -m 0755 -d /etc/apt/keyrings
+          curl -fsSL "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0xFB0B24895113F120&options=mr" \
+            | sudo gpg --dearmor -o /etc/apt/keyrings/librealsense.gpg
+          sudo chmod 0644 /etc/apt/keyrings/librealsense.gpg
+          echo "deb [signed-by=/etc/apt/keyrings/librealsense.gpg] https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" \
+            | sudo tee /etc/apt/sources.list.d/librealsense.list
+          sudo apt-get update
+          sudo apt-get install -y --no-install-recommends librealsense2-dev librealsense2-utils
+
+      # -------------------------------------------------------------------
+      # macOS (Apple Silicon, macos-14). Homebrew handles everything.
+      # If librealsense's brew formula breaks here, the most likely cause
+      # is upstream brew formula state.
+      # -------------------------------------------------------------------
+      - name: Install macOS dependencies
+        if: runner.os == 'macOS'
+        run: |
+          set -euxo pipefail
+          brew update
+          brew install cmake ninja opencv librealsense
+
+      # -------------------------------------------------------------------
+      # Windows: chocolatey for OpenCV, vcpkg for librealsense.
+      #
+      # Why not the Intel SDK installer: current Intel installer versions
+      # (InstallAware-based) have no working silent-install flag — /S and /s
+      # hang on a hidden GUI dialog, /silent and /VERYSILENT exit 0 but write
+      # nothing, and the .exe is not a format 7zip can extract. vcpkg is the
+      # reliable path. To avoid touching CMakeLists.txt (which hardcodes
+      # C:\Program Files (x86)\Intel RealSense SDK 2.0\... for Windows), we
+      # restage vcpkg's output into that same layout.
+      # -------------------------------------------------------------------
+      - name: Export GHA cache env (for vcpkg x-gha binary cache)
+        if: runner.os == 'Windows'
+        uses: actions/github-script@v7
+        with:
+          script: |
+            core.exportVariable('ACTIONS_CACHE_URL', process.env.ACTIONS_CACHE_URL || '');
+            core.exportVariable('ACTIONS_RUNTIME_TOKEN', process.env.ACTIONS_RUNTIME_TOKEN || '');
+
+      - name: Install Windows dependencies
+        if: runner.os == 'Windows'
+        timeout-minutes: 25
+        shell: pwsh
+        env:
+          VCPKG_BINARY_SOURCES: 'clear;x-gha,readwrite'
+        run: |
+          $ErrorActionPreference = 'Stop'
+
+          Write-Host "Installing OpenCV via chocolatey..."
+          choco install opencv --version=4.10.0 -y --no-progress
+          $opencvDir = "C:\tools\opencv\build"
+          if (-not (Test-Path "$opencvDir\OpenCVConfig.cmake")) {
+            $opencvDir = Get-ChildItem -Path "C:\tools\opencv" -Recurse -Filter "OpenCVConfig.cmake" | Select-Object -First 1 -ExpandProperty Directory | Select-Object -ExpandProperty FullName
+          }
+          "OpenCV_DIR=$opencvDir"            | Out-File -FilePath $env:GITHUB_ENV -Append
+          "$opencvDir\x64\vc16\bin"          | Out-File -FilePath $env:GITHUB_PATH -Append
+
+          Write-Host "Installing librealsense via vcpkg (first run ~10-15 min, cached runs ~30s)..."
+          & "C:\vcpkg\vcpkg.exe" install realsense2:x64-windows --triplet=x64-windows
+          if ($LASTEXITCODE -ne 0) { throw "vcpkg install realsense2:x64-windows failed (exit $LASTEXITCODE)." }
+
+          $vcpkgInstalled = "C:\vcpkg\installed\x64-windows"
+          $vcpkgLib = "$vcpkgInstalled\lib\realsense2.lib"
+          $vcpkgDll = "$vcpkgInstalled\bin\realsense2.dll"
+          $vcpkgInc = "$vcpkgInstalled\include\librealsense2"
+          foreach ($p in @($vcpkgLib, $vcpkgDll, $vcpkgInc)) {
+            if (-not (Test-Path $p)) { throw "vcpkg did not produce $p" }
+          }
+
+          $intelDir = "C:\Program Files (x86)\Intel RealSense SDK 2.0"
+          Write-Host "Restaging vcpkg output to $intelDir to satisfy CMakeLists.txt hardcoded paths..."
+          New-Item -ItemType Directory -Force -Path "$intelDir\lib\x64", "$intelDir\bin\x64", "$intelDir\include" | Out-Null
+          Copy-Item $vcpkgLib -Destination "$intelDir\lib\x64\realsense2.lib" -Force
+          Copy-Item $vcpkgDll -Destination "$intelDir\bin\x64\realsense2.dll" -Force
+          Copy-Item $vcpkgInc -Destination "$intelDir\include\librealsense2" -Recurse -Force
+
+          if (-not (Test-Path "$intelDir\lib\x64\realsense2.lib")) {
+            throw "Restage failed: $intelDir\lib\x64\realsense2.lib missing."
+          }
+          Write-Host "RealSense SDK staged at $intelDir."
+
+      # -------------------------------------------------------------------
+      # Configure + build. Windows is pinned to the x64 VS generator so we
+      # never accidentally pick up a 32-bit toolchain (the OpenCV / RealSense
+      # libs we install above are x64-only). /MT linkage on Windows + prebuilt
+      # OpenCV (/MD) relies on the /FORCE linker flag already set in CMakeLists.
+      # -------------------------------------------------------------------
+      - name: Configure (Windows x64)
+        if: runner.os == 'Windows'
+        run: cmake -S . -B build -G "Visual Studio 17 2022" -A x64 -DCMAKE_BUILD_TYPE=Release
+
+      - name: Configure (Linux / macOS)
+        if: runner.os != 'Windows'
+        run: cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
+
+      - name: Build
+        run: cmake --build build --config Release --parallel
diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml
new file mode 100644
index 0000000..ac26110
--- /dev/null
+++ b/.github/workflows/release.yml
@@ -0,0 +1,402 @@
+name: Release
+
+on:
+  push:
+    tags:
+      - 'v*'
+
+permissions:
+  contents: write
+
+jobs:
+  build:
+    name: ${{ matrix.label }}
+    runs-on: ${{ matrix.os }}
+    timeout-minutes: 60
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: ubuntu-22.04
+            label: linux-x86_64
+          # macos-13 (Intel) dropped — GitHub runner queue is unreliable.
+          - os: macos-14
+            label: macos-arm64
+          - os: windows-2022
+            label: windows-x86_64
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      # -----------------------------------------------------------------
+      # Platform-specific dependency install (kept in sync with build.yml).
+      # -----------------------------------------------------------------
+      - name: Install Linux dependencies
+        if: runner.os == 'Linux'
+        run: |
+          set -euxo pipefail
+          sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            ca-certificates curl gnupg lsb-release \
+            build-essential cmake ninja-build pkg-config patchelf \
+            libopencv-dev \
+            libgl1-mesa-dev libglu1-mesa-dev \
+            libxinerama-dev libxcursor-dev libxi-dev libxrandr-dev \
+            libwayland-dev libxkbcommon-dev \
+            libusb-1.0-0-dev libudev-dev \
+            libgtk-3-dev
+
+          # See comment in build.yml — Intel's hosted key is stale, pull from keyserver over HTTP.
+          sudo install -m 0755 -d /etc/apt/keyrings
+          curl -fsSL "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0xFB0B24895113F120&options=mr" \
+            | sudo gpg --dearmor -o /etc/apt/keyrings/librealsense.gpg
+          sudo chmod 0644 /etc/apt/keyrings/librealsense.gpg
+          echo "deb [signed-by=/etc/apt/keyrings/librealsense.gpg] https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" \
+            | sudo tee /etc/apt/sources.list.d/librealsense.list
+          sudo apt-get update
+          sudo apt-get install -y --no-install-recommends librealsense2-dev librealsense2-utils
+
+      - name: Install macOS dependencies
+        if: runner.os == 'macOS'
+        run: |
+          set -euxo pipefail
+          brew update
+          brew install cmake ninja opencv librealsense dylibbundler
+
+      # See build.yml for rationale: the Intel SDK installer has no working
+      # silent flag, so librealsense comes from vcpkg, restaged into the
+      # path CMakeLists.txt hardcodes.
+      - name: Export GHA cache env (for vcpkg x-gha binary cache)
+        if: runner.os == 'Windows'
+        uses: actions/github-script@v7
+        with:
+          script: |
+            core.exportVariable('ACTIONS_CACHE_URL', process.env.ACTIONS_CACHE_URL || '');
+            core.exportVariable('ACTIONS_RUNTIME_TOKEN', process.env.ACTIONS_RUNTIME_TOKEN || '');
+
+      - name: Install Windows dependencies
+        if: runner.os == 'Windows'
+        timeout-minutes: 25
+        shell: pwsh
+        env:
+          VCPKG_BINARY_SOURCES: 'clear;x-gha,readwrite'
+        run: |
+          $ErrorActionPreference = 'Stop'
+
+          choco install opencv --version=4.10.0 -y --no-progress
+          $opencvDir = "C:\tools\opencv\build"
+          if (-not (Test-Path "$opencvDir\OpenCVConfig.cmake")) {
+            $opencvDir = Get-ChildItem -Path "C:\tools\opencv" -Recurse -Filter "OpenCVConfig.cmake" | Select-Object -First 1 -ExpandProperty Directory | Select-Object -ExpandProperty FullName
+          }
+          "OpenCV_DIR=$opencvDir"   | Out-File -FilePath $env:GITHUB_ENV -Append
+          "$opencvDir\x64\vc16\bin" | Out-File -FilePath $env:GITHUB_PATH -Append
+
+          & "C:\vcpkg\vcpkg.exe" install realsense2:x64-windows --triplet=x64-windows
+          if ($LASTEXITCODE -ne 0) { throw "vcpkg install realsense2:x64-windows failed (exit $LASTEXITCODE)." }
+
+          $vcpkgInstalled = "C:\vcpkg\installed\x64-windows"
+          $vcpkgLib = "$vcpkgInstalled\lib\realsense2.lib"
+          $vcpkgDll = "$vcpkgInstalled\bin\realsense2.dll"
+          $vcpkgInc = "$vcpkgInstalled\include\librealsense2"
+          foreach ($p in @($vcpkgLib, $vcpkgDll, $vcpkgInc)) {
+            if (-not (Test-Path $p)) { throw "vcpkg did not produce $p" }
+          }
+
+          $intelDir = "C:\Program Files (x86)\Intel RealSense SDK 2.0"
+          New-Item -ItemType Directory -Force -Path "$intelDir\lib\x64", "$intelDir\bin\x64", "$intelDir\include" | Out-Null
+          Copy-Item $vcpkgLib -Destination "$intelDir\lib\x64\realsense2.lib" -Force
+          Copy-Item $vcpkgDll -Destination "$intelDir\bin\x64\realsense2.dll" -Force
+          Copy-Item $vcpkgInc -Destination "$intelDir\include\librealsense2" -Recurse -Force
+
+      # -----------------------------------------------------------------
+      # Configure + build. Windows pinned to x64 VS generator (the SDK and
+      # OpenCV we installed are x64-only).
+      # -----------------------------------------------------------------
+      - name: Configure (Windows x64)
+        if: runner.os == 'Windows'
+        run: cmake -S . -B build -G "Visual Studio 17 2022" -A x64 -DCMAKE_BUILD_TYPE=Release
+
+      - name: Configure (Linux / macOS)
+        if: runner.os != 'Windows'
+        run: cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
+
+      - name: Build
+        run: cmake --build build --config Release --parallel
+
+      # -----------------------------------------------------------------
+      # Package: produce a platform-native installer.
+      #   Linux   -> .deb (dpkg-deb, depends on system libgtk-3/libgl/libusb)
+      #   macOS   -> .dmg containing a self-contained .app bundle
+      #   Windows -> NSIS .exe installer (Start menu shortcut, Add/Remove entry)
+      # -----------------------------------------------------------------
+      - name: Derive version from tag
+        id: ver
+        shell: bash
+        run: |
+          # ref_name on a tag push is "v1.2.3" — strip leading 'v' to get "1.2.3".
+          VERSION="${GITHUB_REF_NAME#v}"
+          # Releases triggered via workflow_dispatch / branch push won't have a v-tag;
+          # use a 0.0.0+sha fallback so the packaging step still works.
+          if [[ "$VERSION" == "$GITHUB_REF_NAME" && ! "$GITHUB_REF_NAME" =~ ^[0-9] ]]; then
+            VERSION="0.0.0-${GITHUB_SHA::7}"
+          fi
+          echo "version=$VERSION" >> "$GITHUB_OUTPUT"
+          echo "Resolved version: $VERSION"
+
+      - name: Package (Linux .deb)
+        if: runner.os == 'Linux'
+        env:
+          VERSION: ${{ steps.ver.outputs.version }}
+        run: |
+          set -euxo pipefail
+          PKG="ir-tracking-app"
+          STAGE="$RUNNER_TEMP/deb-staging"
+          rm -rf "$STAGE"
+          mkdir -p "$STAGE/DEBIAN" "$STAGE/usr/bin" "$STAGE/usr/lib/$PKG" \
+                   "$STAGE/usr/share/applications" "$STAGE/usr/share/icons/hicolor/256x256/apps"
+
+          # Binary + bundled libs under /usr/lib/<pkg>/ with $ORIGIN rpath.
+          cp build/ir-tracking-app "$STAGE/usr/lib/$PKG/"
+          ldd build/ir-tracking-app \
+            | awk '/(librealsense|libopencv|libtbb|libusb-1\.0)/ { print $3 }' \
+            | grep -v '^$' | sort -u \
+            | while read -r lib; do cp -L "$lib" "$STAGE/usr/lib/$PKG/"; done
+          patchelf --set-rpath '$ORIGIN' "$STAGE/usr/lib/$PKG/ir-tracking-app"
+
+          # Launcher shim on PATH.
+          cat > "$STAGE/usr/bin/$PKG" <<SH
+          #!/usr/bin/env bash
+          exec /usr/lib/$PKG/ir-tracking-app "\$@"
+          SH
+          chmod 0755 "$STAGE/usr/bin/$PKG"
+
+          # Desktop entry so the app shows in app launchers.
+          cat > "$STAGE/usr/share/applications/$PKG.desktop" <<EOF
+          [Desktop Entry]
+          Type=Application
+          Name=IR Tracking App
+          Comment=Intel RealSense IR retro-reflective marker tracking
+          Exec=$PKG
+          Icon=$PKG
+          Categories=Science;Graphics;
+          Terminal=false
+          EOF
+
+          # dpkg control metadata. Depends covers the runtime system libs we
+          # rely on (GTK for nfd, OpenGL, libusb for RealSense USB).
+          INSTALLED_KB=$(du -sk "$STAGE" --exclude=DEBIAN | cut -f1)
+          cat > "$STAGE/DEBIAN/control" <<EOF
+          Package: $PKG
+          Version: $VERSION
+          Section: graphics
+          Priority: optional
+          Architecture: amd64
+          Maintainer: Tianyu Song <tianyu@medivis.com>
+          Homepage: https://github.com/stytim/RealSense-ToolTracker
+          Depends: libgtk-3-0, libgl1, libusb-1.0-0
+          Installed-Size: $INSTALLED_KB
+          Description: Intel RealSense IR retro-reflective marker tool tracker
+           Tracks passive IR sphere markers from an Intel RealSense camera and
+           publishes the tool pose over UDP. See the GitHub README for usage.
+          EOF
+
+          OUT="${PKG}_${VERSION}_amd64.deb"
+          dpkg-deb --build --root-owner-group "$STAGE" "$OUT"
+          ls -lh "$OUT"
+          dpkg-deb -I "$OUT"
+
+      - name: Package (macOS .dmg)
+        if: runner.os == 'macOS'
+        env:
+          VERSION: ${{ steps.ver.outputs.version }}
+        run: |
+          set -euxo pipefail
+          APP_NAME="IR Tracking App"
+          APP_DIR="$RUNNER_TEMP/$APP_NAME.app"
+          rm -rf "$APP_DIR"
+          mkdir -p "$APP_DIR/Contents/MacOS" \
+                   "$APP_DIR/Contents/Resources" \
+                   "$APP_DIR/Contents/Frameworks"
+
+          cp build/ir-tracking-app "$APP_DIR/Contents/MacOS/"
+          cp resources/app_icon.icns "$APP_DIR/Contents/Resources/app_icon.icns"
+
+          # Bundle dylibs into Contents/Frameworks/, with install names rewritten
+          # to @executable_path/../Frameworks/ so the .app is self-contained.
+          dylibbundler -od -b \
+            -x "$APP_DIR/Contents/MacOS/ir-tracking-app" \
+            -d "$APP_DIR/Contents/Frameworks" \
+            -p "@executable_path/../Frameworks/" \
+            || true
+
+          cat > "$APP_DIR/Contents/Info.plist" <<EOF
+          <?xml version="1.0" encoding="UTF-8"?>
+          <!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+          <plist version="1.0">
+          <dict>
+              <key>CFBundleExecutable</key><string>ir-tracking-app</string>
+              <key>CFBundleIdentifier</key><string>com.medivis.ir-tracking-app</string>
+              <key>CFBundleName</key><string>$APP_NAME</string>
+              <key>CFBundleDisplayName</key><string>$APP_NAME</string>
+              <key>CFBundleVersion</key><string>$VERSION</string>
+              <key>CFBundleShortVersionString</key><string>$VERSION</string>
+              <key>CFBundlePackageType</key><string>APPL</string>
+              <key>CFBundleIconFile</key><string>app_icon.icns</string>
+              <key>LSMinimumSystemVersion</key><string>11.0</string>
+              <key>NSHighResolutionCapable</key><true/>
+              <key>NSCameraUsageDescription</key><string>Used to display the IR camera feed for tool tracking.</string>
+          </dict>
+          </plist>
+          EOF
+
+          # Compose the DMG. UDZO = zlib-compressed read-only.
+          DMG_NAME="ir-tracking-app-${VERSION}-${{ matrix.label }}.dmg"
+          DMG_STAGING="$RUNNER_TEMP/dmg-staging"
+          rm -rf "$DMG_STAGING"
+          mkdir -p "$DMG_STAGING"
+          cp -R "$APP_DIR" "$DMG_STAGING/"
+          # Drag-to-/Applications affordance — a symlink the user can drop on.
+          ln -s /Applications "$DMG_STAGING/Applications"
+
+          hdiutil create \
+            -volname "$APP_NAME $VERSION" \
+            -srcfolder "$DMG_STAGING" \
+            -ov \
+            -format UDZO \
+            "$DMG_NAME"
+          ls -lh "$DMG_NAME"
+
+      - name: Package (Windows .exe installer)
+        if: runner.os == 'Windows'
+        shell: pwsh
+        env:
+          VERSION: ${{ steps.ver.outputs.version }}
+        run: |
+          $ErrorActionPreference = 'Stop'
+
+          # Stage every file that needs to end up in C:\Program Files\<App>\
+          $stage = "$env:RUNNER_TEMP\nsis-payload"
+          if (Test-Path $stage) { Remove-Item -Recurse -Force $stage }
+          New-Item -ItemType Directory -Force -Path $stage | Out-Null
+
+          $exe = "build\Release\ir-tracking-app.exe"
+          if (-not (Test-Path $exe)) { $exe = "build\ir-tracking-app.exe" }
+          Copy-Item $exe -Destination $stage
+
+          Copy-Item "C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64\realsense2.dll" -Destination $stage
+          $opencvBin = Join-Path $env:OpenCV_DIR "x64\vc16\bin"
+          Get-ChildItem -Path $opencvBin -Filter "opencv_world*.dll" `
+            | Where-Object { $_.Name -notmatch "d\.dll$" } `
+            | Copy-Item -Destination $stage
+
+          # NSIS comes preinstalled on windows-2022 runners under C:\Program Files (x86)\NSIS.
+          $makensis = "C:\Program Files (x86)\NSIS\makensis.exe"
+          if (-not (Test-Path $makensis)) {
+            choco install nsis -y --no-progress
+          }
+          if (-not (Test-Path $makensis)) {
+            throw "NSIS (makensis.exe) not found after install attempt."
+          }
+
+          $version = $env:VERSION
+          $outFile = "ir-tracking-app-$version-${{ matrix.label }}-setup.exe"
+          $nsiPath = "$env:RUNNER_TEMP\installer.nsi"
+          $stageNsi = $stage.Replace('\','\\')
+
+          # NSIS script — install to Program Files\IR Tracking App, Start Menu
+          # shortcut + uninstaller + Add/Remove Programs entry.
+          $nsi = @"
+          !define APPNAME "IR Tracking App"
+          !define APPID "ir-tracking-app"
+          !define VERSION "$version"
+          !define COMPANY "Medivis"
+          !define DESCRIPTION "Intel RealSense IR retro-reflective marker tool tracker"
+          !define REGKEY "Software\Microsoft\Windows\CurrentVersion\Uninstall\`${APPID}"
+
+          Name "`${APPNAME} `${VERSION}"
+          OutFile "$outFile"
+          Unicode true
+          InstallDir "`$PROGRAMFILES64\`${APPNAME}"
+          InstallDirRegKey HKLM "Software\`${APPNAME}" "InstallDir"
+          RequestExecutionLevel admin
+          ShowInstDetails show
+          ShowUninstDetails show
+
+          Page directory
+          Page instfiles
+          UninstPage uninstConfirm
+          UninstPage instfiles
+
+          Section "Install"
+              SetOutPath "`$INSTDIR"
+              File /r "$stageNsi\\*"
+
+              CreateDirectory "`$SMPROGRAMS\`${APPNAME}"
+              CreateShortcut  "`$SMPROGRAMS\`${APPNAME}\`${APPNAME}.lnk" "`$INSTDIR\ir-tracking-app.exe"
+              CreateShortcut  "`$SMPROGRAMS\`${APPNAME}\Uninstall.lnk"     "`$INSTDIR\Uninstall.exe"
+
+              WriteUninstaller "`$INSTDIR\Uninstall.exe"
+
+              WriteRegStr HKLM "Software\`${APPNAME}" "InstallDir" "`$INSTDIR"
+              WriteRegStr HKLM "`${REGKEY}" "DisplayName"     "`${APPNAME}"
+              WriteRegStr HKLM "`${REGKEY}" "DisplayVersion"  "`${VERSION}"
+              WriteRegStr HKLM "`${REGKEY}" "Publisher"       "`${COMPANY}"
+              WriteRegStr HKLM "`${REGKEY}" "DisplayIcon"     '"`$INSTDIR\ir-tracking-app.exe"'
+              WriteRegStr HKLM "`${REGKEY}" "UninstallString" '"`$INSTDIR\Uninstall.exe"'
+              WriteRegDWORD HKLM "`${REGKEY}" "NoModify" 1
+              WriteRegDWORD HKLM "`${REGKEY}" "NoRepair" 1
+          SectionEnd
+
+          Section "Uninstall"
+              Delete "`$INSTDIR\Uninstall.exe"
+              RMDir /r "`$INSTDIR"
+              Delete "`$SMPROGRAMS\`${APPNAME}\`${APPNAME}.lnk"
+              Delete "`$SMPROGRAMS\`${APPNAME}\Uninstall.lnk"
+              RMDir  "`$SMPROGRAMS\`${APPNAME}"
+              DeleteRegKey HKLM "`${REGKEY}"
+              DeleteRegKey HKLM "Software\`${APPNAME}"
+          SectionEnd
+          "@
+
+          Set-Content -Path $nsiPath -Value $nsi -Encoding UTF8
+          & $makensis $nsiPath
+          if ($LASTEXITCODE -ne 0) { throw "makensis failed (exit $LASTEXITCODE)." }
+          Get-Item $outFile | Format-List Length, Name
+
+      # -----------------------------------------------------------------
+      # Upload the installer for the release job to collect.
+      # -----------------------------------------------------------------
+      - name: Upload artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: ir-tracking-app-${{ matrix.label }}
+          path: |
+            *.deb
+            *.dmg
+            *-setup.exe
+          if-no-files-found: error
+          retention-days: 7
+
+  release:
+    name: Publish GitHub Release
+    needs: build
+    runs-on: ubuntu-latest
+    steps:
+      - name: Download all artifacts
+        uses: actions/download-artifact@v4
+        with:
+          path: artifacts
+          merge-multiple: true
+
+      - name: List artifacts
+        run: ls -lh artifacts
+
+      - name: Create GitHub Release
+        uses: softprops/action-gh-release@v2
+        with:
+          files: artifacts/*
+          generate_release_notes: true
+          fail_on_unmatched_files: true
diff --git a/include/IRKalmanFilter.h b/include/IRKalmanFilter.h
index f925a87..253eeb4 100644
--- a/include/IRKalmanFilter.h
+++ b/include/IRKalmanFilter.h
@@ -52,14 +52,20 @@ class IRToolKalmanFilter
 		cv::Mat R = cv::Mat::eye(3, 3, CV_32F) * m_fMeasurementNoise; // measurement noise
 		m_filter.measurementNoiseCov = R;
 
-		// Initialize the state estimate (x) and the error covariance matrix (P)
-		cv::Mat x = cv::Mat::zeros(6, 1, CV_32F); // initial state is all zeros
+		// Seed position from the first measurement so the filter doesn't have
+		// to spend several frames pulling itself from (0,0,0) up to the marker.
+		// Velocity starts at zero — the next correct() will refine it.
+		cv::Mat x = cv::Mat::zeros(6, 1, CV_32F);
+		x.at<float>(0, 0) = value[0];
+		x.at<float>(1, 0) = value[1];
+		x.at<float>(2, 0) = value[2];
+
 		cv::Mat P = cv::Mat::eye(6, 6, CV_32F); // initial error covariance is identity matrix
 		m_filter.statePre = x;
+		m_filter.statePost = x.clone();
 		m_filter.errorCovPost = P;
 
 		m_bInitialized = true;
-		return;
 	}
 
 	cv::Mat measurement = cv::Mat(1, 3, CV_32F);
diff --git a/include/IRStructs.h b/include/IRStructs.h
index 3dec9e4..54abf54 100644
--- a/include/IRStructs.h
+++ b/include/IRStructs.h
@@ -3,6 +3,7 @@
 #include <vector>
 #include <map>
 #include <thread>
+#include <cmath>
 #include <cstdint>
 
 #include <opencv2/core.hpp>
@@ -10,6 +11,13 @@
 
 #include "IRKalmanFilter.h"
 
+// Bucket sphere radii at 0.1 mm so map<float,...> keys are no longer
+// vulnerable to bit-exact float equality when the same nominal value is
+// produced by different expressions (UI input vs. calibration output).
+inline int SphereRadiusKey(float radius_mm) {
+	return static_cast<int>(std::lround(radius_mm * 10.0f));
+}
+
 struct Side
 {
 	int id_from{ 0 };
@@ -30,7 +38,7 @@ struct AHATFrame {
 	double timestamp;
 	cv::Mat device_pose;
 	cv::Mat cvAbImage;
-	uint16_t* pDepth;
+	std::vector<uint16_t> pDepth;
 	uint32_t depthWidth;
 	uint32_t depthHeight;
 };
@@ -41,9 +49,9 @@ struct ProcessedAHATFrame
 	cv::Mat device_pose;
 	uint num_spheres;
 	cv::Mat3f spheres_xyd;
-	std::map<float, cv::Mat3f> spheres_xyz_per_mm;
-	std::map<float, std::vector<Side>> ordered_sides_per_mm;
-	std::map<float, cv::Mat> map_per_mm;
+	std::map<int, cv::Mat3f> spheres_xyz_per_mm;
+	std::map<int, std::vector<Side>> ordered_sides_per_mm;
+	std::map<int, cv::Mat> map_per_mm;
 };
 
 struct ToolResult
@@ -119,6 +127,4 @@ struct IRTrackedTool
 	cv::Vec3f cur_position_cheap{};
 	std::vector<cv::Vec3f> unfiltered_sphere_positions;
 	double timestamp{ 0 };
-
-	bool tracking_finished = true;
 };
\ No newline at end of file
diff --git a/include/IRToolTrack.h b/include/IRToolTrack.h
index a9b98b7..1104736 100644
--- a/include/IRToolTrack.h
+++ b/include/IRToolTrack.h
@@ -2,7 +2,9 @@
 
 #include <vector>
 #include <map>
+#include <memory>
 #include <thread>
+#include <atomic>
 #include <cstdint>
 
 #include <Eigen/Eigen>
@@ -30,6 +32,8 @@ class IRToolTracker
 			m_pRealSenseToolTracking = pRealSenseToolTracking;
 		}
 
+	~IRToolTracker();
+
 
 	void AddFrame(void* pAbImage, void* pDepth, uint32_t depthWidth, uint32_t depthHeight, cv::Mat _pose, double _timestamp);
 	bool AddTool(cv::Mat3f spheres, float sphere_radius, std::string identifier, uint min_visible_spheres, float lowpass_rotation, float lowpass_position);
@@ -43,7 +47,7 @@ class IRToolTracker
 	void SetThreshold(int threshold);
 	void SetMinMaxSize(int min, int max);
 
-	const cv::Mat& GetProcessedFrame();
+	cv::Mat GetProcessedFrame();
 
 
 	void StopTracking();
@@ -57,24 +61,22 @@ class IRToolTracker
 
 private:
 
-	bool ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame& result);
-
-	void TrackTool(IRTrackedTool &tool, ProcessedAHATFrame &frame, ToolResultContainer &result);
+	bool ProcessFrame(AHATFrame& rawFrame, ProcessedAHATFrame& result);
 
-	void UnionSegmentation(ToolResultContainer* raw_solutions, int num_tools, ProcessedAHATFrame frame);
+	void TrackTool(IRTrackedTool &tool, const ProcessedAHATFrame &frame, ToolResultContainer &result);
 
-	cv::Mat MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame frame, std::vector<int> sphere_ids, std::vector<int> occluded_nodes);
+	void UnionSegmentation(std::vector<ToolResultContainer> &raw_solutions, const ProcessedAHATFrame &frame);
 
-	cv::Mat FlipTransformRightLeft(cv::Mat hololens_transform);
+	cv::Mat MatchPointsKabsch(IRTrackedTool &tool, const ProcessedAHATFrame &frame, const std::vector<int> &sphere_ids, const std::vector<int> &occluded_nodes);
 
 	void ConstructMap(cv::Mat3f spheres_xyz, int num_spheres, cv::Mat& result_map, std::vector<Side>& result_ordered_sides);
 
 
-	bool m_bShouldStop = false;
+	std::atomic<bool> m_bShouldStop{false};
 
 	std::vector<IRTrackedTool> m_Tools;
 
-	AHATFrame* m_CurrentFrame = nullptr;
+	std::unique_ptr<AHATFrame> m_CurrentFrame;
 	std::mutex m_MutexCurFrame;
 
 	std::map<std::string, int> m_ToolIndexMapping;
@@ -82,8 +84,8 @@ class IRToolTracker
 	float m_fToleranceSide = 4.0f;
 	float m_fToleranceAvg = 4.0f;
 
-	bool m_bIsCurrentlyTracking = false;
-	bool m_bIsCurrentlyCalibrating = false;
+	std::atomic<bool> m_bIsCurrentlyTracking{false};
+	std::atomic<bool> m_bIsCurrentlyCalibrating{false};
 
 	std::shared_ptr<std::thread> m_TrackingThread;
 	std::shared_ptr<std::thread> m_CalibrationThread;
@@ -91,8 +93,13 @@ class IRToolTracker
 	double m_lTrackedTimestamp = 0;
 
 	std::mutex mtx_frames;
+	// Tracking-thread-only scratch buffer drawn into during each ProcessFrame/MatchPointsKabsch.
+	cv::Mat m_WorkingFrame;
+	// Published copy snapshot to viewers; only touched under mtx_frames.
 	cv::Mat m_ProcessedFrame;
 
+	void PublishWorkingFrame();
+
 	IRToolTracking* m_pRealSenseToolTracking;
 
 	uchar m_Threshold = 100;
diff --git a/include/IRToolTracking.h b/include/IRToolTracking.h
index 6b9aa2f..a94a176 100644
--- a/include/IRToolTracking.h
+++ b/include/IRToolTracking.h
@@ -9,8 +9,6 @@
 #include <opencv2/imgcodecs/imgcodecs.hpp>
 
 #include <mutex>
-#include <queue>
-#include <condition_variable>
 #include <iostream>
 
 #include "IRToolTrack.h"
@@ -30,40 +28,6 @@ inline void JoinThread(std::shared_ptr<std::thread>& th)
 }
 
 
-
-class FrameQueue {
-private:
-    std::queue<cv::Mat> queue;
-    std::mutex mutex;
-    std::condition_variable cond;
-
-public:
-    cv::Mat lastframe;
-    void push(cv::Mat frame) {
-        std::lock_guard<std::mutex> lock(mutex);
-        queue.push(frame);
-        cond.notify_one();
-    }
-
-    cv::Mat pop() {
-        std::unique_lock<std::mutex> lock(mutex);
-        cond.wait(lock, [this]{ return !queue.empty(); });
-        while (queue.size() > 1) {
-            queue.pop();
-        }
-        auto frame = std::move(queue.front());
-        lastframe = frame.clone();
-        queue.pop();
-        return frame;
-    }
-
-    bool empty() {
-        std::lock_guard<std::mutex> lock(mutex);
-        return queue.empty();
-    }
-};
-
-
 class IRToolTracking {
 public:
     IRToolTracking();
diff --git a/include/ROMParser.h b/include/ROMParser.h
index 3d40b0d..9d14af1 100644
--- a/include/ROMParser.h
+++ b/include/ROMParser.h
@@ -4,6 +4,8 @@
 #include <fstream>
 #include <vector>
 #include <cstdint>
+#include <cmath>
+#include <cstring>
 
 class ROMParser {
 public:
@@ -31,8 +33,28 @@ class ROMParser {
     int num_markers = 4;
 
     void parse_rom_data(const std::vector<char>& rom_data) {
-        num_markers = static_cast<unsigned char>(rom_data[28]);
-        int pos = 72;
+        constexpr int kMarkerCountOffset = 28;
+        constexpr int kMarkerDataOffset = 72;
+        constexpr int kMarkerStride = 12;
+
+        if (rom_data.size() <= kMarkerCountOffset) {
+            std::cerr << "ROM file too small to contain marker count." << std::endl;
+            num_markers = 0;
+            return;
+        }
+
+        const int candidate_count = static_cast<unsigned char>(rom_data[kMarkerCountOffset]);
+        const std::size_t required = static_cast<std::size_t>(kMarkerDataOffset) +
+                                     static_cast<std::size_t>(candidate_count) * kMarkerStride;
+        if (rom_data.size() < required) {
+            std::cerr << "ROM file too small for declared marker count (" << candidate_count
+                      << ")." << std::endl;
+            num_markers = 0;
+            return;
+        }
+
+        num_markers = candidate_count;
+        int pos = kMarkerDataOffset;
         for (int i = 0; i < num_markers; ++i) {
             float x, y, z;
             std::memcpy(&x, &rom_data[pos], sizeof(float));
@@ -51,7 +73,7 @@ class ROMParser {
             marker_positions.push_back(y);
             marker_positions.push_back(z);
 
-            pos += 12;
+            pos += kMarkerStride;
         }
     }
 };
\ No newline at end of file
diff --git a/include/ViewerWindow.h b/include/ViewerWindow.h
index f306d7f..43b7440 100644
--- a/include/ViewerWindow.h
+++ b/include/ViewerWindow.h
@@ -6,7 +6,8 @@
 #include <GLFW/glfw3.h>
 #include <vector>
 #include <string>
-#include <nanosockets.h> 
+#include <atomic>
+#include <nanosockets.h>
 #include "IRToolTracking.h"
 
 class ViewerWindow {
@@ -52,13 +53,13 @@ class ViewerWindow {
     std::shared_ptr<std::thread> processingThread;
 
     std::shared_ptr<std::thread> udpThread;
-    bool udpEnabled = false;
+    std::atomic<bool> udpEnabled{false};
 
     std::shared_ptr<std::thread> udpReceiveThread;
-    bool multiEnabled = false;
+    std::atomic<bool> multiEnabled{false};
 
     std::shared_ptr<std::thread> csvThread;
-    bool csvEnabled = false;
+    std::atomic<bool> csvEnabled{false};
 
     std::map<long long, Eigen::Matrix4f> extrinsics;
 
@@ -104,7 +105,7 @@ class ViewerWindow {
     int recordFrequency = 10;
     int duration = 20;
     std::string csvFileName = "tracking_data.csv";
-    bool finishedRecord = false;
+    std::atomic<bool> finishedRecord{false};
 };
 
 #endif // VIEWER_WINDOW_H
diff --git a/src/IRToolTrack.cpp b/src/IRToolTrack.cpp
index 483a13b..3446bae 100644
--- a/src/IRToolTrack.cpp
+++ b/src/IRToolTrack.cpp
@@ -3,8 +3,27 @@
 #include <set>
 
 
-#define DISABLE_LOWPASS FALSE
-#define DISABLE_KALMAN FALSE
+IRToolTracker::~IRToolTracker()
+{
+	m_bShouldStop = true;
+	if (m_TrackingThread && m_TrackingThread->joinable()) {
+		try { m_TrackingThread->join(); } catch (const std::system_error&) {}
+	}
+	if (m_CalibrationThread && m_CalibrationThread->joinable()) {
+		try { m_CalibrationThread->join(); } catch (const std::system_error&) {}
+	}
+	// m_CurrentFrame is a unique_ptr; its destructor frees any pending frame.
+}
+
+
+// Compile-time toggles for the smoothing filters. Override with -D… if you
+// want to disable a filter without editing the source.
+#ifndef DISABLE_LOWPASS
+#define DISABLE_LOWPASS 0
+#endif
+#ifndef DISABLE_KALMAN
+#define DISABLE_KALMAN 0
+#endif
 
 
 
@@ -29,68 +48,49 @@ void IRToolTracker::TrackTools()
 {
 	while (!m_bShouldStop) {
 		m_bIsCurrentlyTracking = true;
-		m_MutexCurFrame.lock();
-		if (m_CurrentFrame == nullptr) {
-			m_MutexCurFrame.unlock();
+		std::unique_ptr<AHATFrame> rawFrame;
+		{
+			std::lock_guard<std::mutex> lock(m_MutexCurFrame);
+			rawFrame = std::move(m_CurrentFrame);
+		}
+		if (!rawFrame) {
 			std::this_thread::sleep_for(std::chrono::milliseconds(5));
 			continue;
 		}
-		
-		//Copy pointer to frame
-		AHATFrame* rawFrame = m_CurrentFrame;
-		m_CurrentFrame = nullptr;
-		m_MutexCurFrame.unlock();
-
-		int current_num_tools = m_Tools.size();
-		ToolResultContainer* raw_results = new ToolResultContainer[current_num_tools];
 
 		ProcessedAHATFrame processedFrame;
-
-		if (!ProcessFrame(rawFrame, processedFrame)) {
+		if (!ProcessFrame(*rawFrame, processedFrame)) {
 			continue;
 		}
-		
-		//std::vector<std::thread> tool_track_threads(current_num_tools);
 
-		for (int i = 0; i < current_num_tools; i++) {
-			IRTrackedTool tool = m_Tools.at(i);
-			if (!tool.tracking_finished)
-				continue;
+		const int current_num_tools = static_cast<int>(m_Tools.size());
+		std::vector<ToolResultContainer> raw_results(current_num_tools);
 
-			ToolResultContainer result{ i, std::vector<ToolResult>() };
-			
-			//tool_track_threads.at(i) = std::thread(&IRToolTracker::TrackTool, this, tool, processedFrame, result);
-			TrackTool(tool, processedFrame, result);
-			raw_results[i] = result;
-			//ProcessEnvFrame(processedFrame, result);
+		for (int i = 0; i < current_num_tools; i++) {
+			raw_results[i].tool_id = i;
+			TrackTool(m_Tools.at(i), processedFrame, raw_results[i]);
 		}
 
-		//for (auto & thread : tool_track_threads) {
-		//	thread.join();
-		//}
-		UnionSegmentation(raw_results, current_num_tools, processedFrame);
-
-		delete[] raw_results;
-		//TODO: make sure i didnt create a memory leak here
+		UnionSegmentation(raw_results, processedFrame);
+		PublishWorkingFrame();
 	}
 	m_bIsCurrentlyTracking = false;
 }
 
-void IRToolTracker::TrackTool(IRTrackedTool &tool, ProcessedAHATFrame &frame, ToolResultContainer &result)
+void IRToolTracker::TrackTool(IRTrackedTool &tool, const ProcessedAHATFrame &frame, ToolResultContainer &result)
 {
-	tool.tracking_finished = false;
 	if (frame.num_spheres < tool.min_visible_spheres) {
 		//Not enough spheres for the tool are available
-		tool.tracking_finished = true;
 		return;
 	}
 	std::vector<Side> eligible_sides;
 	
 
-	auto it_sides = frame.ordered_sides_per_mm.find(tool.sphere_radius);
+	const int radius_key = SphereRadiusKey(tool.sphere_radius);
+	auto it_sides = frame.ordered_sides_per_mm.find(radius_key);
 	std::vector<Side> frame_ordered_sides = it_sides->second;
 
-	auto it_map = frame.map_per_mm.find(tool.sphere_radius);
+	auto it_map = frame.map_per_mm.find(radius_key);
 	cv::Mat frame_map = it_map->second;
 
 	//Find the set of eligible side to start with - aka sides that have similar length to first side of tool
@@ -122,7 +122,6 @@ void IRToolTracker::TrackTool(IRTrackedTool &tool, ProcessedAHATFrame &frame, To
 			}
 			if (eligible_sides.size() == 0 && max_occluded_spheres == 0)
 			{
-				tool.tracking_finished = true;
 				return;
 			}
 			if (eligible_sides.size() != 0)
@@ -136,7 +135,6 @@ void IRToolTracker::TrackTool(IRTrackedTool &tool, ProcessedAHATFrame &frame, To
 		}
 		if (eligible_sides.size() == 0 && max_occluded_spheres == 0)
 		{
-			tool.tracking_finished = true;
 			return;
 		}
 			
@@ -232,96 +230,69 @@ void IRToolTracker::TrackTool(IRTrackedTool &tool, ProcessedAHATFrame &frame, To
 			search_list.push_back(search_entry{ searched_ids_new, curr.combined_error + error_new, curr.num_sides + error_counter, curr.occluded_nodes_tool});
 		}
 	}
-	tool.tracking_finished = true;
-	return;
 }
 
 
 
-void IRToolTracker::UnionSegmentation(ToolResultContainer* raw_solutions, int num_tools, ProcessedAHATFrame frame) {
-	int* tool_solutions = new int[num_tools];
+void IRToolTracker::UnionSegmentation(std::vector<ToolResultContainer> &raw_solutions, const ProcessedAHATFrame &frame) {
 	std::vector<ToolResult> unique_solutions;
+	const int num_tools = static_cast<int>(raw_solutions.size());
 	for (int i = 0; i < num_tools; i++)
 	{
-		tool_solutions[i] = 0;
-		ToolResultContainer tool_results = raw_solutions[i];
-		
-		//std::cout << "Tool " << i << " has " << tool_results.candidates.size() << " candidates" << std::endl;
-		if (tool_results.candidates.size() == 0)
+		std::vector<ToolResult> &candidates = raw_solutions[i].candidates;
+		if (candidates.empty())
 			continue;
 
-		std::vector<ToolResult> ordered_candidates = tool_results.candidates;
-		std::sort(ordered_candidates.begin(), ordered_candidates.end(), &ToolResult::compare);
+		std::sort(candidates.begin(), candidates.end(), &ToolResult::compare);
 
-		for (ToolResult candidate : ordered_candidates)
+		for (ToolResult candidate : candidates)
 		{
 			candidate.tool_id = i;
-			unique_solutions.push_back(candidate);
-			tool_solutions[i]++;
+			unique_solutions.push_back(std::move(candidate));
 		}
 	}
 
 	std::sort(unique_solutions.begin(), unique_solutions.end(), &ToolResult::compare);
 
-	while (unique_solutions.size() > 0)
+	std::vector<bool> claimed_tools(num_tools, false);
+	std::set<int> used_spheres;
+
+	for (const ToolResult &current : unique_solutions)
 	{
-		ToolResult current = unique_solutions.front();
-		int cur_toolid = current.tool_id;
-		unique_solutions.erase(unique_solutions.begin());
-		cv::Mat result = MatchPointsKabsch(m_Tools[cur_toolid], frame, current.sphere_ids, current.occluded_nodes);
+		if (claimed_tools[current.tool_id])
+			continue;
+
+		bool overlap = false;
+		for (int sid : current.sphere_ids) {
+			if (used_spheres.count(sid) > 0) {
+				overlap = true;
+				break;
+			}
+		}
+		if (overlap)
+			continue;
+
+		cv::Mat result = MatchPointsKabsch(m_Tools[current.tool_id], frame, current.sphere_ids, current.occluded_nodes);
 		if (result.at<float>(7, 0) == 1.f)
 		{
-			m_Tools.at(cur_toolid).cur_transform = result.clone();
-			m_Tools.at(cur_toolid).timestamp = frame.timestamp;
+			m_Tools.at(current.tool_id).cur_transform = result.clone();
+			m_Tools.at(current.tool_id).timestamp = frame.timestamp;
 		}
 
-		std::vector<ToolResult> remaining_unique_solutions;
-		for (ToolResult next_check : unique_solutions) {
-			if (next_check.tool_id == cur_toolid)
-				continue;
-
-			bool used = false;
-			for (auto cursphere : current.sphere_ids)
-			{
-				if (used)
-				{
-					break;
-				}
-				for (auto nexsphere : next_check.sphere_ids)
-				{
-					if (cursphere == nexsphere)
-					{
-						used = true;
-						break;
-					}
-
-				}
-			}
-			// std::vector<int> intersection;
-			//std::set_intersection(current.sphere_ids.begin(), current.sphere_ids.end(), next_check.sphere_ids.begin(), next_check.sphere_ids.end(), intersection.begin());
-			//if (intersection.size() > 0)
-			//	continue;
-			if (used)
-			{
-				continue;
-			}
-			remaining_unique_solutions.push_back(next_check);
-		}
-		unique_solutions = remaining_unique_solutions;
+		claimed_tools[current.tool_id] = true;
+		used_spheres.insert(current.sphere_ids.begin(), current.sphere_ids.end());
 	}
-	delete[] tool_solutions;
 	m_lTrackedTimestamp = frame.timestamp;
-	return;
 }
 
-cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame frame, std::vector<int> sphere_ids, std::vector<int> occluded_nodes) {
-	int num_points = tool.num_spheres-occluded_nodes.size();
+cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool &tool, const ProcessedAHATFrame &frame, const std::vector<int> &sphere_ids, const std::vector<int> &occluded_nodes) {
+	int num_points = static_cast<int>(tool.num_spheres) - static_cast<int>(occluded_nodes.size());
 	cv::Mat p = cv::Mat(num_points, 3, CV_32F);
 	cv::Mat q = cv::Mat(num_points, 3, CV_32F);
 	cv::Vec3f p_center = cv::Vec3f(0.f);
 	cv::Vec3f q_center = cv::Vec3f(0.f);
 
-	auto it_spheres_xyz = frame.spheres_xyz_per_mm.find(tool.sphere_radius);
+	auto it_spheres_xyz = frame.spheres_xyz_per_mm.find(SphereRadiusKey(tool.sphere_radius));
 	cv::Mat3f frame_spheres_xyz = it_spheres_xyz->second;
 
 	cv::Mat hololens_pose_mm = frame.device_pose.clone();
@@ -363,13 +334,13 @@ cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame
 
 		m_pRealSenseToolTracking->ProjectPointToPixel(xyz,uv);
     	cv::Point center(uv[0], uv[1]);
-		cv::drawMarker(m_ProcessedFrame, center, cv::Scalar(0, 255, 0), cv::MARKER_CROSS, 20, 2);
+		cv::drawMarker(m_WorkingFrame, center, cv::Scalar(0, 255, 0), cv::MARKER_CROSS, 20, 2);
 
 		cv::Mat sphere_world_mat = hololens_pose_mm * sphere_frame_mat;
 		cv::Vec3f sphere_world = cv::Vec3f(sphere_world_mat.at<float>(0, 0), sphere_world_mat.at<float>(1, 0), sphere_world_mat.at<float>(2, 0));
 
 		//Filter the resulting world position
-#if !DEBUG_NO_FILTER && !DISABLE_KALMAN
+#if !DISABLE_KALMAN
 		sphere_world = tool.sphere_kalman_filters.at(tool_node_id).FilterData(sphere_world);
 #endif
 		tool_node_id++;
@@ -439,8 +410,6 @@ cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame
 	transform_matrix.at<float>(2, 3) = t.at<float>(2, 0) / 1000.f;
 	transform_matrix.at<float>(3, 3) = 1.f;
 
-	// transform_matrix = FlipTransformRightLeft(transform_matrix);
-
 	//Copy translation and convert mm to m
 	cv::Vec3f position;
 	position[0] = transform_matrix.at<float>(0, 3);
@@ -460,7 +429,7 @@ cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame
 
 	Eigen::Quaternionf rotation(quat[3], quat[0], quat[1], quat[2]);
 
-#if !DISABLE_LOWPASS && !DEBUG_NO_FILTER
+#if !DISABLE_LOWPASS
 	{
 		Eigen::Quaternionf rotation_old(tool.cur_transform.at<float>(6, 0), tool.cur_transform.at<float>(3, 0), tool.cur_transform.at<float>(4, 0), tool.cur_transform.at<float>(5, 0));
     	rotation = rotation_old.slerp(tool.lowpass_factor_rotation, rotation);
@@ -489,19 +458,6 @@ cv::Mat IRToolTracker::MatchPointsKabsch(IRTrackedTool tool, ProcessedAHATFrame
 
 }
 
-cv::Mat IRToolTracker::FlipTransformRightLeft(cv::Mat transform_rhs)
-{
-	//Bring to unity coordinate system
-	cv::Mat flipz = cv::Mat::ones(4, 4, CV_32F);
-	flipz.at<float>(2, 0) = -1.f;
-	flipz.at<float>(0, 2) = -1.f;
-	flipz.at<float>(2, 1) = -1.f;
-	flipz.at<float>(1, 2) = -1.f;
-	flipz.at<float>(2, 3) = -1.f;
-	cv::Mat transform_lhs = transform_rhs.mul(flipz);
-	return transform_lhs;
-}
-
 void IRToolTracker::ConstructMap(cv::Mat3f spheres_xyz, int num_spheres, cv::Mat& map, std::vector<Side>& ordered_sides)
 {
 	for (int i = 0; i < num_spheres; i++) {
@@ -533,21 +489,20 @@ void IRToolTracker::ConstructMap(cv::Mat3f spheres_xyz, int num_spheres, cv::Mat
 
 void IRToolTracker::AddFrame(void* pAbImage, void* pDepth, uint32_t depthWidth, uint32_t depthHeight, cv::Mat _pose, double _timestamp) {
 
-	cv::Mat cvAbImage_origin(cv::Size(depthWidth, depthHeight), CV_8UC1, (void*)pAbImage);
+	cv::Mat cvAbImage_origin(cv::Size(depthWidth, depthHeight), CV_8UC1, pAbImage);
 	cv::Mat cvAbImage = cvAbImage_origin.clone();
 
-	m_MutexCurFrame.lock();
-
-	if (m_CurrentFrame != nullptr) {
-		delete[] m_CurrentFrame->pDepth;
-		delete m_CurrentFrame;
-	}
-
-	m_CurrentFrame = new AHATFrame { _timestamp, _pose, cvAbImage,  new uint16_t[depthWidth * depthHeight], depthWidth, depthHeight };
-	memcpy(m_CurrentFrame->pDepth, pDepth, depthWidth * depthHeight * sizeof(uint16_t));
-
-	m_MutexCurFrame.unlock();
-
+	auto frame = std::make_unique<AHATFrame>();
+	frame->timestamp = _timestamp;
+	frame->device_pose = _pose;
+	frame->cvAbImage = std::move(cvAbImage);
+	frame->depthWidth = depthWidth;
+	frame->depthHeight = depthHeight;
+	frame->pDepth.resize(static_cast<std::size_t>(depthWidth) * depthHeight);
+	std::memcpy(frame->pDepth.data(), pDepth, frame->pDepth.size() * sizeof(uint16_t));
+
+	std::lock_guard<std::mutex> lock(m_MutexCurFrame);
+	m_CurrentFrame = std::move(frame);
 }
 
 void IRToolTracker::SetThreshold(int threshold)
@@ -555,10 +510,20 @@ void IRToolTracker::SetThreshold(int threshold)
 	m_Threshold = threshold;
 }
 
-const cv::Mat& IRToolTracker::GetProcessedFrame()
+cv::Mat IRToolTracker::GetProcessedFrame()
 {
 	std::lock_guard<std::mutex> lock(mtx_frames);
-	return m_ProcessedFrame;
+	if (m_ProcessedFrame.empty())
+		return cv::Mat();
+	return m_ProcessedFrame.clone();
+}
+
+void IRToolTracker::PublishWorkingFrame()
+{
+	if (m_WorkingFrame.empty())
+		return;
+	std::lock_guard<std::mutex> lock(mtx_frames);
+	m_WorkingFrame.copyTo(m_ProcessedFrame);
 }
 
 void IRToolTracker::SetMinMaxSize(int min, int max)
@@ -568,24 +533,19 @@ void IRToolTracker::SetMinMaxSize(int min, int max)
 }
 
 
-bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result) {
-	uchar lowerLimit = m_Threshold;
-	uchar upperLimit = lowerLimit + 1;// 256 * 20;
-	int minSize = m_MinSize, maxSize = m_MaxSize;
+bool IRToolTracker::ProcessFrame(AHATFrame &rawFrame, ProcessedAHATFrame &result) {
+	const int minSize = m_MinSize;
+	const int maxSize = m_MaxSize;
 	cv::Mat labels, stats, centroids;
 	std::vector<float> irToolCenters;
 
-	rawFrame->cvAbImage.forEach<uchar>(
-		[&](uchar& ir, const int* position) -> void {
-			ir = (std::clamp(ir, lowerLimit, upperLimit) - lowerLimit) / (upperLimit - lowerLimit)*255;
-		}
-	);
-	
-	rawFrame->cvAbImage.convertTo(rawFrame->cvAbImage, CV_8UC1);
-	cv::cvtColor(rawFrame->cvAbImage.clone(), m_ProcessedFrame, cv::COLOR_GRAY2RGB);
-	
+	// Binary threshold: any IR pixel above m_Threshold becomes 255, everything else 0.
+	// SIMD-vectorized — replaces a per-pixel forEach divide that was the hottest loop.
+	cv::threshold(rawFrame.cvAbImage, rawFrame.cvAbImage, m_Threshold, 255, cv::THRESH_BINARY);
+	cv::cvtColor(rawFrame.cvAbImage, m_WorkingFrame, cv::COLOR_GRAY2RGB);
+
 
-	int areaCount = cv::connectedComponentsWithStats(rawFrame->cvAbImage, labels, stats, centroids, 8);
+	int areaCount = cv::connectedComponentsWithStats(rawFrame.cvAbImage, labels, stats, centroids, 8);
 	for (int i = 1; i < areaCount; ++i)
 	{
 		auto area = stats.at<int32_t>(i, cv::CC_STAT_AREA);
@@ -595,7 +555,7 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 			double _v = centroids.at<double>(i, 1);
 			float uv[2] = { _u + 0.5, _v + 0.5 };
 			float xy[2] = { 0, 0 };
-			float depth = (static_cast<float>(rawFrame->pDepth[rawFrame->depthWidth * (uint16_t)_v + (uint16_t)_u]));
+			float depth = static_cast<float>(rawFrame.pDepth[rawFrame.depthWidth * static_cast<size_t>(_v) + static_cast<size_t>(_u)]);
 			
 			float uvd[3] = { uv[0], uv[1], depth };
 
@@ -620,17 +580,15 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 	int num_spheres = spheres.size().height;
 
 	if (num_spheres < 3) {
-		//If theres less than 3 points visible, theres no tool to track
-		//Free memory
-		delete[] rawFrame->pDepth;
-		delete rawFrame;
+		// If there are fewer than 3 points visible, there is no tool to track.
+		// Caller owns rawFrame via unique_ptr, so no manual cleanup needed.
 		return false;
 	}
 
 	//Create 3d coordinates for every possible sphere size
-	std::map<float, std::vector<Side>> ordered_sides_per_mm;
-	std::map<float, cv::Mat> map_per_mm;
-	std::map<float, cv::Mat3f> spheres_xyz_per_mm;
+	std::map<int, std::vector<Side>> ordered_sides_per_mm;
+	std::map<int, cv::Mat> map_per_mm;
+	std::map<int, cv::Mat3f> spheres_xyz_per_mm;
 
 
 	if (!m_bIsCurrentlyCalibrating)
@@ -638,7 +596,8 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 		for (IRTrackedTool tool : m_Tools)
 		{
 			float cur_radius = tool.sphere_radius;
-			if (!(spheres_xyz_per_mm.find(cur_radius) == spheres_xyz_per_mm.end())) {
+			const int radius_key = SphereRadiusKey(cur_radius);
+			if (spheres_xyz_per_mm.find(radius_key) != spheres_xyz_per_mm.end()) {
 				//We already created this map
 				continue;
 			}
@@ -649,11 +608,8 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 				[&](cv::Vec3f& xyz, const int* position) -> void {
 					float norm = cv::norm(xyz);
 					xyz = xyz / norm * (cur_radius + norm);
-					// xyz[2] = xyz[2] + cur_radius;
-					// cv::Vec3f temp_vec(xyz[0], xyz[1], 1);
-					// xyz = cv::Vec3f((temp_vec / cv::norm(temp_vec)) * xyz[2]);
 				}
-			);	
+			);
 
 
 			//Construct map
@@ -662,15 +618,16 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 
 			ConstructMap(spheres_xyz, num_spheres, map, ordered_sides);
 
-			ordered_sides_per_mm.insert({ cur_radius, ordered_sides });
-			map_per_mm.insert({ cur_radius, map });
-			spheres_xyz_per_mm.insert({ cur_radius, spheres_xyz });
+			ordered_sides_per_mm.insert({ radius_key, ordered_sides });
+			map_per_mm.insert({ radius_key, map });
+			spheres_xyz_per_mm.insert({ radius_key, spheres_xyz });
 
 		}
 	}
 	else
 	{
 		float cur_radius = m_fCalibrationSphereRadius;
+		const int radius_key = SphereRadiusKey(cur_radius);
 		cv::Mat3f spheres_xyz = spheres.clone();
 		spheres_xyz.forEach(
 			[&](cv::Vec3f& xyz, const int* position) -> void {
@@ -689,7 +646,7 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 				// Convert point coordinates to cv::Point
 				cv::Point center(uv[0], uv[1]);
 
-				cv::drawMarker(m_ProcessedFrame, center, cv::Scalar(255, 255, 0), cv::MARKER_CROSS, 20, 2);
+				cv::drawMarker(m_WorkingFrame, center, cv::Scalar(255, 255, 0), cv::MARKER_CROSS, 20, 2);
 			}
 		);
 		
@@ -699,25 +656,20 @@ bool IRToolTracker::ProcessFrame(AHATFrame* rawFrame, ProcessedAHATFrame &result
 
 		ConstructMap(spheres_xyz, 4, map, ordered_sides);
 
-		ordered_sides_per_mm.insert({ cur_radius, ordered_sides });
-		map_per_mm.insert({ cur_radius, map });
-		spheres_xyz_per_mm.insert({ cur_radius, spheres_xyz });
+		ordered_sides_per_mm.insert({ radius_key, ordered_sides });
+		map_per_mm.insert({ radius_key, map });
+		spheres_xyz_per_mm.insert({ radius_key, spheres_xyz });
 	}
 
 
-	result.timestamp = rawFrame->timestamp;
-	result.device_pose = rawFrame->device_pose;
+	result.timestamp = rawFrame.timestamp;
+	result.device_pose = rawFrame.device_pose;
 	result.num_spheres = static_cast<uint>(num_spheres);
 	result.spheres_xyd = spheres.clone();
 	result.spheres_xyz_per_mm = spheres_xyz_per_mm;
 	result.ordered_sides_per_mm = ordered_sides_per_mm;
 	result.map_per_mm = map_per_mm;
 
-
-	//Free memory
-	delete[] rawFrame->pDepth;
-	delete rawFrame;
-
 	return true;
 }
 
@@ -802,6 +754,9 @@ bool IRToolTracker::RemoveTool(std::string identifier)
 
 bool IRToolTracker::RemoveAllTools()
 {
+	if (m_bIsCurrentlyTracking) {
+		StopTracking();
+	}
 	m_Tools.clear();
 	m_ToolIndexMapping.clear();
 	return true;
@@ -875,25 +830,22 @@ void IRToolTracker::CalibrateTool()
 	processedFrames.reserve(MAX_CALIBRATION_FRAMES);
 
 	while (!m_bShouldStop) {
-		m_MutexCurFrame.lock();
-		if (m_CurrentFrame == nullptr) {
-			m_MutexCurFrame.unlock();
+		std::unique_ptr<AHATFrame> rawFrame;
+		{
+			std::lock_guard<std::mutex> lock(m_MutexCurFrame);
+			rawFrame = std::move(m_CurrentFrame);
+		}
+		if (!rawFrame) {
 			std::this_thread::sleep_for(std::chrono::milliseconds(5));
 			continue;
 		}
-		
-		
-		//Copy pointer to frame
-		AHATFrame* rawFrame = m_CurrentFrame;
-		m_CurrentFrame = nullptr;
-		m_MutexCurFrame.unlock();
 
 		ProcessedAHATFrame processedFrame;
-
-		if (!ProcessFrame(rawFrame, processedFrame)) {
+		if (!ProcessFrame(*rawFrame, processedFrame)) {
 			continue;
 		}
 		processedFrames.push_back(processedFrame);
+		PublishWorkingFrame();
 
 		// If enough data is collected, perform calibration
 		if (processedFrames.size() == MAX_CALIBRATION_FRAMES) {
@@ -906,15 +858,16 @@ void IRToolTracker::CalibrateTool()
 	std::vector<std::vector<Eigen::Vector3f>> markerPoints;
 
 	// Define the number of calibration spheres based on size of frame_spheres_xyz in the first frame
-	NUM_CALIBRATION_SPHERES = processedFrames[0].spheres_xyz_per_mm.at(m_fCalibrationSphereRadius).size().height;
+	const int calib_radius_key = SphereRadiusKey(m_fCalibrationSphereRadius);
+	NUM_CALIBRATION_SPHERES = processedFrames[0].spheres_xyz_per_mm.at(calib_radius_key).size().height;
 
 	markerPoints.resize(NUM_CALIBRATION_SPHERES);
 	for (ProcessedAHATFrame frame : processedFrames)
 	{
-		auto it_sides = frame.ordered_sides_per_mm.find(m_fCalibrationSphereRadius);
+		auto it_sides = frame.ordered_sides_per_mm.find(calib_radius_key);
 		std::vector<Side> frame_ordered_sides = it_sides->second;
 
-		auto it_spheres_xyz = frame.spheres_xyz_per_mm.find(m_fCalibrationSphereRadius);
+		auto it_spheres_xyz = frame.spheres_xyz_per_mm.find(calib_radius_key);
 		cv::Mat3f frame_spheres_xyz = it_spheres_xyz->second;
 
 		//Side shortest = frame_ordered_sides.front();
diff --git a/src/IRToolTracking.cpp b/src/IRToolTracking.cpp
index e33b46f..8051108 100644
--- a/src/IRToolTracking.cpp
+++ b/src/IRToolTracking.cpp
@@ -1,6 +1,7 @@
 #include "IRToolTracking.h"
 #include <iostream>
 #include <algorithm>
+#include <cmath>
 #include <librealsense2/rsutil.h>
 
 IRToolTracking::IRToolTracking() {
@@ -104,7 +105,9 @@ void IRToolTracking::setLaserPower(int power)
             depth_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
             // Ensure the power level is within the allowable range
             auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
-            power = std::min(std::max(power, static_cast<int>(range.min)), static_cast<int>(range.max));
+            const int range_min = static_cast<int>(std::lround(range.min));
+            const int range_max = static_cast<int>(std::lround(range.max));
+            power = std::min(std::max(power, range_min), range_max);
 
             // Set the laser power
             depth_sensor.set_option(RS2_OPTION_LASER_POWER, static_cast<float>(power));
@@ -122,11 +125,12 @@ void IRToolTracking::getLaserPower(int &power, int &min, int &max)
         // Check if the device is a depth sensor and supports laser power control
         auto depth_sensor = dev.first<rs2::depth_sensor>();
         if (depth_sensor.supports(RS2_OPTION_LASER_POWER)) {
-            // Get the current laser power
-            power = depth_sensor.get_option(RS2_OPTION_LASER_POWER);
+            // Get the current laser power. RealSense returns a float in mW; the UI
+            // operates on int sliders, so round to the nearest int.
+            power = static_cast<int>(std::lround(depth_sensor.get_option(RS2_OPTION_LASER_POWER)));
             auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
-            min = static_cast<int>(range.min);
-            max = static_cast<int>(range.max);
+            min = static_cast<int>(std::lround(range.min));
+            max = static_cast<int>(std::lround(range.max));
         } else {
             std::cerr << "This RealSense device does not support laser power option." << std::endl;
         }
@@ -140,13 +144,14 @@ void IRToolTracking::processStreams() {
 
     if (Terminated)
         return;
-    // Start the pipeline
+    // Start the pipeline. If it fails, log and bail out so the GUI thread
+    // can join us cleanly instead of having the whole process killed.
     try {
         profile = pipeline.start(config);
     } catch (const rs2::error &e) {
-        std::cerr << "Error occurred during RealSense pipeline start." << std::endl;
-        std::cerr << e.what() << std::endl;
-        exit(EXIT_FAILURE);
+        std::cerr << "Error during RealSense pipeline start: " << e.what() << std::endl;
+        Terminated = true;
+        return;
     }
     // Warm up the device
     for (int i = 0; i < 50; i++) {
@@ -188,21 +193,27 @@ void IRToolTracking::processStreams() {
 
         // Get the timestamp of the current frame
         double timestamp = ir_frame_left.get_timestamp();
-        long long frame_number = ir_frame_left.get_frame_number();
 
         // Convert RealSense frame to OpenCV matrix
         cv::Mat left_frame_image(cv::Size(frame_width, frame_height), CV_8UC1, (void*)ir_frame_left.get_data(), cv::Mat::AUTO_STEP);
         cv::Mat depth_frame_image(cv::Size(frame_width, frame_height), CV_16UC1, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);
         
-        if (m_IRToolTracker != nullptr && (m_IRToolTracker->IsTracking() || m_IRToolTracker->IsCalibrating()) && timestamp > m_latestTrackedFrame)
+        if (m_IRToolTracker != nullptr)
         {
-            // Create a 4x4 identity matrix
-            cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
-            m_IRToolTracker->AddFrame(left_frame_image.data, depth_frame_image.data, left_frame_image.cols, left_frame_image.rows, pose ,timestamp);
-            m_latestTrackedFrame = playFromFile ? -1 : timestamp;
+            if ((m_IRToolTracker->IsTracking() || m_IRToolTracker->IsCalibrating()) && timestamp > m_latestTrackedFrame)
+            {
+                // Create a 4x4 identity matrix
+                cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
+                m_IRToolTracker->AddFrame(left_frame_image.data, depth_frame_image.data, left_frame_image.cols, left_frame_image.rows, pose ,timestamp);
+                m_latestTrackedFrame = playFromFile ? -1 : timestamp;
+            }
+
+            cv::Mat preview = m_IRToolTracker->GetProcessedFrame();
+            {
+                std::lock_guard<std::mutex> lock(mtx_frames);
+                trackingFrame = preview;
+            }
         }
-        
-        trackingFrame = m_IRToolTracker->GetProcessedFrame();
     }
 }
 
diff --git a/src/ViewerWindow.cpp b/src/ViewerWindow.cpp
index bc93635..b0b1f83 100644
--- a/src/ViewerWindow.cpp
+++ b/src/ViewerWindow.cpp
@@ -199,9 +199,13 @@ bool ViewerWindow::Connect(NanoSocket& _socket, NanoAddress& address, const char
     }
 
     _socket = nanosockets_create(1024, 1024);
-    if (socket < 0)
+    if (_socket < 0)
     {
         std::cerr << "Failed to create a socket." << std::endl;
+        if (!multiEnabled && !udpEnabled)
+        {
+            nanosockets_deinitialize();
+        }
         return false;
     }
 
@@ -212,6 +216,11 @@ bool ViewerWindow::Connect(NanoSocket& _socket, NanoAddress& address, const char
         if (nanosockets_address_set_ip(&address, "127.0.0.1"))
         {
             std::cerr<<"Error setting default address"<<std::endl;
+            nanosockets_destroy(&_socket);
+            if (!multiEnabled && !udpEnabled)
+            {
+                nanosockets_deinitialize();
+            }
             return false;
         }
     }
@@ -220,6 +229,11 @@ bool ViewerWindow::Connect(NanoSocket& _socket, NanoAddress& address, const char
         if (nanosockets_address_set_hostname(&address, host))
         {
             std::cerr << "Error setting hostname to " << host << std::endl;
+            nanosockets_destroy(&_socket);
+            if (!multiEnabled && !udpEnabled)
+            {
+                nanosockets_deinitialize();
+            }
             return false;
         }
     }
@@ -268,11 +282,16 @@ void ViewerWindow::UdpReceiveThreadFunction()
     }
 
 
+    std::vector<uint8_t> buffer(sizeof(TrackingData));
     while (multiEnabled)
     {
+        // Wait up to 50 ms for a packet so the loop doesn't burn a core when idle.
+        const int ready = nanosockets_poll(receiveSocket, 50);
+        if (ready <= 0) {
+            continue;
+        }
+
         NanoAddress sender;
-		std::vector<uint8_t> buffer(sizeof(TrackingData));
-        //toolTransforms.clear();
         if (nanosockets_receive(receiveSocket, &sender, buffer.data(), buffer.size()) > 0)
         {
 			TrackingData data;
@@ -385,7 +404,7 @@ void ViewerWindow::UdpThreadFunction()
             }
         }
 
-        int sleepDurationMs = 1000 / frequency; // Convert frequency to sleep duration in milliseconds
+        int sleepDurationMs = 1000 / std::max(frequency, 1);
         std::this_thread::sleep_for(std::chrono::milliseconds(sleepDurationMs));
     }
 
@@ -459,7 +478,7 @@ void ViewerWindow::WriteToCSV()
             }
         }
 
-        int sleepDurationMs = 1000 / recordFrequency; 
+        int sleepDurationMs = 1000 / std::max(recordFrequency, 1);
         std::this_thread::sleep_for(std::chrono::milliseconds(sleepDurationMs));
 	}
 
@@ -560,6 +579,18 @@ void ViewerWindow::Render() {
 
         // Adjust the size of the tools vector based on numTools
         numTools = std::max(numTools, 1);
+        if (static_cast<int>(tools.size()) > numTools)
+        {
+            // Unregister any tools that are about to be dropped so the tracker
+            // doesn't keep matching ghosts.
+            for (int i = numTools; i < static_cast<int>(tools.size()); ++i)
+            {
+                if (tools[i].isAdded)
+                {
+                    tracker.RemoveToolDefinition(tools[i].toolName);
+                }
+            }
+        }
         if (static_cast<int>(tools.size()) != numTools)
         {
             tools.resize(numTools);
@@ -569,6 +600,7 @@ void ViewerWindow::Render() {
         ImGui::SetNextWindowSize(ImVec2(windowWidth, 0.0f));
         ImGui::Begin("Tool Definitions", nullptr, overlayFlags);
 
+        isToolAdded = false;
         for (int toolIdx = 0; toolIdx < numTools; ++toolIdx)
         {
             tools[toolIdx].toolName = tools[toolIdx].toolName == "Tool" ? "Tool" + std::to_string(toolIdx + 1) : tools[toolIdx].toolName;
@@ -746,15 +778,19 @@ void ViewerWindow::Render() {
         ImGui::SetNextItemWidth(110);
         ImGui::InputInt("Frequency", &frequency);
         ImGui::SameLine();
-        if (ImGui::Checkbox("UDP", &udpEnabled))
         {
-            if (udpEnabled)
-            {
-                udpThread = std::make_shared<std::thread>(&ViewerWindow::UdpThreadFunction, this);
-            }
-            else
+            bool udpEnabledTmp = udpEnabled.load();
+            if (ImGui::Checkbox("UDP", &udpEnabledTmp))
             {
-                JoinThread(udpThread);
+                udpEnabled.store(udpEnabledTmp);
+                if (udpEnabledTmp)
+                {
+                    udpThread = std::make_shared<std::thread>(&ViewerWindow::UdpThreadFunction, this);
+                }
+                else
+                {
+                    JoinThread(udpThread);
+                }
             }
         }
         ImGui::End();
@@ -763,15 +799,19 @@ void ViewerWindow::Render() {
         ImGui::SetNextWindowSize(ImVec2(300, 0.0f));
         ImGui::SetNextWindowPos(ImVec2(740, 80), ImGuiCond_FirstUseEver);
         ImGui::Begin("Multi-Camera Settings", nullptr, overlayFlags);
-        if (ImGui::Checkbox("Multi-Camera", &multiEnabled))
         {
-            if (multiEnabled)
+            bool multiEnabledTmp = multiEnabled.load();
+            if (ImGui::Checkbox("Multi-Camera", &multiEnabledTmp))
             {
-                udpReceiveThread = std::make_shared<std::thread>(&ViewerWindow::UdpReceiveThreadFunction, this);
-            }
-            else
-            {
-                JoinThread(udpReceiveThread);
+                multiEnabled.store(multiEnabledTmp);
+                if (multiEnabledTmp)
+                {
+                    udpReceiveThread = std::make_shared<std::thread>(&ViewerWindow::UdpReceiveThreadFunction, this);
+                }
+                else
+                {
+                    JoinThread(udpReceiveThread);
+                }
             }
         }
         ImGui::SameLine();
@@ -802,17 +842,21 @@ void ViewerWindow::Render() {
             NFD_Quit();
         }
         ImGui::SameLine();
-        if (ImGui::Checkbox("Record", &csvEnabled))
         {
-            if (csvEnabled)
-            {
-				csvThread = std::make_shared<std::thread>(&ViewerWindow::WriteToCSV, this);
-			}
-            else
+            bool csvEnabledTmp = csvEnabled.load();
+            if (ImGui::Checkbox("Record", &csvEnabledTmp))
             {
-				JoinThread(csvThread);
-			}
-		}
+                csvEnabled.store(csvEnabledTmp);
+                if (csvEnabledTmp)
+                {
+                    csvThread = std::make_shared<std::thread>(&ViewerWindow::WriteToCSV, this);
+                }
+                else
+                {
+                    JoinThread(csvThread);
+                }
+            }
+        }
         ImGui::End();
         recordFrequency = std::min(std::max(recordFrequency, 1), 90);
         duration = std::max(duration, 1);
@@ -933,11 +977,17 @@ void ViewerWindow::Shutdown() {
     Terminated = true;
     udpEnabled = false;
     multiEnabled = false;
+    csvEnabled = false;
+
+    const bool pipelineRunning = tracker.IsTrackingTools() || tracker.IsCalibratingTool();
+
     tracker.StopToolTracking();
+    tracker.StopToolCalibration();
     JoinThread(processingThread);
     JoinThread(udpThread);
     JoinThread(udpReceiveThread);
     JoinThread(csvThread);
-    if (tracker.IsTrackingTools() || tracker.IsCalibratingTool())
+
+    if (pipelineRunning)
         tracker.shutdown();
 }
\ No newline at end of file