实现了标记点配准的功能

src/algorithm/landmark_registration.cpp

@@ -1,14 +1,14 @@
-#include "landmark_registration.h"
-
-#include "algorithm/measure_window.hpp"
 #include "core/basic_obj_types.hpp"
-#include "utility/bit_operations.hpp"
+#include "core/tristate_obj.h"
 #include "utility/config_utility.hpp"
 #include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
 #include "utility/variable_helper.hpp"
 
 #include <Eigen/Geometry>
 
+DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
+
 namespace sophiar {
 
     using boost::asio::awaitable;
@@ -19,14 +19,16 @@ namespace sophiar {
 
         using point_set_type = Matrix3Xd;
 
+        landmark_registration *q_this;
+
         size_t point_number;
         int next_index = 0;
         point_set_type fiducial_points;
         point_set_type pickled_points;
 
-        global_obj_index_type input_obj_index;
-        global_obj_index_type output_trans_obj_index;
-        global_obj_index_type output_error_obj_index;
+        variable_index_type input_var_index;
+        variable_index_type output_trans_var_index;
+        variable_index_type output_error_var_index;
 
         coro_worker::pointer main_worker;
 
@@ -34,7 +36,7 @@ namespace sophiar {
             assert(config.is_array());
             point_number = config.size();
             assert(point_number >= 3);
-            fiducial_points = point_set_type(3, point_number);
+            fiducial_points = point_set_type{3, point_number};
             for (int i = 0; i < point_number; ++i) {
                 auto &point_config = config[i];
                 assert(point_config.is_array());
@@ -50,9 +52,9 @@ namespace sophiar {
             assert(config.contains("fiducial_points"));
             load_fiducial_points(config["fiducial_points"]);
             // I/O
-            input_obj_index = LOAD_GLOBAL_OBJ_INDEX(scalarxyz_obj, input_point_obj_name);
-            output_trans_obj_index = LOAD_GLOBAL_OBJ_INDEX(transform_obj, output_transform_obj_name);
-            output_error_obj_index = LOAD_GLOBAL_OBJ_INDEX(double_obj, output_error_obj_name);
+            input_var_index = LOAD_VARIABLE_INDEX(scalarxyz_obj, "point_var_name");
+            output_trans_var_index = LOAD_VARIABLE_INDEX(transform_obj, "transform_var_name");
+            output_error_var_index = LOAD_VARIABLE_INDEX(double_obj, "error_var_name");
         }
 
         int get_closest_pickled_point_index(const Vector3d &point) const {
@@ -82,50 +84,57 @@ namespace sophiar {
 
         void do_registration() const {
             auto result = umeyama(fiducial_points, pickled_points, false); // no scaling
-            UPDATE_GLOBAL_OBJ_VALUE(transform_obj, output_trans_obj_index, result);
             auto error_value = calc_registration_error(Isometry3d(result));
-            UPDATE_GLOBAL_OBJ_VALUE(double_obj, output_error_obj_index, error_value);
+            auto ts = current_timestamp();
+            UPDATE_VARIABLE_VAL_WITH_TS(transform_obj, output_trans_var_index, std::move(result), ts);
+            UPDATE_VARIABLE_VAL_WITH_TS(double_obj, output_error_var_index, error_value, ts);
         }
 
         void create_worker() {
             assert(main_worker == nullptr);
-            auto input_helper = GLOBAL_OBJ_AUTO_DELEGATE(scalarxyz_obj, input_obj_index);
-            input_helper.manual_sync();
+            pickled_points = point_set_type{3, point_number};
             auto func = [this,
-                    input_helper = std::move(input_helper)]() mutable
+                    point = VARIABLE_MANUAL_DELEGATE(scalarxyz_obj, input_var_index)]() mutable
                     -> awaitable<bool> {
-                co_await input_helper.coro_wait_update(false);
-                if (input_helper.empty()) co_return true;
-                pickled_points.col(next_index++) = input_helper->value;
+                co_await point.coro_wait_update(true);
+                if (point.empty()) co_return true;
+                pickled_points.col(next_index++) = point->value;
                 if (next_index == point_number) {
                     do_registration();
                     co_return false;
                 }
                 co_return true;
             };
-            main_worker = make_infinite_coro_worker(get_context(), std::move(func));
+            main_worker = make_infinite_coro_worker(std::move(func), stop_on_exit_func(q_this));
             main_worker->run();
         }
 
     };
 
-    awaitable<bool> landmark_registration::on_init(const nlohmann::json &config) {
+    landmark_registration::landmark_registration()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    landmark_registration::~landmark_registration() = default;
+
+    awaitable<bool> landmark_registration::on_init(const nlohmann::json &config) noexcept {
         co_return true;
     }
 
-    awaitable<bool> landmark_registration::on_start(const nlohmann::json &config) {
+    awaitable<bool> landmark_registration::on_start(const nlohmann::json &config) noexcept {
         pimpl->load_config(config);
-        pimpl->create_worker();
         pimpl->next_index = 0;
+        pimpl->create_worker();
         co_return true;
     }
 
-    awaitable<void> landmark_registration::on_stop() {
+    awaitable<void> landmark_registration::on_stop() noexcept {
         SAFE_CLOSE_CORO_WORKER(pimpl->main_worker)
         co_return;
     }
 
-    awaitable<void> landmark_registration::on_reset() {
+    awaitable<void> landmark_registration::on_reset() noexcept {
         co_return;
     }
 

src/algorithm/landmark_registration.h

@@ -1,8 +0,0 @@
-#ifndef SOPHIAR2_LANDMARK_REGISTRATION_H
-#define SOPHIAR2_LANDMARK_REGISTRATION_H
-
-#include "core/tristate_obj.h"
-
-DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
-
-#endif //SOPHIAR2_LANDMARK_REGISTRATION_H

tests/data/landmark_registration_config.json

@@ -1,158 +0,0 @@
-{
-  "listen_port": 5277,
-  "object_list": [
-    {
-      "type": "transform_tree",
-      "name": "global_transform_tree",
-      "init_config": {
-        "node_list": [
-          {
-            "name": "tracker"
-          },
-          {
-            "name": "probe",
-            "parent": "tracker",
-            "transform_obj_name": "probe_in_tracker"
-          },
-          {
-            "name": "model_ref",
-            "parent": "tracker",
-            "transform_obj_name": "model_ref_in_tracker"
-          },
-          {
-            "name": "model",
-            "parent": "model_ref",
-            "transform_obj_name": "model_in_model_ref"
-          }
-        ]
-      },
-      "start_config": {
-        "watch_list": [
-          {
-            "target": "probe",
-            "observer": "model",
-            "transform_name": "probe_in_model"
-          },
-          {
-            "target": "probe",
-            "observer": "model_ref",
-            "transform_name": "probe_in_model_ref"
-          }
-        ]
-      }
-    },
-    {
-      "type": "ndi_interface",
-      "name": "main_ndi",
-      "init_config": {
-        "address_type": "ethernet",
-        "ip": "169.254.132.51",
-        "tcp_port": 8765,
-        "com_port": "COM3",
-        "tool_list": [
-          {
-            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
-            "outputs": {
-              "transform": "probe_in_tracker",
-              "marker_uncertainty": "probe_in_tracker_uncertainty"
-            }
-          },
-          {
-            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Head_3Ball_2.rom",
-            "outputs": {
-              "transform": "model_ref_in_tracker",
-              "marker_uncertainty": "model_ref_in_tracker_uncertainty"
-            }
-          }
-        ]
-      },
-      "start_config": {
-        "allow_unreliable": true,
-        "prefer_stream_tracking": true
-      }
-    },
-    {
-      "type": "transform_stabilizer",
-      "name": "model_ref_point_pickler",
-      "dependencies": [
-        "global_transform_tree",
-        "main_ndi"
-      ],
-      "start_config": {
-        "stable_type": "point",
-        "input_obj_name": "probe_in_model_ref",
-        "output_obj_name": "pickled_point_in_model_ref",
-        "linear_tolerance_mm": 0.2,
-        "angular_tolerance_deg": 0.05,
-        "temporal_interval_s": 1.5,
-        "counting_interval": 150
-      }
-    },
-    {
-      "type": "landmark_registration",
-      "name": "model_registration",
-      "dependencies": [
-        "model_ref_point_pickler"
-      ],
-      "start_config": {
-        "fiducial_points": [
-          [
-            164.66571453500848,
-            18.71164620295797,
-            82.62518570008311
-          ],
-          [
-            123.52831200584696,
-            14.231978533211485,
-            96.3638417317588
-          ],
-          [
-            93.07753837629471,
-            11.094425704616972,
-            79.15928190277317
-          ],
-          [
-            143.6043085335037,
-            25.819879662834747,
-            108.72907272943397
-          ],
-          [
-            132.738357125514,
-            26.348048379275646,
-            109.0070454170842
-          ],
-          [
-            116.95809799419658,
-            24.962181959549664,
-            109.67909720928142
-          ]
-        ],
-        "input_point_obj_name": "pickled_point_in_model_ref",
-        "output_transform_obj_name": "model_in_model_ref",
-        "output_error_obj_name": "model_registration_error"
-      }
-    },
-    {
-      "type": "transform_obj_recorder",
-      "name": "model_ref_in_tracker_recorder",
-      "dependencies": [
-        "main_ndi"
-      ],
-      "start_config": {
-        "obj_name": "probe_in_tracker",
-        "save_file": "probe_in_tracker.txt"
-      }
-    },
-    {
-      "type": "transform_obj_recorder",
-      "name": "model_ref_in_tracker_recorder",
-      "dependencies": [
-        "main_ndi"
-      ],
-      "start_config": {
-        "obj_name": "model_ref_in_tracker",
-        "save_file": "model_ref_in_tracker.txt"
-      }
-    }
-  ]
-}

tests/data/ndi_registration_config.json

@@ -0,0 +1,281 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_in_model_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_model_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tip_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "model_in_model_ref_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "tip_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model",
+      "type": "scalarxyz_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "name": "model_ref",
+            "parent": "tracker",
+            "transform_var_name": "model_ref_in_tracker"
+          },
+          {
+            "name": "model",
+            "parent": "model_ref",
+            "transform_var_name": "model_in_model_ref"
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "probe",
+            "observer": "model_ref",
+            "transform_var_name": "probe_in_model_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "model",
+            "transform_var_name": "probe_in_model"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "169.254.132.51",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker"
+            }
+          },
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Head_3Ball_1.rom",
+            "outputs": {
+              "transform": "model_ref_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "probe_visibility_watcher",
+      "start_config": {
+        "variable_name": "probe_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "model_ref_visibility_watcher",
+      "start_config": {
+        "variable_name": "model_ref_in_tracker"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model_ref",
+        "target_value": [
+          -0.55,
+          -0.96,
+          1.72
+        ],
+        "output_var_name": "tip_in_model_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model_ref",
+        "output_var_name": "picked_point_in_model_ref",
+        "linear_tolerance_mm": 0.02,
+        "angular_tolerance_deg": 0.01,
+        "temporal_interval_s": 1.5,
+        "counting_interval": 150
+      },
+      "dependencies": [
+        "probe_tip_transformer"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher",
+      "start_config": {
+        "variable_name": "picked_point_in_model_ref"
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "model_registrator",
+      "start_config": {
+        "fiducial_points": [
+          [
+            0,
+            0,
+            0
+          ],
+          [
+            0,
+            50,
+            0
+          ],
+          [
+            50,
+            50,
+            0
+          ],
+          [
+            50,
+            0,
+            0
+          ]
+        ],
+        "point_var_name": "picked_point_in_model_ref",
+        "transform_var_name": "model_in_model_ref",
+        "error_var_name": "model_in_model_ref_error"
+      },
+      "dependencies": [
+        "point_picker"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "registration_result_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "registration_error_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref_error"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer2",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model",
+        "target_value": [
+          -0.55,
+          -0.96,
+          1.72
+        ],
+        "output_var_name": "tip_in_model"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker2",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model",
+        "output_var_name": "picked_point_in_model",
+        "linear_tolerance_mm": 0.02,
+        "angular_tolerance_deg": 0.01,
+        "temporal_interval_s": 1.5,
+        "counting_interval": 150
+      },
+      "dependencies": [
+        "probe_tip_transformer2"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher2",
+      "start_config": {
+        "variable_name": "picked_point_in_model"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher",
+        "registration_result_watcher",
+        "registration_error_watcher",
+        "model_registrator"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "all2",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher2",
+        "point_picker2"
+      ]
+    }
+  ]
+}

tests/interface/ndi_registration.cpp

@@ -0,0 +1,22 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/global_defs.h"
+
+#include <boost/test/unit_test.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <fstream>
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+BOOST_AUTO_TEST_CASE(test_ndi_registration) {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/ndi_registration_config.json");
+    BOOST_TEST(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    auto ok = initialize(config);
+    BOOST_TEST(ok);
+    global_context->run();
+}