<add>

1.Added camera point cloud extrinsic parameter rendering function
2.add image_compressed
3.fix imshow error about slam pointcloud

CMakeLists.txt

@@ -154,6 +154,7 @@ if(ROS_VERSION STREQUAL "ROS1")
     add_executable(host_sdk_sample 
         src/host_sdk_sample.cpp
         src/yaml_parser.cpp
+        src/rawCloudRender.cpp
     )
     target_link_libraries(host_sdk_sample
         ${catkin_LIBRARIES}
@@ -198,6 +199,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
     add_executable(host_sdk_sample 
         src/host_sdk_sample.cpp
         src/yaml_parser.cpp  
+        src/rawCloudRender.cpp
         )
     
     # Link libraries
@@ -292,3 +294,4 @@ message(STATUS "Target platform: ${TARGET_PLATFORM}")
 message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
 message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
 message(STATUS "=======================================")
+

README.md

@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application
 
 ## 1. Version
 
-Current Version: v0.1
+Current Version: v0.2
 
 ## 2. Preparation
 
@@ -99,22 +99,22 @@ sudo udevadm trigger
 git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
 ```
 Note:
-Please clone the source code into the "[workspace]/src/" folder, otherwise compilation errors will occur.
+Please clone the source code into the "[ros_workspace]/src/" folder, otherwise compilation errors will occur.
 
 ### 3.3 make
 
 #### 3.3.1 ROS1 (Noetic for example):
 
 ```shell
-source /opt/ros/noetic/setup.sh
+source /opt/ros/noetic/setup.bash
-./scripty/build_ros.sh
+./script/build_ros.sh
 ```
 
 #### 3.3.2 ROS2 (Foxy for example):
 
 ```shell
-source /opt/ros/foxy/setup.sh
+source /opt/ros/foxy/setup.bash
-./scripty/build_ros2.sh
+./script/build_ros2.sh
 ```
 
 ### 3.4 run:
@@ -122,27 +122,29 @@ source /opt/ros/foxy/setup.sh
 #### 3.4.1 ROS1 (Noetic for example):
 
 ```shell
-source ../../devel/setup.sh
+source [ros_workspace]/install/setup.bash
-roslaunch odin_ros_driver [launch file]
+ros2 launch odin_ros_driver [launch file]
 ```
 ● odin_ros_driver: package name;
 
 ● launch file: launch file;
 
-ROS1 Demo Launch Instructions:
+● ros_workspace: User's ROS environment workspace;
 ```shell
 roslaunch odin_ros_driver odin1_ros1.launch
 ```
 #### 3.4.2 ROS2 (Foxy for example):
 
 ```shell
-source ../../install/setup.sh
+source [ros2_workspace]/install/setup.bash
 ros2 launch odin_ros_driver [launch file]
 ```
 ● odin_ros_driver: package name;
 
 ● launch file: launch file;
 
+● ros2_workspace: User's ROS2 environment workspace;
+
 ROS2 Demo Launch Instructions:
 ```shell
 ros2 launch odin_ros_driver odin1_ros2.launch.py
@@ -155,6 +157,7 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
     src/
         host_sdk_sample.cpp     // Example source code
         yaml_parser.cpp         // Source code for reading yaml parameters
+        rawCloudRender.cpp      // Source code for RenderCloud
     lib/
         liblydHostApi_amd.a     // Static library for AMD platform
         liblydHostApi_arm.a     // Static library for ARM platform
@@ -163,8 +166,10 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
         lidar_api_type.h        // API data structure header file
         lidar_api.h             // API function declarations
         yaml_parser.h           // Parameter file reading header file
+        rawCloudRender.h        // API about RenderCloud
     config/
-        control_command.yaml    // control parameter file for driver
+        control_command.yaml    // Control parameter file for driver
+        calib.yaml              //Machine calibration yaml
     launch_ROS1/
         odin1_ros1.launch       // ROS1 launch file
     launch_ROS2/
@@ -182,38 +187,30 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
 | odin1_ros1.launch        | Launch file for ROS1 - Odin1 Basic Operations Demo |
 | odin1_ros2.launch.py     | Launch file for ROS2 - Odin1 Basic Operations Demo |
 
-### 4.3 Config file
-Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters:
-| Parameter     | Detailed Description | Default |
-|---------------|----------------------|---------|
-| streamctrl: 1 | Master data stream control (0: OFF, 1: ON) | 1 |
-| sendrgb:1     | RGB image stream control (0: OFF, 1: ON) | 1 |
-| sendimu:1     | IMU (Inertial Measurement Unit) stream control (0: OFF, 1: ON) | 1 |
-| sendodom:1    | Odometry data stream control (0: OFF, 1: ON) | 1 |
-| senddtof:0    | Raw PointCloud sensor stream control (0: OFF, 1: ON) | 0 |
-| sendcloudslam:1 | Slam PointCloud stream control (0: OFF, 1: ON) | 1 |
 
-### 4.4 ROS topics
+### 4.3 ROS topics
 Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters:
 
 | Topic               | Detailed Description |
 |---------------------|----------------------|
 | odin1/imu           | Imu Topic |
 | odin1/image         | RGB Camera Topic |
+| odin1/image/compressed         | RGB Camera compressed Topic |
 | odin1/cloud_raw     | Raw_Cloud Topic |
+| odin1/cloud_render     | Render_Cloud Topic |
 | odin1/cloud_slam    | Slam_PointCloud Topic |
 | odin1/odometry_map  | Odom Topic |
 
 ## 5. FAQ
 ### 5.1 Segmentation fault upon re-launching host SDK
 **Error Message**  
-Core dump occurs when restarting host SDK after initial successful run
+No device connected after 60 seconds 
 
 **Solution**  
-```shell
+1.Please power on Odin module again # Disconnect and reconnect odin power
-power cycle Odin1 # Disconnect and reconnect LiDAR power
+
-reinitialize host SDK # Execute SDK after device reboot
+2.Reinitialize Odin SDK # Execute SDK after device reboot
-```
+
 
 ### 5.2 Library binding failure during compilation
 
@@ -221,12 +218,18 @@ reinitialize host SDK # Execute SDK after device reboot
 ld: cannot find -llydHostApi or symbol lookup errors
 
 **Resolution** 
+
+1.Clean previous build artifacts
+
+ROS1 
 ```shell
-Clean previous build artifacts
+rm -rf devel/ build/  
-ROS1 rm -rf devel/ build/
-ROS2 rm -rf devel/ install/ log/
-Re-run script installation (refer to section 2.3)
 ``` 
+ROS2
+```shell
+rm -rf devel/ install/ log/ 
+``` 
+2.Re-run script installation
 
 ### 5.3 Docker GUI passthrough failure
 
@@ -237,3 +240,30 @@ Unable to open X display or No protocol specified
 ```shell
 xhost + #This command enables graphical passthrough to Docker containers
 ``` 
+
+### 5.4 RVIZ has not responded for a long time
+
+**Error Message**  
+Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection...
+
+**Resolution** 
+
+Please power on Odin module again
+
+### 5.5 Device not responding
+
+**Error Message**  
+Missed ok response from device,probably wrong interaction procedure.
+
+**Resolution** 
+
+Please adopt the solution mentioned in 5.1
+
+### 5.6 Device has no external calibration file 
+
+**Error Message**  
+ERROR：Missing camera node 'cam_0'
+
+**Resolution** 
+
+Please plug and unplug the USB again

config/control_command.yaml

@@ -3,5 +3,8 @@ register_keys:
   sendrgb: 1
   sendimu: 1
   sendodom: 1
-  senddtof: 0
+  senddtof: 1
   sendcloudslam: 1
+  sendcloudrender: 1
+  sendrgbcompressed: 1
+

config/odin_ros.rviz

@@ -61,34 +61,6 @@ Visualization Manager:
       Transport Hint: raw
       Unreliable: false
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic: /odin1/cloud_raw
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
     - Angle Tolerance: 0.10000000149011612
       Class: rviz/Odometry
       Covariance:
@@ -110,7 +82,7 @@ Visualization Manager:
       Keep: 1
       Name: Odometry
       Position Tolerance: 0.10000000149011612
-      Queue Size: 100
+      Queue Size: 1
       Shape:
         Alpha: 1
         Axes Length: 1
@@ -134,24 +106,80 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: raw
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /odin1/cloud_raw
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.8493094444274902
+        Min Value: -0.13891705870628357
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: PointCloud2
+      Name: render
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Points
-      Topic: /odin1/cloud_slam
+      Topic: /odin1/cloud_render
       Unreliable: false
       Use Fixed Frame: true
-      Use rainbow: false
+      Use rainbow: true
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: slam
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /odin1/cloud_raw
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -180,7 +208,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 10.812461853027344
+      Distance: 5.067190170288086
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -196,9 +224,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.830398678779602
+      Pitch: -0.01460082083940506
       Target Frame: <Fixed Frame>
-      Yaw: 3.0054032802581787
+      Yaw: 2.2254059314727783
     Saved: ~
 Window Geometry:
   Displays:

config/odin_ros2.rviz

@@ -6,11 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /PointCloud21
-        - /PointCloud22
-        - /Image1
         - /Image1/Topic1
-        - /Odometry1
       Splitter Ratio: 0.5
     Tree Height: 472
   - Class: rviz_common/Selection
@@ -47,72 +43,6 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/cloud_slam
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: ""
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: ""
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/cloud_raw
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
     - Class: rviz_default_plugins/Image
       Enabled: true
       Max Value: 1
@@ -145,7 +75,7 @@ Visualization Manager:
           Value: true
         Value: true
       Enabled: true
-      Keep: 100
+      Keep: 1
       Name: Odometry
       Position Tolerance: 0.10000000149011612
       Shape:
@@ -165,6 +95,105 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /odin1/odometry_map
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 49
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: raw
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odin1/cloud_raw
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.8730175495147705
+        Min Value: -0.12252448499202728
+        Value: true
+      Axis: Z
+      Channel Name: rgb
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.3509885615147286e-38
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: render
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odin1/cloud_render
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: slam
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odin1/cloud_slam
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -208,25 +237,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 10.72309398651123
+      Distance: 3.231567859649658
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
+        X: 0.40898558497428894
-        Y: 0
+        Y: -0.07609735429286957
-        Z: 0
+        Z: 0.5482669472694397
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7503987550735474
+      Pitch: 0.16039875149726868
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.205399513244629
+      Yaw: 2.625401258468628
     Saved: ~
 Window Geometry:
   Displays:

include/host_sdk_sample.h

@@ -30,8 +30,25 @@ limitations under the License.
 #include <Eigen/Dense>
 #include "lidar_api.h"
 #include "lidar_api_type.h"
+#include "rawCloudRender.h"
+#include <deque> 
+#include <mutex>  
+#include <vector> 
+#include <queue>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/wait.h>
 
 
+#define LOG_LEVEL_NONE 0
+#define LOG_LEVEL_ERROR 1
+#define LOG_LEVEL_WARN 2
+#define LOG_LEVEL_INFO 3
+#define LOG_LEVEL_DEBUG 4
+
+
+extern int g_log_level;
+extern int g_sendcloudrender;
 #ifdef ROS2
 
     #include "rclcpp/rclcpp.hpp"
@@ -44,6 +61,9 @@ limitations under the License.
     #include <nav_msgs/msg/odometry.hpp>
     #include <sensor_msgs/msg/point_cloud2.hpp>
     #include <sensor_msgs/msg/point_field.hpp>
+    #include <sensor_msgs/msg/image.hpp>
+    using ImageConstPtr = sensor_msgs::msg::Image::ConstSharedPtr;
+    using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
     namespace ros {
         using namespace rclcpp;
         using namespace std_msgs::msg;
@@ -51,6 +71,12 @@ limitations under the License.
         using namespace nav_msgs::msg;
         using Time = builtin_interfaces::msg::Time;
     }
+    
+
+    #define LOG_ERROR(...)
+    #define LOG_WARN(...)
+    #define LOG_INFO(...)
+    #define LOG_DEBUG(...)
 #else
 
     #include <ros/ros.h>
@@ -60,11 +86,43 @@ limitations under the License.
     #include <sensor_msgs/PointCloud2.h>
     #include <sensor_msgs/point_cloud2_iterator.h>
     #include <nav_msgs/Odometry.h>
+    #include <sensor_msgs/Image.h>
+    using ImageConstPtr = sensor_msgs::ImageConstPtr;
+    using PointCloud2ConstPtr = sensor_msgs::PointCloud2ConstPtr;
     namespace ros {
         using namespace ::ros;
         using namespace sensor_msgs;
         using namespace nav_msgs;
     }
+    
+
+    #define LOG_ERROR(...) \
+        if (g_log_level >= LOG_LEVEL_ERROR) { \
+            ROS_ERROR(__VA_ARGS__); \
+        }
+    #define LOG_WARN(...) \
+        if (g_log_level >= LOG_LEVEL_WARN) { \
+            ROS_WARN(__VA_ARGS__); \
+        }
+    #define LOG_INFO(...) \
+        if (g_log_level >= LOG_LEVEL_INFO) { \
+            ROS_INFO(__VA_ARGS__); \
+        }
+    #define LOG_DEBUG(...) \
+        if (g_log_level >= LOG_LEVEL_DEBUG) { \
+            ROS_DEBUG(__VA_ARGS__); \
+        }
+#endif
+
+
+#ifdef ROS2
+    namespace sensor_msgs {
+        using PointField = msg::PointField;
+    }
+#else
+    namespace sensor_msgs {
+        using PointField = ::sensor_msgs::PointField;
+    }
 #endif
 
 // Common definitions
@@ -117,6 +175,11 @@ public:
         }
     #endif
     
+    void set_log_level(int level) {
+        g_log_level = level;
+    }
+ 
+    rawCloudRender render_;
     void publishImu(icm_6aixs_data_t *stream) {
         #ifdef ROS2
             sensor_msgs::msg::Imu imu_msg;
@@ -146,42 +209,294 @@ public:
             imu_pub_.publish(imu_msg);
         #endif
     }
-
+#ifdef ROS2
-    void publishIntensityCloud(capture_Image_List_t* stream, int idx) 
+    using ImageMsg = sensor_msgs::msg::Image;
+    using PointCloud2Msg = sensor_msgs::msg::PointCloud2;
+    using ImageConstPtr = ImageMsg::ConstSharedPtr;
+    using PointCloud2ConstPtr = PointCloud2Msg::ConstSharedPtr;
+#else
+    using ImageMsg = sensor_msgs::Image;
+    using PointCloud2Msg = sensor_msgs::PointCloud2;
+    using ImageConstPtr = sensor_msgs::ImageConstPtr;
+    using PointCloud2ConstPtr = sensor_msgs::PointCloud2ConstPtr;
+#endif
+void try_process_pair() {
+    // Record queue status
+    size_t rgb_size, pcd_size;
     {
-        #ifdef ROS2
+        std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
-            sensor_msgs::msg::PointCloud2 msg;
+        std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
-            msg.header.frame_id = "map";
+        rgb_size = rgb_image_queue_.size();
-            msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+        pcd_size = pcd_queue_.size();
+    }
     
-            msg.height = stream->imageList[idx].height;
+    while (true) {
-            msg.width = stream->imageList[idx].width;
+        ImageConstPtr rgb_msg = nullptr;
-            msg.is_dense = false;
+        PointCloud2ConstPtr pcd_msg = nullptr;
         
-            sensor_msgs::PointCloud2Modifier modifier(msg);
+        // Get a pair of data from queues (with lock protection)
+        {
+            std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
+            std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
+            
+            if (!rgb_image_queue_.empty() && !pcd_queue_.empty()) {
+                rgb_msg = rgb_image_queue_.front();
+                pcd_msg = pcd_queue_.front();
+            }
+        }
+        
+        if (!rgb_msg || !pcd_msg) {
+            break;
+        }
+        
+        // timestamp
+        uint64_t rgb_stamp = ros_time_to_ns(rgb_msg->header.stamp);
+        uint64_t pcd_stamp = ros_time_to_ns(pcd_msg->header.stamp);
+        int64_t time_diff = static_cast<int64_t>(rgb_stamp) - static_cast<int64_t>(pcd_stamp);
+        int64_t abs_time_diff = std::abs(time_diff);
+        
+        // Check if time difference is within allowed range (50ms)
+        const int64_t MAX_TIME_DIFF = 50000000; // 50ms in nanoseconds
+        if (abs_time_diff > MAX_TIME_DIFF) {
+            // Remove older timestamped message
+            {
+                std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
+                std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
+                
+                if (time_diff > 0) {
+                    // RGB timestamp is newer, remove PCD
+                    pcd_queue_.pop_front();
+                } else {
+                    // PCD timestamp is newer, remove RGB
+                    rgb_image_queue_.pop_front();
+                }
+            }
+            
+            // Try next pair
+            continue;
+        }
+        
+        // Time difference within allowed range, process data pair
+        {
+            std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
+            std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
+            
+            // Remove messages from queues
+            rgb_image_queue_.pop_front();
+            pcd_queue_.pop_front();
+        }
+        
+        // Process data pair
+        process_pair(rgb_msg, pcd_msg);
+    }
+}
+bool validate_render_parameters(std::vector<std::vector<float>>& rgb_image, 
+                               capture_Image_List_t* cloud_stream, 
+                               int pcd_idx) 
+{
+    // 1. Check RGB image validity
+    if (rgb_image.empty()) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid RGB image: empty vector");
+        #endif
+        return false;
+    }
+    
+    // Check RGB image dimension consistency
+    const size_t height = rgb_image.size();
+    const size_t width = (height > 0) ? rgb_image[0].size() : 0;
+    
+    if (height == 0 || width == 0) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid RGB image dimensions: %zux%zu", 
+                     height, width);
+        #endif
+        return false;
+    }
+    
+    // 2. Check point cloud stream pointer validity
+    if (!cloud_stream) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid cloud stream: null pointer");
+        #endif
+        return false;
+    }
+    
+    // 3. Check point cloud index validity
+    if (pcd_idx < 0 || pcd_idx >= 10) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid pcd index: %d (must be 0-9)", pcd_idx);
+        #endif
+        return false;
+    }
+    
+    // 4. Check point cloud data validity
+    buffer_List_t& cloud = cloud_stream->imageList[pcd_idx];
+    if (!cloud.pAddr) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid cloud data: null pointer");
+        #endif
+        return false;
+    }
+    
+    if (cloud.width <= 0 || cloud.height <= 0) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid cloud dimensions: %dx%d", 
+                     cloud.width, cloud.height);
+        #endif
+        return false;
+    }
+    
+    return true;
+}
+
+void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_msg)
+{
+    auto start_time = std::chrono::steady_clock::now();
+
+    const int input_image_width = rgb_msg->width;
+    const int input_image_height = rgb_msg->height;
+
+    // Verify input image format
+    if (rgb_msg->encoding != "bgr8") {
+        #ifndef ROS2
+            ROS_ERROR("Unsupported image format: %s. Only bgr8 is supported.", 
+                      rgb_msg->encoding.c_str());
+        #endif
+        return;
+    }
+
+    // Prepare point cloud data stream
+    capture_Image_List_t cloud_stream;
+    const int pcd_idx = 2;
+    cloud_stream.imageList[pcd_idx].height = pcd_msg->height;
+    cloud_stream.imageList[pcd_idx].width = pcd_msg->width;
+
+    const auto total_point_num = pcd_msg->width * pcd_msg->height;
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*pcd_msg, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*pcd_msg, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*pcd_msg, "z");
+    std::vector<float> cloud_flat;
+    cloud_flat.reserve(total_point_num * 4);
+
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+        cloud_flat.push_back(*iter_y * -1000.0f);
+        cloud_flat.push_back(*iter_z * 1000.0f);
+        cloud_flat.push_back(*iter_x * 1000.0f);
+        cloud_flat.push_back(0.0f);
+    }
+    cloud_stream.imageList[pcd_idx].pAddr = cloud_flat.data();
+
+    
+    if (input_image_height > 0 && input_image_width > 0) {
+        // Use BGR8 image data directly
+        std::vector<std::vector<float>> rgb_image(input_image_height, std::vector<float>(input_image_width));
+        
+        // Convert BGR8 data to required format for rendering
+        const uint8_t* bgr_data = rgb_msg->data.data();
+        for (int y = 0; y < input_image_height; ++y) {
+            for (int x = 0; x < input_image_width; ++x) {
+                // Start position of each pixel (BGR format)
+                int idx = y * rgb_msg->step + x * 3;
+                
+                // Extract RGB values and combine into 32-bit integer
+                uint8_t b = bgr_data[idx];
+                uint8_t g = bgr_data[idx + 1];
+                uint8_t r = bgr_data[idx + 2];
+                uint32_t rgb_int = (r << 16) | (g << 8) | b;
+                
+                // Convert to float
+                float rgb_float;
+                std::memcpy(&rgb_float, &rgb_int, sizeof(float));
+                rgb_image[y][x] = rgb_float;
+            }
+        }
+        // Render colored point cloud
+        std::vector<float> rgbCloud_flat;
+        render_.render(rgb_image, &cloud_stream, pcd_idx, rgbCloud_flat);
+        const int valid_point_num = rgbCloud_flat.size() / 4;
+         
+        // Create and publish RGB point cloud
+        PointCloud2Msg output_msg;
+        output_msg.header.frame_id = "map";
+        output_msg.header.stamp = rgb_msg->header.stamp; // Use original image timestamp
+        output_msg.height = 1;
+        output_msg.width = valid_point_num;
+
+        sensor_msgs::PointCloud2Modifier modifier(output_msg);
         modifier.setPointCloud2Fields(
             4,
-                "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+            "x", 1, sensor_msgs::PointField::FLOAT32,
-                "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+            "y", 1, sensor_msgs::PointField::FLOAT32,
-                "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+            "z", 1, sensor_msgs::PointField::FLOAT32,
-                "intensity", 1, sensor_msgs::msg::PointField::UINT8
+            "rgb", 1, sensor_msgs::PointField::FLOAT32
         );
-            modifier.resize(stream->imageList[idx].width * stream->imageList[idx].height);
+        modifier.resize(valid_point_num);
 
-            sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
+        sensor_msgs::PointCloud2Iterator<float> iter_res_x(output_msg, "x");
-            sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
+        sensor_msgs::PointCloud2Iterator<float> iter_res_y(output_msg, "y");
-            sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
+        sensor_msgs::PointCloud2Iterator<float> iter_res_z(output_msg, "z");
-            sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(msg, "intensity");
+        sensor_msgs::PointCloud2Iterator<float> iter_res_rgb(output_msg, "rgb");
+
+        for (int i = 0; i < valid_point_num; i++) {
+            *iter_res_x = rgbCloud_flat[4*i]; ++iter_res_x;
+            *iter_res_y = rgbCloud_flat[4*i+1]; ++iter_res_y;
+            *iter_res_z = rgbCloud_flat[4*i+2]; ++iter_res_z;
+            *iter_res_rgb = rgbCloud_flat[4*i+3]; ++iter_res_rgb;
+        }
+
+        #ifdef ROS2
+            rgbcloud_pub_->publish(output_msg);
         #else
-            sensor_msgs::PointCloud2 msg;
+            rgbcloud_pub_.publish(output_msg);
-            msg.header.frame_id = "map";
+        #endif
-            msg.header.stamp = ros::Time::now();  
+    } 
-            msg.height = stream->imageList[idx].height;
+}
-            msg.width = stream->imageList[idx].width;
-            msg.is_dense = false;
-            msg.is_bigendian = false;
 
-            sensor_msgs::PointCloud2Modifier modifier(msg);
+   void publishIntensityCloud(capture_Image_List_t* stream, int idx) 
+{
+    // Check index validity
+    if (idx < 0 || idx >= 10) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid index %d for intensity cloud", idx);
+        #endif
+        return;
+    }
+
+    // Check point cloud data validity
+    buffer_List_t &cloud = stream->imageList[idx];
+    if (!cloud.pAddr) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid point cloud: null data pointer at index %d", idx);
+        #endif
+        return;
+    }
+ 
+    if (cloud.width <= 0 || cloud.height <= 0) {
+        #ifndef ROS2
+            ROS_ERROR("Invalid point cloud dimensions: %dx%d at index %d", 
+                     cloud.width, cloud.height, idx);
+        #endif
+        return;
+    }
+
+    #ifdef ROS2
+        auto msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+    #else
+        auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
+    #endif
+
+    // Set message header
+    msg->header.frame_id = "map";
+    msg->header.stamp = ns_to_ros_time(cloud.timestamp);
+
+    msg->height = cloud.height;
+    msg->width = cloud.width;
+    msg->is_dense = false;
+    msg->is_bigendian = false;
+
+    // Set point cloud fields
+    sensor_msgs::PointCloud2Modifier modifier(*msg);
     modifier.setPointCloud2Fields(
         4,
         "x", 1, sensor_msgs::PointField::FLOAT32,
@@ -189,145 +504,172 @@ public:
         "z", 1, sensor_msgs::PointField::FLOAT32,
         "intensity", 1, sensor_msgs::PointField::UINT8
     );
-            modifier.resize(msg.height * msg.width);
+    modifier.resize(msg->height * msg->width);
 
-            sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
+    // Fill point cloud data
-            sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
+    sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
-            sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
+    sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
-            sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(msg, "intensity");
+    sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
-        #endif
+    sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*msg, "intensity");
 
-        float* xyz_data = static_cast<float*>(stream->imageList[idx].pAddr);
+    float* xyz_data = static_cast<float*>(cloud.pAddr);
     uint16_t* intensity_data = static_cast<uint16_t*>(stream->imageList[2].pAddr);
 
-        int total_points = stream->imageList[idx].height * stream->imageList[idx].width;
+    int total_points = cloud.height * cloud.width;
     for (int i = 0; i < total_points; ++i) {
         float* pf = xyz_data + i * 4;
 
-            #ifdef ROS2
         *iter_x = pf[2] / 1000.0f; ++iter_x;
         *iter_y = -pf[0] / 1000.0f; ++iter_y;
         *iter_z = pf[1] / 1000.0f; ++iter_z;
         *iter_intensity = static_cast<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
-            #else
-                *iter_x = pf[2] / 1000.0f; ++iter_x;
-                *iter_y = -pf[0] / 1000.0f; ++iter_y;
-                *iter_z = pf[1] / 1000.0f; ++iter_z;
-                *iter_intensity = static_cast<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
-            #endif
     }
 
-        // Publish Intensity point cloud
+{
+    std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
+    
+    // Get actual point count
+    const int real_point_count = cloud.width * cloud.height;  
+    // Create deep copy of point cloud
     #ifdef ROS2
-            cloud_pub_->publish(std::move(msg));
+        auto msg_copy = std::make_shared<sensor_msgs::msg::PointCloud2>(*msg);
+    #else
+        auto msg_copy = boost::make_shared<sensor_msgs::PointCloud2>();
+        *msg_copy = *msg;  // Deep copy
+    #endif
+    
+    // Queue management
+    if (pcd_queue_.size() >= 10) {
+        pcd_queue_.pop_front();
+    }
+    
+    // Add to queue (using copy)
+    pcd_queue_.push_back(msg_copy);
+}
+
+    // Publish point cloud
+    #ifdef ROS2
+        cloud_pub_->publish(*msg);
     #else
         cloud_pub_.publish(msg);
     #endif
-    }
+}
 
     void publishRgb(capture_Image_List_t *stream) {
     buffer_List_t &image = stream->imageList[0];
 
-        // Validate image parameters
-        if (!image.pAddr) {
-            #ifdef ROS2
-                RCLCPP_ERROR(node_->get_logger(), "Invalid RGB image: null data pointer");
-            #else
-                ROS_ERROR("Invalid RGB image: null data pointer");
-            #endif
-            return;
-        }
-        
-        if (image.width <= 0 || image.height <= 0) {
-            #ifdef ROS2
-                RCLCPP_ERROR(node_->get_logger(), "Invalid RGB image dimensions: %dx%d", image.width, image.height);
-            #else
-                ROS_ERROR("Invalid RGB image dimensions: %dx%d", image.width, image.height);
-            #endif
-            return;
-        }
-        
-        // Calculate NV12 image height
-        const int height_nv12 = image.height * 3 / 2;
-        
-        // Validate NV12 image size
-        const size_t expected_size = static_cast<size_t>(image.width) * height_nv12;
-        if (image.length < expected_size) {
-            #ifdef ROS2
-                RCLCPP_ERROR(node_->get_logger(), "RGB buffer too small: expected %zu bytes, got %u bytes", 
-                            expected_size, image.length);
-            #else
-                ROS_ERROR("RGB buffer too small: expected %zu bytes, got %u bytes", 
-                         expected_size, image.length);
-            #endif
-            return;
-        }
-        
     try {
+        const int height_nv12 = image.height * 3 / 2;
         cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr);
         cv::Mat bgr;
         cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12);
 
         if (bgr.empty()) {
-                #ifdef ROS2
+            #ifndef ROS2
-                    RCLCPP_ERROR(node_->get_logger(), "Failed to convert NV12 to BGR");
-                #else
                 ROS_ERROR("Failed to convert NV12 to BGR");
             #endif
             return;
         }
 
+        //Create ROS image message
         #ifdef ROS2
-                std_msgs::msg::Header header;
+            auto header = std::make_shared<std_msgs::msg::Header>();
-                sensor_msgs::msg::Image::SharedPtr msg;
+            header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
-            #else
+            header->frame_id = "camera_rgb_frame";
-                std_msgs::Header header;
-                sensor_msgs::Image msg;
-            #endif
             
-            header.stamp = ns_to_ros_time(image.timestamp + 719060);
+            auto cv_image = std::make_shared<cv_bridge::CvImage>(*header, "bgr8", bgr);
+            auto msg = cv_image->toImageMsg();
+        
+            // Add to unified queue
+            if (g_sendcloudrender) {
+                std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
+                if (rgb_image_queue_.size() >= 10) {
+                    rgb_image_queue_.pop_front();
+                }
+                rgb_image_queue_.push_back(msg);
+            }
+
+            // Publish original image message
+            rgb_pub_->publish(*msg);
+            
+            // Create compressed image message
+            auto compressed_msg = std::make_shared<sensor_msgs::msg::CompressedImage>();
+            compressed_msg->header = *header;
+            compressed_msg->format = "jpeg";
+            
+            // Set compression parameters
+            std::vector<int> compression_params;
+            compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
+            compression_params.push_back(80);
+            
+            // Compress image
+            cv::imencode(".jpg", bgr, compressed_msg->data, compression_params);
+            
+            compressed_rgb_pub_->publish(*compressed_msg);
+            
+        #else
+            // ROS1 version
+            std_msgs::Header header;
+            header.stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
             header.frame_id = "camera_rgb_frame";
             
-            #ifdef ROS2
+            auto cv_image = boost::make_shared<cv_bridge::CvImage>(header, "bgr8", bgr);
-                auto cv_image = std::make_shared<cv_bridge::CvImage>(header, "bgr8", bgr);
+            auto msg = cv_image->toImageMsg();
-                msg = cv_image->toImageMsg();
+            
-                rgb_pub_->publish(*msg);
+            // Add to unified queue
-            #else
+            if (g_sendcloudrender) {
-                cv_bridge::CvImage(header, "bgr8", bgr).toImageMsg(msg);
+                std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
+                if (rgb_image_queue_.size() >= 10) {
+                    rgb_image_queue_.pop_front();
+                }
+                rgb_image_queue_.push_back(msg);
+            }
+            
+            // Publish original image message
             rgb_pub_.publish(msg);
+            
+            // Publish compressed image - always publish
+            // Create compressed image message
+            sensor_msgs::CompressedImagePtr compressed_msg(new sensor_msgs::CompressedImage());
+            compressed_msg->header = header;
+            compressed_msg->format = "jpeg";
+            
+            // Set compression parameters
+            std::vector<int> compression_params;
+            compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
+            compression_params.push_back(80); // JPEG quality 80%
+            
+            // Compress image
+            cv::imencode(".jpg", bgr, compressed_msg->data, compression_params);
+            
+            compressed_rgb_pub_.publish(compressed_msg);
+            
         #endif
 
     } catch (const cv::Exception& e) {
-            #ifdef ROS2
+        #ifndef ROS2
-                RCLCPP_ERROR(node_->get_logger(), "OpenCV error in publishRgb: %s", e.what());
-            #else
             ROS_ERROR("OpenCV error in publishRgb: %s", e.what());
         #endif
     } catch (const std::exception& e) {
-            #ifdef ROS2
+        #ifndef ROS2
-                RCLCPP_ERROR(node_->get_logger(), "Exception in publishRgb: %s", e.what());
-            #else
             ROS_ERROR("Exception in publishRgb: %s", e.what());
         #endif
     }
-    }
+}
+
 
     void publishPC2XYZRGBA(capture_Image_List_t* stream, int idx) 
     {
-        static int flag = 1;
         #ifdef ROS2
                 sensor_msgs::msg::PointCloud2 msg;
-                // msg.header.frame_id = "base_link";
                 msg.header.frame_id = "map";
                 msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
-                // msg.header.stamp = this->now();
                 size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4;
                 uint32_t points = stream->imageList[idx].length / pt_size;
 
                 msg.height = 1;
                 msg.width = points;
                 msg.is_dense = false;
-                // LOG_INFO("msg.height=%ld, msg.width=%ld.\n", msg.height, msg.width);
 
                 sensor_msgs::PointCloud2Modifier modifier(msg);
                 modifier.setPointCloud2Fields(
@@ -402,10 +744,8 @@ public:
         }
         
         #ifdef ROS2
-            flag = 0; // Preserve assignment if flag is used elsewhere
             xyzrgbacloud_pub_->publish(std::move(msg));
         #else
-            flag = 0;
             xyzrgbacloud_pub_.publish(msg);
         #endif
     }
@@ -440,6 +780,67 @@ public:
     }
 
 private:
+    // Add the following member variables
+    std::mutex rgb_queue_mutex_;
+    std::deque<ImageConstPtr> rgb_image_queue_;
+    const size_t max_rgb_queue_size_ = 10; // Cache up to 10 image frames
+    
+    std::mutex pcd_queue_mutex_;
+    std::deque<PointCloud2ConstPtr> pcd_queue_;
+    const size_t max_pcd_queue_size_ = 10; // Maximum cache frames
+
+
+    // Updated helper functions
+    ImageConstPtr getLatestRgbImage() {
+        std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
+        return (!rgb_image_queue_.empty()) ? rgb_image_queue_.back() : nullptr;
+    }
+
+    PointCloud2ConstPtr getLatestIntensityCloud() {
+        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
+        return (!pcd_queue_.empty()) ? pcd_queue_.back() : nullptr;
+    }
+    std::vector<cv::Mat> getRgbImageQueueSnapshot() {
+        std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
+        std::vector<cv::Mat> images;
+        for (const auto& msg : rgb_image_queue_) {
+            try {
+                cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, "bgr8");
+                images.push_back(cv_ptr->image.clone());
+            } catch (cv_bridge::Exception& e) {
+                #ifndef ROS2
+                    ROS_ERROR("cv_bridge exception: %s", e.what());
+                #endif
+            }
+        }
+        return images;
+    }
+
+
+#ifdef ROS2
+    std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
+        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
+        std::vector<sensor_msgs::msg::PointCloud2> clouds;
+        
+        for (const auto& msg_ptr : pcd_queue_) {
+            clouds.push_back(*msg_ptr);
+        }
+        
+        return clouds;
+    }
+#else
+    std::vector<sensor_msgs::PointCloud2> getIntensityCloudQueueSnapshot() {
+        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
+        std::vector<sensor_msgs::PointCloud2> clouds;
+        
+        for (const auto& msg_ptr : pcd_queue_) {
+            clouds.push_back(*msg_ptr);
+        }
+        
+        return clouds;
+    }
+#endif
+
     void initialize_publishers() {
         #ifdef ROS2
             imu_pub_ = node_->create_publisher<ros::Imu>("odin1/imu", 10);
@@ -447,9 +848,10 @@ private:
             cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", 10);
             xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
             odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_map", 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);
         #endif
     }
-    
     #ifdef ROS1
         void initialize_publishers(ros::NodeHandle& nh) {
             imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 10);
@@ -457,6 +859,8 @@ private:
             cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 10);
             xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 10);
             odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_map", 10);
+            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
+            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
         }
     #endif
 
@@ -467,12 +871,20 @@ private:
         rclcpp::Publisher<ros::PointCloud2>::SharedPtr cloud_pub_;
         rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
         rclcpp::Publisher<ros::Odometry>::SharedPtr odom_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
     #else
         ros::Publisher imu_pub_;
         ros::Publisher rgb_pub_;
         ros::Publisher cloud_pub_;
         ros::Publisher xyzrgbacloud_pub_;
         ros::Publisher odom_publisher_;
+        ros::Publisher rendered_cloud_pub_;
+        ros::Publisher rgbcloud_pub_;
+        ros::Publisher rgbFromnv12_pub_;
+        ros::Publisher compressed_rgb_pub_; // New compressed image publisher
     #endif
 };
 
@@ -494,13 +906,9 @@ public:
             if (it != kv_map.end()) {
                 if (it->second != value) {
                     it->second = value;
-                    std::cout << "Set " << key << " = " << value << std::endl;
                     cb_to_invoke = callback;
-                } else {
-                    std::cout << "Set ignored: " << key << " is already " << value << std::endl;
                 } 
             } else {
-                std::cout << "Unknown key: " << key << std::endl;
                 return;
             }
         }
@@ -516,13 +924,6 @@ public:
         return (it != kv_map.end()) ? it->second : -1;
     }
 
-    void print_all() const {
-        std::lock_guard<std::mutex> lock(mtx);
-        std::cout << "Available keys and values:" << std::endl;
-        for (const auto& [key, value] : kv_map) {
-            std::cout << "  " << key << " = " << value << std::endl;
-        }
-    }
 
     void register_callback(Callback cb) {
         std::lock_guard<std::mutex> lock(mtx);

include/lidar_api.h

@@ -110,6 +110,17 @@ int lidar_open_device(device_handle device);
  * @param device Handle to the device to close
  * @return int 0 on success, negative error code on failure
  */
+int lidar_get_calib_file(device_handle device, const char* path);
+
+/**
+ * @brief Get device calibration parameters
+ * 
+ * Retrieves the current calibration parameters from the device.
+ * 
+ * @param device Handle to the target device
+ * @param param Pointer to receive the calibration parameters
+ * @return int 0 on success, negative error code on failure
+ */
 int lidar_close_device(device_handle device);
 
 /**

include/rawCloudRender.h

@@ -0,0 +1,83 @@
+/*
+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.
+*/
+#ifndef RGBCLOUD_H
+#define RGBCLOUD_H
+
+#include <vector>
+#include <string>
+#include <Eigen/Dense>
+#include "lidar_api.h"
+
+namespace GlobalCameraParams {
+    extern float g_fx;
+    extern float g_fy;
+    extern float g_cx;
+    extern float g_cy;
+    extern float g_skew;
+    extern float g_k2;
+    extern float g_k3;
+    extern float g_k4;
+    extern float g_k5;
+    extern float g_k6;
+    extern float g_k7;
+    extern Eigen::Matrix4f g_T_camera_lidar;
+}
+
+class rawCloudRender {
+public:
+  
+
+    bool init(const std::string& yamlFilePath);
+    
+    void nv12buffer_2_rgb(buffer_List_t &image, std::vector<std::vector<float>>& rgb_image);
+    void render(std::vector<std::vector<float>>& rgb_image, capture_Image_List_t* pcdStream, int pcdIdx, std::vector<float>& rgbCloud_flat);
+    
+    void print_camera_calib();
+    
+    int getImageWidth() const { return image_width_; }
+    int getImageHeight() const { return image_height_; }
+    
+   
+
+private:
+    std::string model_type_;
+    std::string camera_name_;
+    int image_width_;
+    int image_height_;
+    int frame_size_;
+    bool opencv_available_;
+    
+    // 4x4 transformation matrix (T_camera_lidar)
+    Eigen::Matrix4f T_camera_lidar_;
+
+    float k2_;
+    float k3_;
+    float k4_;
+    float k5_;
+    float k6_;
+    float k7_;
+    float p1_;
+    float p2_;
+    float A11_fx_;
+    float A12_skew_;
+    float A22_fy_;
+    float u0_cx_;
+    float v0_cy_;
+    bool isFast_;
+    int numDiff_;
+    float maxIncidentAngle_; 
+    
+
+};
+
+#endif  // RGBCLOUD_H

src/host_sdk_sample.cpp

@@ -1,22 +1,67 @@
 #include "host_sdk_sample.h"
 #include "yaml_parser.h"
+#include "rawCloudRender.h"
 #include <filesystem> 
 #include <thread>
 #include <string>
 #include <stdexcept>
 #include <atomic>
+#include <mutex>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <deque> 
+#include <unistd.h> 
+#include <cstdlib>
+#include <cstring>
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <signal.h>
+#include <chrono>
 
 #ifdef ROS2
     #include <ament_index_cpp/get_package_share_directory.hpp>
+    #include <rclcpp/rclcpp.hpp>
 #else
     #include <ros/package.h>
+    #include <ros/ros.h> 
 #endif
 
+// Global variable declarations
 static device_handle odinDevice = nullptr;
-static std::shared_ptr<MultiSensorPublisher> g_ros_object;
 static std::atomic<bool> deviceConnected(false);
+static std::atomic<bool> deviceDisconnected(false);  // Device disconnection flag
+static std::mutex device_mutex;                      // Device operation mutex lock
 
-// Function to get package share path
+#ifdef ROS2
+    std::shared_ptr<MultiSensorPublisher> g_ros_object = nullptr;
+#else
+    MultiSensorPublisher* g_ros_object = nullptr;
+#endif
+
+int g_log_level = LOG_LEVEL_INFO;  
+int g_show_fps = 0;  // FPS display toggle control
+
+static std::mutex g_rgb_mutex;
+static std::shared_ptr<cv::Mat> g_latest_bgr;
+static uint64_t g_latest_rgb_timestamp = 0;
+static bool g_has_rgb = false;
+static capture_Image_List_t g_latest_rgb;
+static bool g_renderer_initialized = false;
+static std::shared_ptr<rawCloudRender> g_renderer = nullptr;
+
+// Global configuration variables
+int g_sendrgb = 1;
+int g_sendimu = 1;
+int g_senddtof = 1;
+int g_sendodom = 1;
+int g_sendcloudslam = 0;
+int g_sendcloudrender = 0;
+int g_sendrgb_compressed = 0;
+
+// Function declarations
+void clear_all_queues();
+
+// Get package share path
 std::string get_package_share_path(const std::string& package_name) {
 #ifdef ROS2
     try {
@@ -33,15 +78,31 @@ std::string get_package_share_path(const std::string& package_name) {
 #endif
 }
 
-// Global configuration variables
+// Get package path
-static int g_sendrgb = 1;
+std::string get_package_path(const std::string& package_name) {
-static int g_sendimu = 1;
+    #ifdef ROS2
-static int g_senddtof = 1;
+        return ament_index_cpp::get_package_share_directory(package_name);
-static int g_sendodom = 1;
+    #else
-static int g_sendcloudslam = 0;
+        return ros::package::getPath(package_name);
+    #endif
+}
 
+// Clear all queues
+void clear_all_queues() {
+    // Reset state variables
+    g_latest_bgr.reset();
+    g_latest_rgb_timestamp = 0;
+    g_has_rgb = false;
+}
+
+// Lidar data callback
 static void lidar_data_callback(const lidar_data_t *data, void *user_data)
 {
+    // If device is not connected, ignore all data
+    if (!deviceConnected) {
+        return;
+    }
+    
     device_handle *dev_handle = static_cast<device_handle *>(user_data);
     if(!dev_handle || !data) {
         printf("Invalid device handle or data.\n");
@@ -63,7 +124,7 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
             }
             break;
         case LIDAR_DT_RAW_DTOF:
-            if (g_senddtof) {
+        if (g_senddtof ) {
             g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
             }
           break;
@@ -83,6 +144,7 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
     }
 }
 
+// Lidar device callback
 static void lidar_device_callback(const lidar_device_info_t* device, bool attach)
 {
     int type = LIDAR_MODE_SLAM;
@@ -93,16 +155,13 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
             ROS_INFO("Device attaching...");
         #endif
         
-        // Clean up any existing device
+        // Clean up existing device resources
         if (odinDevice) {
-            lidar_stop_stream(odinDevice, type);
+            // Skip stopping data stream, unregistering callbacks, closing device, destroying device
-            lidar_unregister_stream_callback(odinDevice);
-            lidar_close_device(odinDevice);
-            lidar_destory_device(odinDevice);
             odinDevice = nullptr;
         }
         
-        // Use const_cast to remove const qualifier
+        // Create new device
         if (lidar_create_device(const_cast<lidar_device_info_t*>(device), &odinDevice)) {
             #ifdef ROS2
                 RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed");
@@ -124,7 +183,82 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
             return;
         }
         
-        // Set mode
+        // Get package path
+	const std::string package_name = "odin_ros_driver";
+	std::string config_dir = "";
+	#ifdef ROS2
+	    // Get source code directory (not install directory)
+	    char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
+	    if (ros_workspace) {
+		// Infer source directory from COLCON_PREFIX_PATH
+		std::string workspace_path(ros_workspace);
+		// Remove "/install" part
+		size_t pos = workspace_path.find("/install");
+		if (pos != std::string::npos) {
+		    config_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/config";
+		} else {
+		    // Fallback to install directory
+		    config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
+		}
+	    } else {
+		// Fallback to install directory
+		config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
+	    }
+	#else
+	    config_dir = ros::package::getPath(package_name) + "/config";
+	#endif
+
+        // Print path information
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Calibration files will be saved to: %s", config_dir.c_str());
+        #else
+            ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str());
+        #endif
+        
+        // Get calibration files - using modified function
+        if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
+            #else
+                ROS_ERROR("Failed to get calibration file");
+            #endif
+            lidar_close_device(odinDevice);
+            lidar_destory_device(odinDevice);
+            odinDevice = nullptr;
+            return;
+        }
+        
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files");
+        #else
+            ROS_INFO("Successfully retrieved calibration files");
+        #endif
+        
+        //  Move point cloud renderer initialization here
+        std::string calib_config = config_dir + "/calib.yaml";
+        if (std::filesystem::exists(calib_config)) {
+            g_renderer = std::make_shared<rawCloudRender>();
+            if (g_renderer->init(calib_config)) {
+            #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloud renderer initialized");
+            #else
+                    ROS_INFO("Point cloud renderer initialized");
+            #endif
+            } else {
+            #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to initialize point cloud renderer");
+            #else
+                    ROS_ERROR("Failed to initialize point cloud renderer");
+            #endif
+                }
+        } else {
+            #ifdef ROS2
+                    RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Renderer config file not found: %s", calib_config.c_str());
+            #else
+                    ROS_WARN("Renderer config file not found: %s", calib_config.c_str());
+            #endif
+        }
+        
         if (lidar_set_mode(odinDevice, type)) {
             #ifdef ROS2
                 RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed");
@@ -144,7 +278,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
 
         if (lidar_register_stream_callback(odinDevice, data_callback_info)) {
             #ifdef ROS2
-                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Register callback failed");
+                RCLCPP_ERROR(rclcpp::get_logger("device"), "Register callback failed");
             #else
                 ROS_ERROR("Register callback failed");
             #endif
@@ -185,6 +319,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
         }
         
         deviceConnected = true;
+        deviceDisconnected = false;  // Reset disconnection flag
         #ifdef ROS2
             RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device ready and streams activated");
         #else
@@ -197,39 +332,40 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
             ROS_INFO("Device detaching...");
         #endif
 
+        // Set device disconnection flag
         deviceConnected = false;
+        deviceDisconnected = true;
 
-        if (odinDevice) {
+        // Clear all message queues
-            lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM);
+        clear_all_queues();
-            lidar_unregister_stream_callback(odinDevice);
+
-            lidar_close_device(odinDevice);
+        #ifdef ROS2
-            lidar_destory_device(odinDevice);
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Waiting for device reconnection...");
-            odinDevice = nullptr;
+        #else
-        }
+            ROS_INFO("Waiting for device reconnection...");
+        #endif
     }
 }
 
 int main(int argc, char *argv[])
 {
-    // ROS initialization
+#ifdef ROS2
-    #ifdef ROS2
     rclcpp::init(argc, argv);
     auto node = std::make_shared<rclcpp::Node>("lydros_node");
     g_ros_object = std::make_shared<MultiSensorPublisher>(node);
-    #else
+#else
     ros::init(argc, argv, "lydros_node");
     ros::NodeHandle nh;
-        g_ros_object = std::make_shared<MultiSensorPublisher>(nh);
+    g_ros_object = new MultiSensorPublisher(nh);
-    #endif
+#endif
 
     try {
         std::string package_path = get_package_share_path("odin_ros_driver");
         std::string config_file = package_path + "/config/control_command.yaml";   
         
-        // Create YAML parser
-        odin_ros_driver::YamlParser parser(config_file);
         
-        // Load configuration
+
+        odin_ros_driver::YamlParser parser(config_file);
         if (!parser.loadConfig()) {
             #ifdef ROS2
                 RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_file.c_str());
@@ -239,32 +375,25 @@ int main(int argc, char *argv[])
             return -1;
         }
 
-        // Get key-value
         auto keys = parser.getRegisterKeys();
-        
-        // Print configuration
         parser.printConfig();
 
-        auto get_key_value = [&](const std::string& key_name, int default_value) -> int {  
+        auto get_key_value = [&](const std::string& key, int default_value) -> int {
-            auto it = keys.find(key_name);
+            auto it = keys.find(key);
-            if (it != keys.end()) {
+            return it != keys.end() ? it->second : default_value;
-                return it->second;
-            }
-            return default_value;
         };
 
-        // Read configuration values into global variables
         g_sendrgb       = get_key_value("sendrgb", 1);
         g_sendimu       = get_key_value("sendimu", 1);
         g_senddtof      = get_key_value("senddtof", 1);
         g_sendodom      = get_key_value("sendodom", 1);
         g_sendcloudslam = get_key_value("sendcloudslam", 0);
-        
+        g_sendcloudrender = get_key_value("sendcloudrender", 1);
-        // Set log level
+        g_sendrgb_compressed = get_key_value("sendrgbcompressed", 1);
+        g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
         lidar_log_set_level(LIDAR_LOG_INFO);
 
-        // Initialize system and start USB monitoring
+        if (lidar_system_init(lidar_device_callback)) {
-        if(lidar_system_init(lidar_device_callback)) {
         #ifdef ROS2
                     RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
         #else
@@ -273,28 +402,20 @@ int main(int argc, char *argv[])
                     return -1;
         }
 
-        // wait device connect
         #ifdef ROS2
                 RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
         #else
                 ROS_INFO("Waiting for device connection...");
         #endif
 
-        auto start = std::chrono::steady_clock::now();
+        // Wait indefinitely for device connection
         while (!deviceConnected) {
-            auto now = std::chrono::steady_clock::now();
-            auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start);
-            
-            if (elapsed.count() >= 30) {
             #ifdef ROS2
-                RCLCPP_ERROR(node->get_logger(), "No device connected after 30 seconds");
+                RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
             #else
-                ROS_ERROR("No device connected after 30 seconds");
+                ROS_INFO("Waiting for device connection...");
             #endif
-                lidar_system_deinit();
+            std::this_thread::sleep_for(std::chrono::seconds(1)); // Check every second
-                return -1;
-            }
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
         }
 
     } catch (const std::exception& e) {
@@ -307,17 +428,63 @@ int main(int argc, char *argv[])
                 return -1;
     }
 
-    // ROS loop
     #ifdef ROS2
-        rclcpp::spin(node);
+        // Create 10Hz Rate object
+        rclcpp::Rate rate(10);
+        while (rclcpp::ok()) {
+            rclcpp::spin_some(node);
+
+            // Check device disconnection status
+            if (deviceDisconnected.load()) {
+                #ifdef ROS2
+                    RCLCPP_INFO(node->get_logger(), "Device disconnected, waiting for reconnection...");
+                #else
+                    ROS_INFO("Device disconnected, waiting for reconnection...");
+                #endif
+                
+                // Wait 0.1 seconds
+                rate.sleep();
+                continue;  // Skip rest of this loop iteration
+            }
+            
+            // Data processing when device is connected
+            if (g_sendcloudrender) {
+                g_ros_object->try_process_pair();  
+            }
+
+            // Wait 0.1 seconds
+            rate.sleep();
+        }
         rclcpp::shutdown();
     #else
-        ros::spin();
+        // Create 10Hz Rate object
+        ros::Rate rate(10);
+        while (ros::ok()) {
+            ros::spinOnce();
+
+            // Check device disconnection status
+            if (deviceDisconnected.load()) {
+                ROS_INFO("Device disconnected, waiting for reconnection...");
+                
+                // Wait 0.1 seconds
+                rate.sleep();
+                continue;  // Skip rest of this loop iteration
+            }
+            
+            // Data processing when device is connected
+            if (g_sendcloudrender) {
+                g_ros_object->try_process_pair();  
+            }
+
+            // Wait 0.1 seconds
+            rate.sleep();
+        }
         ros::shutdown();
     #endif
 
-    // Cleanup
+    // Cleanup on normal program exit
     if (odinDevice) {
+        // Perform cleanup on normal exit
         lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM);
         lidar_unregister_stream_callback(odinDevice);
         lidar_close_device(odinDevice);

src/rawCloudRender.cpp

@@ -0,0 +1,286 @@
+#include "rawCloudRender.h"
+#include <yaml-cpp/yaml.h>
+#include <iostream>
+#include <vector>
+#include <Eigen/Dense>
+#include <algorithm> 
+#include <iostream> 
+#include <cmath>
+#include <array>
+#include <algorithm>
+
+struct ValidPointInfo {
+    float x;
+    float y;
+    float z;
+    int u;
+    int v;
+};
+
+
+namespace GlobalCameraParams {
+    float g_fx = 0.0f;
+    float g_fy = 0.0f;
+    float g_cx = 0.0f;
+    float g_cy = 0.0f;
+    float g_skew = 0.0f;
+    float g_k2 = 0.0f;
+    float g_k3 = 0.0f;
+    float g_k4 = 0.0f;
+    float g_k5 = 0.0f;
+    float g_k6 = 0.0f;
+    float g_k7 = 0.0f;
+    Eigen::Matrix4f g_T_camera_lidar = Eigen::Matrix4f::Identity();
+}
+bool raw_debug=0;
+bool rawCloudRender::init(const std::string& yamlFilePath) {
+    YAML::Node config;
+    try {
+        config = YAML::LoadFile(yamlFilePath);
+    } catch (const YAML::BadFile& e) {
+        std::cerr << "Error: Could not open file '" << yamlFilePath << "' - " << e.what() << std::endl;
+        return false;
+    } catch (const YAML::ParserException& e) {
+        std::cerr << "Error: YAML parsing failed - " << e.what() << std::endl;
+        return false;
+    } catch (const std::exception& e) {
+        std::cerr << "Unexpected error: " << e.what() << std::endl;
+        return false;
+    }
+
+    // Intrinsic parameters node (cam_0)
+    const std::string cam_node_name = "cam_0";
+    if (!config[cam_node_name]) {
+        std::cerr << "Error: Missing camera node '" << cam_node_name << "'" << std::endl;
+        return false;
+    }
+    YAML::Node cam_node = config[cam_node_name];
+
+    // Directly access Tcl_0 node
+    const std::string tcl_node_name = "Tcl_0";
+    if (!config[tcl_node_name]) {
+        std::cerr << "Error: Missing transformation matrix node '" << tcl_node_name << "'" << std::endl;
+        return false;
+    }
+    YAML::Node tclNode = config[tcl_node_name];
+    if (tclNode.size() != 16) {
+        std::cerr << "Error: Transformation matrix must be 4x4 (16 elements)" << std::endl;
+        return false;
+    }
+
+    // === Read intrinsic parameters ===
+    GlobalCameraParams::g_k2 = cam_node["k2"].as<float>();
+    GlobalCameraParams::g_k3 = cam_node["k3"].as<float>();
+    GlobalCameraParams::g_k4 = cam_node["k4"].as<float>();
+    GlobalCameraParams::g_k5 = cam_node["k5"].as<float>();
+    GlobalCameraParams::g_k6 = cam_node["k6"].as<float>();
+    GlobalCameraParams::g_k7 = cam_node["k7"].as<float>();
+    
+    GlobalCameraParams::g_fx = cam_node["A11"].as<float>();
+    GlobalCameraParams::g_skew = cam_node["A12"].as<float>();
+    GlobalCameraParams::g_fy = cam_node["A22"].as<float>();
+    
+    GlobalCameraParams::g_cx = cam_node["u0"].as<float>();
+    GlobalCameraParams::g_cy = cam_node["v0"].as<float>();
+
+    // === Read extrinsic transformation matrix ===
+    GlobalCameraParams::g_T_camera_lidar << 
+        tclNode[0].as<float>(), tclNode[1].as<float>(), tclNode[2].as<float>(), tclNode[3].as<float>(),
+        tclNode[4].as<float>(), tclNode[5].as<float>(), tclNode[6].as<float>(), tclNode[7].as<float>(),
+        tclNode[8].as<float>(), tclNode[9].as<float>(), tclNode[10].as<float>(), tclNode[11].as<float>(),
+        tclNode[12].as<float>(), tclNode[13].as<float>(), tclNode[14].as<float>(), tclNode[15].as<float>();
+
+    // === Display key parameters concisely ===
+    std::cout << "=== Camera Calibration Parameters ===" << std::endl;
+    std::cout << "Intrinsics:" << std::endl;
+    std::cout << "  fx: " << GlobalCameraParams::g_fx 
+              << ", fy: " << GlobalCameraParams::g_fy
+              << ", cx: " << GlobalCameraParams::g_cx 
+              << ", cy: " << GlobalCameraParams::g_cy << std::endl;
+    std::cout << "Distortion: k2=" << GlobalCameraParams::g_k2 
+              << ", k3=" << GlobalCameraParams::g_k3 << std::endl;
+    
+    Eigen::Vector3f translation = GlobalCameraParams::g_T_camera_lidar.block<3,1>(0,3);
+    Eigen::Matrix3f rotation = GlobalCameraParams::g_T_camera_lidar.block<3,3>(0,0);
+    std::cout << "Extrinsics:" << std::endl;
+    std::cout << "  Translation: [" << translation.x() << ", " 
+              << translation.y() << ", " << translation.z() << "]" << std::endl;
+    std::cout << "  Rotation (euler angles): " 
+              << rotation.eulerAngles(0,1,2).transpose() * 180/M_PI << "°" << std::endl;
+    
+    return true;
+}
+void rawCloudRender::render(std::vector<std::vector<float>>& rgb_image, 
+                           capture_Image_List_t* pcd_stream, 
+                           int pcdIdx, 
+                           std::vector<float>& rgbCloud_flat) 
+{
+    // Initialize constants
+    constexpr float inv_1000 = 0.001f;
+    const float fx = GlobalCameraParams::g_fx;
+    const float fy = GlobalCameraParams::g_fy;
+    const float cx = GlobalCameraParams::g_cx;
+    const float cy = GlobalCameraParams::g_cy;
+    const float skew = GlobalCameraParams::g_skew;
+    const float k2 = GlobalCameraParams::g_k2;
+    const float k3 = GlobalCameraParams::g_k3;
+    const float k4 = GlobalCameraParams::g_k4;
+    const float k5 = GlobalCameraParams::g_k5;
+    const float k6 = GlobalCameraParams::g_k6;
+    const float k7 = GlobalCameraParams::g_k7;
+    const Eigen::Matrix4f& T = GlobalCameraParams::g_T_camera_lidar;
+    
+    // Precompute matrix elements
+    const float T00 = T(0,0), T01 = T(0,1), T02 = T(0,2), T03 = T(0,3);
+    const float T10 = T(1,0), T11 = T(1,1), T12 = T(1,2), T13 = T(1,3);
+    const float T20 = T(2,0), T21 = T(2,1), T22 = T(2,2), T23 = T(2,3);
+    
+    // Initialize lookup table
+    static std::array<float, 10000> dist_table;
+    static bool table_init = [&](){
+        for (size_t i=0; i<dist_table.size(); ++i) {
+            float theta = i * (M_PI/2) / dist_table.size();
+            dist_table[i] = theta*(1 + theta*(k2 + theta*(k3 + theta*(k4 + theta*(k5 + theta*(k6 + theta*k7))))));
+        }
+        return true;
+    }();
+    
+    // Get point cloud data (direct access)
+    if (!pcd_stream || pcdIdx < 0 || pcdIdx >= 10) {
+        std::cerr << "ERROR: Invalid pcd_stream or index in render function" << std::endl;
+        return;
+    }
+    
+    buffer_List_t& pcd_buffer = pcd_stream->imageList[pcdIdx];
+    const int total_points = pcd_buffer.height * pcd_buffer.width;
+    
+    if (!pcd_buffer.pAddr) {
+        std::cerr << "ERROR: Null point cloud data pointer in render function" << std::endl;
+        return;
+    }
+    
+    float* data = static_cast<float*>(pcd_buffer.pAddr);
+    
+    // Prepare output
+    rgbCloud_flat.clear();
+    rgbCloud_flat.resize(total_points * 4); // Preallocate maximum space
+    float* output_ptr = rgbCloud_flat.data();
+    
+    // Get image dimensions
+    const int img_height = 1296;
+    const int img_width = 1600;
+    
+    // Process point cloud
+    int valid_count = 0;
+    for (int idx = 0; idx < total_points; ++idx) 
+    {
+        float* pf = data + idx*4;
+        
+        // Quick check for invalid points
+        if (std::abs(pf[0]) < 1e-5f && std::abs(pf[1]) < 1e-5f && std::abs(pf[2]) < 1e-5f) {
+            continue;
+        }
+        
+        // Coordinate transformation
+        const float x = pf[2] * inv_1000;
+        const float y = -pf[0] * inv_1000;
+        const float z = pf[1] * inv_1000;
+        
+        // Manual matrix transformation
+        const float x1 = T00*x + T01*y + T02*z + T03;
+        const float y1 = T10*x + T11*y + T12*z + T13;
+        const float z1 = T20*x + T21*y + T22*z + T23;
+        
+        // Check for points behind camera
+        if (z1 <= 0.0f) continue;
+        
+        // Calculate projection
+        const float x1_sq = x1*x1;
+        const float y1_sq = y1*y1;
+        const float z1_sq = z1*z1;
+        
+        const float norm = std::sqrt(x1_sq + y1_sq + z1_sq);
+        if (norm < 1e-7f) continue;
+        
+        const float r = std::sqrt(x1_sq + y1_sq);
+        if (r < 1e-7f) continue;
+        
+        const float cost = z1 / norm;
+        const float theta = std::acos(cost);
+        
+        // Safe table lookup
+        const size_t table_idx = static_cast<size_t>(theta * (2.0f/M_PI) * dist_table.size());
+        const size_t safe_idx = std::min(table_idx, dist_table.size()-1);
+        const float thetad = dist_table[safe_idx];
+        
+        const float scaling = thetad / r;
+        const float xd = x1 * scaling;
+        const float yd = y1 * scaling;
+        const float pd_2d_x = xd * fx + yd * skew + cx;
+        const float pd_2d_y = yd * fy + cy;
+        
+        // Quick boundary check
+        const int u = static_cast<int>(pd_2d_x);
+        const int v = static_cast<int>(pd_2d_y);
+        
+        // Strict boundary check
+        if (u < 0 || u >= img_width || v < 0 || v >= img_height) {
+            continue;
+        }
+        
+        // Safe image data access
+        if (v < static_cast<int>(rgb_image.size()) && u < static_cast<int>(rgb_image[v].size())) {
+            *output_ptr++ = x;
+            *output_ptr++ = y;
+            *output_ptr++ = z;
+            *output_ptr++ = rgb_image[v][u];
+            valid_count++;
+        } else {
+            // Handle invalid coordinates
+            static bool warned = false;
+            if (!warned) {
+            	if(raw_debug)
+            	{
+            	          std::cerr << "WARNING: Invalid image coordinates: u=" << u << ", v=" << v 
+                          << " (image size: " << rgb_image.size() << "x" 
+                          << (rgb_image.empty() ? 0 : rgb_image[0].size()) << ")" << std::endl;
+            	}
+
+                warned = true;
+            }
+        }
+    }
+       // Resize output
+    	rgbCloud_flat.resize(output_ptr - rgbCloud_flat.data());
+        if(raw_debug)
+         {
+		std::cout << "Render completed: " << total_points << " points processed, " 
+			  << valid_count << " valid points (" 
+			  << (100.0 * valid_count / total_points) << "%)" << std::endl;
+         }
+}
+void rawCloudRender::print_camera_calib() {
+    std::cout << model_type_ << std::endl;
+    std::cout << image_width_ << std::endl;
+    std::cout << image_height_ << std::endl;
+
+    std::cout << "T_camera_lidar" << std::endl;
+    std::cout << T_camera_lidar_(0,0) << " " << T_camera_lidar_(0,1) << " " << T_camera_lidar_(0,2) << " " << T_camera_lidar_(0,3) << std::endl;
+    std::cout << T_camera_lidar_(1,0) << " " << T_camera_lidar_(1,1) << " " << T_camera_lidar_(1,2) << " " << T_camera_lidar_(1,3) << std::endl;
+    std::cout << T_camera_lidar_(2,0) << " " << T_camera_lidar_(2,1) << " " << T_camera_lidar_(2,2) << " " << T_camera_lidar_(2,3) << std::endl;
+    std::cout << T_camera_lidar_(3,0) << " " << T_camera_lidar_(3,1) << " " << T_camera_lidar_(3,2) << " " << T_camera_lidar_(3,3) << std::endl;
+
+    std::cout << "cam" << std::endl;
+    std::cout << k2_ << std::endl;
+    std::cout << k3_ << std::endl;
+    std::cout << k4_ << std::endl;
+    std::cout << k5_ << std::endl;
+    std::cout << k6_ << std::endl;
+    std::cout << k7_ << std::endl;
+
+    std::cout << A11_fx_ << std::endl;
+    std::cout << A12_skew_ << std::endl;
+    std::cout << A22_fy_ << std::endl;
+    std::cout << u0_cx_ << std::endl;
+    std::cout << v0_cy_ << std::endl;    
+}