Implemented ACL Guidance.

CMakeLists.txt

@@ -35,7 +35,9 @@ 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/guidance/impl/acl_guide.cpp
         src/module/guidance/impl/cursor_guide.cpp
+        src/module/guidance/impl/guide_helper.cpp
         src/module/impl/augment_manager.cpp
         src/module/impl/augment_manager_v2.cpp
         src/module/impl/camera_augment_helper.cpp
@@ -60,6 +62,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         #        src/network/impl/multiplexer.cpp
         #        src/network/impl/network.cpp
         src/render_v3/scene_render.cpp
+        src/render/impl/render_line.cpp
         src/render/impl/render_mesh.cpp
         src/render/impl/render_pc.cpp
         src/render/impl/render_scene.cpp

data/config_endo_guide.yaml

@@ -11,10 +11,14 @@ 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
@@ -22,4 +26,40 @@ augment_list:
     model: /home/tpx/project/DepthGuide/data/model/Femur_3.stl
   - name: Tibia
     transform_var: tibia_in_tracker_denoised
-    model: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl
+    model: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl
+
+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

@@ -58,6 +58,18 @@
       "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"
@@ -114,6 +126,14 @@
       "name": "picked_point_in_tibia",
       "type": "scalarxyz_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"
@@ -179,6 +199,37 @@
             "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
+            ]
           }
         ]
       },
@@ -189,6 +240,16 @@
             "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",
@@ -250,6 +311,12 @@
               "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.rom",
             "serial_number": "39B33001",
@@ -390,6 +457,22 @@
         "variable_name": "tibia_in_tibia_ref"
       }
     },
+    {
+      "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",
@@ -417,7 +500,9 @@
         "femur_visibility_watcher",
         "tibia_visibility_watcher",
         "femur_denoiser",
-        "tibia_denoiser"
+        "tibia_denoiser",
+        "left_camera_denoiser",
+        "right_camera_denoiser"
       ]
     }
   ]

src/codec/scene_encoder.cpp

@@ -26,6 +26,7 @@ namespace scene_encoder_impl {
     using mesh_info = scene_render_info::mesh_info;
     using pc_info = scene_render_info::pc_info;
     using custom_info = scene_render_info::custom_info;
+    using line_info = scene_render_info::line_info;
 
     json info_to_json(const std::monostate &) {
         RET_ERROR_E;
@@ -69,6 +70,10 @@ namespace scene_encoder_impl {
         return ret;
     }
 
+    json info_to_json(const line_info &info) {
+        RET_ERROR_E; // TODO: not implemented
+    }
+
     using light_info = scene_render_info::light_info;
 
     json info_to_json(const light_info &info) {
@@ -108,6 +113,10 @@ struct scene_encoder::impl {
         return conf.pc_enc->encode(info.pc);
     }
 
+    data_type encode_info(const line_info &info) {
+        RET_ERROR_E; // TODO: not implemented
+    }
+
     data_type encode(const scene_ptr &info) {
         auto extra_writer = network_writer();
 

src/core/impl/image_utility_v2.cpp

@@ -201,7 +201,7 @@ cv::Mat generic_image::impl::get_cv_mat(smart_cuda_stream *stream) {
 
 cv::cuda::GpuMat generic_image::impl::get_cv_gpumat(smart_cuda_stream *stream) {
     create_cuda(stream);
-    return cv::cuda::GpuMat(size, type, store_host.ptr.get(), store_host.pitch);
+    return cv::cuda::GpuMat(size, type, store_cuda.ptr.get(), store_cuda.pitch);
 }
 
 template<typename T>

src/core/math_helper.hpp

@@ -79,6 +79,12 @@ inline auto to_vec3(const T &v) {
     return glm::vec3(v.x, v.y, v.z);
 }
 
+template<typename EigenType>
+glm::vec3 to_vec3(const Eigen::MatrixBase<EigenType> &v) {
+    static_assert(EigenType::SizeAtCompileTime == 3);
+    return glm::vec3(v.x(), v.y(), v.z());
+}
+
 inline auto to_homo(const glm::vec3 &v) {
     return glm::vec4(v, 1.f);
 }

src/image_process/camera_utility.hpp

@@ -59,7 +59,7 @@ struct camera_intrinsic {
     camera_intrinsic_v0 to_v0() {
         auto ret = camera_intrinsic_v0{
                 .fx = fx, .fy = fy, .cx = cx, .cy=cy,
-                .dist = {k[0], k[1]},
+                .dist = {k[0], k[1], 0.f, 0.f}, // p1 = 0, p2 = 0
                 .width =width, .height =height,
         };
         return ret;

src/image_process/impl/process_funcs.cpp

@@ -5,6 +5,7 @@
 #include "image_process/cuda_impl/pc_filter.cuh"
 
 #include <opencv2/calib3d.hpp>
+#include <opencv2/cudaimgproc.hpp>
 
 namespace gen_pc_rgb_with_mask {
 
@@ -288,4 +289,20 @@ namespace shrink_pc_zero_depth {
         *conf.out = pc_out;
     }
 
+}
+
+namespace debayer_raw {
+
+    void call(config conf) {
+        auto raw = to_image(conf.in_name);
+        if (raw == nullptr) return;
+        assert(raw->cv_type() == CV_8UC1);
+        auto rgb = create_image(raw->size(), CV_8UC3);
+        cv::cuda::cvtColor(raw->cv_gpumat(conf.stream),
+                           rgb->cv_gpumat(conf.stream),
+                           cv::COLOR_BayerRG2BGR, 3, conf.stream->cv);
+        rgb->cuda_modified(conf.stream);
+        OBJ_SAVE(conf.out_name, rgb);
+    }
+
 }

src/image_process/process_funcs.h

@@ -104,5 +104,16 @@ namespace shrink_pc_zero_depth {
 
 }
 
+namespace debayer_raw {
+
+    struct config {
+        obj_name_type in_name;
+        obj_name_type out_name;
+        smart_cuda_stream *stream;
+    };
+
+    void call(config conf);
+
+}
 
 #endif //DEPTHGUIDE_PROCESS_FUNCS_H

src/impl/apps/endo_guide/endo_guide.cpp

@@ -1,6 +1,7 @@
 #include "endo_guide.h"
 #include "core/imgui_utility.hpp"
 #include "module/camera_augment_helper_v2.h"
+#include "image_process/process_funcs.h"
 
 app_endo_guide::app_endo_guide(const create_config &_conf) {
     main_conf = _conf;
@@ -9,6 +10,19 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     // initialize object manager
     OBJ_SAVE(img_color, image_u8c3());
     OBJ_SAVE(img_out, image_ptr());
+    OBJ_SAVE(left_raw, image_u8c1());
+    OBJ_SAVE(left_rgb, image_ptr());
+    OBJ_SAVE(left_out, image_ptr());
+    OBJ_SAVE(right_raw, image_u8c1());
+    OBJ_SAVE(right_rgb, image_ptr());
+    OBJ_SAVE(right_out, image_ptr());
+
+    OBJ_SIG(left_raw)->connect([this](auto _) {
+        debayer_raw::call({left_raw, left_rgb, &left_cam.stream});
+    });
+    OBJ_SIG(right_raw)->connect([this](auto _) {
+        debayer_raw::call({right_raw, right_rgb, &right_cam.stream});
+    });
 
     // initialize sophiar
     sophiar_thread = std::make_unique<std::thread>([conf_path = LOAD_STR("sophiar_config")] {
@@ -25,13 +39,21 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     uvc_cam = std::make_unique<uvc_camera_ui>(uvc_cam_conf);
 
+    auto mvs_conf = mvs_camera_ui::create_config{.ctx = main_conf.asio_ctx};
+    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);
+//    });
+
     auto bg_viewer_conf = image_viewer::create_config{
             .mode = VIEW_COLOR_ONLY, .flip_y = true,
             .stream = default_cuda_stream,
     };
     auto &bg_extra_conf = bg_viewer_conf.extra.color;
     bg_extra_conf.fmt = COLOR_RGB;
-    bg_extra_conf.name = img_out;
+    bg_extra_conf.name = left_out;
     bg_viewer = std::make_unique<image_viewer>(bg_viewer_conf);
 
     auto saver_conf = image_saver::create_config{.ctx = main_conf.asio_ctx};
@@ -55,6 +77,11 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     cam_calib = std::make_unique<camera_calibrator>(calib_conf);
 
+    auto acl_conf = acl_guide::create_config::from_yaml(LOAD_SUB("acl_guide"));
+    acl_conf.sophiar_conn = sophiar_conn.get();
+    acl_conf.ctx = main_conf.asio_ctx;
+    acl = std::make_unique<acl_guide>(acl_conf);
+
     auto aug_list_v1 = augment_manager::item_list_from_yaml(LOAD_LIST("augment_list"));
     auto aug_list = augment_manager_v2::item_list_from_v1(aug_list_v1, extra_name);
     auto aug_conf = augment_manager_v2::create_config{
@@ -63,8 +90,34 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     std::copy(aug_list.begin(), aug_list.end(), std::back_inserter(aug_conf.item_list));
     aug_manager = std::make_unique<augment_manager_v2>(aug_conf);
+    aug_manager->pre_render_sig.connect(
+            [this](const auto &info) { acl->pre_render_slot(info); });
 
     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,
+            .ctx = main_conf.asio_ctx,
+            .sophiar_conn = sophiar_conn.get(),
+            .transform_var = LOAD_STR("left_camera_transform_var"),
+    };
+    left_cam.aug_helper = std::make_unique<image_augment_helper_v2>(helper_conf);
+    left_cam.aug_helper->set_camera_info(
+            camera_intrinsic::from_yaml(LOAD_SUB("left_camera_info")).to_v0());
+
+    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,
+            .ctx = main_conf.asio_ctx,
+            .sophiar_conn = sophiar_conn.get(),
+            .transform_var = LOAD_STR("right_camera_transform_var"),
+    };
+    right_cam.aug_helper = std::make_unique<image_augment_helper_v2>(helper_conf);
+    right_cam.aug_helper->set_camera_info(
+            camera_intrinsic::from_yaml(LOAD_SUB("right_camera_info")).to_v0());
+
+    helper_conf = image_augment_helper_v2::create_config{
             .img_name = img_color, .mask_name = invalid_obj_name,
             .out_name = img_out, .img_flip_y = false,
             .manager = aug_manager.get(), .stream = default_cuda_stream,
@@ -97,6 +150,11 @@ void app_endo_guide::show_ui() {
             uvc_cam->show();
         }
 
+        if (ImGui::CollapsingHeader("MVS Camera")) {
+            auto id_guard = imgui_id_guard("mvs_camera");
+            mvs_cam->show();
+        }
+
         if (ImGui::CollapsingHeader("Tracker")) {
             if (ImGui::Button("Start")) {
                 start_tracking();
@@ -114,6 +172,11 @@ void app_endo_guide::show_ui() {
             cam_calib->show();
         }
 
+        if (ImGui::CollapsingHeader("Guidance")) {
+            auto id_guard = imgui_id_guard("Guidance");
+            acl->show();
+        }
+
         if (ImGui::CollapsingHeader("Debug")) {
             if (ImGui::TreeNode("Image Saver")) {
                 out_saver->show();

src/impl/apps/endo_guide/endo_guide.h

@@ -2,8 +2,10 @@
 #define DEPTHGUIDE_ENDO_GUIDE_H
 
 #include "core/object_manager.h"
+#include "device/mvs_camera_ui.h"
 #include "device/uvc_camera_ui.h"
 #include "module/augment_manager_v2.h"
+#include "module/guidance/acl_guide.h"
 #include "module/image_augment_helper_v2.h"
 #include "module/image_saver.h"
 #include "module/image_viewer.h"
@@ -36,6 +38,10 @@ private:
         // image output
         img_out,
 
+        left_raw, right_raw,
+        left_rgb, right_rgb,
+        left_out, right_out,
+
         name_end,
     };
 
@@ -55,6 +61,13 @@ private:
     std::unique_ptr<image_viewer> bg_viewer;
     std::unique_ptr<image_saver> out_saver;
     std::unique_ptr<sophiar_monitor> monitor;
+    std::unique_ptr<acl_guide> acl;
+
+    std::unique_ptr<mvs_camera_ui> mvs_cam;
+    struct {
+        std::unique_ptr<image_augment_helper_v2> aug_helper;
+        smart_cuda_stream stream;
+    } left_cam, right_cam;
 };
 
 #endif //DEPTHGUIDE_ENDO_GUIDE_H

src/module/guidance/acl_guide.h

@@ -0,0 +1,41 @@
+#ifndef DEPTHGUIDE_ACL_GUIDE_H
+#define DEPTHGUIDE_ACL_GUIDE_H
+
+#include "core/local_connection.h" // sophiar
+#include "core/yaml_utility.hpp"
+#include "render/render_scene.h"
+
+#include <memory>
+
+class acl_guide {
+public:
+
+    struct create_config {
+        struct item_info {
+            std::string transform_var_name;
+            std::string collect_var_name;
+            std::string collect_obj_name;
+        } femur, tibia;
+
+        sophiar_conn_type *sophiar_conn = nullptr;
+        io_ctx_type *ctx = nullptr;
+
+        void fill_from_yaml(const YAML::Node &conf);
+
+        FROM_YAML_IMPL(create_config);
+    };
+
+    explicit acl_guide(const create_config &conf);
+
+    ~acl_guide();
+
+    void show();
+
+    void pre_render_slot(const scene_ptr &info);
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+#endif //DEPTHGUIDE_ACL_GUIDE_H

src/module/guidance/guide_helper.h

@@ -0,0 +1,8 @@
+#ifndef DEPTHGUIDE_GUIDE_HELPER_H
+#define DEPTHGUIDE_GUIDE_HELPER_H
+
+#include "render/render_mesh.h"
+
+mesh_ptr get_sphere_symbol(float radius);
+
+#endif //DEPTHGUIDE_GUIDE_HELPER_H

src/module/guidance/impl/acl_guide.cpp

@@ -0,0 +1,200 @@
+#include "module/guidance/acl_guide.h"
+#include "core/imgui_utility.hpp"
+#include "core/math_helper.hpp"
+#include "module/guidance/guide_helper.h"
+#include "render/render_utility.h"
+#include "render/render_mesh.h"
+
+#include <boost/asio/post.hpp>
+
+#include <glm/gtc/type_ptr.hpp>
+
+using boost::asio::post;
+
+struct acl_guide::impl {
+
+    create_config conf;
+
+    enum {
+        ACL_EMPTY = -1,
+        ACL_FEMUR = 0,
+        ACL_TIBIA = 1,
+    };
+
+    using item_base_type = create_config::item_info;
+
+    struct item_store_type : public item_base_type {
+        std::optional<glm::vec3> keypoint;
+
+        std::optional<glm::vec3> world_point(impl *i) {
+            if (!keypoint.has_value()) return {};
+            auto trans = i->conf.sophiar_conn
+                    ->query_transform_variable(transform_var_name);
+            if (!trans.has_value()) return {};
+            return transform_p(to_mat4(*trans), *keypoint);
+        }
+    };
+
+    item_store_type femur, tibia;
+
+    bool is_running = false;
+    bool enable_collect = false;
+    int cur_index = ACL_EMPTY, next_index = cur_index;
+    item_store_type *cur_item = nullptr;
+
+    glm::vec3 color;
+    GLfloat line_width = 5.f; // pix
+    GLfloat point_size = 2.f; // mm
+    ogl_range_info lw_info;
+
+    impl(const create_config &_conf) {
+        conf = _conf;
+        *(item_base_type *) &femur = conf.femur; // copy base type part
+        *(item_base_type *) &tibia = conf.tibia;
+
+        lw_info = query_line_width_info();
+    }
+
+    void collect_point() {
+        if (!enable_collect || cur_item == nullptr) return;
+        auto pos = conf.sophiar_conn
+                ->query_scalarxyz_variable(cur_item->collect_var_name);
+        if (!pos.has_value()) return;
+        cur_item->keypoint = to_vec3(*pos);
+    }
+
+    void process() {
+        // update current object
+        if (cur_index != next_index) {
+            if (cur_item != nullptr) {
+                CALL_CHECK(conf.sophiar_conn->stop_object(cur_item->collect_obj_name));
+                cur_item = nullptr;
+            }
+            assert(cur_item == nullptr);
+            switch (next_index) {
+                // @formatter:off
+                case ACL_FEMUR: { cur_item = &femur; break; }
+                case ACL_TIBIA: { cur_item = &tibia; break; }
+                // @formatter:on
+                default: {
+                    RET_ERROR;
+                }
+            }
+            CALL_CHECK(conf.sophiar_conn->start_object(cur_item->collect_obj_name));
+            cur_index = next_index;
+        }
+
+        collect_point();
+    }
+
+    void start() {
+        conf.sophiar_conn->mark_variable_disposal(conf.femur.collect_var_name);
+        conf.sophiar_conn->mark_variable_disposal(conf.tibia.collect_var_name);
+        is_running = true;
+    }
+
+    void stop() {
+        is_running = false;
+        if (cur_item != nullptr) {
+            CALL_CHECK(conf.sophiar_conn->stop_object(cur_item->collect_obj_name));
+            cur_item = nullptr;
+            cur_index = ACL_EMPTY;
+        }
+    }
+
+    void show() {
+        if (!is_running) {
+            if (ImGui::Button("Start")) {
+                post(*conf.ctx, [this] { start(); });
+            }
+        } else {
+            if (ImGui::Button("Stop")) {
+                post(*conf.ctx, [this] { stop(); });
+            }
+        }
+        if (is_running) {
+            ImGui::Checkbox("Enable", &enable_collect);
+            ImGui::SameLine();
+            {
+                auto guard = imgui_valid_style_guard(femur.keypoint.has_value());
+                ImGui::RadioButton("Femur", &next_index, ACL_FEMUR);
+            }
+            ImGui::SameLine();
+            {
+                auto guard = imgui_valid_style_guard(tibia.keypoint.has_value());
+                ImGui::RadioButton("Tibia", &next_index, ACL_TIBIA);
+            }
+            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));
+            process();
+        }
+    }
+
+    void pre_render_slot(const scene_ptr &info) {
+        if (!is_running) return;
+
+        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,},
+            };
+            info->items.push_back(
+                    {.info = item_info, .transform = glm::translate(p),});
+        };
+
+        auto v1 = femur.world_point(this);
+        if (v1.has_value()) {
+            add_point(*v1);
+        }
+
+        auto v2 = tibia.world_point(this);
+        if (v2.has_value()) {
+            add_point(*v2);
+        }
+
+        if (v1.has_value() && v2.has_value()) {
+            auto p1 = *v1, p2 = *v2;
+
+            static constexpr auto extra_length = 100.f; // mm
+            auto dir = glm::normalize(p1 - p2);
+            auto r1 = p1 + extra_length * dir;
+            auto r2 = p2 - extra_length * dir;
+            auto item_info = scene_render_info::line_info{
+                    .p1 = r1, .p2 = r2,
+                    .color = color, .line_width = line_width,
+            };
+            info->items.push_back(
+                    {.info = item_info, .transform = glm::mat4(1.f),});
+        }
+    }
+
+};
+
+void acl_guide::create_config::fill_from_yaml(const YAML::Node &conf) {
+    auto info_from_yaml = [](const YAML::Node &conf) {
+        return item_info{
+                .transform_var_name = LOAD_STR("transform_var"),
+                .collect_var_name = LOAD_STR("collect_var"),
+                .collect_obj_name = LOAD_STR("collect_obj"),
+        };
+    };
+    femur = info_from_yaml(LOAD_SUB("femur"));
+    tibia = info_from_yaml(LOAD_SUB("tibia"));
+}
+
+acl_guide::acl_guide(const create_config &conf)
+        : pimpl(std::make_unique<impl>(conf)) {
+    pimpl->conf = conf;
+}
+
+acl_guide::~acl_guide() = default;
+
+void acl_guide::show() {
+    pimpl->show();
+}
+
+void acl_guide::pre_render_slot(const scene_ptr &info) {
+    pimpl->pre_render_slot(info);
+}

src/module/guidance/impl/cursor_guide.cpp

@@ -1,5 +1,6 @@
 #include "cursor_guide_impl.h"
 #include "core/imgui_utility.hpp"
+#include "module/guidance/guide_helper.h"
 
 #include <glm/gtx/transform.hpp>
 
@@ -36,24 +37,6 @@ namespace cursor_guide_impl {
         });
     }
 
-    mesh_cache_type mesh_cache;
-
-    mesh_ptr get_mesh(float radius) {
-        if (auto iter = mesh_cache.find(radius);
-                iter != mesh_cache.end()) {
-            return iter->second;
-        }
-        vtkNew<vtkSphereSource> source;
-        source->SetRadius(radius);
-        source->SetPhiResolution(16);
-        source->SetThetaResolution(16);
-        source->SetCenter(0, 0, 0);
-        source->Update();
-        auto mesh = mesh_type::from_vtk(source->GetOutput());
-        mesh_cache.emplace(radius, mesh);
-        return mesh;
-    }
-
 }
 
 void cursor_guide::impl::binding_store_type::update_transform() {
@@ -122,7 +105,7 @@ void cursor_guide::impl::show() {
 void cursor_guide::impl::pre_render_slot(const scene_ptr &info) {
     if (free_pos.has_value()) {
         auto item_info = scene_render_info::mesh_info{
-                .mesh = get_mesh(current_radius),
+                .mesh = get_sphere_symbol(current_radius),
                 .material = {.ambient = color_free * amb_factor, .diffuse = color_free,},
         };
         info->items.push_back(
@@ -136,7 +119,7 @@ void cursor_guide::impl::pre_render_slot(const scene_ptr &info) {
             auto wp = transform_p(points.transform, point.position);
             auto color = point.is_picked ? color_picked : color_fixed;
             auto item_info = scene_render_info::mesh_info{
-                    .mesh = get_mesh(point.radius),
+                    .mesh = get_sphere_symbol(point.radius),
                     .material = {.ambient = color * amb_factor, .diffuse = color,},
                     .enable_depth_alpha = true, .alpha_factor = 0.2,
             };

src/module/guidance/impl/cursor_guide_impl.h

@@ -33,12 +33,6 @@ namespace cursor_guide_impl {
         void remove_picked();
     };
 
-    using mesh_cache_type =
-            std::unordered_map<float, mesh_ptr>;
-    extern mesh_cache_type mesh_cache;
-
-    mesh_ptr get_mesh(float radius);
-
 }
 
 using namespace cursor_guide_impl;

src/module/guidance/impl/guide_helper.cpp

@@ -0,0 +1,29 @@
+#include "module/guidance/guide_helper.h"
+
+#include <vtkSphereSource.h>
+
+namespace guide_helper_impl {
+
+    using mesh_cache_type =
+            std::unordered_map<float, mesh_ptr>;
+    mesh_cache_type sphere_cache;
+
+}
+
+using namespace guide_helper_impl;
+
+mesh_ptr get_sphere_symbol(float radius) {
+    if (auto iter = sphere_cache.find(radius);
+            iter != sphere_cache.end()) {
+        return iter->second;
+    }
+    vtkNew<vtkSphereSource> source;
+    source->SetRadius(radius);
+    source->SetPhiResolution(16);
+    source->SetThetaResolution(16);
+    source->SetCenter(0, 0, 0);
+    source->Update();
+    auto mesh = mesh_type::from_vtk(source->GetOutput());
+    sphere_cache.emplace(radius, mesh);
+    return mesh;
+}

src/module/impl/augment_manager_v2.cpp

@@ -37,10 +37,7 @@ augment_manager_v2::impl::impl(const create_config &_conf) {
             st.flip_y = item.flip_y;
         }
     }
-
-    // load point size information
-    glGetFloatv(GL_POINT_SIZE_RANGE, (GLfloat *) &ps_range);
-    glGetFloatv(GL_POINT_SIZE_GRANULARITY, &ps_interval);
+    ps_info = query_point_size_info();
 }
 
 void augment_manager_v2::impl::update(const camera_info &info, bool no_commit) {
@@ -157,8 +154,8 @@ void augment_manager_v2::impl::show() {
             }
 
             if (item.type == ITEM_PC) {
-                ImGui::DragFloat("Point Size", &item.point_size, ps_interval,
-                                 ps_range.min, ps_range.max, "%.01f");
+                ImGui::DragFloat("Point Size", &item.point_size, ps_info.delta,
+                                 ps_info.min, ps_info.max, "%.01f");
             }
         }
     }

src/module/impl/augment_manager_v2_impl.h

@@ -2,6 +2,7 @@
 #define DEPTHGUIDE_AUGMENT_MANAGER_V2_IMPL_H
 
 #include "module/augment_manager_v2.h"
+#include "render/render_utility.h"
 
 #include <imGuIZMOquat.h>
 
@@ -65,11 +66,7 @@ struct augment_manager_v2::impl {
 
     camera_augment_helper_v2 *cam_helper = nullptr;
 
-    // low-level GL_POINT info
-    struct {
-        GLfloat min, max;
-    } ps_range;
-    GLfloat ps_interval;
+    ogl_range_info ps_info;
 
     explicit impl(const create_config &conf);
 

src/module/impl/pc_viewer.cpp

@@ -6,10 +6,7 @@
 
 pc_viewer::impl::impl(pc_viewer::create_config _conf) {
     conf = _conf;
-
-    // load point size information
-    glGetFloatv(GL_POINT_SIZE_RANGE, (GLfloat *) &ps_range);
-    glGetFloatv(GL_POINT_SIZE_GRANULARITY, &ps_interval);
+    ps_info = query_point_size_info();
 }
 
 void pc_viewer::impl::render() {
@@ -32,8 +29,8 @@ void pc_viewer::impl::show() {
     ImGui::DragFloat3("Offset (deg)", glm::value_ptr(rotation),
                       0.1f, -180.0f, 180.0f, "%.01f");
     ImGui::DragFloat("FOV (deg)", &fov, 0.5f, 10.0f, 178.0f, "%.01f");
-    ImGui::DragFloat("Point Size", &point_size, ps_interval,
-                     ps_range.min, ps_range.max, "%.01f");
+    ImGui::DragFloat("Point Size", &point_size, ps_info.delta,
+                     ps_info.min, ps_info.max, "%.01f");
     ImGui::SliderFloat("Clip Near", &clip.near,
                        1.0f, clip.far, "%.f", ImGuiSliderFlags_Logarithmic);
     ImGui::SliderFloat("Clip Far", &clip.far,

src/module/impl/pc_viewer_impl.h

@@ -19,10 +19,7 @@ struct pc_viewer::impl {
         float far = 1000.0f;
     } clip;
 
-    struct {
-        GLfloat min, max;
-    } ps_range;
-    GLfloat ps_interval;
+    ogl_range_info ps_info;
     float point_size = 1.0f;
 
     pc_render ren;

src/render/impl/render_line.cpp

@@ -0,0 +1,54 @@
+#include "render/render_line.h"
+#include "render/render_utility.h"
+
+namespace render_line_impl {
+
+    std::unique_ptr<smart_program> pg;
+    GLuint vbo, vao;
+
+    void initialize() {
+        assert(pg == nullptr);
+        pg = std::unique_ptr<smart_program>(
+                smart_program::create("line",
+                                      {{GL_VERTEX_SHADER,   "line.vert"},
+                                       {GL_FRAGMENT_SHADER, "line.frag"}}));
+
+        // create vertex buffer
+        glGenBuffers(1, &vbo);
+        glBindBuffer(GL_ARRAY_BUFFER, vbo);
+        glBufferStorage(GL_ARRAY_BUFFER, 6 * sizeof(GLfloat), nullptr, GL_DYNAMIC_STORAGE_BIT);
+
+        // config vertex array
+        glGenVertexArrays(1, &vao);
+        glBindVertexArray(vao);
+        glEnableVertexAttribArray(0);
+        glEnableVertexAttribArray(1);
+        glVertexAttribPointer(0, 3, GL_FLOAT, false, 3 * sizeof(GLfloat), (void *) 0);
+    }
+
+}
+
+using namespace render_line_impl;
+
+void render_line(const line_render_info &info) {
+    if (pg == nullptr) [[unlikely]] {
+        initialize();
+    }
+    assert(pg != nullptr);
+    pg->use();
+
+    pg->set_uniform_vec3("color", info.color);
+    glLineWidth(info.line_width);
+
+    // fill vertex buffer
+    glBindVertexArray(vao);
+    glBindBuffer(GL_ARRAY_BUFFER, vbo);
+    glm::vec3 vertices[] = {info.p1, info.p2};
+    static_assert(sizeof(vertices) == 6 * sizeof(GLfloat));
+    glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(vertices), vertices);
+
+    // draw
+    glEnable(GL_DEPTH_TEST);
+    glEnable(GL_LINE_SMOOTH);
+    glDrawArrays(GL_LINES, 0, 2);
+}

src/render/impl/render_scene.cpp

@@ -1,4 +1,6 @@
 #include "render_scene_impl.h"
+#include "core/math_helper.hpp"
+#include "render/render_line.h"
 
 #include <glm/gtc/matrix_access.hpp>
 #include <glm/gtx/transform.hpp>
@@ -76,6 +78,19 @@ namespace render_scene_impl {
         pc_render::render(info.pc, ren_conf);
     }
 
+    void render_line(const render_request &req) {
+        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 ren_conf = line_render_info{
+                .p1 = transform_p(t_mat, info.p1),
+                .p2 = transform_p(t_mat, info.p2),
+                .color = info.color, .line_width = info.line_width,
+        };
+        render_line(ren_conf);
+    }
+
     // standard camera transform matrix ->
     // OpenGL 's camera transform matrix
     glm::mat4 cvt_camera_transform(const glm::mat4 &transform) {
@@ -130,6 +145,12 @@ namespace render_scene_impl {
                     std::get<4>(item.info).func();
                     break;
                 }
+                case 5: {
+                    static_assert(std::is_same_v<std::variant_alternative_t<5, info_type>,
+                            scene_render_info::line_info>);
+                    render_line(req);
+                    break;
+                }
                 default: {
                     RET_ERROR;
                 }

src/render/impl/render_utility.cpp

@@ -21,6 +21,28 @@ cv::Size query_viewport_size() {
     return {vp.width, vp.height};
 }
 
+ogl_range_info query_point_size_info() {
+    static std::unique_ptr<ogl_range_info> ret;
+    if (ret == nullptr) [[unlikely]] {
+        ret = std::make_unique<ogl_range_info>();
+        glGetFloatv(GL_POINT_SIZE_RANGE, (GLfloat *) &ret->min);
+        glGetFloatv(GL_POINT_SIZE_GRANULARITY, &ret->delta);
+    }
+    assert(ret != nullptr);
+    return *ret;
+}
+
+ogl_range_info query_line_width_info() {
+    static std::unique_ptr<ogl_range_info> ret;
+    if (ret == nullptr) [[unlikely]] {
+        ret = std::make_unique<ogl_range_info>();
+        glGetFloatv(GL_SMOOTH_LINE_WIDTH_RANGE, (GLfloat *) &ret->min);
+        glGetFloatv(GL_SMOOTH_LINE_WIDTH_GRANULARITY, &ret->delta);
+    }
+    assert(ret != nullptr);
+    return *ret;
+}
+
 void check_framebuffer() {
     auto status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
     if (status != GL_FRAMEBUFFER_COMPLETE) [[unlikely]] {

src/render/impl/shader/line.frag

@@ -0,0 +1,9 @@
+#version 460
+
+uniform vec3 color;
+
+layout (location = 0) out vec4 frag_color;
+
+void main() {
+    frag_color = vec4(color, 1.0f);
+}

src/render/impl/shader/line.vert

@@ -0,0 +1,7 @@
+#version 460
+
+layout (location = 0) in vec3 pos;
+
+void main() {
+    gl_Position = vec4(pos, 1.0f);
+}

src/render/render_line.h

@@ -0,0 +1,14 @@
+#ifndef DEPTHGUIDE_RENDER_LINE_H
+#define DEPTHGUIDE_RENDER_LINE_H
+
+#include <glm/glm.hpp>
+
+struct line_render_info {
+    glm::vec3 p1, p2; // in NDC
+    glm::vec3 color;
+    float line_width;
+};
+
+void render_line(const line_render_info &info);
+
+#endif //DEPTHGUIDE_RENDER_LINE_H

src/render/render_scene.h

@@ -40,9 +40,15 @@ struct scene_render_info {
         std::function<void()> func;
     };
 
+    struct line_info {
+        glm::vec3 p1, p2; // in world coordinate
+        glm::vec3 color;
+        float line_width;
+    };
+
     struct item_type {
         using item_store_type = std::variant<std::monostate,
-                image_info, mesh_info, pc_info, custom_info>;
+                image_info, mesh_info, pc_info, custom_info, line_info>;
         item_store_type info;
 
         GLfloat alpha = 1.0f;

src/render/render_utility.h

@@ -18,6 +18,14 @@ extern std::filesystem::path shader_folder;
 
 cv::Size query_viewport_size();
 
+struct ogl_range_info {
+    GLfloat min, max, delta;
+};
+
+ogl_range_info query_point_size_info();
+
+ogl_range_info query_line_width_info();
+
 void check_framebuffer();
 
 GLuint compile_shader(GLenum type, const char *filename);