Implemented VTK augmenting.

CMakeLists.txt

@@ -55,7 +55,7 @@ target_sources(${PROJECT_NAME} PRIVATE
 # spdlog config
 find_package(spdlog REQUIRED)
 target_link_libraries(${PROJECT_NAME} spdlog::spdlog)
-#target_compile_definitions(${PROJECT_NAME} PRIVATE SPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE)
+target_compile_definitions(${PROJECT_NAME} PRIVATE SPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE)
 
 # OpenCV config
 find_package(OpenCV REQUIRED COMPONENTS cudaimgproc imgcodecs)

src/augment_renderer.cpp

@@ -5,6 +5,13 @@
 
 #include <glad/gl.h>
 
+#include <vtkCamera.h>
+#include <vtkGenericOpenGLRenderWindow.h>
+#include <vtkOpenGLFramebufferObject.h>
+#include <vtkRenderer.h>
+#include <vtkSmartPointer.h>
+#include <vtkTextureObject.h>
+
 struct augment_renderer::impl {
     const cv::cuda::GpuMat *bg_img = nullptr;
     GLuint bg_tex = 0, bg_pbo = 0;
@@ -13,6 +20,13 @@ struct augment_renderer::impl {
     texture_renderer *tex_renderer = nullptr;
     const render_config *config = nullptr;
 
+    vtkSmartPointer<vtkCamera> vtk_cam;
+    vtkSmartPointer<vtkRenderer> vtk_render;
+    vtkSmartPointer<vtkGenericOpenGLRenderWindow> vtk_window;
+
+    const camera_intrinsics *cam_info = nullptr;
+    bool is_augment = true;
+
     ~impl() {
         cudaGraphicsUnregisterResource(bg_res);
     }
@@ -35,6 +49,19 @@ struct augment_renderer::impl {
         CUDA_API_CHECK(cudaGraphicsGLRegisterBuffer(&bg_res, bg_pbo,
                                                     cudaGraphicsRegisterFlagsWriteDiscard));
 
+        // setup vtk
+        vtk_render = vtkSmartPointer<vtkRenderer>::New();
+//        vtk_render->SetBackground(0.4, 0.4, 0.4);
+//        vtk_render->SetBackgroundAlpha(0.1);
+        vtk_window = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
+        vtk_window->InitializeFromCurrentContext();
+        vtk_window->SetSize(image_width, image_height);
+        vtk_window->SetAlphaBitPlanes(true);
+        vtk_window->SetOffScreenRendering(true);
+        vtk_window->AddRenderer(vtk_render);
+        vtk_cam = vtkSmartPointer<vtkCamera>::New();
+        vtk_render->SetActiveCamera(vtk_cam);
+
         return true;
     }
 
@@ -72,8 +99,40 @@ struct augment_renderer::impl {
         return true;
     }
 
+    void set_camera_pose(const Eigen::Isometry3d &cam_trans) {
+        auto trans_part = cam_trans.translation();
+        auto focal_point = trans_part + cam_trans.rotation().col(2) * cam_info->focal_length;
+        auto view_up = cam_trans.rotation().col(1);
+        vtk_cam->SetPosition(trans_part.x(), trans_part.y(), trans_part.z());
+        vtk_cam->SetFocalPoint(focal_point.x(), focal_point.y(), focal_point.z());
+        vtk_cam->SetViewUp(view_up.x(), view_up.y(), view_up.z());
+        vtk_cam->SetWindowCenter(cam_info->window_center.x, cam_info->window_center.y);
+        vtk_cam->SetViewAngle(cam_info->view_angle);
+        vtk_cam->SetClippingRange(cam_info->focal_length, 2000);
+        vtk_cam->Modified();
+        vtk_render->Modified();
+    }
+
+    void render_vtk() {
+        vtk_window->SetIsCurrent(true);
+        vtk_window->Render();
+        auto vtk_tex = vtk_window->GetDisplayFramebuffer()
+                ->GetColorAttachmentAsTextureObject(0)->GetHandle();
+        texture_renderer::render_config tex_config{
+                .tex = vtk_tex,
+                .x = config->x + config->width,
+                .y = config->y,
+                .width = -config->width,
+                .height = config->height
+        };
+        tex_renderer->render(&tex_config);
+    }
+
     bool render() {
         CALL_CHECK(render_background());
+        if (is_augment) {
+            render_vtk();
+        }
         return true;
     }
 
@@ -84,8 +143,10 @@ augment_renderer::augment_renderer()
 
 augment_renderer::~augment_renderer() = default;
 
-bool augment_renderer::initialize(texture_renderer *renderer) {
+bool augment_renderer::initialize(texture_renderer *renderer,
+                                  const camera_intrinsics *cam_info) {
     pimpl->tex_renderer = renderer;
+    pimpl->cam_info = cam_info;
     return pimpl->initialize();
 }
 
@@ -99,3 +160,16 @@ bool augment_renderer::render(const render_config &config) {
     pimpl->config = &config;
     return pimpl->render();
 }
+
+void augment_renderer::add_model(vtkActor *actor) {
+    assert(pimpl->vtk_render != nullptr);
+    pimpl->vtk_render->AddActor(actor);
+}
+
+void augment_renderer::set_camera_pose(const Eigen::Isometry3d &pose) {
+    pimpl->set_camera_pose(pose);
+}
+
+void augment_renderer::enable_augment(bool enable) {
+    pimpl->is_augment = enable;
+}

src/augment_renderer.h

@@ -5,7 +5,12 @@
 
 #include <opencv2/core/cuda.hpp>
 
+#include <Eigen/Geometry>
+
 #include <memory>
+#include <string_view>
+
+class vtkActor;
 
 class augment_renderer {
 public:
@@ -14,10 +19,28 @@ public:
 
     ~augment_renderer();
 
-    bool initialize(texture_renderer *renderer);
+    struct camera_intrinsics {
+        float focal_length;
+        float view_angle;
+        float pixel_size;
+        struct {
+            float x, y;
+        } window_center;
+        struct {
+            float k1, k2;
+        } distort;
+    };
+
+    bool initialize(texture_renderer *renderer, const camera_intrinsics *camera);
 
     void set_background(const cv::cuda::GpuMat *background);
 
+    void add_model(vtkActor *actor);
+
+    void set_camera_pose(const Eigen::Isometry3d &pose);
+
+    void enable_augment(bool enable);
+
     struct render_config {
         float x, y;
         float width, height;

src/main.cpp

@@ -12,6 +12,12 @@
 #include <imgui_impl_glfw.h>
 #include <imgui_impl_opengl3.h>
 
+#include <ExternalVTKWidget.h>
+#include <vtkActor.h>
+#include <vtkMatrix4x4.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkSTLReader.h>
+
 #include <glad/gl.h>
 #include <GLFW/glfw3.h>
 
@@ -22,6 +28,28 @@
 #include <cassert>
 #include <cstdlib>
 
+// for debug;
+#include <ndi/CombinedApi.h>
+
+// for debug;
+Eigen::Isometry3d port_info_to_transform(const Transform &info) {
+    using namespace Eigen;
+    auto translate = Translation3d(info.tx, info.ty, info.tz);
+    auto rotation = Quaterniond(info.q0, info.qx, info.qy, info.qz);
+    return translate * rotation;
+}
+
+// for debug;
+void transform_to_vtk_matrix(const Eigen::Isometry3d &trans, vtkMatrix4x4 *matrix) {
+    if (matrix == nullptr) return;
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            matrix->SetElement(i, j, trans(i, j));
+        }
+    }
+    matrix->Modified();
+}
+
 CUcontext cuda_ctx;
 
 int main() {
@@ -48,6 +76,10 @@ int main() {
     assert(version > 0);
     SPDLOG_INFO("Loaded OpenGL {}.{}", GLAD_VERSION_MAJOR(version), GLAD_VERSION_MINOR(version));
 
+    // enable color blending
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
 #ifndef NDEBUG
     // log opengl error
     glEnable(GL_DEBUG_OUTPUT);
@@ -73,6 +105,31 @@ int main() {
     cuInit(0);
     create_cuda_context(&cuda_ctx);
 
+    // for debug; setup NDI
+    auto ref_cam_trans_left = Eigen::Translation3d(0.1829, -38.3006, -34.2251) *
+                              Eigen::Quaterniond(0.3623, 0.6095, 0.3499, -0.6122);
+    auto ref_cam_trans_right = Eigen::Translation3d(1.0799, -39.7982, -94.8817) *
+                               Eigen::Quaterniond(0.3669, 0.6074, 0.3504, -0.6113);
+
+    CombinedApi capi;
+    ret = capi.connect("10.0.0.5");
+    assert(ret == 0);
+    ret = capi.initialize();
+    assert(ret == 0);
+    int cam_port, tool_port;
+    tool_port = capi.portHandleRequest();
+    capi.loadSromToPort("/home/tpx/data/roms/GlassProbe_4Ball_3ST.rom", tool_port);
+    cam_port = capi.portHandleRequest();
+    capi.loadSromToPort("/home/tpx/data/roms/Glass_4Ball_1.rom", cam_port);
+    auto portHandles = capi.portHandleSearchRequest(PortHandleSearchRequestOption::NotInit);
+    for (auto &handler: portHandles) {
+        ret = capi.portHandleInitialize(handler.getPortHandle());
+        assert(ret == 0);
+        ret = capi.portHandleEnable(handler.getPortHandle());
+        assert(ret == 0);
+    }
+    capi.startTracking();
+
     // working staffs
     stereo_camera camera;
     texture_renderer tex_renderer;
@@ -85,12 +142,53 @@ int main() {
     video_encoder encoder;
     encoder.initialize();
 
+    auto left_cam_info = augment_renderer::camera_intrinsics{
+            .focal_length= 8,
+            .view_angle = 47.6525,
+            .pixel_size = 0.00345,
+            .window_center = {
+                    .x = 1.0 * (1238.96436323518 - 1224) / 1224,
+                    .y =  -1.0 * (1011.68988239492 - 1024) / 1024
+            },
+            .distort = {
+                    .k1 = -0.0716117335673421,
+                    .k2 = 0.0899896700761875
+            }
+    };
+    auto right_cam_info = augment_renderer::camera_intrinsics{
+            .focal_length= 8,
+            .view_angle = 47.6525,
+            .pixel_size = 0.00345,
+            .window_center = {
+                    .x = 1.0 * (1238.97798867909 - 1224) / 1224,
+                    .y =  -1.0 * (1000.55305718163 - 1024) / 1024
+            },
+            .distort = {
+                    .k1 = -0.0716625513187931,
+                    .k2 = 0.0979453312950340
+            }
+    };
+
+    auto vtk_external = vtkSmartPointer<ExternalVTKWidget>::New();
+
     augment_renderer left_ar, right_ar;
-    left_ar.initialize(&tex_renderer);
-    right_ar.initialize(&tex_renderer);
+    left_ar.initialize(&tex_renderer, &left_cam_info);
+    right_ar.initialize(&tex_renderer, &right_cam_info);
     left_ar.set_background(&camera.left_rgb_image);
     right_ar.set_background(&camera.right_rgb_image);
 
+    // for debug; setup VTK scene
+    vtkNew<vtkSTLReader> reader;
+    reader->SetFileName("/home/tpx/data/stls/GlassProbe_4Ball_3.STL");
+    vtkNew<vtkPolyDataMapper> mapper;
+    mapper->SetInputConnection(reader->GetOutputPort());
+    vtkNew<vtkActor> actor;
+    vtkNew<vtkMatrix4x4> tool_matrix;
+    actor->SetUserMatrix(tool_matrix);
+    actor->SetMapper(mapper);
+    left_ar.add_model(actor);
+    right_ar.add_model(actor);
+
     int camera_fps = default_camera_fps;
     float exposure_time_ms = default_camera_exposure_time_ms;
     float analog_gain = default_camera_analog_gain;
@@ -112,14 +210,14 @@ int main() {
         ImGui_ImplGlfw_NewFrame();
         ImGui::NewFrame();
 
-        ImGui::ShowDemoWindow();
+        // extra actions to make consistency
+        if (!camera.is_capturing() && encoder.is_encoding()) {
+            encoder.stop_encode();
+        }
 
-        if (ImGui::Begin("Remote AR Control")) {
+        // ImGui::ShowDemoWindow();
 
-            // extra actions to make consistency
-            if (!camera.is_capturing() && encoder.is_encoding()) {
-                encoder.stop_encode();
-            }
+        if (ImGui::Begin("Remote AR Control")) {
 
             // camera control
             if (ImGui::CollapsingHeader("Camera")) {
@@ -194,6 +292,7 @@ int main() {
                     }
                 }
 
+                ImGui::SeparatorText("Configs");
                 if (encoder.is_encoding()) {
                     ImGui::BeginDisabled();
                 }
@@ -241,6 +340,22 @@ int main() {
         ImGui::End();
         ImGui::Render();
 
+        // for debug; NDI work
+        auto tool_data = capi.getTrackingDataBX();
+        for (auto &tool: tool_data) {
+            auto port = tool.transform.toolHandle;
+            if (port == 65535) continue;
+            if (tool.transform.isMissing()) continue;
+            auto trans = port_info_to_transform(tool.transform);
+            if (port == cam_port) {
+                left_ar.set_camera_pose(trans * ref_cam_trans_left);
+                right_ar.set_camera_pose(trans * ref_cam_trans_right);
+            } else if (port == tool_port) {
+                transform_to_vtk_matrix(trans, tool_matrix);
+                actor->Modified();
+            }
+        }
+
         std::chrono::high_resolution_clock::time_point start_time;
         if (camera.is_capturing()) {
             camera.retrieve_raw_images();