diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e59982..eb32030 100644 --- a/CMakeLists.txt +++ b/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 "=======================================") + diff --git a/README.md b/README.md index 03b1b7c..1c7c1de 100644 --- a/README.md +++ b/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 -./scripty/build_ros.sh +source /opt/ros/noetic/setup.bash +./script/build_ros.sh ``` #### 3.3.2 ROS2 (Foxy for example): ```shell -source /opt/ros/foxy/setup.sh -./scripty/build_ros2.sh +source /opt/ros/foxy/setup.bash +./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 -roslaunch odin_ros_driver [launch file] +source [ros_workspace]/install/setup.bash +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 -power cycle Odin1 # Disconnect and reconnect LiDAR power -reinitialize host SDK # Execute SDK after device reboot -``` +1.Please power on Odin module again # Disconnect and reconnect odin power + +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 -ROS1 rm -rf devel/ build/ -ROS2 rm -rf devel/ install/ log/ -Re-run script installation (refer to section 2.3) -``` +rm -rf devel/ build/ +``` +ROS2 +```shell +rm -rf devel/ install/ log/ +``` +2.Re-run script installation ### 5.3 Docker GUI passthrough failure @@ -236,4 +239,31 @@ Unable to open X display or No protocol specified **Resolution** ```shell xhost + #This command enables graphical passthrough to Docker containers -``` \ No newline at end of file +``` + +### 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 \ No newline at end of file diff --git a/config/control_command.yaml b/config/control_command.yaml index c3477ce..32102c9 100644 --- a/config/control_command.yaml +++ b/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 + diff --git a/config/odin_ros.rviz b/config/odin_ros.rviz index 26017f0..4435d61 100644 --- a/config/odin_ros.rviz +++ b/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: - Yaw: 3.0054032802581787 + Yaw: 2.2254059314727783 Saved: ~ Window Geometry: Displays: diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index 466414f..bfb001a 100644 --- a/config/odin_ros2.rviz +++ b/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: 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 - Y: 0 - Z: 0 + X: 0.40898558497428894 + Y: -0.07609735429286957 + 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: Value: Orbit (rviz) - Yaw: 2.205399513244629 + Yaw: 2.625401258468628 Saved: ~ Window Geometry: Displays: @@ -245,4 +274,4 @@ Window Geometry: collapsed: false Width: 1920 X: 0 - Y: 27 \ No newline at end of file + Y: 27 diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index 34a5d21..b3048b8 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -30,8 +30,25 @@ limitations under the License. #include #include "lidar_api.h" #include "lidar_api_type.h" +#include "rawCloudRender.h" +#include +#include +#include +#include +#include +#include +#include +#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 #include #include + #include + 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 @@ -60,11 +86,43 @@ limitations under the License. #include #include #include + #include + 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 @@ -116,7 +174,12 @@ public: initialize_publishers(nh); } #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,188 +209,467 @@ public: imu_pub_.publish(imu_msg); #endif } - - void publishIntensityCloud(capture_Image_List_t* stream, int idx) +#ifdef ROS2 + 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 - sensor_msgs::msg::PointCloud2 msg; - msg.header.frame_id = "map"; - msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - - msg.height = stream->imageList[idx].height; - msg.width = stream->imageList[idx].width; - msg.is_dense = false; - - sensor_msgs::PointCloud2Modifier modifier(msg); - modifier.setPointCloud2Fields( - 4, - "x", 1, sensor_msgs::msg::PointField::FLOAT32, - "y", 1, sensor_msgs::msg::PointField::FLOAT32, - "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::UINT8 - ); - modifier.resize(stream->imageList[idx].width * stream->imageList[idx].height); - - sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); - sensor_msgs::PointCloud2Iterator iter_intensity(msg, "intensity"); - #else - sensor_msgs::PointCloud2 msg; - msg.header.frame_id = "map"; - 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); - modifier.setPointCloud2Fields( - 4, - "x", 1, sensor_msgs::PointField::FLOAT32, - "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, - "intensity", 1, sensor_msgs::PointField::UINT8 - ); - modifier.resize(msg.height * msg.width); - - sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); - sensor_msgs::PointCloud2Iterator iter_intensity(msg, "intensity"); - #endif - - float* xyz_data = static_cast(stream->imageList[idx].pAddr); - uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); - - int total_points = stream->imageList[idx].height * stream->imageList[idx].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(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(intensity_data[i] >> 8); ++iter_intensity; - #endif - } - - // Publish Intensity point cloud - #ifdef ROS2 - cloud_pub_->publish(std::move(msg)); - #else - cloud_pub_.publish(msg); - #endif + std::lock_guard lock1(rgb_queue_mutex_); + std::lock_guard lock2(pcd_queue_mutex_); + rgb_size = rgb_image_queue_.size(); + pcd_size = pcd_queue_.size(); } - - void publishRgb(capture_Image_List_t *stream) { - buffer_List_t &image = stream->imageList[0]; + + while (true) { + ImageConstPtr rgb_msg = nullptr; + PointCloud2ConstPtr pcd_msg = nullptr; - // 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(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 { - cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr); - cv::Mat bgr; - cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12); + // Get a pair of data from queues (with lock protection) + { + std::lock_guard lock1(rgb_queue_mutex_); + std::lock_guard lock2(pcd_queue_mutex_); - if (bgr.empty()) { - #ifdef ROS2 - RCLCPP_ERROR(node_->get_logger(), "Failed to convert NV12 to BGR"); - #else - ROS_ERROR("Failed to convert NV12 to BGR"); - #endif - return; + 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(rgb_stamp) - static_cast(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 lock1(rgb_queue_mutex_); + std::lock_guard 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(); + } } - #ifdef ROS2 - std_msgs::msg::Header header; - sensor_msgs::msg::Image::SharedPtr msg; - #else - std_msgs::Header header; - sensor_msgs::Image msg; - #endif + // Try next pair + continue; + } + + // Time difference within allowed range, process data pair + { + std::lock_guard lock1(rgb_queue_mutex_); + std::lock_guard lock2(pcd_queue_mutex_); - header.stamp = ns_to_ros_time(image.timestamp + 719060); + // 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>& 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 iter_x(*pcd_msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*pcd_msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*pcd_msg, "z"); + std::vector 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> rgb_image(input_image_height, std::vector(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 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::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "rgb", 1, sensor_msgs::PointField::FLOAT32 + ); + modifier.resize(valid_point_num); + + sensor_msgs::PointCloud2Iterator iter_res_x(output_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_res_y(output_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_res_z(output_msg, "z"); + sensor_msgs::PointCloud2Iterator 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 + rgbcloud_pub_.publish(output_msg); + #endif + } +} + + 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(); + #else + auto msg = boost::make_shared(); + #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, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "intensity", 1, sensor_msgs::PointField::UINT8 + ); + modifier.resize(msg->height * msg->width); + + // Fill point cloud data + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(*msg, "intensity"); + + float* xyz_data = static_cast(cloud.pAddr); + uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); + + int total_points = cloud.height * cloud.width; + for (int i = 0; i < total_points; ++i) { + float* pf = xyz_data + i * 4; + + *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(intensity_data[i] >> 8); ++iter_intensity; + } + +{ + std::lock_guard 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 + auto msg_copy = std::make_shared(*msg); + #else + auto msg_copy = boost::make_shared(); + *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]; + + 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()) { + #ifndef ROS2 + ROS_ERROR("Failed to convert NV12 to BGR"); + #endif + return; + } + + //Create ROS image message + #ifdef ROS2 + auto header = std::make_shared(); + header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation + header->frame_id = "camera_rgb_frame"; + + auto cv_image = std::make_shared(*header, "bgr8", bgr); + auto msg = cv_image->toImageMsg(); + + // Add to unified queue + if (g_sendcloudrender) { + std::lock_guard 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(); + compressed_msg->header = *header; + compressed_msg->format = "jpeg"; + + // Set compression parameters + std::vector 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 = std::make_shared(header, "bgr8", bgr); - msg = cv_image->toImageMsg(); - rgb_pub_->publish(*msg); - #else - cv_bridge::CvImage(header, "bgr8", bgr).toImageMsg(msg); - rgb_pub_.publish(msg); - #endif + auto cv_image = boost::make_shared(header, "bgr8", bgr); + auto msg = cv_image->toImageMsg(); - } catch (const cv::Exception& e) { - #ifdef 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 - RCLCPP_ERROR(node_->get_logger(), "Exception in publishRgb: %s", e.what()); - #else - ROS_ERROR("Exception in publishRgb: %s", e.what()); - #endif - } + // Add to unified queue + if (g_sendcloudrender) { + std::lock_guard 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 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) { + #ifndef ROS2 + ROS_ERROR("OpenCV error in publishRgb: %s", e.what()); + #endif + } catch (const std::exception& e) { + #ifndef ROS2 + 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 rgb_image_queue_; + const size_t max_rgb_queue_size_ = 10; // Cache up to 10 image frames + + std::mutex pcd_queue_mutex_; + std::deque pcd_queue_; + const size_t max_pcd_queue_size_ = 10; // Maximum cache frames + + + // Updated helper functions + ImageConstPtr getLatestRgbImage() { + std::lock_guard lock(rgb_queue_mutex_); + return (!rgb_image_queue_.empty()) ? rgb_image_queue_.back() : nullptr; + } + + PointCloud2ConstPtr getLatestIntensityCloud() { + std::lock_guard lock(pcd_queue_mutex_); + return (!pcd_queue_.empty()) ? pcd_queue_.back() : nullptr; + } + std::vector getRgbImageQueueSnapshot() { + std::lock_guard lock(rgb_queue_mutex_); + std::vector 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 getIntensityCloudQueueSnapshot() { + std::lock_guard lock(pcd_queue_mutex_); + std::vector clouds; + + for (const auto& msg_ptr : pcd_queue_) { + clouds.push_back(*msg_ptr); + } + + return clouds; + } +#else + std::vector getIntensityCloudQueueSnapshot() { + std::lock_guard lock(pcd_queue_mutex_); + std::vector 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("odin1/imu", 10); @@ -447,9 +848,10 @@ private: cloud_pub_ = node_->create_publisher("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", 10); odom_publisher_ = node_->create_publisher("odin1/odometry_map", 10); + rgbcloud_pub_ = node_->create_publisher("odin1/cloud_render", 10); + compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", 10); #endif } - #ifdef ROS1 void initialize_publishers(ros::NodeHandle& nh) { imu_pub_ = nh.advertise("odin1/imu", 10); @@ -457,6 +859,8 @@ private: cloud_pub_ = nh.advertise("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); odom_publisher_ = nh.advertise("odin1/odometry_map", 10); + rgbcloud_pub_ = nh.advertise("odin1/cloud_render", 10); + compressed_rgb_pub_ = nh.advertise("odin1/image/compressed", 10); } #endif @@ -467,12 +871,20 @@ private: rclcpp::Publisher::SharedPtr cloud_pub_; rclcpp::Publisher::SharedPtr xyzrgbacloud_pub_; rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr rendered_cloud_pub_; + rclcpp::Publisher::SharedPtr rgbcloud_pub_; + rclcpp::Publisher::SharedPtr rgbFromnv12_pub_; + rclcpp::Publisher::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 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 lock(mtx); @@ -544,4 +945,4 @@ private: #define SENDODOM "sendodom" /* send odometry data */ #define SENDDTOF "senddtof" /* send raw cloud data */ #define SENDCLOUDSLAM "sendcloudslam" /* send rgb cloud data */ -#define EXIT "q" /* exit sample */ +#define EXIT "q" /* exit sample */ \ No newline at end of file diff --git a/include/lidar_api.h b/include/lidar_api.h index c19647c..f4fee44 100644 --- a/include/lidar_api.h +++ b/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); /** @@ -213,4 +224,4 @@ void lidar_log_set_level(lidar_log_level_e level); } #endif -#endif // LIDAR_API_H \ No newline at end of file +#endif // LIDAR_API_H diff --git a/include/rawCloudRender.h b/include/rawCloudRender.h new file mode 100644 index 0000000..1781229 --- /dev/null +++ b/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 +#include +#include +#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>& rgb_image); + void render(std::vector>& rgb_image, capture_Image_List_t* pcdStream, int pcdIdx, std::vector& 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 diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index f0fa0ff..8425063 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 8affc90..90710d8 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index 7af06b6..4a52227 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -1,22 +1,67 @@ #include "host_sdk_sample.h" #include "yaml_parser.h" +#include "rawCloudRender.h" #include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifdef ROS2 #include + #include #else #include + #include #endif +// Global variable declarations static device_handle odinDevice = nullptr; -static std::shared_ptr g_ros_object; static std::atomic deviceConnected(false); +static std::atomic 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 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 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 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 -static int g_sendrgb = 1; -static int g_sendimu = 1; -static int g_senddtof = 1; -static int g_sendodom = 1; -static int g_sendcloudslam = 0; +// Get package path +std::string get_package_path(const std::string& package_name) { + #ifdef ROS2 + return ament_index_cpp::get_package_share_directory(package_name); + #else + 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(user_data); if(!dev_handle || !data) { printf("Invalid device handle or data.\n"); @@ -63,10 +124,10 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data) } break; case LIDAR_DT_RAW_DTOF: - if (g_senddtof) { - g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1); + if (g_senddtof ) { + g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1); } - break; + break; case LIDAR_DT_SLAM_CLOUD: if (g_sendcloudslam) { g_ros_object->publishPC2XYZRGBA((capture_Image_List_t *)&data->stream, 0); @@ -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); - lidar_unregister_stream_callback(odinDevice); - lidar_close_device(odinDevice); - lidar_destory_device(odinDevice); + // Skip stopping data stream, unregistering callbacks, closing device, destroying device odinDevice = nullptr; } - // Use const_cast to remove const qualifier + // Create new device if (lidar_create_device(const_cast(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(); + 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"); @@ -136,7 +270,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach odinDevice = nullptr; return; } - + // Register callback lidar_data_callback_info_t data_callback_info; data_callback_info.data_callback = lidar_data_callback; @@ -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 @@ -196,134 +331,166 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach #else ROS_INFO("Device detaching..."); #endif - + + // Set device disconnection flag deviceConnected = false; - - if (odinDevice) { - lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM); - lidar_unregister_stream_callback(odinDevice); - lidar_close_device(odinDevice); - lidar_destory_device(odinDevice); - odinDevice = nullptr; - } + deviceDisconnected = true; + + // Clear all message queues + clear_all_queues(); + + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Waiting for device reconnection..."); + #else + ROS_INFO("Waiting for device reconnection..."); + #endif } } int main(int argc, char *argv[]) { - // ROS initialization - #ifdef ROS2 - rclcpp::init(argc, argv); - auto node = std::make_shared("lydros_node"); - g_ros_object = std::make_shared(node); - #else - ros::init(argc, argv, "lydros_node"); - ros::NodeHandle nh; - g_ros_object = std::make_shared(nh); - #endif +#ifdef ROS2 + rclcpp::init(argc, argv); + auto node = std::make_shared("lydros_node"); + g_ros_object = std::make_shared(node); +#else + ros::init(argc, argv, "lydros_node"); + ros::NodeHandle nh; + g_ros_object = new MultiSensorPublisher(nh); +#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()); - #else - ROS_ERROR("Failed to load config file: %s", config_file.c_str()); - #endif + #ifdef ROS2 + RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_file.c_str()); + #else + ROS_ERROR("Failed to load config file: %s", config_file.c_str()); + #endif 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 it = keys.find(key_name); - if (it != keys.end()) { - return it->second; - } - return default_value; + + auto get_key_value = [&](const std::string& key, int default_value) -> int { + auto it = keys.find(key); + return it != keys.end() ? it->second : 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_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); - - // Set log level + g_sendcloudrender = get_key_value("sendcloudrender", 1); + 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"); + RCLCPP_ERROR(node->get_logger(), "Lidar system init failed"); #else - ROS_ERROR("Lidar system init failed"); + ROS_ERROR("Lidar system init failed"); #endif - return -1; + return -1; } - - // wait device connect + #ifdef ROS2 - RCLCPP_INFO(node->get_logger(), "Waiting for device connection..."); + RCLCPP_INFO(node->get_logger(), "Waiting for device connection..."); #else - ROS_INFO("Waiting for device connection..."); + 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(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(); - return -1; - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::seconds(1)); // Check every second } - + } catch (const std::exception& e) { #ifdef ROS2 - RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what()); + RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what()); #else - ROS_ERROR("Exception: %s", e.what()); + ROS_ERROR("Exception: %s", e.what()); #endif - lidar_system_deinit(); - return -1; + lidar_system_deinit(); + 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); lidar_destory_device(odinDevice); } lidar_system_deinit(); - + return 0; -} +} \ No newline at end of file diff --git a/src/rawCloudRender.cpp b/src/rawCloudRender.cpp new file mode 100644 index 0000000..ac0c86b --- /dev/null +++ b/src/rawCloudRender.cpp @@ -0,0 +1,286 @@ +#include "rawCloudRender.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(); + GlobalCameraParams::g_k3 = cam_node["k3"].as(); + GlobalCameraParams::g_k4 = cam_node["k4"].as(); + GlobalCameraParams::g_k5 = cam_node["k5"].as(); + GlobalCameraParams::g_k6 = cam_node["k6"].as(); + GlobalCameraParams::g_k7 = cam_node["k7"].as(); + + GlobalCameraParams::g_fx = cam_node["A11"].as(); + GlobalCameraParams::g_skew = cam_node["A12"].as(); + GlobalCameraParams::g_fy = cam_node["A22"].as(); + + GlobalCameraParams::g_cx = cam_node["u0"].as(); + GlobalCameraParams::g_cy = cam_node["v0"].as(); + + // === Read extrinsic transformation matrix === + GlobalCameraParams::g_T_camera_lidar << + tclNode[0].as(), tclNode[1].as(), tclNode[2].as(), tclNode[3].as(), + tclNode[4].as(), tclNode[5].as(), tclNode[6].as(), tclNode[7].as(), + tclNode[8].as(), tclNode[9].as(), tclNode[10].as(), tclNode[11].as(), + tclNode[12].as(), tclNode[13].as(), tclNode[14].as(), tclNode[15].as(); + + // === 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>& rgb_image, + capture_Image_List_t* pcd_stream, + int pcdIdx, + std::vector& 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 dist_table; + static bool table_init = [&](){ + for (size_t i=0; i= 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(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(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(pd_2d_x); + const int v = static_cast(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(rgb_image.size()) && u < static_cast(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; +} \ No newline at end of file