Implemented rotation ball calibration.

data/config_remote_ar_v2.yaml

@@ -5,4 +5,6 @@ right_camera_name: "RightEye"
 
 ndi_ip: 192.168.1.202
 ndi_port: 8765
-sophiar_config: ./sophiar_empty.json
+sophiar_config: ./sophiar_config_endo_guide.json
+sophiar_start_name: tracker_all
+camera_ref_transform_var: camera_ref_in_tracker

data/sophiar_config_endo_guide.json

@@ -346,7 +346,7 @@
       "name": "ndi",
       "init_config": {
         "address_type": "ethernet",
-        "ip": "10.0.0.5",
+        "ip": "192.168.1.202",
         "tcp_port": 8765,
         "com_port": "/dev/ttyUSB0",
         "tool_list": [

src/core/math_helper.hpp

@@ -72,6 +72,14 @@ inline Eigen::Matrix3f to_eigen(const glm::mat3 &m) {
     return ret;
 }
 
+inline Eigen::Vector2f to_eigen(const glm::vec2 &v) {
+    auto ret = Eigen::Vector2f();
+    for (auto k = 0; k < 2; ++k) {
+        ret(k) = v[k];
+    }
+    return ret;
+}
+
 inline Eigen::Vector3f to_eigen(const glm::vec3 &v) {
     auto ret = Eigen::Vector3f();
     for (auto k = 0; k < 3; ++k) {
@@ -80,6 +88,24 @@ inline Eigen::Vector3f to_eigen(const glm::vec3 &v) {
     return ret;
 }
 
+inline Eigen::Matrix2Xf to_eigen(const std::vector<glm::vec2> &points) {
+    const auto N = points.size();
+    auto ret = Eigen::Matrix2Xf(2, N);
+    for (auto k = 0; k < N; ++k) {
+        ret.col(k) = to_eigen(points[k]);
+    }
+    return ret;
+}
+
+inline Eigen::Matrix3Xf to_eigen(const std::vector<glm::vec3> &points) {
+    const auto N = points.size();
+    auto ret = Eigen::Matrix3Xf(3, N);
+    for (auto k = 0; k < N; ++k) {
+        ret.col(k) = to_eigen(points[k]);
+    }
+    return ret;
+}
+
 template<typename T>
 concept Vec2Type = requires(T t) {
     { t.x } -> std::convertible_to<float>;
@@ -103,6 +129,12 @@ inline auto to_vec3(const T &v) {
     return glm::vec3(v.x, v.y, v.z);
 }
 
+template<typename EigenType>
+glm::vec2 to_vec2(const Eigen::MatrixBase<EigenType> &v) {
+    static_assert(EigenType::SizeAtCompileTime == 2);
+    return glm::vec2(v.x(), v.y());
+}
+
 template<typename EigenType>
 glm::vec3 to_vec3(const Eigen::MatrixBase<EigenType> &v) {
     static_assert(EigenType::SizeAtCompileTime == 3);

src/core_v2/utility.hpp

@@ -17,12 +17,8 @@ size_t alignment_round(size_t size, const size_t align = Align) {
 
 extern BS::thread_pool *g_thread_pool;
 
-template<typename Func>
-void thread_pool_detach_task(Func &&func) {
-    g_thread_pool->detach_task(std::forward<Func>(func));
-}
-
-#define TP_DETACH(func) thread_pool_detach_task(func)
+#define TP_DETACH(func) g_thread_pool->detach_task(func)
+#define TP_SUBMIT(func) g_thread_pool->submit_task(func)
 #define TP_SYNC g_thread_pool->wait()
 
 #endif //UTILITY_HPP

src/device_v5/ndi_stray_point_tracker.cpp

@@ -27,11 +27,16 @@ using boost::system::error_code;
 
 namespace {
     constexpr auto track_interval = std::chrono::milliseconds(17); // 60Hz
-    constexpr const char track_command[] = "BX 1000\r";
+    constexpr std::string_view track_command = "BX 1000\r";
     constexpr auto ndi_endian = boost::endian::order::little;
     constexpr uint16_t binary_header_magic = 0xA5C4;
+    constexpr auto cr_ascii = '\r';
+
+    ndi_stray_points_sig_type result_sig;
 }
 
+extern ba::io_context *main_ctx;
+
 struct ndi_stray_point_tracker::impl {
     create_config conf;
 
@@ -54,12 +59,19 @@ struct ndi_stray_point_tracker::impl {
 
     ba::awaitable<dynamic_memory::pointer> read_reply() {
         // read header
-        auto header_buf = static_memory<6>{};
+        const auto header_magic =
+                co_await async_read_value<ndi_endian, uint16_t>(*socket);
+        if (header_magic != binary_header_magic) {
+            auto ascii_buf = ba::streambuf();
+            co_await async_read_until(*socket, ascii_buf, cr_ascii, ba::use_awaitable);
+            co_return nullptr;
+        }
+
+        auto header_buf = static_memory<4>{};
         co_await async_fill_memory_from(*socket, header_buf);
         auto header_reader = versatile_reader<ndi_endian>(header_buf);
-        uint16_t header_magic, reply_length, header_crc16;
-        header_reader >> header_magic >> reply_length >> header_crc16;
-        assert(header_magic == binary_header_magic);
+        uint16_t reply_length, header_crc16;
+        header_reader >> reply_length >> header_crc16;
 
         // TODO: check header crc
 
@@ -78,6 +90,7 @@ struct ndi_stray_point_tracker::impl {
         co_await async_write(*socket, ba::buffer(track_command), ba::use_awaitable);
         // read reply
         const auto reply = co_await read_reply();
+        if (reply == nullptr) co_return true;
 
         // parse reply
         auto reader = versatile_reader<ndi_endian>(*reply);
@@ -95,6 +108,10 @@ struct ndi_stray_point_tracker::impl {
                 reader >> point.pos.x >> point.pos.y >> point.pos.z;
             }
         }
+
+        post(*main_ctx, [ret = last_points] {
+            result_sig(ret);
+        });
         co_return true;
     }
 
@@ -133,3 +150,7 @@ void create_ndi_stray_points_tracker(
 ndi_stray_point_tracker::points_type get_ndi_stray_points() {
     return tracker->get_stray_points();
 }
+
+ndi_stray_points_sig_type *get_ndi_stray_points_sig() {
+    return &result_sig;
+}

src/device_v5/ndi_stray_point_tracker.h

@@ -1,6 +1,8 @@
 #ifndef TRACKER_NDI_H
 #define TRACKER_NDI_H
 
+#include <boost/signals2.hpp>
+
 #include <glm/vec3.hpp>
 
 #include <memory>
@@ -34,4 +36,8 @@ private:
 void create_ndi_stray_points_tracker(const ndi_stray_point_tracker::create_config& conf);
 auto get_ndi_stray_points() -> ndi_stray_point_tracker::points_type;
 
+using ndi_stray_points_sig_type =
+    boost::signals2::signal<void(ndi_stray_point_tracker::points_type)>;
+auto get_ndi_stray_points_sig() -> ndi_stray_points_sig_type*;
+
 #endif //TRACKER_NDI_H

src/impl/app_base.cpp

@@ -1,4 +1,5 @@
 #include "app_base.h"
+#include "module_v5/transform_provider.h"
 
 // from sophiar
 #include "core/local_connection.h"
@@ -6,17 +7,21 @@
 
 using namespace sophiar;
 
+extern std::jthread *sophiar_thread;
 extern local_connection *g_sophiar_conn;
 
-void app_base::start_sophiar(const std::string &conf_path) {
-    sophiar_thread.emplace([=] {
+void app_base::start_sophiar(const std::string &conf_path,
+                             const std::string &start_name) {
+    sophiar_thread = new std::jthread([=] {
         run_sophiar(conf_path);
     });
-    g_sophiar_conn = new local_connection();
-}
 
-void app_base::wait_sophiar() {
+    // wait sophiar to start
     while (global_context == nullptr) {
         std::this_thread::yield();
     }
+
+    g_sophiar_conn = new local_connection();
+    g_sophiar_conn->start_object(start_name);
+    create_sophiar_transform_provider();
 }

src/impl/app_base.h

@@ -26,11 +26,8 @@ public:
     virtual void render_background() = 0;
 
 protected:
-    std::optional<std::jthread> sophiar_thread;
-
-    void start_sophiar(const std::string &conf_path);
-
-    static void wait_sophiar(); // block until sophiar is started
+    static void start_sophiar(const std::string &conf_path,
+                                     const std::string &start_name);
 };
 
 #endif //DEPTHGUIDE_APP_BASE_H

src/impl/apps/debug/app_debug.cpp

@@ -1,96 +1,21 @@
 #include "app_debug.h"
-#include "GLFW/glfw3.h"
+#include "core/math_helper.hpp"
+#include "module_v5/algorithms/algorithms.h"
 
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/cudastereo.hpp>
-#include <opencv2/cudaimgproc.hpp>
-
-//app_debug::app_debug(const create_config &conf) {
-//
-//    auto left_path = "/home/tpx/project/DepthGuide/cmake-build-debug/Left.png";
-//    auto right_path = "/home/tpx/project/DepthGuide/cmake-build-debug/Right.png";
-//
-//    auto left_img = cv::imread(left_path);
-//    auto right_img = cv::imread(right_path);
-//
-//    auto left_img_cuda = cv::cuda::GpuMat();
-//    auto right_img_cuda = cv::cuda::GpuMat();
-//
-//    left_img_cuda.upload(left_img);
-//    right_img_cuda.upload(right_img);
-//
-//    // covert to gray
-//    cv::cuda::cvtColor(left_img_cuda, left_img_cuda, cv::COLOR_RGB2GRAY);
-//    cv::cuda::cvtColor(right_img_cuda, right_img_cuda, cv::COLOR_RGB2GRAY);
-//
-//    auto disparity_cuda = cv::cuda::GpuMat();
-//    auto stereo = cv::cuda::createStereoSGM();
-//    stereo->setNumDisparities(128);
-////    stereo->setMode(cv::StereoSGBM::MODE_HH);
-//    stereo->compute(left_img_cuda, right_img_cuda, disparity_cuda);
-//
-//    auto filter = cv::cuda::createDisparityBilateralFilter();
-//    filter->setNumDisparities(128);
-//    filter->setRadius(5);
-//    filter->setNumIters(3);
-//    filter->apply(disparity_cuda, left_img_cuda, disparity_cuda);
-//
-//    auto disparity = cv::Mat();
-//    disparity_cuda.download(disparity);
-//
-//    double min_val, max_val;
-//    cv::minMaxLoc(disparity, &min_val, &max_val);
-//    SPDLOG_INFO("Min: {}, Max: {}", min_val, max_val);
-//
-//    auto tmp = cv::Mat();
-//    disparity.convertTo(tmp, CV_32FC1);
-//    tmp = (tmp - min_val) / (max_val - min_val) * 255.f;
-//    tmp.convertTo(disparity, CV_8UC1);
-//    cv::imwrite("disparity.png", disparity);
-//
-//    glfwSetWindowShouldClose(glfwGetCurrentContext(), true);
-//}
-
-#include "image_process/camera_calibrator.h"
-#include "module/experiment/calib_eval.h"
+#include <GLFW/glfw3.h>
 
 app_debug::app_debug(const create_config &conf) {
-    auto cam_info = camera_calibrator::result_type();
-    auto calib_conf = camera_calibrator::create_config{
-            .pattern_size = cv::Size(11, 8),
-            .corner_distance = 5,
-            .cb_func = [&](auto info) { cam_info = info; },
-    };
-    auto calib = std::make_unique<camera_calibrator>(calib_conf);
-
-    auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605/calib/calib_data_c1.txt",
-            .img_size = cv::Size(1920, 1080),
-    };
-    calib->simulate_process(sim_info);
-
-    std::string eval_path_list[] = {
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_1_1717595907048131",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_2_1717596106615177",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_3_1717596157508744",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_4_1717596214643474",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_5_1717596236889051",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_6_1717596274604857",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_7_1717596308077328",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Stable_Pose_8_1717596347701421",
-            "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605_bak/videos/Dynamic_Pose_1_1717596522706273"
-    };
-
-    auto eval = std::make_unique<calib_eval>(calib_eval::record_config{});
+    auto input = fitting_circle_3d::input_type();
+    auto points = std::vector<glm::vec3>();
+    points.emplace_back(100, 1, 2);
+    points.emplace_back(100, 2, 0);
+    points.emplace_back(100, 1, -1);
+    points.emplace_back(100, 0, 0);
+    input.points = to_eigen(points);
 
-    auto eval_info = calib_eval::simulate_info_type::from_yaml(
-            conf.ext_conf["eval_points"]);
-    eval_info.cam = cam_info;
+    auto result = fitting_circle_3d()(input);
+    SPDLOG_DEBUG("{}, {}, {}", result.center[0], result.center[1], result.radius);
 
-    for (auto &path: eval_path_list) {
-        eval_info.save_folder = path;
-        eval->simulate_eval(eval_info);
-    }
 
     glfwSetWindowShouldClose(glfwGetCurrentContext(), true);
-}
+}

src/impl/apps/remote_ar/remote_ar_v2.cpp

@@ -67,8 +67,8 @@ app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
         saver.emplace(sub_conf);
     }
 
-    start_sophiar(LOAD_STR("sophiar_config"));
-    wait_sophiar();
+    start_sophiar(LOAD_STR("sophiar_config"),
+                  LOAD_STR("sophiar_start_name"));
 
     if (true) {
         auto sub_conf = ndi_stray_point_tracker::create_config();
@@ -76,6 +76,12 @@ app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
         sub_conf.port = LOAD_NUMBER(uint16_t, "ndi_port");
         create_ndi_stray_points_tracker(sub_conf);
     }
+
+    if (true) {
+        auto sub_conf = oblique_calibrator::create_config();
+        sub_conf.camera_ref_name = LOAD_STR("camera_ref_transform_var");
+        endo_calib.emplace(sub_conf);
+    }
 }
 
 app_remote_ar_v2::~app_remote_ar_v2() = default;
@@ -98,6 +104,11 @@ void app_remote_ar_v2::show_ui() {
             uvc_cam->show();
         }
 
+        if (ImGui::CollapsingHeader("Endoscope Calibration")) {
+            auto id_guard = imgui_id_guard("endo_calib");
+            endo_calib->show_ui();
+        }
+
         if (ImGui::CollapsingHeader("Streamer")) {
             auto id_guard = imgui_id_guard("streamer");
             streamer->show();

src/impl/apps/remote_ar/remote_ar_v2.h

@@ -8,6 +8,7 @@
 #include "image_process_v5/image_process.h"
 #include "module/image_streamer.h"
 #include "module_v5/versatile_saver.h"
+#include "module_v5/oblique_calibrator.h"
 
 class app_remote_ar_v2 final : public app_base {
 public:
@@ -37,6 +38,7 @@ private:
     std::optional<image_viewer_v2> bg_viewer;
     std::optional<image_streamer> streamer;
     std::optional<versatile_saver> saver;
+    std::optional<oblique_calibrator> endo_calib;
 };
 
 

src/impl/main_impl.cpp

@@ -30,6 +30,8 @@ GLFWwindow *window = nullptr;
 smart_cuda_stream *default_cuda_stream = nullptr;
 io_context *main_ctx;
 BS::thread_pool *g_thread_pool;
+
+std::jthread *sophiar_thread = nullptr;
 sophiar::local_connection *g_sophiar_conn = nullptr;
 
 using cleanup_list_type =
@@ -325,5 +327,10 @@ void cleanup() {
     delete main_ob;
     delete main_ctx;
     delete g_memory_manager;
-    delete g_sophiar_conn;
+
+    if (sophiar_thread != nullptr) {
+        sophiar::stop_sophiar();
+        delete g_sophiar_conn;
+        delete sophiar_thread;
+    }
 }

src/module_v5/CMakeLists.txt

@@ -1,2 +1,6 @@
 target_sources(${PROJECT_NAME} PRIVATE
-    versatile_saver.cpp)
+        versatile_saver.cpp
+        transform_provider.cpp
+        oblique_calibrator.cpp)
+
+add_subdirectory(algorithms)

src/module_v5/algorithms/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_sources(${PROJECT_NAME} PRIVATE
+        fitting_circle_3d.cpp)

src/module_v5/algorithms/algorithms.h

@@ -0,0 +1,40 @@
+#ifndef ALGORITHMS_H
+#define ALGORITHMS_H
+
+#include <Eigen/Dense>
+#include <glm/glm.hpp>
+
+struct fitting_circle_2d {
+    struct input_type {
+        Eigen::Matrix2Xf points;
+        std::optional<Eigen::VectorXf> weights;
+    };
+
+    struct result_type {
+        glm::vec2 center;
+        float radius;
+        float error_rms;
+    };
+
+    static result_type operator()(
+        const input_type &input);
+};
+
+struct fitting_circle_3d {
+    struct input_type {
+        Eigen::Matrix3Xf points;
+        std::optional<Eigen::VectorXf> weights;
+    };
+
+    struct result_type {
+        glm::vec3 center;
+        glm::vec3 axis;
+        float radius;
+        float error_rms;
+    };
+
+    static result_type operator()(
+        const input_type &input);
+};
+
+#endif //ALGORITHMS_H

src/module_v5/algorithms/fitting_circle_3d.cpp

@@ -0,0 +1,97 @@
+#include "algorithms.h"
+#include "core/math_helper.hpp"
+
+using namespace Eigen;
+
+fitting_circle_2d::result_type
+fitting_circle_2d::operator()(const input_type &input) {
+    const auto N = input.points.cols();
+    auto A = MatrixX3f(N, 3);
+    A.leftCols<2>() = input.points.transpose();
+    A.col(2).setOnes();
+
+    const auto x = A.col(0).array(), y = A.col(1).array();
+    VectorXf b = x.pow(2) + y.pow(2);
+
+    if (input.weights) {
+        const auto w = input.weights->array();
+        A.array().colwise() *= w;
+        b.array().colwise() *= w;
+    }
+
+    const auto solver = ColPivHouseholderQR<MatrixX3f>(A);
+    const auto c = solver.solve(b);
+    const auto center = Vector2f(c[0] / 2, c[1] / 2);
+    auto ret = result_type();
+    ret.center = to_vec2(center);
+    ret.radius = sqrtf(c[2] + center.squaredNorm());
+
+    auto err = VectorXf(N);
+    for (auto k = 0; k < N; ++k) {
+        auto point = input.points.col(k);
+        err[k] = ret.radius - (center - point).norm();
+    }
+    ret.error_rms = sqrtf(err.array().square().sum() / N);
+
+    return ret;
+}
+
+namespace {
+    MatrixX3f rodrigues_rot(const MatrixX3f &points,
+                            Vector3f n0, Vector3f n1) {
+        const auto N = points.rows();
+        n0.normalize();
+        n1.normalize();
+        const auto n2 = n0.cross(n1).normalized();
+        const auto theta = acos(n0.dot(n1));
+        const auto cos_theta = cos(theta);
+        const auto sin_theta = sin(theta);
+
+        auto ret = MatrixX3f(N, 3);
+        for (auto k = 0; k < N; k++) {
+            auto p = points.row(k).transpose();
+            ret.row(k) = p * cos_theta
+                         + n2.cross(p) * sin_theta
+                         + n2 * n2.dot(p) * (1 - cos_theta);
+        }
+        return ret;
+    }
+
+    Vector3f rodrigues_rot(const Vector3f &vec,
+                           const Vector3f &n0, const Vector3f &n1) {
+        auto matrix = MatrixX3f(1, 3);
+        matrix.row(0) = vec;
+        auto ret = rodrigues_rot(matrix, n0, n1);
+        return ret.row(0);
+    }
+}
+
+fitting_circle_3d::result_type
+fitting_circle_3d::operator()(const input_type &input) {
+    auto ret = result_type();
+
+    MatrixX3f P = input.points.transpose();
+    const Vector3f mean = P.colwise().mean();
+    P.array().rowwise() -= mean.array().transpose();
+
+    // Calculate the SVD of the centered points
+    const auto svd = JacobiSVD(P, ComputeFullV);
+    const auto normal = svd.matrixV().col(2);
+    ret.axis = to_vec3(normal);
+
+    // Fit a circle to the rotated points
+    auto P_xy = rodrigues_rot(P, normal, Vector3f::UnitZ());
+    auto input_2d = fitting_circle_2d::input_type();
+    input_2d.points = P_xy.leftCols<2>().transpose();
+    input_2d.weights = input.weights;
+    const auto ret_2d = fitting_circle_2d()(input_2d);
+    ret.radius = ret_2d.radius;
+    ret.error_rms = ret_2d.error_rms;
+
+    auto center = Vector3f();
+    center << ret.center.x, ret.center.y, 0;
+    center = rodrigues_rot(center, Vector3f::UnitZ(), normal);
+    center += mean;
+    ret.center = to_vec3(center);
+    return ret;
+}

src/module_v5/oblique_calibrator.cpp

@@ -0,0 +1,146 @@
+#include "oblique_calibrator.h"
+#include "core/imgui_utility.hpp"
+#include "core/math_helper.hpp"
+#include "core_v2/utility.hpp"
+#include "device_v5/ndi_stray_point_tracker.h"
+#include "module_v5/transform_provider.h"
+#include "module_v5/algorithms/algorithms.h"
+#include "third_party/scope_guard.hpp"
+
+// from sophiar
+#include "core/local_connection.h"
+
+#include <boost/asio/io_context.hpp>
+#include <boost/asio/post.hpp>
+
+namespace ba = boost::asio;
+namespace bs = boost::signals2;
+using namespace std::placeholders;
+
+extern ba::io_context *main_ctx;
+
+namespace {
+    constexpr auto rotation_ball_distance_threshold = 50; // 50mm
+}
+
+struct oblique_calibrator::impl {
+    create_config conf;
+
+    enum {
+        IDLE,
+        ROTATION_BALL
+    } status = IDLE;
+
+    // rotation ball
+    using stray_points_type = ndi_stray_point_tracker::points_type;
+    std::vector<glm::vec3> ball_points;
+    bs::connection stray_points_conn;
+    std::future<void> ball_future;
+    float ball_radius = 0;
+    glm::vec3 ball_center = {}, ball_axis = {}, ball_start = {};
+    bool ball_ok = false;
+
+    void rotation_ball_callback(stray_points_type points) {
+        if (points.empty()) return;
+        const auto ref_matrix =
+                query_transform(conf.camera_ref_name).matrix;
+        if (!ref_matrix) return;
+        const auto t_matrix = inverse(*ref_matrix);
+        std::ranges::for_each(points, [=](auto &point) {
+            point.pos = transform_p(t_matrix, point.pos);
+        });
+
+        if (ball_points.empty()) {
+            if (points.size() != 1) {
+                stop_rotation_ball();
+                SPDLOG_ERROR("Failed to load anchor point.");
+            }
+            ball_points.emplace_back(points[0].pos);
+            return;
+        }
+
+        auto last_point = *ball_points.rbegin();
+        const auto closest_iter = std::ranges::min_element(
+            points, std::less(), [=](const auto &point) {
+                return glm::distance(last_point, point.pos);
+            });
+        const auto closest_point = closest_iter->pos;
+        if (distance(last_point, closest_point)
+            > rotation_ball_distance_threshold) { return; }
+        ball_points.emplace_back(closest_point);
+    }
+
+    void start_rotation_ball() {
+        ball_points.clear();
+        stray_points_conn = get_ndi_stray_points_sig()
+                ->connect(std::bind(&impl::rotation_ball_callback, this, _1));
+        status = ROTATION_BALL;
+    }
+
+    void calc_rotation_ball() {
+        auto closer = sg::make_scope_guard([this] { status = IDLE; });
+        if (ball_points.size() < 3) return;
+
+        auto input = fitting_circle_3d::input_type();
+        input.points = to_eigen(ball_points);
+        const auto result = fitting_circle_3d()(input);
+        ball_center = result.center;
+        ball_axis = result.axis;
+        ball_start = ball_points[0];
+        ball_radius = result.radius;
+        SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", result.error_rms);
+        ball_ok = true;
+    }
+
+    void stop_rotation_ball() {
+        stray_points_conn.disconnect();
+        ball_future = TP_SUBMIT([this] { calc_rotation_ball(); });
+    }
+
+    void show_rotation_ball_ui() {
+        const auto num_stray_point = get_ndi_stray_points().size();
+        const bool can_operate = num_stray_point == 1;
+        if (status == IDLE) {
+            auto guard = imgui_disable_guard(!can_operate);
+            if (ImGui::Button("Start")) {
+                post(*main_ctx, [this] { start_rotation_ball(); });
+            }
+        } else if (status == ROTATION_BALL) {
+            if (ImGui::Button("Stop")) {
+                post(*main_ctx, [this] { stop_rotation_ball(); });
+            }
+        }
+        if (true) {
+            auto guard = imgui_valid_style_guard(can_operate || status == ROTATION_BALL);
+            ImGui::Text("Stray Points: %ld", num_stray_point);
+        }
+        if (status == ROTATION_BALL) {
+            ImGui::Text("Collected Samples: %ld", ball_points.size());
+        }
+    }
+
+    void show_ui() {
+        ImGui::SeparatorText("Rotation Ball");
+        show_rotation_ball_ui();
+    }
+
+    explicit impl(create_config conf)
+        : conf(std::move(conf)) {
+    }
+
+    ~impl() {
+        if (ball_future.valid()) {
+            ball_future.wait();
+        }
+    }
+};
+
+oblique_calibrator::oblique_calibrator(const create_config &conf)
+    : pimpl(std::make_unique<impl>(conf)) {
+}
+
+oblique_calibrator::~oblique_calibrator() = default;
+
+void oblique_calibrator::show_ui() const {
+    pimpl->show_ui();
+}

src/module_v5/oblique_calibrator.h

@@ -0,0 +1,40 @@
+#ifndef OBLIQUE_CALIBRATOR_H
+#define OBLIQUE_CALIBRATOR_H
+
+#include "core_v2/object_manager.h"
+
+#include <glm/vec3.hpp>
+
+#include <boost/signals2.hpp>
+
+#include <memory>
+
+class oblique_calibrator {
+public:
+    struct create_config {
+        obj_name_type image_name; // Input, endoscope image
+        std::string camera_ref_name; // XXX_in_tracker
+
+        obj_name_type angle_name; // Output, currant rotation angle of the len
+    };
+
+    explicit oblique_calibrator(const create_config &conf);
+
+    ~oblique_calibrator();
+
+    struct result_type {
+        glm::vec3 rot_center; // center of the rotation ring
+        glm::vec3 rot_axis; // rotation axis of the rotation ring
+    };
+
+    using result_sig_type = boost::signals2::signal<void(result_type)>;
+    result_sig_type result_sig;
+
+    void show_ui() const;
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+#endif //OBLIQUE_CALIBRATOR_H

src/module_v5/transform_provider.cpp

@@ -0,0 +1,64 @@
+#include "transform_provider.h"
+#include "core/math_helper.hpp"
+
+// from sophiar
+#include "core/local_connection.h"
+
+#include <unordered_map>
+
+using namespace sophiar;
+
+struct transform_provider_base : transform_provider {
+
+    using custom_pool_type =
+        std::unordered_map<key_type, custom_transform_provider_type>;
+    custom_pool_type custom_pool;
+
+    bool is_custom_query(const key_type &name) const {
+        return custom_pool.contains(name);
+    }
+
+    result_type query_transform(const key_type &name) override {
+        const auto iter = custom_pool.find(name);
+        assert(iter != custom_pool.end());
+        return iter->second(name);
+    }
+};
+
+extern local_connection *g_sophiar_conn;
+
+struct sophiar_transform_provider final : transform_provider_base {
+
+    using base_type = transform_provider_base;
+
+    result_type query_transform(const key_type &name) override {
+        if (is_custom_query(name)) {
+            return base_type::query_transform(name);
+        }
+
+        const auto s_ret = g_sophiar_conn->query_transform_variable(name);
+        if (!s_ret) return {};
+        auto ret = result_type();
+        ret.matrix = to_mat4(*s_ret);
+        return ret;
+    }
+};
+
+namespace {
+    std::unique_ptr<transform_provider_base> provider;
+}
+
+void create_sophiar_transform_provider() {
+    provider = std::make_unique<sophiar_transform_provider>();
+}
+
+transform_provider::result_type
+query_transform(const transform_provider::key_type &name) {
+    return provider->query_transform(name);
+}
+
+void register_custom_transform_provider(
+    const transform_provider::key_type &name,
+    custom_transform_provider_type &&func) {
+    provider->custom_pool.emplace(name, func);
+}

src/module_v5/transform_provider.h

@@ -0,0 +1,30 @@
+#ifndef TRANSFORM_PROVIDER_H
+#define TRANSFORM_PROVIDER_H
+
+// #include <boost/any.hpp>
+
+#include <glm/mat4x4.hpp>
+
+#include <functional>
+#include <optional>
+#include <string>
+
+struct transform_provider {
+    using key_type = std::string;
+    struct result_type {
+        std::optional<glm::mat4> matrix;
+        // boost::any extra;
+    };
+    virtual result_type query_transform(const key_type &name) = 0;
+    virtual ~transform_provider() = default;
+};
+
+void create_sophiar_transform_provider();
+auto query_transform(const transform_provider::key_type &name) -> transform_provider::result_type;
+
+using custom_transform_provider_type = std::function<
+    transform_provider::result_type(const transform_provider::key_type &name)>;
+void register_custom_transform_provider(const transform_provider::key_type &name,
+                                        custom_transform_provider_type &&func);
+
+#endif //TRANSFORM_PROVIDER_H