Implemented augment accuracy test.

CMakeLists.txt

@@ -17,6 +17,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/frame_sender/sender_tcp.cpp
         src/image_process.cpp
         src/imgui_utility.cpp
+        src/log_utility.cpp
         src/simple_mq.cpp
         src/third_party/rs.c
         src/vis_marker_detector.cpp)

data/config.yaml

@@ -4,31 +4,31 @@ camera:
     right: RightEye
   intrinsic:
     left:
-      fx: 3591.43951726492
-      fy: 3590.12059005116
-      cx: 1228.96467407067
-      cy: 1026.27011971545
-      k0: -0.0568617986513312
-      k1: 0.0588730412975145
+      fx: 3565.41112619664
+      fy: 3564.05573194168
+      cx: 1233.69545638269
+      cy: 1025.51713185245
+      k0: -0.0520040229813935
+      k1: 0.0398974773143781
       width: 2448
       height: 2048
     right:
-      fx: 3600.88065108231
-      fy: 3599.67273902144
-      cx: 1229.67535197217
-      cy: 1038.94918883869
-      k0: -0.0763206665373899
-      k1: 0.169733866632155
+      fx: 3571.67418317616
+      fy: 3570.36935016201
+      cx: 1222.10981440059
+      cy: 1034.19592532512
+      k0: -0.0760010312471698
+      k1: 0.196624456130940
       width: 2448
       height: 2048
   stereo_trans:
-    tx: -66.4031588292631
-    ty: 0.160811876831140
-    tz: -0.381588440976469
-    qw: 0.999981391974791
-    qx: -0.000249611277660394
-    qy: -0.00484078393169678
-    qz: -0.00370408008765430
+    tx: -65.3265350213620
+    ty: 0.245313839615459
+    tz: -0.508569900882309
+    qw: 0.999987680292989
+    qx: -0.000428742826329354
+    qy: -0.00395921570373319
+    qz: -0.00296311539549921
   capture:
     frame_rate: 40
     expo_time_ms: 12

data/sophiar_config.json

@@ -10,16 +10,7 @@
     },
     {
       "name": "femur_in_femur_ref",
-      "type": "transform_obj",
-      "value": [
-        324.96737353558717,
-        192.97678258884025,
-        -67.4123353235597,
-        -0.3374618708678985,
-        0.7283860379847338,
-        -0.05998432687891823,
-        0.5932749328162472
-      ]
+      "type": "transform_obj"
     },
     {
       "name": "tibia_ref_in_tracker",
@@ -27,16 +18,7 @@
     },
     {
       "name": "tibia_in_tibia_ref",
-      "type": "transform_obj",
-      "value": [
-        111.38885209055383,
-        -111.38354372578567,
-        55.48848071109328,
-        0.044347938544689505,
-        0.5450279992560348,
-        -0.4325668477263556,
-        0.7168428437404429
-      ]
+      "type": "transform_obj"
     },
     {
       "name": "camera_ref_in_tracker",
@@ -187,26 +169,26 @@
             "name": "left_camera",
             "parent": "camera_ref",
             "transform": [
-              8.2979,
-              -2.9563,
-              -32.4969,
-              0.1872,
-              -0.6788,
-              0.1839,
-              0.6859
+              27.5464,
+              -32.0497,
+              -38.7030,
+              0.1844,
+              0.6772,
+              0.1825,
+              -0.6885
             ]
           },
           {
             "name": "right_camera",
             "parent": "camera_ref",
             "transform": [
-              7.9742,
-              -2.8386,
-              -98.9961,
-              0.1839,
-              -0.6813,
-              0.1875,
-              0.6832
+              26.6363,
+              -32.6076,
+              -104.0095,
+              0.1855,
+              0.6805,
+              0.1809,
+              -0.6854
             ]
           }
         ]
@@ -272,7 +254,7 @@
             }
           },
           {
-            "rom_path": "./roms/Glass_4Ball_1.rom",
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_1_Camera_20240130.rom",
             "outputs": {
               "transform": "camera_ref_in_tracker"
             }
@@ -284,7 +266,7 @@
             }
           },
           {
-            "rom_path": "./roms/Glass_3Ball_5.rom",
+            "rom_path": "/home/tpx/data/roms/Glass_3Ball_5_Combined.rom",
             "outputs": {
               "transform": "tibia_ref_in_tracker"
             }

src/cuda_helper.hpp

@@ -64,6 +64,11 @@ struct smart_gpu_buffer {
     T *ptr = nullptr;
     size_t size = 0;
 
+    smart_gpu_buffer() = default;
+
+    template<typename U>
+    smart_gpu_buffer(const smart_gpu_buffer<T> &other) = delete;
+
     ~smart_gpu_buffer() {
         deallocate();
     }

src/experiment_impl/augment_accuracy.cpp

@@ -1,20 +1,30 @@
+#include "core/local_connection.h"
 #include "imgui_utility.h"
+#include "log_utility.h"
 #include "main_impl/impl_types.h"
 #include "vis_marker_detector.h"
 #include "vtk_viewer.h"
 
+#include <vtkProperty.h>
+
+#include <nlohmann/json.hpp>
+
 #include <queue>
 
+using namespace sophiar;
+
 bool enable_augment_accuracy = false;
 vis_marker_detector::options detect_opt;
 std::unique_ptr<vis_marker_detector> marker_detector;
 std::optional<Eigen::Isometry3f> last_marker_trans;
-float last_marker_error = 0;
+std::optional<float> last_marker_error;
+std::optional<float> last_augment_error;
 
-bool show_marker = false;
 Eigen::Matrix3Xf marker_info;
-smart_point_sets marker_points;
-vtkSmartPointer<vtkActor> marker_actor;
+bool show_augment_marker = false;
+smart_point_sets augment_marker_points;
+bool show_real_marker = false;
+smart_point_sets real_marker_points;
 
 extern bool mono_mode;
 extern int preview_camera_index;
@@ -24,60 +34,128 @@ extern std::unique_ptr<camera_related> right;
 extern std::unique_ptr<vtk_viewer> augment_viewer;
 extern std::queue<std::function<void()>> simple_eq;
 extern std::queue<std::function<void()>> close_funcs;
+extern local_connection sophiar_conn;
 
-void initialize_augment_accuracy() {
+void initialize_augment_accuracy() { // TODO, make this module a class
     marker_info.resize(3, 3);
-    marker_info << Eigen::Vector3f{0, 0, 0}, // TODO, load external
-            Eigen::Vector3f{55.071, 0, -0.025},
-            Eigen::Vector3f{34.378, 0, -57.242};
+    marker_info << Eigen::Vector3f{-27.309642, 0, -22.915511}, // TODO, load external
+            Eigen::Vector3f{33.150896, 0, -13.113173},
+            Eigen::Vector3f{6.449524, 0, 35.061957};
 
     // copy points to vtk actor
     for (auto p: marker_info.colwise()) {
-        marker_points.add_point(p.cast<double>());
+        real_marker_points.add_point(p.cast<double>());
+        augment_marker_points.add_point(p.cast<double>());
     }
-    marker_actor = marker_points.get_actor();
-    augment_viewer->add_actor(marker_actor);
+    augment_marker_points.get_actor()->GetProperty()->SetColor(1, 0, 0);
+    augment_viewer->add_actor(real_marker_points.get_actor());
+    augment_viewer->add_actor(augment_marker_points.get_actor());
 
     marker_detector = std::make_unique<vis_marker_detector>();
     close_funcs.emplace([&] { marker_detector.reset(); });
 }
 
 void show_augment_accuracy_ui() {
-    ImGui::Checkbox("Show Marker", &show_marker);
-    show_variable("Marker", last_marker_trans);
-    if (last_marker_trans.has_value()) {
+    ImGui::Checkbox("Enable", &enable_augment_accuracy);
+    if (enable_augment_accuracy) {
+        ImGui::SameLine();
+        ImGui::Checkbox("Show Real Marker", &show_real_marker);
+        ImGui::SameLine();
+        ImGui::Checkbox("Show Augment Marker", &show_augment_marker);
+        show_variable("Marker", last_marker_trans);
         show_variable("Marker Error", last_marker_error);
-    } else {
-        show_variable("Marker Error", std::optional<float>());
+        show_variable("Augment Error", last_augment_error);
     }
 }
 
 void process_augment_accuracy() {
+    if (!enable_augment_accuracy) return;
     if (!mono_mode
         || left->raw_img.host().cols != left->intrinsic.width) {
         enable_augment_accuracy = false;
         return;
     }
-    assert(enable_augment_accuracy);
 
+    float detect_err;
     last_marker_trans = marker_detector->detect(
             &left->raw_img, &right->raw_img,
             stereo_info, marker_info,
-            detect_opt, &last_marker_error);
-    if (last_marker_trans.has_value() && show_marker) {
-        marker_actor->SetVisibility(true);
-        Eigen::Isometry3d marker_pose;
-        if (preview_camera_index == 0) { // left
-            marker_pose = augment_viewer->get_camera_pose()
-                          * last_marker_trans.value().cast<double>();
-        } else {
-            assert(preview_camera_index == 1); // right
-            marker_pose = augment_viewer->get_camera_pose()
-                          * stereo_info.transform.cast<double>()
-                          * last_marker_trans.value().cast<double>();
+            detect_opt, &detect_err);
+
+    // detect error
+    auto marker_found = last_marker_trans.has_value();
+    if (marker_found) {
+        last_marker_error = detect_err;
+    } else {
+        last_marker_error = {};
+    }
+
+    // detected marker
+    if (marker_found) {
+        real_marker_points.clear();
+        Eigen::Matrix3Xf points_mat;
+        marker_detector->get_marker_points(&points_mat);
+        for (auto p: points_mat.colwise()) {
+            real_marker_points.add_point(left->transform * p.cast<double>());
         }
-        update_actor_pose(marker_actor, marker_pose);
+    }
+    real_marker_points.get_actor()->SetVisibility(
+            show_real_marker && marker_found);
+
+    // augmented marker
+    auto ref_trans = sophiar_conn.query_transform_variable("tibia_ref_in_tracker");
+    auto augment_actor = augment_marker_points.get_actor();
+    if (ref_trans.has_value()) {
+        update_actor_pose(augment_actor, ref_trans.value());
+    }
+    augment_marker_points.get_actor()->SetVisibility(
+            show_augment_marker && ref_trans.has_value());
+
+    // calculate error
+    if (marker_found && ref_trans.has_value()) {
+
+        // real points in camera
+        Eigen::Matrix3Xf points_mat;
+        marker_detector->get_marker_points(&points_mat);
+
+        // augmented points in camera
+        auto inv_trans = (left->transform.inverse()  // world observed in camera
+                          * ref_trans.value()) // ref observed in world
+                .cast<float>();
+        Eigen::Matrix3Xf mp_cam(3, marker_info.cols());
+        for (auto k = 0; k < marker_info.cols(); ++k) {
+            mp_cam.col(k) = inv_trans * marker_info.col(k);
+        }
+
+        Eigen::VectorXd err{marker_info.cols()};
+        for (auto k = 0; k < marker_info.cols(); ++k) {
+            auto real_point = points_mat.col(k);
+            auto augment_point = mp_cam.col(k);
+            Eigen::Vector3f offset = real_point - augment_point;
+            auto used_offset = Eigen::Vector2f{offset.x(), offset.y()};
+            err(k) = used_offset.norm();
+        }
+        last_augment_error = err.norm() / sqrt(err.size());
     } else {
-        marker_actor->SetVisibility(false);
+        last_augment_error = {};
+    }
+
+    // save log // TODO: create an enable_XXX
+    if (marker_found && ref_trans.has_value()) {
+        // augmented points in camera
+        auto inv_trans = (left->transform.inverse()  // world observed in camera
+                          * ref_trans.value()) // ref observed in world
+                .cast<float>();
+        Eigen::Matrix3Xf mp_cam(3, marker_info.cols());
+        for (auto k = 0; k < marker_info.cols(); ++k) {
+            mp_cam.col(k) = inv_trans * marker_info.col(k);
+        }
+
+        nlohmann::json log_j;
+        log_j["detector"] = marker_detector->status_as_json();
+        log_j["augment_points"] = matrix_to_json(mp_cam);
+        log_j["detect_error"] = last_marker_error.value();
+        log_j["augment_error"] = last_augment_error.value();
+        write_main_log("augment_accuracy", log_j);
     }
 }

src/frame_sender/sender_udp_fec.cpp

@@ -32,6 +32,10 @@ namespace sender_udp_fec_impl {
         reed_solomon *rs = nullptr;
         smart_buffer<uint8_t *> block_ptrs;
 
+        smart_reed_solomon() = default;
+
+        smart_reed_solomon(const smart_reed_solomon &other) = delete;
+
         ~smart_reed_solomon() {
             deallocate();
         }
@@ -54,7 +58,7 @@ namespace sender_udp_fec_impl {
 
     private:
         uint8_t last_data_blocks = 0;
-        smart_buffer<uint8_t> block_data;
+        smart_buffer <uint8_t> block_data;
 
         void deallocate() {
             if (rs == nullptr) return;
@@ -121,7 +125,7 @@ struct sender_udp_fec::impl {
     uint16_t block_size; // conn_mtu - header_size
     uint32_t max_chunk_size; // max_data_block_count * block_size
     smart_reed_solomon rs;
-    smart_buffer<uint8_t> in_buf, out_buf;
+    smart_buffer <uint8_t> in_buf, out_buf;
     timestamp_type last_confirm_ts = 0;
 
     static uint8_t *write_frag_header(uint8_t *ptr, const frag_header &header) {

src/image_process.cpp

@@ -10,11 +10,16 @@ namespace process_impl {
 
     struct smart_cuda_texture {
         cudaTextureObject_t obj = 0;
+        int mat_type = -1;
 
         ~smart_cuda_texture() {
             deallocate();
         }
 
+        smart_cuda_texture() = default;
+
+        smart_cuda_texture(const smart_cuda_texture &other) = delete;
+
         void create(const cv::cuda::GpuMat &mat) {
             if (last_ptr != mat.cudaPtr()) [[unlikely]] {
                 deallocate();
@@ -40,7 +45,12 @@ namespace process_impl {
             tex_desc.readMode = cudaReadModeNormalizedFloat;
             tex_desc.normalizedCoords = true;
 
-            switch (mat.type()) {
+            mat_type = mat.type();
+            switch (mat_type) {
+                case CV_8UC1: {
+                    res_desc.res.pitch2D.desc = cudaCreateChannelDesc<uint8_t>();
+                    break;
+                }
                 case CV_8UC4: {
                     res_desc.res.pitch2D.desc = cudaCreateChannelDesc<uchar4>();
                     break;
@@ -164,7 +174,7 @@ namespace process_impl {
         return {u, v};
     }
 
-    void resample_image(cudaTextureObject_t in, cv::cuda::GpuMat *out, int depth_type,
+    void resample_image(const smart_cuda_texture &in, cv::cuda::GpuMat *out,
                         cv::Size2f range, const camera_intrinsic &cam, uint32_t height, cudaStream_t stream) {
         constexpr uint2 block_size = {32, 4};
         constexpr uint2 grid_dim = {8, 128};
@@ -175,10 +185,16 @@ namespace process_impl {
         info.x = -range.width;
         info.y = -range.height;
         info.ps = ps;
-        out->create(height, width, CV_MAKE_TYPE(depth_type, 3));
-        switch (depth_type) {
-            case CV_8U: {
-                call_resample_image(in, to_image_type<uchar3>(*out), info,
+        switch (in.mat_type) {
+            case CV_8UC1: {
+                out->create(height, width, CV_8UC1);
+                call_resample_image(in.obj, to_image_type<uint8_t>(*out), info,
+                                    to_camera_info(cam), block_size, grid_dim, stream);
+                return;
+            }
+            case CV_8UC4: {
+                out->create(height, width, CV_8UC3);
+                call_resample_image(in.obj, to_image_type<uchar3>(*out), info,
                                     to_camera_info(cam), block_size, grid_dim, stream);
                 return;
             }
@@ -194,7 +210,8 @@ using namespace process_impl;
 
 struct monocular_processor::impl {
     cv::cuda::GpuMat rgba_dev;
-    smart_cuda_texture rgba_tex;
+    cv::cuda::GpuMat resample_dev;
+    smart_cuda_texture resample_tex;
     smart_gpu_buffer<uchar3> rgb_f;
     smart_gpu_buffer<float> hsv_v_f;
     smart_gpu_buffer<float> hsv_v_max, hsv_v_sum_log;
@@ -239,8 +256,24 @@ struct monocular_processor::impl {
                  const image_process_config &conf, cv::cuda::Stream &stream) {
         auto cuda_stream = (cudaStream_t) stream.cudaPtr();
 
-        // debayer
-        if (!conf.is_mono) {
+        if (conf.is_mono) {
+
+            // undistort
+            if (conf.undistort) {
+                resample_tex.create(in);
+                assert(out->cudaPtr() != in.cudaPtr());
+                resample_image(resample_tex, &resample_dev, conf.valid_range,
+                               *conf.camera, conf.resample_height, cuda_stream);
+            } else {
+                resample_dev = in;
+            }
+
+            // Mono -> RGB
+            opencv_gray2rgb(resample_dev, out, stream);
+
+        } else {
+
+            // debayer
             if (conf.crude_debayer) {
                 if (conf.undistort) {
                     crude_debayer(in, &rgba_dev, true, cuda_stream);
@@ -251,16 +284,14 @@ struct monocular_processor::impl {
                 assert(!conf.undistort);
                 opencv_debayer(in, out, stream);
             }
-        } else {
-            assert(!conf.undistort);
-            opencv_gray2rgb(in, out, stream);
-        }
 
-        if (conf.undistort) {
-            assert(conf.crude_debayer);
-            rgba_tex.create(rgba_dev);
-            resample_image(rgba_tex.obj, out, CV_MAT_DEPTH(in.type()),
-                           conf.valid_range, *conf.camera, conf.resample_height, cuda_stream);
+            // undistort
+            if (conf.undistort) {
+                assert(conf.crude_debayer);
+                resample_tex.create(rgba_dev);
+                resample_image(resample_tex, out, conf.valid_range,
+                               *conf.camera, conf.resample_height, cuda_stream);
+            }
         }
 
         // enhance image

src/image_process/process_kernels.cu

@@ -384,20 +384,21 @@ __global__ void resample_image(cudaTextureObject_t in, image_type<ImgT> out,
             v = cam.fy * v + cam.cy;
 
             // sample origin image
-            auto val = tex2D<float4>(in, u, v);
-            ImgT ret;
-            using ElemT = decltype(ret.x);
-            static_assert(std::is_integral_v<ElemT>,
-                          "Type of output image element must be integer.");
-            static_assert(std::is_same_v<ImgT, typename packed_type<ElemT, 3>::type>,
-                          "Type of output image element must have three channels.");
-            constexpr auto factor = type_max_value<ElemT>::value;
-            ret.x = factor * val.x;
-            ret.y = factor * val.y;
-            ret.z = factor * val.z;
-
-            // store result
-            image_store(out, ix, iy, ret);
+            if constexpr (std::is_integral_v<ImgT>) { // Mono -> Mono
+                constexpr auto factor = type_max_value<ImgT>::value;
+                auto val = tex2D<float>(in, u, v);
+                ImgT ret = val * factor;
+                image_store(out, ix, iy, ret);
+            } else { // RGBA -> RGB
+                ImgT ret;
+                using ElemT = decltype(ret.x);
+                constexpr auto factor = type_max_value<ElemT>::value;
+                auto val = tex2D<float4>(in, u, v);
+                ret.x = factor * val.x;
+                ret.y = factor * val.y;
+                ret.z = factor * val.z;
+                image_store(out, ix, iy, ret);
+            }
         }
 }
 
@@ -483,6 +484,7 @@ void call_crude_debayer(image_type<InT> in, image_type<OutT> out,
                         uint2 _block_size, uint2 _grid_dim, cudaStream_t stream) {
     auto block_size = dim3{_block_size.x, _block_size.y, 1};
     auto grid_dim = dim3{_grid_dim.x, _grid_dim.y, 1};
+    assert((void *) in.ptr != (void *) out.ptr);
     crude_debayer<<<grid_dim, block_size, 0, stream>>>(in, out);
 }
 
@@ -500,4 +502,7 @@ void call_resample_image(cudaTextureObject_t in, image_type<ImgT> out,
 }
 
 template void call_resample_image(cudaTextureObject_t, image_type<uchar3>,
+                                  resample_info, camera_info, uint2, uint2, cudaStream_t);
+
+template void call_resample_image(cudaTextureObject_t, image_type<uint8_t>,
                                   resample_info, camera_info, uint2, uint2, cudaStream_t);

src/image_utility.hpp

@@ -34,25 +34,35 @@ public:
     void set_host_mat(const cv::Mat &mat) {
         host_mat = mat;
         is_host_valid = true;
+        is_dev_valid = false;
     }
 
     void set_dev_mat(const cv::cuda::GpuMat &mat) {
         dev_mat = mat;
+        is_host_valid = false;
         is_dev_valid = true;
     }
 
     cv::Mat host(cv::cuda::Stream &stream = cv::cuda::Stream::Null()) {
         if (!is_host_valid) {
-            assert(is_dev_valid);
-            dev_mat.download(host_mat, stream);
+            if (is_dev_valid) {
+                dev_mat.download(host_mat, stream);
+                is_host_valid = true;
+            } else {
+                return cv::Mat{};
+            }
         }
         return host_mat;
     }
 
     cv::cuda::GpuMat dev(cv::cuda::Stream &stream = cv::cuda::Stream::Null()) {
         if (!is_dev_valid) {
-            assert(is_host_valid);
-            dev_mat.upload(host_mat, stream);
+            if (is_host_valid) {
+                dev_mat.upload(host_mat, stream);
+                is_dev_valid = true;
+            } else {
+                return cv::cuda::GpuMat{};
+            }
         }
         return dev_mat;
     }
@@ -67,6 +77,25 @@ public:
         return *this;
     }
 
+    cv::Size size() const {
+        if (is_host_valid) {
+            return host_mat.size();
+        } else {
+            assert(is_dev_valid);
+            return dev_mat.size();
+        }
+    }
+
+    bool empty() const {
+        if (!(is_host_valid || is_dev_valid)) return true;
+        if (is_host_valid) {
+            return host_mat.empty();
+        } else {
+            assert(is_dev_valid);
+            return dev_mat.empty();
+        }
+    }
+
 private:
     cv::Mat host_mat;
     cv::cuda::GpuMat dev_mat;

src/log_utility.cpp

@@ -0,0 +1,21 @@
+#include "log_utility.h"
+
+#include <fmt/chrono.h>
+
+void write_main_log(const std::string &tag, const nlohmann::json &data, uint64_t timestamp) {
+    // ensure thread consistency
+    static auto thread_id = std::this_thread::get_id();
+    assert(std::this_thread::get_id() == thread_id);
+
+    if (!main_log_file.is_open()) [[unlikely]] {
+        auto file_name = fmt::format("main_log_{:%Y_%m_%d_%H_%M_%S}.txt",
+                                     now_since_epoch_in_seconds());
+        main_log_file.open(file_name);
+    }
+
+    nlohmann::json log_item;
+    log_item["tag"] = tag;
+    log_item["time"] = timestamp;
+    log_item["data"] = data;
+    main_log_file << log_item.dump() << std::endl;
+}

src/log_utility.h

@@ -0,0 +1,30 @@
+#ifndef REMOTEAR3_LOG_UTILITY_H
+#define REMOTEAR3_LOG_UTILITY_H
+
+#include "utility.hpp"
+
+#include <Eigen/Dense>
+
+#include <nlohmann/json.hpp>
+
+#include <fstream>
+
+extern std::ofstream main_log_file;
+
+void write_main_log(const std::string &tag, const nlohmann::json &data,
+                    uint64_t timestamp = system_timestamp());
+
+template<typename Derived>
+inline nlohmann::json matrix_to_json(const Eigen::MatrixBase<Derived> &matrix) {
+    nlohmann::json mat_j;
+    for (auto j = 0; j < matrix.cols(); ++j) {
+        nlohmann::json col_j;
+        for (auto i = 0; i < matrix.rows(); ++i) {
+            col_j.push_back(matrix(i, j));
+        }
+        mat_j.push_back(col_j);
+    }
+    return mat_j;
+}
+
+#endif //REMOTEAR3_LOG_UTILITY_H

src/main_ext.cpp

@@ -29,6 +29,8 @@
 
 #include <boost/iostreams/device/mapped_file.hpp>
 
+#include <fstream>
+
 #include <cassert>
 #include <thread>
 #include <queue>
@@ -39,6 +41,7 @@ using namespace sophiar;
 
 using boost::iostreams::mapped_file;
 
+std::ofstream main_log_file;
 log_timer global_timer;
 
 // global variable definition
@@ -52,6 +55,7 @@ float process_frame_rate = 0;
 extern bool mono_mode;
 
 // render related
+extern bool enable_augment_without_nav;
 extern std::vector<augment_store_type> augment_items;
 
 std::string sophiar_config_path;
@@ -69,8 +73,6 @@ bool show_vtk_debug = false;
 bool show_imgui_demo = false;
 std::unique_ptr<vtk_viewer> vtk_test1, vtk_test2;
 
-extern bool enable_augment_accuracy;
-
 std::queue<std::function<void()>> simple_eq;
 std::queue<std::function<void()>> close_funcs;
 
@@ -365,15 +367,14 @@ void prepare_imgui_frame() {
             ImGui::Checkbox("Debug VTK Viewer", &show_vtk_debug);
             ImGui::Checkbox("Show ImGui Demo", &show_imgui_demo);
             ImGui::Checkbox("Debug Options", &debug_options);
+            ImGui::Checkbox("Enable Augment without Navigation",
+                            &enable_augment_without_nav);
             ImGui::PopID();
         }
 
         if (ImGui::CollapsingHeader("Experiment")) {
             ImGui::PushID("Experiment");
             if (mono_mode) {
-                ImGui::Checkbox("Augment Accuracy", &enable_augment_accuracy);
-            }
-            if (enable_augment_accuracy) {
                 if (ImGui::TreeNode("Augment Accuracy")) {
                     show_augment_accuracy_ui();
                     ImGui::TreePop();
@@ -406,17 +407,14 @@ void prepare_imgui_frame() {
 
 void process_camera_frames();
 
-void process_augment();
+void prepare_augment_info();
 
 void process_augment_accuracy();
 
 void process_frame() {
-    process_augment();
+    prepare_augment_info();
+    process_augment_accuracy();
     process_camera_frames();
-
-    if (enable_augment_accuracy) {
-        process_augment_accuracy();
-    }
 }
 
 void handle_imgui_events() {

src/main_impl/camera_related.cpp

@@ -5,6 +5,9 @@
 #include "mvs_camera.h"
 #include "variable_defs.h"
 
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
 #include <fmt/chrono.h>
 
 #include <queue>
@@ -21,22 +24,23 @@ struct simple_image_saver {
         std::filesystem::create_directories(prefix_path);
     }
 
-    void save_image_raw(smart_mat *left, smart_mat *right) const {
-        auto left_host = left->host();
-        auto right_host = right->host();
-        assert(left_host.isContinuous());
-        assert(right_host.isContinuous());
+    void save_image_raw(smart_mat *left, smart_mat *right) const;
+
+    void save_image_png(smart_mat *left, smart_mat *right) const;
+
+    void save_image_png(const smart_texture &left,
+                        const smart_texture &right) const;
 
+private:
+    auto get_save_path(std::string_view ext) const {
         auto ts_str = fmt::format("{}", system_timestamp());
-        auto left_name = fmt::format("left_{}.raw", ts_str);
-        auto right_name = fmt::format("right_{}.raw", ts_str);
+        auto left_name = fmt::format("left_{}.{}", ts_str, ext);
+        auto right_name = fmt::format("right_{}.{}", ts_str, ext);
         auto left_path = prefix_path / left_name;
         auto right_path = prefix_path / right_name;
-        save_file(left_path.c_str(), left_host.data, left_host.total() * left_host.elemSize());
-        save_file(right_path.c_str(), right_host.data, right_host.total() * right_host.elemSize());
+        return std::make_tuple(left_path, right_path);
     }
 
-private:
     static void save_file(const char *file_path, void *data, size_t length) {
         static constexpr auto block_size = 512 * 1024; // 512KiB
         auto file = fopen(file_path, "wb");
@@ -50,14 +54,78 @@ private:
         }
         fclose(file);
     }
+
+    static void save_png(const std::string &file_path, const cv::Mat &img) {
+        cv::Mat out_img = img;
+        if (out_img.channels() == 3) { // RGB -> BGR
+            cv::cvtColor(img, out_img, cv::COLOR_RGB2BGR);
+        }
+        cv::imwrite(file_path, out_img);
+    }
+
+    static void save_tex(const std::string &file_path, const smart_texture &tex) {
+        // download texture
+        GLenum format;
+        GLsizei width, height;
+        glBindTexture(GL_TEXTURE_2D, tex.id);
+        glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, GL_TEXTURE_INTERNAL_FORMAT, (int *) &format);
+        glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, GL_TEXTURE_WIDTH, &width);
+        glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, GL_TEXTURE_HEIGHT, &height);
+        auto closer = sg::make_scope_guard([&] {
+            glBindTexture(GL_TEXTURE_2D, 0);
+        });
+
+        switch (format) {
+            case GL_RGBA8: {
+                auto img = cv::Mat(height, width, CV_8UC4);
+                assert(img.isContinuous());
+                glGetTexImage(GL_TEXTURE_2D, 0, GL_BGRA, GL_UNSIGNED_BYTE, img.data);
+                save_png(file_path, img);
+                return;
+            }
+            default: {
+                RET_ERROR;
+            }
+        }
+    }
 };
 
-bool mono_mode = false;
+void simple_image_saver::save_image_raw(smart_mat *left, smart_mat *right) const {
+    auto left_host = left->host();
+    auto right_host = right->host();
+    assert(left_host.isContinuous());
+    assert(right_host.isContinuous());
+    auto [left_path, right_path] = get_save_path(".raw");
+    save_file(left_path.c_str(), left_host.data, left_host.total() * left_host.elemSize());
+    save_file(right_path.c_str(), right_host.data, right_host.total() * right_host.elemSize());
+}
+
+void simple_image_saver::save_image_png(smart_mat *left, smart_mat *right) const {
+    auto [left_path, right_path] = get_save_path(".p.png");
+    save_png(left_path, left->host());
+    save_png(right_path, right->host());
+}
+
+void simple_image_saver::save_image_png(const smart_texture &left,
+                                        const smart_texture &right) const {
+    auto [left_path, right_path] = get_save_path(".t.png");
+    save_tex(left_path, left);
+    save_tex(right_path, right);
+}
+
+static constexpr auto CAPTURE_RAW_BIT = 1 << 0;
+static constexpr auto CAPTURE_PROCESSED_BIT = 1 << 1;
+static constexpr auto CAPTURE_AUGMENT_BIT = 1 << 2;
+uint8_t capture_flag = 0;
+bool capture_raw = false;
+bool capture_processed = false;
+bool capture_augment = false;
+
+bool mono_mode = true;
 bool enhance_image = false;
 bool use_crude_debayer = true; // debug options
 bool undistort_image = true; // debug options
 int preview_camera_index = 0;  // 0 for left, 1 for right
-bool save_next_raw = false;
 
 mvs::capture_config capture_conf = {};
 stereo_camera_info stereo_info;
@@ -69,6 +137,7 @@ extern nvenc_config main_nvenc_conf;
 extern bool debug_options;
 extern float augment_render_angle;
 extern float process_frame_rate;
+extern bool augment_enable;
 
 extern local_connection sophiar_conn;
 extern std::queue<std::function<void()>> simple_eq;
@@ -89,31 +158,24 @@ void camera_related::load_intrinsic(YAML::Node conf) {
     process_conf.camera = &intrinsic;
 }
 
-void camera_related::wait_frame(simple_mq::index_type index) const {
+void camera_related::get_next_frame(simple_mq::index_type index) {
     uint64_t cur_cnt;
-    if (auto ptr = mq().query_variable_ptr<cv::Mat>(index, &cur_cnt);
-            ptr == nullptr || cur_cnt <= raw_cnt) {
+    auto ptr = mq().query_variable_ptr<cv::Mat>(index, &cur_cnt);
+    if (ptr == nullptr || cur_cnt <= raw_cnt) {
         mq().wait_variable(index, raw_cnt);
+        ptr = mq().query_variable_ptr<cv::Mat>(index, &cur_cnt);
+        assert(cur_cnt > raw_cnt);
     }
-}
-
-void camera_related::process_frame(simple_mq::index_type index) {
-    uint64_t cur_cnt;
-    auto raw_ptr = mq().query_variable_ptr<cv::Mat>(index, &cur_cnt);
-    assert(cur_cnt > raw_cnt);
     raw_cnt = cur_cnt;
-    raw_img = *raw_ptr;
+    raw_img = *ptr;
+}
 
+void camera_related::process_frame() {
     // OpenCV debayer does not support alpha channel
-    if (undistort_image) {
+    if (undistort_image && !mono_mode) {
         use_crude_debayer = true;
     }
 
-    // Undistort is currently not supported in mono mode
-    if (mono_mode) {
-        undistort_image = false;
-    }
-
     // update process config
     process_conf.is_mono = mono_mode;
     process_conf.crude_debayer = use_crude_debayer;
@@ -122,7 +184,9 @@ void camera_related::process_frame(simple_mq::index_type index) {
     process_conf.resample_height = mq().query_variable<int>(OUTPUT_HEIGHT);
 
     // process image
+    auto img_dev = img_mat.dev(stream.cv);
     processor.process(raw_img.dev(stream.cv), &img_dev, process_conf, stream.cv);
+    img_mat.set_dev_mat(img_dev);
 
     // augment render
     process_augment();
@@ -146,18 +210,20 @@ void load_camera_config(YAML::Node conf) {
 
     // load stereo trans
     auto trans_info = conf["stereo_trans"];
-    stereo_info.transform =
-            Eigen::Translation3f(
-                    trans_info["tx"].as<float>(),
-                    trans_info["ty"].as<float>(),
-                    trans_info["tz"].as<float>()
-            ) * Eigen::Quaternionf(
-                    trans_info["qw"].as<float>(),
-                    trans_info["qx"].as<float>(),
-                    trans_info["qy"].as<float>(),
-                    trans_info["qz"].as<float>());
+    left->transform =
+            Eigen::Translation3d(
+                    trans_info["tx"].as<double>(),
+                    trans_info["ty"].as<double>(),
+                    trans_info["tz"].as<double>()
+            ) * Eigen::Quaterniond(
+                    trans_info["qw"].as<double>(),
+                    trans_info["qx"].as<double>(),
+                    trans_info["qy"].as<double>(),
+                    trans_info["qz"].as<double>());
+    right->transform = Eigen::Isometry3d::Identity();
     stereo_info.left = &left->intrinsic;
     stereo_info.right = &right->intrinsic;
+    stereo_info.transform = left->transform.cast<float>();
 
     // calculate valid resample range
     auto range = calc_valid_range(left->intrinsic, right->intrinsic, &augment_render_angle);
@@ -236,22 +302,38 @@ void upload_encoder_config();
 
 void wait_camera_frames() {
     assert(is_capturing());
-    left->wait_frame(IMG_RAW_HOST_LEFT);
-    right->wait_frame(IMG_RAW_HOST_RIGHT);
+    left->get_next_frame(IMG_RAW_HOST_LEFT);
+    right->get_next_frame(IMG_RAW_HOST_RIGHT);
 }
 
-void process_camera_frames() {
-    left->process_frame(IMG_RAW_HOST_LEFT);
-    right->process_frame(IMG_RAW_HOST_RIGHT);
+void set_capture_flag() {
+    capture_flag = 0;
+    capture_flag |= capture_raw ? CAPTURE_RAW_BIT : 0;
+    capture_flag |= capture_processed ? CAPTURE_PROCESSED_BIT : 0;
+    capture_flag |= capture_augment ? CAPTURE_AUGMENT_BIT : 0;
 
-    // save image if required
-    if (save_next_raw) {
-        if (image_saver == nullptr) [[unlikely]] {
-            image_saver = std::make_unique<simple_image_saver>();
-        }
+    if (image_saver == nullptr) {
+        image_saver = std::make_unique<simple_image_saver>();
+    }
+}
+
+void process_camera_frames() {
+    if (capture_flag & CAPTURE_RAW_BIT) {
         image_saver->save_image_raw(&left->raw_img, &right->raw_img);
-        save_next_raw = false;
     }
+
+    left->process_frame();
+    right->process_frame();
+
+    if (capture_flag & CAPTURE_PROCESSED_BIT) {
+        image_saver->save_image_png(&left->img_mat, &right->img_mat);
+    }
+    if (capture_flag & CAPTURE_AUGMENT_BIT) {
+        image_saver->save_image_png(left->augment_tex, right->augment_tex);
+    }
+
+    // reset capture flag
+    capture_flag = 0;
 }
 
 void show_camera_ui() {
@@ -274,10 +356,6 @@ void show_camera_ui() {
             if (ImGui::Button("Stop")) {
                 simple_eq.emplace(stop_capture);
             }
-            ImGui::SameLine();
-            if (ImGui::Button("Capture Raw")) {
-                simple_eq.emplace([&] { save_next_raw = true; });
-            }
         }
     }
 
@@ -302,10 +380,10 @@ void show_camera_ui() {
     }
     ImGui::SameLine();
     ImGui::Checkbox("Enhance", &enhance_image);
-    if (!mono_mode && debug_options) {
+    if (debug_options) {
         ImGui::SameLine();
         ImGui::Checkbox("Undistort", &undistort_image);
-        {
+        if (!mono_mode) {
             auto guard = imgui_disable_guard{undistort_image};
             ImGui::SameLine();
             ImGui::Checkbox("Crude Debayer", &use_crude_debayer);
@@ -325,36 +403,17 @@ void show_camera_ui() {
             ImGui::DragFloat("Process Frame Rate (fps)", &process_frame_rate, 0, 0, 60, "%.01f");
         }
 
-        // auto save raw config
-//                ImGui::SeparatorText("Auto Shoot");
-//                ImGui::PushID("Auto Shoot");
-//
-//                if (!auto_save_raw) {
-//                    if (ImGui::Button("Start")) {
-//                        auto_save_raw = true;
-//                    }
-//                } else {
-//                    if (ImGui::Button("Stop")) {
-//                        auto_save_raw = false;
-//                    }
-//                }
-//
-//                if (auto_save_raw_interval < 1) {
-//                    auto_save_raw_interval = 1;
-//                }
-//
-//                if (auto_save_raw) {
-//                    ImGui::BeginDisabled();
-//                    auto now_time = std::chrono::system_clock::now();
-//                    if (now_time - last_save_raw_time >
-//                        std::chrono::seconds{auto_save_raw_interval}) {
-//                                camera.request_save_raw();
-//                        last_save_raw_time = now_time;
-//                    }
-//                }
-//                ImGui::InputInt("Shoot Interval (s)", &auto_save_raw_interval, 1, 10);
-//                if (auto_save_raw) {
-//                    ImGui::EndDisabled();
-//                }
+        ImGui::SeparatorText("Capture Config");
+        ImGui::Checkbox("Raw", &capture_raw);
+        ImGui::SameLine();
+        ImGui::Checkbox("Processed", &capture_processed);
+        if (augment_enable) {
+            ImGui::SameLine();
+            ImGui::Checkbox("Augment", &capture_augment);
+        }
+        ImGui::SameLine();
+        if (ImGui::Button("Confirm")) {
+            simple_eq.emplace([&] { set_capture_flag(); });
+        }
     }
 }

src/main_impl/encoder_related.cpp

@@ -147,10 +147,10 @@ void start_encoder() {
     // determine output frame size
     cv::Size frame_size;
     if (output_full_frame) {
-        assert(!left->img_dev.empty() && !right->img_dev.empty());
-        assert(left->img_dev.size() == right->img_dev.size());
-        frame_size.width = left->img_dev.size().width * 2;
-        frame_size.height = left->img_dev.size().height;
+        assert(!left->img_mat.empty() && !right->img_mat.empty());
+        assert(left->img_mat.size() == right->img_mat.size());
+        frame_size.width = left->img_mat.size().width * 2;
+        frame_size.height = left->img_mat.size().height;
     } else {
         frame_size = {mq().query_variable<int>(OUTPUT_WIDTH),
                       mq().query_variable<int>(OUTPUT_HEIGHT)};

src/main_impl/impl_types.h

@@ -13,10 +13,11 @@
 
 struct camera_related {
     uint64_t raw_cnt = 0;
-    smart_mat raw_img;
-    cv::cuda::GpuMat img_dev;
+    smart_mat raw_img; // raw image from camera
+    smart_mat img_mat; // processed image
     smart_cuda_stream stream;
     std::string trans_var;
+    Eigen::Isometry3d transform; // observed in world
     camera_intrinsic intrinsic;
     image_process_config process_conf;
     monocular_processor processor;
@@ -26,11 +27,11 @@ struct camera_related {
 
     void load_intrinsic(YAML::Node conf);
 
-    void wait_frame(simple_mq::index_type index) const;
+    void get_next_frame(simple_mq::index_type index);
 
     void process_augment();
 
-    void process_frame(simple_mq::index_type index);
+    void process_frame();
 
     void render(const simple_rect &rect, bool flip_y = false);
 

src/main_impl/render_related.cpp

@@ -16,7 +16,8 @@
 using namespace sophiar;
 using namespace simple_mq_singleton;
 
-bool augment_enable = false;
+bool augment_enable = true;
+bool enable_augment_without_nav = true;
 float augment_render_angle = 0;
 bool output_full_frame = false;
 bool output_halve_width = false;
@@ -41,16 +42,19 @@ extern std::queue<std::function<void()>> close_funcs;
 void camera_related::process_augment() {
     if (augment_enable) {
         auto trans = sophiar_conn.query_transform_variable(trans_var);
-        augment_available = trans.has_value() || true;
+        if (trans.has_value()) {
+            transform = trans.value();
+        }
+        augment_available = trans.has_value() || enable_augment_without_nav;
         if (augment_available) {
-            if (trans.has_value()) // TODO: hacked
-                augment_viewer->set_camera_pose(trans.value());
-            augment_viewer->render(img_dev.size());
+            auto img_size = img_mat.size();
+            augment_viewer->set_camera_pose(transform);
+            augment_viewer->render(img_size);
 
             // copy rendered image
-            augment_tex.create(GL_RGBA8, img_dev.size());
+            augment_tex.create(GL_RGBA8, img_size);
             glCopyImageSubData(augment_viewer->get_tex(), GL_TEXTURE_2D, 0, 0, 0, 0,
-                               augment_tex.id, GL_TEXTURE_2D, 0, 0, 0, 0, img_dev.cols, img_dev.rows, 1);
+                               augment_tex.id, GL_TEXTURE_2D, 0, 0, 0, 0, img_size.width, img_size.height, 1);
         }
     } else {
         augment_available = false;
@@ -58,8 +62,8 @@ void camera_related::process_augment() {
 }
 
 void camera_related::render(const simple_rect &rect, bool flip_y) {
-    assert(!img_dev.empty());
-    opengl_render->render_rect(img_dev, rect, !flip_y, stream.cuda);
+    assert(!img_mat.empty());
+    opengl_render->render_rect(img_mat.dev(stream.cv), rect, !flip_y, stream.cuda);
     if (augment_available) {
         opengl_render->render_rect(augment_tex.id, rect, flip_y);
     }
@@ -99,16 +103,16 @@ void render_main_window() {
 
     if (is_capturing()) {
         // draw preview camera frame
-        assert(left->img_dev.size() == right->img_dev.size());
-        float width_normal = left->img_dev.size().aspectRatio() / frame_size.aspectRatio();
+        assert(left->img_mat.size() == right->img_mat.size());
+        float width_normal = left->img_mat.size().aspectRatio() / frame_size.aspectRatio();
         auto render_rect = simple_rect{-1, -1, 2, 2}.fit_aspect(width_normal);
         if (preview_camera_index == 0) { // left camera
-            if (!left->img_dev.empty()) {
+            if (!left->img_mat.empty()) {
                 left->render(render_rect);
             }
         } else { // right camera
             assert(preview_camera_index == 1);
-            if (!right->img_dev.empty()) {
+            if (!right->img_mat.empty()) {
                 right->render(render_rect);
             }
         }
@@ -140,7 +144,7 @@ void generate_output_frame() {
         left_rect = simple_rect{-1, -1, 1, 2};
         right_rect = simple_rect{0, -1, 1, 2};
     } else {
-        float width_normal = left->img_dev.size().aspectRatio() /
+        float width_normal = left->img_mat.size().aspectRatio() /
                              output_fbo->size().aspectRatio();
         if (output_halve_width) {
             width_normal *= 0.5f;
@@ -167,7 +171,7 @@ void generate_output_frame() {
     mq().update_variable_ptr(OUTPUT_FRAME, output_frame_dev);
 }
 
-void process_augment() {
+void prepare_augment_info() {
     for (auto &item: augment_items) {
         auto trans = sophiar_conn.query_transform_variable(item.trans_var);
         update_actor_pose(item.actor, trans);

src/simple_opengl.cpp

@@ -34,6 +34,10 @@ namespace simple_opengl_impl {
         GLuint id = 0;
         cudaGraphicsResource *res = nullptr;
 
+        smart_pixel_buffer() = default;
+
+        smart_pixel_buffer(const smart_pixel_buffer &other) = delete;
+
         ~smart_pixel_buffer() {
             deallocate();
         }

src/utility.hpp

@@ -76,6 +76,11 @@ struct smart_buffer {
     T *ptr = nullptr;
     size_t length = 0;
 
+    smart_buffer() = default;
+
+    template<typename U=T>
+    smart_buffer(const smart_buffer<U> &other) = delete;
+
     ~smart_buffer() {
         delete ptr;
     }

src/vis_marker_detector.cpp

@@ -1,6 +1,7 @@
 #include "vis_marker_detector.h"
 #include "cuda_helper.hpp"
 #include "image_process/vis_marker_kernels.cuh"
+#include "log_utility.h"
 #include "simple_mq.h"
 #include "utility.hpp"
 #include "variable_defs.h"
@@ -138,7 +139,11 @@ namespace vis_marker_detector_impl {
 
     float calc_transform_error(const Eigen::Matrix3Xf &points,
                                const Eigen::Matrix3Xf &marker,
-                               const Eigen::Isometry3f &trans) {
+                               const Eigen::Isometry3f &trans,
+                               Eigen::Matrix3Xf *marker_points = nullptr) {
+        if (marker_points != nullptr) {
+            marker_points->resize(3, marker.cols());
+        }
         Eigen::VectorXf err{marker.cols()};
         for (auto k = 0; k < marker.cols(); ++k) {
             auto mp = marker.col(k);
@@ -146,7 +151,12 @@ namespace vis_marker_detector_impl {
             err(k) = std::numeric_limits<float>::max();
             for (auto p: points.colwise()) {
                 auto dis = (p - tp).norm();
-                err(k) = std::min(dis, err(k));
+                if (dis < err(k)) {
+                    err(k) = dis;
+                    if (marker_points != nullptr) {
+                        marker_points->col(k) = p;
+                    }
+                }
             }
         }
 //    return err.maxCoeff();
@@ -155,7 +165,7 @@ namespace vis_marker_detector_impl {
 
     std::optional<Eigen::Isometry3f>
     match_marker(const Eigen::Matrix3Xf &points, const Eigen::Matrix3Xf &marker,
-                 float *err = nullptr, float max_tolerance = 1.f) {
+                 float max_tolerance = 1.f) {
         assert(marker.cols() >= 3);
         if (points.cols() < marker.cols()) return {};
 
@@ -171,11 +181,7 @@ namespace vis_marker_detector_impl {
                     if (abs((p2 - p1).norm() - dis3) > max_tolerance) [[likely]] continue;
                     Eigen::Matrix3f p_rep;
                     p_rep << p0, p1, p2;
-                    auto trans = Eigen::Isometry3f{Eigen::umeyama(m_rep, p_rep)};
-                    if (err != nullptr) {
-                        *err = calc_transform_error(points, marker, trans);
-                    }
-                    return trans;
+                    return Eigen::Isometry3f{Eigen::umeyama(m_rep, p_rep, false)};
                 }
             }
         return {};
@@ -254,7 +260,6 @@ namespace vis_marker_detector_impl {
         // accept 8-bit (CV_8UC1) or 12-bit (CV_16UC1)
         void detect(const cv::cuda::GpuMat &in, const cv::Mat &in_host,
                     Eigen::Matrix2Xf *out, const options &opt) {
-            constexpr auto block_width = 16;
             constexpr auto lm_border = 1;
             auto refine_criteria = cv::TermCriteria{
                     cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 10, 0.001};
@@ -274,7 +279,7 @@ namespace vis_marker_detector_impl {
                     call_corner_score(in_img, score_img,
                                       kernels->to_kernel_bunch(),
                                       kernels->kernel_weight_dev.ptr,
-                                      block_width, stream.cuda);
+                                      opt.cuda_block_width, stream.cuda);
                     break;
                 }
                 case CV_16U: {
@@ -282,7 +287,7 @@ namespace vis_marker_detector_impl {
                     call_corner_score(in_img, score_img,
                                       kernels->to_kernel_bunch(),
                                       kernels->kernel_weight_dev.ptr,
-                                      block_width, stream.cuda);
+                                      opt.cuda_block_width, stream.cuda);
                     break;
                 }
                 default: {
@@ -298,7 +303,7 @@ namespace vis_marker_detector_impl {
             auto filter_result = filter_result_type<ushort2>{
                     opt.max_corners, corner_index.ptr, corner_coord.ptr};
             call_lm_filter(score_img, filter_result,
-                           opt.score_threshold, lm_border, block_width, stream.cuda);
+                           opt.score_threshold, lm_border, opt.cuda_block_width, stream.cuda);
             uint32_t corner_count = 0;
             CUDA_API_CHECK(cudaMemcpyAsync(&corner_count, corner_index.ptr,
                                            sizeof(uint32_t), cudaMemcpyDeviceToHost, stream.cuda));
@@ -348,6 +353,7 @@ struct vis_marker_detector::impl {
     corner_detector detector_left, detector_right;
     Eigen::Matrix2Xf corner_left, corner_right;
     Eigen::Matrix3Xf space_points;
+    Eigen::Matrix3Xf marker_points; // origin position of matched marker points
 
     std::optional<Eigen::Isometry3f>
     detect(smart_mat *left_img, smart_mat *right_img,
@@ -387,9 +393,25 @@ struct vis_marker_detector::impl {
         undistort_points(*cam_info.left, &corner_left);
         undistort_points(*cam_info.right, &corner_right);
         match_points(corner_left, corner_right, cam_info, &space_points, opt.pixel_tolerance);
-        return match_marker(space_points, marker, err, opt.space_tolerance);
+        auto trans = match_marker(space_points, marker, opt.space_tolerance);
+
+        // calculate error
+        if (!trans.has_value()) return {};
+        auto trans_err = calc_transform_error(space_points, marker, trans.value(), &marker_points);
+        if (err != nullptr) {
+            *err = trans_err;
+        }
+        return trans;
     }
 
+    nlohmann::json save_status() const {
+        nlohmann::json ret_j;
+        ret_j["corner_left"] = matrix_to_json(corner_left);
+        ret_j["corner_right"] = matrix_to_json(corner_right);
+        ret_j["space_points"] = matrix_to_json(space_points);
+        ret_j["marker_points"] = matrix_to_json(marker_points);
+        return ret_j;
+    }
 };
 
 vis_marker_detector::vis_marker_detector()
@@ -402,4 +424,16 @@ vis_marker_detector::detect(smart_mat *left_img, smart_mat *right_img,
                             const stereo_camera_info &cam_info, const Eigen::Matrix3Xf &marker,
                             const vis_marker_detector::options &opt, float *err) {
     return pimpl->detect(left_img, right_img, cam_info, marker, opt, err);
+}
+
+void vis_marker_detector::get_space_points(Eigen::Matrix3Xf *points) {
+    *points = pimpl->space_points;
+}
+
+void vis_marker_detector::get_marker_points(Eigen::Matrix3Xf *points) {
+    *points = pimpl->marker_points;
+}
+
+nlohmann::json vis_marker_detector::status_as_json() {
+    return pimpl->save_status();
 }

src/vis_marker_detector.h

@@ -6,6 +6,8 @@
 
 #include <Eigen/Geometry>
 
+#include <nlohmann/json.hpp>
+
 #include <memory>
 #include <optional>
 
@@ -21,7 +23,7 @@ public:
         uint8_t detect_kernel_number = 6;
         float score_threshold = 0.3;
         float pixel_tolerance = 5.0;
-        float space_tolerance = 1.0;
+        float space_tolerance = 1.5;
         uint8_t cuda_block_width = 16;
     };
 
@@ -30,6 +32,12 @@ public:
            const stereo_camera_info &cam_info, const Eigen::Matrix3Xf &marker,
            const options &opt, float *err = nullptr);
 
+    void get_space_points(Eigen::Matrix3Xf *points);
+
+    void get_marker_points(Eigen::Matrix3Xf *points);
+
+    nlohmann::json status_as_json();
+
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;

src/vtk_viewer.h

@@ -45,6 +45,7 @@ public:
 
     Eigen::Isometry3d get_camera_pose();
 
+    // camera observed in world
     void set_camera_pose(const Eigen::Isometry3d &trans);
 
     void reset_camera();