K4AViewer: Add IMU playback (#234)

Add IMU playback support to K4AViewer.

To support this, move graph data aggregation into the listener. This lets us do things like clear the graph data when a seek happens without having direct access to the IMU window.

src/record/internal/matroska_read.cpp

@@ -1890,6 +1890,7 @@ k4a_stream_result_t get_imu_sample(k4a_playback_context_t *context, k4a_imu_samp
             {
                 imu_sample->acc_timestamp_usec = sample->acc_timestamp_ns / 1000;
                 imu_sample->gyro_timestamp_usec = sample->gyro_timestamp_ns / 1000;
+                imu_sample->temperature = std::numeric_limits<float>::quiet_NaN();
                 for (size_t i = 0; i < 3; i++)
                 {
                     imu_sample->acc_sample.v[i] = sample->acc_data[i];

tools/k4aviewer/CMakeLists.txt

@@ -11,8 +11,8 @@ set(SOURCE_FILES
     k4adevicedockcontrol.cpp
     k4afilepicker.cpp
     k4aimguiextensions.cpp
-    k4aimudatagraph.cpp
-    k4aimusamplesource.cpp
+    k4aimugraph.cpp
+    k4aimugraphdatagenerator.cpp
     k4aimuwindow.cpp
     k4alogdockcontrol.cpp
     k4amicrophone.cpp

tools/k4aviewer/ik4aobserver.h

@@ -22,6 +22,7 @@ template<typename NotificationType> class IK4AObserver
 public:
     virtual void NotifyData(const NotificationType &data) = 0;
     virtual void NotifyTermination() = 0;
+    virtual void ClearData() = 0;
 
     virtual ~IK4AObserver() = default;
 

tools/k4aviewer/k4aconvertingimagesource.h

@@ -79,6 +79,14 @@ public:
         m_failed = true;
     }
 
+    void ClearData() override
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+
+        m_textureBuffers.Clear();
+        m_inputImageBuffer.Clear();
+    }
+
     ~K4AConvertingImageSourceImpl() override
     {
         m_workerThreadShouldExit = true;

tools/k4aviewer/k4adatasource.h

@@ -84,6 +84,28 @@ public:
         m_observers.clear();
     }
 
+    void ClearData()
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        m_primed = false;
+        m_mostRecentData = T();
+        for (auto wpObserver = m_observers.begin(); wpObserver != m_observers.end();)
+        {
+            auto spObserver = wpObserver->lock();
+            if (spObserver)
+            {
+                spObserver->ClearData();
+                ++wpObserver;
+            }
+            else
+            {
+                auto toDelete = wpObserver;
+                ++wpObserver;
+                m_observers.erase(toDelete);
+            }
+        }
+    }
+
 private:
     std::list<std::weak_ptr<IK4AObserver<T>>> m_observers;
 

tools/k4aviewer/k4aimudatagraph.h

@@ -1,74 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef K4AIMUDATAGRAPH_H
-#define K4AIMUDATAGRAPH_H
-
-// System headers
-//
-#include <array>
-#include <string>
-
-// Library headers
-//
-#include <k4a/k4a.hpp>
-#include "k4aimgui_all.h"
-
-// Project headers
-//
-
-namespace k4aviewer
-{
-class K4AImuDataGraph
-{
-public:
-    K4AImuDataGraph(std::string &&title,
-                    std::string &&xLabel,
-                    std::string &&yLabel,
-                    std::string &&zLabel,
-                    std::string &&units,
-                    float minRange,
-                    float maxRange,
-                    float defaultRange,
-                    float scaleFactor);
-
-    void AddSample(const k4a_float3_t &sample, uint64_t timestampUs);
-    void Show(ImVec2 maxSize);
-
-private:
-    // Number of samples to show in the graphs
-    //
-    static constexpr int GraphSampleCount = 150;
-
-    // Number of data samples we average together to compute a graph sample
-    //
-    static constexpr int DataSamplesPerGraphSample = 20;
-
-    void PlotGraph(const char *name, const std::array<float, GraphSampleCount> &data, ImVec2 graphSize);
-
-    const std::string m_title;
-    const std::string m_xLabel;
-    const std::string m_yLabel;
-    const std::string m_zLabel;
-    const std::string m_units;
-
-    const float m_minRange;
-    const float m_maxRange;
-    float m_currentRange;
-
-    const float m_scaleFactor;
-
-    uint64_t m_lastTimestamp = 0;
-    size_t m_offset = 0;
-    std::array<float, GraphSampleCount> m_x = {};
-    std::array<float, GraphSampleCount> m_y = {};
-    std::array<float, GraphSampleCount> m_z = {};
-
-    k4a_float3_t m_nextSampleAccumulator{ { 0.f, 0.f, 0.f } };
-    int m_nextSampleAccumulatorCount = 0;
-
-    const std::string m_scaleTitle;
-};
-} // namespace k4aviewer
-
-#endif

tools/k4aviewer/k4aimugraph.cpp

@@ -3,7 +3,7 @@
 
 // Associated header
 //
-#include "k4aimudatagraph.h"
+#include "k4aimugraph.h"
 
 // System headers
 //
@@ -41,15 +41,14 @@ constexpr float MinHeight = 50.f;
 
 } // namespace
 
-K4AImuDataGraph::K4AImuDataGraph(std::string &&title,
-                                 std::string &&xLabel,
-                                 std::string &&yLabel,
-                                 std::string &&zLabel,
-                                 std::string &&units,
-                                 const float minRange,
-                                 const float maxRange,
-                                 const float defaultRange,
-                                 const float scaleFactor) :
+K4AImuGraph::K4AImuGraph(std::string &&title,
+                         std::string &&xLabel,
+                         std::string &&yLabel,
+                         std::string &&zLabel,
+                         std::string &&units,
+                         const float minRange,
+                         const float maxRange,
+                         const float defaultRange) :
     m_title(std::move(title)),
     m_xLabel(std::move(xLabel)),
     m_yLabel(std::move(yLabel)),
@@ -58,38 +57,14 @@ K4AImuDataGraph::K4AImuDataGraph(std::string &&title,
     m_minRange(minRange),
     m_maxRange(maxRange),
     m_currentRange(-defaultRange),
-    m_scaleFactor(scaleFactor),
     m_scaleTitle(GetScaleTitle(m_title))
 {
 }
 
-void K4AImuDataGraph::AddSample(const k4a_float3_t &sample, const uint64_t timestampUs)
-{
-    // We want to average a few samples together to slow down the graph enough that it's readable
-    //
-    m_nextSampleAccumulator.xyz.x += sample.xyz.x;
-    m_nextSampleAccumulator.xyz.y += sample.xyz.y;
-    m_nextSampleAccumulator.xyz.z += sample.xyz.z;
-    ++m_nextSampleAccumulatorCount;
-
-    if (m_nextSampleAccumulatorCount >= DataSamplesPerGraphSample)
-    {
-        m_offset = (m_offset + 1) % m_x.size();
-
-        m_x[m_offset] = m_nextSampleAccumulator.xyz.x / m_nextSampleAccumulatorCount * m_scaleFactor;
-        m_y[m_offset] = m_nextSampleAccumulator.xyz.y / m_nextSampleAccumulatorCount * m_scaleFactor;
-        m_z[m_offset] = m_nextSampleAccumulator.xyz.z / m_nextSampleAccumulatorCount * m_scaleFactor;
-
-        m_lastTimestamp = timestampUs;
-
-        m_nextSampleAccumulator.xyz.x = 0.f;
-        m_nextSampleAccumulator.xyz.y = 0.f;
-        m_nextSampleAccumulator.xyz.z = 0.f;
-        m_nextSampleAccumulatorCount = 0;
-    }
-}
-
-void K4AImuDataGraph::Show(ImVec2 maxSize)
+void K4AImuGraph::Show(ImVec2 maxSize,
+                       const K4AImuGraphData::AccumulatorArray &graphData,
+                       int graphFrontIdx,
+                       uint64_t timestamp)
 {
     // One line for the graph type (accelerometer/gyro), one for the timestamp
     //
@@ -116,7 +91,7 @@ void K4AImuDataGraph::Show(ImVec2 maxSize)
 
     ImGui::BeginGroup();
     ImGui::Text("%s", m_title.c_str());
-    ImGui::Text("Time (us): %llu", static_cast<long long unsigned int>(m_lastTimestamp));
+    ImGui::Text("Time (us): %llu", static_cast<long long unsigned int>(timestamp));
 
     // We use negative min/max ranges to reverse the direction of the slider, which makes it
     // grow when you drag up, which is a bit more intuitive.
@@ -130,19 +105,23 @@ void K4AImuDataGraph::Show(ImVec2 maxSize)
     ImGui::SameLine();
 
     ImGui::BeginGroup();
-    PlotGraph(m_xLabel.c_str(), m_x, graphSize);
-    PlotGraph(m_yLabel.c_str(), m_y, graphSize);
-    PlotGraph(m_zLabel.c_str(), m_z, graphSize);
+    PlotGraph(m_xLabel.c_str(), graphSize, graphData, graphFrontIdx, 0);
+    PlotGraph(m_yLabel.c_str(), graphSize, graphData, graphFrontIdx, 1);
+    PlotGraph(m_zLabel.c_str(), graphSize, graphData, graphFrontIdx, 2);
     ImGui::EndGroup();
     ImGui::EndGroup();
 }
 
-void K4AImuDataGraph::PlotGraph(const char *name, const std::array<float, GraphSampleCount> &data, ImVec2 graphSize)
+void K4AImuGraph::PlotGraph(const char *name,
+                            ImVec2 graphSize,
+                            const K4AImuGraphData::AccumulatorArray &graphData,
+                            int graphFrontIdx,
+                            int offset)
 {
     std::stringstream nameBuilder;
     nameBuilder << "##" << name;
 
-    const float currentData = data[m_offset];
+    const float currentData = graphData[static_cast<size_t>(graphFrontIdx)].v[offset];
 
     std::string label;
     if (K4AViewerSettingsManager::Instance().GetShowInfoPane())
@@ -175,10 +154,23 @@ void K4AImuDataGraph::PlotGraph(const char *name, const std::array<float, GraphS
         label = labelBuilder.str();
     }
 
+    struct SelectorData
+    {
+        const k4a_float3_t *data;
+        int offset;
+    } selectorData;
+
+    selectorData.data = &graphData[0];
+    selectorData.offset = offset;
+
     ImGui::PlotLines(nameBuilder.str().c_str(),
-                     &data[0],
-                     static_cast<int>(data.size()),
-                     static_cast<int>(m_offset),
+                     [](void *data, int idx) {
+                         auto *sd = reinterpret_cast<SelectorData *>(data);
+                         return sd->data[idx].v[sd->offset];
+                     },
+                     &selectorData,
+                     static_cast<int>(graphData.size()),
+                     graphFrontIdx,
                      label.c_str(),
                      m_currentRange,
                      -m_currentRange,

tools/k4aviewer/k4aimugraph.h

@@ -0,0 +1,60 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef K4AIMUGRAPH_H
+#define K4AIMUGRAPH_H
+
+// System headers
+//
+#include <array>
+#include <string>
+
+// Library headers
+//
+#include <k4a/k4a.hpp>
+#include "k4aimgui_all.h"
+
+// Project headers
+//
+#include "k4aimugraphdatagenerator.h"
+
+namespace k4aviewer
+{
+
+class K4AImuGraph
+{
+public:
+    K4AImuGraph(std::string &&title,
+                std::string &&xLabel,
+                std::string &&yLabel,
+                std::string &&zLabel,
+                std::string &&units,
+                float minRange,
+                float maxRange,
+                float defaultRange);
+
+    void
+    Show(ImVec2 maxSize, const K4AImuGraphData::AccumulatorArray &graphData, int graphFrontIdx, uint64_t timestamp);
+
+private:
+    void PlotGraph(const char *name,
+                   ImVec2 graphSize,
+                   const K4AImuGraphData::AccumulatorArray &graphData,
+                   int graphFrontIdx,
+                   int offset);
+
+    const std::string m_title;
+    const std::string m_xLabel;
+    const std::string m_yLabel;
+    const std::string m_zLabel;
+    const std::string m_units;
+
+    const float m_minRange;
+    const float m_maxRange;
+    float m_currentRange;
+
+    const std::string m_scaleTitle;
+};
+} // namespace k4aviewer
+
+#endif

tools/k4aviewer/k4aimugraphdatagenerator.cpp

@@ -0,0 +1,83 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+// Associated header
+//
+#include "k4aimugraphdatagenerator.h"
+
+// System headers
+//
+
+// Library headers
+//
+
+// Project headers
+//
+
+using namespace k4aviewer;
+
+void K4AImuGraphDataGenerator::NotifyData(const k4a_imu_sample_t &sample)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    for (int i = 0; i < 3; ++i)
+    {
+        m_accAccumulator.v[i] += sample.acc_sample.v[i];
+        m_gyroAccumulator.v[i] += sample.gyro_sample.v[i];
+    }
+
+    ++m_accumulatorCount;
+
+    // 'Commit' the samples we've accumulated to the graph
+    //
+    if (m_accumulatorCount >= SamplesPerAggregateSample)
+    {
+        size_t insertOffset = static_cast<size_t>(m_graphData.StartOffset);
+        m_graphData.StartOffset = (m_graphData.StartOffset + 1) % K4AImuGraphData::GraphSampleCount;
+
+        for (int i = 0; i < 3; ++i)
+        {
+            m_graphData.AccData[insertOffset].v[i] = m_accAccumulator.v[i] / SamplesPerAggregateSample;
+            m_graphData.GyroData[insertOffset].v[i] = m_gyroAccumulator.v[i] / SamplesPerAggregateSample;
+        }
+
+        m_graphData.AccTimestamp = sample.acc_timestamp_usec;
+        m_graphData.GyroTimestamp = sample.gyro_timestamp_usec;
+        m_graphData.LastTemperature = sample.temperature;
+
+        ResetAccumulators();
+    }
+}
+
+void K4AImuGraphDataGenerator::NotifyTermination()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    m_failed = true;
+}
+
+void K4AImuGraphDataGenerator::ClearData()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    ResetAccumulators();
+    std::fill(m_graphData.AccData.begin(), m_graphData.AccData.end(), k4a_float3_t{ { 0.f, 0.f, 0.f } });
+    std::fill(m_graphData.GyroData.begin(), m_graphData.GyroData.end(), k4a_float3_t{ { 0.f, 0.f, 0.f } });
+
+    m_graphData.AccTimestamp = 0;
+    m_graphData.GyroTimestamp = 0;
+
+    m_graphData.StartOffset = 0;
+    m_graphData.LastTemperature = std::numeric_limits<float>::quiet_NaN();
+}
+
+K4AImuGraphDataGenerator::K4AImuGraphDataGenerator()
+{
+    ClearData();
+    ResetAccumulators();
+}
+
+void K4AImuGraphDataGenerator::ResetAccumulators()
+{
+    m_gyroAccumulator = { { 0.f, 0.f, 0.f } };
+    m_accAccumulator = { { 0.f, 0.f, 0.f } };
+    m_accumulatorCount = 0;
+}

tools/k4aviewer/k4aimugraphdatagenerator.h

@@ -0,0 +1,89 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef K4AIMUGRAPHDATAGENERATOR_H
+#define K4AIMUGRAPHDATAGENERATOR_H
+
+// System headers
+//
+#include <mutex>
+
+// Library headers
+//
+#include <k4a/k4a.hpp>
+
+// Project headers
+//
+#include "ik4aobserver.h"
+#include "k4aringbuffer.h"
+
+namespace k4aviewer
+{
+
+struct K4AImuGraphData
+{
+public:
+    static constexpr int GraphSampleCount = 150;
+
+    using AccumulatorArray = std::array<k4a_float3_t, GraphSampleCount>;
+
+    AccumulatorArray AccData;
+    AccumulatorArray GyroData;
+
+    uint64_t AccTimestamp;
+    uint64_t GyroTimestamp;
+
+    float LastTemperature;
+
+    int StartOffset;
+};
+
+class K4AImuGraphDataGenerator : public IK4AImuObserver
+{
+public:
+    void NotifyData(const k4a_imu_sample_t &sample) override;
+    void NotifyTermination() override;
+    void ClearData() override;
+
+    struct GraphReader
+    {
+        std::unique_lock<std::mutex> Lock;
+        const K4AImuGraphData *Data;
+    };
+
+    // Returns a pointer-to-graph-data and a lock that guarantees
+    // that the graph data won't be modified.  You must release the lock
+    // in a timely fashion or the graph generator will hang.
+    //
+    GraphReader GetGraphData() const
+    {
+        return GraphReader{ std::unique_lock<std::mutex>(m_mutex), &m_graphData };
+    }
+
+    bool IsFailed() const
+    {
+        return m_failed;
+    }
+
+    K4AImuGraphDataGenerator();
+    ~K4AImuGraphDataGenerator() override = default;
+
+    static constexpr int SamplesPerAggregateSample = 20;
+    static constexpr int SamplesPerGraph = SamplesPerAggregateSample * K4AImuGraphData::GraphSampleCount;
+
+private:
+    void ResetAccumulators();
+
+    K4AImuGraphData m_graphData;
+
+    bool m_failed = false;
+
+    k4a_float3_t m_gyroAccumulator;
+    k4a_float3_t m_accAccumulator;
+    int m_accumulatorCount = 0;
+
+    mutable std::mutex m_mutex;
+};
+} // namespace k4aviewer
+
+#endif

tools/k4aviewer/k4aimusamplesource.cpp

@@ -1,48 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-// Associated header
-//
-#include "k4aimusamplesource.h"
-
-// System headers
-//
-
-// Library headers
-//
-
-// Project headers
-//
-
-using namespace k4aviewer;
-
-void K4AImuSampleSource::NotifyData(const k4a_imu_sample_t &data)
-{
-    if (!m_sampleBuffer.BeginInsert())
-    {
-        // Buffer overflowed; drop the sample.
-        //
-        return;
-    }
-
-    *m_sampleBuffer.InsertionItem() = data;
-
-    m_sampleBuffer.EndInsert();
-}
-
-void K4AImuSampleSource::NotifyTermination()
-{
-    std::lock_guard<std::mutex> lock(m_mutex);
-    m_failed = true;
-}
-
-bool K4AImuSampleSource::PopSample(k4a_imu_sample_t *out)
-{
-    if (!m_sampleBuffer.AdvanceRead())
-    {
-        return false;
-    }
-
-    *out = *m_sampleBuffer.CurrentItem();
-    return true;
-}

tools/k4aviewer/k4aimusamplesource.h

@@ -1,45 +0,0 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
-
-#ifndef K4AIMUSAMPLESOURCE_H
-#define K4AIMUSAMPLESOURCE_H
-
-// System headers
-//
-#include <mutex>
-
-// Library headers
-//
-#include <k4a/k4a.hpp>
-
-// Project headers
-//
-#include "ik4aobserver.h"
-#include "k4aringbuffer.h"
-
-namespace k4aviewer
-{
-class K4AImuSampleSource : public IK4AImuObserver
-{
-public:
-    void NotifyData(const k4a_imu_sample_t &data) override;
-    void NotifyTermination() override;
-
-    bool PopSample(k4a_imu_sample_t *out);
-
-    bool IsFailed() const
-    {
-        return m_failed;
-    }
-
-    ~K4AImuSampleSource() override = default;
-
-private:
-    K4ARingBuffer<k4a_imu_sample_t, 1000> m_sampleBuffer;
-    bool m_failed = false;
-
-    mutable std::mutex m_mutex;
-};
-} // namespace k4aviewer
-
-#endif

tools/k4aviewer/k4aimuwindow.cpp

@@ -7,6 +7,7 @@
 
 // System headers
 //
+#include <cmath>
 #include <utility>
 
 // Library headers
@@ -15,7 +16,6 @@
 
 // Project headers
 //
-#include "k4aimudatagraph.h"
 #include "k4aviewererrormanager.h"
 #include "k4awindowsizehelpers.h"
 
@@ -26,59 +26,35 @@ namespace
 constexpr float AccelMinRange = 5.0f;
 constexpr float AccelMaxRange = 100.0f;
 constexpr float AccelDefaultRange = 20.0f;
-constexpr float AccelScaleFactor = 1.0f;
 
 constexpr float GyroMinRange = 5.0f;
 constexpr float GyroMaxRange = 50.0f;
 constexpr float GyroDefaultRange = 20.0f;
-constexpr float GyroScaleFactor = 1.0f;
 } // namespace
 
-K4AImuWindow::K4AImuWindow(std::string &&title, std::shared_ptr<K4AImuSampleSource> sampleSource) :
-    m_sampleSource(std::move(sampleSource)),
+K4AImuWindow::K4AImuWindow(std::string &&title, std::shared_ptr<K4AImuGraphDataGenerator> graphDataGenerator) :
+    m_graphDataGenerator(std::move(graphDataGenerator)),
     m_title(std::move(title)),
-    m_accelerometerGraph("Accelerometer",
-                         "X",
-                         "Y",
-                         "Z",
-                         "m/s/s",
-                         AccelMinRange,
-                         AccelMaxRange,
-                         AccelDefaultRange,
-                         AccelScaleFactor),
-    m_gyroscopeGraph("Gyroscope",
-                     " Roll",
-                     "Pitch",
-                     "  Yaw",
-                     "Rad/s",
-                     GyroMinRange,
-                     GyroMaxRange,
-                     GyroDefaultRange,
-                     GyroScaleFactor)
+    m_accGraph("Accelerometer", "X", "Y", "Z", "m/s/s", AccelMinRange, AccelMaxRange, AccelDefaultRange),
+    m_gyroGraph("Gyroscope", " Roll", "Pitch", "  Yaw", "Rad/s", GyroMinRange, GyroMaxRange, GyroDefaultRange)
 {
 }
 
 void K4AImuWindow::Show(K4AWindowPlacementInfo placementInfo)
 {
-    if (!m_failed && m_sampleSource->IsFailed())
+    if (!m_failed && m_graphDataGenerator->IsFailed())
     {
-        K4AViewerErrorManager::Instance().SetErrorStatus(m_title + ": sample source failed!");
+        K4AViewerErrorManager::Instance().SetErrorStatus(m_title + ": data source failed!");
         m_failed = true;
     }
 
     if (m_failed)
     {
-        ImGui::Text("Sample source failed!");
+        ImGui::Text("Data source failed!");
         return;
     }
 
-    k4a_imu_sample_t sample;
-    while (m_sampleSource->PopSample(&sample))
-    {
-        m_accelerometerGraph.AddSample(sample.acc_sample, sample.acc_timestamp_usec);
-        m_gyroscopeGraph.AddSample(sample.gyro_sample, sample.gyro_timestamp_usec);
-        m_sensorTemperature = static_cast<double>(sample.temperature);
-    }
+    K4AImuGraphDataGenerator::GraphReader reader = m_graphDataGenerator->GetGraphData();
 
     // Sizing math
     //
@@ -100,14 +76,18 @@ void K4AImuWindow::Show(K4AWindowPlacementInfo placementInfo)
 
     // Actually draw the widgets
     //
-    m_accelerometerGraph.Show(graphSize);
+    m_accGraph.Show(graphSize, reader.Data->AccData, reader.Data->StartOffset, reader.Data->AccTimestamp);
 
     ImGui::Separator();
 
-    m_gyroscopeGraph.Show(graphSize);
+    m_gyroGraph.Show(graphSize, reader.Data->GyroData, reader.Data->StartOffset, reader.Data->GyroTimestamp);
 
     ImGui::Separator();
-    ImGui::Text("Sensor temperature: %.2f C", m_sensorTemperature);
+
+    if (!std::isnan(reader.Data->LastTemperature))
+    {
+        ImGui::Text("Sensor temperature: %.2f C", static_cast<double>(reader.Data->LastTemperature));
+    }
 }
 
 const char *K4AImuWindow::GetTitle() const

tools/k4aviewer/k4aimuwindow.h

@@ -14,9 +14,9 @@
 
 // Project headers
 //
-#include "k4aimusamplesource.h"
 #include "ik4avisualizationwindow.h"
-#include "k4aimudatagraph.h"
+#include "k4aimugraphdatagenerator.h"
+#include "k4aimugraph.h"
 
 namespace k4aviewer
 {
@@ -27,7 +27,7 @@ public:
 
     const char *GetTitle() const override;
 
-    K4AImuWindow(std::string &&title, std::shared_ptr<K4AImuSampleSource> sampleSource);
+    K4AImuWindow(std::string &&title, std::shared_ptr<K4AImuGraphDataGenerator> graphDataGenerator);
     ~K4AImuWindow() override = default;
 
     K4AImuWindow(const K4AImuWindow &) = delete;
@@ -36,13 +36,13 @@ public:
     K4AImuWindow &operator=(const K4AImuWindow &&) = delete;
 
 private:
-    std::shared_ptr<K4AImuSampleSource> m_sampleSource;
+    std::shared_ptr<K4AImuGraphDataGenerator> m_graphDataGenerator;
+
     std::string m_title;
     bool m_failed = false;
 
-    K4AImuDataGraph m_accelerometerGraph;
-    K4AImuDataGraph m_gyroscopeGraph;
-    double m_sensorTemperature;
+    K4AImuGraph m_accGraph;
+    K4AImuGraph m_gyroGraph;
 };
 } // namespace k4aviewer
 

tools/k4aviewer/k4anonbufferingcapturesource.h

@@ -49,6 +49,12 @@ public:
         }
     }
 
+    void ClearData() override
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        m_lastCapture.reset();
+    }
+
     void NotifyTermination() override
     {
         m_failed = true;

tools/k4aviewer/k4apollingthread.h

@@ -14,6 +14,7 @@
 
 // Project headers
 //
+#include "k4aviewerutil.h"
 
 namespace k4aviewer
 {
@@ -34,6 +35,11 @@ public:
         }
     }
 
+    bool IsRunning() const
+    {
+        return m_isRunning;
+    }
+
     ~K4APollingThread()
     {
         Stop();
@@ -42,6 +48,9 @@ public:
 private:
     static void Run(K4APollingThread *instance)
     {
+        instance->m_isRunning = true;
+        CleanupGuard runGuard([instance]() { instance->m_isRunning = false; });
+
         while (!instance->m_shouldExit)
         {
             if (!instance->m_pollFn())
@@ -53,6 +62,7 @@ private:
 
     std::thread m_thread;
     volatile bool m_shouldExit = false;
+    volatile bool m_isRunning = false;
 
     std::function<bool()> m_pollFn;
 };

tools/k4aviewer/k4arecordingdockcontrol.cpp

@@ -21,6 +21,7 @@
 #include "k4atypeoperators.h"
 #include "k4aviewerutil.h"
 #include "k4awindowmanager.h"
+#include "k4aimugraphdatagenerator.h"
 
 using namespace k4aviewer;
 namespace
@@ -129,7 +130,7 @@ K4ARecordingDockControl::K4ARecordingDockControl(std::string &&path, k4a::playba
 
 K4ADockControlStatus K4ARecordingDockControl::Show()
 {
-    ImGui::Text("%s", m_filenameLabel.c_str());
+    ImGui::TextUnformatted(m_filenameLabel.c_str());
     ImGui::SameLine();
     ImGuiExtensions::ButtonColorChanger cc(ImGuiExtensions::ButtonColor::Red);
     if (ImGui::SmallButton("Close"))
@@ -140,14 +141,15 @@ K4ADockControlStatus K4ARecordingDockControl::Show()
     cc.Clear();
     ImGui::Separator();
 
-    ImGui::Text("%s", "Image formats");
+    ImGui::TextUnformatted("Recording Settings");
     ImGui::Text("FPS:              %s", m_fpsLabel.c_str());
     ImGui::Text("Depth mode:       %s", m_depthModeLabel.c_str());
     ImGui::Text("Color format:     %s", m_colorFormatLabel.c_str());
     ImGui::Text("Color resolution: %s", m_colorResolutionLabel.c_str());
+    ImGui::Text("IMU enabled:      %s", m_recordConfiguration.imu_track_enabled ? "Yes" : "No");
     ImGui::Separator();
 
-    ImGui::Text("%s", "Sync settings");
+    ImGui::TextUnformatted("Sync settings");
     ImGui::Text("Depth/color delay (us): %d", m_depthDelayOffColorUsec);
     ImGui::Text("Sync mode:              %s", m_wiredSyncModeLabel.c_str());
     ImGui::Text("Subordinate delay (us): %d", m_subordinateDelayOffMasterUsec);
@@ -155,12 +157,18 @@ K4ADockControlStatus K4ARecordingDockControl::Show()
     ImGui::Text("Recording Length (us):  %lu", m_recordingLengthUsec);
     ImGui::Separator();
 
-    ImGui::Text("%s", "Device info");
+    ImGui::TextUnformatted("Device info");
     ImGui::Text("Device S/N:      %s", m_deviceSerialNumber.c_str());
     ImGui::Text("RGB camera FW:   %s", m_colorFirmwareVersion.c_str());
     ImGui::Text("Depth camera FW: %s", m_depthFirmwareVersion.c_str());
     ImGui::Separator();
 
+    if (!m_playbackThread->IsRunning())
+    {
+        ImGui::Text("Playback failed!");
+        return K4ADockControlStatus::Ok;
+    }
+
     if (ImGui::Button("<|"))
     {
         std::lock_guard<std::mutex> lock(m_playbackThreadState.Mutex);
@@ -232,8 +240,13 @@ bool K4ARecordingDockControl::PlaybackThreadFn(PlaybackThreadState *state)
 
         std::unique_lock<std::mutex> lock(state->Mutex);
 
+        bool forceRefreshImuData = false;
         if (state->SeekTimestamp != InvalidSeekTime)
         {
+            // We need to read back a few seconds from before the time we seeked to.
+            //
+            forceRefreshImuData = true;
+
             state->Recording.seek_timestamp(state->SeekTimestamp, K4A_PLAYBACK_SEEK_BEGIN);
             state->SeekTimestamp = InvalidSeekTime;
 
@@ -248,6 +261,11 @@ bool K4ARecordingDockControl::PlaybackThreadFn(PlaybackThreadState *state)
             backward = state->Step == StepDirection::Backward;
             state->Step = StepDirection::None;
 
+            // Stepping backwards is closer to a seek - we can't just add
+            // new samples on the end, we need to regenerate the graph
+            //
+            forceRefreshImuData |= backward;
+
             // We don't want to restart from the beginning after stepping
             // under most circumstances.  If the user stepped to the last
             // capture, we'll pick up on that when we try to read it and
@@ -266,6 +284,7 @@ bool K4ARecordingDockControl::PlaybackThreadFn(PlaybackThreadState *state)
             //
             state->Recording.seek_timestamp(std::chrono::microseconds(0), K4A_PLAYBACK_SEEK_BEGIN);
             state->RecordingAtEnd = false;
+            forceRefreshImuData = true;
         }
 
         k4a::capture nextCapture;
@@ -293,10 +312,77 @@ bool K4ARecordingDockControl::PlaybackThreadFn(PlaybackThreadState *state)
 
         state->CurrentCaptureTimestamp = GetCaptureTimestamp(nextCapture);
 
-        // Update the images' timestamps using the timing data embedded in the recording
-        // so we show comparable timestamps whenplaying back synchronized recordings
+        // Read IMU data up to the next timestamp, if applicable
         //
+        if (state->ImuPlaybackEnabled)
+        {
+            try
+            {
+                // On seek operations, we need to load historic data or the graph will be wrong.
+                // Move the IMU read pointer back enough samples to populate the entire graph (if available).
+                //
+                if (forceRefreshImuData)
+                {
+                    state->ImuDataSource.ClearData();
 
+                    k4a_imu_sample_t sample;
+
+                    // Seek to the first IMU sample that was before the camera frame we're trying to show
+                    //
+                    while (state->Recording.get_previous_imu_sample(&sample))
+                    {
+                        if (sample.acc_timestamp_usec < static_cast<uint64_t>(state->CurrentCaptureTimestamp.count()))
+                        {
+                            break;
+                        }
+                    }
+
+                    // Then seek back the length of the graph
+                    //
+                    for (int i = 0; i < K4AImuGraphDataGenerator::SamplesPerGraph; ++i)
+                    {
+                        if (!state->Recording.get_previous_imu_sample(&sample))
+                        {
+                            break;
+                        }
+                    }
+                }
+
+                // Read enough samples to catch up to the images that we're about to show
+                //
+                k4a_imu_sample_t nextImuSample;
+                nextImuSample.acc_timestamp_usec = 0;
+
+                while (nextImuSample.acc_timestamp_usec < static_cast<uint64_t>(state->CurrentCaptureTimestamp.count()))
+                {
+                    if (!state->Recording.get_next_imu_sample(&nextImuSample))
+                    {
+                        break;
+                    }
+
+                    // Update the timestamps on the IMU samples using the timing data embedded in the recording
+                    // so we show comparable timestamps when playing back synchronized recordings
+                    //
+                    nextImuSample.acc_timestamp_usec += static_cast<uint64_t>(state->TimestampOffset.count());
+                    nextImuSample.gyro_timestamp_usec += static_cast<uint64_t>(state->TimestampOffset.count());
+
+                    state->ImuDataSource.NotifyObservers(nextImuSample);
+                }
+            }
+            catch (const k4a::error &e)
+            {
+                // If something went wrong while reading the IMU data, mark the IMU failed, but allow
+                // the camera playback to continue.
+                //
+                K4AViewerErrorManager::Instance().SetErrorStatus(e.what());
+                state->ImuDataSource.NotifyTermination();
+                state->ImuPlaybackEnabled = false;
+            }
+        }
+
+        // Update the timestamps on the images using the timing data embedded in the recording
+        // so we show comparable timestamps when playing back synchronized recordings
+        //
         k4a::image images[] = { nextCapture.get_color_image(),
                                 nextCapture.get_depth_image(),
                                 nextCapture.get_ir_image() };
@@ -309,7 +395,7 @@ bool K4ARecordingDockControl::PlaybackThreadFn(PlaybackThreadState *state)
             }
         }
 
-        state->DataSource.NotifyObservers(nextCapture);
+        state->CaptureDataSource.NotifyObservers(nextCapture);
         lock.unlock();
 
         // Account for the time we spent getting captures and such when figuring out how long to wait
@@ -369,12 +455,19 @@ void K4ARecordingDockControl::SetViewType(K4AWindowSet::ViewType viewType)
     K4AWindowManager::Instance().ClearWindows();
 
     std::lock_guard<std::mutex> lock(m_playbackThreadState.Mutex);
+
+    K4ADataSource<k4a_imu_sample_t> *imuDataSource = nullptr;
     switch (viewType)
     {
     case K4AWindowSet::ViewType::Normal:
+        if (m_recordConfiguration.imu_track_enabled)
+        {
+            m_playbackThreadState.ImuPlaybackEnabled = true;
+            imuDataSource = &m_playbackThreadState.ImuDataSource;
+        }
         K4AWindowSet::StartNormalWindows(m_filenameLabel.c_str(),
-                                         &m_playbackThreadState.DataSource,
-                                         nullptr, // IMU playback not supported yet
+                                         &m_playbackThreadState.CaptureDataSource,
+                                         imuDataSource,
                                          nullptr, // Audio source - sound is not supported in recordings
                                          m_recordingHasDepth,
                                          m_recordConfiguration.depth_mode,
@@ -391,7 +484,7 @@ void K4ARecordingDockControl::SetViewType(K4AWindowSet::ViewType viewType)
                                             m_recordConfiguration.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32;
             K4AWindowSet::StartPointCloudWindow(m_filenameLabel.c_str(),
                                                 std::move(calibration),
-                                                &m_playbackThreadState.DataSource,
+                                                &m_playbackThreadState.CaptureDataSource,
                                                 colorPointCloudAvailable);
         }
         catch (const k4a::error &e)

tools/k4aviewer/k4arecordingdockcontrol.h

@@ -18,6 +18,7 @@
 
 #include "ik4adockcontrol.h"
 #include "k4adatasource.h"
+#include "k4aimugraphdatagenerator.h"
 #include "k4apollingthread.h"
 #include "k4awindowset.h"
 
@@ -59,7 +60,10 @@ private:
         // Recording state
         //
         k4a::playback Recording;
-        K4ADataSource<k4a::capture> DataSource;
+        K4ADataSource<k4a::capture> CaptureDataSource;
+        K4ADataSource<k4a_imu_sample_t> ImuDataSource;
+
+        bool ImuPlaybackEnabled;
     } m_playbackThreadState;
 
     static bool PlaybackThreadFn(PlaybackThreadState *state);

tools/k4aviewer/k4aringbuffer.h

@@ -43,6 +43,13 @@ public:
         return m_count == 0;
     }
 
+    void Clear()
+    {
+        std::lock_guard<std::mutex> lock(m_mutex);
+        m_count = 0;
+        m_buffer.fill(T());
+    }
+
     bool Full()
     {
         return m_buffer.size() == m_count;

tools/k4aviewer/k4awindowset.cpp

@@ -17,7 +17,7 @@
 #include "k4acolorimageconverter.h"
 #include "k4adepthimageconverter.h"
 #include "k4aimguiextensions.h"
-#include "k4aimusamplesource.h"
+#include "k4aimugraphdatagenerator.h"
 #include "k4aimuwindow.h"
 #include "k4ainfraredimageconverter.h"
 #include "k4apointcloudwindow.h"
@@ -159,10 +159,10 @@ void K4AWindowSet::StartNormalWindows(const char *sourceIdentifier,
     {
         std::string title = std::string(sourceIdentifier) + ": IMU Data";
 
-        auto imuSampleSource = std::make_shared<K4AImuSampleSource>();
-        imuDataSource->RegisterObserver(std::static_pointer_cast<IK4AImuObserver>(imuSampleSource));
+        auto imuGraphDataGenerator = std::make_shared<K4AImuGraphDataGenerator>();
+        imuDataSource->RegisterObserver(std::static_pointer_cast<IK4AImuObserver>(imuGraphDataGenerator));
 
-        graphWindows.emplace_back(std14::make_unique<K4AImuWindow>(std::move(title), std::move(imuSampleSource)));
+        graphWindows.emplace_back(std14::make_unique<K4AImuWindow>(std::move(title), std::move(imuGraphDataGenerator)));
     }
 
     if (microphoneDataSource != nullptr)