Added stereo output for endo guide.

data/config_endo_guide.yaml

@@ -30,6 +30,22 @@ augment_list:
     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/project/DepthGuide/data/model/Femur_3.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"
 
@@ -56,6 +72,9 @@ right_camera_info:
 left_camera_transform_var: left_camera_in_tracker_denoised
 right_camera_transform_var: right_camera_in_tracker_denoised
 
+output_width: 1920
+output_height: 1080
+
 acl_guide:
   femur:
     collect_obj: point_picker_in_femur_ref

src/impl/apps/endo_guide/endo_guide.cpp

@@ -18,10 +18,10 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     OBJ_SAVE(right_out, image_ptr());
 
     OBJ_SIG(left_raw)->connect([this](auto _) {
-        debayer_raw::call({left_raw, left_rgb, &left_cam.stream});
+        debayer_raw::call({left_raw, left_rgb, default_cuda_stream});
     });
     OBJ_SIG(right_raw)->connect([this](auto _) {
-        debayer_raw::call({right_raw, right_rgb, &right_cam.stream});
+        debayer_raw::call({right_raw, right_rgb, default_cuda_stream});
     });
 
     // initialize sophiar
@@ -43,9 +43,9 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     mvs_conf.cameras.push_back({.dev_name = LOAD_STR("left_camera_name"), .img_name = left_raw});
     mvs_conf.cameras.push_back({.dev_name = LOAD_STR("right_camera_name"), .img_name = right_raw});
     mvs_cam = std::make_unique<mvs_camera_ui>(mvs_conf);
-//    mvs_cam->cap_info_sig.connect([this](auto info) {
-//        out_streamer->change_frame_rate(info.frame_rate);
-//    });
+    mvs_cam->cap_info_sig.connect([this](auto info) {
+        out_streamer->change_frame_rate(info.frame_rate);
+    });
 
     auto bg_viewer_conf = image_viewer::create_config{
             .mode = VIEW_STEREO, .flip_y = true,
@@ -97,7 +97,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     auto helper_conf = image_augment_helper_v2::create_config{
             .img_name = left_rgb, .mask_name = invalid_obj_name,
             .out_name = left_out, .img_flip_y = false,
-            .manager = aug_manager.get(), .stream = &left_cam.stream,
+            .manager = aug_manager.get(), .stream = default_cuda_stream,
             .ctx = main_conf.asio_ctx,
             .sophiar_conn = sophiar_conn.get(),
             .transform_var = LOAD_STR("left_camera_transform_var"),
@@ -109,7 +109,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     helper_conf = image_augment_helper_v2::create_config{
             .img_name = right_rgb, .mask_name = invalid_obj_name,
             .out_name = right_out, .img_flip_y = false,
-            .manager = aug_manager.get(), .stream = &right_cam.stream,
+            .manager = aug_manager.get(), .stream = default_cuda_stream,
             .ctx = main_conf.asio_ctx,
             .sophiar_conn = sophiar_conn.get(),
             .transform_var = LOAD_STR("right_camera_transform_var"),
@@ -135,22 +135,41 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     monitor = std::make_unique<sophiar_monitor>(monitor_conf);
 
+    auto output_size = cv::Size(
+            LOAD_NUMBER(int, "output_width"),
+            LOAD_NUMBER(int, "output_height"));
+    auto stereo_aug_conf = stereo_augment_helper::create_config{
+            .left_name = left_out, .right_name = right_out,
+            .out_name = stereo_out, .fbo_size = output_size,
+            .flip_image = false, .stream = default_cuda_stream
+    };
+    stereo_aug = std::make_unique<stereo_augment_helper>(stereo_aug_conf);
+
+    auto streamer_conf = image_streamer::create_config{
+            .img_name = stereo_out, .asio_ctx = main_conf.asio_ctx,
+            .cuda_ctx = main_conf.cuda_ctx, .stream = default_cuda_stream
+    };
+    out_streamer = std::make_unique<image_streamer>(streamer_conf);
+    out_streamer->sig_req_size.connect([this](auto size) {
+        post(*main_conf.asio_ctx, [=, this] { stereo_aug->resize(size); });
+    });
+
     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 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",
@@ -173,25 +192,35 @@ void app_endo_guide::show_ui() {
             mvs_cam->show();
         }
 
-        if (ImGui::CollapsingHeader("Tracker")) {
+        if (ImGui::CollapsingHeader("Augmentation")) {
+            auto guard = imgui_id_guard("augmentation");
+
+            ImGui::SeparatorText("Actions");
             if (ImGui::Button("Start")) {
                 start_tracking();
             }
             ImGui::SameLine();
             ImGui::Checkbox("Registration", &enable_reg);
+
+            ImGui::SeparatorText("Status");
             monitor->show();
+
+            ImGui::SeparatorText("Scene");
             {
-                ImGui::SeparatorText("Scene");
                 auto id_guard = imgui_id_guard("augment_scene");
                 aug_manager->show();
-                if (ImGui::TreeNode("Endo Camera")) {
-                    aug_helper->get_camera_helper()->show();
-                    ImGui::TreePop();
-                }
-                if (ImGui::TreeNode("MVS Camera")) {
-                    left_cam.aug_helper->get_camera_helper()->show();
-                    ImGui::TreePop();
-                }
+            }
+
+            ImGui::SeparatorText("Camera");
+            auto helper = aug_helper->get_camera_helper();
+            if (helper != nullptr && ImGui::TreeNode("Endo Camera")) {
+                helper->show();
+                ImGui::TreePop();
+            }
+            helper = left_cam.aug_helper->get_camera_helper();
+            if (helper != nullptr && ImGui::TreeNode("MVS Camera")) {
+                helper->show();
+                ImGui::TreePop();
             }
         }
 
@@ -205,6 +234,18 @@ void app_endo_guide::show_ui() {
             acl->show();
         }
 
+        if (ImGui::CollapsingHeader("Streamer")) {
+            {
+                ImGui::SeparatorText("Output");
+                auto id_guard = imgui_id_guard("stereo");
+                stereo_aug->show();
+            }
+            {
+                auto id_guard = imgui_id_guard("streamer");
+                out_streamer->show();
+            }
+        }
+
         if (ImGui::CollapsingHeader("Debug")) {
             if (ImGui::TreeNode("Background")) {
                 bg_viewer->show();
@@ -222,10 +263,10 @@ void app_endo_guide::show_ui() {
     }
     ImGui::End();
 
-//    if (enable_reg) {
-//        reg->process();
-//        reg->show();
-//    }
+    if (enable_reg) {
+        reg->process();
+        reg->show();
+    }
 }
 
 void app_endo_guide::render_background() {

src/impl/apps/endo_guide/endo_guide.h

@@ -7,8 +7,10 @@
 #include "module/augment_manager_v2.h"
 #include "module/experiment/calib_eval.h"
 #include "module/guidance/acl_guide.h"
+#include "module/image_augment_helper.h"
 #include "module/image_augment_helper_v2.h"
 #include "module/image_saver.h"
+#include "module/image_streamer.h"
 #include "module/image_viewer.h"
 #include "module/sophiar_monitor.h"
 #include "module_v3/registration.h"
@@ -43,6 +45,7 @@ private:
         left_raw, right_raw,
         left_rgb, right_rgb,
         left_out, right_out,
+        stereo_out,
 
         name_end,
     };
@@ -64,11 +67,13 @@ private:
     std::unique_ptr<image_saver> out_saver;
     std::unique_ptr<sophiar_monitor> monitor;
     std::unique_ptr<acl_guide> acl;
+    std::unique_ptr<stereo_augment_helper> stereo_aug;
+    std::unique_ptr<image_streamer> out_streamer;
 
     std::unique_ptr<mvs_camera_ui> mvs_cam;
     struct {
         std::unique_ptr<image_augment_helper_v2> aug_helper;
-        smart_cuda_stream stream;
+//        smart_cuda_stream stream;
     } left_cam, right_cam;
 
     bool enable_reg = false;

src/module/guidance/impl/acl_guide.cpp

@@ -24,6 +24,7 @@ struct acl_guide::impl {
     using item_base_type = create_config::item_info;
 
     struct item_store_type : public item_base_type {
+        bool allow_ui_edit = false;
         std::optional<glm::vec3> keypoint;
 
         std::optional<glm::vec3> world_point(impl *i) {
@@ -138,6 +139,7 @@ struct acl_guide::impl {
     }
 
     void show() {
+        ImGui::SeparatorText("Actions");
         if (!is_running) {
             if (ImGui::Button("Start")) {
                 post(*conf.ctx, [this] { start(); });
@@ -167,6 +169,8 @@ struct acl_guide::impl {
                 auto guard = imgui_valid_style_guard(tibia.keypoint.has_value());
                 ImGui::RadioButton("Tibia", &next_index, ACL_TIBIA);
             }
+
+            ImGui::SeparatorText("Configs");
             ImGui::DragFloat("Line Radius", &line_radius, 0.1, 0.1, 5., "%.1f");
             ImGui::DragFloat("Point Radius", &point_radius, 0.1, 0.1, 5., "%.1f");
             ImGui::ColorEdit3("Line Color", glm::value_ptr(line_color));
@@ -178,6 +182,26 @@ struct acl_guide::impl {
             ImGui::PopItemWidth();
             ImGui::ColorEdit3("Drill Color", glm::value_ptr(drill_color));
 
+            ImGui::SeparatorText("Info");
+            if (femur.keypoint.has_value()) {
+                auto &val = *femur.keypoint;
+                {
+                    auto guard = imgui_disable_guard(!femur.allow_ui_edit);
+                    ImGui::DragFloat3("Femur Keypoint", glm::value_ptr(val), 0.1f);
+                }
+                ImGui::SameLine();
+                ImGui::Checkbox("##femur_keypoint_edit", &femur.allow_ui_edit);
+            }
+            if (tibia.keypoint.has_value()) {
+                auto &val = *tibia.keypoint;
+                {
+                    auto guard = imgui_disable_guard(!tibia.allow_ui_edit);
+                    ImGui::DragFloat3("Tibia Keypoint", glm::value_ptr(val), 0.1f);
+                }
+                ImGui::SameLine();
+                ImGui::Checkbox("##tibia_keypoint_edit", &tibia.allow_ui_edit);
+            }
+
             process();
         }
     }

src/module/impl/camera_augment_helper_v2.cpp

@@ -82,7 +82,7 @@ void camera_augment_helper_v2::impl::freedom_info_type::pitch(float deg) {
 }
 
 glm::mat4 camera_augment_helper_v2::impl::fixed_info_type::extra_transform() const {
-    return to_transform_mat(extra_offset, extra_rotation);
+    return to_transform_mat(extra_offset, glm::radians(extra_rotation));
 }
 
 camera_augment_helper_v2::impl::impl(const create_config &conf) {

src/module/impl/camera_augment_helper_v2_impl.h

@@ -37,7 +37,7 @@ struct camera_augment_helper_v2::impl {
 
     struct fixed_info_type {
         glm::vec3 extra_offset = glm::vec3();
-        glm::vec3 extra_rotation = glm::vec3();
+        glm::vec3 extra_rotation = glm::vec3(); // in degree
 
         glm::mat4 extra_transform() const;