Implemented UDP hole punch.

data/config.yaml

@@ -4,21 +4,21 @@ camera:
     right: RightEye
   intrinsic:
     left:
-      fx: 3572.10386197042
-      fy: 3570.68084102920
-      cx: 1229.80492150397
-      cy: 1023.20068686429
-      k0: -0.0596384363017306
-      k1: 0.0479007910422696
+      fx: 3571.52848771264
+      fy: 3570.57318451437
+      cx: 1233.52703624453
+      cy: 1025.15117581285
+      k0: -0.0579298417960705
+      k1: 0.0537095025162189
       width: 2448
       height: 2048
     right:
-      fx: 3581.94172450578
-      fy: 3580.63177534790
-      cx: 1220.42154859988
-      cy: 1030.02704801102
-      k0: -0.0766755999523193
-      k1: 0.177092451010692
+      fx: 3573.03298854121
+      fy: 3572.30496881714
+      cx: 1223.80603997798
+      cy: 1035.75801617604
+      k0: -0.0736243464203426
+      k1: 0.126764214115614
       width: 2448
       height: 2048
   capture:

data/config_pelvic.yaml

@@ -0,0 +1,94 @@
+camera:
+  names:
+    left: LeftEye
+    right: RightEye
+  intrinsic:
+    left:
+      fx: 3571.52848771264
+      fy: 3570.57318451437
+      cx: 1233.52703624453
+      cy: 1025.15117581285
+      k0: -0.0579298417960705
+      k1: 0.0537095025162189
+      width: 2448
+      height: 2048
+    right:
+      fx: 3573.03298854121
+      fy: 3572.30496881714
+      cx: 1223.80603997798
+      cy: 1035.75801617604
+      k0: -0.0736243464203426
+      k1: 0.126764214115614
+      width: 2448
+      height: 2048
+  capture:
+    frame_rate: 40
+    expo_time_ms: 12
+    gain_db: 20
+
+main_window:
+  width: 800
+  height: 600
+
+output:
+  width: 1920
+  height: 1080
+  hevc_bitrate: 5 # Mbps
+  jpeg_quality: 15
+
+sender:
+  mtu: 1400
+  port: 5279
+  parity: 0.2
+
+sophiar:
+  config: ./sophiar_config.json
+  left_camera_trans_var: left_camera_in_tracker_denoised
+  right_camera_trans_var: right_camera_in_tracker_denoised
+  probe_model: ./models/Probe.stl
+  augment:
+    #    - name: Femur
+    #      trans_var: femur_in_tracker_denoised
+    #      stl_file: ./models/femur.stl
+    #      registration:
+    #        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
+    #    - name: Tibia
+    #      trans_var: tibia_in_tracker_denoised
+    #      stl_file: ./models/tibia.stl
+    #      registration:
+    #        collect_obj: point_picker_in_tibia_ref
+    #        collect_var: picked_point_in_tibia_ref
+    #        target_var: tibia_in_tibia_ref
+    #        probe_var: probe_in_tibia
+    - name: BoneA
+      trans_var: tibia_in_tracker_denoised
+      stl_file: ./models/pelvic/1.stl
+      registration:
+        collect_obj: point_picker_in_tibia_ref
+        collect_var: picked_point_in_tibia_ref
+        target_var: tibia_in_tibia_ref
+        probe_var: probe_in_tibia
+    - name: BoneB
+      trans_var: tibia_in_tracker_denoised
+      stl_file: ./models/pelvic/0.stl
+    - name: BoneC
+      trans_var: tibia_in_tracker_denoised
+      stl_file: ./models/pelvic/_Merge_of_screw0_001.stl
+#    - name: BoneD
+#      trans_var: tibia_in_tracker_denoised
+#      stl_file: ./models/pelvic/3.stl
+#    - name: BoneE
+#      trans_var: tibia_in_tracker_denoised
+#      stl_file: ./models/pelvic/4.stl
+#    - name: ScrewA
+#      trans_var: tibia_in_tracker_denoised
+#      stl_file: ./models/pelvic/screw0.stl
+#    - name: ScrewB
+#      trans_var: tibia_in_tracker_denoised
+#      stl_file: ./models/pelvic/screw1.stl
+#    - name: ScrewC
+#      trans_var: tibia_in_tracker_denoised
+#      stl_file: ./models/pelvic/screw2.stl

data/sophiar_config.json

@@ -29,13 +29,13 @@
       "name": "tibia_in_tibia_ref",
       "type": "transform_obj",
       "value": [
-        -3.91566,
-        -85.3472,
-        -42.0667,
-        0.360607,
-        0.661265,
-        -0.283426,
-        -0.5936
+        -62.500806571785446,
+        413.39747324976304,
+        -167.02739487371574,
+        -0.28854626194860417,
+        0.4854199096619968,
+        -0.2804163071762634,
+        0.7761927986582033
       ]
     },
     {
@@ -187,26 +187,26 @@
             "name": "left_camera",
             "parent": "camera_ref",
             "transform": [
-              91.0962,
-              35.5978,
-              -25.9658,
-              0.6130,
-              -0.3530,
-              0.6086,
-              0.3595
+              1.2530,
+              36.5557,
+              -89.9613,
+              0.6107,
+              -0.3547,
+              -0.6114,
+              -0.3569
             ]
           },
           {
             "name": "right_camera",
             "parent": "camera_ref",
             "transform": [
-              90.1815,
-              35.1063,
-              -91.4749,
-              0.6089,
-              -0.3537,
-              0.6133,
-              0.3577
+              1.3691,
+              35.9684,
+              -24.6520,
+              0.6149,
+              -0.3536,
+              -0.6076,
+              -0.3574
             ]
           }
         ]
@@ -260,7 +260,7 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "ethernet",
+        "address_type": "serial",
         "ip": "10.0.0.5",
         "tcp_port": 8765,
         "com_port": "/dev/ttyUSB0",

src/frame_sender/sender_tcp.cpp

@@ -6,6 +6,7 @@
 #include <boost/asio/io_context.hpp>
 #include <boost/asio/ip/tcp.hpp>
 #include <boost/asio/post.hpp>
+#include <boost/asio/read.hpp>
 #include <boost/asio/write.hpp>
 
 #include <spdlog/spdlog.h>
@@ -16,6 +17,7 @@ using namespace boost::asio::ip;
 using boost::asio::buffer;
 using boost::asio::io_context;
 using boost::asio::post;
+using boost::asio::read;
 using boost::asio::write;
 using boost::system::error_code;
 
@@ -69,11 +71,50 @@ struct sender_tcp::impl {
                      frame->frame_id, frame->length);
     }
 
+//    void hole_punch(const tcp::endpoint &listen_ep) { // UDP hole punch
+//        static constexpr auto fake_stun_server = "38.59.254.192"; // TODO: setting in config file
+//        static constexpr auto fake_stun_port = 5281;
+//        auto fake_stun_ep = tcp::endpoint{
+//                address::from_string(fake_stun_server), fake_stun_port};
+//
+//        socket = std::make_unique<tcp::socket>(*q_this->get_ctx());
+//        socket->open(tcp::v4());
+//        socket->set_option(boost::asio::socket_base::reuse_address{true});
+//        socket->bind(listen_ep);
+//        socket->connect(fake_stun_ep);
+//
+//        static constexpr auto hole_punch_request_len =
+//                sizeof(size_t) + 2 * sizeof(uint8_t);
+//        out_buf.create(hole_punch_request_len);
+//        auto ptr = write_binary_number(out_buf.ptr, (size_t) (2 * sizeof(uint8_t)));
+//        ptr = write_binary_number(ptr, 'R');
+//        ptr = write_binary_number(ptr, 'S');
+//        auto pkt_len = ptr - out_buf.ptr;
+//        assert(pkt_len == hole_punch_request_len);
+//        socket->send(buffer(out_buf.ptr, pkt_len));
+//
+//        smart_buffer<uint8_t> in_buf;
+//        size_t rep_len;
+//        in_buf.create(sizeof(rep_len));
+//        read(*socket, buffer(in_buf.ptr, in_buf.length));
+//        read_binary_number(in_buf.ptr, &rep_len);
+//        in_buf.create(rep_len);
+//        read(*socket, buffer(in_buf.ptr, in_buf.length));
+//        assert(rep_len == 2);
+//        assert(std::string_view((char *) in_buf.ptr, 2) == "OK");
+//        SPDLOG_INFO("TCP hole punch succeeded.");
+//
+//        socket.reset();
+//    }
+
     void async_waiting_client() {
+        socket = std::make_unique<tcp::socket>(*q_this->get_ctx());
         acceptor->async_accept(*socket, [this](error_code err) {
             if (err) {
-                SPDLOG_ERROR("Error while accepting client: {}", err.to_string());
+                SPDLOG_ERROR("Error while accepting client: {}", err.what());
+                assert(false);
                 async_waiting_client();
+                return;
             }
             assert(socket->is_open());
             q_this->request_idr_frame();
@@ -89,8 +130,13 @@ struct sender_tcp::impl {
         auto ret = new impl;
         ret->q_this = q_this;
         auto listen_ep = tcp::endpoint{tcp::v4(), listen_port};
+//        ret->hole_punch(listen_ep);
         ret->acceptor = std::make_unique<tcp::acceptor>(*q_this->get_ctx(), listen_ep);
-        ret->socket = std::make_unique<tcp::socket>(*q_this->get_ctx());
+//        ret->acceptor = std::make_unique<tcp::acceptor>(*q_this->get_ctx());
+//        ret->acceptor->open(tcp::v4());
+//        ret->acceptor->set_option(boost::asio::socket_base::reuse_address{true});
+//        ret->acceptor->bind(listen_ep);
+//        ret->acceptor->listen();
         ret->async_waiting_client();
         return ret;
     }

src/frame_sender/sender_udp_fec.cpp

@@ -185,7 +185,7 @@ struct sender_udp_fec::impl {
     }
 
     void send_frame(frame_ptr_type &&frame) {
-        if (remote_ep == nullptr) return;
+        if (!is_connected()) return;
 
         frag_header header;
         header.frame_type = frame->idr ? 'I' : 'P';
@@ -207,6 +207,27 @@ struct sender_udp_fec::impl {
         SPDLOG_TRACE("Frame {} sent.", frame->frame_id);
     }
 
+    void hole_punch() { // UDP hole punch
+        static constexpr auto fake_stun_server = "38.59.254.192"; // TODO: setting in config file
+        static constexpr auto fake_stun_port = 5281;
+        auto fake_stun_ep = udp::endpoint{
+                address::from_string(fake_stun_server), fake_stun_port};
+
+        out_buf.create(2 * sizeof(uint8_t));
+        auto ptr = write_binary_number(out_buf.ptr, 'R');
+        ptr = write_binary_number(ptr, 'S');
+        auto pkt_len = ptr - out_buf.ptr;
+        socket->send_to(buffer(out_buf.ptr, pkt_len), fake_stun_ep);
+
+        udp::endpoint reply_ep;
+        in_buf.create(max_package_size);
+        auto rep_len = socket->receive_from(buffer(in_buf.ptr, in_buf.length), reply_ep);
+        assert(reply_ep == fake_stun_ep);
+        assert(rep_len == 2);
+        assert(std::string_view((char *) in_buf.ptr, 2) == "OK");
+        SPDLOG_INFO("UDP hole punch succeeded.");
+    }
+
     void async_handle_request() {
         in_buf.create(max_package_size);
         auto buf = buffer(in_buf.ptr, max_package_size);
@@ -293,6 +314,7 @@ struct sender_udp_fec::impl {
         ret->q_this = q_this;
         ret->socket = std::make_unique<udp::socket>(*q_this->get_ctx(), local_ep);
         ret->socket->set_option(udp::socket::send_buffer_size{udp_buffer_size});
+        ret->hole_punch();
         ret->async_handle_request();
 
         // constant configs

src/image_process.cpp

@@ -200,6 +200,7 @@ namespace process_impl {
         constexpr uint2 grid_dim = {8, 128};
         float ps = 2 * range.height / height;
         uint32_t width = 2 * range.width / ps;
+        width = (width + 3) & (-4); // make OpenGL happy
         resample_info info{};
         info.x = -range.width;
         info.y = -range.height;

src/main_ext.cpp

@@ -319,7 +319,7 @@ void load_config() {
                 item["name"].as<std::string>(),
                 item["trans_var"].as<std::string>(),
                 create_actor(item["stl_file"].as<std::string>()));
-        if (auto reg_conf = item["registration"]; !reg_conf.IsNull()) {
+        if (auto reg_conf = item["registration"]; reg_conf) {
             reg_targets.emplace_back(
                     item["name"].as<std::string>(),
                     item["stl_file"].as<std::string>(),