Преглед на файлове

Implemented TKA femur registration.

jcsyshc преди 2 години
родител
ревизия
0f30998471
променени са 3 файла, в които са добавени 499 реда и са изтрити 3 реда
  1. 3 3
      app/data/pose_collect.json
  2. 495 0
      app/data/tka_ar_navigation.json
  3. 1 0
      src/core/external_variable_io.cpp

+ 3 - 3
app/data/pose_collect.json

@@ -15,7 +15,7 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "serial",
+        "address_type": "ethernet",
         "ip": "10.0.0.5",
         "tcp_port": 8765,
         "com_port": "/dev/ttyUSB0",
@@ -47,8 +47,8 @@
         "stable_type": "transform",
         "input_var_name": "target_in_tracker",
         "output_var_name": "collect_result",
-        "linear_tolerance_mm": 0.05,
-        "temporal_interval_s": 3
+        "linear_tolerance_mm": 0.01,
+        "temporal_interval_s": 10
       },
       "dependencies": [
         "ndi"

+ 495 - 0
app/data/tka_ar_navigation.json

@@ -0,0 +1,495 @@
+{
+  "variable_list": [
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "femur_in_femur_ref",
+      "type": "transform_obj",
+      "value": [
+        49.7073,
+        -72.6173,
+        -9.09289,
+        0.440928,
+        0.542853,
+        0.490098,
+        0.520286
+      ]
+    },
+    {
+      "name": "tibia_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tibia_in_tibia_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "camera_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": "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": "femur_in_femur_ref_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "tibia_in_tibia_ref_error",
+      "type": "double_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"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe_ref",
+            "parent": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "probe_ref",
+            "transform": [
+              -0.23,
+              -13.98,
+              -119.65,
+              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": "left_camera",
+            "parent": "camera_ref",
+            "transform": [
+              -11.3375,
+              21.2730,
+              -37.5694,
+              0.0009,
+              -0.7088,
+              -0.0091,
+              0.7053
+            ]
+          },
+          {
+            "name": "right_camera",
+            "parent": "camera_ref",
+            "transform": [
+              -11.2100,
+              22.1807,
+              -97.3032,
+              0.0011,
+              0.7066,
+              0.0120,
+              -0.7075
+            ]
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "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": "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"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "/home/tpx/data/roms/GlassProbe_4Ball_4.rom",
+            "outputs": {
+              "transform": "probe_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_1.rom",
+            "outputs": {
+              "transform": "camera_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_6.rom",
+            "outputs": {
+              "transform": "femur_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_5.rom",
+            "outputs": {
+              "transform": "tibia_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": "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": "landmark_registration",
+      "name": "femur_landmark",
+      "start_config": {
+        "fiducial_points": [
+          [
+            1.68412,
+            22.5766,
+            202.831
+          ],
+          [
+            13.5083,
+            -24.1831,
+            200.386
+          ],
+          [
+            26.621,
+            2.0151,
+            191.341
+          ]
+        ],
+        "point_var_name": "picked_point_in_femur_ref",
+        "transform_var_name": "femur_in_femur_ref",
+        "error_var_name": "femur_in_femur_ref_error"
+      },
+      "dependencies": [
+        "point_picker_in_femur_ref"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "femur_registration_result_watcher",
+      "start_config": {
+        "variable_name": "femur_in_femur_ref"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "femur_registration_error_watcher",
+      "start_config": {
+        "variable_name": "femur_in_femur_ref_error"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_in_femur_transformer",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_femur",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "probe_tip_in_femur"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_in_femur",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "probe_tip_in_femur",
+        "output_var_name": "picked_point_in_femur",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "probe_tip_in_femur_transformer"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_femur",
+      "start_config": {
+        "variable_name": "picked_point_in_femur"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "tracker_all",
+      "dependencies": [
+        "transform_tree",
+        "ndi",
+        "probe_visibility_watcher",
+        "camera_visibility_watcher",
+        "femur_visibility_watcher",
+        "tibia_visibility_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "femur_landmark_all",
+      "dependencies": [
+        "tracker_all",
+        "femur_landmark",
+        "picked_point_watcher_for_femur_ref",
+        "femur_registration_result_watcher",
+        "femur_registration_error_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "femur_icp_all",
+      "dependencies": [
+        "tracker_all",
+        "point_picker_in_femur",
+        "picked_point_watcher_for_femur"
+      ]
+    }
+  ]
+}

+ 1 - 0
src/core/external_variable_io.cpp

@@ -243,6 +243,7 @@ namespace sophiar {
 
             // fill message size
             auto length = buf->size() - header_offset;
+            if (length == 0) co_return true; // avoid sending empty package
             assert(length <= std::numeric_limits<header_type>::max());
             write_binary_value<sophiar_endian>(buf->data(), (header_type) length);