Implemented tracker latency compensation.

CMakeLists.txt

@@ -30,6 +30,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/codec/scene_encoder.cpp
         src/core/impl/event_timer.cpp
         src/core/impl/image_utility_v2.cpp
+        src/core/impl/math_helper.cpp
         src/core/impl/memory_pool.cpp
         src/core/impl/object_manager.cpp
         src/core/impl/pc_utility.cpp

src/core/impl/math_helper.cpp

@@ -0,0 +1,58 @@
+#include "core/math_helper.hpp"
+
+#include <map>
+
+struct transform_buffer::impl {
+    using buffer_type =
+            std::map<timestamp_type, glm::mat4>;
+    buffer_type buffer;
+
+    void add(const glm::mat4 &mat, timestamp_type ts) {
+        assert(!buffer.contains(ts));
+        buffer.emplace(ts, mat);
+    }
+
+    glm::mat4 query(timestamp_type ts) {
+        auto iter = buffer.lower_bound(ts);
+        assert(iter != buffer.end());
+        if (iter == buffer.begin()
+            || iter->first == ts) { // do not need interpolation
+            return iter->second;
+        }
+        assert(iter != buffer.begin());
+        auto next = iter, prev = --iter;
+        auto next_ts = next->first, prev_ts = prev->first;
+        auto interp_t = 1.0f * (ts - prev_ts) / (next_ts - prev_ts);
+        return transform_mix(prev->second, next->second, interp_t);
+    }
+
+    void remove_old(int max_offset) {
+        auto last_ts = buffer.rbegin()->first;
+        for (;;) {
+            auto next = buffer.begin()++;
+            if (last_ts - next->first > max_offset) {
+                buffer.erase(buffer.begin());
+            } else {
+                break;
+            }
+        }
+    }
+
+};
+
+transform_buffer::transform_buffer()
+        : pimpl(std::make_unique<impl>()) {}
+
+transform_buffer::~transform_buffer() = default;
+
+void transform_buffer::add(const glm::mat4 &mat, timestamp_type ts) {
+    pimpl->add(mat, ts);
+}
+
+glm::mat4 transform_buffer::query(timestamp_type ts) {
+    return pimpl->query(ts);
+}
+
+void transform_buffer::remove_old(int max_offset) {
+    pimpl->remove_old(max_offset);
+}

src/core/math_helper.hpp

@@ -1,7 +1,10 @@
 #ifndef DEPTHGUIDE_MATH_HELPER_HPP
 #define DEPTHGUIDE_MATH_HELPER_HPP
 
+#include "core/utility.hpp"
+
 #include <glm/glm.hpp>
+#include <glm/gtc/matrix_access.hpp>
 #include <glm/gtc/quaternion.hpp>
 #include <glm/gtx/transform.hpp>
 
@@ -106,4 +109,39 @@ inline glm::vec3 transform_v(const glm::mat4 &mat, const glm::vec3 &vec) {
     return glm::mat3(mat) * vec;
 }
 
+inline glm::vec3 to_translation(const glm::mat4 &mat) {
+    return glm::column(mat, 3);
+}
+
+inline glm::mat4 transform_mix(const glm::mat4 &x, const glm::mat4 &y, float a) {
+    // translation
+    auto xt = to_translation(x);
+    auto yt = to_translation(y);
+    auto zt = glm::mix(xt, yt, a);
+
+    // rotation
+    auto xq = glm::quat_cast(glm::mat3(x));
+    auto yq = glm::quat_cast(glm::mat3(y));
+    auto zq = glm::slerp(xq, yq, a);
+
+    return to_transform_mat(zt, glm::mat3_cast(zq));
+}
+
+class transform_buffer {
+public:
+    transform_buffer();
+
+    ~transform_buffer();
+
+    void add(const glm::mat4 &mat, timestamp_type ts);
+
+    glm::mat4 query(timestamp_type ts);
+
+    void remove_old(int max_offset);
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
 #endif //DEPTHGUIDE_MATH_HELPER_HPP

src/image_process/camera_calibrator.h

@@ -18,6 +18,11 @@ public:
         CALIB_CIRCLE,
     };
 
+    struct result_type {
+        camera_intrinsic_v0 cam_in;
+        int time_offset = 0;
+    };
+
     struct create_config {
         obj_name_type img_name;
         calib_board_enum board_type;
@@ -31,7 +36,7 @@ public:
         io_context *ctx;
 
         using cb_func_type =
-                std::function<void(camera_intrinsic_v0)>;
+                std::function<void(result_type)>;
         cb_func_type cb_func;
 
         // for sophiar

src/image_process/impl/camera_calibrator.cpp

@@ -95,10 +95,6 @@ namespace camera_calibrator_impl {
         return ret;
     }
 
-    glm::vec3 to_translation(const glm::mat4 &mat) {
-        return glm::column(mat, 3);
-    }
-
     void hand_eye_calib::set_object_points(cv::Size size, float dis) {
         obj_ps.clear();
         for (auto iy = 0; iy < size.height; ++iy)
@@ -108,28 +104,7 @@ namespace camera_calibrator_impl {
     }
 
     glm::mat4 hand_eye_calib::calc_track_mat(timestamp_type t) {
-        auto iter = track_pool.lower_bound(t);
-        assert(iter != track_pool.end());
-        if (iter == track_pool.begin()
-            || iter->first == t) { // do not need interpolation
-            return iter->second;
-        }
-
-        auto next = iter, prev = --iter;
-        assert(next != track_pool.begin());
-        auto next_ts = next->first, prev_ts = prev->first;
-        auto interp_t = 1.0f * (t - prev_ts) / (next_ts - prev_ts);
-
-        auto next_mat = next->second, prev_mat = prev->second;
-        auto next_t = to_translation(next_mat);
-        auto prev_t = to_translation(prev_mat);
-        auto cur_t = glm::mix(prev_t, next_t, interp_t);
-
-        auto next_quat = glm::quat_cast(glm::mat3(next_mat));
-        auto prev_quat = glm::quat_cast(glm::mat3(prev_mat));
-        auto cur_quat = glm::slerp(prev_quat, next_quat, interp_t);
-
-        return to_transform_mat(cur_t, glm::mat3_cast(cur_quat));
+        return track_pool.query(t);
     }
 
     glm::vec2 hand_eye_calib::distort_point(glm::vec2 p) {
@@ -405,9 +380,8 @@ void camera_calibrator::impl::process() {
             });
     std::ranges::copy(valid_imgs, std::back_inserter(calib->img_pool));
 
-    // TODO: C++23 can done this by std::ranges::to
     for (auto &item: track_pool) {
-        calib->track_pool.emplace(item.sample_ts, item.ref_mat);
+        calib->track_pool.add(item.ref_mat, item.sample_ts);
     }
 
     calib->calc();
@@ -418,7 +392,10 @@ void camera_calibrator::impl::process() {
         conf.sophiar_conn->update_transform_variable(
                 conf.result_transform_var, ret);
     }
-    conf.cb_func(calib->intrinsic_v0());
+    conf.cb_func(result_type{
+            .cam_in = calib->intrinsic_v0(),
+            .time_offset = calib->result_ts_offset,
+    });
 }
 
 void camera_calibrator::impl::simulate_process(const simulate_info_type &info) {
@@ -467,6 +444,7 @@ void camera_calibrator::impl::save_track_data() {
         for (auto &point: item.corners) {
             fout << point.x << " " << point.y << " ";
         }
+        fout << item.corner_sharpness;
         fout << std::endl;
     }
     fout << std::endl;
@@ -504,6 +482,7 @@ void camera_calibrator::impl::load_track_data(const std::string &path) {
             auto &p = item.corners.emplace_back();
             fin >> p.x >> p.y;
         }
+        fin >> item.corner_sharpness;
         item.corners_detected = true;
         item.process_finished = true;
     }

src/image_process/impl/camera_calibrator_impl.h

@@ -33,15 +33,11 @@ namespace camera_calibrator_impl {
 
     glm::vec3 to_vec3(const cv::Mat &mat);
 
-    glm::vec3 to_translation(const glm::mat4 &mat);
-
     struct hand_eye_calib {
 
         // fill before use
 
-        using track_pool_type =
-                std::map<timestamp_type, glm::mat4>;
-        track_pool_type track_pool;
+        transform_buffer track_pool;
 
         struct image_store_type {
             corner_type corner;

src/impl/apps/endo_guide/endo_guide.cpp

@@ -1,5 +1,6 @@
 #include "endo_guide.h"
 #include "core/imgui_utility.hpp"
+#include "module/camera_augment_helper_v2.h"
 
 app_endo_guide::app_endo_guide(const create_config &_conf) {
     main_conf = _conf;
@@ -42,8 +43,10 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
             .board_type = camera_calibrator::CALIB_CHESS,
             .pattern_size = cv::Size(11, 8), .corner_distance = 5,
             .stream = default_cuda_stream, .ctx = main_conf.asio_ctx,
-            .cb_func = [this](auto info) {
-                aug_helper->set_camera_info(info);
+            .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(),
             .ref_transform_var = LOAD_STR("camera_ref_transform_var"),
@@ -77,11 +80,11 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     monitor = std::make_unique<sophiar_monitor>(monitor_conf);
 
-    auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_10.96.txt",
-            .img_size = cv::Size(1920, 1080),
-    };
-    cam_calib->simulate_process(sim_info);
+//    auto sim_info = camera_calibrator::simulate_info_type{
+//            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_10.96.txt",
+//            .img_size = cv::Size(1920, 1080),
+//    };
+//    cam_calib->simulate_process(sim_info);
 }
 
 void app_endo_guide::show_ui() {

src/module/camera_augment_helper_v2.h

@@ -74,6 +74,9 @@ public:
 
     camera_info get_camera();
 
+    // latency of image to tracked transform, offset < 0
+    void set_fixed_transform_latency(int offset);
+
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;

src/module/image_augment_helper_v2.h

@@ -33,6 +33,8 @@ public:
 
     void render();
 
+    camera_augment_helper_v2 *get_camera_helper();
+
     using render_sig_type = boost::signals2::signal<void()>;
     render_sig_type post_render_sig;
 

src/module/impl/camera_augment_helper_v2.cpp

@@ -132,7 +132,10 @@ void camera_augment_helper_v2::impl::update_tracked_transform() {
             query_transform_variable(transform_var);
     is_missing = !trans.has_value();
     if (trans.has_value()) {
-        tracked_transform = to_mat4(*trans);
+        auto cur_ts = current_timestamp();
+        track_pool.add(to_mat4(*trans), cur_ts);
+        tracked_transform = track_pool.query(cur_ts + time_offset);
+        track_pool.remove_old(-time_offset);
     } else {
         tracked_transform = {};
     }
@@ -377,6 +380,10 @@ camera_info camera_augment_helper_v2::get_camera() {
     return pimpl->last_camera;
 }
 
+void camera_augment_helper_v2::set_fixed_transform_latency(int offset) {
+    pimpl->time_offset = offset;
+}
+
 void camera_augment_helper_v2::set_active_guidance(bool flag) {
     pimpl->allow_active_guide = flag;
 }

src/module/impl/camera_augment_helper_v2_impl.h

@@ -2,6 +2,7 @@
 #define DEPTHGUIDE_CAMERA_AUGMENT_HELPER_V2_IMPL_H
 
 #include "module/camera_augment_helper_v2.h"
+#include "core/math_helper.hpp"
 #include "module/viewport_downloader.hpp"
 
 struct camera_augment_helper_v2::impl {
@@ -12,6 +13,9 @@ struct camera_augment_helper_v2::impl {
     std::optional<glm::mat4> tracked_transform; // transform matrix from sophiar
     bool is_missing = false;
 
+    int time_offset = -76000; // -76ms
+    transform_buffer track_pool;
+
     augment_manager_v2 *manager = nullptr;
     sophiar_conn_type *sophiar_conn = nullptr;
     using io_context = boost::asio::io_context;

src/module/impl/image_augment_helper_v2.cpp

@@ -102,6 +102,10 @@ void image_augment_helper_v2::impl::render() {
     out_ren.render_v2(output_img, img_conf);
 }
 
+camera_augment_helper_v2 *image_augment_helper_v2::get_camera_helper() {
+    return pimpl->cam_helper.get();
+}
+
 image_augment_helper_v2::image_augment_helper_v2(create_config conf)
         : pimpl(std::make_unique<impl>(conf)) {
     pimpl->q_this = this;

src/render/impl/render_mesh.cpp

@@ -226,7 +226,7 @@ namespace render_mesh_impl {
         glDrawElements(GL_TRIANGLES, 3 * gl_info->num_triangle, GL_UNSIGNED_INT, nullptr);
     }
 
-    void ren_mesh_normal(const mesh_render_info &info) {
+    void ren_mesh(const mesh_render_info &info) {
         if (pg_normal == nullptr) {
             pg_normal = std::unique_ptr<smart_program>(
                     smart_program::create("mesh_normal",
@@ -250,6 +250,25 @@ namespace render_mesh_impl {
         draw_mesh(info.model.mesh);
     }
 
+    // only opacity
+    void ren_mesh_normal(const mesh_render_info &info) {
+        fbo_conf.size = query_viewport_size();
+        mesh_fbo.create(fbo_conf);
+        mesh_fbo.bind();
+        ren_mesh(info);
+        mesh_fbo.unbind();
+
+        // color channel
+        auto tex_info = tex_render_info();
+        tex_info.mode = TEX_COLOR_DEPTH;
+        tex_info.color.fmt = COLOR_RGBA;
+        tex_info.color.id = mesh_fbo.color_tex[0].id;
+        tex_info.color.opacity = info.alpha;
+        tex_info.depth.id = mesh_fbo.depth_tex.id;
+        tex_info.depth.enable_alpha_effect = false;
+        render_texture(tex_info);
+    }
+
     void ren_mesh_depth_alpha(const mesh_render_info &info) {
         auto &extra = info.extra.depth_alpha;
         assert(extra.bg.mesh == nullptr);
@@ -257,13 +276,13 @@ namespace render_mesh_impl {
         fbo_conf.size = query_viewport_size();
         mesh_fbo.create(fbo_conf);
         mesh_fbo.bind();
-        ren_mesh_normal(info);
+        ren_mesh(info);
         mesh_fbo.unbind();
 
         auto tex_info = tex_render_info();
         tex_info.mode = TEX_COLOR_DEPTH;
         tex_info.color.id = mesh_fbo.color_tex[0].id;
-        tex_info.color.alpha = extra.alpha;
+        tex_info.color.alpha = info.alpha;
         tex_info.depth.id = mesh_fbo.depth_tex.id;
         tex_info.depth.enable_alpha_effect = true;
         auto &tex_extra = tex_info.extra.color_depth;

src/render/impl/render_mesh_impl.h

@@ -12,6 +12,8 @@ namespace render_mesh_impl {
 
     void draw_mesh(mesh_type *mesh);
 
+    void ren_mesh(const mesh_render_info &info);
+
     void ren_mesh_normal(const mesh_render_info &info);
 
     using mesh_cache_type =

src/render/impl/render_scene.cpp

@@ -40,7 +40,8 @@ namespace render_scene_impl {
                 .mode = MESH_NORMAL,
                 .model = mesh_conf,
                 .camera = req.camera,
-                .light= req.light,
+                .light = req.light,
+                .alpha = req.item.alpha,
         };
 
         // depth alpha
@@ -49,7 +50,6 @@ namespace render_scene_impl {
             auto &extra = ren_conf.extra.depth_alpha;
             extra.bg_tex = scene_fbo.depth_tex.id;
             extra.alpha_factor = info.alpha_factor;
-            extra.alpha = req.item.alpha;
         }
 
         render_mesh(ren_conf);

src/render/impl/render_texture.cpp

@@ -17,7 +17,7 @@ namespace render_texture_impl {
     pg_type pg_rgb;   // render rgb texture
     pg_type pg_rgb_remap; // render rgb texture with remap
     pg_type pg_rgba;  // render rgba texture
-    pg_type pg_rgba_remap;  // render rgba texture with remap
+    pg_type pg_rgba_d;  // render rgba and depth texture
     pg_type pg_rgb_d; // render rgb and depth texture
     pg_type pg_rgbd2; // render rgb and depth texture (use depth alpha)
     pg_type pg_nv12;  // render nv12 textures
@@ -242,6 +242,31 @@ namespace render_texture_impl {
         draw();
     }
 
+    void ren_rgba_d(const tex_render_info &info) {
+        auto &pg = pg_rgba_d;
+        if (pg == nullptr) {
+            pg = std::unique_ptr<smart_program>(
+                    smart_program::create("tex_rgba_d",
+                                          {{GL_VERTEX_SHADER,   "tex.vert"},
+                                           {GL_FRAGMENT_SHADER, "tex_rgba_d.frag"}}));
+        }
+        assert(pg != nullptr);
+        pg->use();
+
+        pg->set_uniform_f("opacity", info.color.alpha);
+
+        glActiveTexture(GL_TEXTURE0 + 0);
+        glBindTexture(GL_TEXTURE_2D, info.color.id);
+        pg->set_uniform_i("c_tex", 0);
+        glActiveTexture(GL_TEXTURE0 + 1);
+        glBindTexture(GL_TEXTURE_2D, info.depth.id);
+        pg->set_uniform_i("d_tex", 1);
+
+        glEnable(GL_DEPTH_TEST);
+        config_buffers(info);
+        draw();
+    }
+
     void ren_rgb_d_v2(const tex_render_info &info) {
         auto &pg = pg_rgbd2;
         if (pg == nullptr) {
@@ -340,6 +365,10 @@ namespace render_texture_impl {
                 }
                 break;
             }
+            case COLOR_RGBA: {
+                ren_rgba_d(info);
+                break;
+            }
             default: {
                 RET_ERROR;
             }

src/render/impl/shader/tex_rgba_d.frag

@@ -0,0 +1,17 @@
+#version 460
+
+uniform float opacity;
+
+uniform sampler2D c_tex; // color texture
+uniform sampler2D d_tex; // depth texture
+
+in vec2 frag_uv;
+
+layout (location = 0) out vec4 frag_color;
+layout (depth_less) out float gl_FragDepth;
+
+void main() {
+    frag_color = texture(c_tex, frag_uv);
+    frag_color.a *= opacity;
+    gl_FragDepth = texture(d_tex, frag_uv).x;
+}

src/render/render_mesh.h

@@ -95,6 +95,8 @@ struct mesh_render_info {
     camera_info_type camera = {};
     light_info_type light = {};
 
+    float alpha = 1.0f;
+
     union {
         struct {
         } normal;
@@ -102,7 +104,6 @@ struct mesh_render_info {
             mesh_info_type bg; // if nullptr, bg_tex should be provided.
             GLuint bg_tex = 0; // background depth texture
             float alpha_factor;
-            float alpha;
             bool show_bg;
         } depth_alpha;
     } extra = {};