激光雷达生成地图和位置

功能类LidarMap

#pragma once

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <fast_gicp/gicp/fast_gicp.hpp>

namespace slam_lidar
{
    class LidarMap
    {
    public:
        LidarMap() = default;
        ~LidarMap() = default;

        void update(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
        {
            try
            {
                pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZI>);
                pcl::fromROSMsg(*cloud_msg, *pcl_cloud);

                // 降采样
                pcl::PointCloud<pcl::PointXYZI>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZI>);
                pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
                voxel_grid.setLeafSize(0.5f, 0.5f, 0.5f); // 设置体素网格大小
                voxel_grid.setInputCloud(pcl_cloud);
                voxel_grid.setDownsampleAllData(true);
                voxel_grid.filter(*downsampled_cloud);

                if (is_first_)
                {
                    referent_cloud_ = downsampled_cloud;
                    is_first_ = false;
                }

                fast_gicp::FastGICP<pcl::PointXYZI, pcl::PointXYZI> gicp;
                gicp.setNumThreads(4);                  // 设置线程数
                gicp.setMaxCorrespondenceDistance(1.0); // 设置最大匹配距离
                gicp.setTransformationEpsilon(1e-6);    // 设置变换收敛阈值
                gicp.setEuclideanFitnessEpsilon(1e-6);  // 设置拟合误差阈值
                gicp.setInputTarget(referent_cloud_);
                gicp.setInputSource(downsampled_cloud);

                auto temp_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
                gicp.align(*temp_cloud, Eigen::Matrix4f::Identity());
                Eigen::Matrix4f cur_transf = gicp.getFinalTransformation();
                global_transf_ = global_transf_ * cur_transf;

                // 将点云转换到全局坐标系
                map_cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
                pcl::transformPointCloud(*pcl_cloud, *map_cloud_, global_transf_);
                referent_cloud_ = downsampled_cloud;
            }
            catch (...)
            {
                ROS_ERROR("lidar update error");
            }
        }

        pcl::PointCloud<pcl::PointXYZI>::Ptr getMapPoint()
        {
            return map_cloud_;
        }

        Eigen::Matrix4f getLidarPose()
        {
            return global_transf_;
        }

    private:
        pcl::PointCloud<pcl::PointXYZI>::Ptr referent_cloud_;
        Eigen::Matrix4f global_transf_{Eigen::Matrix4f::Identity()};
        bool is_first_{true};
        pcl::PointCloud<pcl::PointXYZI>::Ptr map_cloud_;
    };
}

使用

LidarMap lidar_map_;
pcl::PointCloud<pcl::PointXYZI>::Ptr all_cloud_{new pcl::PointCloud<pcl::PointXYZI>()};
pcl::PointCloud<pcl::PointXYZI>::Ptr cur_cloud_;
Eigen::Matrix4f lidar_pose_;
        
void handleLidarPoint(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
    // std::cout << "Lidar Data Received: " << cloud_msg->width << " points" << std::endl;

    // 将点云转换到全局坐标系
    try
    {
        lidar_map_.update(cloud_msg);
        cur_cloud_ = lidar_map_.getMapPoint();
        *all_cloud_ += *cur_cloud_;

        // // 降采样
        // pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
        // voxel_grid.setLeafSize(0.5f, 0.5f, 0.5f); // 设置体素网格大小
        // voxel_grid.setInputCloud(all_cloud_);
        // voxel_grid.setDownsampleAllData(true);
        // voxel_grid.filter(*all_cloud_);

        // 获取位姿
        lidar_pose_ = lidar_map_.getLidarPose();
    }
    catch (...)
    {
        ROS_ERROR("slam node handleLidarPoint error");
    }
}