diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1135cf7..4bf049f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -209,6 +209,12 @@ if(ROS_VERSION STREQUAL "ROS1")
     ${PCL_LIBRARIES}
     )
 
+    add_executable(image_overlay_node src/image_overlay_node.cpp)
+    target_link_libraries(image_overlay_node
+    ${catkin_LIBRARIES}
+    ${OpenCV_LIBS}
+    )
+
     # Installation rules
     install(TARGETS host_sdk_sample
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -340,12 +346,26 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         message_filters
     )
 
+    add_executable(image_overlay_node src/image_overlay_node.cpp)
+    target_compile_definitions(image_overlay_node PRIVATE ROS2)
+    target_link_libraries(image_overlay_node
+        ${OpenCV_LIBS}
+    )
+    ament_target_dependencies(image_overlay_node
+        rclcpp
+        sensor_msgs
+        cv_bridge
+        image_transport
+        message_filters
+    )
+
     # Installation rules - ensure all install targets are defined before ament_package()
     # Install executable
     install(TARGETS 
         host_sdk_sample 
         pcd2depth_ros2_node
         cloud_reprojection_ros2_node
+        image_overlay_node
         EXPORT export_${PROJECT_NAME}
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
diff --git a/config/control_command.yaml b/config/control_command.yaml
index a2ec35c..cc6a95f 100644
--- a/config/control_command.yaml
+++ b/config/control_command.yaml
@@ -6,7 +6,7 @@ register_keys:
   # 0: use odin internal system time as data time stamp, typical and recommended;
   # 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users
   # 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis
-  use_host_ros_time: 2
+  use_host_ros_time: 0  # Changed to 0 to use device timestamp without PTP offset correction
 
   streamctrl: 1           # 0: off; 1: on
 
@@ -25,6 +25,14 @@ register_keys:
   # IMU data
   sendimu: 1              # 0: off; 1: on
 
+  # SDK IMU smooth sending feature
+  # When enabled, SDK will send IMU data at precise intervals (default 400Hz) 
+  # using a dedicated high-priority thread to reduce jitter and timing variance
+  enable_imu_smooth: 0    # 0: disable SDK IMU smooth sending; 1: enable (default)
+
+  # SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
+  imu_smooth_frequency: 400  # 1-1000 Hz, recommended 400 Hz
+
   # Odometry data
   sendodom: 1             # 0: off; 1: on
 
@@ -57,6 +65,14 @@ register_keys:
   # Processed on host device
   sendreprojection: 0    # 0: off; 1: on
 
+  # image overlay settings - overlays reprojected points on camera image
+  # Processed on host device
+  sendoverlay: 0                                    # 0: off; 1: on
+  overlay_reprojected_topic: "/odin1/reprojected_image"  # reprojected image topic
+  overlay_camera_topic: "/odin1/image"       # camera image topic (undistorted)
+  overlay_output_topic: "/odin1/overlay_image"           # output overlay image topic
+  overlay_alpha: 0.6                                     # blend alpha (0.0-1.0, higher = more reproj color)
+
   # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software. 
   # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
   # ATTENTION: please copy the full folder for post-processing.
@@ -80,3 +96,10 @@ register_keys:
   # To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
   mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
   mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
+
+  # Image mask transfer settings
+  sendimagemask: 0        # 0: off; 1: on - transfer image mask to device on startup
+  image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296))
+
+  # Algorithm reset settings
+  resetalgo: 0            # 0: off; 1: on - send algo_reset command to device on startup
diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz
index 2a8eb7a..48d42c3 100644
--- a/config/odin_ros2.rviz
+++ b/config/odin_ros2.rviz
@@ -385,7 +385,7 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: false
-      Enabled: true
+      Enabled: false
       Name: dense_depth_demo
     - Class: rviz_default_plugins/TF
       Enabled: true
@@ -460,9 +460,9 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.28456413745880127
-        Y: 1.8771981000900269
-        Z: 0.38664406538009644
+        X: 0.2568470537662506
+        Y: 2.1451337337493896
+        Z: 0.3774382472038269
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -483,16 +483,16 @@ Window Geometry:
     collapsed: false
   Image_undistort:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d0061006700650300000293000000dd000002d400000205fb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000044b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1850
-  X: 70
-  Y: 27
+  Width: 1920
+  X: 540
+  Y: 124
   cloudslam_reprojected:
     collapsed: false
   dense_depth_image:
diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp
index 4408255..ef3c96b 100644
--- a/include/cloud_reprojection_ros_node.hpp
+++ b/include/cloud_reprojection_ros_node.hpp
@@ -23,6 +23,7 @@ limitations under the License.
     #include <message_filters/subscriber.h>
     #include <message_filters/time_synchronizer.h>
     #include <message_filters/sync_policies/exact_time.h>
+    #include <message_filters/sync_policies/approximate_time.h>
 #else
     #include <ros/ros.h>
     #include <sensor_msgs/PointCloud2.h>
@@ -32,6 +33,7 @@ limitations under the License.
     #include <message_filters/subscriber.h>
     #include <message_filters/time_synchronizer.h>
     #include <message_filters/sync_policies/exact_time.h>
+    #include <message_filters/sync_policies/approximate_time.h>
 #endif
 
 #include <pcl_conversions/pcl_conversions.h>
@@ -56,12 +58,14 @@ private:
 
     std::string cloud_slam_topic_;
     std::string odometry_topic_;
+    std::string wiwc_topic_;
     std::string reprojected_image_topic_;
 
     message_filters::Subscriber<PointCloud2> cloud_sub_;
     message_filters::Subscriber<Odometry> odom_sub_;
+    message_filters::Subscriber<Odometry> wiwc_sub_;
 
-    typedef message_filters::sync_policies::ExactTime<PointCloud2, Odometry> MySyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
     typedef message_filters::Synchronizer<MySyncPolicy> Sync;
     std::shared_ptr<Sync> sync_;
 
@@ -71,7 +75,8 @@ private:
 
     void loadParameters();
     void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
-                      const Odometry::ConstSharedPtr& odom_msg);
+                      const Odometry::ConstSharedPtr& odom_msg,
+                      const Odometry::ConstSharedPtr& wiwc_msg);
 };
 #else
 class CloudReprojectionRosNode
@@ -84,12 +89,14 @@ private:
 
     std::string cloud_slam_topic_;
     std::string odometry_topic_;
+    std::string wiwc_topic_;
     std::string reprojected_image_topic_;
 
     message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
     message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+    message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
 
-    typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
     typedef message_filters::Synchronizer<MySyncPolicy> Sync;
     std::shared_ptr<Sync> sync_;
 
@@ -99,6 +106,7 @@ private:
 
     void loadParameters();
     void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
-                      const nav_msgs::OdometryConstPtr& odom_msg);
+                      const nav_msgs::OdometryConstPtr& odom_msg,
+                      const nav_msgs::OdometryConstPtr& wiwc_msg);
 };
 #endif
diff --git a/include/cloud_reprojector.hpp b/include/cloud_reprojector.hpp
index e7ff884..77939fc 100644
--- a/include/cloud_reprojector.hpp
+++ b/include/cloud_reprojector.hpp
@@ -63,6 +63,13 @@ public:
     const CameraParams& getCameraParams() const { return camera_params_; }
     const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
 
+    // Update extrinsic parameters at runtime with real-time values from module
+    void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
+        extrinsic_params_.Tcl = Tcl;
+        extrinsic_params_.Til = Til;
+        extrinsic_params_.Tic = calculateTic(Tcl, Til);
+    }
+
     static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
 
 private:
diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h
index 5909823..0fd079e 100644
--- a/include/host_sdk_sample.h
+++ b/include/host_sdk_sample.h
@@ -1019,12 +1019,25 @@ void publishRgb(capture_Image_List_t *stream) {
                 for (int idx = 0; idx < 16; ++idx) {
                     T_CL(idx / 4, idx % 4) = static_cast<double>(odom_data->pose_cov[idx]);
                 }
+                // Force last row to be [0, 0, 0, 1] for valid transformation matrix
+                T_CL(3, 0) = 0.0; T_CL(3, 1) = 0.0; T_CL(3, 2) = 0.0; T_CL(3, 3) = 1.0;
                 
                 // Build TIL matrix (4x4) from twist_cov
                 Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
                 for (int idx = 0; idx < 16; ++idx) {
                     T_IL(idx / 4, idx % 4) = static_cast<double>(odom_data->twist_cov[idx]);
                 }
+                // Force last row to be [0, 0, 0, 1] for valid transformation matrix
+                T_IL(3, 0) = 0.0; T_IL(3, 1) = 0.0; T_IL(3, 2) = 0.0; T_IL(3, 3) = 1.0;
+                
+                // Debug print to compare with cloud_reprojection values
+                // static int host_print_count = 0;
+                // if (host_print_count++) {
+                //     std::cout << "=== host_sdk_sample T_CL from odom_data->pose_cov ===" << std::endl;
+                //     std::cout << T_CL << std::endl;
+                //     std::cout << "=== host_sdk_sample T_IL from odom_data->twist_cov ===" << std::endl;
+                //     std::cout << T_IL << std::endl;
+                // }
                 
                 // Extract rotation and translation from T_CL
                 Eigen::Matrix3d RCL = T_CL.block<3, 3>(0, 0);
@@ -1033,7 +1046,20 @@ void publishRgb(capture_Image_List_t *stream) {
                 // Extract rotation and translation from T_IL
                 Eigen::Matrix3d RIL = T_IL.block<3, 3>(0, 0);
                 Eigen::Vector3d TIL = T_IL.block<3, 1>(0, 3);
-                    
+
+                // Debug print extracted RCL and TCL
+                // static int rcl_print_count = 0;
+                // if (rcl_print_count++ ) {
+                //     std::cout << "=== host_sdk_sample RCL (3x3 rotation from T_CL) ===" << std::endl;
+                //     std::cout << RCL << std::endl;
+                //     std::cout << "=== host_sdk_sample TCL (3x1 translation from T_CL) ===" << std::endl;
+                //     std::cout << TCL.transpose() << std::endl;
+                //     std::cout << "=== host_sdk_sample RIL (3x3 rotation from T_IL) ===" << std::endl;
+                //     std::cout << RIL << std::endl;
+                //     std::cout << "=== host_sdk_sample TIL (3x1 translation from T_IL) ===" << std::endl;
+                //     std::cout << TIL.transpose() << std::endl;
+                // }
+                
                 // Save RIL, TIL, RCL, TCL to YAML file
                 static int save_count = 0;
                 static int index_count = 0;
@@ -1096,6 +1122,59 @@ void publishRgb(capture_Image_List_t *stream) {
 
     }
 
+    // Publish WIWC data (T_CL and T_IL extrinsics) as a separate topic
+    void publishWiwc(capture_Image_List_t* stream) {
+        uint32_t data_len = stream->imageList[0].length;
+        if (data_len != sizeof(ros_odom_convert_complete_t)) {
+            return;
+        }
+        
+        ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
+        
+#ifdef ROS2
+        auto msg = nav_msgs::msg::Odometry();
+        msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
+        msg.header.frame_id = "odom";
+#else
+        nav_msgs::Odometry msg;
+        msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
+        msg.header.frame_id = "odom";
+#endif
+        
+        // Store T_CL in pose.covariance (first 16 elements)
+        // Force last row to be [0, 0, 0, 1] for valid transformation matrix
+        for (int i = 0; i < 16; ++i) {
+            msg.pose.covariance[i] = odom_data->pose_cov[i];
+        }
+        msg.pose.covariance[12] = 0.0;
+        msg.pose.covariance[13] = 0.0;
+        msg.pose.covariance[14] = 0.0;
+        msg.pose.covariance[15] = 1.0;
+        // Fill remaining with zeros
+        for (int i = 16; i < 36; ++i) {
+            msg.pose.covariance[i] = 0.0;
+        }
+        
+        // Store T_IL in twist.covariance (first 16 elements)
+        // Force last row to be [0, 0, 0, 1] for valid transformation matrix
+        for (int i = 0; i < 16; ++i) {
+            msg.twist.covariance[i] = odom_data->twist_cov[i];
+        }
+        msg.twist.covariance[12] = 0.0;
+        msg.twist.covariance[13] = 0.0;
+        msg.twist.covariance[14] = 0.0;
+        msg.twist.covariance[15] = 1.0;
+        // Fill remaining with zeros
+        for (int i = 16; i < 36; ++i) {
+            msg.twist.covariance[i] = 0.0;
+        }
+        
+#ifdef ROS2
+        wiwc_publisher_->publish(msg);
+#else
+        wiwc_publisher_.publish(msg);
+#endif
+    }
 
     void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) {
         
@@ -1161,6 +1240,7 @@ void publishRgb(capture_Image_List_t *stream) {
                 msg.twist.twist.angular.y = static_cast<double>(odom_data->angular_velocity[1]) / 1e6;
                 msg.twist.twist.angular.z = static_cast<double>(odom_data->angular_velocity[2]) / 1e6;
 
+                // Copy original covariance data
                 for (int i = 0; i < 36; ++i) {
                     msg.pose.covariance[i] = odom_data->pose_cov[i];
                 }
@@ -1606,6 +1686,7 @@ private:
             compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_small);
             undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_sensor);
             intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_sensor);
+            wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_small);
             tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
         #endif
     }
@@ -1623,6 +1704,7 @@ private:
             compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 100);
             undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 100);
             intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 100);
+            wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 100);
             tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
         }
     #endif
@@ -1643,6 +1725,7 @@ private:
         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_;
+        rclcpp::Publisher<ros::Odometry>::SharedPtr wiwc_publisher_;
         camera_pose_visualization cameraposevisual_;
         std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
     #else
@@ -1661,6 +1744,7 @@ private:
         ros::Publisher compressed_rgb_pub_; // New compressed image publisher
         ros::Publisher undistort_rgb_pub_;
         ros::Publisher intensity_gray_pub_;
+        ros::Publisher wiwc_publisher_;
         std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
     #endif
 };
diff --git a/include/image_overlay_node.hpp b/include/image_overlay_node.hpp
new file mode 100644
index 0000000..4501b57
--- /dev/null
+++ b/include/image_overlay_node.hpp
@@ -0,0 +1,91 @@
+/*
+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 <sensor_msgs/msg/image.hpp>
+    #include <cv_bridge/cv_bridge.h>
+    #include <mutex>
+#else
+    #include <ros/ros.h>
+    #include <sensor_msgs/Image.h>
+    #include <cv_bridge/cv_bridge.h>
+    #include <message_filters/subscriber.h>
+    #include <message_filters/sync_policies/approximate_time.h>
+    #include <message_filters/synchronizer.h>
+#endif
+
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <memory>
+
+#ifdef ROS2
+class ImageOverlayNode : public rclcpp::Node
+{
+public:
+    ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+private:
+    using Image = sensor_msgs::msg::Image;
+
+    std::string reprojected_topic_;
+    std::string camera_topic_;
+    std::string overlay_topic_;
+    double alpha_;  // blend alpha for overlay
+
+    rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
+    rclcpp::Subscription<Image>::SharedPtr camera_sub_;
+    rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
+
+    // Cache latest images
+    cv::Mat latest_reproj_img_;
+    cv::Mat latest_camera_img_;
+    std_msgs::msg::Header latest_header_;
+    std::mutex mutex_;
+
+    void reprojCallback(const Image::ConstSharedPtr& msg);
+    void cameraCallback(const Image::ConstSharedPtr& msg);
+    void publishOverlay();
+};
+#else
+#include <mutex>
+class ImageOverlayNode
+{
+public:
+    ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
+
+private:
+    ros::NodeHandle nh_, pnh_;
+
+    std::string reprojected_topic_;
+    std::string camera_topic_;
+    std::string overlay_topic_;
+    double alpha_;  // blend alpha for overlay
+
+    ros::Subscriber reproj_sub_;
+    ros::Subscriber camera_sub_;
+    ros::Publisher overlay_pub_;
+
+    // Cache latest images
+    cv::Mat latest_reproj_img_;
+    cv::Mat latest_camera_img_;
+    std_msgs::Header latest_header_;
+    std::mutex mutex_;
+
+    void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
+    void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
+    void publishOverlay();
+};
+#endif
diff --git a/include/lidar_api.h b/include/lidar_api.h
index 21268a0..ff9d985 100644
--- a/include/lidar_api.h
+++ b/include/lidar_api.h
@@ -244,6 +244,18 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
  */
  int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
 
+/**
+ * @brief Set the image mask file for the device
+ *
+ * Read & send specified image mask file to device
+ *
+ * @param device Handle to the target device
+ * @param abs_path Absolute path to the image mask file (e.g., mask.png)
+ * @return int 0 on success, -1 on failure, -2 if file transfer in progress
+ */
+ int lidar_set_image_mask(device_handle device, const char* abs_path);
+
+
   /**
  * @brief enable device log
  *
@@ -266,6 +278,29 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
  */
 int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
 
+/**
+ * @brief Enable or disable IMU smooth sending feature
+ *
+ * When enabled, IMU data will be sent at precise intervals (default 400Hz) 
+ * using a dedicated high-priority thread to reduce jitter and timing variance.
+ * When disabled, IMU data will be sent immediately upon reception.
+ *
+ * @param enable 1 to enable smooth sending, 0 to disable
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_enable_imu_smooth_sending(int enable);
+
+/**
+ * @brief Set IMU smooth sending frequency
+ *
+ * Set the target frequency for IMU smooth sending. Only effective when 
+ * smooth sending is enabled via lidar_enable_imu_smooth_sending().
+ *
+ * @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/include/yaml_parser.h b/include/yaml_parser.h
index b5231eb..81a2b65 100644
--- a/include/yaml_parser.h
+++ b/include/yaml_parser.h
@@ -87,7 +87,7 @@ private:
     std::map<std::string, std::string> register_keys_str_val_;
     std::map<std::string, ParameterValue> custom_parameters_;
 
-    std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name"};
+    std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name", "image_mask_abs_path"};
 };
 
 }
diff --git a/launch_ROS1/odin1_ros1.launch b/launch_ROS1/odin1_ros1.launch
index e9790f2..8a0fd18 100644
--- a/launch_ROS1/odin1_ros1.launch
+++ b/launch_ROS1/odin1_ros1.launch
@@ -26,6 +26,11 @@
         <rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
         <param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
     </node>
+
+    <!-- Image overlay node - overlays reprojected points on camera image -->
+    <node pkg="odin_ros_driver" type="image_overlay_node" name="image_overlay_node" output="screen" >
+        <rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
+    </node>
     
     <!-- Launch RViz with configuration -->
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" output="screen"/>
diff --git a/launch_ROS2/odin1_ros2.launch.py b/launch_ROS2/odin1_ros2.launch.py
index 060859c..6f51d6b 100644
--- a/launch_ROS2/odin1_ros2.launch.py
+++ b/launch_ROS2/odin1_ros2.launch.py
@@ -65,6 +65,18 @@ def generate_launch_description():
         parameters=[reprojection_params]
     )
 
+    # Image overlay node - overlays reprojected points on camera image
+    overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
+    with open(overlay_config_path, 'r') as f:
+        overlay_params = yaml.safe_load(f)
+    image_overlay_node = Node(
+        package='odin_ros_driver',
+        executable='image_overlay_node',  
+        name='image_overlay_node',
+        output='screen',
+        parameters=[overlay_params]
+    )
+
     # Create RViz2 node - loads specified configuration file
     rviz_node = Node(
         package='rviz2',
@@ -81,6 +93,7 @@ def generate_launch_description():
     ld.add_action(host_sdk_node)
     ld.add_action(pcd2depth_node)
     ld.add_action(cloud_reprojection_node)
-    # ld.add_action(rviz_node)  # Add RViz node
+    ld.add_action(image_overlay_node)
+    ld.add_action(rviz_node)  # Add RViz node
     
     return ld
diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a
index 9a25888..ff5ce2c 100644
Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ
diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a
index 981b1d4..67f027c 100644
Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ
diff --git a/package.xml b/package.xml
index b277f0b..17f851d 100755
--- a/package.xml
+++ b/package.xml
@@ -2,27 +2,28 @@
 <package format="3">
   <name>odin_ros_driver</name>
   <version>0.0.1</version>
-  <description>ROS2 driver for Odin sensor</description>
+  <description>ROS driver for Odin sensor</description>
   <maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
   <license>Apache 2.0</license>
-  <!-- ROS2 uses colcon as the build tool -->
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <!-- ROS2 dependencies -->
-  <depend>rclcpp</depend>
-  <!-- System dependencies -->
+  
+  <!-- ROS1 uses catkin as the build tool -->
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <!-- ROS1 dependencies -->
+  <depend>roscpp</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
-  <depend>geometry_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
-  <depend>pcl_conversions</depend>
-  <depend>message_filters</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <!-- Specify build type as ament -->
-  <export>  
-    <build_type>ament_cmake</build_type>  
-  </export> 
-</package>
\ No newline at end of file
+  
+  <!-- System dependencies -->
+  <depend>eigen</depend>
+  <depend>opencv</depend>
+  <depend>yaml-cpp</depend>
+  
+  <!-- Specify build type as catkin -->
+  <export>
+    <build_type>catkin</build_type>
+  </export>
+</package>
diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp
index 9207c53..f928467 100644
--- a/src/cloud_reprojection_ros.cpp
+++ b/src/cloud_reprojection_ros.cpp
@@ -63,14 +63,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
     RCLCPP_INFO_STREAM(this->get_logger(), 
         "\n  cloud_slam_topic: " << cloud_slam_topic_
         << "\n  odometry_topic: " << odometry_topic_
+        << "\n  wiwc_topic: " << wiwc_topic_
         << "\n  reprojected_image_topic: " << reprojected_image_topic_);
 
     cloud_sub_.subscribe(this, cloud_slam_topic_);
     odom_sub_.subscribe(this, odometry_topic_);
+    wiwc_sub_.subscribe(this, wiwc_topic_);
 
-    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_);
+    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
     sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this, 
-                                       std::placeholders::_1, std::placeholders::_2));
+                                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
     reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
 
@@ -82,10 +84,12 @@ void CloudReprojectionRosNode::loadParameters()
     // Declare and get parameters
     this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
     this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
+    this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
     this->declare_parameter<std::string>("reprojected_image_topic", "/odin1/reprojected_image");
 
     cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
     odometry_topic_ = this->get_parameter("odometry_topic").as_string();
+    wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
     reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
 
     // Load camera parameters from calib.yaml file directly
@@ -167,8 +171,13 @@ void CloudReprojectionRosNode::loadParameters()
 
 void CloudReprojectionRosNode::syncCallback(
     const PointCloud2::ConstSharedPtr& cloud_msg,
-    const Odometry::ConstSharedPtr& odom_msg)
+    const Odometry::ConstSharedPtr& odom_msg,
+    const Odometry::ConstSharedPtr& wiwc_msg)
 {
+    // Debug: print that syncCallback is called
+    static int sync_count = 0;
+    // RCLCPP_INFO(this->get_logger(), "=== syncCallback called, count: %d ===", ++sync_count);
+    
     pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
     pcl::fromROSMsg(*cloud_msg, cloud_odom);
 
@@ -178,6 +187,45 @@ void CloudReprojectionRosNode::syncCallback(
         return;
     }
 
+    // Extract real-time extrinsics from WIWC message covariance fields
+    // pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
+    Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
+    Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
+    for (int i = 0; i < 16; ++i) {
+        T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
+        T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
+    }
+    
+    // Update extrinsics if valid (not identity matrix)
+    bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
+    bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
+    
+    // // Debug print to compare with host_sdk_sample values
+    // static int print_count = 0;
+    // if (print_count++) {
+    //     // Extract rotation (3x3) and translation (3x1) from T_CL
+    //     Eigen::Matrix3d RCL = T_CL.block<3,3>(0,0);
+    //     Eigen::Vector3d TCL = T_CL.block<3,1>(0,3);
+    //     RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RCL (3x3 rotation from T_CL) ===\n" << RCL);
+    //     RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TCL (3x1 translation from T_CL) ===\n" << TCL.transpose());
+        
+    //     // Extract rotation (3x3) and translation (3x1) from T_IL
+    //     Eigen::Matrix3d RIL = T_IL.block<3,3>(0,0);
+    //     Eigen::Vector3d TIL = T_IL.block<3,1>(0,3);
+    //     RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RIL (3x3 rotation from T_IL) ===\n" << RIL);
+    //     RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TIL (3x1 translation from T_IL) ===\n" << TIL.transpose());
+        
+    //     RCLCPP_INFO(this->get_logger(), "T_CL_valid: %d, T_IL_valid: %d", T_CL_valid, T_IL_valid);
+    //     if (T_CL_valid && T_IL_valid) {
+    //         Eigen::Matrix4d Tic = CloudReprojector::calculateTic(T_CL, T_IL);
+    //         RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection calculated Tic ===\n" << Tic);
+    //     }
+    // }
+    
+    if (T_CL_valid && T_IL_valid) {
+        reprojector_->updateExtrinsics(T_CL, T_IL);
+    }
+
     CloudReprojector::OdomPose odom_pose;
     odom_pose.orientation = Eigen::Quaterniond(
         odom_msg->pose.pose.orientation.w,
@@ -276,13 +324,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
 
     ROS_INFO_STREAM("\n  cloud_slam_topic: " << cloud_slam_topic_
                 << "\n  odometry_topic: " << odometry_topic_
+                << "\n  wiwc_topic: " << wiwc_topic_
                 << "\n  reprojected_image_topic: " << reprojected_image_topic_);
 
     cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
     odom_sub_.subscribe(nh_, odometry_topic_, 1);
+    wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
 
-    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_);
-    sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2));
+    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
+    sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3));
 
     reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
 
@@ -293,6 +343,7 @@ void CloudReprojectionRosNode::loadParameters()
 {
     pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
     pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
+    pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
     pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
 
     // Load camera parameters
@@ -349,7 +400,8 @@ void CloudReprojectionRosNode::loadParameters()
 
 void CloudReprojectionRosNode::syncCallback(
     const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
-    const nav_msgs::OdometryConstPtr& odom_msg)
+    const nav_msgs::OdometryConstPtr& odom_msg,
+    const nav_msgs::OdometryConstPtr& wiwc_msg)
 {
     pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
     pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -360,6 +412,22 @@ void CloudReprojectionRosNode::syncCallback(
         return;
     }
 
+    // Extract real-time extrinsics from WIWC message covariance fields
+    // pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
+    Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
+    Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
+    for (int i = 0; i < 16; ++i) {
+        T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
+        T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
+    }
+    
+    // Update extrinsics if valid (not identity matrix)
+    bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
+    bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
+    if (T_CL_valid && T_IL_valid) {
+        reprojector_->updateExtrinsics(T_CL, T_IL);
+    }
+
     CloudReprojector::OdomPose odom_pose;
     odom_pose.orientation = Eigen::Quaterniond(
         odom_msg->pose.pose.orientation.w,
diff --git a/src/cloud_reprojector.cpp b/src/cloud_reprojector.cpp
index 7ede028..f45d0d3 100644
--- a/src/cloud_reprojector.cpp
+++ b/src/cloud_reprojector.cpp
@@ -3,7 +3,7 @@ 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
+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.
@@ -90,16 +90,31 @@ cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXY
     cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);
     img.setTo(cv::Scalar(255, 255, 255));
 
+    const double fx = camera_model_->fx();
+    const double fy = camera_model_->fy();
+    const double cx = camera_model_->cx();
+    const double cy = camera_model_->cy();
+
     for (const auto& pt : cloud_in_cam)
     {
         if (pt.z <= 0.01)
             continue;
 
-        Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z);
-        Eigen::Vector2d uv = camera_model_->world2cam(pt_cam);
-
-        int u_int = static_cast<int>(std::round(uv[0]));
-        int v_int = static_cast<int>(std::round(uv[1]));
+        int u_int, v_int;
+        if (0)
+        {
+            // Pinhole projection (undistorted image)
+            u_int = static_cast<int>(std::round(fx * pt.x / pt.z + cx));
+            v_int = static_cast<int>(std::round(fy * pt.y / pt.z + cy));
+        }
+        else
+        {
+            // Distorted projection (original image)
+            Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z);
+            Eigen::Vector2d uv = camera_model_->world2cam(pt_cam);
+            u_int = static_cast<int>(std::round(uv[0]));
+            v_int = static_cast<int>(std::round(uv[1]));
+        }
 
         if (u_int >= 0 && u_int < camera_params_.image_width &&
             v_int >= 0 && v_int < camera_params_.image_height)
@@ -110,4 +125,4 @@ cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXY
     }
 
     return img;
-}
+}
\ No newline at end of file
diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp
index 3640d9e..bfc4001 100644
--- a/src/host_sdk_sample.cpp
+++ b/src/host_sdk_sample.cpp
@@ -23,8 +23,11 @@ limitations under the License.
 #include <memory>
 #include <opencv2/opencv.hpp>
 #include <deque> 
+#include <queue>
 #include <unistd.h> 
 #include <cstdlib>
+#include <sched.h>
+#include <pthread.h>
 #include <cstring>
 #include <sys/types.h>
 #include <sys/wait.h>
@@ -46,7 +49,7 @@ limitations under the License.
     #include <ros/package.h>
     #include <ros/ros.h> 
 #endif
-#define ros_driver_version "0.9.0"
+#define ros_driver_version "0.10.0"
 #define required_firmware_version_major 0
 #define required_firmware_version_minor 10
 #define required_firmware_version_patch 0
@@ -85,13 +88,21 @@ static std::shared_ptr<rawCloudRender> g_renderer = nullptr;
 std::string calib_file_ = "";
 static std::shared_ptr<odin_ros_driver::YamlParser> g_parser = nullptr;
 
-static constexpr size_t PTP_SMOOTH_WINDOW_SIZE = 30;
+static constexpr size_t PTP_SMOOTH_WINDOW_SIZE = 300;
 static std::mutex g_ptp_mutex;
 static std::deque<double> g_ptp_delay_buf;
 static std::deque<double> g_ptp_offset_buf;
 static std::atomic<double> g_ptp_delay_smooth{0.0};
 static std::atomic<double> g_ptp_offset_smooth{0.0};
 
+// IMU dedicated processing thread
+static std::atomic<bool> g_imu_thread_running(false);
+static std::thread g_imu_thread;
+static std::queue<imu_convert_data_t> g_imu_queue;
+static std::mutex g_imu_queue_mutex;
+static std::condition_variable g_imu_queue_cv;
+static const size_t IMU_QUEUE_MAX_SIZE = 200;
+
 double get_ptp_smoothed_delay() {
     return g_ptp_delay_smooth.load(std::memory_order_relaxed);
 }
@@ -109,6 +120,10 @@ int g_sendimu = 1;
 int g_senddtof = 1;
 int g_sendodom = 1;
 int g_send_odom_baselink_tf = 0;
+
+// SDK IMU smooth sending configuration
+int g_enable_imu_smooth = 0;
+int g_imu_smooth_frequency = 400;
 int g_sendcloudslam = 0;
 int g_sendcloudrender = 0;
 int g_sendrgb_compressed = 0;
@@ -132,6 +147,11 @@ std::string g_relocalization_map_abs_path = "";
 std::string g_mapping_result_dest_dir = "";
 std::string g_mapping_result_file_name = "";
 
+int g_send_image_mask = 0;
+std::string g_image_mask_abs_path = "";
+
+int g_reset_algo = 0;
+
 const char* DEV_STATUS_CSV_FILE = "dev_status.csv";
 FILE* dev_status_csv_file = nullptr;
 
@@ -764,6 +784,105 @@ void clear_all_queues() {
     g_latest_bgr.reset();
     g_latest_rgb_timestamp = 0;
     g_has_rgb = false;
+    
+    // Clear IMU queue
+    {
+        std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
+        while (!g_imu_queue.empty()) {
+            g_imu_queue.pop();
+        }
+    }
+}
+
+// IMU dedicated processing thread routine
+static void imu_thread_routine()
+{
+    // Try to set higher thread priority for IMU processing
+    pthread_t this_thread = pthread_self();
+    struct sched_param param;
+    param.sched_priority = 70;
+    
+    int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &param);
+    if (ret != 0) {
+        ret = pthread_setschedparam(this_thread, SCHED_RR, &param);
+    }
+    
+    #ifdef ROS2
+        RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread started (priority: %d)", param.sched_priority);
+    #else
+        ROS_INFO("IMU dedicated thread started (priority: %d)", param.sched_priority);
+    #endif
+    
+    while (g_imu_thread_running) {
+        std::unique_lock<std::mutex> lock(g_imu_queue_mutex);
+        
+        // Wait for IMU data
+        g_imu_queue_cv.wait(lock, []() {
+            return !g_imu_queue.empty() || !g_imu_thread_running;
+        });
+        
+        if (!g_imu_thread_running) {
+            break;
+        }
+        
+        // Process all pending IMU data
+        while (!g_imu_queue.empty() && g_imu_thread_running) {
+            imu_convert_data_t imu_data = g_imu_queue.front();
+            g_imu_queue.pop();
+            lock.unlock();
+            
+            // Publish IMU data
+            if (g_ros_object && g_sendimu) {
+                g_ros_object->publishImu(&imu_data);
+            }
+            
+            lock.lock();
+        }
+    }
+    
+    #ifdef ROS2
+        RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread exiting");
+    #else
+        ROS_INFO("IMU dedicated thread exiting");
+    #endif
+}
+
+// Start IMU dedicated thread
+static void start_imu_thread()
+{
+    if (!g_imu_thread_running) {
+        g_imu_thread_running = true;
+        g_imu_thread = std::thread(imu_thread_routine);
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread created");
+        #else
+            ROS_INFO("IMU dedicated thread created");
+        #endif
+    }
+}
+
+// Stop IMU dedicated thread
+static void stop_imu_thread()
+{
+    if (g_imu_thread_running) {
+        g_imu_thread_running = false;
+        g_imu_queue_cv.notify_all();
+        if (g_imu_thread.joinable()) {
+            g_imu_thread.join();
+        }
+        
+        // Clear queue
+        std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
+        while (!g_imu_queue.empty()) {
+            g_imu_queue.pop();
+        }
+        
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread stopped");
+        #else
+            ROS_INFO("IMU dedicated thread stopped");
+        #endif
+    }
 }
 
 // Lidar data callback
@@ -800,7 +919,15 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
         case LIDAR_DT_RAW_IMU:
             if (g_sendimu) {
                 imudata = (imu_convert_data_t *)data->stream.imageList[0].pAddr;
-                g_ros_object->publishImu(imudata);
+                // Enqueue IMU data for dedicated thread processing
+                {
+                    std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
+                    if (g_imu_queue.size() >= IMU_QUEUE_MAX_SIZE) {
+                        g_imu_queue.pop();  // Drop oldest if full
+                    }
+                    g_imu_queue.push(*imudata);
+                }
+                g_imu_queue_cv.notify_one();
             }
             update_count(&imu_rx_fps);
             break;
@@ -999,6 +1126,9 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
             break;
             case LIDAR_DT_SLAM_WIWC:
             {
+                // Always publish WIWC data for real-time extrinsics
+                g_ros_object->publishWiwc((capture_Image_List_t *)&data->stream);
+                
                 if(g_record_data ) {
                     g_ros_object->recordrotate((capture_Image_List_t *)&data->stream);
                 }
@@ -1451,6 +1581,51 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
                 return;
             }
         }
+
+        // Transfer image mask if enabled
+        if (g_send_image_mask == 1) {
+            if (g_image_mask_abs_path != "" && std::filesystem::exists(g_image_mask_abs_path)) {
+                int ret = lidar_set_image_mask(odinDevice, g_image_mask_abs_path.c_str());
+                if (ret == 0) {
+                    #ifdef ROS2
+                        RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Image mask set successfully: %s", g_image_mask_abs_path.c_str());
+                    #else
+                        ROS_INFO("Image mask set successfully: %s", g_image_mask_abs_path.c_str());
+                    #endif
+                } else {
+                    #ifdef ROS2
+                        RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to set image mask: %s, error: %d", g_image_mask_abs_path.c_str(), ret);
+                    #else
+                        ROS_ERROR("Failed to set image mask: %s, error: %d", g_image_mask_abs_path.c_str(), ret);
+                    #endif
+                }
+            } else {
+                #ifdef ROS2
+                    RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Image mask path not set or file not found: %s", g_image_mask_abs_path.c_str());
+                #else
+                    ROS_WARN("Image mask path not set or file not found: %s", g_image_mask_abs_path.c_str());
+                #endif
+            }
+        }
+
+        // Send algo_reset command if enabled
+        if (g_reset_algo == 1) {
+            int reset_value = 1;
+            int ret = lidar_set_custom_parameter(odinDevice, "algo_reset", &reset_value, sizeof(int));
+            if (ret == 0) {
+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Algo reset command sent successfully");
+                #else
+                    ROS_INFO("Algo reset command sent successfully");
+                #endif
+            } else {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to send algo reset command, error: %d", ret);
+                #else
+                    ROS_ERROR("Failed to send algo reset command, error: %d", ret);
+                #endif
+            }
+        }
  
         lidar_data_callback_info_t data_callback_info;
         data_callback_info.data_callback = lidar_data_callback;
@@ -1532,6 +1707,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
         deviceConnected = true;
         deviceDisconnected = false;
         
+        // Start IMU dedicated thread
+        start_imu_thread();
+        
         // Start custom parameter monitoring thread
         g_param_monitor_running = true;
         g_param_monitor_thread = std::thread(custom_parameter_monitor);
@@ -1567,6 +1745,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
         deviceConnected = false;
         deviceDisconnected = true;
         
+        // Stop IMU dedicated thread
+        stop_imu_thread();
+        
         // Stop custom parameter monitoring thread
         g_param_monitor_running = false;
         if (g_param_monitor_thread.joinable()) {
@@ -1647,6 +1828,10 @@ int main(int argc, char *argv[])
         g_sendrgb       = get_key_value("sendrgb", 1);
         g_sendimu       = get_key_value("sendimu", 1);
         g_senddtof      = get_key_value("senddtof", 1);
+        
+        // SDK IMU smooth sending configuration
+        g_enable_imu_smooth = get_key_value("enable_imu_smooth", 0);
+        g_imu_smooth_frequency = get_key_value("imu_smooth_frequency", 400);
         g_cloud_raw_confidence_threshold = get_key_value("cloud_raw_confidence_threshold", 35);
         g_rosNodeControlImpl.setCloudRawConfidenceThreshold(g_cloud_raw_confidence_threshold);
         g_dtof_fps      = get_key_value("dtof_fps", 145);  // Read DTOF frame rate from config (100=10fps, 145=14.5fps)
@@ -1679,7 +1864,10 @@ int main(int argc, char *argv[])
         g_relocalization_map_abs_path = get_key_str_value("relocalization_map_abs_path", "");
         g_mapping_result_dest_dir = get_key_str_value("mapping_result_dest_dir", "");
         g_mapping_result_file_name = get_key_str_value("mapping_result_file_name", "");
+        g_image_mask_abs_path = get_key_str_value("image_mask_abs_path", "");
 
+        g_send_image_mask = get_key_value("sendimagemask", 0);
+        g_reset_algo = get_key_value("resetalgo", 0);
         g_custom_map_mode = g_parser->getCustomMapMode(2);
 
         lidar_log_set_level(LIDAR_LOG_INFO);
@@ -1747,6 +1935,24 @@ int main(int argc, char *argv[])
             return -1;
         }
         
+        // Configure SDK IMU smooth sending AFTER lidar_system_init
+        // SDK now defaults to disabled, only enable if configured
+        if (g_enable_imu_smooth) {
+            lidar_enable_imu_smooth_sending(1);
+            lidar_set_imu_smooth_frequency(g_imu_smooth_frequency);
+            #ifdef ROS2
+                RCLCPP_INFO(node->get_logger(), "Enabling SDK IMU smooth sending at %d Hz", g_imu_smooth_frequency);
+            #else
+                ROS_INFO("Enabling SDK IMU smooth sending at %d Hz", g_imu_smooth_frequency);
+            #endif
+        } else {
+            #ifdef ROS2
+                RCLCPP_INFO(node->get_logger(), "SDK IMU smooth sending disabled");
+            #else
+                ROS_INFO("SDK IMU smooth sending disabled");
+            #endif
+        }
+        
 
         bool usbPresent = false;
         bool usbVersionChecked = false; 
diff --git a/src/image_overlay_node.cpp b/src/image_overlay_node.cpp
new file mode 100644
index 0000000..2e1f4c0
--- /dev/null
+++ b/src/image_overlay_node.cpp
@@ -0,0 +1,254 @@
+/*
+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 "image_overlay_node.hpp"
+
+#ifdef ROS2
+// ==================== ROS2 Implementation ====================
+
+ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options)
+    : Node("image_overlay_node", options)
+{
+    // Read from register_keys (same structure as control_command.yaml)
+    this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
+    this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
+    this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/overlay_image");
+    this->declare_parameter<double>("register_keys.overlay_alpha", 0.6);
+
+    reprojected_topic_ = this->get_parameter("register_keys.overlay_reprojected_topic").as_string();
+    camera_topic_ = this->get_parameter("register_keys.overlay_camera_topic").as_string();
+    overlay_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string();
+    alpha_ = this->get_parameter("register_keys.overlay_alpha").as_double();
+
+    RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s", 
+                reprojected_topic_.c_str(), camera_topic_.c_str());
+    RCLCPP_INFO(this->get_logger(), "Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
+
+    // Independent subscriptions - no synchronization needed
+    reproj_sub_ = this->create_subscription<Image>(
+        reprojected_topic_, 10,
+        std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1));
+    
+    camera_sub_ = this->create_subscription<Image>(
+        camera_topic_, 10,
+        std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1));
+
+    overlay_pub_ = this->create_publisher<sensor_msgs::msg::Image>(overlay_topic_, 10);
+
+    RCLCPP_INFO(this->get_logger(), "ImageOverlayNode initialized (no-sync mode)");
+}
+
+void ImageOverlayNode::reprojCallback(const Image::ConstSharedPtr& msg)
+{
+    try {
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            latest_reproj_img_ = cv_ptr->image.clone();
+            latest_header_ = msg->header;
+        }
+    } catch (cv_bridge::Exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (reproj): %s", e.what());
+        return;
+    }
+    publishOverlay();
+}
+
+void ImageOverlayNode::cameraCallback(const Image::ConstSharedPtr& msg)
+{
+    try {
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            latest_camera_img_ = cv_ptr->image.clone();
+        }
+    } catch (cv_bridge::Exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (camera): %s", e.what());
+        return;
+    }
+    publishOverlay();
+}
+
+void ImageOverlayNode::publishOverlay()
+{
+    cv::Mat reproj_copy, camera_copy;
+    std_msgs::msg::Header header_copy;
+    
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
+            return;
+        }
+        reproj_copy = latest_reproj_img_.clone();
+        camera_copy = latest_camera_img_.clone();
+        header_copy = latest_header_;
+    }
+
+    if (reproj_copy.size() != camera_copy.size()) {
+        RCLCPP_WARN(this->get_logger(),
+            "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
+            reproj_copy.cols, reproj_copy.rows, 
+            camera_copy.cols, camera_copy.rows);
+        return;
+    }
+
+    // Create overlay using alpha blending
+    // Replace white background in reproj with camera image, keep colored points
+    cv::Mat overlay = camera_copy.clone();
+    
+    // Blend: where reproj has color (non-white), show reproj color semi-transparently
+    // where reproj is white (background), show camera image
+    
+    for (int y = 0; y < reproj_copy.rows; ++y) {
+        for (int x = 0; x < reproj_copy.cols; ++x) {
+            cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
+            // Check if pixel is not white (has point cloud color)
+            if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
+                // Blend reproj color with camera color
+                cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
+                overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
+                    static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
+                    static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
+                    static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
+                );
+            }
+            // else: keep camera image (already in overlay)
+        }
+    }
+
+    auto overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
+    overlay_pub_->publish(*overlay_msg);
+}
+
+// ==================== ROS2 Main ====================
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    
+    auto node = std::make_shared<ImageOverlayNode>();
+    
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
+
+#else
+// ==================== ROS1 Implementation ====================
+
+ImageOverlayNode::ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
+    : nh_(nh), pnh_(pnh)
+{
+    // Read from register_keys (same structure as control_command.yaml)
+    pnh_.param<std::string>("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image");
+    pnh_.param<std::string>("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted");
+    pnh_.param<std::string>("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image");
+    pnh_.param<double>("register_keys/overlay_alpha", alpha_, 0.6);
+
+    ROS_INFO("Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str());
+    ROS_INFO("Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
+
+    // Independent subscriptions - no synchronization needed
+    reproj_sub_ = nh_.subscribe(reprojected_topic_, 10, &ImageOverlayNode::reprojCallback, this);
+    camera_sub_ = nh_.subscribe(camera_topic_, 10, &ImageOverlayNode::cameraCallback, this);
+
+    overlay_pub_ = nh_.advertise<sensor_msgs::Image>(overlay_topic_, 10);
+
+    ROS_INFO("ImageOverlayNode initialized (no-sync mode)");
+}
+
+void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+    try {
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            latest_reproj_img_ = cv_ptr->image.clone();
+            latest_header_ = msg->header;
+        }
+    } catch (cv_bridge::Exception& e) {
+        ROS_ERROR("cv_bridge exception (reproj): %s", e.what());
+        return;
+    }
+    publishOverlay();
+}
+
+void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+    try {
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            latest_camera_img_ = cv_ptr->image.clone();
+        }
+    } catch (cv_bridge::Exception& e) {
+        ROS_ERROR("cv_bridge exception (camera): %s", e.what());
+        return;
+    }
+    publishOverlay();
+}
+
+void ImageOverlayNode::publishOverlay()
+{
+    cv::Mat reproj_copy, camera_copy;
+    std_msgs::Header header_copy;
+    
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
+            return;
+        }
+        reproj_copy = latest_reproj_img_.clone();
+        camera_copy = latest_camera_img_.clone();
+        header_copy = latest_header_;
+    }
+
+    if (reproj_copy.size() != camera_copy.size()) {
+        ROS_WARN_THROTTLE(2, "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
+                 reproj_copy.cols, reproj_copy.rows, camera_copy.cols, camera_copy.rows);
+        return;
+    }
+
+    // Create overlay using alpha blending
+    cv::Mat overlay = camera_copy.clone();
+    
+    for (int y = 0; y < reproj_copy.rows; ++y) {
+        for (int x = 0; x < reproj_copy.cols; ++x) {
+            cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
+            if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
+                cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
+                overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
+                    static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
+                    static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
+                    static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
+                );
+            }
+        }
+    }
+
+    sensor_msgs::ImagePtr overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
+    overlay_pub_.publish(overlay_msg);
+}
+
+// ==================== ROS1 Main ====================
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "image_overlay_node");
+    ros::NodeHandle nh;
+    ros::NodeHandle pnh("~");
+
+    ImageOverlayNode node(nh, pnh);
+
+    ros::spin();
+    return 0;
+}
+#endif