Broken OSG Scene Render.

CMakeLists.txt

@@ -321,4 +321,6 @@ else ()
 endif ()
 add_subdirectory(${Sophiar2DIR}/src Sophiar2Lib)
 target_include_directories(${PROJECT_NAME} PRIVATE ${Sophiar2DIR}/src)
-target_link_libraries(${PROJECT_NAME} Sophiar2Lib)
+target_link_libraries(${PROJECT_NAME} Sophiar2Lib)
+add_compile_definitions(EIGEN_DONT_VECTORIZE)
+add_compile_definitions(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)

data/config_remote_ar_v2.yaml

@@ -9,6 +9,28 @@ sophiar_config: ./sophiar_config_endo_guide.json
 sophiar_start_name: tracker_all
 camera_ref_transform_var: camera_ref_in_tracker
 
+left_camera:
+  fx: 3569.14196423103
+  fy: 3568.02459571424
+  cx: 1229.37096752839
+  cy: 1026.81619824569
+  k0: -0.0681166229505913
+  k1: 0.0751335815980414
+  width: 2448
+  height: 2048
+
+right_camera:
+  fx: 3581.91081422450
+  fy: 3580.97918934816
+  cx: 1218.86459178897
+  cy: 1034.61633754506
+  k0: -0.0753067946100483
+  k1: 0.106748846894580
+  width: 2448
+  height: 2048
+
+left_camera_transform: left_camera_in_tracker_denoised
+right_camera_transform: right_camera_in_tracker_denoised
 scene_info:
   - name: BoneTransform
     transform: femur_in_tracker_denoised
@@ -16,4 +38,13 @@ scene_info:
       - name: Bone
         model: /home/tpx/ext/data/models/old_head/bone.stl
       - name: Nerve
-        model: /home/tpx/ext/data/models/old_head/nerve.stl
+        model: /home/tpx/ext/data/models/old_head/nerve.stl
+
+probe_model: /home/tpx/data/models/Probe.stl
+registration_list:
+  - name: Bone
+    model_file: /home/tpx/ext/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

data/sophiar_config_endo_guide.json

@@ -16,13 +16,13 @@
       "name": "femur_in_femur_ref",
       "type": "transform_obj",
       "value": [
-        405.60876310826154,
-        -11.723419024851964,
-        -64.43931776227325,
-        -0.25881623739773235,
-        0.5976871267586126,
-        -0.3985157377272106,
-        0.6457317249059794
+        233.96470027218044,
+        42.187788316685904,
+        306.58865752584546,
+        0.8190555328954703,
+        -0.26342165437712295,
+        0.24510986086006864,
+        -0.4468536921048027
       ]
     },
     {
@@ -181,9 +181,9 @@
             "name": "probe",
             "parent": "probe_ref",
             "transform": [
-              -0.23,
-              -13.98,
-              -119.65,
+              0,
+              0,
+              0,
               1,
               0,
               0,
@@ -229,26 +229,26 @@
             "name": "left_camera",
             "parent": "mvs_ref",
             "transform": [
-              33.2854,
-              -37.7917,
-              -24.4682,
-              0.6318,
-              0.3547,
-              0.4719,
-              -0.5023
+              1.69529294997093,
+              -40.8504461910221,
+              -38.4356249156025,
+              0.328217190586743,
+              0.623382258094322,
+              0.318037852337519,
+              -0.634444607965983
             ]
           },
           {
             "name": "right_camera",
             "parent": "mvs_ref",
             "transform": [
-              37.3992,
-              -56.7108,
-              -85.8847,
-              0.6304,
-              0.3605,
-              0.4738,
-              -0.4982
+              0.0941597771490770,
+              -41.0431819166041,
+              -105.682270133671,
+              0.329203554783163,
+              0.626106195513828,
+              0.316605284270741,
+              -0.631962930423275
             ]
           },
           {
@@ -351,7 +351,7 @@
         "com_port": "/dev/ttyUSB0",
         "tool_list": [
           {
-            "rom_path": "/home/tpx/data/roms/GlassProbe_4Ball_4.rom",
+            "rom_path": "/home/tpx/data/roms/Probe_Small_4Ball.rom",
             "serial_number": "3DD50000",
             "outputs": {
               "transform": "probe_ref_in_tracker"

src/image_process_v5/CMakeLists.txt

@@ -1,4 +1,5 @@
 target_sources(${PROJECT_NAME} PRIVATE
+        image_augment.cpp
         image_enhance.cpp
         image_viewer.cpp
         image_process.cpp

src/image_process_v5/cuda_impl/image_enhance.cu

@@ -4,12 +4,9 @@
 #include <thrust/transform.h>
 
 struct extract_lum_op {
-    float eps_factor;
-
     __device__
     float operator()(const uchar3 pix) const {
         const float lum = max(pix.x, max(pix.y, pix.z)) / 255.f;
-        // return logf(lum + eps_factor);
         return lum;
     }
 };
@@ -84,8 +81,8 @@ struct aces_enhance_op {
 
 namespace image_enhance_impl {
     float call_reduce_lum_mean(const uchar3 *in, float *buf, const uint32_t num,
-                               const float eps_factor, cudaStream_t stream) {
-        const auto op = extract_lum_op(eps_factor);
+                               cudaStream_t stream) {
+        constexpr auto op = extract_lum_op();
         thrust::transform(thrust::cuda::par.on(stream),
                           in, in + num, buf, op);
         const auto ret = thrust::reduce(thrust::cuda::par.on(stream),

src/image_process_v5/cuda_impl/image_enhance.cuh

@@ -5,7 +5,7 @@
 
 namespace image_enhance_impl {
     float call_reduce_lum_mean(const uchar3 *in, float *buf, uint32_t num,
-                               float eps_factor, cudaStream_t stream);
+                               cudaStream_t stream);
     void call_aces_enhance(const uchar3 *in, uchar3 *out, uint32_t num,
                            float lum_factor, cudaStream_t stream);
 }

src/image_process_v5/image_augment.cpp

@@ -0,0 +1,203 @@
+#include "image_augment.h"
+#include "image_process_v5/sp_image.h"
+#include "image_process_v5/image_process.h"
+#include "image_process_v5/osg_helper.h"
+#include "image_process/camera_utility.hpp"
+#include "core/imgui_utility.hpp"
+
+#include <opencv2/calib3d.hpp>
+
+#include <osgViewer/Viewer>
+#include <utility>
+
+using namespace osg;
+
+namespace {
+    sp_image create_remap_image(const camera_intrinsic_v0 &info) {
+        const auto img_size = cv::Size(info.width, info.height);
+        const 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) {
+                const auto id = iy * info.width + ix;
+                pix_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
+            }
+        auto ret = sp_image::create(CV_32FC2, img_size);
+        const auto write_helper = write_access_helper(ret.host());
+        auto ret_mat = cv::Mat(1, pix_num, CV_32FC2,
+                               ret.start_ptr(write_helper.ptr()));
+        cv::undistortPoints(pix_mat, ret_mat,
+                            info.intrinsic_cv_mat(), info.dist_cv_mat());
+        return ret;
+    }
+
+    cv::Rect2d calc_remap_range(const sp_image &remap) {
+        assert(CV_MAT_CN(remap.cv_type()) == 2);
+        const auto read_helper = read_access_helper(remap.host());
+        const auto cv_mat = remap.cv_mat(read_helper.ptr());
+        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;
+        cv::minMaxLoc(cv_xy[0], &x_min, &x_max);
+        cv::minMaxLoc(cv_xy[1], &y_min, &y_max);
+        return {
+            x_min, y_min,
+            x_max - x_min, y_max - y_min
+        };
+    }
+
+    constexpr auto max_camera_y_axis_len = 3.f;
+    constexpr auto max_camera_x_axis_len = 6.f;
+}
+
+struct image_augment::impl {
+    create_config conf;
+    obj_conn_type conn;
+
+    struct {
+        float fov = 60.f, aspect = 1.f;
+    } cam_conf;
+
+    struct _ui_conf {
+        float near = 10.f, far = 10000.f;
+    };
+
+    std::shared_ptr<_ui_conf> ui_conf;
+
+    ref_ptr<Camera> aug_cam;
+    ref_ptr<Texture2DSP> aug_tex; // Augmented scene will be rendered here
+    ref_ptr<ImageGeomSP> aug_img_osg;
+    timestamp_type last_update_ts = {};
+
+    ref_ptr<osgViewer::Viewer> viewer;
+    ref_ptr<ImageGeomSP> img_osg;
+    ref_ptr<Texture2DSP> out_tex;
+
+    void update_remap() {
+        const auto info = OBJ_QUERY(camera_intrinsic_v0, conf.cam_info_name);
+        const auto info_ts = OBJ_TS(conf.cam_info_name);
+        const auto out_size = cv::Size(info.width, info.height);
+        const auto remap_np = create_remap_image(info);
+        const cv::Rect2f remap_range = calc_remap_range(remap_np);
+
+        const auto x_min = remap_range.x, x_max = x_min + remap_range.width;
+        const auto y_min = remap_range.y, y_max = y_min + remap_range.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);
+        const auto cam_fov = 2.f * std::atan(y_len);
+
+        const auto aspect = x_len / y_len;
+        auto aug_size = cv::Size(info.height * aspect, // width
+                                 info.height);
+        aug_size *= 2; // double render size
+        const auto remap_tex = image_remap_np_to_tex(remap_np, cam_fov, aspect);
+
+        MAIN_DETACH([=, this] {
+            setupTextureHelper(aug_tex, aug_size);
+            aug_cam->setViewport(0, 0, aug_size.width, aug_size.height);
+            aug_cam->attach(Camera::COLOR_BUFFER0, aug_tex);
+            aug_img_osg->setImageTex(aug_tex);
+            aug_img_osg->setRemapImage(remap_tex);
+            cam_conf.fov = glm::degrees(cam_fov);
+            cam_conf.aspect = aspect;
+
+            setupTextureHelper(out_tex, out_size);
+            const auto out_cam = viewer->getCamera();
+            out_cam->setViewport(0, 0, out_size.width, out_size.height);
+            out_cam->attach(Camera::COLOR_BUFFER0, out_tex);
+
+            last_update_ts = info_ts;
+        });
+    }
+
+    void image_callback_impl() {
+        if (const auto info_ts = OBJ_TS(conf.cam_info_name);
+            info_ts > last_update_ts) {
+            TP_SUBMIT([this] { update_remap(); });
+        }
+        if (last_update_ts == 0) return;
+        const auto base_img = OBJ_QUERY(sp_image, conf.in_name);
+        img_osg->setImageSP(base_img);
+        aug_cam->setProjectionMatrixAsPerspective(
+            cam_conf.fov, cam_conf.aspect, ui_conf->near, ui_conf->far);
+        viewer->frame();
+        const auto img_out = out_tex->asImageSP();
+        OBJ_SAVE(conf.out_name, img_out);
+    }
+
+    void image_callback() {
+        try {
+            image_callback_impl();
+        } catch (...) {
+            const auto img = OBJ_QUERY(sp_image, conf.in_name);
+            OBJ_SAVE(conf.out_name, img);
+        }
+    }
+
+    void build_augment_scene() {
+        ref_ptr<TransformSP> cam_transform;
+        if (true) {
+            auto sub_conf = TransformSP::create_config();
+            sub_conf.name = conf.cam_name;
+            sub_conf.key_name = conf.cam_transform_name;
+            sub_conf.inverse = true;
+            cam_transform = new TransformSP(sub_conf);
+        }
+        cam_transform->addChild(conf.scene);
+
+        aug_cam = new Camera();
+        aug_cam->setRenderTargetImplementation(Camera::FRAME_BUFFER_OBJECT);
+        aug_cam->setRenderOrder(Camera::PRE_RENDER);
+        aug_cam->addChild(cam_transform);
+
+        aug_img_osg = new ImageGeomSP();
+        aug_img_osg->getOrCreateStateSet()->setRenderingHint(StateSet::TRANSPARENT_BIN);
+        img_osg = new ImageGeomSP();
+        img_osg->getOrCreateStateSet()->setRenderingHint(StateSet::OPAQUE_BIN);
+
+        const ref_ptr root = new Group();
+        root->addChild(img_osg);
+        root->addChild(aug_img_osg);
+
+        viewer = new osgViewer::Viewer();
+        viewer->setSceneData(root);
+        viewer->addSlave(aug_cam, false);
+        const auto out_cam = viewer->getCamera();
+        out_cam->setRenderTargetImplementation(Camera::FRAME_BUFFER_OBJECT);
+    }
+
+    explicit impl(create_config _conf) : conf(std::move(_conf)) {
+        build_augment_scene();
+        ui_conf = std::make_shared<_ui_conf>();
+        conn = OBJ_SIG(conf.in_name)->connect(
+            [this](auto _) { image_callback(); });
+    }
+
+    ~impl() {
+        conn.disconnect();
+    }
+
+    void show_ui() const {
+        ImGui::SliderFloat("Clip Near", &ui_conf->near, 1.0f, ui_conf->far,
+                           "%.f", ImGuiSliderFlags_Logarithmic);
+        ImGui::SliderFloat("Clip Far", &ui_conf->far, ui_conf->near, 10000.0f,
+                           "%.f", ImGuiSliderFlags_Logarithmic);
+    }
+};
+
+image_augment::image_augment(const create_config &conf)
+    : pimpl(std::make_unique<impl>(conf)) {
+}
+
+image_augment::~image_augment() = default;
+
+void image_augment::show_ui() const {
+    pimpl->show_ui();
+}
+
+void image_augment::sync_with(const image_augment *other) const {
+    pimpl->ui_conf = other->pimpl->ui_conf;
+}

src/image_process_v5/image_augment.h

@@ -0,0 +1,36 @@
+#ifndef IMAGE_AUGMENT_H
+#define IMAGE_AUGMENT_H
+
+#include <glad/gl.h> // make OpenGL happy.
+
+#include "core_v2/object_manager.h"
+#include "render_osg/osg_scene.h"
+
+#include <memory>
+
+class image_augment {
+public:
+    struct create_config {
+        obj_name_type in_name = 0, out_name = 0;
+        bool img_flip = true;
+        osg::ref_ptr<SceneSP> scene;
+        obj_name_type cam_info_name = 0; // camera_intrinsic_v0
+        std::string cam_name;
+        std::string cam_transform_name;
+    };
+
+    explicit image_augment(const create_config &conf);
+
+    ~image_augment();
+
+    void show_ui() const;
+
+    void sync_with(const image_augment *other) const;
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+
+#endif //IMAGE_AUGMENT_H

src/image_process_v5/image_enhance.cpp

@@ -13,7 +13,7 @@ sp_image image_enhance(sp_image img, const image_enhance_options opts) {
         const auto write_helper = write_access_helper(buf.cuda());
         lum_mean = impl::call_reduce_lum_mean(
             (uchar3 *) read_helper.ptr(), (float *) write_helper.ptr(),
-            img.elem_count(), opts.black_eps_factor, current_cuda_stream());
+            img.elem_count(), current_cuda_stream());
     }
 
     const auto lum_factor = opts.luminance_target / lum_mean;

src/image_process_v5/image_enhance.h

@@ -4,7 +4,6 @@
 #include "sp_image.h"
 
 struct image_enhance_options {
-    float [[deprecated]] black_eps_factor = 0.01;
     float luminance_target = 0.36;
 };
 

src/image_process_v5/image_process.cpp

@@ -273,6 +273,16 @@ sp_image image_yuyv_to_rgb(const sp_image &img) {
     return ret;
 }
 
+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 helper = image_cuda_v2_helper<float2, float2>(img, ret);
+    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) {

src/image_process_v5/image_process.h

@@ -30,6 +30,9 @@ 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);
 
+// normalized plane -> texture
+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_translate(const sp_image &img, glm::vec2 offset);

src/image_process_v5/image_viewer.cpp

@@ -30,8 +30,6 @@ struct image_viewer::impl {
         viewer->setSceneData(geode);
         viewer->setUpViewerAsEmbeddedInWindow(0, 0, 800, 600);
         const auto camera = viewer->getCamera();
-        camera->setViewMatrix(osg::Matrix::identity());
-        camera->setProjectionMatrix(osg::Matrix::identity());
         camera->setClearColor({0, 0, 0, 0});
 
         std::ranges::transform(

src/image_process_v5/osg_helper.cpp

@@ -77,15 +77,6 @@ void ogl_buffer_proxy::download(sp_image &img) {
     down_res->unmap();
 }
 
-void Texture2DSP::setImageSP(const sp_image &img) {
-    assert(getTextureWidth() == img.width());
-    assert(getTextureHeight() == img.height());
-    if (!pbo) { pbo.emplace(); }
-    pbo->upload(img);
-    setSourceFormat(get_tex_format(img.cv_type()));
-    setSourceType(get_tex_type(img.cv_type()));
-}
-
 namespace {
     auto get_tex_format_channels(const GLenum format) {
         switch (format) {
@@ -115,40 +106,86 @@ namespace {
     }
 }
 
-sp_image Texture2DSP::asImageSP(GLenum format, GLenum type) {
-    const auto channels = get_tex_format_channels(format);
-    const auto cv_depth = get_date_type_cv_depth(type);
-    const auto cv_type = CV_MAKE_TYPE(cv_depth, channels);
-    const auto img_size = cv::Size(getTextureWidth(), getTextureHeight());
-    auto img = sp_image::create(cv_type, img_size);
-
-    if (!pbo) [[unlikely]] { pbo.emplace(); }
-    if (pbo->modify_ts < draw_ts) {
-        pbo->create(img.byte_size());
-        glBindTexture(getTextureTarget(), getTextureObject(0)->id());
-        glBindBuffer(GL_PIXEL_PACK_BUFFER, pbo->id);
+struct Texture2DSP::impl {
+    Texture2DSP *q_this;
+    std::optional<ogl_buffer_proxy> pbo_down; // tex -> PBO
+    std::optional<ogl_buffer_proxy> pbo_up; // PBO -> tex
+    timestamp_type download_ts = {}; // tex -> PBO
+
+    explicit impl(Texture2DSP *_q_this)
+        : q_this(_q_this) { (void) 0; }
+
+    void set_image(const sp_image &img) {
+        assert(q_this->getTextureWidth() == img.width());
+        assert(q_this->getTextureHeight() == img.height());
+        if (!pbo_up) { pbo_up.emplace(); }
+        pbo_up->upload(img);
+        q_this->setSourceFormat(get_tex_format(img.cv_type()));
+        q_this->setSourceType(get_tex_type(img.cv_type()));
+    }
+
+    sp_image as_image(const GLenum format, const GLenum type) {
+        const auto channels = get_tex_format_channels(format);
+        const auto cv_depth = get_date_type_cv_depth(type);
+        const auto cv_type = CV_MAKE_TYPE(cv_depth, channels);
+        const auto img_size = cv::Size(q_this->getTextureWidth(), q_this->getTextureHeight());
+        auto img = sp_image::create(cv_type, img_size);
+
+        if (!pbo_down) [[unlikely]] { pbo_down.emplace(); }
+        pbo_down->create(img.byte_size());
+        const auto tex_target = q_this->getTextureTarget();
+        glBindTexture(tex_target, q_this->getTextureObject(0)->id());
+        glBindBuffer(GL_PIXEL_PACK_BUFFER, pbo_down->id);
         glReadPixels(0, 0, img.width(), img.height(), format, type, (void *) 0);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
-        glBindTexture(getTextureTarget(), 0);
-        pbo->modify_ts = current_timestamp();
+        glBindTexture(tex_target, 0);
+        pbo_down->modify_ts = current_timestamp();
+        return img;
     }
-    pbo->download(img);
-    return img;
+
+    void try_fill_from_pbo() {
+        if (!pbo_up) return;
+        if (download_ts < pbo_up->modify_ts) {
+            glBindBuffer(GL_PIXEL_UNPACK_BUFFER, pbo_up->id);
+            glTexSubImage2D(q_this->getTextureTarget(), 0, 0, 0,
+                            q_this->getTextureWidth(), q_this->getTextureHeight(),
+                            q_this->getSourceFormat(), q_this->getSourceType(), nullptr);
+            glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
+            download_ts = current_timestamp();
+        }
+    }
+};
+
+Texture2DSP::Texture2DSP()
+    : pimpl(std::make_unique<impl>(this)) {
+}
+
+Texture2DSP::~Texture2DSP() = default;
+
+void Texture2DSP::setImageSP(const sp_image &img) const {
+    pimpl->set_image(img);
+}
+
+sp_image Texture2DSP::asImageSP(const GLenum format, const GLenum type) const {
+    return pimpl->as_image(format, type);
 }
 
 void Texture2DSP::apply(osg::State &state) const {
     setNumMipmapLevels(1);
     Texture2D::apply(state);
+
     // texture has already been bind
+    pimpl->try_fill_from_pbo();
+}
 
-    if (!pbo) return;
-    if (download_ts < pbo->modify_ts) {
-        glBindBuffer(GL_PIXEL_UNPACK_BUFFER, pbo->id);
-        glTexSubImage2D(getTextureTarget(), 0, 0, 0,
-                        getTextureWidth(), getTextureHeight(),
-                        getSourceFormat(), getSourceType(), nullptr);
-        glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
-        download_ts = current_timestamp();
+void setupTextureHelper(osg::ref_ptr<Texture2DSP> &tex, const cv::Size size) {
+    if (tex == nullptr
+        || tex->getTextureWidth() != size.width
+        || tex->getTextureHeight() != size.height) [[unlikely]] {
+        const auto next_tex = new Texture2DSP();
+        next_tex->setTextureSize(size.width, size.height);
+        next_tex->setInternalFormat(GL_RGBA);
+        tex = next_tex;
     }
 }
 
@@ -206,9 +243,21 @@ namespace {
     std::optional<prog_image_default_type> prog_image_default;
     std::optional<prog_image_remap_type> prog_image_remap;
 
-    void setup_program(ImageGeomSP *img) {
+    void upload_image_to_texture(const sp_image &img, ref_ptr<Texture2DSP> &tex) {
+        setupTextureHelper(tex, img.cv_size());
+        tex->setImageSP(img);
+    }
+}
+
+struct ImageGeomSP::impl {
+    ImageGeomSP *q_this = nullptr;
+    ref_ptr<Texture2DSP> tex;
+    ref_ptr<Texture2DSP> remap_tex;
+    simple_rect last_rect = {};
+
+    void setup_program(ImageGeomSP *img) const {
         assert(img != nullptr);
-        if (img->remap_tex) {
+        if (remap_tex) {
             if (!prog_image_remap) [[unlikely]] {
                 prog_image_remap.emplace();
             }
@@ -221,94 +270,108 @@ namespace {
         }
     }
 
-    void upload_image_to_texture(const sp_image &img, ref_ptr<Texture2DSP> &tex) {
-        if (tex == nullptr
-            || tex->getTextureWidth() != img.width()
-            || tex->getTextureHeight() != img.height()) {
-            const auto next_tex = new Texture2DSP();
-            next_tex->setTextureSize(img.width(), img.height());
-            next_tex->setInternalFormat(GL_RGBA);
-            tex = next_tex;
+    explicit impl(ImageGeomSP *_q_this) : q_this(_q_this) {
+        const ref_ptr vertex = new Vec3Array(image_vertex_num);
+        vertex->setName("vertex");
+        q_this->setVertexArray(vertex);
+        set_viewport_range({-1, -1, 2, 2});
+
+        const ref_ptr tex_uv = new Vec2Array();
+        tex_uv->setName("vertex_uv");
+        tex_uv->push_back({0, 0});
+        tex_uv->push_back({1, 0});
+        tex_uv->push_back({1, 1});
+        tex_uv->push_back({0, 1});
+        q_this->setVertexAttribArray(1, tex_uv, Array::BIND_PER_VERTEX);
+
+        q_this->addPrimitiveSet(new DrawArrays(
+            PrimitiveSet::QUADS, 0, 4));
+        setup_program(q_this);
+
+        // enable depth test
+        q_this->getOrCreateStateSet()->setMode(
+            GL_DEPTH_TEST, StateAttribute::OFF);
+    }
+
+    void reset_vertex() const {
+        const auto vertex = (Vec3Array *) q_this->getVertexArray();
+        assert(vertex->size() == image_vertex_num);
+        auto [x, y, w, h] = last_rect;
+        vertex->at(0) = {x + 0, y + 0, 0};
+        vertex->at(1) = {x + w, y + 0, 0};
+        vertex->at(2) = {x + w, y + h, 0};
+        vertex->at(3) = {x + 0, y + h, 0};
+        vertex->dirty();
+        q_this->dirtyBound();
+        q_this->dirtyGLObjects();
+    }
+
+    void set_viewport_range(const simple_rect rect) {
+        if (rect == last_rect) [[likely]] return;
+        last_rect = rect;
+        reset_vertex();
+    }
+
+    void set_viewport_range(const float viewport_aspect, const bool flip_y) {
+        if (tex == nullptr) return;
+        simple_rect rect = {-1, -1, 2, 2};
+        auto img_aspect = 1.f * tex->getTextureWidth()
+                          / tex->getTextureHeight();
+        rect = rect.fit_aspect(img_aspect / viewport_aspect);
+        if (flip_y) {
+            rect.y *= -1;
+            rect.height *= -1;
         }
-        tex->setImageSP(img);
+        set_viewport_range(rect);
+    }
+
+    void set_image(const sp_image &img) {
+        upload_image_to_texture(img, tex);
+        q_this->getOrCreateStateSet()->setTextureAttributeAndModes(
+            0, tex, StateAttribute::ON);
+    }
+
+    void set_image_tex(Texture2DSP *_tex) const {
+        q_this->getOrCreateStateSet()->setTextureAttributeAndModes(
+            0, _tex, StateAttribute::ON);
     }
+
+    void set_remap_image(const sp_image &img) {
+        upload_image_to_texture(img, remap_tex);
+        if (remap_tex) {
+            q_this->getOrCreateStateSet()->setTextureAttributeAndModes(
+                1, remap_tex, StateAttribute::ON);
+        } else {
+            q_this->getOrCreateStateSet()->setTextureAttributeAndModes(
+                1, 0, StateAttribute::OFF);
+        }
+        setup_program(q_this);
+    }
+};
+
+ImageGeomSP::ImageGeomSP()
+    : pimpl(std::make_unique<impl>(this)) {
 }
 
-ImageGeomSP::ImageGeomSP() {
-    const ref_ptr vertex = new Vec3Array(image_vertex_num);
-    vertex->setName("vertex");
-    setVertexArray(vertex);
-    setViewportRange({-1, -1, 2, 2});
-
-    const ref_ptr tex_uv = new Vec2Array();
-    tex_uv->setName("vertex_uv");
-    tex_uv->push_back({0, 0});
-    tex_uv->push_back({1, 0});
-    tex_uv->push_back({1, 1});
-    tex_uv->push_back({0, 1});
-    setVertexAttribArray(1, tex_uv, Array::BIND_PER_VERTEX);
-
-    const ref_ptr colors = new Vec4Array();
-    colors->setName("color");
-    colors->push_back({1, 1, 1, 1});
-    setColorArray(colors, Array::BIND_OVERALL);
-
-    addPrimitiveSet(new DrawArrays(
-        PrimitiveSet::QUADS, 0, 4));
-    setup_program(this);
-
-    ArrayList array_list;
-    getArrayList(array_list);
-    array_list.clear();
+ImageGeomSP::~ImageGeomSP() = default;
+
+void ImageGeomSP::setViewportRange(const simple_rect rect) const {
+    pimpl->set_viewport_range(rect);
 }
 
-void ImageGeomSP::setViewportRange(simple_rect rect) {
-    if (rect == last_rect) [[likely]] return;
-    last_rect = rect;
-    const auto vertex = (Vec3Array *) getVertexArray();
-    assert(vertex->size() == image_vertex_num);
-    auto [x, y, w, h] = rect;
-    vertex->at(0) = {x + 0, y + 0, 0};
-    vertex->at(1) = {x + w, y + 0, 0};
-    vertex->at(2) = {x + w, y + h, 0};
-    vertex->at(3) = {x + 0, y + h, 0};
-    vertex->dirty();
-    dirtyBound();
-    dirtyGLObjects();
-
-    // disable depth test
-    getOrCreateStateSet()->setMode(GL_DEPTH_TEST,
-                                   StateAttribute::OVERRIDE | StateAttribute::OFF);
+void ImageGeomSP::setViewportRange(const float viewport_aspect,
+                                   const bool flip_y) const {
+    pimpl->set_viewport_range(viewport_aspect, flip_y);
 }
 
-void ImageGeomSP::setImageSP(const sp_image &img) {
-    upload_image_to_texture(img, tex);
-    getOrCreateStateSet()->setTextureAttributeAndModes(
-        0, tex, StateAttribute::ON);
+void ImageGeomSP::setImageSP(const sp_image &img) const {
+    pimpl->set_image(img);
 }
 
-void ImageGeomSP::setRemapImage(const sp_image &img) {
-    upload_image_to_texture(img, remap_tex);
-    if (remap_tex) {
-        getOrCreateStateSet()->setTextureAttributeAndModes(
-            1, remap_tex, StateAttribute::ON);
-    } else {
-        getOrCreateStateSet()->setTextureAttributeAndModes(
-            1, 0, StateAttribute::OFF);
-    }
-    setup_program(this);
+void ImageGeomSP::setImageTex(Texture2DSP *tex) const {
+    pimpl->set_image_tex(tex);
 }
 
-void ImageGeomSP::setViewportRange(const float viewport_aspect,
-                                   const bool flip_y) {
-    if (tex == nullptr) return;
-    simple_rect rect = {-1, -1, 2, 2};
-    auto img_aspect = 1.f * tex->getTextureWidth()
-                      / tex->getTextureHeight();
-    rect = rect.fit_aspect(img_aspect / viewport_aspect);
-    if (flip_y) {
-        rect.y *= -1;
-        rect.height *= -1;
-    }
-    setViewportRange(rect);
+void ImageGeomSP::setRemapImage(const sp_image &img) const {
+    pimpl->set_remap_image(img);
 }

src/image_process_v5/osg_helper.h

@@ -29,28 +29,36 @@ struct ogl_buffer_proxy : private boost::noncopyable {
 // width, height and internal format cannot be changed
 // call setTextureWidth(), setTextureHeight() and setInternalFormat() after create
 // use setSourceFormat() and setSourceType() to notify the source format and type in PBO
-struct Texture2DSP final : osg::Texture2D {
+class Texture2DSP final : public osg::Texture2D {
+public:
     //@formatter:off
-    std::optional<ogl_buffer_proxy> pbo;
-    mutable timestamp_type download_ts = {}; // PBO -> tex
-    timestamp_type draw_ts = {}; // OpenGL draw, modify in the outer
-    void setImageSP(const sp_image& img);
-    sp_image asImageSP(GLenum format = GL_RGB, GLenum type = GL_UNSIGNED_BYTE);
+    Texture2DSP();
+    ~Texture2DSP() override;
+    void setImageSP(const sp_image& img) const;
+    sp_image asImageSP(GLenum format = GL_RGB, GLenum type = GL_UNSIGNED_BYTE) const;
     void apply(osg::State &state) const override;
     //@formatter:on
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
 };
 
-struct ImageGeomSP final : osg::Geometry {
+void setupTextureHelper(osg::ref_ptr<Texture2DSP> &tex, cv::Size size);
+
+class ImageGeomSP final : public osg::Geometry {
+public:
     //@formatter:off
-    osg::ref_ptr<Texture2DSP> tex;
-    osg::ref_ptr<Texture2DSP> remap_tex;
-    simple_rect last_rect = {};
     ImageGeomSP();
-    void setImageSP(const sp_image& img);
-    void setRemapImage(const sp_image& img);
-    void setViewportRange(simple_rect rect);
-    void setViewportRange(float viewport_aspect, bool flip_y = false);
+    ~ImageGeomSP() override;
+    void setImageSP(const sp_image &img) const;
+    void setImageTex(Texture2DSP *tex) const;
+    void setRemapImage(const sp_image &img) const;
+    void setViewportRange(simple_rect rect) const;
+    void setViewportRange(float viewport_aspect, bool flip_y = false) const;
     //@formatter:on
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
 };
 
 #endif //OSG_HELPER_H

src/image_process_v5/sp_image.cpp

@@ -42,6 +42,7 @@ namespace {
         type_map[typeid(uchar4)] = {sizeof(uchar4), CV_8UC4};
         type_map[typeid(ushort1)] = {sizeof(ushort1), CV_16UC1};
         type_map[typeid(float1)] = {sizeof(float1), CV_32FC1};
+        type_map[typeid(float2)] = {sizeof(float2), CV_32FC2};
     }
 
     using cv_map_type = map_proxy<int, cv_info>;
@@ -54,6 +55,7 @@ namespace {
         cv_map[CV_8UC4] = cv_info::create<uchar4>();
         cv_map[CV_16UC1] = cv_info::create<ushort1>();
         cv_map[CV_32FC1] = cv_info::create<float1>();
+        cv_map[CV_32FC2] = cv_info::create<float2>();
     }
 
     auto to_index_pack(const cv::Size size) {

src/impl/apps/remote_ar/remote_ar_v2.cpp

@@ -1,4 +1,5 @@
 #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"
@@ -11,6 +12,8 @@
 
 #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)) {
     auto conf = main_conf.ext_conf;
@@ -63,8 +66,8 @@ app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
     if (true) {
         auto sub_conf = image_viewer_v2::create_config();
         sub_conf.items.emplace_back(uvc_img_id, "Endoscope", true);
-        sub_conf.items.emplace_back(left_img_stab_id, "Left", true);
-        sub_conf.items.emplace_back(right_img_id, "Right", 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);
     }
 
@@ -83,8 +86,45 @@ 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"));
+    start_sophiar(LOAD_STR("sophiar_config"),
+                  LOAD_STR("sophiar_start_name"));
+
+    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")) {
+        auto item_conf = registration_target::from_yaml(reg_item);
+        reg->add_target(item_conf);
+    }
+
+    aug_scene = SceneSP::createFromYAML(LOAD_LIST("scene_info"));
+
+    if (true) {
+        auto sub_conf = image_augment::create_config();
+        sub_conf.in_name = left_img_id;
+        sub_conf.out_name = left_aug_id;
+        sub_conf.scene = aug_scene;
+        sub_conf.cam_info_name = left_cam_info_id;
+        sub_conf.cam_name = "Stereo Camera";
+        sub_conf.cam_transform_name = LOAD_STR("left_camera_transform");
+        left_aug.emplace(sub_conf);
+    }
+
+    if (true) {
+        auto sub_conf = image_augment::create_config();
+        sub_conf.in_name = right_img_id;
+        sub_conf.out_name = right_aug_id;
+        sub_conf.scene = aug_scene;
+        sub_conf.cam_info_name = right_cam_info_id;
+        sub_conf.cam_transform_name = LOAD_STR("right_camera_transform");
+        right_aug.emplace(sub_conf);
+        right_aug->sync_with(&left_aug.value());
+    }
+
+    OBJ_SAVE(left_cam_info_id, camera_intrinsic::from_yaml(LOAD_SUB("left_camera")).to_v0());
+    OBJ_SAVE(right_cam_info_id, camera_intrinsic::from_yaml(LOAD_SUB("right_camera")).to_v0());
 
     // if (true) {
     //     auto sub_conf = ndi_stray_point_tracker::create_config();
@@ -137,6 +177,18 @@ void app_remote_ar_v2::show_ui() {
         //     endo_calib->show_ui();
         // }
 
+        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")) {
             auto id_guard = imgui_id_guard("streamer");
             streamer->show();
@@ -156,4 +208,9 @@ void app_remote_ar_v2::show_ui() {
         ImGui::PopItemWidth();
     }
     ImGui::End();
+
+    if (enable_reg) {
+        reg->process();
+        reg->show();
+    }
 }

src/impl/apps/remote_ar/remote_ar_v2.h

@@ -4,11 +4,13 @@
 #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/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"
 
@@ -34,7 +36,11 @@ private:
             output_img_id = 3,
             uvc_img_id = 4,
             left_img_fix_id = 5,
-            left_img_stab_id = 6;
+            left_img_stab_id = 6,
+            left_aug_id = 7,
+            right_aug_id = 8,
+            left_cam_info_id = 9,
+            right_cam_info_id = 10;
 
     std::optional<mvs_camera_ui> mvs_cam;
     std::optional<uvc_camera_ui> uvc_cam;
@@ -45,6 +51,13 @@ private:
     std::optional<image_streamer> streamer;
     std::optional<versatile_saver> saver;
     std::optional<oblique_calibrator> endo_calib;
+
+    bool enable_reg = false;
+    std::unique_ptr<registration> reg;
+
+    osg::ref_ptr<SceneSP> aug_scene;
+    std::optional<image_augment> left_aug;
+    std::optional<image_augment> right_aug;
 };
 
 

src/render_osg/osg_scene.cpp

@@ -1,10 +1,13 @@
 #include "osg_scene.h"
 #include "osg_utility.h"
+#include "core/imgui_utility.hpp"
+#include "core/math_helper.hpp"
+
+#include <glm/gtc/type_ptr.hpp>
 
 #include <spdlog/spdlog.h>
 
 #include <utility>
-#include <osgViewer/Scene>
 
 using namespace osg;
 
@@ -25,6 +28,10 @@ struct TransformSP::impl {
     bool is_tracked_once = false;
     bool ignore_missing = false;
 
+    // for cheating
+    glm::vec3 extra_offset = {};
+    glm::vec3 extra_rotation = {};
+
     struct update_callback final : NodeCallback {
         impl *pimpl;
 
@@ -51,12 +58,29 @@ struct TransformSP::impl {
             }
         } else {
             q_this->setNodeMask(-1); // enable
-            q_this->setMatrix(to_osg_matrix(*ret));
+            const auto extra_mat = to_transform_mat(extra_offset, extra_rotation);
+            auto mat = extra_mat * *ret;
+            if (conf.inverse) {
+                mat = glm::inverse(mat);
+            }
+            q_this->setMatrix(to_osg_matrix(mat));
         }
     }
 
     void show_ui() {
-        // TODO
+        if (ImGui::TreeNode(conf.name.c_str())) {
+            ImGui::Checkbox("Ignore Missing", &ignore_missing);
+            ImGui::DragFloat3("Offset (mm)", glm::value_ptr(extra_offset),
+                              0.05f, 0.0f, 0.0f, "%.02f");
+            ImGui::DragFloat3("Offset (deg)", glm::value_ptr(extra_rotation),
+                              0.1f, -180.0f, 180.0f, "%.01f");
+            for (auto i = 0; i < q_this->getNumChildren(); ++i) {
+                const auto child = dynamic_cast<osgHasUI *>(q_this->getChild(i));
+                if (child == nullptr) continue;
+                child->showUI();
+            }
+            ImGui::TreePop();
+        }
     }
 };
 
@@ -79,8 +103,6 @@ void TransformSP::showUI() {
 #include <vtkPolyDataNormals.h>
 #include <vtkPointData.h>
 
-#include <glm/gtc/type_ptr.hpp>
-
 namespace {
     struct prog_mesh_type {
         ref_ptr<Program> prog;
@@ -109,9 +131,6 @@ namespace {
             const ref_ptr base_weight = new Uniform(
                 Uniform::FLOAT, "base_weight");
             state->addUniform(base_weight);
-
-            // state->setMode(GL_BLEND, StateAttribute::ON); // TODO: modified
-            // state->setRenderingHint(StateSet::TRANSPARENT_BIN);
         }
     };
 
@@ -253,6 +272,28 @@ struct MeshSP::impl {
             assert(states != nullptr);
             const auto base_weight = states->getUniform("base_weight");
             base_weight->set(pimpl->base_weight);
+
+            const auto color = pimpl->conf.color;
+            const auto opacity = pimpl->conf.opacity;
+            const auto color_osg = Vec4(color.x, color.y, color.z, opacity);
+            for (auto i = 0; i < pimpl->q_this->getNumChildren(); i++) {
+                const auto child = pimpl->q_this->getChild(i)->asGeometry();
+                if (child == nullptr) continue;
+                if (child->getColorBinding() != Geometry::BIND_OVERALL) continue;
+                const auto colors = dynamic_cast<Vec4Array *>(child->getColorArray());
+                assert(colors != nullptr);
+                assert(colors->size() == 1);
+                if (colors->at(0) == color_osg) continue;
+                colors->at(0) = color_osg;
+                colors->dirty();
+                child->dirtyGLObjects();
+            }
+            if (opacity != 1.f) {
+                states->setRenderingHint(StateSet::TRANSPARENT_BIN);
+            } else {
+                states->setRenderingHint(StateSet::OPAQUE_BIN);
+            }
+
             pimpl->q_this->onUpdate();
             traverse(node, nv);
         }
@@ -281,7 +322,11 @@ struct MeshSP::impl {
     }
 
     void show_ui() {
-        // TODO
+        if (ImGui::TreeNode(conf.name.c_str())) {
+            ImGui::ColorEdit3("Color", glm::value_ptr(conf.color));
+            ImGui::DragFloat("Opacity", &conf.opacity, 0.005f, 0.0f, 1.0f);
+            ImGui::TreePop();
+        }
     }
 };
 
@@ -352,6 +397,14 @@ struct SceneSP::impl {
         state->addUniform(light_direction);
     }
 
+    void show_ui() const {
+        for (auto i = 0; i < q_this->getNumChildren(); ++i) {
+            const auto child = dynamic_cast<osgHasUI *>(q_this->getChild(i));
+            if (child == nullptr) continue;
+            child->showUI();
+        }
+    }
+
     explicit impl(SceneSP *_q_this)
         : q_this(_q_this) {
         setup_uniforms();
@@ -373,7 +426,7 @@ Camera *SceneSP::getCamera() const {
 }
 
 void SceneSP::showUI() {
-    // TODO
+    pimpl->show_ui();
 }
 
 namespace {

src/render_osg/osg_scene.h

@@ -21,6 +21,7 @@ public:
     struct create_config {
         std::string name;
         transform_key_type key_name;
+        bool inverse = false;
     };
 
     explicit TransformSP(const create_config &conf);
@@ -43,8 +44,8 @@ public:
         std::string name;
         std::string model_path;
         vtkSmartPointer<vtkPolyData> vtk_model = nullptr;
-        // glm::vec3 color;
-        // float transparency = 0; // opaque
+        glm::vec3 color = {1, 1, 1};
+        float opacity = 1.f;
     };
 
     MeshSP();
@@ -73,7 +74,7 @@ public:
 
     ~SceneSP() override;
 
-    static osg::ref_ptr<SceneSP> createFromYAML(const YAML::Node& conf);
+    static osg::ref_ptr<SceneSP> createFromYAML(const YAML::Node &conf);
 
     void setCamera(osg::Camera *camera) const;