Implemented drill position guide.

CMakeLists.txt

@@ -35,6 +35,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/core/impl/object_manager.cpp
         src/core/impl/pc_utility.cpp
         src/module_v3/registration.cpp
+        src/module/experiment/impl/calib_eval.cpp
         src/module/guidance/impl/acl_guide.cpp
         src/module/guidance/impl/cursor_guide.cpp
         src/module/guidance/impl/guide_helper.cpp

data/config_endo_guide.yaml

@@ -5,7 +5,7 @@ sophiar_start_var: tracker_all
 
 camera_ref_transform_var: camera_ref_in_tracker
 camera_ref_error_var: camera_ref_in_tracker_error
-camera_transform_var: camera_in_tracker
+camera_transform_var: camera_in_tracker_denoised
 camera_calib_transform_var: camera_in_camera_ref
 
 monitor:
@@ -19,6 +19,8 @@ monitor:
     var: tibia_ref_in_tracker
   - name: Probe
     var: probe_ref_in_tracker
+  - name: Drill
+    var: drill_ref_in_tracker
 
 augment_list:
   - name: Femur
@@ -62,4 +64,5 @@ acl_guide:
   tibia:
     collect_obj: point_picker_in_tibia_ref
     collect_var: picked_point_in_tibia_ref
-    transform_var: tibia_ref_in_tracker
+    transform_var: tibia_ref_in_tracker
+  drill_transform_var: drill_in_tracker

data/config_endo_guide_oldhead.yaml

@@ -0,0 +1,81 @@
+app_name: endo_guide
+
+sophiar_config: /home/tpx/project/DepthGuide/data/sophiar_config_endo_guide_oldhead.json
+sophiar_start_var: tracker_all
+
+camera_ref_transform_var: camera_ref_in_tracker
+camera_ref_error_var: camera_ref_in_tracker_error
+camera_transform_var: camera_in_tracker
+camera_calib_transform_var: camera_in_camera_ref
+
+monitor:
+  - name: Camera
+    var: camera_ref_in_tracker
+  - name: MVS Camera
+    var: mvs_ref_in_tracker
+  - name: Femur
+    var: femur_ref_in_tracker
+  - name: Tibia
+    var: tibia_ref_in_tracker
+  - name: Probe
+    var: probe_ref_in_tracker
+
+augment_list:
+  - name: Femur
+    transform_var: femur_in_tracker_denoised
+    model: /home/tpx/data/OldHead/OldBone.stl
+  - name: Tibia
+    transform_var: tibia_in_tracker_denoised
+    model: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl
+
+probe_model: /home/tpx/project/RemoteAR3/data/models/Probe.stl
+
+registration_list:
+  - name: Femur
+    model_file: /home/tpx/data/OldHead/OldBone.stl
+    collect_obj: point_picker_in_femur_ref
+    collect_var: picked_point_in_femur_ref
+    target_var: femur_in_femur_ref
+    probe_var: probe_in_femur
+  - name: Tibia
+    model_file: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl
+    collect_obj: point_picker_in_tibia_ref
+    collect_var: picked_point_in_tibia_ref
+    target_var: tibia_in_tibia_ref
+    probe_var: probe_in_tibia
+
+left_camera_name: "LeftEye"
+right_camera_name: "RightEye"
+
+left_camera_info:
+  fx: 3566.07514575013
+  fy: 3565.09801365950
+  cx: 1230.14290684677
+  cy: 1026.56369172491
+  k0: -0.0668340443448111
+  k1: 0.0831050102080411
+  width: 2448
+  height: 2048
+
+right_camera_info:
+  fx: 3579.11217391698
+  fy: 3578.22682676712
+  cx: 1219.97484179738
+  cy: 1036.82186898493
+  k0: -0.0802083588903196
+  k1: 0.158880530651155
+  width: 2448
+  height: 2048
+
+left_camera_transform_var: left_camera_in_tracker_denoised
+right_camera_transform_var: right_camera_in_tracker_denoised
+
+acl_guide:
+  femur:
+    collect_obj: point_picker_in_femur_ref
+    collect_var: picked_point_in_femur_ref
+    transform_var: femur_ref_in_tracker
+  tibia:
+    collect_obj: point_picker_in_tibia_ref
+    collect_var: picked_point_in_tibia_ref
+    transform_var: tibia_ref_in_tracker

data/sophiar_config_endo_guide.json

@@ -70,6 +70,14 @@
       "name": "right_camera_in_tracker",
       "type": "transform_obj"
     },
+    {
+      "name": "drill_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "drill_in_tracker",
+      "type": "transform_obj"
+    },
     {
       "name": "femur_in_tracker",
       "type": "transform_obj"
@@ -94,6 +102,14 @@
       "name": "probe_in_tibia",
       "type": "transform_obj"
     },
+    {
+      "name": "femur_in_camera_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_camera_ref",
+      "type": "transform_obj"
+    },
     {
       "name": "probe_tip_in_femur_ref",
       "type": "scalarxyz_obj"
@@ -126,6 +142,10 @@
       "name": "picked_point_in_tibia",
       "type": "scalarxyz_obj"
     },
+    {
+      "name": "camera_in_tracker_denoised",
+      "type": "transform_obj"
+    },
     {
       "name": "left_camera_in_tracker_denoised",
       "type": "transform_obj"
@@ -230,6 +250,24 @@
               0.4738,
               -0.4982
             ]
+          },
+          {
+            "name": "drill_ref",
+            "parent": "tracker",
+            "transform_var_name": "drill_ref_in_tracker"
+          },
+          {
+            "name": "drill",
+            "parent": "drill_ref",
+            "transform": [
+              -134.93834363508242,
+              -38.64937251450866,
+              76.05756748057985,
+              0.5651659820674402,
+              0.4249557773624757,
+              -0.4256067427602504,
+              -0.5646759252146404
+            ]
           }
         ]
       },
@@ -250,6 +288,11 @@
             "observer": "tracker",
             "transform_var_name": "right_camera_in_tracker"
           },
+          {
+            "target": "drill",
+            "observer": "tracker",
+            "transform_var_name": "drill_in_tracker"
+          },
           {
             "target": "femur",
             "observer": "tracker",
@@ -284,6 +327,16 @@
             "target": "probe",
             "observer": "tibia",
             "transform_var_name": "probe_in_tibia"
+          },
+          {
+            "target": "femur",
+            "observer": "camera_ref",
+            "transform_var_name": "femur_in_camera_ref"
+          },
+          {
+            "target": "tibia",
+            "observer": "camera_ref",
+            "transform_var_name": "tibia_in_camera_ref"
           }
         ]
       }
@@ -317,6 +370,12 @@
               "transform": "mvs_ref_in_tracker"
             }
           },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_7_Drill.rom",
+            "outputs": {
+              "transform": "drill_ref_in_tracker"
+            }
+          },
           {
             "rom_path": "/home/tpx/data/roms/Glass_3Ball_5.rom",
             "serial_number": "39B33001",
@@ -457,6 +516,14 @@
         "variable_name": "tibia_in_tibia_ref"
       }
     },
+    {
+      "type": "kalman_denoiser",
+      "name": "camera_denoiser",
+      "init_config": {
+        "variable_in": "camera_in_tracker",
+        "variable_out": "camera_in_tracker_denoised"
+      }
+    },
     {
       "type": "kalman_denoiser",
       "name": "left_camera_denoiser",
@@ -501,6 +568,7 @@
         "tibia_visibility_watcher",
         "femur_denoiser",
         "tibia_denoiser",
+        "camera_denoiser",
         "left_camera_denoiser",
         "right_camera_denoiser"
       ]

data/sophiar_config_endo_guide_oldhead.json

@@ -0,0 +1,539 @@
+{
+  "variable_list": [
+    {
+      "name": "probe_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_in_femur_ref",
+      "type": "transform_obj",
+      "value": [
+        -158.57010140440562,
+        109.08408048026303,
+        311.7159241940251,
+        0.6426166606045021,
+        -0.3019245048766848,
+        -0.18681286301124206,
+        0.6789597742738753
+      ]
+    },
+    {
+      "name": "tibia_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_tibia_ref",
+      "type": "transform_obj",
+      "value": [
+        117.21224919149104,
+        38.32624851921699,
+        -72.90989200004259,
+        -0.19759914802407674,
+        0.6557689584549056,
+        -0.28931051779904626,
+        0.6687458965242118
+      ]
+    },
+    {
+      "name": "camera_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "camera_ref_in_tracker_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "camera_in_camera_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "camera_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "mvs_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "left_camera_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "right_camera_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_femur_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_tibia_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_femur",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_tibia",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_in_camera_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_camera_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_tip_in_femur_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "probe_tip_in_tibia_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_femur_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_tibia_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "probe_tip_in_femur",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "probe_tip_in_tibia",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_femur",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_tibia",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "camera_in_tracker_denoised",
+      "type": "transform_obj"
+    },
+    {
+      "name": "left_camera_in_tracker_denoised",
+      "type": "transform_obj"
+    },
+    {
+      "name": "right_camera_in_tracker_denoised",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_in_tracker_denoised",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_tracker_denoised",
+      "type": "transform_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe_ref",
+            "parent": "tracker",
+            "transform_var_name": "probe_ref_in_tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "probe_ref",
+            "transform": [
+              -0.69,
+              -13.95,
+              -186.84,
+              1,
+              0,
+              0,
+              0
+            ]
+          },
+          {
+            "name": "femur_ref",
+            "parent": "tracker",
+            "transform_var_name": "femur_ref_in_tracker"
+          },
+          {
+            "name": "femur",
+            "parent": "femur_ref",
+            "transform_var_name": "femur_in_femur_ref"
+          },
+          {
+            "name": "tibia_ref",
+            "parent": "tracker",
+            "transform_var_name": "tibia_ref_in_tracker"
+          },
+          {
+            "name": "tibia",
+            "parent": "tibia_ref",
+            "transform_var_name": "tibia_in_tibia_ref"
+          },
+          {
+            "name": "camera_ref",
+            "parent": "tracker",
+            "transform_var_name": "camera_ref_in_tracker"
+          },
+          {
+            "name": "camera",
+            "parent": "camera_ref",
+            "transform_var_name": "camera_in_camera_ref"
+          },
+          {
+            "name": "mvs_ref",
+            "parent": "tracker",
+            "transform_var_name": "mvs_ref_in_tracker"
+          },
+          {
+            "name": "left_camera",
+            "parent": "mvs_ref",
+            "transform": [
+              33.2854,
+              -37.7917,
+              -24.4682,
+              0.6318,
+              0.3547,
+              0.4719,
+              -0.5023
+            ]
+          },
+          {
+            "name": "right_camera",
+            "parent": "mvs_ref",
+            "transform": [
+              37.3992,
+              -56.7108,
+              -85.8847,
+              0.6304,
+              0.3605,
+              0.4738,
+              -0.4982
+            ]
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "camera",
+            "observer": "tracker",
+            "transform_var_name": "camera_in_tracker"
+          },
+          {
+            "target": "left_camera",
+            "observer": "tracker",
+            "transform_var_name": "left_camera_in_tracker"
+          },
+          {
+            "target": "right_camera",
+            "observer": "tracker",
+            "transform_var_name": "right_camera_in_tracker"
+          },
+          {
+            "target": "femur",
+            "observer": "tracker",
+            "transform_var_name": "femur_in_tracker"
+          },
+          {
+            "target": "tibia",
+            "observer": "tracker",
+            "transform_var_name": "tibia_in_tracker"
+          },
+          {
+            "target": "probe",
+            "observer": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "target": "probe",
+            "observer": "femur_ref",
+            "transform_var_name": "probe_in_femur_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "tibia_ref",
+            "transform_var_name": "probe_in_tibia_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "femur",
+            "transform_var_name": "probe_in_femur"
+          },
+          {
+            "target": "probe",
+            "observer": "tibia",
+            "transform_var_name": "probe_in_tibia"
+          },
+          {
+            "target": "femur",
+            "observer": "camera_ref",
+            "transform_var_name": "femur_in_camera_ref"
+          },
+          {
+            "target": "tibia",
+            "observer": "camera_ref",
+            "transform_var_name": "tibia_in_camera_ref"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "/dev/ttyUSB0",
+        "tool_list": [
+          {
+            "rom_path": "/home/tpx/data/roms/GlassProbe_4Ball_4_SHC.rom",
+            "serial_number": "3DD50000",
+            "outputs": {
+              "transform": "probe_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_2_Endo.rom",
+            "outputs": {
+              "transform": "camera_ref_in_tracker",
+              "marker_uncertainty": "camera_ref_in_tracker_error"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_1_Camera_20240312.rom",
+            "outputs": {
+              "transform": "mvs_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_5_OldHead.rom",
+            "serial_number": "39B33001",
+            "outputs": {
+              "transform": "femur_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_6.rom",
+            "outputs": {
+              "transform": "tibia_ref_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "variable_validity_watcher",
+      "name": "probe_visibility_watcher",
+      "start_config": {
+        "variable_name": "probe_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "camera_visibility_watcher",
+      "start_config": {
+        "variable_name": "camera_ref_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "femur_visibility_watcher",
+      "start_config": {
+        "variable_name": "femur_ref_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "tibia_visibility_watcher",
+      "start_config": {
+        "variable_name": "tibia_ref_in_tracker"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_in_femur_ref_transformer",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_femur_ref",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "probe_tip_in_femur_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_in_tibia_ref_transformer",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_tibia_ref",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "probe_tip_in_tibia_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_in_femur_ref",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "probe_tip_in_femur_ref",
+        "output_var_name": "picked_point_in_femur_ref",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "probe_tip_in_femur_ref_transformer"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_in_tibia_ref",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "probe_tip_in_tibia_ref",
+        "output_var_name": "picked_point_in_tibia_ref",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "probe_tip_in_tibia_ref_transformer"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_femur_ref",
+      "start_config": {
+        "variable_name": "picked_point_in_femur_ref"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_tibia_ref",
+      "start_config": {
+        "variable_name": "picked_point_in_tibia_ref"
+      }
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "femur_registration_result_watcher",
+      "start_config": {
+        "variable_name": "femur_in_femur_ref"
+      }
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "tibia_registration_result_watcher",
+      "start_config": {
+        "variable_name": "tibia_in_tibia_ref"
+      }
+    },
+    {
+      "type": "kalman_denoiser",
+      "name": "camera_denoiser",
+      "init_config": {
+        "variable_in": "camera_in_tracker",
+        "variable_out": "camera_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "kalman_denoiser",
+      "name": "left_camera_denoiser",
+      "init_config": {
+        "variable_in": "left_camera_in_tracker",
+        "variable_out": "left_camera_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "kalman_denoiser",
+      "name": "right_camera_denoiser",
+      "init_config": {
+        "variable_in": "right_camera_in_tracker",
+        "variable_out": "right_camera_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "kalman_denoiser",
+      "name": "femur_denoiser",
+      "init_config": {
+        "variable_in": "femur_in_tracker",
+        "variable_out": "femur_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "kalman_denoiser",
+      "name": "tibia_denoiser",
+      "init_config": {
+        "variable_in": "tibia_in_tracker",
+        "variable_out": "tibia_in_tracker_denoised"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "tracker_all",
+      "dependencies": [
+        "transform_tree",
+        "ndi",
+        "probe_visibility_watcher",
+        "camera_visibility_watcher",
+        "femur_visibility_watcher",
+        "tibia_visibility_watcher",
+        "femur_denoiser",
+        "tibia_denoiser",
+        "left_camera_denoiser",
+        "right_camera_denoiser"
+      ]
+    }
+  ]
+}

src/image_process/camera_calibrator.h

@@ -8,6 +8,8 @@
 
 #include <opencv2/core/types.hpp>
 
+#include <glm/glm.hpp>
+
 #include <memory>
 
 class camera_calibrator {
@@ -20,6 +22,7 @@ public:
 
     struct result_type {
         camera_intrinsic_v0 cam_in;
+        glm::mat4 transform; // camera in camera reference
         int time_offset = 0;
     };
 

src/image_process/impl/camera_calibrator.cpp

@@ -249,7 +249,7 @@ namespace camera_calibrator_impl {
 
         auto rd = std::random_device();
         auto gen = std::mt19937(rd());
-        auto sampler = std::uniform_int_distribution<size_t>(0, sample_num);
+        auto sampler = std::uniform_int_distribution<size_t>(0, sample_num - 1);
         auto err_vec = std::vector<float>(sample_num);
 
         auto ret = camera_calib_result();
@@ -275,6 +275,14 @@ namespace camera_calibrator_impl {
         }
         assert(!ret.intrinsic.empty());
         SPDLOG_INFO("Camera calibration error is {:.2f}pix", ret_err);
+
+        // remove outlier samples
+        auto end_iter = std::ranges::remove_if(img_pool, [=, this](const auto &img) {
+            return reproject_error(ret, img.corner) > accept_threshold;
+        });
+        img_pool.erase(end_iter.begin(), end_iter.end());
+        sample_num = img_pool.size();
+
         return ret;
     }
 
@@ -337,7 +345,7 @@ namespace camera_calibrator_impl {
         // find minimal error using trichotomy algorithm
         auto offset_l = -128000; // -128ms
         auto offset_r = 0; // 0ms
-        static constexpr auto eps = 5000; // 5ms
+        static constexpr auto eps = 1000; // 1ms
         auto eval_func = [this](int offset) {
             ts_offset = offset;
             auto ret_mat = calib_hand_eye();
@@ -444,6 +452,7 @@ void camera_calibrator::impl::process() {
     }
     conf.cb_func(result_type{
             .cam_in = calib->intrinsic_v0(),
+            .transform = calib->result_mat,
             .time_offset = calib->result_ts_offset,
     });
 }

src/impl/apps/endo_guide/endo_guide.cpp

@@ -134,11 +134,28 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     monitor = std::make_unique<sophiar_monitor>(monitor_conf);
 
-    auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_b9.txt",
-            .img_size = cv::Size(1920, 1080),
-    };
-    cam_calib->simulate_process(sim_info);
+//    auto sim_info = camera_calibrator::simulate_info_type{
+//            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605/calib/calib_data_c5.txt",
+//            .img_size = cv::Size(1920, 1080),
+//    };
+//    cam_calib->simulate_process(sim_info);
+
+//    auto reg_conf = registration_config{
+//            .conn = sophiar_conn.get(),
+//            .probe_model_path = LOAD_STR("probe_model"),
+//    };
+//    reg.reset(registration::create(reg_conf));
+//    for (auto reg_item: LOAD_LIST("registration_list")) {
+//        auto item_conf = registration_target::from_yaml(reg_item);
+//        reg->add_target(item_conf);
+//    }
+//
+//    auto eval_conf = calib_eval::record_config{
+//            .img_name = img_color,
+//            .sophiar_conn = sophiar_conn.get(), .transform_var_name = "femur_in_camera_ref",
+//            .ctx = main_conf.asio_ctx,
+//    };
+//    eval_calib = std::make_unique<calib_eval>(eval_conf);
 }
 
 void app_endo_guide::show_ui() {
@@ -159,6 +176,8 @@ void app_endo_guide::show_ui() {
             if (ImGui::Button("Start")) {
                 start_tracking();
             }
+            ImGui::SameLine();
+            ImGui::Checkbox("Registration", &enable_reg);
             monitor->show();
             {
                 ImGui::SeparatorText("Scene");
@@ -182,10 +201,18 @@ void app_endo_guide::show_ui() {
                 out_saver->show();
                 ImGui::TreePop();
             }
-
+            if (ImGui::TreeNode("Evaluation")) {
+                eval_calib->show();
+                ImGui::TreePop();
+            }
         }
     }
     ImGui::End();
+
+//    if (enable_reg) {
+//        reg->process();
+//        reg->show();
+//    }
 }
 
 void app_endo_guide::render_background() {

src/impl/apps/endo_guide/endo_guide.h

@@ -5,11 +5,13 @@
 #include "device/mvs_camera_ui.h"
 #include "device/uvc_camera_ui.h"
 #include "module/augment_manager_v2.h"
+#include "module/experiment/calib_eval.h"
 #include "module/guidance/acl_guide.h"
 #include "module/image_augment_helper_v2.h"
 #include "module/image_saver.h"
 #include "module/image_viewer.h"
 #include "module/sophiar_monitor.h"
+#include "module_v3/registration.h"
 #include "impl/app_base.h"
 #include "image_process/camera_calibrator.h"
 
@@ -68,6 +70,11 @@ private:
         std::unique_ptr<image_augment_helper_v2> aug_helper;
         smart_cuda_stream stream;
     } left_cam, right_cam;
+
+    bool enable_reg = false;
+    std::unique_ptr<registration> reg;
+
+    std::unique_ptr<calib_eval> eval_calib;
 };
 
 #endif //DEPTHGUIDE_ENDO_GUIDE_H

src/module/experiment/calib_eval.h

@@ -0,0 +1,30 @@
+#ifndef DEPTHGUIDE_CALIB_EVAL_H
+#define DEPTHGUIDE_CALIB_EVAL_H
+
+#include "core/object_manager.h"
+#include "core/local_connection.h" // sophiar
+
+#include <memory>
+
+class calib_eval {
+public:
+
+    struct record_config {
+        obj_name_type img_name;
+        sophiar_conn_type *sophiar_conn;
+        std::string transform_var_name; // model in camera_ref
+        io_ctx_type *ctx = nullptr;
+    };
+
+    explicit calib_eval(const record_config &rec);
+
+    ~calib_eval();
+
+    void show();
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+#endif //DEPTHGUIDE_CALIB_EVAL_H

src/module/experiment/impl/calib_eval.cpp

@@ -0,0 +1,99 @@
+#include "calib_eval_impl.h"
+#include "core/math_helper.hpp"
+#include "core/imgui_utility.hpp"
+
+#include <opencv2/imgcodecs.hpp>
+
+#include <fstream>
+
+calib_eval::impl::~impl() {
+    if (img_conn.connected()) {
+        stop();
+    }
+}
+
+void calib_eval::impl::start() {
+    auto dir_name = fmt::format("save_{}", current_timestamp());
+    save_dir = std::filesystem::current_path() / dir_name;
+    std::filesystem::create_directories(save_dir);
+    img_count = 0;
+    img_conn = OBJ_SIG(rec_conf.img_name)->connect(
+            [this](auto name) { img_callback(name); });
+}
+
+void calib_eval::impl::img_callback(obj_name_type name) {
+    assert(name == rec_conf.img_name);
+
+    // tracking information
+    auto cur_ts = OBJ_TS(name);
+    auto ref_t = rec_conf.sophiar_conn
+            ->query_transform_variable(rec_conf.transform_var_name);
+    if (!ref_t.has_value()) return;
+    auto &item = track_list.emplace_back();
+    item.ref_mat = to_mat4(*ref_t);
+    item.sample_ts = cur_ts;
+
+    // compress and save image
+    auto img = to_image(name);
+    if (img == nullptr) return;
+    auto img_name = fmt::format("img_{}.jpg", cur_ts);
+    auto img_path = save_dir / img_name;
+    auto img_cv = img->cv_mat(&stream);
+    CUDA_API_CHECK(cudaStreamSynchronize(stream.cuda));
+    cv::imwrite(img_path.string(), img_cv);
+    ++img_count;
+}
+
+void calib_eval::impl::save_track_data() {
+    auto file_path = save_dir / "track_data.txt";
+    auto fout = std::ofstream(file_path);
+    fout << track_list.size() << std::endl;
+    for (auto &item: track_list) {
+        fout << item.sample_ts << " ";
+
+        using mat_type = typeof(item.ref_mat);
+        using value_type = mat_type::value_type;
+        static constexpr auto len = sizeof(mat_type) / sizeof(value_type);
+
+        auto ptr = (value_type *) &item.ref_mat;
+        for (auto i = 0; i < len; ++i) {
+            fout << ptr[i] << " ";
+        }
+        fout << std::endl;
+    }
+    fout << std::endl;
+}
+
+void calib_eval::impl::stop() {
+    assert(img_conn.connected());
+    img_conn.disconnect();
+    save_track_data();
+}
+
+void calib_eval::impl::show() {
+    using boost::asio::post;
+    auto is_running = img_conn.connected();
+    if (!is_running) {
+        if (ImGui::Button("Start")) {
+            post(*rec_conf.ctx, [this] { start(); });
+        }
+    } else {
+        if (ImGui::Button("Stop")) {
+            post(*rec_conf.ctx, [this] { stop(); });
+        }
+    }
+    if (is_running) {
+        ImGui::Text("Image Count: %ld", (size_t) img_count);
+    }
+}
+
+calib_eval::calib_eval(const record_config &rec)
+        : pimpl(std::make_unique<impl>()) {
+    pimpl->rec_conf = rec;
+}
+
+calib_eval::~calib_eval() = default;
+
+void calib_eval::show() {
+    pimpl->show();
+}

src/module/experiment/impl/calib_eval_impl.h

@@ -0,0 +1,41 @@
+#ifndef DEPTHGUIDE_CALIB_EVAL_IMPL_H
+#define DEPTHGUIDE_CALIB_EVAL_IMPL_H
+
+#include "module/experiment/calib_eval.h"
+#include "core/image_utility_v2.h"
+
+#include <glm/glm.hpp>
+
+struct calib_eval::impl {
+
+    record_config rec_conf;
+    obj_conn_type img_conn;
+
+    struct track_store_type {
+        glm::mat4 ref_mat;
+        timestamp_type sample_ts;
+    };
+
+    using track_list_type =
+            std::vector<track_store_type>;
+    track_list_type track_list;
+
+    std::filesystem::path save_dir;
+    smart_cuda_stream stream;
+    std::atomic_size_t img_count = 0;
+
+    ~impl();
+
+    void start();
+
+    void img_callback(obj_name_type name);
+
+    void save_track_data();
+
+    void stop();
+
+    void show();
+
+};
+
+#endif //DEPTHGUIDE_CALIB_EVAL_IMPL_H

src/module/guidance/acl_guide.h

@@ -17,6 +17,7 @@ public:
             std::string collect_obj_name;
         } femur, tibia;
 
+        std::string drill_transform_var_name;
         sophiar_conn_type *sophiar_conn = nullptr;
         io_ctx_type *ctx = nullptr;
 

src/module/guidance/impl/acl_guide.cpp

@@ -42,10 +42,12 @@ struct acl_guide::impl {
     int cur_index = ACL_EMPTY, next_index = cur_index;
     item_store_type *cur_item = nullptr;
 
-    glm::vec3 color;
+    glm::vec3 line_color = glm::vec3(1, 0, 0);
+    glm::vec3 drill_color = glm::vec3(0, 1, 0);
     GLfloat line_width = 5.f; // pix
     GLfloat point_size = 2.f; // mm
     ogl_range_info lw_info;
+    GLfloat drill_pos = 100.f, drill_neg = -10.f;
 
     impl(const create_config &_conf) {
         conf = _conf;
@@ -127,7 +129,15 @@ struct acl_guide::impl {
             ImGui::DragFloat("Line Width", &line_width, lw_info.delta,
                              lw_info.min, lw_info.max, "%.01f");
             ImGui::DragFloat("Point Radius", &point_size, 0.1, 0.1, 10, "%.1f");
-            ImGui::ColorEdit3("Color", glm::value_ptr(color));
+            ImGui::ColorEdit3("Line Color", glm::value_ptr(line_color));
+
+            ImGui::PushItemWidth(75);
+            ImGui::DragFloat("Drill(+)", &drill_pos);
+            ImGui::SameLine();
+            ImGui::DragFloat("Drill(-)", &drill_neg);
+            ImGui::PopItemWidth();
+            ImGui::ColorEdit3("Drill Color", glm::value_ptr(drill_color));
+
             process();
         }
     }
@@ -138,7 +148,7 @@ struct acl_guide::impl {
         auto add_point = [&](glm::vec3 p) {
             auto item_info = scene_render_info::mesh_info{
                     .mesh = get_sphere_symbol(point_size),
-                    .material = {.ambient = glm::vec3(), .diffuse = color,},
+                    .material = {.ambient = glm::vec3(), .diffuse = line_color,},
             };
             info->items.push_back(
                     {.info = item_info, .transform = glm::translate(p),});
@@ -163,11 +173,23 @@ struct acl_guide::impl {
             auto r2 = p2 - extra_length * dir;
             auto item_info = scene_render_info::line_info{
                     .p1 = r1, .p2 = r2,
-                    .color = color, .line_width = line_width,
+                    .color = line_color, .line_width = line_width,
             };
             info->items.push_back(
                     {.info = item_info, .transform = glm::mat4(1.f),});
         }
+
+        // show drill
+        if (auto trans = conf.sophiar_conn
+                    ->query_transform_variable(conf.drill_transform_var_name);
+                trans.has_value()) {
+            auto item_info = scene_render_info::line_info{
+                    .p1 = glm::vec3(0, 0, drill_neg), .p2 = glm::vec3(0, 0, drill_pos),
+                    .color = drill_color, .line_width = line_width,
+            };
+            info->items.push_back(
+                    {.info = item_info, .transform = to_mat4(*trans),});
+        }
     }
 
 };
@@ -182,6 +204,7 @@ void acl_guide::create_config::fill_from_yaml(const YAML::Node &conf) {
     };
     femur = info_from_yaml(LOAD_SUB("femur"));
     tibia = info_from_yaml(LOAD_SUB("tibia"));
+    drill_transform_var_name = LOAD_STR("drill_transform_var");
 }
 
 acl_guide::acl_guide(const create_config &conf)

src/render/impl/render_mesh.cpp

@@ -35,7 +35,8 @@ auto mesh_type::impl::create(const mesh_type::create_config &conf)
 -> std::unique_ptr<impl> {
     auto ret = std::make_unique<impl>();
     auto scene = ret->importer.ReadFile(
-            conf.path, aiProcessPreset_TargetRealtime_MaxQuality);
+            conf.path, aiProcessPreset_TargetRealtime_MaxQuality
+                       ^ aiProcess_SplitLargeMeshes); // do not split meshes
     if (scene == nullptr) {
         RET_ERROR_E;
     }

src/render/impl/render_scene.cpp

@@ -82,7 +82,9 @@ namespace render_scene_impl {
         using info_type = scene_render_info::line_info;
         auto info = std::get<info_type>(req.item.info);
 
-        auto t_mat = req.camera.project * req.camera.transform;
+        auto t_mat = req.camera.project
+                     * req.camera.transform
+                     * req.item.transform;
         auto ren_conf = line_render_info{
                 .p1 = transform_p(t_mat, info.p1),
                 .p2 = transform_p(t_mat, info.p2),