wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

<add>

Browse Source 
1.Added camera point cloud extrinsic parameter rendering function
2.add image_compressed
3.fix imshow error about slam pointcloud
This commit is contained in:
manifoldsdk
2025-07-23 19:15:59 +08:00
parent 876cc931b4
commit eb750f9fe0
12 changed files with 1462 additions and 421 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+3
View File
@@ -154,6 +154,7 @@ if(ROS_VERSION STREQUAL "ROS1")
add_executable(host_sdk_sample
src/host_sdk_sample.cpp
src/yaml_parser.cpp
src/rawCloudRender.cpp
)
target_link_libraries(host_sdk_sample
${catkin_LIBRARIES}
@@ -198,6 +199,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
add_executable(host_sdk_sample
src/host_sdk_sample.cpp
src/yaml_parser.cpp
src/rawCloudRender.cpp
)
# Link libraries
@@ -292,3 +294,4 @@ message(STATUS "Target platform: ${TARGET_PLATFORM}")
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
message(STATUS "=======================================")
README.md
+63 -33
View File
@@ -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
```
```
### 5.4 RVIZ has not responded for a long time
**Error Message**
Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection...
**Resolution**
Please power on Odin module again
### 5.5 Device not responding
**Error Message**
Missed ok response from device,probably wrong interaction procedure.
**Resolution**
Please adopt the solution mentioned in 5.1
### 5.6 Device has no external calibration file
**Error Message**
ERROR：Missing camera node 'cam_0'
**Resolution**
Please plug and unplug the USB again
config/control_command.yaml
+4 -1
View File
@@ -3,5 +3,8 @@ register_keys:
sendrgb: 1
sendimu: 1
sendodom: 1
senddtof: 0
senddtof: 1
sendcloudslam: 1
sendcloudrender: 1
sendrgbcompressed: 1
config/odin_ros.rviz
+63 -35
View File
@@ -61,34 +61,6 @@ Visualization Manager:
Transport Hint: raw
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /odin1/cloud_raw
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
@@ -110,7 +82,7 @@ Visualization Manager:
Keep: 1
Name: Odometry
Position Tolerance: 0.10000000149011612
Queue Size: 100
Queue Size: 1
Shape:
Alpha: 1
Axes Length: 1
@@ -134,24 +106,80 @@ Visualization Manager:
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: raw
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /odin1/cloud_raw
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8493094444274902
Min Value: -0.13891705870628357
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Name: render
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /odin1/cloud_slam
Topic: /odin1/cloud_render
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: slam
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /odin1/cloud_raw
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -180,7 +208,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 10.812461853027344
Distance: 5.067190170288086
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -196,9 +224,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.830398678779602
Pitch: -0.01460082083940506
Target Frame: <Fixed Frame>
Yaw: 3.0054032802581787
Yaw: 2.2254059314727783
Saved: ~
Window Geometry:
Displays:
config/odin_ros2.rviz
+107 -78
View File
@@ -6,11 +6,7 @@ Panels:
Expanded:
- /Global Options1
- /Status1
- /PointCloud21
- /PointCloud22
- /Image1
- /Image1/Topic1
- /Odometry1
Splitter Ratio: 0.5
Tree Height: 472
- Class: rviz_common/Selection
@@ -47,72 +43,6 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_slam
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: ""
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: ""
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_raw
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/Image
Enabled: true
Max Value: 1
@@ -145,7 +75,7 @@ Visualization Manager:
Value: true
Value: true
Enabled: true
Keep: 100
Keep: 1
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
@@ -165,6 +95,105 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /odin1/odometry_map
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 49
Min Color: 0; 0; 0
Min Intensity: 0
Name: raw
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_raw
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8730175495147705
Min Value: -0.12252448499202728
Value: true
Axis: Z
Channel Name: rgb
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 2.3509885615147286e-38
Min Color: 0; 0; 0
Min Intensity: 0
Name: render
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_render
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: ""
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: slam
Position Transformer: ""
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_slam
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -208,25 +237,25 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10.72309398651123
Distance: 3.231567859649658
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
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: <Fixed 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
Y: 27
include/host_sdk_sample.h
+576 -175
View File
@@ -30,8 +30,25 @@ limitations under the License.
#include <Eigen/Dense>
#include "lidar_api.h"
#include "lidar_api_type.h"
#include "rawCloudRender.h"
#include <deque>
#include <mutex>
#include <vector>
#include <queue>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#define LOG_LEVEL_NONE 0
#define LOG_LEVEL_ERROR 1
#define LOG_LEVEL_WARN 2
#define LOG_LEVEL_INFO 3
#define LOG_LEVEL_DEBUG 4
extern int g_log_level;
extern int g_sendcloudrender;
#ifdef ROS2
#include "rclcpp/rclcpp.hpp"
@@ -44,6 +61,9 @@ limitations under the License.
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/point_field.hpp>
#include <sensor_msgs/msg/image.hpp>
using ImageConstPtr = sensor_msgs::msg::Image::ConstSharedPtr;
using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
namespace ros {
using namespace rclcpp;
using namespace std_msgs::msg;
@@ -51,6 +71,12 @@ limitations under the License.
using namespace nav_msgs::msg;
using Time = builtin_interfaces::msg::Time;
}
#define LOG_ERROR(...)
#define LOG_WARN(...)
#define LOG_INFO(...)
#define LOG_DEBUG(...)
#else
#include <ros/ros.h>
@@ -60,11 +86,43 @@ limitations under the License.
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
using ImageConstPtr = sensor_msgs::ImageConstPtr;
using PointCloud2ConstPtr = sensor_msgs::PointCloud2ConstPtr;
namespace ros {
using namespace ::ros;
using namespace sensor_msgs;
using namespace nav_msgs;
}
#define LOG_ERROR(...) \
if (g_log_level >= LOG_LEVEL_ERROR) { \
ROS_ERROR(__VA_ARGS__); \
}
#define LOG_WARN(...) \
if (g_log_level >= LOG_LEVEL_WARN) { \
ROS_WARN(__VA_ARGS__); \
}
#define LOG_INFO(...) \
if (g_log_level >= LOG_LEVEL_INFO) { \
ROS_INFO(__VA_ARGS__); \
}
#define LOG_DEBUG(...) \
if (g_log_level >= LOG_LEVEL_DEBUG) { \
ROS_DEBUG(__VA_ARGS__); \
}
#endif
#ifdef ROS2
namespace sensor_msgs {
using PointField = msg::PointField;
}
#else
namespace sensor_msgs {
using PointField = ::sensor_msgs::PointField;
}
#endif
// Common definitions
@@ -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<float> iter_x(msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> 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<float> iter_x(msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(msg, "intensity");
#endif
float* xyz_data = static_cast<float*>(stream->imageList[idx].pAddr);
uint16_t* intensity_data = static_cast<uint16_t*>(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<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
#else
*iter_x = pf[2] / 1000.0f; ++iter_x;
*iter_y = -pf[0] / 1000.0f; ++iter_y;
*iter_z = pf[1] / 1000.0f; ++iter_z;
*iter_intensity = static_cast<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
#endif
}
// Publish Intensity point cloud
#ifdef ROS2
cloud_pub_->publish(std::move(msg));
#else
cloud_pub_.publish(msg);
#endif
std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
std::lock_guard<std::mutex> 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<size_t>(image.width) * height_nv12;
if (image.length < expected_size) {
#ifdef ROS2
RCLCPP_ERROR(node_->get_logger(), "RGB buffer too small: expected %zu bytes, got %u bytes",
expected_size, image.length);
#else
ROS_ERROR("RGB buffer too small: expected %zu bytes, got %u bytes",
expected_size, image.length);
#endif
return;
}
try {
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<std::mutex> lock1(rgb_queue_mutex_);
std::lock_guard<std::mutex> 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<int64_t>(rgb_stamp) - static_cast<int64_t>(pcd_stamp);
int64_t abs_time_diff = std::abs(time_diff);
// Check if time difference is within allowed range (50ms)
const int64_t MAX_TIME_DIFF = 50000000; // 50ms in nanoseconds
if (abs_time_diff > MAX_TIME_DIFF) {
// Remove older timestamped message
{
std::lock_guard<std::mutex> lock1(rgb_queue_mutex_);
std::lock_guard<std::mutex> lock2(pcd_queue_mutex_);
if (time_diff > 0) {
// RGB timestamp is newer, remove PCD
pcd_queue_.pop_front();
} else {
// PCD timestamp is newer, remove RGB
rgb_image_queue_.pop_front();
}
}
#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<std::mutex> lock1(rgb_queue_mutex_);
std::lock_guard<std::mutex> 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<std::vector<float>>& rgb_image,
capture_Image_List_t* cloud_stream,
int pcd_idx)
{
// 1. Check RGB image validity
if (rgb_image.empty()) {
#ifndef ROS2
ROS_ERROR("Invalid RGB image: empty vector");
#endif
return false;
}
// Check RGB image dimension consistency
const size_t height = rgb_image.size();
const size_t width = (height > 0) ? rgb_image[0].size() : 0;
if (height == 0 || width == 0) {
#ifndef ROS2
ROS_ERROR("Invalid RGB image dimensions: %zux%zu",
height, width);
#endif
return false;
}
// 2. Check point cloud stream pointer validity
if (!cloud_stream) {
#ifndef ROS2
ROS_ERROR("Invalid cloud stream: null pointer");
#endif
return false;
}
// 3. Check point cloud index validity
if (pcd_idx < 0 || pcd_idx >= 10) {
#ifndef ROS2
ROS_ERROR("Invalid pcd index: %d (must be 0-9)", pcd_idx);
#endif
return false;
}
// 4. Check point cloud data validity
buffer_List_t& cloud = cloud_stream->imageList[pcd_idx];
if (!cloud.pAddr) {
#ifndef ROS2
ROS_ERROR("Invalid cloud data: null pointer");
#endif
return false;
}
if (cloud.width <= 0 || cloud.height <= 0) {
#ifndef ROS2
ROS_ERROR("Invalid cloud dimensions: %dx%d",
cloud.width, cloud.height);
#endif
return false;
}
return true;
}
void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_msg)
{
auto start_time = std::chrono::steady_clock::now();
const int input_image_width = rgb_msg->width;
const int input_image_height = rgb_msg->height;
// Verify input image format
if (rgb_msg->encoding != "bgr8") {
#ifndef ROS2
ROS_ERROR("Unsupported image format: %s. Only bgr8 is supported.",
rgb_msg->encoding.c_str());
#endif
return;
}
// Prepare point cloud data stream
capture_Image_List_t cloud_stream;
const int pcd_idx = 2;
cloud_stream.imageList[pcd_idx].height = pcd_msg->height;
cloud_stream.imageList[pcd_idx].width = pcd_msg->width;
const auto total_point_num = pcd_msg->width * pcd_msg->height;
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*pcd_msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*pcd_msg, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*pcd_msg, "z");
std::vector<float> cloud_flat;
cloud_flat.reserve(total_point_num * 4);
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
cloud_flat.push_back(*iter_y * -1000.0f);
cloud_flat.push_back(*iter_z * 1000.0f);
cloud_flat.push_back(*iter_x * 1000.0f);
cloud_flat.push_back(0.0f);
}
cloud_stream.imageList[pcd_idx].pAddr = cloud_flat.data();
if (input_image_height > 0 && input_image_width > 0) {
// Use BGR8 image data directly
std::vector<std::vector<float>> rgb_image(input_image_height, std::vector<float>(input_image_width));
// Convert BGR8 data to required format for rendering
const uint8_t* bgr_data = rgb_msg->data.data();
for (int y = 0; y < input_image_height; ++y) {
for (int x = 0; x < input_image_width; ++x) {
// Start position of each pixel (BGR format)
int idx = y * rgb_msg->step + x * 3;
// Extract RGB values and combine into 32-bit integer
uint8_t b = bgr_data[idx];
uint8_t g = bgr_data[idx + 1];
uint8_t r = bgr_data[idx + 2];
uint32_t rgb_int = (r << 16) | (g << 8) | b;
// Convert to float
float rgb_float;
std::memcpy(&rgb_float, &rgb_int, sizeof(float));
rgb_image[y][x] = rgb_float;
}
}
// Render colored point cloud
std::vector<float> rgbCloud_flat;
render_.render(rgb_image, &cloud_stream, pcd_idx, rgbCloud_flat);
const int valid_point_num = rgbCloud_flat.size() / 4;
// Create and publish RGB point cloud
PointCloud2Msg output_msg;
output_msg.header.frame_id = "map";
output_msg.header.stamp = rgb_msg->header.stamp; // Use original image timestamp
output_msg.height = 1;
output_msg.width = valid_point_num;
sensor_msgs::PointCloud2Modifier modifier(output_msg);
modifier.setPointCloud2Fields(
4,
"x", 1, sensor_msgs::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<float> iter_res_x(output_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_res_y(output_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_res_z(output_msg, "z");
sensor_msgs::PointCloud2Iterator<float> iter_res_rgb(output_msg, "rgb");
for (int i = 0; i < valid_point_num; i++) {
*iter_res_x = rgbCloud_flat[4*i]; ++iter_res_x;
*iter_res_y = rgbCloud_flat[4*i+1]; ++iter_res_y;
*iter_res_z = rgbCloud_flat[4*i+2]; ++iter_res_z;
*iter_res_rgb = rgbCloud_flat[4*i+3]; ++iter_res_rgb;
}
#ifdef ROS2
rgbcloud_pub_->publish(output_msg);
#else
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<sensor_msgs::msg::PointCloud2>();
#else
auto msg = boost::make_shared<sensor_msgs::PointCloud2>();
#endif
// Set message header
msg->header.frame_id = "map";
msg->header.stamp = ns_to_ros_time(cloud.timestamp);
msg->height = cloud.height;
msg->width = cloud.width;
msg->is_dense = false;
msg->is_bigendian = false;
// Set point cloud fields
sensor_msgs::PointCloud2Modifier modifier(*msg);
modifier.setPointCloud2Fields(
4,
"x", 1, sensor_msgs::PointField::FLOAT32,
"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<float> iter_x(*msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*msg, "intensity");
float* xyz_data = static_cast<float*>(cloud.pAddr);
uint16_t* intensity_data = static_cast<uint16_t*>(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<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
}
{
std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
// Get actual point count
const int real_point_count = cloud.width * cloud.height;
// Create deep copy of point cloud
#ifdef ROS2
auto msg_copy = std::make_shared<sensor_msgs::msg::PointCloud2>(*msg);
#else
auto msg_copy = boost::make_shared<sensor_msgs::PointCloud2>();
*msg_copy = *msg; // Deep copy
#endif
// Queue management
if (pcd_queue_.size() >= 10) {
pcd_queue_.pop_front();
}
// Add to queue (using copy)
pcd_queue_.push_back(msg_copy);
}
// Publish point cloud
#ifdef ROS2
cloud_pub_->publish(*msg);
#else
cloud_pub_.publish(msg);
#endif
}
void publishRgb(capture_Image_List_t *stream) {
buffer_List_t &image = stream->imageList[0];
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<std_msgs::msg::Header>();
header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
header->frame_id = "camera_rgb_frame";
auto cv_image = std::make_shared<cv_bridge::CvImage>(*header, "bgr8", bgr);
auto msg = cv_image->toImageMsg();
// Add to unified queue
if (g_sendcloudrender) {
std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
if (rgb_image_queue_.size() >= 10) {
rgb_image_queue_.pop_front();
}
rgb_image_queue_.push_back(msg);
}
// Publish original image message
rgb_pub_->publish(*msg);
// Create compressed image message
auto compressed_msg = std::make_shared<sensor_msgs::msg::CompressedImage>();
compressed_msg->header = *header;
compressed_msg->format = "jpeg";
// Set compression parameters
std::vector<int> compression_params;
compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
compression_params.push_back(80);
// Compress image
cv::imencode(".jpg", bgr, compressed_msg->data, compression_params);
compressed_rgb_pub_->publish(*compressed_msg);
#else
// ROS1 version
std_msgs::Header header;
header.stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
header.frame_id = "camera_rgb_frame";
#ifdef ROS2
auto cv_image = std::make_shared<cv_bridge::CvImage>(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<cv_bridge::CvImage>(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<std::mutex> lock(rgb_queue_mutex_);
if (rgb_image_queue_.size() >= 10) {
rgb_image_queue_.pop_front();
}
rgb_image_queue_.push_back(msg);
}
// Publish original image message
rgb_pub_.publish(msg);
// Publish compressed image - always publish
// Create compressed image message
sensor_msgs::CompressedImagePtr compressed_msg(new sensor_msgs::CompressedImage());
compressed_msg->header = header;
compressed_msg->format = "jpeg";
// Set compression parameters
std::vector<int> compression_params;
compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
compression_params.push_back(80); // JPEG quality 80%
// Compress image
cv::imencode(".jpg", bgr, compressed_msg->data, compression_params);
compressed_rgb_pub_.publish(compressed_msg);
#endif
} catch (const cv::Exception& e) {
#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<ImageConstPtr> rgb_image_queue_;
const size_t max_rgb_queue_size_ = 10; // Cache up to 10 image frames
std::mutex pcd_queue_mutex_;
std::deque<PointCloud2ConstPtr> pcd_queue_;
const size_t max_pcd_queue_size_ = 10; // Maximum cache frames
// Updated helper functions
ImageConstPtr getLatestRgbImage() {
std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
return (!rgb_image_queue_.empty()) ? rgb_image_queue_.back() : nullptr;
}
PointCloud2ConstPtr getLatestIntensityCloud() {
std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
return (!pcd_queue_.empty()) ? pcd_queue_.back() : nullptr;
}
std::vector<cv::Mat> getRgbImageQueueSnapshot() {
std::lock_guard<std::mutex> lock(rgb_queue_mutex_);
std::vector<cv::Mat> images;
for (const auto& msg : rgb_image_queue_) {
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, "bgr8");
images.push_back(cv_ptr->image.clone());
} catch (cv_bridge::Exception& e) {
#ifndef ROS2
ROS_ERROR("cv_bridge exception: %s", e.what());
#endif
}
}
return images;
}
#ifdef ROS2
std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
std::vector<sensor_msgs::msg::PointCloud2> clouds;
for (const auto& msg_ptr : pcd_queue_) {
clouds.push_back(*msg_ptr);
}
return clouds;
}
#else
std::vector<sensor_msgs::PointCloud2> getIntensityCloudQueueSnapshot() {
std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
std::vector<sensor_msgs::PointCloud2> clouds;
for (const auto& msg_ptr : pcd_queue_) {
clouds.push_back(*msg_ptr);
}
return clouds;
}
#endif
void initialize_publishers() {
#ifdef ROS2
imu_pub_ = node_->create_publisher<ros::Imu>("odin1/imu", 10);
@@ -447,9 +848,10 @@ private:
cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", 10);
xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_map", 10);
rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", 10);
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
#endif
}
#ifdef ROS1
void initialize_publishers(ros::NodeHandle& nh) {
imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 10);
@@ -457,6 +859,8 @@ private:
cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 10);
xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 10);
odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_map", 10);
rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
}
#endif
@@ -467,12 +871,20 @@ private:
rclcpp::Publisher<ros::PointCloud2>::SharedPtr cloud_pub_;
rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
rclcpp::Publisher<ros::Odometry>::SharedPtr odom_publisher_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr rendered_cloud_pub_;
rclcpp::Publisher<PointCloud2Msg>::SharedPtr rgbcloud_pub_;
rclcpp::Publisher<ImageMsg>::SharedPtr rgbFromnv12_pub_;
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_rgb_pub_; // New compressed image publisher
#else
ros::Publisher imu_pub_;
ros::Publisher rgb_pub_;
ros::Publisher cloud_pub_;
ros::Publisher xyzrgbacloud_pub_;
ros::Publisher odom_publisher_;
ros::Publisher rendered_cloud_pub_;
ros::Publisher rgbcloud_pub_;
ros::Publisher rgbFromnv12_pub_;
ros::Publisher compressed_rgb_pub_; // New compressed image publisher
#endif
};
@@ -494,13 +906,9 @@ public:
if (it != kv_map.end()) {
if (it->second != value) {
it->second = value;
std::cout << "Set " << key << " = " << value << std::endl;
cb_to_invoke = callback;
} else {
std::cout << "Set ignored: " << key << " is already " << value << std::endl;
}
}
} else {
std::cout << "Unknown key: " << key << std::endl;
return;
}
}
@@ -516,13 +924,6 @@ public:
return (it != kv_map.end()) ? it->second : -1;
}
void print_all() const {
std::lock_guard<std::mutex> lock(mtx);
std::cout << "Available keys and values:" << std::endl;
for (const auto& [key, value] : kv_map) {
std::cout << " " << key << " = " << value << std::endl;
}
}
void register_callback(Callback cb) {
std::lock_guard<std::mutex> lock(mtx);
@@ -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 */
include/lidar_api.h
+12 -1
View File
@@ -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
#endif // LIDAR_API_H
include/rawCloudRender.h
+83
View File
@@ -0,0 +1,83 @@
/*
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#ifndef RGBCLOUD_H
#define RGBCLOUD_H
#include <vector>
#include <string>
#include <Eigen/Dense>
#include "lidar_api.h"
namespace GlobalCameraParams {
extern float g_fx;
extern float g_fy;
extern float g_cx;
extern float g_cy;
extern float g_skew;
extern float g_k2;
extern float g_k3;
extern float g_k4;
extern float g_k5;
extern float g_k6;
extern float g_k7;
extern Eigen::Matrix4f g_T_camera_lidar;
}
class rawCloudRender {
public:
bool init(const std::string& yamlFilePath);
void nv12buffer_2_rgb(buffer_List_t &image, std::vector<std::vector<float>>& rgb_image);
void render(std::vector<std::vector<float>>& rgb_image, capture_Image_List_t* pcdStream, int pcdIdx, std::vector<float>& rgbCloud_flat);
void print_camera_calib();
int getImageWidth() const { return image_width_; }
int getImageHeight() const { return image_height_; }
private:
std::string model_type_;
std::string camera_name_;
int image_width_;
int image_height_;
int frame_size_;
bool opencv_available_;
// 4x4 transformation matrix (T_camera_lidar)
Eigen::Matrix4f T_camera_lidar_;
float k2_;
float k3_;
float k4_;
float k5_;
float k6_;
float k7_;
float p1_;
float p2_;
float A11_fx_;
float A12_skew_;
float A22_fy_;
float u0_cx_;
float v0_cy_;
bool isFast_;
int numDiff_;
float maxIncidentAngle_;
};
#endif // RGBCLOUD_H
lib/liblydHostApi_amd.a
BIN
View File
Binary file not shown.
lib/liblydHostApi_arm.a
BIN
View File
Binary file not shown.
src/host_sdk_sample.cpp
+265 -98
View File
@@ -1,22 +1,67 @@
#include "host_sdk_sample.h"
#include "yaml_parser.h"
#include "rawCloudRender.h"
#include <filesystem>
#include <thread>
#include <string>
#include <stdexcept>
#include <atomic>
#include <mutex>
#include <memory>
#include <opencv2/opencv.hpp>
#include <deque>
#include <unistd.h>
#include <cstdlib>
#include <cstring>
#include <sys/types.h>
#include <sys/wait.h>
#include <signal.h>
#include <chrono>
#ifdef ROS2
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/rclcpp.hpp>
#else
#include <ros/package.h>
#include <ros/ros.h>
#endif
// Global variable declarations
static device_handle odinDevice = nullptr;
static std::shared_ptr<MultiSensorPublisher> g_ros_object;
static std::atomic<bool> deviceConnected(false);
static std::atomic<bool> deviceDisconnected(false); // Device disconnection flag
static std::mutex device_mutex; // Device operation mutex lock
// Function to get package share path
#ifdef ROS2
std::shared_ptr<MultiSensorPublisher> g_ros_object = nullptr;
#else
MultiSensorPublisher* g_ros_object = nullptr;
#endif
int g_log_level = LOG_LEVEL_INFO;
int g_show_fps = 0; // FPS display toggle control
static std::mutex g_rgb_mutex;
static std::shared_ptr<cv::Mat> g_latest_bgr;
static uint64_t g_latest_rgb_timestamp = 0;
static bool g_has_rgb = false;
static capture_Image_List_t g_latest_rgb;
static bool g_renderer_initialized = false;
static std::shared_ptr<rawCloudRender> g_renderer = nullptr;
// Global configuration variables
int g_sendrgb = 1;
int g_sendimu = 1;
int g_senddtof = 1;
int g_sendodom = 1;
int g_sendcloudslam = 0;
int g_sendcloudrender = 0;
int g_sendrgb_compressed = 0;
// Function declarations
void clear_all_queues();
// Get package share path
std::string get_package_share_path(const std::string& package_name) {
#ifdef ROS2
try {
@@ -33,15 +78,31 @@ std::string get_package_share_path(const std::string& package_name) {
#endif
}
// Global configuration variables
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<device_handle *>(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<lidar_device_info_t*>(device), &odinDevice)) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed");
@@ -124,7 +183,82 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
// Set mode
// Get package path
const std::string package_name = "odin_ros_driver";
std::string config_dir = "";
#ifdef ROS2
// Get source code directory (not install directory)
char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
if (ros_workspace) {
// Infer source directory from COLCON_PREFIX_PATH
std::string workspace_path(ros_workspace);
// Remove "/install" part
size_t pos = workspace_path.find("/install");
if (pos != std::string::npos) {
config_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/config";
} else {
// Fallback to install directory
config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
}
} else {
// Fallback to install directory
config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
}
#else
config_dir = ros::package::getPath(package_name) + "/config";
#endif
// Print path information
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Calibration files will be saved to: %s", config_dir.c_str());
#else
ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str());
#endif
// Get calibration files - using modified function
if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
#else
ROS_ERROR("Failed to get calibration file");
#endif
lidar_close_device(odinDevice);
lidar_destory_device(odinDevice);
odinDevice = nullptr;
return;
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files");
#else
ROS_INFO("Successfully retrieved calibration files");
#endif
// Move point cloud renderer initialization here
std::string calib_config = config_dir + "/calib.yaml";
if (std::filesystem::exists(calib_config)) {
g_renderer = std::make_shared<rawCloudRender>();
if (g_renderer->init(calib_config)) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloud renderer initialized");
#else
ROS_INFO("Point cloud renderer initialized");
#endif
} else {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to initialize point cloud renderer");
#else
ROS_ERROR("Failed to initialize point cloud renderer");
#endif
}
} else {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Renderer config file not found: %s", calib_config.c_str());
#else
ROS_WARN("Renderer config file not found: %s", calib_config.c_str());
#endif
}
if (lidar_set_mode(odinDevice, type)) {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed");
@@ -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<rclcpp::Node>("lydros_node");
g_ros_object = std::make_shared<MultiSensorPublisher>(node);
#else
ros::init(argc, argv, "lydros_node");
ros::NodeHandle nh;
g_ros_object = std::make_shared<MultiSensorPublisher>(nh);
#endif
#ifdef ROS2
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("lydros_node");
g_ros_object = std::make_shared<MultiSensorPublisher>(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<std::chrono::seconds>(now - start);
if (elapsed.count() >= 30) {
#ifdef ROS2
RCLCPP_ERROR(node->get_logger(), "No device connected after 30 seconds");
RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
#else
ROS_ERROR("No device connected after 30 seconds");
ROS_INFO("Waiting for device connection...");
#endif
lidar_system_deinit();
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;
}
}
src/rawCloudRender.cpp
+286
View File
@@ -0,0 +1,286 @@
#include "rawCloudRender.h"
#include <yaml-cpp/yaml.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <algorithm>
#include <iostream>
#include <cmath>
#include <array>
#include <algorithm>
struct ValidPointInfo {
float x;
float y;
float z;
int u;
int v;
};
namespace GlobalCameraParams {
float g_fx = 0.0f;
float g_fy = 0.0f;
float g_cx = 0.0f;
float g_cy = 0.0f;
float g_skew = 0.0f;
float g_k2 = 0.0f;
float g_k3 = 0.0f;
float g_k4 = 0.0f;
float g_k5 = 0.0f;
float g_k6 = 0.0f;
float g_k7 = 0.0f;
Eigen::Matrix4f g_T_camera_lidar = Eigen::Matrix4f::Identity();
}
bool raw_debug=0;
bool rawCloudRender::init(const std::string& yamlFilePath) {
YAML::Node config;
try {
config = YAML::LoadFile(yamlFilePath);
} catch (const YAML::BadFile& e) {
std::cerr << "Error: Could not open file '" << yamlFilePath << "' - " << e.what() << std::endl;
return false;
} catch (const YAML::ParserException& e) {
std::cerr << "Error: YAML parsing failed - " << e.what() << std::endl;
return false;
} catch (const std::exception& e) {
std::cerr << "Unexpected error: " << e.what() << std::endl;
return false;
}
// Intrinsic parameters node (cam_0)
const std::string cam_node_name = "cam_0";
if (!config[cam_node_name]) {
std::cerr << "Error: Missing camera node '" << cam_node_name << "'" << std::endl;
return false;
}
YAML::Node cam_node = config[cam_node_name];
// Directly access Tcl_0 node
const std::string tcl_node_name = "Tcl_0";
if (!config[tcl_node_name]) {
std::cerr << "Error: Missing transformation matrix node '" << tcl_node_name << "'" << std::endl;
return false;
}
YAML::Node tclNode = config[tcl_node_name];
if (tclNode.size() != 16) {
std::cerr << "Error: Transformation matrix must be 4x4 (16 elements)" << std::endl;
return false;
}
// === Read intrinsic parameters ===
GlobalCameraParams::g_k2 = cam_node["k2"].as<float>();
GlobalCameraParams::g_k3 = cam_node["k3"].as<float>();
GlobalCameraParams::g_k4 = cam_node["k4"].as<float>();
GlobalCameraParams::g_k5 = cam_node["k5"].as<float>();
GlobalCameraParams::g_k6 = cam_node["k6"].as<float>();
GlobalCameraParams::g_k7 = cam_node["k7"].as<float>();
GlobalCameraParams::g_fx = cam_node["A11"].as<float>();
GlobalCameraParams::g_skew = cam_node["A12"].as<float>();
GlobalCameraParams::g_fy = cam_node["A22"].as<float>();
GlobalCameraParams::g_cx = cam_node["u0"].as<float>();
GlobalCameraParams::g_cy = cam_node["v0"].as<float>();
// === Read extrinsic transformation matrix ===
GlobalCameraParams::g_T_camera_lidar <<
tclNode[0].as<float>(), tclNode[1].as<float>(), tclNode[2].as<float>(), tclNode[3].as<float>(),
tclNode[4].as<float>(), tclNode[5].as<float>(), tclNode[6].as<float>(), tclNode[7].as<float>(),
tclNode[8].as<float>(), tclNode[9].as<float>(), tclNode[10].as<float>(), tclNode[11].as<float>(),
tclNode[12].as<float>(), tclNode[13].as<float>(), tclNode[14].as<float>(), tclNode[15].as<float>();
// === Display key parameters concisely ===
std::cout << "=== Camera Calibration Parameters ===" << std::endl;
std::cout << "Intrinsics:" << std::endl;
std::cout << " fx: " << GlobalCameraParams::g_fx
<< ", fy: " << GlobalCameraParams::g_fy
<< ", cx: " << GlobalCameraParams::g_cx
<< ", cy: " << GlobalCameraParams::g_cy << std::endl;
std::cout << "Distortion: k2=" << GlobalCameraParams::g_k2
<< ", k3=" << GlobalCameraParams::g_k3 << std::endl;
Eigen::Vector3f translation = GlobalCameraParams::g_T_camera_lidar.block<3,1>(0,3);
Eigen::Matrix3f rotation = GlobalCameraParams::g_T_camera_lidar.block<3,3>(0,0);
std::cout << "Extrinsics:" << std::endl;
std::cout << " Translation: [" << translation.x() << ", "
<< translation.y() << ", " << translation.z() << "]" << std::endl;
std::cout << " Rotation (euler angles): "
<< rotation.eulerAngles(0,1,2).transpose() * 180/M_PI << "°" << std::endl;
return true;
}
void rawCloudRender::render(std::vector<std::vector<float>>& rgb_image,
capture_Image_List_t* pcd_stream,
int pcdIdx,
std::vector<float>& rgbCloud_flat)
{
// Initialize constants
constexpr float inv_1000 = 0.001f;
const float fx = GlobalCameraParams::g_fx;
const float fy = GlobalCameraParams::g_fy;
const float cx = GlobalCameraParams::g_cx;
const float cy = GlobalCameraParams::g_cy;
const float skew = GlobalCameraParams::g_skew;
const float k2 = GlobalCameraParams::g_k2;
const float k3 = GlobalCameraParams::g_k3;
const float k4 = GlobalCameraParams::g_k4;
const float k5 = GlobalCameraParams::g_k5;
const float k6 = GlobalCameraParams::g_k6;
const float k7 = GlobalCameraParams::g_k7;
const Eigen::Matrix4f& T = GlobalCameraParams::g_T_camera_lidar;
// Precompute matrix elements
const float T00 = T(0,0), T01 = T(0,1), T02 = T(0,2), T03 = T(0,3);
const float T10 = T(1,0), T11 = T(1,1), T12 = T(1,2), T13 = T(1,3);
const float T20 = T(2,0), T21 = T(2,1), T22 = T(2,2), T23 = T(2,3);
// Initialize lookup table
static std::array<float, 10000> dist_table;
static bool table_init = [&](){
for (size_t i=0; i<dist_table.size(); ++i) {
float theta = i * (M_PI/2) / dist_table.size();
dist_table[i] = theta*(1 + theta*(k2 + theta*(k3 + theta*(k4 + theta*(k5 + theta*(k6 + theta*k7))))));
}
return true;
}();
// Get point cloud data (direct access)
if (!pcd_stream || pcdIdx < 0 || pcdIdx >= 10) {
std::cerr << "ERROR: Invalid pcd_stream or index in render function" << std::endl;
return;
}
buffer_List_t& pcd_buffer = pcd_stream->imageList[pcdIdx];
const int total_points = pcd_buffer.height * pcd_buffer.width;
if (!pcd_buffer.pAddr) {
std::cerr << "ERROR: Null point cloud data pointer in render function" << std::endl;
return;
}
float* data = static_cast<float*>(pcd_buffer.pAddr);
// Prepare output
rgbCloud_flat.clear();
rgbCloud_flat.resize(total_points * 4); // Preallocate maximum space
float* output_ptr = rgbCloud_flat.data();
// Get image dimensions
const int img_height = 1296;
const int img_width = 1600;
// Process point cloud
int valid_count = 0;
for (int idx = 0; idx < total_points; ++idx)
{
float* pf = data + idx*4;
// Quick check for invalid points
if (std::abs(pf[0]) < 1e-5f && std::abs(pf[1]) < 1e-5f && std::abs(pf[2]) < 1e-5f) {
continue;
}
// Coordinate transformation
const float x = pf[2] * inv_1000;
const float y = -pf[0] * inv_1000;
const float z = pf[1] * inv_1000;
// Manual matrix transformation
const float x1 = T00*x + T01*y + T02*z + T03;
const float y1 = T10*x + T11*y + T12*z + T13;
const float z1 = T20*x + T21*y + T22*z + T23;
// Check for points behind camera
if (z1 <= 0.0f) continue;
// Calculate projection
const float x1_sq = x1*x1;
const float y1_sq = y1*y1;
const float z1_sq = z1*z1;
const float norm = std::sqrt(x1_sq + y1_sq + z1_sq);
if (norm < 1e-7f) continue;
const float r = std::sqrt(x1_sq + y1_sq);
if (r < 1e-7f) continue;
const float cost = z1 / norm;
const float theta = std::acos(cost);
// Safe table lookup
const size_t table_idx = static_cast<size_t>(theta * (2.0f/M_PI) * dist_table.size());
const size_t safe_idx = std::min(table_idx, dist_table.size()-1);
const float thetad = dist_table[safe_idx];
const float scaling = thetad / r;
const float xd = x1 * scaling;
const float yd = y1 * scaling;
const float pd_2d_x = xd * fx + yd * skew + cx;
const float pd_2d_y = yd * fy + cy;
// Quick boundary check
const int u = static_cast<int>(pd_2d_x);
const int v = static_cast<int>(pd_2d_y);
// Strict boundary check
if (u < 0 || u >= img_width || v < 0 || v >= img_height) {
continue;
}
// Safe image data access
if (v < static_cast<int>(rgb_image.size()) && u < static_cast<int>(rgb_image[v].size())) {
*output_ptr++ = x;
*output_ptr++ = y;
*output_ptr++ = z;
*output_ptr++ = rgb_image[v][u];
valid_count++;
} else {
// Handle invalid coordinates
static bool warned = false;
if (!warned) {
if(raw_debug)
{
std::cerr << "WARNING: Invalid image coordinates: u=" << u << ", v=" << v
<< " (image size: " << rgb_image.size() << "x"
<< (rgb_image.empty() ? 0 : rgb_image[0].size()) << ")" << std::endl;
}
warned = true;
}
}
}
// Resize output
rgbCloud_flat.resize(output_ptr - rgbCloud_flat.data());
if(raw_debug)
{
std::cout << "Render completed: " << total_points << " points processed, "
<< valid_count << " valid points ("
<< (100.0 * valid_count / total_points) << "%)" << std::endl;
}
}
void rawCloudRender::print_camera_calib() {
std::cout << model_type_ << std::endl;
std::cout << image_width_ << std::endl;
std::cout << image_height_ << std::endl;
std::cout << "T_camera_lidar" << std::endl;
std::cout << T_camera_lidar_(0,0) << " " << T_camera_lidar_(0,1) << " " << T_camera_lidar_(0,2) << " " << T_camera_lidar_(0,3) << std::endl;
std::cout << T_camera_lidar_(1,0) << " " << T_camera_lidar_(1,1) << " " << T_camera_lidar_(1,2) << " " << T_camera_lidar_(1,3) << std::endl;
std::cout << T_camera_lidar_(2,0) << " " << T_camera_lidar_(2,1) << " " << T_camera_lidar_(2,2) << " " << T_camera_lidar_(2,3) << std::endl;
std::cout << T_camera_lidar_(3,0) << " " << T_camera_lidar_(3,1) << " " << T_camera_lidar_(3,2) << " " << T_camera_lidar_(3,3) << std::endl;
std::cout << "cam" << std::endl;
std::cout << k2_ << std::endl;
std::cout << k3_ << std::endl;
std::cout << k4_ << std::endl;
std::cout << k5_ << std::endl;
std::cout << k6_ << std::endl;
std::cout << k7_ << std::endl;
std::cout << A11_fx_ << std::endl;
std::cout << A12_skew_ << std::endl;
std::cout << A22_fy_ << std::endl;
std::cout << u0_cx_ << std::endl;
std::cout << v0_cy_ << std::endl;
}