去除激光雷达运动畸变

#include 
#include 
#include 
#include 

#include 

#include 
#include 
#include 

#include 
#include 
#include 
#include 

pcl::visualization::CloudViewer g_PointCloudView（“PointCloud View“）;

class LidarMotionCalibrator
{
public:

    LidarMotionCalibrator（tf::TransformListener* tf）
    {
        tf_ = tf;
        scan_sub_ = nh_.subscribe（“scan“ 10 &LidarMotionCalibrator::ScanCallBack this）;
    }


    ~LidarMotionCalibrator（）
    {
        if（tf_!=NULL）
            delete tf_;
    }

    // 拿到原始的激光数据来进行处理
    void ScanCallBack（const sensor_msgs::LaserScanConstPtr& scan_msg）
    {
        //转换到矫正需要的数据
        ros::Time startTime endTime;
        startTime = scan_msg->header.stamp;

        sensor_msgs::LaserScan laserScanMsg = *scan_msg;

        //得到最终点的时间
        int beamNum = laserScanMsg.ranges.size（）;
        endTime = startTime + ros::Duration（laserScanMsg.time_increment * beamNum）;

        // 将数据复制出来
        std::vector anglesranges;
        for（int i = 0; i < beamNum;i++）
        {
            double lidar_dist = laserScanMsg.ranges[i];
            double lidar_angle = laserScanMsg.angle_min + laserScanMsg.angle_increment * i;

            ranges.push_back（lidar_dist）;
            angles.push_back（lidar_angle）;
        }

        //转换为pcl::pointcloud for visuailization

        visual_cloud_.clear（）;
        for（int i = 0; i < ranges.size（）;i++）
        {

            if（ranges[i] < 0.05 || std::isnan（ranges[i]） || std::isinf（ranges[i]））
                continue;


            pcl::PointXYZRGB pt;
            pt.x = ranges[i] * cos（angles[i]）;
            pt.y = ranges[i] * sin（angles[i]）;
            pt.z = 1.0;

            // pack r/g/b into rgb
            unsigned char r = 255 g = 0 b = 0;    //red color
            unsigned int rgb = （（unsigned int）r << 16 | （unsigned int）g << 8 | （unsigned int）b）;
            pt.rgb = *reinterpret_cast（&rgb）;

            visual_cloud_.push_back（pt）;
        }
        std::cout << std::endl;


        //进行矫正
        Lidar_Calibration（rangesangles
                          startTime
                          endTime
                          tf_）;

        //转换为pcl::pointcloud for visuailization
        for（int i = 0; i < ranges.size（）;i++）
        {

            if（ranges[i] < 0.05 || std::isnan（ranges[i]） || std::isinf（ranges[i]））
                continue;

            pcl::PointXYZRGB pt;
            pt.x = ranges[i] * cos（angles[i]）;
            pt.y = ranges[i] * sin（angles[i]）;
            pt.z = 1.0;

            unsigned char r = 0 g = 255 b = 0;    // blue color
            unsigned int rgb = （（unsigned int）r << 16 | （unsigned int）g << 8 | （unsigned int）b）;
            pt.rgb = *reinterpret_cast（&rgb）;

            visual_cloud_