Implemented oblique endoscope calibration.

.clang-format

@@ -0,0 +1,125 @@
+# Generated from CLion C/C++ Code Style settings
+---
+Language: Cpp
+Standard: Latest
+BasedOnStyle: LLVM
+AccessModifierOffset: -4
+AlignAfterOpenBracket: DontAlign
+AlignArrayOfStructures: Right
+AlignConsecutiveAssignments: true
+AlignConsecutiveBitFields: true
+AlignConsecutiveDeclarations:
+  Enabled: true
+  AcrossComments: true
+AlignConsecutiveMacros: true
+AlignConsecutiveShortCaseStatements:
+  Enabled: true
+AlignEscapedNewlines: DontAlign
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseExpressionOnASingleLine: true
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortCompoundRequirementOnASingleLine: true
+AllowShortEnumsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: WithoutElse
+AllowShortLambdasOnASingleLine: All
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakBeforeMultilineStrings: true
+BinPackArguments: true
+BinPackParameters: BinPack
+BitFieldColonSpacing: Before
+BraceWrapping: 
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: Never
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterStruct: false
+  AfterUnion: false
+  AfterExternBlock: false
+  BeforeCatch: false
+  BeforeElse: false
+  BeforeLambdaBody: false
+  BeforeWhile: false
+  IndentBraces: true
+  SplitEmptyFunction: false
+  SplitEmptyRecord: false
+  SplitEmptyNamespace: false
+BracedInitializerIndentWidth: 2
+BreakAdjacentStringLiterals: false
+BreakAfterAttributes: Leave
+BreakAfterReturnType: Automatic
+BreakArrays: false
+BreakBeforeBinaryOperators: All
+BreakBeforeBraces: Attach
+BreakBeforeConceptDeclarations: Always
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakFunctionDefinitionParameters: false
+BreakInheritanceList: BeforeColon
+BreakStringLiterals: true
+BreakTemplateDeclarations: Yes
+ColumnLimit: 128
+CompactNamespaces: true
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: false
+EmptyLineAfterAccessModifier: Never
+EmptyLineBeforeAccessModifier: LogicalBlock
+ExperimentalAutoDetectBinPacking: true
+IncludeBlocks: Regroup
+IncludeCategories:
+  - Regex: '^".*'
+    Priority: 1
+  - Regex: '^<.*'
+    Priority: 2
+  - Regex: '.*'
+    Priority: 3
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IndentAccessModifiers: false
+IndentCaseBlocks: false
+IndentCaseLabels: true
+IndentExternBlock: NoIndent
+IndentGotoLabels: false
+IndentPPDirectives: BeforeHash
+IndentRequiresClause: true
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+InsertBraces: false
+InsertNewlineAtEOF: true
+LambdaBodyIndentation: OuterScope
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: All
+PackConstructorInitializers: BinPack
+PointerAlignment: Right
+ReferenceAlignment: Right
+ReflowComments: true
+RequiresClausePosition: OwnLineWithBrace
+RequiresExpressionIndentation: OuterScope
+SeparateDefinitionBlocks: Always
+SortIncludes: CaseInsensitive
+SortUsingDeclarations: Lexicographic
+SpaceAfterCStyleCast: true
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
+SpaceBeforeCpp11BracedList: true
+SpaceBeforeInheritanceColon: false
+SpaceBeforeJsonColon: false
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeSquareBrackets: false
+SpaceInEmptyBlock: true
+SpaceInEmptyParentheses: false
+SpacesInAngles: false
+SpacesInConditionalStatement: true
+SpacesInCStyleCastParentheses: false
+SpacesInContainerLiterals: false
+SpacesInParentheses: false
+TabWidth: 4

data/config_remote_ar_v2.yaml

@@ -5,7 +5,7 @@ right_camera_name: "RightEye"
 
 ndi_ip: 192.168.1.202
 ndi_port: 8765
-sophiar_config: ./sophiar_config_endo_guide_oldhead.json
+sophiar_config: ./sophiar_config_endo_guide.json
 sophiar_start_name: tracker_all
 camera_ref_transform_var: camera_ref_in_tracker
 
@@ -14,8 +14,10 @@ monitor:
     var: camera_ref_in_tracker
   - name: MVS Camera
     var: mvs_ref_in_tracker
-  - name: Head
-    var: head_ref_in_tracker
+  - name: Femur
+    var: femur_ref_in_tracker
+  - name: Tibia
+    var: tibia_ref_in_tracker
   - name: Probe
     var: probe_ref_in_tracker
 
@@ -43,17 +45,32 @@ left_camera_transform: left_camera_in_tracker_denoised
 right_camera_transform: right_camera_in_tracker_denoised
 augment_list:
   - name: Head
-    transform_var: head_in_tracker_denoised
-    model: /media/tpx/590d7229-f129-4612-b7ff-6d2cb3fe49b2/home/tpx/project/RemoteAR3/data/models/femur.stl
+    transform_var: femur_in_tracker_denoised
+    model: /home/tpx/data/models/old_head/bone.stl
   - name: ExtraA
-    transform_var: head_in_tracker_denoised
-    model: /media/tpx/590d7229-f129-4612-b7ff-6d2cb3fe49b2/home/tpx/project/RemoteAR3/data/models/Femur_1.stl
+    transform_var: femur_in_tracker_denoised
+    model: /home/tpx/data/models/old_head/nerve.stl
 
 probe_model: /home/tpx/data/models/Probe.stl
 registration_list:
   - name: Bone
-    model_file: /media/tpx/590d7229-f129-4612-b7ff-6d2cb3fe49b2/home/tpx/project/RemoteAR3/data/models/femur.stl
-    collect_obj: point_picker_in_head_ref
-    collect_var: picked_point_in_head_ref
-    target_var: head_in_head_ref
-    probe_var: probe_in_head
+    model_file: /home/tpx/data/models/old_head/bone.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
+
+oblique_rotation_center: 3W1sRPPKA0Q=
+oblique_rotation_ball: FOR0QeXogcMZz//B/nbywZ+afj/zk9W9+IGCOowc0j34RHw/Mu4KPg==
+#oblique_rotation_ball: hmhzQTb9gcPFqADCRCLoweuvfj8v9M693iKOu791zT0Nj34/srELvQ==
+
+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
+  drill_transform_var: drill_in_tracker

data/sophiar_config_endo_guide.json

@@ -51,7 +51,15 @@
       "type": "double_obj"
     },
     {
-      "name": "camera_in_camera_ref",
+      "name": "len_ref_in_camera_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "camera_in_len_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "len_ref_in_tracker",
       "type": "transform_obj"
     },
     {
@@ -216,9 +224,14 @@
             "transform_var_name": "camera_ref_in_tracker"
           },
           {
-            "name": "camera",
+            "name": "len_ref",
             "parent": "camera_ref",
-            "transform_var_name": "camera_in_camera_ref"
+            "transform_var_name": "len_ref_in_camera_ref"
+          },
+          {
+            "name": "camera",
+            "parent": "len_ref",
+            "transform_var_name": "camera_in_len_ref"
           },
           {
             "name": "mvs_ref",
@@ -273,6 +286,11 @@
       },
       "start_config": {
         "watch_list": [
+          {
+            "target": "len_ref",
+            "observer": "tracker",
+            "transform_var_name": "len_ref_in_tracker"
+          },
           {
             "target": "camera",
             "observer": "tracker",
@@ -365,7 +383,7 @@
             }
           },
           {
-            "rom_path": "/home/tpx/data/roms/Glass_4Ball_1_Camera_20240312.rom",
+            "rom_path": "/home/tpx/data/roms/NR_1_STCAM_20241115.rom",
             "outputs": {
               "transform": "mvs_ref_in_tracker"
             }

src/core/math_helper.hpp

@@ -19,6 +19,20 @@ size_t alignment_round(size_t size, const size_t align = Align) {
     return size;
 }
 
+template<> struct fmt::formatter<glm::vec2> {
+    constexpr auto parse(format_parse_context &ctx) { return ctx.begin(); }
+    template<typename Context> constexpr auto format(const glm::vec2 &vec, Context &ctx) const {
+        return format_to(ctx.out(), "({}, {})", vec.x, vec.y);
+    }
+};
+
+template<> struct fmt::formatter<glm::vec3> {
+    constexpr auto parse(format_parse_context &ctx) { return ctx.begin(); }
+    template<typename Context> constexpr auto format(const glm::vec3 &vec, Context &ctx) const {
+        return format_to(ctx.out(), "({}, {}, {})", vec.x, vec.y, vec.z);
+    }
+};
+
 // r in radius
 inline glm::mat4 to_transform_mat(glm::vec3 t, glm::vec3 r) {
     static constexpr auto unit_x = glm::vec3(1.0f, 0.0f, 0.0f);

src/core_v2/object_manager.cpp

@@ -99,3 +99,13 @@ object_manager::object_manager(const create_config conf)
 object_manager::~object_manager() = default;
 
 object_manager *main_ob = nullptr;
+
+namespace {
+    auto main_thread_id = std::this_thread::get_id();
+    obj_name_type last_name = 0;
+}
+
+obj_name_type next_name() {
+    assert(std::this_thread::get_id() == main_thread_id);
+    return ++last_name;
+}

src/core_v2/object_manager.h

@@ -108,6 +108,8 @@ private:
     std::unique_ptr<impl> pimpl;
 };
 
+obj_name_type next_name();
+
 extern object_manager *main_ob;
 
 #define OBJ_QUERY(type, name) \

src/device_v5/ndi_stray_point_tracker.cpp

@@ -1,4 +1,5 @@
 #include "ndi_stray_point_tracker.h"
+#include "core/math_helper.hpp"
 
 // from sophiar
 #include "utility/coro_worker.hpp"
@@ -16,6 +17,7 @@
 #include <boost/asio/detached.hpp>
 
 #include <shared_mutex>
+#include <spdlog/spdlog.h>
 
 using namespace sophiar;
 
@@ -97,9 +99,12 @@ struct ndi_stray_point_tracker::impl {
         const auto num_handle = reader.read_value<uint8_t>();
         reader.manual_offset(2 * sizeof(uint8_t) * num_handle); // skip the tool data
 
-        const auto num_points = reader.read_value<uint8_t>();
-        const auto ov_size = (num_points + 8) / 8;
-        reader.manual_offset(ov_size * sizeof(uint8_t)); // TODO: check point status, P56
+        auto       num_points = reader.read_value<uint8_t>();
+        const auto ov_size    = (num_points + 7) / 8;
+        for ( auto k = 0; k < ov_size; ++k ) { // Out of volume points will not be reported.
+            const auto ov_flag = reader.read_value<uint8_t>();
+            num_points -= std::popcount(ov_flag);
+        }
         if (true) {
             auto lock = std::unique_lock(mu);
             last_points.resize(num_points);

src/image_process/camera_calibrator.h

@@ -34,16 +34,12 @@ public:
 
         uint8_t num_threads = 2;
         timestamp_type min_interval = 1e5; // 100ms, 10fps
-        smart_cuda_stream *stream = nullptr;
-        using io_context = boost::asio::io_context;
-        io_context *ctx;
 
         using cb_func_type =
                 std::function<void(result_type)>;
         cb_func_type cb_func;
 
         // for sophiar
-        sophiar_conn_type *sophiar_conn = nullptr;
         std::string ref_transform_var;
         std::string ref_error_var;
         std::string result_transform_var;

src/image_process/impl/camera_calibrator.cpp

@@ -1,6 +1,7 @@
 #include "camera_calibrator_impl.h"
 #include "core/imgui_utility.hpp"
 #include "third_party/scope_guard.hpp"
+#include "image_process_v5/image_process.h"
 #include "render/render_utility.h"
 #include "render/render_texture.h"
 
@@ -21,6 +22,8 @@
 #include <random>
 #include <ranges>
 
+extern sophiar::local_connection *g_sophiar_conn;
+
 namespace camera_calibrator_impl {
 
     cv::Mat to_cv_mat(const glm::mat3 &mat) {
@@ -600,10 +603,10 @@ void camera_calibrator::impl::process() {
 
     calib->calc();
 
-    if (conf.sophiar_conn != nullptr) {
+    if (g_sophiar_conn != nullptr) {
         auto ret = Eigen::Isometry3d();
         to_eigen_transform(calib->result_mat, ret);
-        conf.sophiar_conn->update_transform_variable(
+        g_sophiar_conn->update_transform_variable(
                 conf.result_transform_var, ret);
     }
     if (conf.cb_func) {
@@ -710,13 +713,13 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
     auto closer = sg::make_scope_guard([&] {
         st->process_finished = true;
         // release image to save memory
-        st->img_mat = cv::Mat();
-        st->img = nullptr;
+        st->img = {};
     });
 
     // color to gray
-    cv::Mat img_gray;
-    cv::cvtColor(st->img_mat, img_gray, cv::COLOR_BGR2GRAY);
+    auto img_gray = image_rgb_to_gray(st->img);
+    auto read_helper = read_access_helper(img_gray.host());
+    auto img_gray_mat = img_gray.cv_mat(read_helper.ptr());
 
     // find control points
     assert(conf.board_type == CALIB_CHESS);
@@ -724,7 +727,7 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
                        | cv::CALIB_CB_NORMALIZE_IMAGE
                        | cv::CALIB_CB_FAST_CHECK;
     corner_type img_corners;
-    bool ok = cv::findChessboardCorners(img_gray, conf.pattern_size,
+    bool ok = cv::findChessboardCorners(img_gray_mat, conf.pattern_size,
                                         img_corners, corner_flag);
     if (!ok) {
 //        last_finish.store(nullptr, std::memory_order::relaxed);
@@ -736,12 +739,12 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
     auto win_size = cv::Size(10, 10);
     auto zero_zone = cv::Size(-1, -1);
     auto term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
-    cv::cornerSubPix(img_gray, img_corners, win_size, zero_zone, term_crit);
+    cv::cornerSubPix(img_gray_mat, img_corners, win_size, zero_zone, term_crit);
     st->corners = img_corners;
 
     // evaluate sharpness
     auto sharpness = cv::estimateChessboardSharpness(
-            img_gray, conf.pattern_size, img_corners);
+            img_gray_mat, conf.pattern_size, img_corners);
     st->corner_sharpness = sharpness[0];
     if (st->corner_sharpness > sharpness_threshold) return;
 
@@ -750,25 +753,28 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
 
 void camera_calibrator::impl::img_callback(obj_name_type name) {
     assert(name == conf.img_name);
-    auto img = to_image(name);
-    if (img == nullptr) return;
+    auto img = sp_image();
+    try {
+        img = OBJ_QUERY(sp_image, conf.img_name);
+    } catch (...) { return; }
+    if (img.empty()) return;
 
     if (img_size.empty()) {
-        img_size = img->size();
+        img_size = img.cv_size();
     } else {
-        assert(img_size == img->size());
+        assert(img_size == img.cv_size());
     }
 
     // tracking information
     auto cur_ts = OBJ_TS(conf.img_name);
-    auto ref_t = conf.sophiar_conn
+    auto ref_t = g_sophiar_conn
             ->query_transform_variable(conf.ref_transform_var);
     if (!ref_t.has_value()) return;
     auto &item = track_pool.emplace_back();
     item.ref_mat = to_mat4(*ref_t);
     item.sample_ts = cur_ts;
 
-    auto err_t = conf.sophiar_conn
+    auto err_t = g_sophiar_conn
             ->query_double_variable(conf.ref_error_var);
     if (!err_t.has_value()) return;
 //    assert(err_t.has_value());
@@ -783,7 +789,6 @@ void camera_calibrator::impl::img_callback(obj_name_type name) {
 
     auto &st = img_pool.emplace_back();
     st.img = img;
-    st.img_mat = img->cv_mat(conf.stream);
     st.sample_ts = cur_ts;
 
     // post task
@@ -847,17 +852,16 @@ void camera_calibrator::impl::render() {
 }
 
 void camera_calibrator::impl::show() {
-    auto ctx = conf.ctx;
     bool is_running = (tp != nullptr);
 
     ImGui::SeparatorText("Actions");
     if (!is_running) {
         if (ImGui::Button("Start")) {
-            post(*ctx, [this] { start(); });
+            MAIN_DETACH([this] { start(); });
         }
     } else {
         if (ImGui::Button("Close")) {
-            post(*ctx, [this] { stop(); });
+            MAIN_DETACH([this] { stop(); });
         }
     }
 

src/image_process/impl/camera_calibrator_impl.h

@@ -19,7 +19,7 @@ namespace camera_calibrator_impl {
 
     // TODO: make configurable
     static constexpr auto sharpness_threshold = 4.f;
-    static constexpr auto track_err_threshold = 0.25f;
+    static constexpr auto track_err_threshold = 0.5f; // old 0.25
     static constexpr auto sample_neighbor_radius = 5.f;
 
     // for evaluation
@@ -142,8 +142,7 @@ struct camera_calibrator::impl {
     conn_type img_conn;
 
     struct img_store_type {
-        image_ptr img;
-        cv::Mat img_mat;
+        sp_image img;
         timestamp_type sample_ts;
         corner_type corners;
         float corner_sharpness;

src/image_process_v5/image_process.cpp

@@ -2,7 +2,6 @@
 
 #include <glm/ext/matrix_transform.hpp>
 #include <glm/gtx/matrix_transform_2d.hpp>
-
 #include <opencv2/cudaarithm.hpp>
 #include <opencv2/cudaimgproc.hpp>
 #include <opencv2/cudawarping.hpp>
@@ -14,12 +13,10 @@ namespace {
 
     auto &get_cv_stream() {
         current_cuda_stream(); // initialize CUDA
-        if (!cv_stream) [[unlikely]] {
-            cv_stream.emplace();
-        }
+        if ( !cv_stream ) [[unlikely]] { cv_stream.emplace(); }
         return *cv_stream;
     }
-}
+} // namespace
 
 size_t normal_height_to_nv12(const size_t height) {
     assert(height % 2 == 0);
@@ -48,8 +45,7 @@ sp_image nv12_luma_view(const sp_image &img) {
 sp_image nv12_chrome_view(const sp_image &img) {
     assert(img.cv_type() == CV_8UC1);
     const auto chroma_size = cv::Size(img.width(), img.height() / 3);
-    const auto img_chrome = img.sub_view(
-        chroma_size, cv::Size(0, nv12_height_to_normal(img.height())));
+    const auto img_chrome  = img.sub_view(chroma_size, cv::Size(0, nv12_height_to_normal(img.height())));
     return img_chrome.cast_view(CV_8UC2);
 }
 
@@ -68,14 +64,16 @@ cv::cuda::Stream &cv_stream_guard::cv_stream() {
 namespace {
     struct image_opencv_cuda_helper {
         const sp_image *read;
-        sp_image *write;
+        sp_image       *write;
         using proxy_type = auto_memory_info::cuda_proxy;
-        cv_stream_guard stream_guard;
+        cv_stream_guard                            stream_guard;
         pair_access_helper<proxy_type, proxy_type> access_helper;
 
         image_opencv_cuda_helper(const sp_image &src, sp_image &dst)
-            : read(&src), write(&dst),
-              access_helper(read->cuda(), write->cuda()) { (void) 0; }
+            : read(&src), write(&dst), //
+              access_helper(read->cuda(), write->cuda()) {
+            (void) 0;
+        }
 
         [[nodiscard]] cv::cuda::GpuMat input() const {
             return read->cv_gpu_mat(access_helper.read_ptr());
@@ -85,14 +83,36 @@ namespace {
             return write->cv_gpu_mat(access_helper.write_ptr());
         }
     };
-}
+
+    struct image_opencv_host_helper {
+        const sp_image *read;
+        sp_image       *write;
+        using proxy_type = auto_memory_info::host_proxy;
+        cv_stream_guard                            stream_guard;
+        pair_access_helper<proxy_type, proxy_type> access_helper;
+
+        image_opencv_host_helper(const sp_image &src, sp_image &dst)
+            : read(&src), write(&dst), //
+              access_helper(read->host(), write->host()) {
+            (void) 0;
+        }
+
+        [[nodiscard]] cv::Mat input() const {
+            return read->cv_mat(access_helper.read_ptr());
+        }
+
+        [[nodiscard]] cv::Mat output() const {
+            return write->cv_mat(access_helper.write_ptr());
+        }
+    };
+} // namespace
 
 sp_image image_debayer(const sp_image &img) {
     assert(img.cv_type() == CV_8UC1);
-    auto ret = sp_image::create<uchar3>(img.cv_size());
+    auto       ret    = sp_image::create<uchar3>(img.cv_size());
     const auto helper = image_opencv_cuda_helper(img, ret);
-    cv::cuda::cvtColor(helper.input(), helper.output(),
-                       cv::COLOR_BayerRG2BGR, 3, get_cv_stream());
+    cv::cuda::cvtColor(helper.input(), helper.output(), //
+        cv::COLOR_BayerRG2BGR, 3, get_cv_stream());
     ret.merge_meta(img);
     return ret;
 }
@@ -100,8 +120,8 @@ sp_image image_debayer(const sp_image &img) {
 void image_resize(const sp_image &src, sp_image &dst) {
     assert(src.cv_type() == dst.cv_type());
     const auto helper = image_opencv_cuda_helper(src, dst);
-    cv::cuda::resize(helper.input(), helper.output(),
-                     dst.cv_size(), 0, 0, cv::INTER_LINEAR, get_cv_stream());
+    cv::cuda::resize(helper.input(), helper.output(), //
+        dst.cv_size(), 0, 0, cv::INTER_LINEAR, get_cv_stream());
     dst.merge_meta(src);
 }
 
@@ -112,7 +132,7 @@ sp_image image_resize(const sp_image &img, const cv::Size size) {
 }
 
 sp_image image_flip_y(const sp_image &img) {
-    auto ret = sp_image::create(img.cv_type(), img.cv_size());
+    auto       ret    = sp_image::create(img.cv_type(), img.cv_size());
     const auto helper = image_opencv_cuda_helper(img, ret);
     cv::cuda::flip(helper.input(), helper.output(), 1, get_cv_stream()); // flip vertically
     ret.merge_meta(img);
@@ -121,16 +141,12 @@ sp_image image_flip_y(const sp_image &img) {
 
 sp_image image_warp_affine(const sp_image &img, const glm::mat3 &matrix) {
     auto cv_matrix = cv::Mat(2, 3, CV_32FC1);
-    for (auto i = 0; i < 3; ++i)
-        for (auto j = 0; j < 2; ++j) {
-            cv_matrix.at<float>(j, i) = matrix[i][j];
-        }
-    auto ret = sp_image::create_like(img);
+    for ( auto i = 0; i < 3; ++i )
+        for ( auto j = 0; j < 2; ++j ) { cv_matrix.at<float>(j, i) = matrix[i][j]; }
+    auto       ret    = sp_image::create_like(img);
     const auto helper = image_opencv_cuda_helper(img, ret);
-    cv::cuda::warpAffine(helper.input(), helper.output(),
-                         cv_matrix, img.cv_size(), cv::INTER_LINEAR,
-                         cv::BORDER_CONSTANT, {},
-                         get_cv_stream());
+    cv::cuda::warpAffine(helper.input(), helper.output(), cv_matrix, img.cv_size(), //
+        cv::INTER_LINEAR, cv::BORDER_CONSTANT, {}, get_cv_stream());
     ret.merge_meta(img);
     return ret;
 }
@@ -139,19 +155,30 @@ namespace {
     float pixel_center(const float size) {
         return 0.5f * size - 0.5f;
     }
-}
+} // namespace
 
-sp_image image_rotate(const sp_image &img, const float angle,
-                      std::optional<glm::vec2> center) {
-    if (!center) {
-        center = glm::vec2(pixel_center(img.width()),
-                           pixel_center(img.height()));
+namespace {
+    glm::mat3 cv_mat23_to_mat3(const cv::Mat &mat) {
+        assert(mat.rows == 2 && mat.cols == 3);
+        assert(mat.type() == CV_64FC1);
+        auto ret = glm::mat3(0.f);
+        for ( auto i = 0; i < 3; ++i )
+            for ( auto j = 0; j < 2; ++j ) { //
+                ret[i][j] = mat.at<double>(j, i);
+            }
+        return ret;
     }
-    auto matrix = glm::identity<glm::mat3>();
-    matrix = glm::translate(matrix, -*center);
-    matrix = glm::rotate(matrix, angle);
-    matrix = glm::translate(matrix, *center);
-    return image_warp_affine(img, matrix);
+} // namespace
+
+sp_image image_rotate(const sp_image &img, const float angle, std::optional<glm::vec2> center) {
+    if ( !center ) {
+        center = glm::vec2(            //
+            pixel_center(img.width()), //
+            pixel_center(img.height()));
+    }
+    const auto center_cv = cv::Point2f(center->x, center->y);
+    const auto matrix_cv = cv::getRotationMatrix2D(center_cv, angle, 1);
+    return image_warp_affine(img, cv_mat23_to_mat3(matrix_cv));
 }
 
 sp_image image_translate(const sp_image &img, const glm::vec2 offset) {
@@ -162,12 +189,11 @@ sp_image image_translate(const sp_image &img, const glm::vec2 offset) {
 image_stereo_pair image_stereo_split(const sp_image &img) {
     assert(img.width() % 2 == 0);
     const auto mono_size = cv::Size(img.width() / 2, img.height());
-    auto ret_left = sp_image::create(img.cv_type(), mono_size);
-    const auto img_left = img.sub_view(mono_size);
+    auto       ret_left  = sp_image::create(img.cv_type(), mono_size);
+    const auto img_left  = img.sub_view(mono_size);
     copy_sp_image(img_left, ret_left);
-    auto ret_right = sp_image::create(img.cv_type(), mono_size);
-    const auto img_right = img.sub_view(mono_size,
-                                        cv::Size(img.width() / 2, 0));
+    auto       ret_right = sp_image::create(img.cv_type(), mono_size);
+    const auto img_right = img.sub_view(mono_size, cv::Size(img.width() / 2, 0));
     copy_sp_image(img_right, ret_right);
     return std::make_tuple(ret_left, ret_right);
 }
@@ -175,17 +201,16 @@ image_stereo_pair image_stereo_split(const sp_image &img) {
 image_stereo_pair image_stereo_split_view(const sp_image &img) {
     assert(img.width() % 2 == 0);
     const auto mono_size = cv::Size(img.width() / 2, img.height());
-    const auto img_left = img.sub_view(mono_size);
-    const auto img_right = img.sub_view(mono_size,
-                                        cv::Size(img.width() / 2, 0));
+    const auto img_left  = img.sub_view(mono_size);
+    const auto img_right = img.sub_view(mono_size, cv::Size(img.width() / 2, 0));
     return std::make_tuple(img_left, img_right);
 }
 
 sp_image image_stereo_combine(const sp_image &left, const sp_image &right) {
     assert(left.cv_type() == right.cv_type());
     assert(left.shape_array() == right.shape_array());
-    const auto stereo_shape = cv::Size(left.width() * 2, left.height());
-    auto ret_img = sp_image::create(left.cv_type(), stereo_shape);
+    const auto stereo_shape      = cv::Size(left.width() * 2, left.height());
+    auto       ret_img           = sp_image::create(left.cv_type(), stereo_shape);
     auto [left_view, right_view] = image_stereo_split_view(ret_img);
     copy_sp_image(left, left_view);
     copy_sp_image(right, right_view);
@@ -200,13 +225,14 @@ namespace {
     template<typename Input, typename Output>
     struct image_cuda_v2_helper {
         const sp_image *read;
-        sp_image *write;
+        sp_image       *write;
         using proxy_type = auto_memory_info::cuda_proxy;
         pair_access_helper<proxy_type, proxy_type> access_helper;
 
         image_cuda_v2_helper(const sp_image &src, sp_image &dst)
-            : read(&src), write(&dst),
-              access_helper(read->cuda(), write->cuda()) { (void) 0; }
+            : read(&src), write(&dst), access_helper(read->cuda(), write->cuda()) {
+            (void) 0;
+        }
 
         template<typename T = Input>
         image_type_v2<T> input() {
@@ -218,11 +244,11 @@ namespace {
             return to_cuda_v2(write->as_ndarray<T>(access_helper.write_ptr()));
         }
     };
-}
+} // namespace
 
 sp_image image_rgb_to_bgr(const sp_image &img) {
     assert(img.cv_type() == CV_8UC3);
-    auto ret = sp_image::create(CV_8UC3, img.cv_size());
+    auto ret    = sp_image::create(CV_8UC3, img.cv_size());
     auto helper = image_cuda_v2_helper<uchar3, uchar3>(img, ret);
     call_cvt_rgb_bgr_u8(helper.input(), helper.output(), current_cuda_stream());
     ret.merge_meta(img);
@@ -231,7 +257,7 @@ sp_image image_rgb_to_bgr(const sp_image &img) {
 
 sp_image image_rgb_to_bgra(const sp_image &img) {
     assert(img.cv_type() == CV_8UC3);
-    auto ret = sp_image::create(CV_8UC4, img.cv_size());
+    auto ret    = sp_image::create(CV_8UC4, img.cv_size());
     auto helper = image_cuda_v2_helper<uchar3, uchar4>(img, ret);
     call_cvt_rgb_bgra_u8(helper.input(), helper.output(), current_cuda_stream());
     ret.merge_meta(img);
@@ -240,17 +266,34 @@ sp_image image_rgb_to_bgra(const sp_image &img) {
 
 sp_image image_rgb_to_gray(const sp_image &img) {
     assert(img.cv_type() == CV_8UC3);
-    auto ret = sp_image::create<uchar1>(img.cv_size());
+    auto       ret    = sp_image::create<uchar1>(img.cv_size());
     const auto helper = image_opencv_cuda_helper(img, ret);
-    cv::cuda::cvtColor(helper.input(), helper.output(),
-                       cv::COLOR_RGB2GRAY, 1, get_cv_stream());
+    cv::cuda::cvtColor(helper.input(), helper.output(), cv::COLOR_RGB2GRAY, 1, get_cv_stream());
+    ret.merge_meta(img);
+    return ret;
+}
+
+sp_image image_gray_to_binary(const sp_image &img, float threshold) {
+    assert(img.cv_type() == CV_8UC1);
+    auto ret    = sp_image::create<uchar1>(img.cv_size());
+    auto status = img.mem->status();
+    if ( status.cuda_available ) {
+        const auto helper = image_opencv_cuda_helper(img, ret);
+        cv::cuda::threshold(helper.input(), helper.output(), //
+            threshold, 255, cv::THRESH_BINARY);
+    } else {
+        assert(status.host_available);
+        const auto helper = image_opencv_host_helper(img, ret);
+        cv::threshold(helper.input(), helper.output(), //
+            threshold, 255, cv::THRESH_BINARY);
+    }
     ret.merge_meta(img);
     return ret;
 }
 
 sp_image image_rgb_to_nv12(const sp_image &img) {
     assert(img.cv_type() == CV_8UC3);
-    auto ret = sp_image::create(CV_8UC1, normal_size_to_nv12(img.cv_size()));
+    auto ret    = sp_image::create(CV_8UC1, normal_size_to_nv12(img.cv_size()));
     auto helper = image_cuda_v2_helper<uchar3, uchar1>(img, ret);
     call_rgb_to_nv12(helper.input(), helper.output(), current_cuda_stream());
     ret.merge_meta(img);
@@ -259,7 +302,7 @@ sp_image image_rgb_to_nv12(const sp_image &img) {
 
 sp_image image_nv12_to_rgb(const sp_image &img) {
     assert(img.cv_type() == CV_8UC1);
-    auto ret = sp_image::create(CV_8UC3, nv12_size_to_normal(img.cv_size()));
+    auto ret    = sp_image::create(CV_8UC3, nv12_size_to_normal(img.cv_size()));
     auto helper = image_cuda_v2_helper<uchar1, uchar3>(img, ret);
     call_nv12_to_rgb(helper.input(), helper.output(), current_cuda_stream());
     ret.merge_meta(img);
@@ -268,7 +311,7 @@ sp_image image_nv12_to_rgb(const sp_image &img) {
 
 sp_image image_yuyv_to_rgb(const sp_image &img) {
     assert(img.cv_type() == CV_8UC2);
-    auto ret = sp_image::create(CV_8UC3, img.cv_size());
+    auto ret    = sp_image::create(CV_8UC3, img.cv_size());
     auto helper = image_cuda_v2_helper<uchar2, uchar3>(img, ret);
     call_yuyv_to_rgb(helper.input(), helper.output(), current_cuda_stream());
     ret.merge_meta(img);
@@ -277,24 +320,21 @@ sp_image image_yuyv_to_rgb(const sp_image &img) {
 
 sp_image image_remap_np_to_tex(const sp_image &img, const float fov, const float aspect) {
     assert(img.cv_type() == CV_32FC2);
-    auto ret = sp_image::create(CV_32FC2, img.cv_size());
+    auto ret    = sp_image::create(CV_32FC2, img.cv_size());
     auto helper = image_cuda_v2_helper<float2, float2>(img, ret);
-    call_np_to_tex(helper.input(), helper.output(),
-                   fov, aspect, current_cuda_stream());
+    call_np_to_tex(helper.input(), helper.output(), fov, aspect, current_cuda_stream());
     ret.merge_meta(img);
     return ret;
 }
 
 namespace {
     void image_save_opencv(sp_image img, const std::string &filename) {
-        if (CV_MAT_CN(img.cv_type()) == 3) {
-            img = image_rgb_to_bgr(img);
-        }
-        const auto helper = read_access_helper(img.host());
+        if ( CV_MAT_CN(img.cv_type()) == 3 ) { img = image_rgb_to_bgr(img); }
+        const auto helper  = read_access_helper(img.host());
         const auto img_mat = img.cv_mat(helper.ptr());
         cv::imwrite(filename, img_mat);
     }
-}
+} // namespace
 
 void image_save_jpg(const sp_image &img, const std::string &filename) {
     image_save_opencv(img, fmt::format("{}.jpg", filename));
@@ -311,15 +351,15 @@ struct image_output_helper::impl {
     obj_conn_type conn;
 
     void image_callback_impl() {
-        const auto img = OBJ_QUERY(sp_image, conf.in_name);
-        auto ret_rect = simple_rect(0, 0, conf.size.width, conf.size.height);
-        ret_rect = ret_rect.fit_aspect(img.cv_size().aspectRatio());
-        auto ret_img = sp_image::create(img.cv_type(), conf.size);
+        const auto img      = OBJ_QUERY(sp_image, conf.in_name);
+        auto       ret_rect = simple_rect(0, 0, conf.size.width, conf.size.height);
+        ret_rect            = ret_rect.fit_aspect(img.cv_size().aspectRatio());
+        auto ret_img        = sp_image::create(img.cv_type(), conf.size);
         ret_img.initialize_meta();
-        auto ret_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height),
-                                         cv::Size(ret_rect.x, ret_rect.y));
+        auto ret_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height), //
+            cv::Size(ret_rect.x, ret_rect.y));
         image_resize(img, ret_view);
-        if (conf.flip_y) ret_img = image_flip_y(ret_img);
+        if ( conf.flip_y ) ret_img = image_flip_y(ret_img);
         OBJ_SAVE(conf.out_name, ret_img);
     }
 
@@ -327,12 +367,11 @@ struct image_output_helper::impl {
         assert(conf.in_name == _name);
         try {
             image_callback_impl();
-        } catch (...) { (void) 0; }
+        } catch ( ... ) { (void) 0; }
     }
 
     explicit impl(const create_config _conf) : conf(_conf) {
-        conn = OBJ_SIG(conf.in_name)->connect(
-            [this](auto name) { image_callback(name); });
+        conn = OBJ_SIG(conf.in_name)->connect([this](auto name) { image_callback(name); });
     }
 
     ~impl() {
@@ -340,9 +379,7 @@ struct image_output_helper::impl {
     }
 };
 
-image_output_helper::image_output_helper(create_config conf)
-    : pimpl(std::make_unique<impl>(conf)) {
-}
+image_output_helper::image_output_helper(create_config conf) : pimpl(std::make_unique<impl>(conf)) { }
 
 image_output_helper::~image_output_helper() = default;
 
@@ -350,59 +387,58 @@ struct stereo_output_helper::impl {
     create_config conf;
     obj_conn_type left_conn, right_conn;
 
-    bool left_updated = false;
+    bool left_updated  = false;
     bool right_updated = false;
 
     void image_callback_impl() {
-        const auto left_img = OBJ_QUERY(sp_image, conf.left_name);
+        const auto left_img  = OBJ_QUERY(sp_image, conf.left_name);
         const auto right_img = OBJ_QUERY(sp_image, conf.right_name);
         assert(left_img.cv_type() == right_img.cv_type());
         assert(left_img.cv_size() == right_img.cv_size());
         auto ret_size = conf.size;
-        if (ret_size.empty()) {
-            if (conf.halve_width) {
+        if ( ret_size.empty() ) {
+            if ( conf.halve_width ) {
                 ret_size = left_img.cv_size();
             } else {
                 ret_size = cv::Size(left_img.width() * 2, left_img.height());
             }
         }
         assert(ret_size.width % 2 == 0);
-        auto ret_rect = simple_rect(0, 0,
-                                    conf.halve_width ? ret_size.width : (ret_size.width / 2),
-                                    ret_size.height);
-        ret_rect = ret_rect.fit_aspect(left_img.cv_size().aspectRatio());
-        if (conf.halve_width) {
+        auto ret_rect = simple_rect(0, 0, //
+            conf.halve_width ? ret_size.width : (ret_size.width / 2), ret_size.height);
+        ret_rect      = ret_rect.fit_aspect(left_img.cv_size().aspectRatio());
+        if ( conf.halve_width ) {
             ret_rect.x /= 2;
             ret_rect.width /= 2;
         }
         auto ret_img = sp_image::create(left_img.cv_type(), ret_size);
         ret_img.initialize_meta();
-        auto left_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height),
-                                          cv::Size(ret_rect.x, ret_rect.y));
+        auto left_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height), //
+            cv::Size(ret_rect.x, ret_rect.y));
         image_resize(left_img, left_view);
-        auto right_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height),
-                                           cv::Size(ret_rect.x + ret_size.width / 2, ret_rect.y));
+        auto right_view = ret_img.sub_view(cv::Size(ret_rect.width, ret_rect.height), //
+            cv::Size(ret_rect.x + ret_size.width / 2, ret_rect.y));
         image_resize(right_img, right_view);
-        if (conf.flip_y) ret_img = image_flip_y(ret_img);
+        if ( conf.flip_y ) ret_img = image_flip_y(ret_img);
         OBJ_SAVE(conf.out_name, ret_img);
     }
 
     void image_callback(const obj_name_type name) {
-        if (name == conf.left_name) left_updated = true;
-        if (name == conf.right_name) right_updated = true;
-        if (!left_updated || !right_updated) return;
+        if ( name == conf.left_name ) left_updated = true;
+        if ( name == conf.right_name ) right_updated = true;
+        if ( !left_updated || !right_updated ) return;
         try {
             image_callback_impl();
-        } catch (...) { (void) 0; }
-        left_updated = false;
+        } catch ( ... ) { (void) 0; }
+        left_updated  = false;
         right_updated = false;
     }
 
     explicit impl(const create_config &_conf) : conf(_conf) {
-        left_conn = OBJ_SIG(conf.left_name)->connect(
-            [this](auto name) { image_callback(name); });
-        right_conn = OBJ_SIG(conf.right_name)->connect(
-            [this](auto name) { image_callback(name); });
+        left_conn = OBJ_SIG(conf.left_name) //
+                        ->connect([this](auto name) { image_callback(name); });
+        right_conn = OBJ_SIG(conf.right_name) //
+                         ->connect([this](auto name) { image_callback(name); });
     }
 
     ~impl() {
@@ -411,8 +447,6 @@ struct stereo_output_helper::impl {
     }
 };
 
-stereo_output_helper::stereo_output_helper(create_config conf)
-    : pimpl(std::make_unique<impl>(conf)) {
-}
+stereo_output_helper::stereo_output_helper(create_config conf) : pimpl(std::make_unique<impl>(conf)) { }
 
 stereo_output_helper::~stereo_output_helper() = default;

src/image_process_v5/image_process.h

@@ -11,21 +11,22 @@ struct cv_stream_guard {
     cv::cuda::Stream &cv_stream();
 };
 
-size_t normal_height_to_nv12(size_t height);
-size_t nv12_height_to_normal(size_t height);
+size_t   normal_height_to_nv12(size_t height);
+size_t   nv12_height_to_normal(size_t height);
 cv::Size normal_size_to_nv12(cv::Size size);
 cv::Size nv12_size_to_normal(cv::Size size);
 
-sp_image nv12_luma_view(const sp_image& img);
-sp_image nv12_chrome_view(const sp_image& img);
+sp_image nv12_luma_view(const sp_image &img);
+sp_image nv12_chrome_view(const sp_image &img);
 
 sp_image image_debayer(const sp_image &img); // TODO: add an option for bayer type
-void image_resize(const sp_image &src, sp_image &dst);
+void     image_resize(const sp_image &src, sp_image &dst);
 sp_image image_resize(const sp_image &img, cv::Size size);
 sp_image image_flip_y(const sp_image &img);
 sp_image image_rgb_to_bgr(const sp_image &img);
 sp_image image_rgb_to_bgra(const sp_image &img);
 sp_image image_rgb_to_gray(const sp_image &img);
+sp_image image_gray_to_binary(const sp_image &img, float threshold = 10);
 sp_image image_rgb_to_nv12(const sp_image &img);
 sp_image image_nv12_to_rgb(const sp_image &img);
 sp_image image_yuyv_to_rgb(const sp_image &img);
@@ -34,13 +35,13 @@ sp_image image_yuyv_to_rgb(const sp_image &img);
 sp_image image_remap_np_to_tex(const sp_image &img, float fov, float aspect);
 
 sp_image image_warp_affine(const sp_image &img, const glm::mat3 &matrix);
-sp_image image_rotate(const sp_image &img, float angle, std::optional<glm::vec2> center);
+sp_image image_rotate(const sp_image &img, float angle, std::optional<glm::vec2> center); // angle in degree
 sp_image image_translate(const sp_image &img, glm::vec2 offset);
 
 using image_stereo_pair = std::tuple<sp_image, sp_image>;
 image_stereo_pair image_stereo_split(const sp_image &img);
 image_stereo_pair image_stereo_split_view(const sp_image &img);
-sp_image image_stereo_combine(const sp_image &left, const sp_image &right);
+sp_image          image_stereo_combine(const sp_image &left, const sp_image &right);
 
 void image_save_jpg(const sp_image &img, const std::string &filename); // filename without extension
 void image_save_png(const sp_image &img, const std::string &filename);
@@ -51,11 +52,13 @@ class image_output_helper {
 public:
     struct create_config {
         obj_name_type in_name, out_name;
-        cv::Size size;
-        bool flip_y = false;
+        cv::Size      size;
+        bool          flip_y = false;
     };
+
     explicit image_output_helper(create_config conf);
     ~image_output_helper();
+
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;
@@ -66,15 +69,17 @@ public:
     struct create_config {
         obj_name_type left_name, right_name;
         obj_name_type out_name;
-        cv::Size size; // if empty(), will be determined from input
-        bool halve_width = false;
-        bool flip_y = false;
+        cv::Size      size; // if empty(), will be determined from input
+        bool          halve_width = false;
+        bool          flip_y      = false;
     };
+
     explicit stereo_output_helper(create_config conf);
     ~stereo_output_helper();
+
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;
 };
 
-#endif //IMAGE_PROCESS_H
+#endif // IMAGE_PROCESS_H

src/impl/apps/depth_guide_v2/depth_guide_v2.cpp

@@ -57,7 +57,7 @@ app_depth_guide_v2::app_depth_guide_v2(const create_config &_conf) {
     orb_cam = std::make_unique<orb_camera_ui>(orb_cam_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_list = augment_manager_v2::item_list_from_v1(aug_list_v1);
     auto aug_conf = augment_manager_v2::create_config{
             .scene_name = scene_out,
             .sophiar_conn = sophiar_conn.get(),

src/impl/apps/endo_guide/endo_guide.cpp

@@ -64,13 +64,13 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
             .img_name = img_color,
             .board_type = camera_calibrator::CALIB_CHESS,
             .pattern_size = cv::Size(11, 8), .corner_distance = 5,
-            .stream = default_cuda_stream, .ctx = main_conf.asio_ctx,
+            // .stream = default_cuda_stream, .ctx = main_conf.asio_ctx,
             .cb_func = [this](const auto &ret) {
                 aug_helper->set_camera_info(ret.cam_in);
                 aug_helper->get_camera_helper()
                         ->set_fixed_transform_latency(ret.time_offset);
             },
-            .sophiar_conn = sophiar_conn.get(),
+            // .sophiar_conn = sophiar_conn.get(),
             .ref_transform_var = LOAD_STR("camera_ref_transform_var"),
             .ref_error_var =  LOAD_STR("camera_ref_error_var"),
             .result_transform_var = LOAD_STR("camera_calib_transform_var"),
@@ -83,7 +83,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
 //    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_list = augment_manager_v2::item_list_from_v1(aug_list_v1);
     auto aug_conf = augment_manager_v2::create_config{
             .sophiar_conn = sophiar_conn.get(),
             .stream = default_cuda_stream,

src/impl/apps/remote_ar/remote_ar.cpp

@@ -101,7 +101,7 @@ app_remote_ar::app_remote_ar(const create_config &_conf) {
     guide_controller = std::make_unique<depth_guide_controller>(guide_control_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_list = augment_manager_v2::item_list_from_v1(aug_list_v1);
     auto aug_conf = augment_manager_v2::create_config{
             .item_list = aug_list,
             .sophiar_conn = sophiar_conn.get(),

src/impl/apps/remote_ar/remote_ar_v2.cpp

@@ -1,89 +1,170 @@
 #include "remote_ar_v2.h"
-#include "image_process/camera_utility.hpp"
-#include "image_process_v5/sp_image.h"
-#include "image_process_v5/osg_helper.h"
-#include "image_process_v5/image_process.h"
-#include "core/yaml_utility.hpp"
+
 #include "core/imgui_utility.hpp"
+#include "core/yaml_utility.hpp"
 #include "device_v5/ndi_stray_point_tracker.h"
+#include "image_process/camera_utility.hpp"
+#include "image_process_v5/image_process.h"
+#include "image_process_v5/osg_helper.h"
+#include "image_process_v5/sp_image.h"
 
 // from sophiar
-#include <image_process_v5/cuda_impl/image_enhance.cuh>
-
 #include "core/local_connection.h"
 
 extern sophiar::local_connection *g_sophiar_conn;
 
-app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
-    : main_conf(std::move(_conf)) {
+app_remote_ar_v2::app_remote_ar_v2(create_config _conf) : main_conf(std::move(_conf)) {
     auto conf = main_conf.ext_conf;
 
-    if (true) {
-        auto sub_conf = mvs_camera_ui::create_config{.ctx = main_conf.asio_ctx};
-        sub_conf.cameras.push_back({.dev_name = LOAD_STR("left_camera_name"), .img_name = left_img_id});
-        sub_conf.cameras.push_back({.dev_name = LOAD_STR("right_camera_name"), .img_name = right_img_id});
+    auto left_img_id = next_name(), right_img_id = next_name();
+    {
+        auto sub_conf = mvs_camera_ui::create_config { .ctx = main_conf.asio_ctx };
+        sub_conf.cameras.push_back({ .dev_name = LOAD_STR("left_camera_name"), .img_name = left_img_id });
+        sub_conf.cameras.push_back({ .dev_name = LOAD_STR("right_camera_name"), .img_name = right_img_id });
         mvs_cam.emplace(sub_conf);
-        // mvs_cam->cap_info_sig.connect([this](auto info) {
-        //     out_streamer->change_frame_rate(info.frame_rate);
-        // });
     }
 
-    if (true) {
-        auto uvc_cam_conf = uvc_camera_ui::create_config{
+    auto uvc_img_id = next_name();
+    {
+        auto uvc_cam_conf = uvc_camera_ui::create_config {
             .img_name = uvc_img_id,
-            .ctx = main_conf.asio_ctx,
+            .ctx      = main_conf.asio_ctx,
         };
         uvc_cam.emplace(uvc_cam_conf);
     }
 
-    if (true) {
-        auto sub_conf = image_enhance_ui::create_config();
-        sub_conf.in_name = left_img_id;
+    auto left_fix_id = next_name(), right_fix_id = next_name();
+    {
+        auto sub_conf     = image_enhance_ui::create_config();
+        sub_conf.in_name  = left_img_id;
         sub_conf.out_name = left_fix_id;
         left_cam.img_isp.emplace(sub_conf);
 
-        sub_conf.in_name = right_img_id;
+        sub_conf.in_name  = right_img_id;
         sub_conf.out_name = right_fix_id;
         right_cam.img_isp.emplace(sub_conf);
         right_cam.img_isp->sync_with(&left_cam.img_isp.value());
     }
 
-    if (true) {
-        auto sub_conf = video_stabilization_ui::create_config();
-        sub_conf.opts.stereo_mode = true;
-        sub_conf.in_name = left_aug_id;
-        sub_conf.in2_name = right_aug_id;
-        sub_conf.out_name = left_stab_id;
-        sub_conf.out2_name = right_stab_id;
-        video_stab.emplace(sub_conf);
+    start_sophiar(LOAD_STR("sophiar_config"), LOAD_STR("sophiar_start_name"));
+    {
+        auto sub_conf    = ndi_stray_point_tracker::create_config();
+        sub_conf.ip_addr = LOAD_STR("ndi_ip");
+        sub_conf.port    = LOAD_NUMBER(uint16_t, "ndi_port");
+        create_ndi_stray_points_tracker(sub_conf);
     }
 
-    if (true) {
-        auto sub_conf = stereo_output_helper::create_config();
-        sub_conf.left_name = left_stab_id;
-        sub_conf.right_name = right_stab_id;
-        sub_conf.out_name = output_img_id;
-        sub_conf.size = cv::Size(1920, 804);
+    {
+        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);
+        auto aug_conf         = augment_manager_v2::create_config();
+        aug_conf.sophiar_conn = g_sophiar_conn;
+        std::copy(aug_list.begin(), aug_list.end(), std::back_inserter(aug_conf.item_list));
+        aug_manager.emplace(aug_conf);
+        // aug_manager->pre_render_sig.connect(
+        //     [this](const auto &info) { acl->pre_render_slot(info); });
+    }
+
+    auto left_aug_id = next_name(), right_aug_id = next_name();
+    auto uvc_inv_rot_id = next_name();
+    auto uvc_aug_id     = next_name();
+    {
+        auto sub_conf          = image_augment_helper_v2::create_config();
+        sub_conf.img_flip_y    = false;
+        sub_conf.img_name      = left_fix_id;
+        sub_conf.out_name      = left_aug_id;
+        sub_conf.manager       = &aug_manager.value();
+        sub_conf.ctx           = main_conf.asio_ctx;
+        sub_conf.sophiar_conn  = g_sophiar_conn;
+        sub_conf.transform_var = LOAD_STR("left_camera_transform");
+        left_cam.aug_helper.emplace(sub_conf);
+        left_cam.aug_helper->set_camera_info(camera_intrinsic::from_yaml(LOAD_SUB("left_camera")).to_v0());
+
+        sub_conf.img_name      = right_fix_id;
+        sub_conf.out_name      = right_aug_id;
+        sub_conf.transform_var = LOAD_STR("right_camera_transform");
+        right_cam.aug_helper.emplace(sub_conf);
+        right_cam.aug_helper->set_camera_info(camera_intrinsic::from_yaml(LOAD_SUB("right_camera")).to_v0());
+
+        sub_conf.img_name      = uvc_inv_rot_id;
+        sub_conf.out_name      = uvc_aug_id;
+        sub_conf.transform_var = "camera_in_tracker";
+        uvc_aug_helper.emplace(sub_conf);
+    }
+
+    // auto left_stab_id = next_name(), right_stab_id = next_name();
+    // if (true) {
+    //     auto sub_conf = video_stabilization_ui::create_config();
+    //     sub_conf.opts.stereo_mode = true;
+    //     sub_conf.in_name = left_aug_id;
+    //     sub_conf.in2_name = right_aug_id;
+    //     sub_conf.out_name = left_stab_id;
+    //     sub_conf.out2_name = right_stab_id;
+    //     video_stab.emplace(sub_conf);
+    // }
+
+    auto output_img_id = next_name();
+    {
+        auto sub_conf        = stereo_output_helper::create_config();
+        sub_conf.left_name   = left_aug_id;
+        sub_conf.right_name  = right_aug_id;
+        sub_conf.out_name    = output_img_id;
+        sub_conf.size        = cv::Size(1920, 804);
         sub_conf.halve_width = false;
         output_helper.emplace(sub_conf);
     }
 
-    if (true) {
+    auto uvc_out_id = next_name();
+    {
+        auto sub_conf                       = oblique_calibrator::create_config();
+        sub_conf.image_name                 = uvc_img_id;
+        sub_conf.angle_name                 = next_name();
+        sub_conf.image_inv_rotate_name      = uvc_inv_rot_id;
+        sub_conf.image_fix_in_name          = uvc_aug_id;
+        sub_conf.image_fix_out_name         = uvc_out_id;
+        sub_conf.camera_ref_name            = "camera_ref_in_tracker";
+        sub_conf.len_ref_to_camera_ref_name = "len_ref_in_camera_ref";
+        sub_conf.len_ref_name               = "len_ref_in_tracker";
+        sub_conf.board_type                 = camera_calibrator::CALIB_CHESS;
+        sub_conf.pattern_size               = cv::Size(11, 8);
+        sub_conf.corner_distance            = 1.5; // mm
+        sub_conf.camera_ref_error_name      = "camera_ref_in_tracker_error";
+        sub_conf.camera_to_len_ref_name     = "camera_in_len_ref";
+        sub_conf.cb_func                    = [this](const auto &ret) {
+            uvc_aug_helper->set_camera_info(ret.cam_in);
+            uvc_aug_helper->get_camera_helper()->set_fixed_transform_latency(ret.time_offset);
+        };
+        endo_calib.emplace(sub_conf);
+
+        uvc_aug_helper->post_render_sig.connect( //
+            [this] { endo_calib->render(); });
+
+        endo_calib->set_rotation_center(LOAD_STR("oblique_rotation_center"));
+        endo_calib->set_rotation_ball(LOAD_STR("oblique_rotation_ball"));
+        auto cam_sim      = camera_calibrator::simulate_info_type();
+        cam_sim.data_path = "calib_data_v2.txt";
+        cam_sim.img_size  = cv::Size(1920, 1080);
+        endo_calib->set_camera_calib(cam_sim);
+    }
+
+    {
         auto sub_conf = image_viewer_v2::create_config();
-        sub_conf.items.emplace_back(uvc_img_id, "Endoscope", true);
-        sub_conf.items.emplace_back(left_stab_id, "Left", true);
-        sub_conf.items.emplace_back(right_stab_id, "Right", true);
+        sub_conf.items.emplace_back(uvc_img_id, "Endo", true);
+        sub_conf.items.emplace_back(uvc_inv_rot_id, "EndoInvRot", true);
+        sub_conf.items.emplace_back(uvc_out_id, "EndoAug", true);
+        sub_conf.items.emplace_back(left_aug_id, "Left", true);
+        sub_conf.items.emplace_back(right_aug_id, "Right", true);
         bg_viewer.emplace(sub_conf);
     }
 
-    if (true) {
-        auto sub_conf = image_streamer::create_config();
+    {
+        auto sub_conf     = image_streamer::create_config();
         sub_conf.img_name = output_img_id;
         sub_conf.asio_ctx = main_conf.asio_ctx;
         streamer.emplace(sub_conf);
     }
 
-    if (true) {
+    {
         auto sub_conf = versatile_saver::create_config();
         sub_conf.items.emplace_back(left_aug_id, "Left", true);
         sub_conf.items.emplace_back(right_aug_id, "Right", true);
@@ -91,56 +172,30 @@ app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
         saver.emplace(sub_conf);
     }
 
-    start_sophiar(LOAD_STR("sophiar_config"),
-                  LOAD_STR("sophiar_start_name"));
-
-    if (true) {
-        auto sub_conf = sophiar_monitor::create_config();
-        sub_conf.item_list = sophiar_monitor::item_list_from_yaml(LOAD_LIST("monitor"));
+    {
+        auto sub_conf         = sophiar_monitor::create_config();
+        sub_conf.item_list    = sophiar_monitor::item_list_from_yaml(LOAD_LIST("monitor"));
         sub_conf.sophiar_conn = g_sophiar_conn;
         monitor.emplace(sub_conf);
     }
 
-    auto reg_conf = registration_config{
-        .conn = g_sophiar_conn,
+    auto reg_conf = registration_config {
+        .conn             = g_sophiar_conn,
         .probe_model_path = LOAD_STR("probe_model"),
     };
     reg.reset(registration::create(reg_conf));
-    for (auto reg_item: LOAD_LIST("registration_list")) {
+    for ( auto reg_item : LOAD_LIST("registration_list") ) {
         auto item_conf = registration_target::from_yaml(reg_item);
         reg->add_target(item_conf);
     }
 
-    if (true) {
-        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();
-        aug_conf.sophiar_conn = g_sophiar_conn;
-        std::copy(aug_list.begin(), aug_list.end(), std::back_inserter(aug_conf.item_list));
-        aug_manager.emplace(aug_conf);
-        // aug_manager->pre_render_sig.connect(
-        //     [this](const auto &info) { acl->pre_render_slot(info); });
-    }
-
-    if (true) {
-        auto sub_conf = image_augment_helper_v2::create_config();
-        sub_conf.img_flip_y = false;
-        sub_conf.img_name = left_fix_id;
-        sub_conf.out_name = left_aug_id;
-        sub_conf.manager = &aug_manager.value();
-        sub_conf.ctx = main_conf.asio_ctx;
+    {
+        auto sub_conf         = acl_guide::create_config::from_yaml(LOAD_SUB("acl_guide"));
         sub_conf.sophiar_conn = g_sophiar_conn;
-        sub_conf.transform_var = LOAD_STR("left_camera_transform");
-        left_cam.aug_helper.emplace(sub_conf);
-        left_cam.aug_helper->set_camera_info(
-            camera_intrinsic::from_yaml(LOAD_SUB("left_camera")).to_v0());
+        acl.emplace(sub_conf);
 
-        sub_conf.img_name = right_fix_id;
-        sub_conf.out_name = right_aug_id;
-        sub_conf.transform_var = LOAD_STR("right_camera_transform");
-        right_cam.aug_helper.emplace(sub_conf);
-        right_cam.aug_helper->set_camera_info(
-            camera_intrinsic::from_yaml(LOAD_SUB("right_camera")).to_v0());
+        aug_manager->pre_render_sig.connect( //
+            [this](const auto &info) { acl->pre_render_slot(info); });
     }
 }
 
@@ -151,37 +206,42 @@ void app_remote_ar_v2::render_background() {
 }
 
 void app_remote_ar_v2::show_ui() {
-    if (ImGui::Begin("Remote AR Control")) {
+    if ( ImGui::Begin("Remote AR Control") ) {
         ImGui::PushItemWidth(200);
 
-        if (ImGui::CollapsingHeader("MVS Camera")) {
-            if (true) {
+        if ( ImGui::CollapsingHeader("MVS Camera") ) {
+            {
                 auto id_guard = imgui_id_guard("mvs_camera");
                 mvs_cam->show();
             }
-            if (true) {
+            {
                 auto id_guard = imgui_id_guard("image_enhance");
                 ImGui::SeparatorText("Enhance");
                 left_cam.img_isp->show_ui();
             }
-            if (true) {
-                auto id_guard = imgui_id_guard("video_stabilization");
-                ImGui::SeparatorText("Stabilization");
-                video_stab->show_ui();
-            }
+            // if (true) {
+            //     auto id_guard = imgui_id_guard("video_stabilization");
+            //     ImGui::SeparatorText("Stabilization");
+            //     video_stab->show_ui();
+            // }
+        }
+
+        if ( ImGui::CollapsingHeader("UVC Camera") ) {
+            auto id_guard = imgui_id_guard("uvc_camera");
+            uvc_cam->show();
         }
 
-        // if (ImGui::CollapsingHeader("UVC Camera")) {
-        //     auto id_guard = imgui_id_guard("uvc_camera");
-        //     uvc_cam->show();
-        // }
+        if ( ImGui::CollapsingHeader("Calibration") ) {
+            auto id_guard = imgui_id_guard("endo_calib");
+            endo_calib->show_ui();
+        }
 
-        // if (ImGui::CollapsingHeader("Endoscope Calibration")) {
-        //     auto id_guard = imgui_id_guard("endo_calib");
-        //     endo_calib->show_ui();
-        // }
+        if ( ImGui::CollapsingHeader("Guidance") ) {
+            auto id_guard = imgui_id_guard("Guidance");
+            acl->show();
+        }
 
-        if (ImGui::CollapsingHeader("Augmentation")) {
+        if ( ImGui::CollapsingHeader("Augmentation") ) {
             auto guard = imgui_id_guard("augmentation");
 
             ImGui::SeparatorText("Actions");
@@ -194,47 +254,36 @@ void app_remote_ar_v2::show_ui() {
             ImGui::SeparatorText("Status");
             monitor->show();
 
-            ImGui::SeparatorText("Scene"); {
+            ImGui::SeparatorText("Scene");
+            {
                 auto id_guard = imgui_id_guard("augment_scene");
                 aug_manager->show();
             }
 
             ImGui::SeparatorText("Camera");
-            // auto helper = aug_helper->get_camera_helper();
-            // if (helper != nullptr && ImGui::TreeNode("Endo Camera")) {
-            //     helper->show();
-            //     ImGui::TreePop();
-            // }
-            auto helper = left_cam.aug_helper->get_camera_helper();
-            if (helper != nullptr && ImGui::TreeNode("MVS Camera")) {
+            auto helper = uvc_aug_helper->get_camera_helper();
+            if ( helper != nullptr && ImGui::TreeNode("UVC Camera") ) {
+                helper->show();
+                ImGui::TreePop();
+            }
+            helper = left_cam.aug_helper->get_camera_helper();
+            if ( helper != nullptr && ImGui::TreeNode("MVS Camera") ) {
                 helper->show();
                 ImGui::TreePop();
             }
         }
 
-        if (ImGui::CollapsingHeader("AR Navigation")) {
-            ImGui::Checkbox("Registration", &enable_reg);
-            // if (ImGui::TreeNode("Camera Control")) {
-            //     left_aug->show_ui();
-            //     ImGui::TreePop();
-            // }
-            // if (ImGui::TreeNode("Scene Control")) {
-            //     aug_scene->showUI();
-            //     ImGui::TreePop();
-            // }
-        }
-
-        if (ImGui::CollapsingHeader("Streamer")) {
+        if ( ImGui::CollapsingHeader("Streamer") ) {
             auto id_guard = imgui_id_guard("streamer");
             streamer->show();
         }
 
-        if (ImGui::CollapsingHeader("Debug")) {
-            if (ImGui::TreeNode("Background")) {
+        if ( ImGui::CollapsingHeader("Debug") ) {
+            if ( ImGui::TreeNode("Background") ) {
                 bg_viewer->show_ui();
                 ImGui::TreePop();
             }
-            if (ImGui::TreeNode("Saver")) {
+            if ( ImGui::TreeNode("Saver") ) {
                 saver->show_ui();
                 ImGui::TreePop();
             }
@@ -244,7 +293,7 @@ void app_remote_ar_v2::show_ui() {
     }
     ImGui::End();
 
-    if (enable_reg) {
+    if ( enable_reg ) {
         reg->process();
         reg->show();
     }

src/impl/apps/remote_ar/remote_ar_v2.h

@@ -1,30 +1,33 @@
 #ifndef REMOTE_AR_V2_H
 #define REMOTE_AR_V2_H
 
-#include "impl/app_base.h"
 #include "device/mvs_camera_ui.h"
 #include "device/uvc_camera_ui.h"
 #include "image_process_v5/image_augment.h"
-#include "image_process_v5/image_viewer.h"
-#include "image_process_v5/image_process.h"
 #include "image_process_v5/image_enhance.h"
+#include "image_process_v5/image_process.h"
+#include "image_process_v5/image_viewer.h"
 #include "image_process_v5/video_stabilization.h"
-#include "module/image_streamer.h"
-#include "module_v3/registration.h"
-#include "module_v5/versatile_saver.h"
-#include "module_v5/oblique_calibrator.h"
+#include "impl/app_base.h"
 #include "module/augment_manager_v2.h"
-#include "module/image_augment_helper_v2.h"
 #include "module/camera_augment_helper_v2.h"
+#include "module/guidance/acl_guide.h"
+#include "module/image_augment_helper_v2.h"
+#include "module/image_streamer.h"
 #include "module/sophiar_monitor.h"
+#include "module_v3/registration.h"
+#include "module_v5/oblique_calibrator.h"
+#include "module_v5/versatile_saver.h"
 
-class app_remote_ar_v2 final : public app_base {
+class app_remote_ar_v2 final: public app_base {
 public:
     explicit app_remote_ar_v2(create_config);
 
     ~app_remote_ar_v2() override;
 
-    const char *window_name() override { return "RemoteAR V5.-1"; }
+    const char *window_name() override {
+        return "RemoteAR V5.-1";
+    }
 
     void show_ui() override;
 
@@ -33,39 +36,28 @@ public:
 private:
     create_config main_conf;
 
-    enum : obj_name_type {
-        bg_img_id,
-        left_img_id, left_fix_id, left_aug_id, left_stab_id,
-        right_img_id, right_fix_id, right_aug_id, right_stab_id,
-        output_img_id,
-        uvc_img_id,
-    };
-
-    obj_name_type extra_name = 100;
-
-    std::optional<mvs_camera_ui> mvs_cam;
-    std::optional<uvc_camera_ui> uvc_cam;
-    std::optional<video_stabilization_ui> video_stab;
+    std::optional<mvs_camera_ui>        mvs_cam;
+    std::optional<uvc_camera_ui>        uvc_cam;
+    // std::optional<video_stabilization_ui> video_stab;
     std::optional<stereo_output_helper> output_helper;
-    std::optional<image_viewer_v2> bg_viewer;
-    std::optional<image_streamer> streamer;
-    std::optional<versatile_saver> saver;
-    // std::optional<oblique_calibrator> endo_calib;
-    std::optional<sophiar_monitor> monitor;
-
-    bool enable_reg = false;
+    std::optional<image_viewer_v2>      bg_viewer;
+    std::optional<image_streamer>       streamer;
+    std::optional<versatile_saver>      saver;
+    std::optional<oblique_calibrator>   endo_calib;
+    std::optional<sophiar_monitor>      monitor;
+    std::optional<acl_guide>            acl;
+
+    bool                          enable_reg = false;
     std::unique_ptr<registration> reg;
 
     std::optional<augment_manager_v2> aug_manager;
+
     struct {
-        std::optional<image_enhance_ui> img_isp;
+        std::optional<image_enhance_ui>        img_isp;
         std::optional<image_augment_helper_v2> aug_helper;
     } left_cam, right_cam;
 
-    // osg::ref_ptr<SceneSP> aug_scene;
-    // std::optional<image_augment> left_aug;
-    // std::optional<image_augment> right_aug;
+    std::optional<image_augment_helper_v2> uvc_aug_helper;
 };
 
-
-#endif //REMOTE_AR_V2_H
+#endif // REMOTE_AR_V2_H

src/module/augment_manager_v2.h

@@ -51,8 +51,7 @@ public:
     using item_list_type = create_config::item_list_type;
     using item_list_v1_type = augment_manager::item_list_type;
 
-    static item_list_type item_list_from_v1(const item_list_v1_type &items,
-                                            obj_name_type &item_name);
+    static item_list_type item_list_from_v1(const item_list_v1_type &items);
 
     explicit augment_manager_v2(const create_config &conf);
 

src/module/guidance/acl_guide.h

@@ -20,7 +20,6 @@ public:
 
         std::string drill_transform_var_name;
         sophiar_conn_type *sophiar_conn = nullptr;
-        io_ctx_type *ctx = nullptr;
 
         void fill_from_yaml(const YAML::Node &conf);
 

src/module/guidance/impl/acl_guide.cpp

@@ -142,11 +142,11 @@ struct acl_guide::impl {
         ImGui::SeparatorText("Actions");
         if (!is_running) {
             if (ImGui::Button("Start")) {
-                post(*conf.ctx, [this] { start(); });
+                MAIN_DETACH([this] { start(); });
             }
         } else {
             if (ImGui::Button("Stop")) {
-                post(*conf.ctx, [this] { stop(); });
+                MAIN_DETACH([this] { stop(); });
             }
             ImGui::SameLine();
             if (ImGui::Button("Save")) {

src/module/impl/augment_manager_v2.cpp

@@ -190,8 +190,7 @@ void augment_manager_v2::show() {
 }
 
 augment_manager_v2::item_list_type
-augment_manager_v2::item_list_from_v1(const item_list_v1_type &items,
-                                      obj_name_type &item_name) {
+augment_manager_v2::item_list_from_v1(const item_list_v1_type &items) {
     auto ret = item_list_type();
     ret.reserve(items.size());
 
@@ -201,7 +200,7 @@ augment_manager_v2::item_list_from_v1(const item_list_v1_type &items,
         auto item = item_type{
                 .type = ITEM_MESH,
                 .disp_name = v1.name,
-                .name = item_name++,
+                .name = next_name(),
                 .transform_var = v1.transform_var,
         };
         // TODO: bg_name ignored

src/module_v5/algorithms/fitting_circle_3d.cpp

@@ -89,7 +89,7 @@ fitting_circle_3d::operator()(const input_type &input) {
     ret.error_rms = ret_2d.error_rms;
 
     auto center = Vector3f();
-    center << ret.center.x, ret.center.y, 0;
+    center << ret_2d.center.x, ret_2d.center.y, 0;
     center = rodrigues_rot(center, Vector3f::UnitZ(), normal);
     center += mean;
     ret.center = to_vec3(center);

src/module_v5/oblique_calibrator.cpp

@@ -1,22 +1,28 @@
 #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 "image_process_v5/image_process.h"
+#include "image_process_v5/process_python.h"
+#include "image_process_v5/sp_image.h"
 #include "module_v5/algorithms.h"
+#include "module_v5/transform_provider.h"
 #include "third_party/scope_guard.hpp"
-#include "image_process_v5/sp_image.h"
-#include "image_process_v5/process_python.h"
-#include "image_process_v5/image_process.h"
 
 // from sophiar
 #include "core/local_connection.h"
 
 #include <boost/asio/io_context.hpp>
 #include <boost/asio/post.hpp>
+#include <boost/beast/core/detail/base64.hpp>
 #include <boost/filesystem/operations.hpp>
+#include <glm/gtx/rotate_vector.hpp>
+#include <glm/trigonometric.hpp>
+#include <opencv2/imgproc.hpp>
 #include <utility>
+#include <vgMath.h>
 
 namespace ba = boost::asio;
 namespace bs = boost::signals2;
@@ -25,23 +31,114 @@ using namespace sophiar;
 extern local_connection *g_sophiar_conn;
 
 namespace {
-    struct rotation_center_calib
-            : std::enable_shared_from_this<rotation_center_calib> {
+    namespace b64 = boost::beast::detail::base64;
+
+    template<typename T>
+        requires(std::is_pod_v<std::remove_cvref_t<T>>)
+    std::string any_to_string(T &&v) {
+        const auto ret_size = b64::encoded_size(sizeof(T));
+        auto       ret      = std::string(ret_size, '\0');
+        b64::encode(ret.data(), &v, sizeof(T));
+        return ret;
+    }
+
+    template<typename T>
+        requires(std::is_pod_v<T>)
+    T string_to_any(const std::string &str) {
+        constexpr auto buf_size = (sizeof(T) + 2) / 3 * 3;
+        char           buf[buf_size];
+        assert(b64::decoded_size(str.length()) == buf_size);
+        b64::decode(buf, str.data(), str.length());
+        return *(T *) buf;
+    }
+
+    using contour_type = std::vector<cv::Point>;
+
+    Eigen::Matrix2Xf to_matrix(const contour_type &ps) {
+        auto ret = Eigen::Matrix2Xf(2, ps.size());
+        for ( int i = 0; i < ps.size(); ++i ) {
+            ret(0, i) = ps[i].x;
+            ret(1, i) = ps[i].y;
+        }
+        return ret;
+    }
+
+    std::optional<float> calc_rotation_angle(const sp_image &img) { // RGB image
+        std::vector<contour_type> contours;
+        {
+            const auto img_gray   = image_rgb_to_gray(img);
+            const auto img_binary = image_gray_to_binary(img_gray, 20);
+            const auto helper     = read_access_helper(img_binary.host());
+            cv::findContours(img_binary.cv_mat(helper.ptr()), contours, //
+                cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+        }
+
+        // find largest contour
+        contour_type pin_contour;
+        float        max_area = 0.f;
+        for ( const auto &contour : contours ) {
+            if ( auto area = std::abs(cv::contourArea(contour)); //
+                area > max_area ) {
+                pin_contour = contour;
+                max_area    = area;
+            }
+        }
+        if ( pin_contour.empty() ) return {};
+
+        // 2D circle fitting
+        auto fit_input    = fitting_circle_2d::input_type();
+        fit_input.points  = to_matrix(pin_contour);
+        const auto fit_2d = fitting_circle_2d()(fit_input);
+        const auto radius = fit_2d.radius;
+        const auto center = fit_2d.center;
+        if ( radius < 410 || radius > 420 ) return {}; // TODO: config magic number
+
+        std::vector<glm::vec2> pin_points;
+        for ( const auto &p : pin_contour ) {
+            auto point = to_vec2(p);
+            if ( auto dis = glm::distance(point, center); //
+                std::abs(dis - radius) > 10 ) {
+                pin_points.push_back(point);
+            }
+        }
+
+        auto pin_center = std::ranges::fold_left(pin_points, glm::vec2(), std::plus());
+        pin_center /= pin_points.size();
+        auto angle = std::atan2(pin_center.y - center.y, pin_center.x - center.x);
+        return angle + M_PI / 2;
+    }
+
+} // namespace
+
+namespace {
+    struct rotation_center_calib {
         obj_name_type image_name = {};
 
         using result_type = rotate_registration_result_type;
 
-        std::optional<glm::vec2> result;
-        std::vector<sp_image> calib_images;
-        std::atomic_size_t reg_tasks;
-        std::vector<glm::vec2> reg_results;
+        std::optional<glm::vec2>    result;
+        bs::signal<void(glm::vec2)> result_sig;
+        std::vector<sp_image>       calib_images;
+        std::atomic_size_t          reg_tasks;
+        std::vector<glm::vec2>      reg_results;
+
+        void set_result(const std::string &str) {
+            set_result(string_to_any<glm::vec2>(str));
+        }
+
+        void set_result(glm::vec2 _ret) {
+            result = _ret;
+            SPDLOG_INFO("Rotation Center: {}", *result);
+            SPDLOG_INFO("Rotation Center Str: {}", any_to_string(*result));
+            MAIN_DETACH([this] { result_sig(*result); });
+        }
 
         void reg_callback(const result_type ret, const size_t index) {
             reg_results[index] = ret.center;
-            if (--reg_tasks == 0) {
+            if ( --reg_tasks == 0 ) {
                 auto sum = std::ranges::fold_left(reg_results, glm::vec2(), std::plus());
                 sum /= reg_results.size();
-                MAIN_DETACH([=, ptr = shared_from_this()] { ptr->result = sum; });
+                MAIN_DETACH([=, this] { set_result(sum); });
             }
         }
 
@@ -51,57 +148,68 @@ namespace {
             reg_tasks = num_samples * (num_samples - 1);
             reg_results.resize(num_samples);
             auto index = 0;
-            for (auto i = 0; i < num_samples; i++)
-                for (auto j = 0; j < num_samples; j++) {
-                    image_rotate_registration(
-                        calib_images[i], calib_images[j],
-                        [ptr = shared_from_this(), _index = index++](auto ret) {
-                            ptr->reg_callback(ret, _index);
-                        });
+            for ( auto i = 0; i < num_samples; i++ )
+                for ( auto j = 0; j < num_samples; j++ ) {
+                    image_rotate_registration(calib_images[i], calib_images[j], //
+                        [this, _index = index++](auto ret) { reg_callback(ret, _index); });
                 }
         }
 
         void show_ui() {
             const auto num_samples = calib_images.size();
             const bool can_operate = num_samples >= 2;
-            if (ImGui::Button("Collect")) {
-                MAIN_DETACH([ptr = shared_from_this()] {
-                    auto img = OBJ_QUERY(sp_image, ptr->image_name);
-                    ptr->calib_images.emplace_back(img);
+            if ( ImGui::Button("Collect") ) {
+                MAIN_DETACH([this] {
+                    auto img = OBJ_QUERY(sp_image, image_name);
+                    calib_images.emplace_back(img);
                 });
             }
-            if (true) {
-                auto guard = imgui_valid_style_guard(can_operate);
+            if ( true ) {
+                auto guard = imgui_disable_guard(!can_operate);
                 ImGui::SameLine();
-                if (ImGui::Button("Calibrate")) {
-                    TP_DETACH([ptr = shared_from_this()] { ptr->start_calib(); });
+                if ( ImGui::Button("Calibrate") ) {
+                    TP_DETACH([this] { start_calib(); });
                 }
+            }
+            if ( true ) {
+                auto guard = imgui_valid_style_guard(can_operate);
                 ImGui::Text("Image Samples: %ld", num_samples);
             }
+            if ( true ) {
+                const auto is_ok = (bool) result;
+                auto       guard = imgui_valid_style_guard(is_ok);
+                ImGui::Text("Result OK: %s", is_ok ? "True" : "False");
+            }
         }
     };
 
-    struct rotation_ball_calib
-            : std::enable_shared_from_this<rotation_ball_calib> {
+    struct rotation_ball_calib {
         static constexpr auto distance_threshold = 50; // 50mm
-        static constexpr auto radius_threshold = 2; // 2mm
-        static constexpr auto invalid_angle = std::numeric_limits<float>::quiet_NaN();
+        static constexpr auto radius_threshold   = 2;  // 2mm
+        static constexpr auto invalid_angle      = std::numeric_limits<float>::quiet_NaN();
 
         using stray_points_type = ndi_stray_point_tracker::points_type;
         std::vector<glm::vec3> ball_points;
-        bs::connection stray_points_conn;
-        std::atomic_flag is_calib;
+        bs::connection         stray_points_conn;
+        std::atomic_flag       is_calib;
 
         struct result_type {
-            float radius;
-            glm::vec3 center, axis, start;
+            float     radius;
+            glm::vec3 center, axis, start_vec;
         } result = {};
 
-        std::atomic_flag result_ok;
+        std::atomic_flag              result_ok;
+        bs::signal<void(result_type)> result_sig;
+        float                         last_ball_error = invalid_angle; // distance in mm
+        bool                          passthrough     = false;
+        bool                          frozen_angle    = false;
+
+        obj_conn_type img_conn;
+        float         img_angle = invalid_angle;
 
         void calib_callback(const stray_points_type &points) {
-            if (ball_points.empty()) {
-                if (points.size() != 1) {
+            if ( ball_points.empty() ) {
+                if ( points.size() != 1 ) {
                     stop_calib();
                     SPDLOG_ERROR("Failed to load anchor point.");
                 }
@@ -109,84 +217,121 @@ namespace {
                 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);
-                });
+            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) > distance_threshold) { return; }
+            if ( distance(last_point, closest_point) > distance_threshold ) { return; }
             ball_points.emplace_back(closest_point);
         }
 
+        void update_image_angle(const float angle) { // angle in radius
+            if ( !result_ok.test() ) return;
+            if ( passthrough ) {
+                g_sophiar_conn->update_transform_variable( //
+                    conf.len_to_camera_name, Eigen::Isometry3d::Identity());
+                return;
+            }
+            if ( frozen_angle ) { return; }
+
+            OBJ_SAVE(conf.angle_name, angle);
+            if ( std::isnan(angle) ) {
+                g_sophiar_conn->update_transform_variable( //
+                    conf.len_to_camera_name, {});
+                return;
+            }
+
+            const auto cur_axis = glm::rotate(result.start_vec, angle, -result.axis);
+            auto       matrix   = glm::mat4();
+            matrix[2]           = glm::vec4(result.axis, 0);         // Z-axis
+            matrix[0]           = glm::vec4(cur_axis, 0);            // X-axis
+            const auto y_axis   = glm::cross(result.axis, cur_axis); // Z x X -> Y
+            matrix[1]           = glm::vec4(y_axis, 0);              // Y-axis
+            matrix[3]           = glm::vec4(result.center, 1);
+            g_sophiar_conn->update_transform_variable(conf.len_to_camera_name, //
+                to_eigen_transform<double, Eigen::Isometry>(matrix));
+        }
+
         void tracking_callback(const stray_points_type &points) {
-            if (!result_ok.test()) return;
             auto dis_op = [=, this](const glm::vec3 point) {
                 const auto dis = glm::distance(result.center, point);
                 return std::abs(dis - result.radius);
             };
-            const auto closest_iter = std::ranges::min_element(
-                points, std::less(), [=](auto p) { return dis_op(p.pos); });
+            const auto closest_iter  = std::ranges::min_element(points, std::less(), //
+                 [=](auto p) { return dis_op(p.pos); });
             const auto closest_point = closest_iter->pos;
-            if (dis_op(closest_point) > radius_threshold) {
-                OBJ_SAVE(conf.angle_name, invalid_angle);
-                g_sophiar_conn->update_transform_variable(
-                    conf.len_to_camera_name, {});
+            last_ball_error          = dis_op(closest_point);
+            if ( last_ball_error > radius_threshold ) {
+                // update_image_angle(invalid_angle); // TODO: add options to ignore missing
                 return;
             }
 
-            auto start_vec = result.start - result.center;
             auto current_vec = closest_point - result.center;
-            start_vec /= glm::length(start_vec);
+            current_vec -= glm::dot(current_vec, result.axis);
             current_vec /= glm::length(current_vec);
-            auto angle = std::acos(glm::dot(start_vec, current_vec));
-            OBJ_SAVE(conf.angle_name, angle);
-
-            auto matrix = glm::mat4();
-            matrix[2] = glm::vec4(result.axis, 0); // Z-axis
-            matrix[0] = glm::vec4(current_vec, 0); // X-axis
-            const auto y_axis = glm::cross(result.axis, current_vec); // Z x X -> Y
-            matrix[1] = glm::vec4(y_axis, 0); // Y-axis
-            matrix[3] = glm::vec4(result.center, 1);
-            auto eigen_tm = Eigen::Isometry3d();
-            to_eigen_transform(matrix, eigen_tm);
-            g_sophiar_conn->update_transform_variable(
-                conf.len_to_camera_name, eigen_tm);
+            // https://stackoverflow.com/questions/5188561/signed-angle-between-two-3d-vectors-with-same-origin-within-the-same-plane
+            auto angle = std::atan2(                                              //
+                glm::dot(glm::cross(result.start_vec, current_vec), result.axis), //
+                glm::dot(result.start_vec, current_vec));
+            update_image_angle(angle);
         }
 
         void points_callback(stray_points_type points) {
-            if (points.empty()) return;
-            const auto ref_matrix =
-                    query_transform(conf.camera_ref_name).matrix;
-            if (!ref_matrix) return;
+            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) {
+            std::ranges::for_each(points, [=](auto &point) { //
                 point.pos = transform_p(t_matrix, point.pos);
             });
 
-            if (is_calib.test()) {
+            if ( result_ok.test() ) {
+                // tracking_callback(points); // TODO: Debug, use angle from image
+            } else if ( is_calib.test() ) {
                 calib_callback(points);
-            } else {
-                tracking_callback(points);
             }
         }
 
         void start_calib() {
+            result_ok.clear();
             ball_points.clear();
             is_calib.test_and_set();
         }
 
-        void calc_rotation_ball() {
-            if (ball_points.size() < 3) return;
-            auto input = fitting_circle_3d::input_type();
-            input.points = to_eigen(ball_points);
-            const auto ret = fitting_circle_3d()(input);
-            result.center = ret.center;
-            result.axis = ret.axis;
-            result.start = ball_points[0];
-            result.radius = ret.radius;
+        void set_result(const std::string &str) {
+            set_result(string_to_any<result_type>(str));
+        }
+
+        void set_result(const result_type &_ret) {
+            result = _ret;
+            SPDLOG_INFO("Rotation Information:");
+            SPDLOG_INFO("\tRadius: {}", result.radius);
+            SPDLOG_INFO("\tCenter: {}", result.center);
+            SPDLOG_INFO("\tAxis: {}", result.axis);
+            SPDLOG_INFO("\tStart Dir: {}", result.start_vec);
+            SPDLOG_INFO("Rotation Str: {}", any_to_string(result));
+
             result_ok.test_and_set();
-            SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", ret.error_rms);
+            MAIN_DETACH([this] { result_sig(result); });
+        }
+
+        void calc_rotation_ball() {
+            if ( ball_points.size() < 3 ) return;
+            auto input         = fitting_circle_3d::input_type();
+            input.points       = to_eigen(ball_points);
+            const auto fit_ret = fitting_circle_3d()(input);
+            SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", fit_ret.error_rms);
+
+            auto start_vec = ball_points[0] - fit_ret.center;
+            start_vec -= glm::dot(fit_ret.axis, start_vec);
+            start_vec /= glm::length(start_vec);
+
+            auto ret      = result_type();
+            ret.center    = fit_ret.center;
+            ret.axis      = fit_ret.axis;
+            ret.start_vec = start_vec;
+            ret.radius    = fit_ret.radius;
+            set_result(ret);
         }
 
         void stop_calib() {
@@ -194,149 +339,228 @@ namespace {
             calc_rotation_ball();
         }
 
+        void img_callback(const obj_name_type name) {
+            assert(name == conf.image_name);
+            try {
+                const auto img = OBJ_QUERY(sp_image, conf.image_name);
+                if ( const auto angle = calc_rotation_angle(img); angle ) {
+                    img_angle = *angle;
+                } else {
+                    img_angle = invalid_angle;
+                }
+            } catch ( ... ) { //
+                img_angle = invalid_angle;
+            }
+            update_image_angle(img_angle);
+        }
+
         void show_ui() {
             const auto num_stray_point = get_ndi_stray_points().size();
-            const bool can_operate = num_stray_point == 1;
-            const auto is_calib_val = is_calib.test();
-            if (!is_calib_val) {
+            const bool can_operate     = num_stray_point == 1;
+            const auto is_calib_val    = is_calib.test();
+            if ( !is_calib_val ) {
                 auto guard = imgui_disable_guard(!can_operate);
-                if (ImGui::Button("Start")) {
-                    MAIN_DETACH([ptr=shared_from_this()] { ptr->start_calib(); });
+                if ( ImGui::Button("Start") ) {
+                    MAIN_DETACH([this] { start_calib(); });
                 }
             } else {
-                if (ImGui::Button("Stop")) {
-                    MAIN_DETACH([ptr=shared_from_this()] { ptr->stop_calib(); });
+                if ( ImGui::Button("Stop") ) {
+                    MAIN_DETACH([this] { stop_calib(); });
                 }
             }
-            if (true) {
+            if ( true ) {
                 auto guard = imgui_valid_style_guard(can_operate);
                 ImGui::Text("Stray Points: %ld", num_stray_point);
             }
-            if (is_calib_val) {
-                ImGui::Text("Collected Samples: %ld", ball_points.size());
+            if ( is_calib_val ) { ImGui::Text("Collected Samples: %ld", ball_points.size()); }
+
+            const bool is_ok = result_ok.test();
+            if ( true ) {
+                auto guard = imgui_valid_style_guard(is_ok);
+                ImGui::Text("Result OK: %s", is_ok ? "True" : "False");
             }
-            if (result_ok.test()) {
-                ImGui::Text("Current Angle (deg): %.2f",
-                            glm::degrees(OBJ_QUERY(float, conf.angle_name)));
+            if ( is_ok ) {
+                ImGui::Checkbox("Passthrough", &passthrough);
+                if ( !passthrough ) {
+                    ImGui::SameLine();
+                    ImGui::Checkbox("Frozen", &frozen_angle);
+
+                    {
+                        auto guard = imgui_valid_style_guard(!std::isnan(img_angle));
+                        ImGui::Text("Image Angle (deg): %.2f", glm::degrees(img_angle));
+                    }
+
+                    try {
+
+                        // ImGui::Text("Current Angle (deg): %.2f", //
+                        //     glm::degrees(OBJ_QUERY(float, conf.angle_name)));
+                        // ImGui::Text("Current Point Error (mm): %.2f", last_ball_error);
+                    } catch ( ... ) { (void) 0; }
+                }
             }
         }
 
         struct create_config {
-            std::string camera_ref_name;
-            std::string len_to_camera_name;
+            std::string   camera_ref_name;
+            std::string   len_to_camera_name;
             obj_name_type angle_name = {};
+            obj_name_type image_name = {};
         } conf;
 
-        explicit rotation_ball_calib(create_config _conf)
-            : conf(std::move(_conf)) {
-            stray_points_conn = get_ndi_stray_points_sig()->connect(
-                [ptr = shared_from_this()](auto points) {
-                    ptr->points_callback(points);
-                });
+        explicit rotation_ball_calib(create_config _conf) : conf(std::move(_conf)) {
+            stray_points_conn = get_ndi_stray_points_sig()->connect( //
+                [this](auto points) { points_callback(std::move(points)); });
+            img_conn          = OBJ_SIG(conf.image_name) //
+                           ->connect([this](auto name) { img_callback(name); });
         }
 
         ~rotation_ball_calib() {
             stray_points_conn.disconnect();
+            img_conn.disconnect();
         }
     };
 
-    struct reverse_rotation_image {
-        obj_conn_type img_conn;
+    struct image_rotation_helper {
+        obj_conn_type            img_conn;
+        std::optional<glm::vec2> center;
+        float                    last_angle = 0.f;
 
         void img_callback_impl() {
-            if (!conf.center->result) return;
+            if ( !center ) return;
             const auto img = OBJ_QUERY(sp_image, conf.input_name);
-            const auto angle = OBJ_QUERY(float, conf.angle_name);
-            const auto rot_img = image_rotate(img, -angle, conf.center->result);
+            if ( conf.source->passthrough ) {
+                OBJ_SAVE(conf.output_name, img);
+                return;
+            }
+            const auto rot_img = image_rotate(img, glm::degrees(last_angle), center);
             OBJ_SAVE(conf.output_name, rot_img);
         }
 
         void img_callback(const obj_name_type name) {
             assert(name == conf.input_name);
+            try {
+                auto angle = OBJ_QUERY(float, conf.angle_name);
+                if ( !conf.inverse ) { angle = -angle; }
+                if ( !std::isnan(angle) ) { last_angle = angle; }
+            } catch ( ... ) { (void) 0; }
             try {
                 img_callback_impl();
-            } catch (...) { (void) 0; }
+            } catch ( ... ) { (void) 0; }
         }
 
         struct create_config {
-            obj_name_type input_name = {};
-            obj_name_type output_name = {};
-            obj_name_type angle_name = {};
-            std::shared_ptr<rotation_center_calib> center;
+            obj_name_type        input_name  = {};
+            obj_name_type        output_name = {};
+            obj_name_type        angle_name  = {};
+            bool                 inverse     = false;
+            rotation_ball_calib *source      = nullptr;
         } conf;
 
-        explicit reverse_rotation_image(create_config _conf)
-            : conf(std::move(_conf)) {
-            img_conn = OBJ_SIG(conf.input_name)->connect(
-                [this](auto name) { img_callback(name); });
+        explicit image_rotation_helper(create_config _conf) : conf(_conf) {
+            img_conn = OBJ_SIG(conf.input_name)->connect([this](auto name) { //
+                img_callback(name);
+            });
         }
 
-        ~reverse_rotation_image() {
+        ~image_rotation_helper() {
             img_conn.disconnect();
         }
     };
-}
-
-namespace {
-    constexpr obj_name_type rev_rot_name_offset = 0x0100;
-}
+} // namespace
 
 struct oblique_calibrator::impl {
     create_config conf;
 
+    rotation_center_calib                calib_center;
+    std::optional<rotation_ball_calib>   calib_ball;
+    std::optional<image_rotation_helper> rev_rot_helper;
+    std::optional<image_rotation_helper> img_rot_helper;
+    std::optional<camera_calibrator>     calib_cam;
 
-    enum {
-        IDLE,
-        ROTATION_BALL
-    } status = IDLE;
+    void show_ui() {
+        if ( ImGui::TreeNode("Rotation Center") ) {
+            calib_center.show_ui();
+            ImGui::TreePop();
+        }
+        if ( calib_center.result && ImGui::TreeNode("Rotation Ball") ) {
+            calib_ball->show_ui();
+            ImGui::TreePop();
+        }
+        if ( calib_ball->result_ok.test() && ImGui::TreeNode("Hand-Eye") ) {
+            calib_cam->show();
+            ImGui::TreePop();
+        }
+    }
 
-    std::shared_ptr<rotation_center_calib> rot_center;
-    std::shared_ptr<rotation_ball_calib> rot_ball;
-    std::optional<reverse_rotation_image> rev_rot_img;
+    explicit impl(create_config _conf) : conf(std::move(_conf)) {
+        if ( true ) {
+            calib_center.image_name = conf.image_name;
+            calib_center.result_sig.connect([this](auto center) {
+                rev_rot_helper->center = center;
+                img_rot_helper->center = center;
+            });
+        }
 
-    void show_ui() const {
-        if (true) {
-            ImGui::SeparatorText("Rotation Center");
-            auto id_guard = imgui_id_guard("rotation_ball");
-            rot_center->show_ui();
+        if ( true ) {
+            auto sub_conf               = rotation_ball_calib::create_config();
+            sub_conf.camera_ref_name    = conf.camera_ref_name;
+            sub_conf.len_to_camera_name = conf.len_ref_to_camera_ref_name;
+            sub_conf.angle_name         = conf.angle_name;
+            sub_conf.image_name         = conf.image_name;
+            calib_ball.emplace(sub_conf);
         }
-        if (true) {
-            ImGui::SeparatorText("Rotation Ball");
-            auto id_guard = imgui_id_guard("rotation_center");
-            rot_ball->show_ui();
+
+        if ( true ) {
+            auto sub_conf        = image_rotation_helper::create_config();
+            sub_conf.input_name  = conf.image_name;
+            sub_conf.output_name = conf.image_inv_rotate_name;
+            sub_conf.angle_name  = conf.angle_name;
+            sub_conf.inverse     = true;
+            sub_conf.source      = &calib_ball.value();
+            rev_rot_helper.emplace(sub_conf);
+
+            sub_conf.input_name  = conf.image_fix_in_name;
+            sub_conf.output_name = conf.image_fix_out_name;
+            sub_conf.inverse     = false;
+            img_rot_helper.emplace(sub_conf);
         }
-    }
 
-    explicit impl(create_config conf)
-        : conf(std::move(conf)) {
-        if constexpr (true) {
-            rot_center = std::make_shared<rotation_center_calib>();
-            rot_center->image_name = conf.image_name;
-        }
-        if constexpr (true) {
-            auto sub_conf = rotation_ball_calib::create_config();
-            sub_conf.camera_ref_name = conf.camera_ref_name;
-            sub_conf.len_to_camera_name = conf.len_to_camera_name;
-            sub_conf.angle_name = conf.angle_name;
-            rot_ball = std::make_shared<rotation_ball_calib>(sub_conf);
-        }
-        if constexpr (true) {
-            auto sub_conf = reverse_rotation_image::create_config();
-            sub_conf.input_name = conf.image_name;
-            sub_conf.output_name = conf.image_name + rev_rot_name_offset;
-            sub_conf.angle_name = conf.angle_name;
-            sub_conf.center = rot_center;
-            rev_rot_img.emplace(sub_conf);
+        if ( true ) {
+            auto sub_conf                 = camera_calibrator::create_config();
+            sub_conf.img_name             = conf.image_inv_rotate_name;
+            sub_conf.board_type           = conf.board_type;
+            sub_conf.pattern_size         = conf.pattern_size;
+            sub_conf.corner_distance      = conf.corner_distance;
+            sub_conf.cb_func              = conf.cb_func;
+            sub_conf.ref_transform_var    = conf.len_ref_name;
+            sub_conf.ref_error_var        = conf.camera_ref_error_name;
+            sub_conf.result_transform_var = conf.camera_to_len_ref_name;
+            calib_cam.emplace(sub_conf);
         }
     }
 };
 
-oblique_calibrator::oblique_calibrator(const create_config &conf)
-    : pimpl(std::make_unique<impl>(conf)) {
-}
+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();
 }
+
+void oblique_calibrator::render() const {
+    pimpl->calib_cam->render();
+}
+
+void oblique_calibrator::set_rotation_center(const std::string &str) const {
+    pimpl->calib_center.set_result(str);
+}
+
+void oblique_calibrator::set_rotation_ball(const std::string &str) const {
+    pimpl->calib_ball->set_result(str);
+}
+
+void oblique_calibrator::set_camera_calib(const camera_calibrator::simulate_info_type &sim_info) const {
+    pimpl->calib_cam->simulate_process(sim_info);
+}

src/module_v5/oblique_calibrator.h

@@ -2,47 +2,48 @@
 #define OBLIQUE_CALIBRATOR_H
 
 #include "core_v2/object_manager.h"
-
-#include <opencv2/core/types.hpp>
-
-#include <glm/vec3.hpp>
-
-#include <boost/signals2.hpp>
+#include "image_process/camera_calibrator.h"
 
 #include <memory>
+#include <opencv2/core/types.hpp>
 
 class oblique_calibrator {
 public:
     struct create_config {
-        obj_name_type image_name; // Input, endoscope image
-        std::string camera_ref_name; // XXX_in_tracker
-        std::string len_to_camera_name;
-        std::string len_ref_name; // XXX_in_tracker
+        obj_name_type image_name;            // Endoscope image
+        obj_name_type angle_name;            // Output, currant rotation angle of the len, float
+        obj_name_type image_inv_rotate_name; // Output, inverse rotated image
+
+        // helper for image output
+        obj_name_type image_fix_in_name, image_fix_out_name;
 
-        obj_name_type angle_name; // Output, currant rotation angle of the len, float
+        std::string camera_ref_name;            // camera_ref_in_tracker
+        std::string len_ref_to_camera_ref_name; // len_ref_to_camera_ref
+        std::string len_ref_name;               // len_ref_in_tracker
 
         // for camera_calibrator
-        cv::Size pattern_size;
-        float corner_distance; // in mm
+        camera_calibrator::calib_board_enum board_type;
+        cv::Size                            pattern_size;
+        float                               corner_distance; // in mm
+        std::string                         camera_ref_error_name;
+        std::string                         camera_to_len_ref_name; // Endoscope end to len reference
+
+        using calib_result_type  = camera_calibrator::result_type;
+        using calib_cb_func_type = std::function<void(calib_result_type)>;
+        calib_cb_func_type cb_func;
     };
 
     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;
+    void render() const; // forward to camera calib
+    void set_rotation_center(const std::string &str) const;
+    void set_rotation_ball(const std::string &str) const;
+    void set_camera_calib(const camera_calibrator::simulate_info_type& sim_info) const;
 
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;
 };
 
-#endif //OBLIQUE_CALIBRATOR_H
+#endif // OBLIQUE_CALIBRATOR_H

src/module_v5/versatile_saver.cpp

@@ -69,7 +69,11 @@ struct versatile_saver::impl {
     void save_all() {
         for (auto &item: items) {
             if (!item.is_selected) continue;
-            TP_DETACH([&] { save_item(item); });
+            TP_DETACH([&] {
+                try {
+                    save_item(item);
+                } catch ( ... ) { (void) 0; }
+            });
         }
         last_save_ts = current_timestamp();
         ++save_cnt;