update

beacon_ros2/config/rviz.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 380
+    Tree Height: 364
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -62,7 +62,7 @@ Visualization Manager:
         Value: /left_image
       Value: true
     - Class: rviz_default_plugins/Image
-      Enabled: true
+      Enabled: false
       Max Value: 1
       Median window: 5
       Min Value: 0
@@ -74,6 +74,34 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /right_image
+      Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /stereo_match
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /mono_match
       Value: true
   Enabled: true
   Global Options:
@@ -144,12 +172,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 926
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000292000002b4fc020000000afb0000000a0049006d006100670065010000003b000001690000002800fffffffb0000000a0049006d00610067006501000001aa000001450000002800fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000001a100000205000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000036b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000218000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000029200000304fc020000000cfb0000000a0049006d006100670065010000003b0000010e0000002800fffffffb0000000a0049006d006100670065000000012f000001040000002800fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007302000002b2000001b100000340000001f7fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000014f000001020000002800fffffffb0000000a0049006d0061006700650100000257000000e80000002800ffffff000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000036b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002180000030400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -159,5 +187,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1200
-  X: 79
-  Y: 97
+  X: 71
+  Y: 38

beacon_ros2/include/beacon_ros2/offline_process.h

@@ -35,6 +35,10 @@ class PohangStereoImageLoader : public rclcpp::Node {
                           std::placeholders::_2));
     }
 
+    ~PohangStereoImageLoader() {
+        m_reader.close();
+    }
+
   protected:
     void loadData() {
         constexpr int batch_size{100};

beacon_ros2/include/beacon_ros2/stereo_odometry_offline.h

@@ -26,6 +26,7 @@ class StereoOdometryOffline
   protected:
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_left_img_pub{};
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_right_img_pub{};
+    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_mono_img_pub{};
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_stereo_img_pub{};
 
   protected:

beacon_ros2/src/stereo_odometry_offline.cpp

@@ -14,6 +14,8 @@ StereoOdometryOffline::StereoOdometryOffline(const std::string& path_to_bag,
         this->create_publisher<sensor_msgs::msg::Image>("left_image", 10000);
     m_right_img_pub =
         this->create_publisher<sensor_msgs::msg::Image>("right_image", 10000);
+    m_mono_img_pub =
+        this->create_publisher<sensor_msgs::msg::Image>("mono_match", 10000);
     m_stereo_img_pub =
         this->create_publisher<sensor_msgs::msg::Image>("stereo_match", 10000);
 }
@@ -35,7 +37,6 @@ void StereoOdometryOffline::runImpl(StereoImageMsg msg) {
     display_left.header = left->header;
     display_left.encoding = left->encoding;
     m_left_img_pub->publish(*display_left.toImageMsg());
-    // m_left_img_pub->publish(*left->toImageMsg());
 
     cv_bridge::CvImage display_right{};
     display_right.image = display_imgs.right_image;
@@ -43,10 +44,16 @@ void StereoOdometryOffline::runImpl(StereoImageMsg msg) {
     display_right.encoding = right->encoding;
     m_right_img_pub->publish(*display_right.toImageMsg());
 
+    cv_bridge::CvImage display_mono{};
+    display_mono.image = display_imgs.monocular_match;
+    display_mono.header = left->header;
+    display_mono.encoding = left->encoding;
+    m_mono_img_pub->publish(*display_mono.toImageMsg());
+
     cv_bridge::CvImage display_stereo{};
     display_stereo.image = display_imgs.stereo_match;
-    display_stereo.header = left->header;
-    display_stereo.encoding = left->encoding;
+    display_stereo.header = right->header;
+    display_stereo.encoding = right->encoding;
     m_stereo_img_pub->publish(*display_stereo.toImageMsg());
 }