Implemented Kalman denoise filter.

app/data/tka_ar_navigation.json

@@ -4,6 +4,10 @@
       "name": "probe_in_tracker",
       "type": "transform_obj"
     },
+    {
+      "name": "probe_in_tracker_denoised",
+      "type": "transform_obj"
+    },
     {
       "name": "femur_ref_in_tracker",
       "type": "transform_obj"
@@ -244,7 +248,7 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "ethernet",
+        "address_type": "serial",
         "ip": "10.0.0.5",
         "tcp_port": 8765,
         "com_port": "/dev/ttyUSB0",
@@ -281,7 +285,7 @@
       }
     },
     {
-      "type": "object_validity_watcher",
+      "type": "variable_validity_watcher",
       "name": "probe_visibility_watcher",
       "start_config": {
         "variable_name": "probe_in_tracker"
@@ -550,6 +554,25 @@
         "variable_name": "picked_point_in_tibia"
       }
     },
+    {
+      "type": "kalman_denoiser",
+      "name": "test_denoiser",
+      "init_config": {
+        "variable_in": "probe_in_tracker",
+        "variable_out": "probe_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "variable_recorder",
+      "name": "test_recorder",
+      "init_config": {
+        "variable_name": "probe_in_tracker_denoised",
+        "save_file": "probe_in_tracker.csv"
+      },
+      "dependencies": [
+        "test_denoiser"
+      ]
+    },
     {
       "type": "empty_object",
       "name": "tracker_all",
@@ -559,7 +582,8 @@
         "probe_visibility_watcher",
         "camera_visibility_watcher",
         "femur_visibility_watcher",
-        "tibia_visibility_watcher"
+        "tibia_visibility_watcher",
+        "test_recorder"
       ]
     },
     {

src/algorithm/algorithm_defs.cpp

@@ -6,6 +6,7 @@
 DEFAULT_TRISTATE_OBJ_DEF(transform_tree)
 DEFAULT_TRISTATE_OBJ_DEF(transform_stabilizer)
 DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
+DEFAULT_TRISTATE_OBJ_DEF(kalman_denoiser)
 
 namespace sophiar {
 
@@ -13,6 +14,7 @@ namespace sophiar {
         REGISTER_TYPE(transform_tree);
         REGISTER_TYPE(transform_stabilizer);
         REGISTER_TYPE(landmark_registration);
+        REGISTER_TYPE(kalman_denoiser);
         // control utility
         using five_dof_offset_calculator = simple_tristate_obj_wrapper<make_5dof_offset_calculator_func>;
         using tcp_offset_to_flange_calculator = simple_tristate_obj_wrapper<make_tcp_offset_to_flange_calculator_func>;

src/algorithm/kalman_denoiser.cpp

@@ -0,0 +1,173 @@
+#include "core/basic_obj_types.hpp"
+#include "core/tristate_obj.h"
+#include "utility/config_utility.hpp"
+
+DEFAULT_TRISTATE_OBJ_DEF(kalman_denoiser)
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+
+    using namespace Eigen;
+
+    namespace kalman_denoiser_impl {
+
+        constexpr auto TRANS_R = 0.2;
+        constexpr auto TRANS_P0 = 1000;
+        constexpr auto ROT_R = 0.002;
+        constexpr auto ROT_P0 = 1;
+
+        constexpr auto TRANS_R2 = TRANS_R * TRANS_R;
+        constexpr auto TRANS_P02 = TRANS_P0 * TRANS_P0;
+        constexpr auto ROT_R2 = ROT_R * ROT_R;
+        constexpr auto ROT_P02 = ROT_P0 * ROT_P0;
+
+        struct simple_kalman {
+            double X, P, Q; // state related
+            double R; // parameters
+
+            double progress(double Z) {
+                auto X_p = X;
+                auto P_p = P + Q;
+                auto K = P_p / (P_p + R);
+                auto X_n = X_p + K * (Z - X_p);
+                auto P_n = P_p - K * P_p;
+                Q = (Z - X_p) * (Z - X_p);
+                if (Q <= R) {
+                    Q = 0;
+                }
+                X = X_n;
+                P = P_n;
+                return X;
+            }
+        };
+
+        struct transform_denoiser {
+            simple_kalman filters[7] = {};
+            double buffer[7] = {};
+            int iter = 0;
+
+            transform_denoiser() {
+                reset();
+            }
+
+            void reset() {
+                for (auto k = 0; k < 3; ++k) {
+                    filters[k].X = 0;
+                    filters[k].P = TRANS_P02;
+                    filters[k].Q = 0;
+                    filters[k].R = TRANS_R2;
+                }
+                for (auto k = 3; k < 7; ++k) {
+                    filters[k].X = 0;
+                    filters[k].P = ROT_P02;
+                    filters[k].Q = 0;
+                    filters[k].R = ROT_R2;
+                }
+            }
+
+            transform_denoiser &operator<<(double val) {
+                buffer[iter] = filters[iter].progress(val);
+                ++iter;
+                return *this;
+            }
+
+            transform_denoiser &operator>>(double &val) {
+                val = buffer[iter++];
+                return *this;
+            }
+
+            transform_obj::pointer progress(const transform_obj::pointer &in) {
+                // handle empty
+                if (!in) {
+                    reset();
+                    return nullptr;
+                }
+
+                // load values
+                iter = 0;
+                in->write_to(*this);
+
+                // save values
+                iter = 0;
+                auto ret = transform_obj::new_instance();
+                ret->fill_from(*this);
+                return ret;
+            }
+        };
+
+    }
+
+    using namespace kalman_denoiser_impl;
+
+    struct kalman_denoiser::impl {
+
+        variable_index_type var_index_in, var_index_out;
+        variable_type_index_type var_type;
+        sophiar_pool::callback_token_type cb_token = nullptr;
+        sophiar_pool::attach_token_type ath_token = nullptr;
+
+        transform_denoiser transform_worker;
+
+        void progress() {
+            switch (var_type) {
+                case transform_var_type_index: {
+                    auto val = QUERY_VARIABLE(transform_obj, var_index_in);
+                    auto val_n = transform_worker.progress(val);
+                    UPDATE_VARIABLE(transform_obj, var_index_out, val_n);
+                    break;
+                }
+                default: {
+                    assert(false);
+                    break;
+                }
+            }
+        }
+
+        void load_config(const nlohmann::json &config) {
+            // input
+            auto var_name_in = LOAD_STRING_ITEM("variable_in");
+            auto var_info_in = global_sophiar_pool->query_variable_information(var_name_in);
+            var_index_in = var_info_in.index;
+
+            // output
+            auto var_name_out = LOAD_STRING_ITEM("variable_out");
+            auto var_info_out = global_sophiar_pool->query_variable_information(var_name_out);
+            var_index_out = var_info_out.index;
+
+            assert(var_info_in.type_index == var_info_out.type_index);
+            var_type = var_info_in.type_index;
+        }
+
+    };
+
+    kalman_denoiser::kalman_denoiser()
+            : pimpl(std::make_unique<impl>()) {}
+
+    kalman_denoiser::~kalman_denoiser() = default;
+
+    awaitable<bool> kalman_denoiser::on_init(const nlohmann::json &config) noexcept {
+        pimpl->load_config(config);
+        assert(pimpl->cb_token == nullptr);
+        pimpl->cb_token = REGISTER_CALLBACK([this] { pimpl->progress(); });
+        co_return true;
+    }
+
+    awaitable<bool> kalman_denoiser::on_start(const nlohmann::json &) noexcept {
+        assert(pimpl->ath_token == nullptr);
+        pimpl->ath_token = ATTACH_CALLBACK(pimpl->var_index_in, pimpl->cb_token);
+        co_return true;
+    }
+
+    awaitable<void> kalman_denoiser::on_stop() noexcept {
+        DETACH_CALLBACK(pimpl->ath_token);
+        pimpl->ath_token = nullptr;
+        co_return;
+    }
+
+    awaitable<void> kalman_denoiser::on_reset() noexcept {
+        UNREGISTER_CALLBACK(pimpl->cb_token);
+        pimpl->cb_token = nullptr;
+        co_return;
+    }
+}

src/core/sophiar_pool.cpp

@@ -343,15 +343,18 @@ namespace sophiar {
     }
 
     void sophiar_pool::unregister_callback(callback_token_type token) {
+        if (token == nullptr) return;
         return pimpl->unregister_callback(std::bit_cast<impl::callback_iter_type>(token));
     }
 
     sophiar_pool::attach_token_type sophiar_pool::attach_callback(variable_index_type var_index,
                                                                   callback_token_type token) {
+        assert(token != nullptr);
         return pimpl->attach_callback(var_index, std::bit_cast<impl::callback_iter_type>(token));
     }
 
     void sophiar_pool::detach_callback(attach_token_type token) {
+        if (token == nullptr) return;
         return pimpl->detach_callback(std::bit_cast<impl::attach_iter_type>(token));
     }
 

src/utility/variable_utility2.cpp

@@ -8,6 +8,7 @@
 
 #include <spdlog/spdlog.h>
 
+#include <fstream>
 #include <vector>
 
 using boost::asio::awaitable;
@@ -116,11 +117,62 @@ namespace sophiar {
         }
     };
 
+    class variable_recorder : public tristate_obj {
+    public:
+        variable_recorder()
+                : buffer(string_writer(",")) {
+        }
+
+        DEFAULT_NEW_INSTANCE(variable_recorder)
+
+        awaitable<bool> on_init(const nlohmann::json &config) noexcept override {
+            auto var_name = LOAD_STRING_ITEM("variable_name");
+            auto var_info = global_sophiar_pool->query_variable_information(var_name);
+            var_index = var_info.index;
+            auto save_file_path = LOAD_STRING_ITEM("save_file");
+            ofs = std::ofstream(save_file_path, std::ofstream::out);
+            assert(ofs.is_open());
+            auto cb_func = [=, this]() mutable {
+                auto ts = QUERY_VARIABLE_TS(var_index);
+                buffer << (static_cast<double>(ts) / 1000.0); // us -> ms
+                raw_pointer_write_to(buffer, var_info.placeholder, var_info.type_index);
+                ofs << buffer.get_string_and_reset() << std::endl;
+            };
+            assert(cb_token == nullptr);
+            cb_token = REGISTER_CALLBACK(cb_func);
+            co_return true;
+        }
 
+        boost::asio::awaitable<bool> on_start(const nlohmann::json &) noexcept override {
+            assert(ath_token == nullptr);
+            ath_token = ATTACH_CALLBACK(var_index, cb_token);
+            co_return true;
+        }
+
+        boost::asio::awaitable<void> on_stop() noexcept override {
+            DETACH_CALLBACK(ath_token);
+            ath_token = nullptr;
+            co_return;
+        }
+
+        boost::asio::awaitable<void> on_reset() noexcept override {
+            UNREGISTER_CALLBACK(cb_token);
+            cb_token = nullptr;
+            co_return;
+        }
+
+    private:
+        string_writer buffer;
+        std::ofstream ofs;
+        variable_index_type var_index = -1;
+        sophiar_pool::callback_token_type cb_token = nullptr;
+        sophiar_pool::attach_token_type ath_token = nullptr;
+    };
 
     void register_variable_utility() {
-        REGISTER_TYPE2(variable_debug_watcher, "object_watcher");
-        REGISTER_TYPE2(variable_validity_watcher, "object_validity_watcher");
+        REGISTER_TYPE2(variable_debug_watcher, "variable_watcher");
+        REGISTER_TYPE2(variable_validity_watcher, "variable_validity_watcher");
+        REGISTER_TYPE2(variable_recorder, "variable_recorder");
     }
 
 }