<fix> 1. optimize dense depth demo, now one-to-one with image_undistort

2. fix per-point time_offset for raw point cloud data 3. optimize how data rx fps is calculated 4. optimize log file structure
2025-10-21 11:03:31 +08:00
parent bd2f80d82d
commit 28c17e5611
17 changed files with 1084 additions and 290 deletions
@@ -161,6 +161,7 @@ if(ROS_VERSION STREQUAL "ROS1")
        src/host_sdk_sample.cpp
        src/yaml_parser.cpp
        src/rawCloudRender.cpp
+        src/camera_pose_visualization.cpp
    )
    target_link_libraries(host_sdk_sample
        ${catkin_LIBRARIES}
@@ -218,6 +219,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
    find_package(std_msgs REQUIRED)
    find_package(sensor_msgs REQUIRED)
    find_package(nav_msgs REQUIRED)
+    find_package(visualization_msgs REQUIRED)
    find_package(cv_bridge REQUIRED)
    find_package(image_transport REQUIRED)
    find_package(pcl_conversions REQUIRED)
@@ -228,6 +230,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        src/host_sdk_sample.cpp
        src/yaml_parser.cpp  
        src/rawCloudRender.cpp
+        src/camera_pose_visualization.cpp
        )
    
    # Link libraries
@@ -244,6 +247,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        std_msgs 
        sensor_msgs 
        nav_msgs 
+        visualization_msgs
        cv_bridge
        image_transport
    )
@@ -339,6 +343,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        std_msgs 
        sensor_msgs 
        nav_msgs 
+        visualization_msgs
        cv_bridge
        image_transport
        pcl_conversions
@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application

 ## 1. Version

-Current Version: v0.5.1
+Current Version: v0.5.2

 ## 2. Preparation

@@ -178,7 +178,7 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
        pointcloud_depth_converter.hpp // pointcloud_depth_convert
    config/
        control_command.yaml    // Control parameter file for driver
-        calib.yaml              //Machine calibration yaml
+        calib.yaml              // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver
    launch_ROS1/
        odin1_ros1.launch       // ROS1 launch file
    launch_ROS2/
@@ -188,6 +188,9 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
        build_ros2.sh           // Installation script for ROS2
    recorddata/                 // holds recorded data that can import into MindCloud
    log/                        // holds log files
+        Driver_{timestamp}/     // holds all log folders for each time driver started
+            Conn_{timestamp}/   // holds all log files for each odin1 device connection
+                dev_status.csv  // device status log file
    README.md                   // Usage instructions
    CMakeLists.txt              // CMake build file
    License                     // License file
@@ -204,17 +207,17 @@ Internal parameters of the Odin ROS driver are defined in config/control_command

 | Topic               |control_command.yaml  | Detailed Description |
 |---------------------|----------------------|----------------------|
-| odin1/imu           | sendimu  | Imu Topic |
-| odin1/image         | sendrgb  | RGB Camera Topic, decoded from jpeg data from device |
-| odin1/image_undistort  | sendrgbundistort  | undistorted RGB Camera Topic, processed with calib.yaml from device |
-| odin1/image/compressed | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device |
-| odin1/cloud_raw        | senddtof          | Raw_Cloud Topic |
-| odin1/cloud_render     | sendcloudrender  | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device |
-| odin1/cloud_slam    | sendcloudslam    | Slam_PointCloud Topic |
-| odin1/odometry      | sendodom         | Odom Topic |
-| odin1/odometry_high | sendodom         | high frequency Odom Topic |
-| odin1/depth_img_competetion        | senddepth | Dense depth image Topic, demo, high computing power required |
-| odin1/depth_img_competetion_cloud  | senddepth | Dense Depth_Cloud Topic, demo, high computing power required |
+| odin1/imu                     | sendimu           | Imu Topic |
+| odin1/image                   | sendrgb           | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format |
+| odin1/image_undistort         | sendrgbundistort  | undistorted RGB Camera Topic, processed with calib.yaml from device |
+| odin1/image/compressed        | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device |
+| odin1/cloud_raw               | senddtof          | Raw_Cloud Topic |
+| odin1/cloud_render            | sendcloudrender   | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device |
+| odin1/cloud_slam              | sendcloudslam     | Slam_PointCloud Topic |
+| odin1/odometry                | sendodom          | Odom Topic |
+| odin1/odometry_high           | sendodom          | high frequency Odom Topic |
+| odin1/depth_img_competetion   | senddepth         | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. |
+| odin1/depth_img_competetion_cloud  | senddepth         | Dense Depth_Cloud Topic. Demo, high computing power required |

 ### 4.4 Data format

@@ -225,7 +228,7 @@ float32 y             // Y axis, in meters
 float32 z             // Z axis, in meters
 uint8  intensity      // Reflectivity, range 0–255
 uint16 confidence     // Point confidence, range 0–65535
-float32 offset_time   // Time offset relative to the base timestamp 
+float32 offset_time   // Time offset relative to the base timestamp unit: s 
 ```

 To work with this custom format in PCL, first define the point type:
@@ -257,6 +260,15 @@ Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL p
 pcl::PointCloud<ls_ros::Point> ls_cloud;
 pcl::fromROSMsg(*msg, ls_cloud);
 ```
+
+2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields:
+```
+float32 x             // X axis, in meters
+float32 y             // Y axis, in meters
+float32 z             // Z axis, in meters
+float32 rgb           // RGB value
+```
+
 ### 4.5 Other functionalities

 |control_command.yaml   | Detailed Description |
@@ -368,6 +380,23 @@ export ROS_LOCALHOST_ONLY=1

 If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended.

+### 5.9 ROS Driver died immediately after stream started
+
+**Error Message**  
+
+```shell
+Device ready and streams activated
+[host_sdk_sample-2] process has died ......
+```
+
+**Test**
+
+Disable odin1/image	with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system.
+
+**Resolution** 
+
+Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again.
+
 ## 6.  Contact Information
 To help diagnose the issue, please provide the following details to our FAE engineer:

@@ -11,4 +11,6 @@ register_keys:
  sendrgbundistort: 0   # 0: off; 1: on
  recorddata: 0         # 0: off; 1: on
  devstatuslog: 1       # 0: off; 1: on
-
+  pubintensitygray: 0   # 0: off; 1: on
+  showpath: 0           # 0: off; 1: on
+  showcamerapose: 0     # 0: off; 1: on
@@ -1,14 +1,14 @@
 Panels:
  - Class: rviz/Displays
-    Help Height: 138
+    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
-        - /Odometry_high1/Shape1
+        - /Odometry1
        - /slam1
        - /dense_depth_demo1
      Splitter Ratio: 0.4993045926094055
-    Tree Height: 1052
+    Tree Height: 549
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@@ -26,7 +26,7 @@ Panels:
  - Class: rviz/Time
    Name: Time
    SyncMode: 0
-    SyncSource: raw
+    SyncSource: ""
 Preferences:
  PromptSaveOnExit: true
 Toolbars:
@@ -76,76 +76,6 @@ Visualization Manager:
      Transport Hint: raw
      Unreliable: false
      Value: false
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: false
-      Enabled: true
-      Keep: 1
-      Name: Odometry
-      Position Tolerance: 0.10000000149011612
-      Queue Size: 1
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Axes
-      Topic: /odin1/odometry
-      Unreliable: false
-      Value: true
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: false
-      Keep: 100
-      Name: Odometry_high
-      Position Tolerance: 0.10000000149011612
-      Queue Size: 10
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 52; 101; 164
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Arrow
-      Topic: /odin1/odometry_highfreq
-      Unreliable: false
-      Value: false
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
@@ -161,9 +91,7 @@ Visualization Manager:
      Enabled: false
      Invert Rainbow: false
      Max Color: 255; 255; 255
-      Max Intensity: 255
      Min Color: 0; 0; 0
-      Min Intensity: 0
      Name: raw
      Position Transformer: XYZ
      Queue Size: 10
@@ -204,6 +132,88 @@ Visualization Manager:
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: false
+          Enabled: true
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Queue Size: 1
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Axes
+          Topic: /odin1/odometry
+          Unreliable: false
+          Value: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Keep: 100
+          Name: Odometry_high
+          Position Tolerance: 0.10000000149011612
+          Queue Size: 10
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 52; 101; 164
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic: /odin1/odometry_highfreq
+          Unreliable: false
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: false
+          Marker Topic: /odin1/camera_pose_visual
+          Name: camera_view
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: false
+      Enabled: true
+      Name: Odometry
    - Class: rviz/Group
      Displays:
        - Alpha: 1
@@ -246,7 +256,7 @@ Visualization Manager:
          Color: 255; 255; 255
          Color Transformer: RGB8
          Decay Time: 5
-          Enabled: true
+          Enabled: false
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Min Color: 0; 0; 0
@@ -261,13 +271,21 @@ Visualization Manager:
          Unreliable: false
          Use Fixed Frame: true
          Use rainbow: true
-          Value: true
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: false
+          Marker Topic: /odin1/path
+          Name: path
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: false
      Enabled: false
      Name: slam
    - Class: rviz/Group
      Displays:
        - Class: rviz/Image
-          Enabled: false
+          Enabled: true
          Image Topic: /odin1/depth_img_competetion
          Max Value: 1
          Median window: 5
@@ -277,7 +295,7 @@ Visualization Manager:
          Queue Size: 2
          Transport Hint: raw
          Unreliable: false
-          Value: false
+          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
@@ -336,7 +354,7 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 7.223263740539551
+      Distance: 8.56683349609375
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
@@ -344,29 +362,29 @@ Visualization Manager:
        Value: false
      Field of View: 0.7853981852531433
      Focal Point:
-        X: -0.4511165916919708
-        Y: -0.1217871829867363
-        Z: 0.8345625996589661
-      Focal Shape Fixed Size: true
+        X: 1.4208989143371582
+        Y: -0.7709289789199829
+        Z: 0.5377280712127686
+      Focal Shape Fixed Size: false
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6153989434242249
+      Pitch: 0.8353985548019409
      Target Frame: <Fixed Frame>
-      Yaw: 3.0504050254821777
+      Yaw: 2.530402660369873
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1736
+  Height: 1016
  Hide Left Dock: false
  Hide Right Dock: false
  Image:
    collapsed: false
  Image_undistort:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000060afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000526000001210000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600ffffff000000010000015f0000060afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000060a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000af80000005efc0100000002fb0000000800540069006d0065010000000000000af8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007ce0000060a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000033afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000262000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002a5000000d20000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000001d5000000d70000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f0072007400000002690000010e0000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d00650100000000000007380000033700fffffffb0000000800540069006d006501000000000000045000000000000000000000040e0000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@@ -375,8 +393,8 @@ Window Geometry:
    collapsed: false
  Views:
    collapsed: false
-  Width: 2808
-  X: 72
+  Width: 1848
+  X: 57
  Y: 27
  dense_depth_image:
    collapsed: false
@@ -1,17 +1,17 @@
 Panels:
  - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /Image1/Topic1
-        - /Odometry_high1/Shape1
+        - /Odometry1
        - /slam1
        - /dense_depth_demo1
      Splitter Ratio: 0.5
-    Tree Height: 548
+    Tree Height: 667
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
@@ -74,84 +74,6 @@ Visualization Manager:
        Reliability Policy: Reliable
        Value: /odin1/image/undistorted
      Value: false
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz_default_plugins/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: true
-      Keep: 1
-      Name: Odometry
-      Position Tolerance: 0.10000000149011612
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Axes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/odometry
-      Value: true
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz_default_plugins/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: false
-      Keep: 10
-      Name: Odometry_high
-      Position Tolerance: 0.10000000149011612
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 98; 160; 234
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Arrow
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/odometry_highfreq
-      Value: false
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
@@ -220,6 +142,100 @@ Visualization Manager:
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/odometry
+          Value: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Keep: 10
+          Name: Odometry_high
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 98; 160; 234
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/odometry_highfreq
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: camera_view
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/camera_pose_visual
+          Value: false
+      Enabled: true
+      Name: Odometry
    - Class: rviz_common/Group
      Displays:
        - Alpha: 1
@@ -268,7 +284,7 @@ Visualization Manager:
          Color: 255; 255; 255
          Color Transformer: RGB8
          Decay Time: 10
-          Enabled: true
+          Enabled: false
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Max Intensity: 4096
@@ -289,13 +305,25 @@ Visualization Manager:
            Value: /odin1/cloud_slam
          Use Fixed Frame: true
          Use rainbow: true
-          Value: true
+          Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: path
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/path
+          Value: false
      Enabled: false
      Name: slam
    - Class: rviz_common/Group
      Displays:
        - Class: rviz_default_plugins/Image
-          Enabled: false
+          Enabled: true
          Max Value: 1
          Median window: 5
          Min Value: 0
@@ -307,7 +335,7 @@ Visualization Manager:
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /odin1/depth_img_competetion
-          Value: false
+          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
@@ -390,45 +418,45 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
-      Distance: 6.814720153808594
+      Distance: 9.397579193115234
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 0.38628554344177246
-        Y: -1.1808340549468994
-        Z: 1.2151938676834106
+        X: 1.1073570251464844
+        Y: 0.6017969250679016
+        Z: 0.4314231276512146
      Focal Shape Fixed Size: false
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5653982758522034
+      Pitch: 1.035398006439209
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
-      Yaw: 3.0103981494903564
+      Yaw: 2.960400104522705
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1029
+  Height: 1016
  Hide Left Dock: false
  Hide Right Dock: false
  Image:
    collapsed: false
  Image_undistort:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000242000003abfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f2000000f60000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000002cd0000011b0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000031d000000cb0000002800ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ab000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000423000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f50000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000031b000000c00000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004280000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
-  Width: 1920
-  X: 498
-  Y: 112
+  Width: 1848
+  X: 392
+  Y: 165
  dense_depth_image:
    collapsed: false
@@ -0,0 +1,78 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#pragma once
+
+#ifdef ROS2
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#else
+#include <ros/ros.h>
+#include <std_msgs/ColorRGBA.h>
+#include <std_msgs/Header.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#endif
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+class camera_pose_visualization {
+  public:
+    std::string m_marker_ns;
+
+    camera_pose_visualization(float r, float g, float b, float a);
+
+    void setImageBoundaryColor(float r, float g, float b, float a = 1.0);
+    void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);
+    void setScale(double s);
+    void setLineWidth(double width);
+
+    void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
+    void reset();
+
+    #ifdef ROS2
+    using ColorRGBA = std_msgs::msg::ColorRGBA;
+    using Marker = visualization_msgs::msg::Marker;
+    using MarkerArray = visualization_msgs::msg::MarkerArray;
+    using Header = std_msgs::msg::Header;
+    using Publisher = rclcpp::Publisher<MarkerArray>;
+    #else
+    using ColorRGBA = std_msgs::ColorRGBA;
+    using Marker = visualization_msgs::Marker;
+    using MarkerArray = visualization_msgs::MarkerArray;
+    using Header = std_msgs::Header;
+    using Publisher = ros::Publisher;
+    #endif
+
+    void publish_by(Publisher& pub, const Header& header);
+    void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+    void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+  private:
+    std::vector<Marker> m_markers;
+    ColorRGBA m_image_boundary_color;
+    ColorRGBA m_optical_center_connector_color;
+    double m_scale;
+    double m_line_width;
+
+    static const Eigen::Vector3d imlt;
+    static const Eigen::Vector3d imlb;
+    static const Eigen::Vector3d imrt;
+    static const Eigen::Vector3d imrb;
+    static const Eigen::Vector3d oc  ;
+    static const Eigen::Vector3d lt0 ;
+    static const Eigen::Vector3d lt1 ;
+    static const Eigen::Vector3d lt2 ;
+};
@@ -42,9 +42,11 @@ limitations under the License.
 #include <unistd.h>
 #include <sys/types.h>
 #include <sys/wait.h>
+#include <filesystem>

 #include <yaml-cpp/yaml.h>
 #include "polynomial_camera.hpp"
+#include "camera_pose_visualization.h"

 struct CameraParams {
    int width, height;
@@ -67,6 +69,7 @@ extern int g_sendcloudrender;
    #include "rclcpp/rclcpp.hpp"
    #include "std_msgs/msg/string.hpp"
    #include <std_msgs/msg/header.hpp>
+    #include <visualization_msgs/msg/marker_array.hpp>
    #include "sensor_msgs/msg/image.hpp"
    #include "sensor_msgs/msg/imu.hpp"
    #include "sensor_msgs/msg/point_cloud2.hpp"
@@ -74,12 +77,14 @@ extern int g_sendcloudrender;
    #include <sensor_msgs/msg/compressed_image.hpp>
    #include <builtin_interfaces/msg/time.hpp>
    #include <nav_msgs/msg/odometry.hpp>
+    #include <nav_msgs/msg/path.hpp>
    #include <sensor_msgs/msg/point_field.hpp>
    namespace ros {
        using namespace rclcpp;
        using namespace std_msgs::msg;
        using namespace sensor_msgs::msg;
        using namespace nav_msgs::msg;
+        using namespace visualization_msgs::msg;
        using Time = builtin_interfaces::msg::Time;
    }
    
@@ -99,7 +104,9 @@ extern int g_sendcloudrender;
    #include <sensor_msgs/point_cloud2_iterator.h>
    #include <sensor_msgs/CompressedImage.h>
    #include <nav_msgs/Odometry.h>
+    #include <nav_msgs/Path.h>
    #include <sensor_msgs/Image.h>
+    
    namespace ros {
        using namespace ::ros;
        using namespace sensor_msgs;
@@ -174,12 +181,13 @@ class MultiSensorPublisher {
 public:
    #ifdef ROS2
        MultiSensorPublisher(rclcpp::Node::SharedPtr node)
-            : node_(node) {
+            : node_(node),cameraposevisual_ {1.0f, 0.0f, 0.0f, 1.0f} {
            initialize_publishers();
            // initialize_data_logger();
        }
    #else
-        MultiSensorPublisher(ros::NodeHandle& nh) {
+        MultiSensorPublisher(ros::NodeHandle& nh)
+            : cameraposevisual_(1.0f, 0.0f, 0.0f, 1.0f) {
            initialize_publishers(nh);
            // initialize_data_logger();
        }
@@ -573,7 +581,8 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                *iter_confidence = confidence_data[i]; ++iter_confidence;
                
                if (dtof_subframe_odr > 0.0) {
-                    int group = i / DTOF_NUM_ROW_PER_GROUP;
+                    int line_num = i / 256;
+                    int group = line_num / DTOF_NUM_ROW_PER_GROUP;
                    float timestamp_offset = group * 1.0 / dtof_subframe_odr;

                    *iter_offsettime = timestamp_offset;
@@ -634,6 +643,33 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
    #endif
 }

+    void publishGrayUInt8(capture_Image_List_t *stream, int idx) {
+        ImageMsg msg;
+        msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp);
+        msg.header.frame_id = "map";
+
+        int width = stream->imageList[idx].width;
+        int height = stream->imageList[idx].height;
+
+        msg.height = height;
+        msg.width = width;
+        msg.encoding = "mono8";
+        msg.is_bigendian = false;
+        msg.step = width * sizeof(uint8_t);
+
+        size_t image_size = msg.step * height;
+
+        msg.data.resize(image_size);
+
+        memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size);
+
+        #ifdef ROS2
+            intensity_gray_pub_->publish(msg);
+        #else
+            intensity_gray_pub_.publish(msg);
+        #endif
+    }
+
    void publishRgb(capture_Image_List_t *stream) {
    buffer_List_t &image = stream->imageList[0];

@@ -991,7 +1027,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
 #endif
    }

-    void publishOdometry(capture_Image_List_t* stream, bool is_highfreq) {
+    void publishOdometry(capture_Image_List_t* stream, bool is_highfreq, bool show_path, bool show_camerapose) {
        
 #ifdef ROS2
            auto msg = nav_msgs::msg::Odometry();
@@ -1080,12 +1116,123 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                odom_highfreq_publisher_->publish(std::move(msg));
            } else {
                odom_publisher_->publish(std::move(msg));
+
+                // Publish odom trajectory as visualization markers (green lines connecting adjacent points)
+                static visualization_msgs::msg::Marker marker;
+                static std::vector<geometry_msgs::msg::Point> path_points;
+                
+                if (show_path) {
+                    marker.header = msg.header;
+                    marker.ns = "odom_trajectory";
+                    marker.id = 0;
+                    marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+                    marker.action = visualization_msgs::msg::Marker::ADD;
+                    marker.pose.orientation.w = 1.0;
+                    marker.scale.x = 0.02;  // Line width
+                    marker.color.r = 0.0;
+                    marker.color.g = 1.0;
+                    marker.color.b = 0.0;
+                    marker.color.a = 1.0;
+    
+                    geometry_msgs::msg::Point pt;
+                    pt.x = msg.pose.pose.position.x;
+                    pt.y = msg.pose.pose.position.y;
+                    pt.z = msg.pose.pose.position.z;
+                    path_points.push_back(pt);
+                    
+                    // Keep only recent points to avoid memory issues (e.g., last 1000 points)
+                    if (path_points.size() > 30000) {
+                        path_points.erase(path_points.begin());
+                    }
+                    
+                    marker.points = path_points;
+    
+                    // Publish marker array
+                    static visualization_msgs::msg::MarkerArray marker_array;
+                    marker_array.markers.clear();  // Clear previous markers
+                    marker_array.markers.push_back(marker);
+                    path_publisher_->publish(marker_array);
+                }
+
+                if (show_camerapose) {
+                    // camera pose visualization (ROS2)
+                    Eigen::Vector3d P(msg.pose.pose.position.x,
+                                    msg.pose.pose.position.y,
+                                    msg.pose.pose.position.z);
+                    Eigen::Quaterniond R(msg.pose.pose.orientation.w,
+                                        msg.pose.pose.orientation.x,
+                                        msg.pose.pose.orientation.y,
+                                        msg.pose.pose.orientation.z);
+                    if (extrinsic_ok_) {
+                        P = P + R * t_ic_;
+                        R = R * R_ic_;
+                    }
+                    cameraposevisual_.reset();
+                    cameraposevisual_.add_pose(P, R);
+                    cameraposevisual_.publish_by(*pub_camera_pose_visual_, msg.header);
+                }
            }
 #else
            if (is_highfreq) {
                odom_highfreq_publisher_.publish(msg);
            } else {
                odom_publisher_.publish(msg);
+
+                if (show_path) {
+                    // Publish odom trajectory as visualization markers (green lines connecting adjacent points)
+                    static visualization_msgs::Marker marker;
+                    static std::vector<geometry_msgs::Point> path_points;
+                    
+                    marker.header = msg.header;
+                    marker.ns = "odom_trajectory";
+                    marker.id = 0;
+                    marker.type = visualization_msgs::Marker::LINE_STRIP;
+                    marker.action = visualization_msgs::Marker::ADD;
+                    marker.pose.orientation.w = 1.0;
+                    marker.scale.x = 0.02;  // Line width
+                    marker.color.r = 0.0;
+                    marker.color.g = 1.0;
+                    marker.color.b = 0.0;
+                    marker.color.a = 1.0;
+
+                    geometry_msgs::Point pt;
+                    pt.x = msg.pose.pose.position.x;
+                    pt.y = msg.pose.pose.position.y;
+                    pt.z = msg.pose.pose.position.z;
+                    path_points.push_back(pt);
+                    
+                    // Keep only recent points to avoid memory issues (e.g., last 1000 points)
+                    if (path_points.size() > 30000) {
+                        path_points.erase(path_points.begin());
+                    }
+                    
+                    marker.points = path_points;
+
+                    // Publish marker array
+                    static visualization_msgs::MarkerArray marker_array;
+                    marker_array.markers.clear();  // Clear previous markers
+                    marker_array.markers.push_back(marker);
+                    path_publisher_.publish(marker_array);
+                }
+
+                if (show_camerapose) {
+                    // camera pose visualization (ROS1)
+                    Eigen::Vector3d P(msg.pose.pose.position.x,
+                                    msg.pose.pose.position.y,
+                                    msg.pose.pose.position.z);
+                    Eigen::Quaterniond R(msg.pose.pose.orientation.w,
+                                        msg.pose.pose.orientation.x,
+                                        msg.pose.pose.orientation.y,
+                                        msg.pose.pose.orientation.z);
+                        
+                    if (extrinsic_ok_) {
+                        P = P + R * t_ic_;
+                        R = R * R_ic_;
+                    }
+                    cameraposevisual_.reset();
+                    cameraposevisual_.add_pose(P, R);
+                    cameraposevisual_.publish_by(pub_camera_pose_visual_, msg.header);
+                }
            }
 #endif
    }
@@ -1153,6 +1300,25 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                m_camera_params.fx, m_camera_params.fy, m_camera_params.cx, m_camera_params.cy, m_camera_params.skew,
                m_camera_params.k2, m_camera_params.k3, m_camera_params.k4, m_camera_params.k5, m_camera_params.k6, m_camera_params.k7);

+            // Load extrinsic Tcl_0 (camera to lidar)
+            extrinsic_ok_ = false;
+            if (config["Tcl_0"]) {
+                auto T = config["Tcl_0"];
+                if (T.IsSequence() && T.size() == 16) {
+                    Eigen::Matrix4d Tcl;
+                    for (int i = 0; i < 16; ++i) Tcl(i/4, i%4) = T[i].as<double>();
+                    T_cl_ = Tcl;
+                }
+                std::cout << "Tcl: " << T_cl_ << std::endl;
+
+                Eigen::Matrix4d Tic = T_il_ * T_cl_.inverse();
+                Eigen::Matrix3d Ric = Tic.block<3,3>(0,0);
+                Eigen::Vector3d tic = Tic.block<3,1>(0,3);
+                R_ic_ = Eigen::Quaterniond(Ric);
+                t_ic_ = tic;
+                extrinsic_ok_ = true;
+            }
+
            m_cam_init_success = true;
            return 0;
        } catch (const std::exception& e) {
@@ -1235,6 +1401,15 @@ private:
    bool m_undistort_map_init_success = false;
    cv::Mat m_undistort_map_x;
    cv::Mat m_undistort_map_y;
+    Eigen::Quaterniond R_ic_ {Eigen::Quaterniond::Identity()};
+    Eigen::Vector3d t_ic_ {Eigen::Vector3d::Zero()};
+    bool extrinsic_ok_ {false};
+    Eigen::Matrix4d T_il_ = (Eigen::Matrix4d() <<
+        1.0, 0.0, 0.0, 0.00347,
+        0.0, 1.0, 0.0, 0.03447,
+        0.0, 0.0, 1.0, 0.02174,
+        0.0, 0.0, 0.0, 1.0).finished();
+    Eigen::Matrix4d T_cl_ = Eigen::Matrix4d::Identity(); // Camera->Lidar from YAML
 #ifdef ROS2
    std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
@@ -1267,9 +1442,12 @@ private:
            xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", 10);
            odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", 10);
+            path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", 10);
+            pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", 10);
            rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", 10);
-	    compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
-        undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", 10);
+            compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
+            undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", 10);
+            intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", 10);
        #endif
    }
    #ifdef ROS1
@@ -1280,9 +1458,12 @@ private:
            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 10);
            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 10);
            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 10);
+            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 10);
+            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 10);
            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 10);
+            intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 10);
        }
    #endif

@@ -1294,11 +1475,15 @@ private:
        rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_publisher_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_highfreq_publisher_;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_publisher_;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr rendered_cloud_pub_;
        rclcpp::Publisher<PointCloud2Msg>::SharedPtr rgbcloud_pub_;
        rclcpp::Publisher<ImageMsg>::SharedPtr rgbFromnv12_pub_;
        rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_rgb_pub_; // New compressed image publisher
        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr undistort_rgb_pub_;
+        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
+        camera_pose_visualization cameraposevisual_;
    #else
        ros::Publisher imu_pub_;
        ros::Publisher rgb_pub_;
@@ -1306,11 +1491,15 @@ private:
        ros::Publisher xyzrgbacloud_pub_;
        ros::Publisher odom_publisher_;
        ros::Publisher odom_highfreq_publisher_;
+        ros::Publisher path_publisher_;
+        ros::Publisher pub_camera_pose_visual_;
+        camera_pose_visualization cameraposevisual_;
        ros::Publisher rendered_cloud_pub_;
        ros::Publisher rgbcloud_pub_;
        ros::Publisher rgbFromnv12_pub_;
        ros::Publisher compressed_rgb_pub_; // New compressed image publisher
        ros::Publisher undistort_rgb_pub_;
+        ros::Publisher intensity_gray_pub_;
    #endif
 };

@@ -69,7 +69,6 @@ private:

    cv::Mat map_x_, map_y_;
    cv::Mat inv_map_x_, inv_map_y_;
-    cv::Mat depth_undistorted_;

    int scaled_width_, scaled_height_;

@@ -0,0 +1,232 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include "camera_pose_visualization.h"
+
+const Eigen::Vector3d camera_pose_visualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);
+const Eigen::Vector3d camera_pose_visualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);
+const Eigen::Vector3d camera_pose_visualization::imlb = Eigen::Vector3d(-1.0,  0.5, 1.0);
+const Eigen::Vector3d camera_pose_visualization::imrb = Eigen::Vector3d( 1.0,  0.5, 1.0);
+const Eigen::Vector3d camera_pose_visualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);
+const Eigen::Vector3d camera_pose_visualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);
+const Eigen::Vector3d camera_pose_visualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);
+const Eigen::Vector3d camera_pose_visualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);
+
+#ifdef ROS2
+using GeometryPoint = geometry_msgs::msg::Point;
+#else
+using GeometryPoint = geometry_msgs::Point;
+#endif
+
+void Eigen2Point(const Eigen::Vector3d& v, GeometryPoint& p) {
+    p.x = v.x();
+    p.y = v.y();
+    p.z = v.z();
+}
+
+camera_pose_visualization::camera_pose_visualization(float r, float g, float b, float a)
+    : m_marker_ns("camera_pose_visualization"), m_scale(0.3), m_line_width(0.03) {
+    m_image_boundary_color.r = r;
+    m_image_boundary_color.g = g;
+    m_image_boundary_color.b = b;
+    m_image_boundary_color.a = a;
+    m_optical_center_connector_color.r = r;
+    m_optical_center_connector_color.g = g;
+    m_optical_center_connector_color.b = b;
+    m_optical_center_connector_color.a = a;
+}
+
+void camera_pose_visualization::setImageBoundaryColor(float r, float g, float b, float a) {
+    m_image_boundary_color.r = r;
+    m_image_boundary_color.g = g;
+    m_image_boundary_color.b = b;
+    m_image_boundary_color.a = a;
+}
+
+void camera_pose_visualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {
+    m_optical_center_connector_color.r = r;
+    m_optical_center_connector_color.g = g;
+    m_optical_center_connector_color.b = b;
+    m_optical_center_connector_color.a = a;
+}
+
+void camera_pose_visualization::setScale(double s) {
+    m_scale = s;
+}
+void camera_pose_visualization::setLineWidth(double width) {
+    m_line_width = width;
+}
+void camera_pose_visualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
+    Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = m_markers.size() + 1;
+#ifdef ROS2
+    marker.type = Marker::LINE_LIST;
+    marker.action = Marker::ADD;
+#else
+    marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.action = visualization_msgs::Marker::ADD;
+#endif
+    marker.scale.x = 0.005;
+
+    marker.color.g = 1.0f;
+    marker.color.a = 1.0;
+
+    GeometryPoint point0, point1;
+
+    Eigen2Point(p0, point0);
+    Eigen2Point(p1, point1);
+
+    marker.points.push_back(point0);
+    marker.points.push_back(point1);
+
+    m_markers.push_back(marker);
+}
+
+void camera_pose_visualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
+    Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = m_markers.size() + 1;
+#ifdef ROS2
+    marker.type = Marker::LINE_LIST;
+    marker.action = Marker::ADD;
+#else
+    marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.action = visualization_msgs::Marker::ADD;
+#endif
+    marker.scale.x = 0.04;
+    //marker.scale.x = 0.3;
+
+    marker.color.r = 1.0f;
+    marker.color.b = 1.0f;
+    marker.color.a = 1.0;
+
+    GeometryPoint point0, point1;
+
+    Eigen2Point(p0, point0);
+    Eigen2Point(p1, point1);
+
+    marker.points.push_back(point0);
+    marker.points.push_back(point1);
+
+    m_markers.push_back(marker);
+}
+
+
+void camera_pose_visualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {
+    Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = m_markers.size() + 1;
+#ifdef ROS2
+    marker.type = Marker::LINE_STRIP;
+    marker.action = Marker::ADD;
+#else
+    marker.type = visualization_msgs::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::Marker::ADD;
+#endif
+    marker.scale.x = m_line_width;
+
+    marker.pose.position.x = 0.0;
+    marker.pose.position.y = 0.0;
+    marker.pose.position.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+
+
+    GeometryPoint pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;
+
+    Eigen2Point(q * (m_scale * imlt) + p, pt_lt);
+    Eigen2Point(q * (m_scale * imlb) + p, pt_lb);
+    Eigen2Point(q * (m_scale * imrt) + p, pt_rt);
+    Eigen2Point(q * (m_scale * imrb) + p, pt_rb);
+    Eigen2Point(q * (m_scale * lt0 ) + p, pt_lt0);
+    Eigen2Point(q * (m_scale * lt1 ) + p, pt_lt1);
+    Eigen2Point(q * (m_scale * lt2 ) + p, pt_lt2);
+    Eigen2Point(q * (m_scale * oc  ) + p, pt_oc);
+
+    // image boundaries
+    marker.points.push_back(pt_lt);
+    marker.points.push_back(pt_lb);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_lb);
+    marker.points.push_back(pt_rb);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_rb);
+    marker.points.push_back(pt_rt);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_rt);
+    marker.points.push_back(pt_lt);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    // top-left indicator
+    marker.points.push_back(pt_lt0);
+    marker.points.push_back(pt_lt1);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_lt1);
+    marker.points.push_back(pt_lt2);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    // optical center connector
+    marker.points.push_back(pt_lt);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+
+    marker.points.push_back(pt_lb);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    marker.points.push_back(pt_rt);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    marker.points.push_back(pt_rb);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    m_markers.push_back(marker);
+}
+
+void camera_pose_visualization::reset() {
+    m_markers.clear();
+}
+
+void camera_pose_visualization::publish_by( Publisher& pub, const Header& header ) {
+    MarkerArray markerArray_msg;
+
+    for (auto& marker : m_markers) {
+        marker.header = header;
+        markerArray_msg.markers.push_back(marker);
+    }
+
+    pub.publish(markerArray_msg);
+}
@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "depth_image_ros2_node.hpp"
 #include <functional>

@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "depth_image_ros_node.hpp"
 #include <boost/bind.hpp>

@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "host_sdk_sample.h"
 #include "yaml_parser.h"
 #include "rawCloudRender.h"
@@ -31,7 +44,7 @@
    #include <ros/package.h>
    #include <ros/ros.h> 
 #endif
-#define ros_driver_version "0.5.0"
+#define ros_driver_version "0.5.2"
 // Global variable declarations
 static device_handle odinDevice = nullptr;
 static std::atomic<bool> deviceConnected(false);
@@ -71,42 +84,52 @@ int g_sendrgb_compressed = 0;
 int g_sendrgb_undistort = 0;
 int g_record_data = 0;
 int g_devstatus_log = 0;
+int g_pub_intensity_gray = 0;
+int g_show_path = 0;
+int g_show_camerapose = 0;

+std::filesystem::path log_root_dir_;
 const char* DEV_STATUS_CSV_FILE = "dev_status.csv";
 FILE* dev_status_csv_file = nullptr;

 typedef struct  {
    struct timespec start = {0, 0};
-    double frame_count = 0.0;
-    std::atomic<double> fps;
+    struct timespec last = {0, 0};
+    int count = 0;
+    std::mutex fps_mutex;
 } fpsHandle;

-static void sensor_fps(fpsHandle* handle, const char* name, bool print = false) 
-{
+void update_count(fpsHandle* handle) {
    struct timespec now;
+    std::lock_guard<std::mutex> lock(handle->fps_mutex);
    clock_gettime(CLOCK_MONOTONIC, &now);
-
-    if (handle->start.tv_sec == 0 && handle->start.tv_nsec == 0) {
+    if (handle->count == 0) {
        handle->start = now;
+    } else {
+        handle->last = now;
    }
+    handle->count++;
+}

-    handle->frame_count += 1.0;
-
-    double elapsed = (now.tv_sec - handle->start.tv_sec)
-                   + (now.tv_nsec - handle->start.tv_nsec) / 1e9;
-
-    if (elapsed >= 1.0) {
-        handle->fps.store(handle->frame_count / elapsed);
-        if (print) {
-            #ifdef ROS2
-                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "%s FPS: %f", name, handle->fps.load());
-            #else
-                ROS_INFO("%s FPS: %f", name, handle->fps.load());
-            #endif
-        }
-        handle->frame_count = 0;
-        handle->start = now;
+double cal_fps(fpsHandle* handle, const char* name, bool print = false) 
+{
+    std::lock_guard<std::mutex> lock(handle->fps_mutex);
+    if (handle->count < 2) {
+        return 0.0;
    }
+    double elapsed = (handle->last.tv_sec - handle->start.tv_sec)
+                   + (handle->last.tv_nsec - handle->start.tv_nsec) / 1e9;
+    double fps = (handle->count - 1) / elapsed;
+    if (print) {
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "%s FPS: %f (count: %d, elapsed: %f)", name, fps, handle->count, elapsed);
+        #else
+            ROS_INFO("%s FPS: %f (count: %d, elapsed: %f)", name, fps, handle->count, elapsed);
+        #endif
+    }
+    handle->start = handle->last;
+    handle->count = 1;
+    return fps;
 }

 static fpsHandle rgb_rx_fps;
@@ -136,6 +159,51 @@ RosNodeControlInterface* getRosNodeControl() {
    return &g_rosNodeControlImpl;
 }

+// Return resident memory (RSS) in **megabytes** for a given PID
+double read_rss_mb(pid_t pid) {
+    std::string path = "/proc/" + std::to_string(pid) + "/status";
+    std::ifstream in(path);
+    if (!in) return 0.0;
+    std::string key;
+    long kb = 0;
+    while (in >> key) {
+        if (key == "VmRSS:") {        // VmRSS is reported in kB
+            in >> kb;
+            break;
+        }
+        in.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+    }
+    return kb / 1024.0;               // convert to MB
+}
+
+double read_pss_mb(pid_t pid) {
+    std::string path = "/proc/" + std::to_string(pid) + "/smaps_rollup";
+    std::ifstream in(path);
+    if (!in) return 0.0;
+    std::string key;
+    long kb = 0;
+    while (in >> key) {
+        if (key == "Pss:") {     // Proportional Set Size in kB
+            in >> kb;
+            break;
+        }
+        in.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+    }
+    return kb / 1024.0;          // convert to MB
+}
+
+// Recursively collect child PIDs of the given pid
+void collect_children(pid_t pid, std::vector<pid_t>& all) {
+    std::string task_path = "/proc/" + std::to_string(pid) +
+                            "/task/" + std::to_string(pid) + "/children";
+    std::ifstream in(task_path);
+    pid_t child;
+    while (in >> child) {
+        all.push_back(child);
+        collect_children(child, all);
+    }
+}
+
 void clear_all_queues();

 // detect USB3.0
@@ -403,6 +471,11 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
    imu_convert_data_t *imudata = nullptr;
    lidar_device_status_t *dev_info_data;
    
+    pid_t self = getpid();
+    std::vector<pid_t> pids;
+
+    double total_mb = 0.0;
+
    switch(data->type) {
        case LIDAR_DT_NONE:
            printf("empty lidar data type: %x\n", data->type);
@@ -411,35 +484,45 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
            if (g_sendrgb) {
                g_ros_object->publishRgb((capture_Image_List_t *)&data->stream);
            }
-            sensor_fps(&rgb_rx_fps, "rgb_rx");
+            update_count(&rgb_rx_fps);
            break;
        case LIDAR_DT_RAW_IMU:
            if (g_sendimu) {
                imudata = (imu_convert_data_t *)data->stream.imageList[0].pAddr;
                g_ros_object->publishImu(imudata);
            }
-            sensor_fps(&imu_rx_fps, "imu_rx");
+            update_count(&imu_rx_fps);
            break;
        case LIDAR_DT_RAW_DTOF:
            if (g_senddtof ) {
                g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
            }
-            sensor_fps(&dtof_rx_fps, "dtof_rx");
+            if (g_pub_intensity_gray) {
+                g_ros_object->publishGrayUInt8((capture_Image_List_t *)&data->stream, 2);
+            }
+            update_count(&dtof_rx_fps);
            break;
        case LIDAR_DT_SLAM_CLOUD:
            if (g_sendcloudslam) {
                g_ros_object->publishPC2XYZRGBA((capture_Image_List_t *)&data->stream, 0);
            }
-            sensor_fps(&slam_cloud_rx_fps, "slam_cloud_rx");
+            update_count(&slam_cloud_rx_fps);
            break;
        case LIDAR_DT_SLAM_ODOMETRY:
            if (g_sendodom) {
-                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false);
+                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false, g_show_path, g_show_camerapose);
            }
-            sensor_fps(&slam_odom_rx_fps, "slam_odom_rx");
+            update_count(&slam_odom_rx_fps);
            break;
        case LIDAR_DT_DEV_STATUS:
            dev_info_data = (lidar_device_status_t *)data->stream.imageList[0].pAddr;
+
+            pids.push_back(self);
+            collect_children(self, pids);
+            for (pid_t p : pids) {
+                total_mb += read_pss_mb(p); // read_rss_mb(p);
+            }
+
            if (g_devstatus_log) {
                if (dev_status_csv_file) {
                    // append the data row
@@ -471,7 +554,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,", 
                        ((float)dev_info_data->rgb_sensor.configured_odr)/1000,
                        ((float)dev_info_data->rgb_sensor.tx_odr)/1000,
-                        (rgb_rx_fps.fps.load()));
+                        cal_fps(&rgb_rx_fps, "rgb_rx")
+                    );
                    if (rc < 0) { 
                        printf("Failed to write to dev_status_csv_file\n");
                    }
@@ -479,7 +563,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,",
                        ((float)dev_info_data->dtof_sensor.configured_odr)/1000,
                        ((float)dev_info_data->dtof_sensor.tx_odr)/1000,
-                        (dtof_rx_fps.fps.load()));
+                        cal_fps(&dtof_rx_fps, "dtof_rx")
+                    );
                    if (rc < 0) { 
                        printf("Failed to write to dev_status_csv_file\n");
                    }
@@ -487,18 +572,24 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,",
                        ((float)dev_info_data->imu_sensor.configured_odr)/1000,
                        ((float)dev_info_data->imu_sensor.tx_odr)/1000,
-                        (imu_rx_fps.fps.load()));
+                        cal_fps(&imu_rx_fps, "imu_rx")
+                    );
                    if (rc < 0) { 
                        printf("Failed to write to dev_status_csv_file\n");
                    }

-                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", 
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,", 
                        ((float)dev_info_data->slam_cloud_tx_odr)/1000,
-                        (slam_cloud_rx_fps.fps.load()),
+                        cal_fps(&slam_cloud_rx_fps, "slam_cloud_rx"),
                        ((float)dev_info_data->slam_odom_tx_odr)/1000,
-                        (slam_odom_rx_fps.fps.load()),
+                        cal_fps(&slam_odom_rx_fps, "slam_odom_rx"),
                        ((float)dev_info_data->slam_odom_highfreq_tx_odr)/1000,
-                        (slam_odom_highfreq_rx_fps.fps.load()));
+                        cal_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx"));
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+                                   
+                    rc = std::fprintf(dev_status_csv_file, "%.2f\n", total_mb);
                    if (rc < 0) { 
                        printf("Failed to write to dev_status_csv_file\n");
                    }
@@ -531,12 +622,14 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                printf("\n [dev_info] [rgb]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ \n", 
                    ((float)dev_info_data->rgb_sensor.configured_odr)/1000,
                    ((float)dev_info_data->rgb_sensor.tx_odr)/1000,
-                    (rgb_rx_fps.fps.load()));
+                    cal_fps(&rgb_rx_fps, "rgb_rx")
+                );

                printf("\n [dev_info] [dtof]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ \n", 
                    ((float)dev_info_data->dtof_sensor.configured_odr)/1000,
                    ((float)dev_info_data->dtof_sensor.tx_odr)/1000,
-                    (dtof_rx_fps.fps.load()));
+                    cal_fps(&dtof_rx_fps, "dtof_rx")
+                );
                printf("\n [dev_info] [dtof]: subframe_odr: %.2f \n", 
                    ((float)dev_info_data->dtof_sensor.subframe_odr)/1000);
                printf("\n [dev_info] [dtof]: txtemp:%dC, rxtemp:%dC \n", dev_info_data->dtof_sensor.tx_temp, dev_info_data->dtof_sensor.rx_temp);
@@ -544,33 +637,39 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                printf("\n [dev_info] [imu]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ\n",
                    ((float) dev_info_data->imu_sensor.configured_odr)/1000,
                    ((float) dev_info_data->imu_sensor.tx_odr)/1000,
-                    (imu_rx_fps.fps.load())
+                    cal_fps(&imu_rx_fps, "imu_rx")
                );

                printf("\n [dev_info] [slam]: slam_cloud_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
                    ((float)dev_info_data->slam_cloud_tx_odr)/1000,
-                    (slam_cloud_rx_fps.fps.load())
+                    cal_fps(&slam_cloud_rx_fps, "slam_cloud_rx")
                );

                printf("\n [dev_info] [slam]: slam_odom_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
                    ((float)dev_info_data->slam_odom_tx_odr)/1000,
-                    (slam_odom_rx_fps.fps.load())
+                    cal_fps(&slam_odom_rx_fps, "slam_odom_rx")
                );

                printf("\n [dev_info] [slam]: slam_odom_highfreq_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
                    ((float)dev_info_data->slam_odom_highfreq_tx_odr)/1000,
-                    (slam_odom_highfreq_rx_fps.fps.load())
+                    cal_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx")
                );

+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("dev_info"), "Total RAM (parent + children): %.2f MB", total_mb);
+                #else
+                    ROS_INFO("Total RAM (parent + children): %.2f MB", total_mb);
+                #endif
+
                printf("\n------------------------------------------\n");
            }
            break;
            case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ:
            {
                if (g_sendodom) {
-                    g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true);
+                    g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true, false, false);
                }
-                sensor_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx");
+                update_count(&slam_odom_highfreq_rx_fps);
            }
            break;
        default:
@@ -762,6 +861,46 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            odinDevice = nullptr;
            return;
        }
+        
+        auto con_time = std::chrono::system_clock::now();
+        std::time_t t = std::chrono::system_clock::to_time_t(con_time);
+        std::tm tm{};
+        #ifdef _WIN32
+            localtime_s(&tm, &t);
+        #else
+            localtime_r(&t, &tm);
+        #endif
+        char buf[32];
+        std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
+        std::string folder_name = std::string("Conn_") + std::string(buf);
+        std::filesystem::path per_con_log_root_dir_ = log_root_dir_ / folder_name;
+        std::filesystem::create_directories(per_con_log_root_dir_);
+        std::string dev_status_csv_file_path_ = per_con_log_root_dir_ / "dev_status.csv";
+
+        if (dev_status_csv_file) {
+            std::fflush(dev_status_csv_file);
+            fclose(dev_status_csv_file);
+            dev_status_csv_file = nullptr;
+        }
+
+        // Open the file in append mode
+        dev_status_csv_file = fopen(dev_status_csv_file_path_.c_str(), "a");
+        if (!dev_status_csv_file) {
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("init"), "Failed to open dev_status CSV file");
+            #else
+                ROS_ERROR("Failed to open dev_status CSV file");
+            #endif
+        } else {
+            const char* header =
+            "uptime_seconds,package_temp,cpu_temp,center_temp,gpu_temp,npu_temp,dtof_tx_temp,dtof_rx_temp,"
+            "cpu0,cpu1,cpu2,cpu3,cpu4,cpu5,cpu6,cpu7,ram_use(%),"
+            "rgb_configured_odr,rgb_tx_odr,rgb_rx_odr,dtof_configured_odr,dtof_tx_odr,dtof_rx_odr,imu_configured_odr,imu_tx_odr,imu_rx_odr,"
+            "slam_cloud_tx_odr,slam_cloud_rx_odr,slam_odom_tx_odr,slam_odom_rx_odr,slam_odom_highfreq_tx_odr,slam_odom_highfreq_rx_odr,"
+            "host_ram_use(mb)\n";
+            fprintf(dev_status_csv_file, "%s", header);
+            std::fflush(dev_status_csv_file);
+        }

        uint32_t dtof_subframe_odr = 0;
        if (lidar_start_stream(odinDevice, type, dtof_subframe_odr)) {
@@ -800,7 +939,8 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        deviceConnected = true;
        deviceDisconnected = false; 
        
-        if (g_sendrgb_undistort && g_ros_object->loadCameraParams(calib_config) == 0) {
+        bool load_status = g_ros_object->loadCameraParams(calib_config);
+        if (g_sendrgb_undistort &&  load_status == 0) {
            g_ros_object->buildUndistortMap();
        }

@@ -825,6 +965,12 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach

        clear_all_queues();

+        if (dev_status_csv_file) {
+            std::fflush(dev_status_csv_file);
+            fclose(dev_status_csv_file);
+            dev_status_csv_file = nullptr;
+        }
+
        #ifdef ROS2
            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Waiting for device reconnection...");
        #else
@@ -883,6 +1029,9 @@ int main(int argc, char *argv[])
        g_record_data = get_key_value("recorddata", 0);
        g_show_fps = get_key_value("showfps", 0);
        g_devstatus_log = get_key_value("devstatuslog", 0);
+        g_pub_intensity_gray = get_key_value("pubintensitygray", 0);
+        g_show_path = get_key_value("showpath", 0);
+        g_show_camerapose = get_key_value("showcamerapose", 0);
        g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
        lidar_log_set_level(LIDAR_LOG_INFO);

@@ -925,28 +1074,9 @@ int main(int argc, char *argv[])
            #endif
            char buf[32];
            std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
-
-            std::filesystem::path log_root_dir_ = std::filesystem::path(log_dir) / buf;
+            std::string folder_name = std::string("Driver_") + std::string(buf);
+            log_root_dir_ = std::filesystem::path(log_dir) / folder_name;
            std::filesystem::create_directories(log_root_dir_);
-            std::string dev_status_csv_file_path_ = log_root_dir_ / "dev_status.csv";
-
-            // Open the file in append mode
-            dev_status_csv_file = fopen(dev_status_csv_file_path_.c_str(), "a");
-            if (!dev_status_csv_file) {
-                #ifdef ROS2
-                    RCLCPP_ERROR(rclcpp::get_logger("init"), "Failed to open dev_status CSV file");
-                #else
-                    ROS_ERROR("Failed to open dev_status CSV file");
-                #endif
-            } else {
-                const char* header =
-                "uptime_seconds,package_temp,cpu_temp,center_temp,gpu_temp,npu_temp,dtof_tx_temp,dtof_rx_temp,"
-                "cpu0,cpu1,cpu2,cpu3,cpu4,cpu5,cpu6,cpu7,ram_use,"
-                "rgb_configured_odr,rgb_tx_odr,rgb_rx_odr,dtof_configured_odr,dtof_tx_odr,dtof_rx_odr,imu_configured_odr,imu_tx_odr,imu_rx_odr,"
-                "slam_cloud_tx_odr,slam_cloud_rx_odr,slam_odom_tx_odr,slam_odom_rx_odr,slam_odom_highfreq_tx_odr,slam_odom_highfreq_rx_odr\n";
-                fprintf(dev_status_csv_file, "%s", header);
-                std::fflush(dev_status_csv_file);
-            }
        }

        if (lidar_system_init(lidar_device_callback)) {
@@ -1121,8 +1251,11 @@ int main(int argc, char *argv[])
        // lidar_close_device(odinDevice);
        // lidar_destory_device(odinDevice);

-        std::fflush(dev_status_csv_file);
-        fclose(dev_status_csv_file);
+        if (dev_status_csv_file) {
+            std::fflush(dev_status_csv_file);
+            fclose(dev_status_csv_file);
+            dev_status_csv_file = nullptr;
+        }
    }
    // lidar_system_deinit();

@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include <ros/ros.h>
 #include <fstream>
 #include <sys/stat.h>
@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include <rclcpp/rclcpp.hpp>
 #include <fstream>
 #include <sys/stat.h>
@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "pointcloud_depth_converter.hpp"
 #include <cmath>
 #include <iostream>
@@ -23,7 +36,7 @@ void PointCloudToDepthConverter::initializeInternalParams()

    Kl_ = Eigen::Matrix3d::Identity();
    Kl_(0, 0) = params_.A11 / params_.scale;
-    Kl_(0, 1) = params_.A12 / params_.scale;
+    Kl_(0, 1) = 0.0;
    Kl_(0, 2) = params_.u0 / params_.scale;
    Kl_(1, 1) = params_.A22 / params_.scale;
    Kl_(1, 2) = params_.v0 / params_.scale;
@@ -241,16 +254,7 @@ cv::Mat PointCloudToDepthConverter::postProcessDepthImage(const cv::Mat &depth_i
        return cv::Mat();
    }

-    cv::Mat depth_img_distorted;
-    try {
-        depth_undistorted_ = depth_img_upsampled.clone();
-        cv::remap(depth_img_upsampled, depth_img_distorted, map_x_, map_y_, cv::INTER_LINEAR);
-    } catch (const cv::Exception& e) {
-        std::cerr << "ERROR: Remap failed: " << e.what() << std::endl;
-        return cv::Mat();
-    }
-    
-    return depth_img_distorted;
+    return depth_img_upsampled;
 }

 cv::Mat PointCloudToDepthConverter::customResize(const cv::Mat& src, const cv::Size& size) {
@@ -291,9 +295,8 @@ pcl::PointCloud<pcl::PointXYZRGB> PointCloudToDepthConverter::generateColoredClo
    const cv::Mat &depth_img, const cv::Mat &color_img)
 {
    cv::Mat depth_undistorted, color_undistorted;
-    depth_undistorted = depth_undistorted_.clone();
+    depth_undistorted = depth_img.clone();
    cv::remap(color_img, color_undistorted, inv_map_x_, inv_map_y_, cv::INTER_LINEAR);
-    

    pcl::PointCloud<pcl::PointXYZRGB> cloud_colored;

@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "rawCloudRender.h"
 #include <yaml-cpp/yaml.h>
 #include <iostream>
@@ -1,3 +1,16 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
 #include "yaml_parser.h"
 #include <fstream>
 #include <filesystem>