Implemented probe point-to-point test.

CMakeLists.txt

@@ -4,7 +4,8 @@ project(RemoteAR3)
 set(CMAKE_CXX_STANDARD 20)
 
 add_executable(${PROJECT_NAME} src/main.cpp
-        src/experiment_impl/augment_accuracy.cpp
+        src/experiment/augment_accuracy.cpp
+        src/experiment/probe_p2p.cpp
         src/main_ext.cpp
         src/main_impl/camera_related.cpp
         src/main_impl/encoder_related.cpp

data/sophiar_config.json

@@ -1,5 +1,9 @@
 {
   "variable_list": [
+    {
+      "name": "probe_ref_in_tracker",
+      "type": "transform_obj"
+    },
     {
       "name": "probe_in_tracker",
       "type": "transform_obj"
@@ -125,7 +129,7 @@
           {
             "name": "probe_ref",
             "parent": "tracker",
-            "transform_var_name": "probe_in_tracker"
+            "transform_var_name": "probe_ref_in_tracker"
           },
           {
             "name": "probe",
@@ -215,6 +219,11 @@
             "observer": "tracker",
             "transform_var_name": "tibia_in_tracker"
           },
+          {
+            "target": "probe",
+            "observer": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
           {
             "target": "probe",
             "observer": "femur_ref",
@@ -250,7 +259,7 @@
           {
             "rom_path": "./roms/GlassProbe_4Ball_4.rom",
             "outputs": {
-              "transform": "probe_in_tracker"
+              "transform": "probe_ref_in_tracker"
             }
           },
           {

src/experiment/probe_p2p.cpp

@@ -0,0 +1,160 @@
+#include "probe_p2p.h"
+#include "core/local_connection.h"
+#include "imgui_utility.h"
+#include "main_impl/impl_types.h"
+#include "vtk_viewer.h"
+
+#include <functional>
+#include <random>
+#include <queue>
+
+using namespace sophiar;
+
+extern local_connection sophiar_conn;
+extern std::unique_ptr<vtk_viewer> augment_viewer;
+extern std::unique_ptr<camera_related> left;
+
+struct probe_p2p::impl {
+
+    bool enable = false;
+    bool is_running = false;
+    bool use_camera_coord = true;
+    std::optional<float> last_dis_err, last_angle_err;
+    std::string probe_var_name;
+    Eigen::Isometry3d augment_probe_pose; // in camera or tracker
+    vtkSmartPointer<vtkActor> real_probe_actor;
+    vtkSmartPointer<vtkActor> augment_probe_actor;
+    std::queue<std::function<void()>> eq; // local event queue
+
+    ~impl() {
+        // TODO deconstruct earlier
+//        augment_viewer->remove_actor(real_probe_actor);
+//        augment_viewer->remove_actor(augment_probe_actor);
+    }
+
+    static impl *create(const create_config &conf) {
+        auto ret = new impl{};
+        ret->real_probe_actor = create_actor(conf.probe_model_path);
+        ret->augment_probe_actor = create_actor(conf.probe_model_path);
+        augment_viewer->add_actor(ret->real_probe_actor);
+        augment_viewer->add_actor(ret->augment_probe_actor);
+        ret->probe_var_name = conf.probe_var_name;
+        return ret;
+    }
+
+    void process() {
+        if (!enable) return;
+        while (!eq.empty()) { // handle events from UI
+            eq.front()();
+            eq.pop();
+        }
+        if (!is_running) {
+            real_probe_actor->SetVisibility(false);
+            augment_probe_actor->SetVisibility(false);
+            return;
+        }
+        augment_probe_actor->SetVisibility(true);
+        auto real_trans = sophiar_conn.query_transform_variable(probe_var_name);
+        update_actor_pose(real_probe_actor, real_trans);
+
+        // recalculate augment pose
+        auto augment_trans = augment_probe_pose;
+        if (use_camera_coord) {
+            augment_trans = left->transform * augment_trans;
+            update_actor_pose(augment_probe_actor, augment_trans);
+        }
+
+        // calculate error
+        if (!real_trans.has_value()) {
+            last_dis_err = {};
+            last_angle_err = {};
+            return;
+        }
+        static auto origin = Eigen::Vector3d::Zero();
+        auto real_origin = real_trans.value() * origin;
+        auto augment_origin = augment_trans * origin;
+        last_dis_err = (real_origin - augment_origin).norm();
+        auto real_z = real_trans->rotation().col(2);
+        auto augment_z = augment_trans.rotation().col(2);
+        auto angle_err = std::acos(real_z.dot(augment_z));
+        angle_err *= 180.0 / std::numbers::pi;
+        last_angle_err = 180.0 - angle_err;
+    }
+
+    void prepare_target() {
+        constexpr auto x_min = -50, x_max = 50;
+        constexpr auto y_min = -50, y_max = 50;
+        constexpr auto z_min = 400, z_max = 600;
+
+        static std::random_device rd;
+        static std::mt19937 gen{rd()};
+        static auto rand_x = std::uniform_real_distribution<double>(x_min, x_max);
+        static auto rand_y = std::uniform_real_distribution<double>(y_min, y_max);
+        static auto rand_z = std::uniform_real_distribution<double>(z_min, z_max);
+        static auto rand_q = std::uniform_real_distribution<double>(-1, 1);
+
+        static auto augment_base_trans = Eigen::AngleAxisd{ // Flip at Z axis
+                std::numbers::pi, Eigen::Vector3d::UnitX()};
+        auto augment_transl = Eigen::Translation3d{rand_x(gen), rand_y(gen), rand_z(gen)};
+        auto quat_vec = Eigen::Vector4d{rand_q(gen), rand_q(gen), rand_q(gen), rand_q(gen)};
+        quat_vec /= quat_vec.norm();
+        auto augment_rot = Eigen::Quaterniond{quat_vec};
+        augment_probe_pose = augment_transl * augment_rot * augment_base_trans;
+        if (use_camera_coord) {
+            update_actor_pose(augment_probe_actor,
+                              left->transform * augment_probe_pose); // transform to world coordinate
+        } else {
+            augment_probe_pose = left->transform * augment_probe_pose;
+            update_actor_pose(augment_probe_actor, augment_probe_pose);
+        }
+    }
+
+    void show_ui() {
+        {
+            auto guard = imgui_disable_guard{is_running};
+            ImGui::Checkbox("Enable", &enable);
+        }
+        if (!enable) return;
+        {
+            auto guard = imgui_disable_guard{is_running};
+            ImGui::SameLine();
+            ImGui::Checkbox("Camera Coordinate", &use_camera_coord);
+        }
+        if (!is_running) {
+            if (ImGui::Button("Start")) {
+                eq.emplace([&] {
+                    prepare_target();
+                    is_running = true;
+                });
+            }
+        } else {
+            if (ImGui::Button("Record")) {
+                eq.emplace([&] {
+                    // TODO
+                    is_running = false;
+                });
+            }
+            show_variable("Distance Error", last_dis_err);
+            show_variable("Angular Error", last_angle_err);
+        }
+    }
+
+};
+
+probe_p2p::~probe_p2p() = default;
+
+probe_p2p *probe_p2p::create(const create_config &conf) {
+    auto pimpl = impl::create(conf);
+//    if (pimpl == nullptr) return nullptr;
+    auto ret = new probe_p2p;
+    ret->pimpl.reset(pimpl);
+    return ret;
+}
+
+void probe_p2p::process() {
+    pimpl->process();
+}
+
+void probe_p2p::show_ui() {
+    pimpl->show_ui();
+}

src/experiment/probe_p2p.h

@@ -0,0 +1,28 @@
+#ifndef REMOTEAR3_PROBE_P2P_H
+#define REMOTEAR3_PROBE_P2P_H
+
+#include <memory>
+
+class probe_p2p {
+public:
+
+    struct create_config {
+        std::string probe_model_path;
+        std::string probe_var_name; // observed in world
+    };
+
+    ~probe_p2p();
+
+    static probe_p2p *create(const create_config &conf);
+
+    void show_ui();
+
+    void process();
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+
+#endif //REMOTEAR3_PROBE_P2P_H

src/main_ext.cpp

@@ -1,6 +1,7 @@
 #include "components/registration.h"
 #include "core/local_connection.h"
 #include "cuda_helper.hpp"
+#include "experiment/probe_p2p.h"
 #include "main_impl/impl_types.h"
 #include "simple_mq.h"
 #include "utility.hpp"
@@ -68,6 +69,8 @@ std::string probe_model_path;
 std::vector<registration_target> reg_targets;
 std::unique_ptr<registration> reg;
 
+std::unique_ptr<probe_p2p> exp_probe;
+
 bool debug_options = false;
 bool show_vtk_debug = false;
 bool show_imgui_demo = false;
@@ -234,6 +237,9 @@ void initialize_main() {
     for (auto &item: reg_targets) {
         reg->add_target(item);
     }
+    exp_probe.reset(probe_p2p::create(
+            {"./models/Probe.stl",
+             "probe_in_tracker"}));
 
     // initialize vtk test viewer
     vtk_test1 = std::make_unique<vtk_viewer>();
@@ -380,6 +386,10 @@ void prepare_imgui_frame() {
                     ImGui::TreePop();
                 }
             }
+            if (ImGui::TreeNode("Probe P2P")) {
+                exp_probe->show_ui();
+                ImGui::TreePop();
+            }
             ImGui::PopID();
         }
 
@@ -414,6 +424,7 @@ void process_augment_accuracy();
 void process_frame() {
     prepare_augment_info();
     process_augment_accuracy();
+    exp_probe->process();
     process_camera_frames();
 }