Added tracker error to camera calibrator.

data/config_endo_guide.yaml

@@ -4,6 +4,7 @@ sophiar_config: /home/tpx/project/DepthGuide/data/sophiar_config_endo_guide.json
 sophiar_start_var: tracker_all
 
 camera_ref_transform_var: camera_ref_in_tracker
+camera_ref_error_var: camera_ref_in_tracker_error
 camera_transform_var: camera_in_tracker
 camera_calib_transform_var: camera_in_camera_ref
 

data/sophiar_config_endo_guide.json

@@ -46,6 +46,10 @@
       "name": "camera_ref_in_tracker",
       "type": "transform_obj"
     },
+    {
+      "name": "camera_ref_in_tracker_error",
+      "type": "double_obj"
+    },
     {
       "name": "camera_in_camera_ref",
       "type": "transform_obj"
@@ -242,7 +246,8 @@
           {
             "rom_path": "/home/tpx/data/roms/Glass_4Ball_2.rom",
             "outputs": {
-              "transform": "camera_ref_in_tracker"
+              "transform": "camera_ref_in_tracker",
+              "marker_uncertainty": "camera_ref_in_tracker_error"
             }
           },
           {

src/core/imgui_utility.hpp

@@ -36,4 +36,18 @@ struct imgui_id_guard {
     }
 };
 
+struct imgui_valid_style_guard {
+    explicit imgui_valid_style_guard(bool valid) {
+        if (valid) {
+            ImGui::PushStyleColor(ImGuiCol_Text, (ImVec4) ImColor(0, 255, 0));
+        } else {
+            ImGui::PushStyleColor(ImGuiCol_Text, (ImVec4) ImColor(255, 0, 0));
+        }
+    }
+
+    ~imgui_valid_style_guard() {
+        ImGui::PopStyleColor();
+    }
+};
+
 #endif //DEPTHGUIDE_IMGUI_UTILITY_HPP

src/device/impl/orb_camera.cpp

@@ -274,12 +274,8 @@ bool orb_camera::impl::start(start_config conf) {
     auto d_prof = std::shared_ptr<ob::StreamProfile>();
     if (conf.depth.enable) {
         assert(d_pf_list != nullptr);
-        if (conf.pc.enable) { // TODO: better determine method
-            assert(c_prof != nullptr);
-            d_prof = pipe->getD2CDepthProfileList(c_prof, align_mode)->getProfile(conf.depth.config_index);
-        } else {
-            d_prof = d_pf_list->getProfile(conf.depth.config_index);
-        }
+        d_prof = pipe->getD2CDepthProfileList(c_prof, align_mode)
+                ->getProfile(conf.depth.config_index);
         ob_conf->enableStream(d_prof);
         d_name = conf.depth.name;
     }

src/device/impl/orb_camera_ui.cpp

@@ -19,7 +19,6 @@ orb_camera_ui::impl::impl(create_config conf) {
     cam_s_conf.color.name = conf.cf_name;
     cam_s_conf.depth.enable = true;
     cam_s_conf.depth.name = conf.df_name;
-    cam_s_conf.pc.name = conf.pc_name;
 
     refresh_dev_info_list();
 }
@@ -31,11 +30,7 @@ void orb_camera_ui::impl::refresh_dev_info_list() {
 void orb_camera_ui::impl::refresh_d_conf_list() {
     d_conf_list.clear();
     auto dv_info = orb_camera::vi_list_ptr();
-    if (!enable_pc) {
-        dv_info = cam->query_video_info(orb_camera::V_DEPTH);
-    } else {
-        dv_info = cam->query_d2c_info(c_conf_list[c_conf_index].index);
-    }
+    dv_info = cam->query_d2c_info(c_conf_list[c_conf_index].index);
     for (auto v_info: *dv_info) {
         if (strcmp(v_info.fmt_name, "y16") == 0) {
             d_conf_list.emplace_back(v_info.index, std::string(v_info));
@@ -67,7 +62,6 @@ void orb_camera_ui::impl::start_camera() {
     assert(cam != nullptr);
     cam_s_conf.color.config_index = c_conf_list[c_conf_index].index;
     cam_s_conf.depth.config_index = d_conf_list[d_conf_index].index;
-    cam_s_conf.pc.enable = enable_pc;
     cam->start(cam_s_conf);
 }
 

src/device/impl/orb_camera_ui_impl.h

@@ -28,8 +28,6 @@ struct orb_camera_ui::impl {
     orb_camera::start_config cam_s_conf;
     std::unique_ptr<orb_camera> cam;
 
-    bool enable_pc = true;
-
     explicit impl(create_config conf);
 
     void refresh_dev_info_list();

src/device/orb_camera.h

@@ -70,10 +70,6 @@ public:
             uint32_t config_index = 0;
             obj_name_type name = -1;
         } depth;
-        struct pc_config_type { // point cloud
-            bool enable = false;
-            obj_name_type name = -1;
-        } pc;
     };
 
     bool start(start_config conf);

src/device/orb_camera_ui.h

@@ -12,7 +12,6 @@ public:
     struct create_config {
         obj_name_type cf_name = invalid_obj_name; // color frame name
         obj_name_type df_name = invalid_obj_name; // depth frame name
-        obj_name_type pc_name = invalid_obj_name; // point cloud name
         obj_name_type remap_name = invalid_obj_name;
         boost::asio::io_context *ctx = nullptr;
         smart_cuda_stream *stream = default_cuda_stream;

src/image_process/camera_calibrator.h

@@ -42,6 +42,7 @@ public:
         // 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

@@ -2,6 +2,7 @@
 #include "core/imgui_utility.hpp"
 #include "third_party/scope_guard.hpp"
 #include "render/render_utility.h"
+#include "render/render_texture.h"
 
 #include <boost/asio/post.hpp>
 
@@ -356,6 +357,8 @@ void camera_calibrator::impl::start() {
     track_pool.clear();
     img_ok_cnt = 0;
 
+    coverage_fbo = std::make_unique<smart_frame_buffer>();
+
     assert(tp == nullptr);
     tp = std::make_unique<BS::thread_pool>(conf.num_threads);
     img_conn = OBJ_SIG(conf.img_name)->connect(
@@ -408,6 +411,7 @@ void camera_calibrator::impl::stop(bool on_exit) {
     assert(img_conn.connected());
     img_conn.disconnect();
     tp = nullptr; // implicit wait()
+    coverage_fbo = nullptr;
     if (!on_exit) {
         save_track_data();
         process();
@@ -545,13 +549,18 @@ void camera_calibrator::impl::img_callback(obj_name_type name) {
     auto cur_ts = OBJ_TS(conf.img_name);
     auto ref_t = conf.sophiar_conn
             ->query_transform_variable(conf.ref_transform_var);
-    if (!ref_t.has_value()) {
-        return;
-    } else {
-        auto &item = track_pool.emplace_back();
-        item.ref_mat = to_mat4(*ref_t);
-        item.sample_ts = cur_ts;
-    }
+    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
+            ->query_double_variable(conf.ref_error_var);
+    if (!err_t.has_value()) return;
+//    assert(err_t.has_value());
+    item.track_error = *err_t;
+    last_track_err = item.track_error;
+    if (item.track_error > track_err_threshold) return;
 
     // throttle
     if (tp->get_tasks_queued() > conf.num_threads - 1) {
@@ -570,13 +579,7 @@ void camera_calibrator::impl::img_callback(obj_name_type name) {
     });
 }
 
-void camera_calibrator::impl::render() {
-    if (tp == nullptr) return; // not running
-    auto st = last_finish.load(std::memory_order::acquire);
-    if (st == nullptr) return;
-    assert(st->process_finished);
-    if (!st->corners_detected) return;
-
+void camera_calibrator::impl::render_corners(const corner_type &corner, NVGcolor color) {
     auto size = query_viewport_size();
     nvgBeginFrame(vg, size.width, size.height, 1.0f);
     auto closer = sg::make_scope_guard([this] {
@@ -588,14 +591,47 @@ void camera_calibrator::impl::render() {
 
     nvgBeginPath(vg);
     static constexpr auto r = 5.0f;
-    for (auto &p: st->corners) {
+    for (auto &p: corner) {
         nvgCircle(vg, p.x, size.height - p.y, r); // flip Y
     }
     nvgPathWinding(vg, NVG_HOLE);
-    nvgFillColor(vg, nvgRGB(255, 0, 0));
+    nvgFillColor(vg, color);
     nvgFill(vg);
 }
 
+void camera_calibrator::impl::render() {
+    if (tp == nullptr) return; // not running
+
+    // render coverage
+    assert(coverage_fbo != nullptr);
+    if (coverage_fbo->id == 0) [[unlikely]] { // lazy creation for img_size
+        auto fbo_conf = smart_frame_buffer::create_config{
+                .size = img_size,
+                .depth_fmt = GL_DEPTH24_STENCIL8,
+        };
+        coverage_fbo->create(fbo_conf);
+    } else {
+        auto tex_conf = tex_render_info{
+                .mode = TEX_COLOR_ONLY,
+        };
+        tex_conf.color.fmt = COLOR_RGB;
+        tex_conf.color.id = coverage_fbo->color_tex[0].id;
+        tex_conf.color.alpha = 0.5f;
+        render_texture(tex_conf);
+    }
+
+    auto st = last_finish.load(std::memory_order::acquire);
+    if (st == nullptr) return;
+    assert(st->process_finished);
+    if (!st->corners_detected) return;
+
+    render_corners(st->corners, nvgRGB(255, 0, 0));
+
+    coverage_fbo->bind();
+    render_corners(st->corners, nvgRGB(0, 255, 0));
+    coverage_fbo->unbind();
+}
+
 void camera_calibrator::impl::show() {
     auto ctx = conf.ctx;
     bool is_running = (tp != nullptr);
@@ -618,15 +654,18 @@ void camera_calibrator::impl::show() {
         ImGui::Text("Pending Images: %ld", tp->get_tasks_total());
         ImGui::Text("Track Count: %ld", track_pool.size());
         ImGui::Text("Image Count: %d / %ld", (int) img_ok_cnt, img_pool.size());
+
+        { // track error
+            auto guard = imgui_valid_style_guard(
+                    last_track_err <= track_err_threshold);
+            ImGui::Text("Last Track Error: %.2f", last_track_err);
+        }
+
         auto last_st = last_finish.load(std::memory_order::consume);
         if (last_st != nullptr && last_st->corners_detected) {
-            if (last_st->corner_sharpness <= sharpness_threshold) {
-                ImGui::PushStyleColor(ImGuiCol_Text, (ImVec4) ImColor(0, 255, 0));
-            } else {
-                ImGui::PushStyleColor(ImGuiCol_Text, (ImVec4) ImColor(255, 0, 0));
-            }
-            ImGui::Text("Last sharpness: %.2f", last_st->corner_sharpness);
-            ImGui::PopStyleColor();
+            auto guard = imgui_valid_style_guard(
+                    last_st->corner_sharpness <= sharpness_threshold);
+            ImGui::Text("Last Sharpness: %.2f", last_st->corner_sharpness);
         }
     }
 }

src/image_process/impl/camera_calibrator_impl.h

@@ -4,6 +4,7 @@
 #include "image_process/camera_calibrator.h"
 #include "core/image_utility_v2.h"
 #include "core/math_helper.hpp"
+#include "render/render_utility.h"
 
 #include <opencv2/core/mat.hpp>
 
@@ -18,6 +19,7 @@ namespace camera_calibrator_impl {
 
     // TODO: make configurable
     static constexpr auto sharpness_threshold = 4.f;
+    static constexpr auto track_err_threshold = 0.25f;
 
     using corner_type = std::vector<cv::Point2f>;
 
@@ -133,6 +135,7 @@ struct camera_calibrator::impl {
 
     struct track_store_type {
         glm::mat4 ref_mat;
+        float track_error;
         timestamp_type sample_ts;
     };
 
@@ -140,6 +143,8 @@ struct camera_calibrator::impl {
     track_pool_type track_pool;
 
     std::atomic<img_store_type *> last_finish = nullptr;
+    float last_track_err = std::numeric_limits<float>::quiet_NaN();
+    std::unique_ptr<smart_frame_buffer> coverage_fbo;
     NVGcontext *vg = nullptr;
 
     std::unique_ptr<BS::thread_pool> tp; // thread pool
@@ -167,6 +172,8 @@ struct camera_calibrator::impl {
 
     void stop(bool on_exit = false);
 
+    void render_corners(const corner_type &corner, NVGcolor color);
+
     void render();
 
     void show();

src/impl/apps/depth_guide/depth_guide.cpp

@@ -18,7 +18,6 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
     OBJ_SAVE(img_out, image_ptr());
     OBJ_SAVE(img_obj_mask, image_ptr());
     OBJ_SAVE(pc_raw, pc_ptr());
-    OBJ_SAVE(pc_obj_raw, pc_ptr());
     OBJ_SAVE(pc_obj, pc_ptr());
 
     // initialize meta
@@ -28,17 +27,18 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
     OBJ_SIG(img_obj_mask)->connect([](auto _) {
         gen_pc_rgb_with_mask::call(
                 {img_color, img_depth, img_obj_mask,
-                 img_remap, pc_obj_raw, default_cuda_stream});
+                 img_remap, pc_obj, default_cuda_stream});
     });
-    OBJ_SIG(pc_obj_raw)->connect([](auto _) {
-        shrink_pc_zero_depth::call(
-                {pc_obj_raw, pc_obj, default_cuda_stream});
+    OBJ_SIG(img_depth)->connect([](auto _) {
+        gen_pc_rgbd::call(
+                {img_color, img_depth, img_remap,
+                 pc_raw, default_cuda_stream});
     });
 
     // initialize modules
     auto orb_cam_conf = orb_camera_ui::create_config{
             .cf_name = img_color, .df_name = img_depth,
-            .pc_name = pc_raw, .remap_name = img_remap,
+            .remap_name = img_remap,
             .ctx = conf.asio_ctx, .stream = default_cuda_stream,
     };
     orb_cam = std::make_unique<orb_camera_ui>(orb_cam_conf);

src/impl/apps/depth_guide/depth_guide.h

@@ -50,7 +50,7 @@ private:
         img_obj_mask,
 
         // object point cloud
-        pc_obj_raw, pc_obj,
+        pc_obj,
     };
 
     create_config conf;

src/impl/apps/endo_guide/endo_guide.cpp

@@ -50,6 +50,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
             },
             .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"),
     };
     cam_calib = std::make_unique<camera_calibrator>(calib_conf);

src/module/impl/image_augment_helper_v2.cpp

@@ -70,6 +70,7 @@ void image_augment_helper_v2::impl::set_camera_info(const camera_intrinsic_v0 &i
     auto y_min = remap_bound.y, y_max = y_min + remap_bound.height;
     auto x_len = std::max(-x_min, x_max);
     auto y_len = std::max(-y_min, y_max);
+    x_len = std::min(x_len, max_camera_x_axis_len);
     y_len = std::min(y_len, max_camera_y_axis_len);
     auto cam_fov = 2.f * std::atan(y_len);
 

src/module/impl/image_augment_helper_v2_impl.h

@@ -10,6 +10,7 @@ struct image_augment_helper_v2::impl {
 
     // TODO: make configurable
     static constexpr auto max_camera_y_axis_len = 3.f;
+    static constexpr auto max_camera_x_axis_len = 6.f;
 
     image_augment_helper_v2 *q_this = nullptr;
     create_config conf;

src/module/impl/image_saver.cpp

@@ -17,6 +17,7 @@ using boost::asio::post;
 void image_saver::impl::item_store_type::save_ply() {
     auto file_name = fmt::format("{}_{}.ply", ui_name, par->save_cnt);
     auto pc = OBJ_QUERY(pc_ptr, img_name);
+    if (pc == nullptr) return;
     assert(pc->format() == PC_XYZ_RGB);
 
     auto size = pc->size();