多传感器数据融合到base_link坐标系下

1.简介

此项目实现四个传感器的数据融合，包括顶部激光雷达（如：图中1），两个单线激光雷达(如：图中2，3)，和一个交叉线激光雷达(如：图中4)。这个代码主要包括：时间对齐和空间对齐。其中时间对齐和空间对齐的方法值得看官借鉴。

2.时间对齐

将交叉线激光雷达数据和两个线激光雷达数据放入成员变量中，如下面代码中的声明，然后裁剪出与当前时间最近的数据进行融合。

    // Data queuesstd::deque<sensor_msgs::msg::PointCloud2> cross_lidar_queue_;std::deque<sensor_msgs::msg::PointCloud2> left_lidar_queue_;    std::deque<sensor_msgs::msg::PointCloud2> right_lidar_queue_;void LidarFusion::TopLidarCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) {const rclcpp::Time current_stamp = msg->header.stamp;    is_used_cross_lidar_ = PruneCrossLidarDeque(current_stamp);is_used_left_edge_ = PruneLeftEdgeDeque(current_stamp);is_used_right_edge_ = PruneRightEdgeDeque(current_stamp);FuseAndPublish(*msg, current_cross_msg_, current_left_msg_,current_right_msg_);}---------------------------------bool LidarFusion::PruneCrossLidarDeque(const rclcpp::Time& stamp) {std::lock_guard<std::mutex> lock(cross_lock_);if (cross_lidar_queue_.empty()) return false;auto best_match = cross_lidar_queue_.end();double min_diff = std::numeric_limits<double>::max();for (auto it = cross_lidar_queue_.begin(); it != cross_lidar_queue_.end(); ++it) {const auto msg_time = rclcpp::Time(it->header.stamp);const double diff = fabs((msg_time - stamp).seconds());if (diff < min_diff) {min_diff = diff;best_match = it;}// Early exit if we pass the target time  过了目标时间就提前退出              if (msg_time > stamp) {// Check previous element if it existsif (it != cross_lidar_queue_.begin()) {auto prev_it = std::prev(it);const double prev_diff = fabs((rclcpp::Time(prev_it->header.stamp) - stamp).seconds());if (prev_diff < diff) {min_diff = prev_diff;best_match = prev_it;}}break;}}// RCLCPP_INFO(get_logger(), "PruneCrossLidarDeque ,min_diff : %f", min_diff);if (best_match != cross_lidar_queue_.end() && min_diff <= time_tolerance_) {current_cross_msg_ = *best_match;cross_lidar_queue_.erase(cross_lidar_queue_.begin(), best_match + 1);return true;}return false;}

3.空间对齐

void LidarFusion::FuseAndPublish(const sensor_msgs::msg::PointCloud2& scan_lidar,const sensor_msgs::msg::PointCloud2& cross_cloud,const sensor_msgs::msg::PointCloud2& edge_lidar,const sensor_msgs::msg::PointCloud2& right_cloud) {// 处理数据（示例：打印激光扫描范围和点云尺寸）// RCLCPP_INFO(get_logger(), "edge_lidar width: %d, right_cloud width: %d, scan_lidar width: %d, cross_cloud width: %d",//   edge_lidar.width,//   right_cloud.width, //   scan_lidar.width,//   cross_cloud.width); //记录顶部激光雷达数目laser_point_num_ = scan_lidar.width;// 转换顶部激光雷达点云pcl::PointCloud<pcl::PointXYZ> single_cloud;pcl::fromROSMsg(scan_lidar, single_cloud);pcl::PointCloud<pcl::PointXYZ> transformed_cloud_single;transformed_cloud_single.header = single_cloud.header;  Eigen::Affine3f transform = Eigen::Affine3f::Identity();transform.translation() << 0.0095f, 0.0f, 0.084089f;Eigen::Matrix3f rotation_matrix;rotation_matrix = Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX()) *Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitZ());transform.rotate(rotation_matrix);pcl::transformPointCloud(single_cloud, transformed_cloud_single, transform);// 融合的点云pcl::PointCloud<pcl::PointXYZ> merged_cloud;merged_cloud = transformed_cloud_single; // 低矮障碍物点云pcl::PointCloud<pcl::PointXYZ> obstacle_cloud; // 左线激光雷达if(is_used_left_edge_)  {pcl::PointCloud<pcl::PointXYZ> edge_cloud;pcl::fromROSMsg(edge_lidar, edge_cloud);pcl::PointCloud<pcl::PointXYZ> transformed_cloud_edge;transformed_cloud_edge.header = edge_cloud.header;    transform = Eigen::Affine3f::Identity();transform.translation() << -0.06f, 0.19f, 0.024f;rotation_matrix = Eigen::AngleAxisf(1.6019082f,Eigen::Vector3f::UnitZ()) *Eigen::AngleAxisf(0.87f,Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX());transform.rotate(rotation_matrix);pcl::transformPointCloud(edge_cloud, transformed_cloud_edge, transform);// 创建一个新的点云对象用于存储过滤后的点pcl::PointCloud<pcl::PointXYZ> filtered_transformed_cloud;// 遍历变换后的点云，并过滤掉y值大于0.25的点for (const auto& point : transformed_cloud_edge.points) {if (point.z > -0.05) { // 只保留y值小于等于0.25的点filtered_transformed_cloud.push_back(point);            // // 打印满足条件的点的信息// RCLCPP_INFO(get_logger(), "left edge lidar, Point : X: %f, Y: %f, Z: %f", //             point.x, point.y, point.z);}}// 将过滤后的点云合并到最终结果中merged_cloud += filtered_transformed_cloud;obstacle_cloud += filtered_transformed_cloud;}if(is_used_right_edge_){// 沿边传感器变换pcl::PointCloud<pcl::PointXYZ> right_edge_cloud;pcl::fromROSMsg(right_cloud, right_edge_cloud);  pcl::PointCloud<pcl::PointXYZ> transformed_cloud_right_edge;transformed_cloud_right_edge.header = right_edge_cloud.header;transform = Eigen::Affine3f::Identity();transform.translation() << -0.06f , -0.19f, 0.024f; rotation_matrix = Eigen::AngleAxisf(-1.6019082f,Eigen::Vector3f::UnitZ()) *      Eigen::AngleAxisf(0.87f,Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX()); transform.rotate(rotation_matrix);pcl::transformPointCloud(right_edge_cloud, transformed_cloud_right_edge, transform);// 遍历变换后的点云，并过滤掉y值大于0.25的点         pcl::PointCloud<pcl::PointXYZ> filtered_transformed_cloud_right;  // 创建一个新的点云对象用于存储过滤后的点for (const auto& point : transformed_cloud_right_edge.points) {if (point.z > -0.05) {  filtered_transformed_cloud_right.push_back(point);  // 打印满足条件的点的信息// RCLCPP_INFO(get_logger(), "right edge lidar, Point : X: %f, Y: %f, Z: %f", //               point.x, point.y, point.z);}} merged_cloud += filtered_transformed_cloud_right;obstacle_cloud += filtered_transformed_cloud_right; }if(is_used_cross_lidar_) {// 交叉激光雷达变换pcl::PointCloud<pcl::PointXYZ> pcl_cloud;pcl::fromROSMsg(cross_cloud, pcl_cloud);pcl::PointCloud<pcl::PointXYZ> transformed_cloud_cross;transformed_cloud_cross.header = pcl_cloud.header;    transform = Eigen::Affine3f::Identity();transform.translation() << 0.125f, 0.0f, 0.084089f;rotation_matrix = Eigen::AngleAxisf(0.0f,Eigen::Vector3f::UnitX()) *Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(1.5708f,Eigen::Vector3f::UnitZ());transform.rotate(rotation_matrix);pcl::transformPointCloud(pcl_cloud, transformed_cloud_cross, transform);// 对转换后的点云的x值乘以-1.0for (auto& point : transformed_cloud_cross.points) {point.y *= -1.0f;}// 计算交叉激光左点std::vector<float> focus_point = CalculateFocusPoint(transformed_cloud_cross);if(focus_point.size() > 2){cross_left_point_.x = focus_point[0];cross_left_point_.y = focus_point[1];cross_right_point_.x = focus_point[2];cross_right_point_.y = focus_point[3];cross_left_point_.is_valid = true;cross_right_point_.is_valid = true;}merged_cloud += transformed_cloud_cross;obstacle_cloud  += transformed_cloud_cross;} //发布低矮障碍物点云 sensor_msgs::msg::PointCloud2 output_msg1;pcl::toROSMsg(obstacle_cloud, output_msg1);// 添加到队列中if(is_used_lower_obstacle_)UpdateLowerObstacle(output_msg1);// 将融合的点云发布出去sensor_msgs::msg::PointCloud2 output_msg;pcl::toROSMsg(merged_cloud, output_msg);    output_msg.header.stamp = this->now();output_msg.header.frame_id = "base_link";   fusion_pointcloud_publisher_->publish(output_msg);//发布融合后的二维激光雷达数据sensor_msgs::msg::LaserScan point2scan_ = PointCloudToLaserscan(merged_cloud);point2scan_.header.frame_id = "base_link";pub_fusion_scan_->publish(point2scan_);//清空数据cross_left_point_.clear();cross_right_point_.clear();}
} // namespace beefast_filter_mask