Implemented image augment helper v2.

data/config_endo_guide.yaml

@@ -13,4 +13,12 @@ monitor:
   - name: Femur
     var: femur_ref_in_tracker
   - name: Tibia
-    var: tibia_ref_in_tracker
+    var: tibia_ref_in_tracker
+
+augment_list:
+  - name: Femur
+    transform_var: femur_in_tracker_denoised
+    model: /home/tpx/project/DepthGuide/data/model/Femur_3.stl
+  - name: Tibia
+    transform_var: tibia_in_tracker_denoised
+    model: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl

data/sophiar_config_endo_guide.json

@@ -14,7 +14,16 @@
     },
     {
       "name": "femur_in_femur_ref",
-      "type": "transform_obj"
+      "type": "transform_obj",
+      "value": [
+        405.60876310826154,
+        -11.723419024851964,
+        -64.43931776227325,
+        -0.25881623739773235,
+        0.5976871267586126,
+        -0.3985157377272106,
+        0.6457317249059794
+      ]
     },
     {
       "name": "tibia_ref_in_tracker",
@@ -22,7 +31,16 @@
     },
     {
       "name": "tibia_in_tibia_ref",
-      "type": "transform_obj"
+      "type": "transform_obj",
+      "value": [
+        117.21224919149104,
+        38.32624851921699,
+        -72.90989200004259,
+        -0.19759914802407674,
+        0.6557689584549056,
+        -0.28931051779904626,
+        0.6687458965242118
+      ]
     },
     {
       "name": "camera_ref_in_tracker",

src/core/impl/image_utility_v2.cpp

@@ -466,6 +466,7 @@ void generic_image::cuda_modified(smart_cuda_stream *stream) {
 }
 
 image_ptr to_image(obj_name_type name) {
+    if (name == invalid_obj_name) return nullptr;
     auto img_type = OBJ_TYPE(name);
     if (OBJ_TYPE(name) == typeid(image_ptr)) {
         return OBJ_QUERY(image_ptr, name);

src/core/object_manager.h

@@ -165,6 +165,7 @@ private:
 
 using obj_name_type = object_manager::name_type;
 using obj_conn_type = boost::signals2::connection;
+using io_ctx_type = object_manager::io_context;
 
 static constexpr obj_name_type invalid_obj_name = -1;
 

src/device/impl/orb_camera.cpp

@@ -102,6 +102,16 @@ namespace orb_camera_impl {
         return create_image(f32_info);
     }
 
+    camera_intrinsic_v0 remap_codec::remap_info::to_v0() {
+        auto ret = camera_intrinsic_v0{
+                .fx = fx, .fy = fy, .cx = cx, .cy=cy,
+        };
+        std::ranges::copy(k, std::back_inserter(ret.dist));
+        ret.width = width;
+        ret.height = height;
+        return ret;
+    }
+
     data_type remap_codec::encode(const image_ptr &img, bool force_idr) {
         if (force_idr) {
             cache.clear();
@@ -124,34 +134,14 @@ namespace orb_camera_impl {
         info.fill_from(reader);
         assert(reader.empty());
 
-        // @formatter:off
-        cv::Mat cam_mat_cv = (cv::Mat_<float>(3, 3) <<
-                info.fx, 0.f, info.cx,
-                0, info.fy, info.cy,
-                0, 0, 1);
-        cv::Mat dist_cv = cv::Mat_<float>(info.k);
-        // @formatter:on
-
-        auto img_size = cv::Size(info.width, info.height);
-        auto points_size = cv::Size(img_size.area(), 1);
-        auto points_mat = cv::Mat(points_size, CV_32FC2);
-        for (auto iy = 0; iy < info.height; ++iy)
-            for (auto ix = 0; ix < info.width; ++ix) {
-                auto id = iy * info.width + ix;
-                points_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
-            }
-
-        auto undistort_img = create_image(img_size, CV_32FC2);
-        auto img_data = undistort_img->memory(MEM_HOST).start_ptr();
-        auto undistort_mat = cv::Mat(points_size, CV_32FC2, img_data);
-        cv::undistortPoints(points_mat, undistort_mat, cam_mat_cv, dist_cv);
-        undistort_img->host_modified();
-        undistort_img->set_meta_any(META_ORB_REMAP_INFO, info);
-        undistort_img->set_meta_any(META_IMAGE_SPECIAL_CODEC, REMAP_CODEC_NAME);
-
+        image_ptr ret_img;
+        auto remap_conf = gen_undistort_remap_image::config_direct{
+                .cam_info = info.to_v0(), .img = &ret_img,
+        };
+        gen_undistort_remap_image::call_direct(remap_conf);
         assert(!cache.contains(key_id));
-        cache.emplace(key_id, undistort_img);
-        return undistort_img;
+        cache.emplace(key_id, ret_img);
+        return ret_img;
     }
 
     image_ptr remap_codec::decode(const OBCameraParam &param) { // TODO: accelerate with CUDA

src/device/impl/orb_camera_impl.h

@@ -6,6 +6,7 @@
 #include "core/image_utility_v2.h"
 #include "core/pc_utility.h"
 #include "network/binary_utility.hpp"
+#include "image_process/camera_utility.hpp"
 
 #include <boost/asio/io_context.hpp>
 
@@ -51,6 +52,8 @@ namespace orb_camera_impl {
                 writer << k;
                 writer << width << height;
             }
+
+            camera_intrinsic_v0 to_v0();
         };
 
         static constexpr auto hash_seed = 0;

src/device/impl/uvc_camera.cpp

@@ -1,6 +1,8 @@
 #include "uvc_camera_impl.h"
 #include "core/image_utility.hpp"
+#include "core/image_utility_v2.h"
 #include "core/utility.hpp"
+#include "image_process/cuda_impl/pixel_convert.cuh"
 #include "third_party/scope_guard.hpp"
 
 #include <opencv2/imgcodecs.hpp>
@@ -11,6 +13,8 @@
 #include <fmt/format.h>
 #include <spdlog/spdlog.h>
 
+#include <boost/asio/post.hpp>
+
 namespace uvc_camera_impl {
 
     bool check_api_call(uvc_error_t api_ret, unsigned int line_number,
@@ -98,6 +102,26 @@ namespace uvc_camera_impl {
         return create_image(img_rgb_info);
     }
 
+    image_ptr yuyv_frame_to_image(uvc_frame_t *frame) {
+        assert(frame->frame_format == UVC_FRAME_FORMAT_YUYV);
+        auto img_size = cv::Size(frame->width, frame->height);
+        auto img_yuyv = create_image(img_size, CV_8UC2);
+        if (img_yuyv->size_in_bytes() != frame->data_bytes) return nullptr;
+        auto yuyv_mem = img_yuyv->memory(MEM_HOST);
+        memcpy(yuyv_mem.start_ptr(), frame->data, frame->data_bytes);
+        return img_yuyv;
+    }
+
+    image_u8c3 image_yuyv_to_rgb(const image_ptr &img_yuyv,
+                                 smart_cuda_stream *stream) {
+        auto img_rgb = create_image(img_yuyv->size(), CV_8UC3);
+        call_yuyv_to_rgb(img_yuyv->cuda<uchar2>(stream),
+                         img_rgb->cuda<uchar3>(stream),
+                         stream->cuda);
+        img_rgb->cuda_modified(stream);
+        return img_rgb->v1<uchar3>();
+    }
+
 }
 
 uvc_camera::impl::~impl() {
@@ -117,6 +141,7 @@ uvc_camera::impl::create(create_config conf) {
     assert(uvc_ctx != nullptr);
 
     auto ret = std::make_unique<impl>();
+    ret->conf = conf;
     ret->dev = (uvc_device_t *) conf.h_dev;
     uvc_ref_device(ret->dev);
     API_CHECK_P(uvc_open(ret->dev, &ret->dev_h));
@@ -146,16 +171,26 @@ uvc_camera::impl::create(create_config conf) {
 }
 
 void uvc_camera::impl::frame_callback(uvc_frame *frame) {
-    auto img = image_u8c3();
     switch (frame->frame_format) {
-        // @formatter:off
-        case UVC_FRAME_FORMAT_MJPEG: { img = mjpeg_frame_to_image(frame); break; }
-        // @formatter:on
+        case UVC_FRAME_FORMAT_MJPEG: {
+            auto img = mjpeg_frame_to_image(frame);
+            OBJ_SAVE(img_name, img);
+            break;
+        }
+        case UVC_FRAME_FORMAT_YUYV: {
+            auto img_yuv = yuyv_frame_to_image(frame);
+            if (img_yuv == nullptr) break;
+            boost::asio::post(*conf.ctx, [=, this] { // handle CUDA context problem
+                auto img_rgb = image_yuyv_to_rgb(img_yuv, conf.stream);
+                OBJ_SAVE(img_name, img_rgb);
+            });
+            break;
+        }
         default: {
             RET_ERROR;
         }
     }
-    OBJ_SAVE(img_name, img);
+
 }
 
 void uvc_camera::impl::start(start_config conf) {

src/device/impl/uvc_camera_impl.h

@@ -22,6 +22,8 @@ using namespace uvc_camera_impl;
 
 struct uvc_camera::impl {
 
+    create_config conf;
+
     uvc_device_t *dev = nullptr;
     uvc_device_handle_t *dev_h = nullptr;
 

src/device/impl/uvc_camera_ui.cpp

@@ -4,6 +4,7 @@
 uvc_camera_ui::impl::impl(create_config conf) {
     img_name = conf.img_name;
     ctx = conf.ctx;
+    stream = conf.stream;
 }
 
 void uvc_camera_ui::impl::show_config() {
@@ -57,6 +58,7 @@ void uvc_camera_ui::impl::open_camera() {
     assert(dev_index < dev_list->size());
     auto conf = uvc_camera::create_config{
             .h_dev = dev_list->at(dev_index).h_dev,
+            .stream = stream, .ctx = ctx,
     };
     cam = uvc_camera::create(conf);
 

src/device/impl/uvc_camera_ui_impl.h

@@ -8,6 +8,7 @@ struct uvc_camera_ui::impl {
 
     obj_name_type img_name = invalid_obj_name;
     io_context *ctx = nullptr;
+    smart_cuda_stream *stream = nullptr;
 
     uvc_camera::dev_info_list_type dev_list;
     int dev_index = 0;

src/device/uvc_camera.h

@@ -1,6 +1,7 @@
 #ifndef DEPTHGUIDE_UVC_CAMERA_H
 #define DEPTHGUIDE_UVC_CAMERA_H
 
+#include "core/cuda_helper.hpp"
 #include "core/object_manager.h"
 
 #include <memory>
@@ -20,7 +21,9 @@ public:
     static dev_info_list_type get_device_list();
 
     struct create_config {
-        void *h_dev; // uvc_device_t *
+        void *h_dev = nullptr; // uvc_device_t *
+        smart_cuda_stream *stream = nullptr;
+        io_ctx_type *ctx = nullptr;
     };
 
     using this_type = uvc_camera;
@@ -37,6 +40,7 @@ public:
 
         explicit operator std::string();
     };
+
     using stream_info_list_type =
             std::shared_ptr<std::vector<stream_info_type>>;
 

src/device/uvc_camera_ui.h

@@ -1,6 +1,7 @@
 #ifndef DEPTHGUIDE_UVC_CAMERA_UI_H
 #define DEPTHGUIDE_UVC_CAMERA_UI_H
 
+#include "core/cuda_helper.hpp"
 #include "core/object_manager.h"
 
 #include <boost/asio/io_context.hpp>
@@ -16,6 +17,7 @@ public:
     struct create_config {
         obj_name_type img_name = invalid_obj_name;
         io_context *ctx = nullptr;
+        smart_cuda_stream *stream = nullptr;
     };
 
     explicit uvc_camera_ui(create_config conf);

src/image_process/camera_calibrator.h

@@ -4,6 +4,7 @@
 #include "core/cuda_helper.hpp"
 #include "core/object_manager.h"
 #include "core/local_connection.h" // sophiar
+#include "image_process/camera_utility.hpp"
 
 #include <opencv2/core/types.hpp>
 
@@ -29,6 +30,10 @@ public:
         using io_context = boost::asio::io_context;
         io_context *ctx;
 
+        using cb_func_type =
+                std::function<void(camera_intrinsic_v0)>;
+        cb_func_type cb_func;
+
         // for sophiar
         sophiar_conn_type *sophiar_conn = nullptr;
         std::string ref_transform_var;

src/image_process/camera_utility.hpp

@@ -5,8 +5,38 @@
 
 #include <Eigen/Geometry>
 
+#include <boost/container/static_vector.hpp>
+
 #include <cstdint>
 
+struct camera_intrinsic_v0 {
+    static constexpr auto max_dist_coeffs_num = 14;
+    using dist_coeffs_type = // in OpenCV order
+            boost::container::static_vector<float, max_dist_coeffs_num>;
+
+    float fx, fy; // focus length in pixel
+    float cx, cy; // optical center in pixel
+    dist_coeffs_type dist; // distort coefficients
+    uint32_t width, height;
+
+    cv::Mat intrinsic_cv_mat() const {
+        // @formatter:off
+        cv::Mat ret = (cv::Mat_<float>(3, 3) <<
+                fx,  0.f, cx,
+                0.f, fy,  cy,
+                0.f, 0.f, 1.f);
+        // @formatter:on
+        return ret;
+    }
+
+    cv::Mat dist_cv_mat() const {
+        auto vec = std::vector<float>();
+        std::copy(dist.begin(), dist.end(), std::back_inserter(vec));
+        cv::Mat ret = cv::Mat_(vec, true); // copy data
+        return ret;
+    }
+};
+
 struct camera_intrinsic {
     float fx, fy; // focus length in pixel
     float cx, cy; // optical center in pixel
@@ -26,6 +56,15 @@ struct camera_intrinsic {
 
     FROM_YAML_IMPL(camera_intrinsic)
 
+    camera_intrinsic_v0 to_v0() {
+        auto ret = camera_intrinsic_v0{
+                .fx = fx, .fy = fy, .cx = cx, .cy=cy,
+                .dist = {k[0], k[1]},
+                .width =width, .height =height,
+        };
+        return ret;
+    }
+
 };
 
 struct camera_intrinsic_v2 { // for Orbbec Camera
@@ -33,6 +72,15 @@ struct camera_intrinsic_v2 { // for Orbbec Camera
     float cx, cy; // optical center in pixel
     float k[6], p[2]; // distort coefficients
     uint32_t width, height;
+
+    camera_intrinsic_v0 to_v0() {
+        auto ret = camera_intrinsic_v0{
+                .fx = fx, .fy = fy, .cx = cx, .cy=cy,
+                .dist = {k[0], k[1], p[0], p[1], k[2], k[3], k[4], k[5]},
+                .width =width, .height =height,
+        };
+        return ret;
+    }
 };
 
 inline Eigen::Isometry3f transform_from_yaml_f(const YAML::Node &conf) {

src/image_process/cuda_impl/pixel_convert.cu

@@ -73,6 +73,51 @@ namespace rgb_to_yuv {
 
 }
 
+void call_yuv_to_rgb(image_type_v2<uchar3> in,
+                     image_type_v2<uchar3> out,
+                     cudaStream_t stream) {
+    auto func_type =
+            call_image_element_wise_unary<uchar3, uchar3, yuv_to_rgb::cvt>;
+    func_type(in, out, stream);
+}
+
+struct uchar6 {
+    union {
+        struct {
+            uchar3 a3, b3;
+        };
+        struct {
+            uchar3 u3[2];
+        };
+    };
+};
+static_assert(sizeof(uchar6) == 2 * sizeof(uchar3));
+
+namespace yuyv_to_rgb {
+
+    using cvt_type = yuv_to_rgb::cvt;
+
+    struct cvt {
+        __device__ static constexpr uchar6 Op(uchar4 in) { // Y0 Cb Y1 Cr
+            auto ret = uchar6();
+            ret.a3 = cvt_type::Op(uchar3(in.x, in.y, in.w));
+            ret.b3 = cvt_type::Op(uchar3(in.z, in.y, in.w));
+            return ret;
+        }
+    };
+
+}
+
+void call_yuyv_to_rgb(image_type_v2<uchar2> in,
+                      image_type_v2<uchar3> out,
+                      cudaStream_t stream) {
+    auto func_type =
+            call_image_element_wise_unary<uchar4, uchar6, yuyv_to_rgb::cvt>;
+    auto in_alt = in.cast<uchar4>();
+    auto out_alt = out.cast<uchar6>();
+    func_type(in_alt, out_alt, stream);
+}
+
 __global__ void nv12_to_rgb(image_type_v2<uchar1> luma_img, image_type_v2<uchar2> chroma_img,
                             image_type_v2<uchar3> rgb_img) {
 
@@ -256,4 +301,31 @@ void call_yuv_to_nv12(image_type_v2<uchar3> in,
     auto [luma_img, chroma_img] = split_chroma_luma(out);
     auto [grid_dim, block_dim] = get_kernel_size(chroma_img.width, chroma_img.height);
     yuv_to_nv12<<<grid_dim, block_dim, 0, stream>>>(in, luma_img, chroma_img);
+}
+
+struct cvt_np_tex {
+
+    // nx =  (tx - 0.5) * aspect * 2 * tan(fov / 2)
+    // ny = -(ty - 0.5) * 2 * tan(fov / 2)
+
+    // x_coeff = 1 / (2 * tan(fov / 2)) / aspect
+    // y_coeff = -1 / (2 * tan(fov / 2))
+    __device__ static constexpr float2 Op(float2 in,
+                                          float x_coeff, float y_coeff) {
+        auto tx = in.x * x_coeff + 0.5f;
+        auto ty = in.y * y_coeff + 0.5f;
+        return float2(tx, ty);
+    }
+};
+
+void call_np_to_tex(image_type_v2<float2> in,
+                    image_type_v2<float2> out,
+                    float fov, float aspect,
+                    cudaStream_t stream) {
+    auto func_type =
+            call_image_element_wise_unary<
+                    float2, float2, cvt_np_tex, float, float>;
+    auto y_coeff = -0.5f / std::tan(0.5f * fov);
+    auto x_coeff = -y_coeff / aspect;
+    func_type(in, out, stream, x_coeff, y_coeff);
 }

src/image_process/cuda_impl/pixel_convert.cuh

@@ -7,6 +7,15 @@ void call_cvt_rgb_bgra_u8(image_type_v2<uchar3> in,
                           image_type_v2<uchar4> out,
                           cudaStream_t stream);
 
+void call_yuv_to_rgb(image_type_v2<uchar3> in,
+                     image_type_v2<uchar3> out,
+                     cudaStream_t stream);
+
+// YUV 4:2:2
+void call_yuyv_to_rgb(image_type_v2<uchar2> in,
+                      image_type_v2<uchar3> out,
+                      cudaStream_t stream);
+
 void call_nv12_to_rgb(image_type_v2<uchar1> in,
                       image_type_v2<uchar3> out,
                       cudaStream_t stream);
@@ -23,4 +32,12 @@ void call_yuv_to_nv12(image_type_v2<uchar3> in,
                       image_type_v2<uchar1> out,
                       cudaStream_t stream);
 
+// convert coordinate in normalized plane to texture plane
+// fov: in radius
+// aspect: width / height
+void call_np_to_tex(image_type_v2<float2> in,
+                    image_type_v2<float2> out,
+                    float fov, float aspect,
+                    cudaStream_t stream);
+
 #endif //DEPTHGUIDE_PIXEL_CONVERT_CUH

src/image_process/impl/camera_calibrator.cpp

@@ -293,17 +293,14 @@ namespace camera_calibrator_impl {
                 // rotate corners if needed
                 auto rot_mat = rodrigues_to_mat(chess_r);
                 auto rot_quat = glm::quat_cast(rot_mat);
-                break; // TODO: test the commented code
-
-//                if (!last_rot_quat.has_value()
-//                    // https://math.stackexchange.com/questions/90081/quaternion-distance
-//                    || 1 - glm::dot(rot_quat, *last_rot_quat) < quat_threshold) {
-//                    last_rot_quat = rot_quat;
-//                    break;
-//                } else {
-//                    std::ranges::reverse(corner);
-//                    SPDLOG_DEBUG("Flipped.");
-//                }
+                if (!last_rot_quat.has_value()
+                    // https://math.stackexchange.com/questions/90081/quaternion-distance
+                    || 1 - glm::dot(rot_quat, *last_rot_quat) < quat_threshold) {
+                    last_rot_quat = rot_quat;
+                    break;
+                } else {
+                    std::ranges::reverse(corner);
+                }
             }
 
             auto ret_mat = to_transform_mat(to_vec3(chess_t),
@@ -341,7 +338,7 @@ namespace camera_calibrator_impl {
         result_ts_offset = (offset_l + offset_r) / 2;
 
         ts_offset = result_ts_offset;
-        result_mat = calib_hand_eye();
+        result_mat = glm::inverse(calib_hand_eye());
         SPDLOG_INFO("Estimated temporal offset is {}ms", result_ts_offset / 1000);
 
         auto err_val = evaluate_hand_eye(result_mat);
@@ -351,6 +348,18 @@ namespace camera_calibrator_impl {
         SPDLOG_INFO("Average 2D projection error is {:.2f}pix", img_reproj_err);
     }
 
+    camera_intrinsic_v0 camera_calibrator_impl::hand_eye_calib::intrinsic_v0() {
+        auto ret = camera_intrinsic_v0();
+        ret.fx = intrinsic_mat[0][0];
+        ret.fy = intrinsic_mat[1][1];
+        ret.cx = intrinsic_mat[2][0];
+        ret.cy = intrinsic_mat[2][1];
+        std::ranges::copy(dist_coeffs, std::back_inserter(ret.dist));
+        ret.width = img_size.width;
+        ret.height = img_size.height;
+        return ret;
+    }
+
 }
 
 camera_calibrator::impl::impl(const create_config &_conf) {
@@ -361,7 +370,7 @@ camera_calibrator::impl::impl(const create_config &_conf) {
 camera_calibrator::impl::~impl() {
     if (tp != nullptr) {
         tp->purge();
-        stop();
+        stop(true); // on_exit = true, force exit
     }
 }
 
@@ -386,7 +395,9 @@ void camera_calibrator::impl::process() {
     auto valid_imgs =
             img_pool | std::views::filter([](const auto &item) {
                 assert(item.process_finished);
-                return item.corners_detected;
+                if (!item.corners_detected) return false;
+                if (item.corner_sharpness > sharpness_threshold) return false;
+                return true;
             }) | std::views::transform([](const auto &item) {
                 assert(!item.corners.empty());
                 return hand_eye_calib::image_store_type(
@@ -407,6 +418,7 @@ void camera_calibrator::impl::process() {
         conf.sophiar_conn->update_transform_variable(
                 conf.result_transform_var, ret);
     }
+    conf.cb_func(calib->intrinsic_v0());
 }
 
 void camera_calibrator::impl::simulate_process(const simulate_info_type &info) {
@@ -415,12 +427,14 @@ void camera_calibrator::impl::simulate_process(const simulate_info_type &info) {
     process();
 }
 
-void camera_calibrator::impl::stop() {
+void camera_calibrator::impl::stop(bool on_exit) {
     assert(img_conn.connected());
     img_conn.disconnect();
     tp = nullptr; // implicit wait()
-    save_track_data();
-    process();
+    if (!on_exit) {
+        save_track_data();
+        process();
+    }
 }
 
 void camera_calibrator::impl::save_track_data() {
@@ -493,20 +507,6 @@ void camera_calibrator::impl::load_track_data(const std::string &path) {
         item.corners_detected = true;
         item.process_finished = true;
     }
-}
-
-void camera_calibrator::impl::load_fake_data() {
-    auto dist_coeffs = cv::Mat::zeros(1, 8, CV_64FC1);
-    // @formatter:off
-    cv::Mat intrinsic_mat = (cv::Mat_<double>(3, 3) <<
-            1000.f, 0.f, 500.f,
-            0.f, 1000.f, 500.f,
-            0.f, 0.f, 1.f);
-    // @formatter:on
-
-
-
-
 }
 
 void camera_calibrator::impl::per_image_process(img_store_type *st) {
@@ -530,7 +530,7 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
     bool ok = cv::findChessboardCorners(img_gray, conf.pattern_size,
                                         img_corners, corner_flag);
     if (!ok) {
-        last_finish.store(nullptr, std::memory_order::relaxed);
+//        last_finish.store(nullptr, std::memory_order::relaxed);
         return;
     }
     st->corners_detected = true;
@@ -540,7 +540,14 @@ void camera_calibrator::impl::per_image_process(img_store_type *st) {
     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);
-    st->corners = std::move(img_corners);
+    st->corners = img_corners;
+
+    // evaluate sharpness
+    auto sharpness = cv::estimateChessboardSharpness(
+            img_gray, conf.pattern_size, img_corners);
+    st->corner_sharpness = sharpness[0];
+    if (st->corner_sharpness > sharpness_threshold) return;
+
     ++img_ok_cnt;
 }
 
@@ -627,6 +634,16 @@ 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());
+        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();
+        }
     }
 }
 

src/image_process/impl/camera_calibrator_impl.h

@@ -16,6 +16,9 @@
 
 namespace camera_calibrator_impl {
 
+    // TODO: make configurable
+    static constexpr auto sharpness_threshold = 6.f;
+
     using corner_type = std::vector<cv::Point2f>;
 
     cv::Mat to_cv_mat(const glm::mat3 &mat);
@@ -72,6 +75,8 @@ namespace camera_calibrator_impl {
 
         void calc();
 
+        camera_intrinsic_v0 intrinsic_v0();
+
     private:
 
         // interpolate track matrix
@@ -90,6 +95,7 @@ namespace camera_calibrator_impl {
                                      const corner_type &c2);
 
         // do hand-eye calibration
+        // return reference in camera
         glm::mat4 calib_hand_eye();
 
         // evaluate hand-eye calibration result
@@ -119,6 +125,7 @@ struct camera_calibrator::impl {
         cv::Mat img_mat;
         timestamp_type sample_ts;
         corner_type corners;
+        float corner_sharpness;
 
         bool process_finished: 1 = false;
         bool corners_detected: 1 = false;
@@ -156,16 +163,13 @@ struct camera_calibrator::impl {
 
     void load_track_data(const std::string &path);
 
-    // generate sample data for debug
-    void load_fake_data();
-
     void process(); // do calibration
 
     void simulate_process(const simulate_info_type &info);
 
     void start();
 
-    void stop();
+    void stop(bool on_exit = false);
 
     void render();
 

src/image_process/impl/process_funcs.cpp

@@ -4,6 +4,8 @@
 #include "image_process/cuda_impl/pc_generate.cuh"
 #include "image_process/cuda_impl/pc_filter.cuh"
 
+#include <opencv2/calib3d.hpp>
+
 namespace gen_pc_rgb_with_mask {
 
     void call(config conf) {
@@ -133,6 +135,109 @@ namespace concatenate_image {
 
 }
 
+namespace gen_undistort_remap_image {
+
+//    struct mask_helper {
+//
+//        mask_helper(const image_ptr &_mask, cv::Size _size,
+//                    smart_cuda_stream *_stream = nullptr) {
+//            mask = _mask;
+//            size = _size;
+//            stream = _stream;
+//
+//            if (mask != nullptr) {
+//                assert(mask->cv_type() == CV_8UC1);
+//                assert(mask->size() == size);
+//            }
+//        }
+//
+//        explicit mask_helper(cv::Size _size)
+//                : mask_helper(nullptr, _size) {
+//        }
+//
+//        size_t count() { // number of non-zero elements
+//            if (mask == nullptr) return size.area();
+//            return cv::countNonZero(mat());
+//        }
+//
+//        bool at(int row, int col) {
+//            if (mask == nullptr) return true;
+//            auto pix = mem().at(row, col);
+//            return (*(uint8_t *) pix) != 0;
+//        }
+//
+//    private:
+//        image_ptr mask;
+//        cv::Size size;
+//        smart_cuda_stream *stream = nullptr;
+//
+//        std::optional<image_memory> _mem = {};
+//        std::optional<cv::Mat> _mat = {};
+//
+//        cv::Mat mat() {
+//            assert(mask != nullptr);
+//            if (!_mat.has_value()) {
+//                _mat = mask->cv_mat(stream);
+//            }
+//            assert(_mat.has_value());
+//            return *_mat;
+//        }
+//
+//        image_memory mem() {
+//            assert(mask != nullptr);
+//            if (!_mem.has_value()) {
+//                _mem = mask->memory(MEM_HOST, stream);
+//            }
+//            assert(_mem.has_value());
+//            return *_mem;
+//        }
+//
+//    };
+
+    void call_direct(const config_direct &conf) {
+        auto &info = conf.cam_info;
+        auto img_size = cv::Size(info.width, info.height);
+        auto pix_num = img_size.area();
+        auto pix_mat = cv::Mat(1, pix_num, CV_32FC2);
+        for (auto iy = 0; iy < info.height; ++iy)
+            for (auto ix = 0; ix < info.width; ++ix) {
+                auto id = iy * info.width + ix;
+                pix_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
+            }
+
+        auto ret = create_image(img_size, CV_32FC2);
+        auto ret_data = ret->memory(MEM_HOST).start_ptr();
+        auto ret_mat = cv::Mat(1, pix_num, CV_32FC2, ret_data);
+        cv::undistortPoints(pix_mat, ret_mat,
+                            info.intrinsic_cv_mat(), info.dist_cv_mat());
+        ret->host_modified();
+        assert(conf.img != nullptr);
+        *conf.img = ret;
+
+        // calculate bounding box
+        if (conf.bound != nullptr) {
+            auto cv_mat = ret->cv_mat(conf.stream);
+            auto cv_xy = std::vector<cv::Mat>();
+            cv::split(cv_mat, cv_xy);
+            assert(cv_xy.size() == 2);
+
+            double x_min, x_max, y_min, y_max;
+            if (conf.mask != nullptr) {
+                auto mask_mat = conf.mask->cv_mat(conf.stream);
+                cv::minMaxLoc(cv_xy[0], &x_min, &x_max, nullptr, nullptr, mask_mat);
+                cv::minMaxLoc(cv_xy[1], &y_min, &y_max, nullptr, nullptr, mask_mat);
+            } else {
+                cv::minMaxLoc(cv_xy[0], &x_min, &x_max);
+                cv::minMaxLoc(cv_xy[1], &y_min, &y_max);
+            }
+            auto ret_rect = cv::Rect2f(x_min, y_min,
+                                       x_max - x_min, y_max - y_min);
+            *conf.bound = ret_rect;
+        }
+    }
+
+}
+
 namespace shrink_pc_zero_depth {
 
     template<typename PointT>

src/image_process/process_funcs.h

@@ -5,6 +5,7 @@
 #include "core/image_utility_v2.h"
 #include "core/pc_utility.h"
 #include "core/object_manager.h"
+#include "image_process/camera_utility.hpp"
 
 namespace gen_pc_rgb_with_mask {
 
@@ -67,6 +68,22 @@ namespace concatenate_image {
 
 }
 
+// I[iy, ix] -> (nx, ny)
+// undistorted coordinate in the normalized plane
+namespace gen_undistort_remap_image {
+
+    struct config_direct {
+        camera_intrinsic_v0 cam_info;
+        image_ptr mask; // CV_8UC1
+        image_ptr *img; // CV_32FC2
+        cv::Rect2f *bound = nullptr; // bounding box of the return image
+        smart_cuda_stream *stream = nullptr;
+    };
+
+    void call_direct(const config_direct &conf);
+
+}
+
 namespace shrink_pc_zero_depth {
 
     struct config {

src/impl/apps/debug/app_debug.cpp

@@ -61,7 +61,7 @@ app_debug::app_debug(const create_config &conf) {
     auto calib = std::make_unique<camera_calibrator>(calib_conf);
 
     auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_2.txt",
+            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data.txt",
             .img_size = cv::Size(1920, 1080),
     };
     calib->simulate_process(sim_info);

src/impl/apps/endo_guide/endo_guide.cpp

@@ -18,7 +18,9 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
 
     // initialize modules
     auto uvc_cam_conf = uvc_camera_ui::create_config{
-            .img_name = img_color, .ctx = main_conf.asio_ctx,
+            .img_name = img_color,
+            .ctx = main_conf.asio_ctx,
+            .stream = default_cuda_stream,
     };
     uvc_cam = std::make_unique<uvc_camera_ui>(uvc_cam_conf);
 
@@ -40,17 +42,31 @@ 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);
+            },
             .sophiar_conn = sophiar_conn.get(),
             .ref_transform_var = LOAD_STR("camera_ref_transform_var"),
             .result_transform_var = LOAD_STR("camera_calib_transform_var"),
     };
     cam_calib = std::make_unique<camera_calibrator>(calib_conf);
 
+    auto aug_list_v1 = augment_manager::item_list_from_yaml(LOAD_LIST("augment_list"));
+    auto aug_list = augment_manager_v2::item_list_from_v1(aug_list_v1, extra_name);
+    auto aug_conf = augment_manager_v2::create_config{
+            .sophiar_conn = sophiar_conn.get(),
+            .stream = default_cuda_stream,
+    };
+    std::copy(aug_list.begin(), aug_list.end(), std::back_inserter(aug_conf.item_list));
+    aug_manager = std::make_unique<augment_manager_v2>(aug_conf);
+
     auto helper_conf = image_augment_helper_v2::create_config{
-            .img_name = img_color, .remap_name = invalid_obj_name,
+            .img_name = img_color, .mask_name = invalid_obj_name,
             .out_name = img_out, .img_flip_y = false,
-            .manager = nullptr, .stream = default_cuda_stream,
-            .ctx = main_conf.asio_ctx, .sophiar_conn = nullptr,
+            .manager = aug_manager.get(), .stream = default_cuda_stream,
+            .ctx = main_conf.asio_ctx,
+            .sophiar_conn = sophiar_conn.get(),
+            .transform_var = LOAD_STR("camera_transform_var"),
     };
     aug_helper = std::make_unique<image_augment_helper_v2>(helper_conf);
     aug_helper->post_render_sig.connect([this] { cam_calib->render(); });
@@ -60,6 +76,12 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
             .sophiar_conn = sophiar_conn.get(),
     };
     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);
 }
 
 void app_endo_guide::show_ui() {
@@ -76,6 +98,11 @@ void app_endo_guide::show_ui() {
                 start_tracking();
             }
             monitor->show();
+            {
+                ImGui::SeparatorText("Scene");
+                auto id_guard = imgui_id_guard("augment_scene");
+                aug_manager->show();
+            }
         }
 
         if (ImGui::CollapsingHeader("Calibration")) {

src/impl/apps/endo_guide/endo_guide.h

@@ -3,6 +3,7 @@
 
 #include "core/object_manager.h"
 #include "device/uvc_camera_ui.h"
+#include "module/augment_manager_v2.h"
 #include "module/image_augment_helper_v2.h"
 #include "module/image_saver.h"
 #include "module/image_viewer.h"
@@ -33,10 +34,13 @@ private:
         img_color,
 
         // image output
-        img_out
+        img_out,
+
+        name_end,
     };
 
     create_config main_conf;
+    obj_name_type extra_name = name_end;
 
     // sophiar
     std::unique_ptr<sophiar_conn_type> sophiar_conn;
@@ -46,6 +50,7 @@ private:
     // modules
     std::unique_ptr<uvc_camera_ui> uvc_cam;
     std::unique_ptr<camera_calibrator> cam_calib;
+    std::unique_ptr<augment_manager_v2> aug_manager;
     std::unique_ptr<image_augment_helper_v2> aug_helper;
     std::unique_ptr<image_viewer> bg_viewer;
     std::unique_ptr<image_saver> out_saver;

src/module/image_augment_helper_v2.h

@@ -3,6 +3,7 @@
 
 #include "core/object_manager.h"
 #include "module/augment_manager_v2.h"
+#include "image_process/camera_utility.hpp"
 
 #include <memory>
 
@@ -11,7 +12,7 @@ public:
 
     struct create_config {
         obj_name_type img_name;
-        obj_name_type remap_name; // CV_32FC2, pixel -> undistorted normalized plane
+        obj_name_type mask_name; // image_ptr, CV_8UC1, mask of augment area
         obj_name_type out_name; // image output, image_ptr, CV_8UC3
         bool img_flip_y = true;
         augment_manager_v2 *manager;
@@ -21,12 +22,15 @@ public:
         using io_context = boost::asio::io_context;
         io_context *ctx = nullptr;
         sophiar_conn_type *sophiar_conn = nullptr;
+        std::string transform_var; // for sophiar
     };
 
     explicit image_augment_helper_v2(create_config conf);
 
     ~image_augment_helper_v2();
 
+    void set_camera_info(const camera_intrinsic_v0 &info);
+
     void render();
 
     using render_sig_type = boost::signals2::signal<void()>;

src/module/impl/camera_augment_helper_v2.cpp

@@ -180,8 +180,8 @@ void camera_augment_helper_v2::impl::update_camera() {
 }
 
 void camera_augment_helper_v2::impl::render() {
+    update_camera();
     if (!is_missing || ui->ignore_missing) {
-        update_camera();
         last_vp_size = query_viewport_size();
         manager->record_current_camera_helper(q_this);
         manager->render(last_camera);

src/module/impl/image_augment_helper_v2.cpp

@@ -1,4 +1,6 @@
 #include "image_augment_helper_v2_impl.h"
+#include "image_process/cuda_impl/pixel_convert.cuh"
+#include "image_process/process_funcs.h"
 #include "third_party/scope_guard.hpp"
 
 image_augment_helper_v2::impl::impl(create_config _conf) {
@@ -16,6 +18,23 @@ image_augment_helper_v2::impl::~impl() {
     img_conn.disconnect();
 }
 
+void image_augment_helper_v2::impl::render_augment() {
+    if (cam_helper == nullptr) return;
+    fbo_aug.create(fbo_aug_conf);
+    fbo_aug.bind();
+    cam_helper->render();
+    fbo_aug.unbind();
+
+    auto ren_info = tex_render_info{
+            .mode = TEX_COLOR_REMAP,
+    };
+    ren_info.color.fmt = COLOR_RGBA;
+    ren_info.color.id = fbo_aug.color_tex[0].id;
+    auto &extra = ren_info.extra.color_remap;
+    extra.remap_id = remap_tex.id;
+    render_texture(ren_info);
+}
+
 void image_augment_helper_v2::impl::img_callback(obj_name_type name) {
     assert(name == conf.img_name);
 
@@ -30,14 +49,51 @@ void image_augment_helper_v2::impl::img_callback(obj_name_type name) {
             .flip_y = conf.img_flip_y, .stream = conf.stream,
     };
     img_ren.render_v2(img, img_conf);
-
-    assert(conf.manager == nullptr); // not implemented
+    render_augment();
 
     q_this->post_render_sig();
     output_img = img_down->download_v2(COLOR_RGB);
     OBJ_SAVE(conf.out_name, output_img);
 }
 
+void image_augment_helper_v2::impl::set_camera_info(const camera_intrinsic_v0 &info) {
+    cv::Rect2f remap_bound;
+    image_ptr remap_img;
+    auto remap_conf = gen_undistort_remap_image::config_direct{
+            .cam_info = info, .mask = to_image(conf.mask_name),
+            .img = &remap_img, .bound = &remap_bound,
+            .stream = conf.stream,
+    };
+    gen_undistort_remap_image::call_direct(remap_conf);
+
+    auto x_min = remap_bound.x, x_max = x_min + remap_bound.width;
+    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);
+    auto cam_fov = 2.f * std::atan(y_len);
+
+    auto aspect = x_len / y_len;
+    auto aug_size = cv::Size(info.height * aspect, // width
+                             info.height);
+    fbo_aug_conf.size = aug_size;
+
+    // convert to texture coordinate
+    auto remap_cuda = remap_img->cuda<float2>(conf.stream);
+    call_np_to_tex(remap_cuda, remap_cuda,
+                   cam_fov, aspect, conf.stream->cuda);
+    remap_img->cuda_modified(conf.stream);
+    remap_tex.upload(remap_img, conf.stream);
+
+    auto cam_conf = camera_augment_helper_v2::create_config{
+            .camera = camera_augment_helper_v2::create_config::fixed_camera_config{
+                    .fov = glm::degrees(cam_fov), .transform_var = conf.transform_var,
+            },
+            .sophiar_conn = conf.sophiar_conn,
+            .manager = conf.manager, .ctx = conf.ctx
+    };
+    cam_helper = std::make_unique<camera_augment_helper_v2>(cam_conf);
+}
+
 void image_augment_helper_v2::impl::render() {
     if (output_img == nullptr) return;
     auto img_conf = color_image_render::config_type{
@@ -55,4 +111,8 @@ image_augment_helper_v2::~image_augment_helper_v2() = default;
 
 void image_augment_helper_v2::render() {
     pimpl->render();
+}
+
+void image_augment_helper_v2::set_camera_info(const camera_intrinsic_v0 &info) {
+    pimpl->set_camera_info(info);
 }

src/module/impl/image_augment_helper_v2_impl.h

@@ -2,6 +2,7 @@
 #define DEPTHGUIDE_IMAGE_AUGMENT_HELPER_V2_IMPL_H
 
 #include "module/image_augment_helper_v2.h"
+#include "module/camera_augment_helper_v2.h"
 #include "module/viewport_downloader.hpp"
 #include "render/render_tools.h"
 
@@ -11,7 +12,14 @@ struct image_augment_helper_v2::impl {
     create_config conf;
     obj_conn_type img_conn;
 
-    using fbo_conf_type = smart_frame_buffer::create_config;
+    using fbo_conf_type =
+            smart_frame_buffer::create_config;
+
+    smart_texture remap_tex;
+    std::unique_ptr<camera_augment_helper_v2> cam_helper;
+    fbo_conf_type fbo_aug_conf;
+    smart_frame_buffer fbo_aug;
+
     fbo_conf_type fbo_conf;
     smart_frame_buffer fbo;
 
@@ -27,8 +35,13 @@ struct image_augment_helper_v2::impl {
 
     void img_callback(obj_name_type name);
 
+    void set_camera_info(const camera_intrinsic_v0 &info);
+
     void render();
 
+    // render augment frame buffer
+    void render_augment();
+
 };
 
 #endif //DEPTHGUIDE_IMAGE_AUGMENT_HELPER_V2_IMPL_H

src/render/impl/render_scene.cpp

@@ -155,7 +155,7 @@ void render_scene(const scene_ptr &info) {
     auto t_conf = tex_render_info{
             .mode = TEX_COLOR_DEPTH
     };
-    t_conf.color.fmt = COLOR_RGB;
+    t_conf.color.fmt = COLOR_RGBA;
     t_conf.color.id = scene_fbo.color_tex[0].id;
     t_conf.depth.id = scene_fbo.depth_tex.id;
     t_conf.depth.enable_alpha_effect = false;

src/render/impl/render_texture.cpp

@@ -15,7 +15,9 @@ namespace render_texture_impl {
     using pg_type = std::unique_ptr<smart_program>;
     pg_type pg_bw;    // render black/white texture
     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_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
@@ -140,6 +142,33 @@ namespace render_texture_impl {
         draw();
     }
 
+    // render rgb texture with remap
+    void ren_rgb_remap(const tex_render_info &info) {
+        auto &pg = pg_rgb_remap;
+        if (pg == nullptr) {
+            pg = std::unique_ptr<smart_program>(
+                    smart_program::create("tex_rgb_remap",
+                                          {{GL_VERTEX_SHADER,   "tex.vert"},
+                                           {GL_FRAGMENT_SHADER, "tex_rgb_remap.frag"}}));
+        }
+        assert(pg != nullptr);
+        pg->use();
+
+        pg->set_uniform_f("alpha", info.color.alpha);
+
+        auto &extra = info.extra.color_remap;
+        glActiveTexture(GL_TEXTURE0 + 0);
+        glBindTexture(GL_TEXTURE_2D, extra.remap_id);
+        pg->set_uniform_i("r_tex", 0);
+        glActiveTexture(GL_TEXTURE0 + 1);
+        glBindTexture(GL_TEXTURE_2D, info.color.id);
+        pg->set_uniform_i("c_tex", 1);
+
+        glDisable(GL_DEPTH_TEST);
+        config_buffers(info);
+        draw();
+    }
+
     void ren_rgba_only(const tex_render_info &info) {
         auto &pg = pg_rgba;
         if (pg == nullptr) {
@@ -162,6 +191,32 @@ namespace render_texture_impl {
         draw();
     }
 
+    void ren_rgba_remap(const tex_render_info &info) {
+        auto &pg = pg_rgba;
+        if (pg == nullptr) {
+            pg = std::unique_ptr<smart_program>(
+                    smart_program::create("tex_rgba_remap",
+                                          {{GL_VERTEX_SHADER,   "tex.vert"},
+                                           {GL_FRAGMENT_SHADER, "tex_rgba_remap.frag"}}));
+        }
+        assert(pg != nullptr);
+        pg->use();
+
+        pg->set_uniform_f("opacity", info.color.opacity);
+
+        auto &extra = info.extra.color_remap;
+        glActiveTexture(GL_TEXTURE0 + 0);
+        glBindTexture(GL_TEXTURE_2D, extra.remap_id);
+        pg->set_uniform_i("r_tex", 0);
+        glActiveTexture(GL_TEXTURE0 + 1);
+        glBindTexture(GL_TEXTURE_2D, info.color.id);
+        pg->set_uniform_i("c_tex", 1);
+
+        glDisable(GL_DEPTH_TEST);
+        config_buffers(info);
+        draw();
+    }
+
     void ren_rgb_d(const tex_render_info &info) {
         auto &pg = pg_rgb_d;
         if (pg == nullptr) {
@@ -291,6 +346,22 @@ namespace render_texture_impl {
         }
     }
 
+    void ren_c_remap(const tex_render_info &info) {
+        switch (info.color.fmt) {
+            case COLOR_RGB: {
+                ren_rgb_remap(info);
+                break;
+            }
+            case COLOR_RGBA: {
+                ren_rgba_remap(info);
+                break;
+            }
+            default: {
+                RET_ERROR;
+            }
+        }
+    }
+
 }
 
 using namespace render_texture_impl;
@@ -309,6 +380,10 @@ void render_texture(const tex_render_info &info) {
             ren_c_d(info);
             break;
         }
+        case TEX_COLOR_REMAP: {
+            ren_c_remap(info);
+            break;
+        }
         default: {
             RET_ERROR;
         }

src/render/impl/render_texturer_impl.h

@@ -11,8 +11,12 @@ namespace render_texture_impl {
 
     void ren_rgb_only(const tex_render_info &info);
 
+    void ren_rgb_remap(const tex_render_info &info);
+
     void ren_rgba_only(const tex_render_info &info);
 
+    void ren_rgba_remap(const tex_render_info &info);
+
     void ren_rgb_d(const tex_render_info &info);
 
     void ren_rgb_d_v2(const tex_render_info &info);
@@ -23,6 +27,8 @@ namespace render_texture_impl {
 
     void ren_c_d(const tex_render_info &info);
 
+    void ren_c_remap(const tex_render_info &info);
+
 }
 
 #endif //DEPTHGUIDE_RENDER_TEXTURER_IMPL_H

src/render/impl/shader/tex_rgb_remap.frag

@@ -0,0 +1,16 @@
+#version 460
+
+uniform float alpha;
+
+uniform sampler2D r_tex; // remap texture
+uniform sampler2D c_tex; // color texture
+
+in vec2 frag_uv;
+
+layout (location = 0) out vec4 frag_color;
+
+void main() {
+    vec2 r_uv = texture(r_tex, frag_uv).xy; // remap coordinate
+    frag_color.rgb = texture(c_tex, r_uv).rgb;
+    frag_color.a = alpha;
+}

src/render/impl/shader/tex_rgba_remap.frag

@@ -0,0 +1,16 @@
+#version 460
+
+uniform float opacity;
+
+uniform sampler2D r_tex; // remap texture
+uniform sampler2D c_tex; // color texture
+
+in vec2 frag_uv;
+
+layout (location = 0) out vec4 frag_color;
+
+void main() {
+    vec2 r_uv = texture(r_tex, frag_uv).xy; // remap coordinate
+    frag_color = texture(c_tex, r_uv);
+    frag_color.a *= opacity;
+}

src/render/render_texture.h

@@ -9,6 +9,7 @@ enum tex_render_mode {
     TEX_COLOR_ONLY = 0x1,
     TEX_DEPTH_ONLY = 0x2,
     TEX_COLOR_DEPTH = TEX_COLOR_ONLY | TEX_DEPTH_ONLY,
+    TEX_COLOR_REMAP,
 };
 
 struct tex_render_info {
@@ -39,6 +40,9 @@ struct tex_render_info {
             glm::mat4 proj_mat;
             GLfloat alpha_factor;
         } color_depth;
+        struct {
+            GLuint remap_id; // remap texture
+        } color_remap;
     } extra = {};
 };
 

src/render/render_utility.h

@@ -92,8 +92,9 @@ constexpr inline GLenum get_tex_type() {
 constexpr inline GLenum get_tex_type(int type) {
     switch (CV_MAT_DEPTH(type)) {
         // @formatter:off
-        case CV_8U: { return GL_UNSIGNED_BYTE; }
-            // @formatter:on
+        case CV_8U:  { return GL_UNSIGNED_BYTE; }
+        case CV_32F: { return GL_FLOAT; }
+        // @formatter:on
         default: {
             RET_ERROR_E;
         }
@@ -118,6 +119,7 @@ constexpr inline GLenum get_tex_internal_format(int type) { // OpenCV type
         case CV_8UC2: { return GL_RG8;   }
         case CV_8UC3: { return GL_RGB8;  }
         case CV_8UC4: { return GL_RGBA8; }
+        case CV_32FC2: { return GL_RG32F; }
         // @formatter:on
         default: {
             RET_ERROR_E;