激光SLAM理论与实践-第五期 第三次作业（去运动畸变）

1、章节：

1、激光SLAM理论与实践-第五期 第一次作业（矩阵坐标变换）
2、激光SLAM理论与实践-第五期 第二次作业（里程计标定）
3、激光SLAM理论与实践-第五期 第三次作业（去运动畸变）
4、激光SLAM理论与实践-第五期 第四次作业（-帧间匹配算法，imls-icp和csm）
5、激光SLAM理论与实践-第五期 第五次作业（高斯牛顿法优化）
6、激光SLAM理论与实践-第五期 第六次作业 (g2o优化方法)
7、激光SLAM理论与实践-第五期 第七次作业 （mapping）

2、课程PPt和源码

https://download.csdn.net/download/weixin_44023934/85491811








全部代码：

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <sensor_msgs/LaserScan.h>

#include <champion_nav_msgs/ChampionNavLaserScan.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

#include <iostream>
#include <dirent.h>
#include <fstream>
#include <iostream>
//Eigen
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

pcl::visualization::CloudViewer g_PointCloudView("PointCloud View");

class LidarMotionCalibrator
{
public:

    LidarMotionCalibrator(tf::TransformListener* tf)
    {
        tf_ = tf;
        //每当检测到topic champion_scan 就会运行回调函数ScanCallBack的内容
        scan_sub_ = nh_.subscribe("champion_scan", 10, &LidarMotionCalibrator::ScanCallBack, this);
    }


    ~LidarMotionCalibrator()
    {
        if(tf_!=NULL)
            delete tf_;
    }

    // 拿到原始的激光数据来进行处理
    void ScanCallBack(const champion_nav_msgs::ChampionNavLaserScanPtr& scan_msg)
    {
        //转换到矫正需要的数据
        ros::Time startTime, endTime;
        startTime = scan_msg->header.stamp;//记录起始时间

        champion_nav_msgs::ChampionNavLaserScan laserScanMsg = *scan_msg;

        //
        int beamNum = laserScanMsg.ranges.size();
        //time_increment每两个点的时间间隔 得到最终点的时间
        endTime = startTime + ros::Duration(laserScanMsg.time_increment * (beamNum - 1));

        // 将数据复制出来
        std::vector<double> angles,ranges;
        for(int i = beamNum - 1; i >= 0; --i)
        {   //每个激光点的距离，角度
            double lidar_dist = laserScanMsg.ranges[i];
            double lidar_angle = laserScanMsg.angles[i];
            //去除异常值
            if(lidar_dist < 0.05 || std::isnan(lidar_dist) || std::isinf(lidar_dist))
                lidar_dist = 0.0;
            //存放至容器
            ranges.push_back(lidar_dist);
            angles.push_back(lidar_angle);
        }

        //转换为pcl::pointcloud for visuailization
        //获取开始时刻的位姿
        tf::Stamped<tf::Pose> visualPose;
        if(!getLaserPose(visualPose, startTime, tf_))
        {

            ROS_WARN("Not visualPose,Can not Calib");
            return ;
        }
        //获取位姿的旋转角
        double visualYaw = tf::getYaw(visualPose.getRotation());

        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;
            //距离转化为坐标
            double x = ranges[i] * cos(angles[i]);
            double y = ranges[i] * sin(angles[i]);

            pcl::PointXYZRGB pt;
            //将激光坐标转化变换到机器人坐标
            pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
            pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
            pt.z = 1.0;

            // pack r/g/b into rgb
            unsigned char r = 255, g = 124, b = 0;    //red color
            unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
            //reinterpret_cast强制转换类型
            //显示绿色
            pt.rgb = *reinterpret_cast<float*>(&rgb);

            visual_cloud_.push_back(pt);
        }
        std::cout << std::endl;



        //进行矫正
        Lidar_Calibration(ranges,
                          angles,
                          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;

            double x = ranges[i] * cos(angles[i]);
            double y = ranges[i] * sin(angles[i]);


            pcl::PointXYZRGB pt;
            pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
            pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
            pt.z = 1.0;

            unsigned char r = 0, g = 255, b = 0;    // green color
            unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
            pt.rgb = *reinterpret_cast<float*>(&rgb);

            visual_cloud_.push_back(pt);
        }

        //进行显示
         g_PointCloudView.showCloud(visual_cloud_.makeShared());
    }


    /**                                                                                                   
     * @name getLaserPose()
     * @brief 得到机器人在里程计坐标系中的位姿tf::Pose
     *        得到dt时刻激光雷达在odom坐标系的位姿
     * @param odom_pos  机器人的位姿
     * @param dt        dt时刻
     * @param tf_
    */
   //tf::Stamped 模板类
    bool getLaserPose(tf::Stamped<tf::Pose> &odom_pose,
                      ros::Time dt,
                      tf::TransformListener * tf_)
    {
        odom_pose.setIdentity();//设置为单位矩阵

        tf::Stamped < tf::Pose > robot_pose;
        robot_pose.setIdentity();
        robot_pose.frame_id_ = "base_laser";
        robot_pose.stamp_ = dt;   //设置为ros::Time()表示返回最近的转换关系

        // get the global pose of the robot
        try
        {
            //base-laser 在odom 坐标系下dt时刻的坐标
            if(!tf_->waitForTransform("/odom", "/base_laser", dt, ros::Duration(0.5)))             // 0.15s 的时间可以修改
            {
                ROS_ERROR("LidarMotion-Can not Wait Transform()");
                return false;
            }
            //将robotpose转到里程计"/odom"坐标系下，输出的位置为odompose
            tf_->transformPose("/odom", robot_pose, odom_pose);
        }    
        
    // 异常类：tf::ConnectivityException
    // 作用：如果由于两个坐标系ID不在同一个连接的树中而无法完成请求，则抛出。
    // 异常类：tf::ExtrapolationException
    // 作用：如果请求的坐标系id之间存在连接，但一个或多个变换已过期，则抛出。
    // 异常类：tf::InvalidArgument
    // 作用：如果参数无效则抛出。 最常见的情况是非规范化的四元数。
    // 异常类：tf::LookupException
    // 作用：如果引用了未发布的坐标系ID，则抛出

        catch (tf::LookupException& ex)
        {
            ROS_ERROR("LidarMotion: No Transform available Error looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf::ConnectivityException& ex)
        {
            ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf::ExtrapolationException& ex)
        {
            ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %s\n", ex.what());
            return false;
        }

        return true;
    }


    /**
     * @brief Lidar_MotionCalibration
     *        激光雷达运动畸变去除分段函数;
     *        在此分段函数中，认为机器人是匀速运动；
     *        主要通过插值，求出机器人在该点的里程计运动量，接着将激光点变换到里程计坐标下，
     *        然后将该点的起始位姿变换到这一帧的第一个点的起始位姿，
     *        再保存到在激光雷达世界坐标系下的容器中，替换之前的数据
     * @param frame_base_pose       标定完毕之后的基准坐标系
     * @param frame_start_pose      本分段第一个激光点对应的位姿
     * @param frame_end_pose        本分段最后一个激光点对应的位姿
     * @param ranges                激光数据－－距离
     * @param angles                激光数据－－角度
     * @param startIndex            本分段第一个激光点在激光帧中的下标
     * @param beam_number           本分段的激光点数量
     */
    void Lidar_MotionCalibration(
            tf::Stamped<tf::Pose> frame_base_pose,
            tf::Stamped<tf::Pose> frame_start_pose,
            tf::Stamped<tf::Pose> frame_end_pose,
            std::vector<double>& ranges,//待求
            std::vector<double>& angles,//待求
            int startIndex,
            int& beam_number)
    {
       //TODO
       //end of TODO
       //用于位姿插值
      tf::Vector3 base_pose = frame_base_pose.getOrigin();
      tf::Vector3 start_pose = frame_start_pose.getOrigin();
      tf::Vector3 end_pose = frame_end_pose.getOrigin();
       base_pose.setZ(0);
       start_pose.setZ(0);
       end_pose.setZ(0);
       //四元数用于计算旋转角插值
       tf::Quaternion base_pose_q =frame_base_pose.getRotation();
       tf::Quaternion start_pose_q = frame_start_pose.getRotation();
       tf::Quaternion end_pose_q = frame_end_pose.getRotation();
       //获取弧度
       double base_yaw = tf::getYaw( base_pose_q);
       double start_yaw = tf::getYaw(start_pose_q);
       double end_yaw = tf::getYaw(end_pose_q);
       //插值后获取的角度和位置
       double mid_yaw ;
       tf::Vector3 mid_pose;
       for(int i= 0;i<beam_number;i++)
       {
           double k = i/(beam_number-1);
           mid_pose = start_pose.lerp(end_pose,k);
           mid_yaw =tf::getYaw(start_pose_q.slerp(end_pose_q,k));
           if(ranges[i+startIndex] > 0.05 ||   
           !std::isnan(ranges[i+startIndex]) ||  
            !std::isinf(ranges[i+startIndex]))

          {
            //距离转化为坐标
            double lida_x = ranges[i+startIndex] * cos(angles[i+startIndex]);
            double lida_y = ranges[i+startIndex] * sin(angles[i+startIndex]);
            //因为激光雷达的位姿和机器人的位姿一致，所以不想要将传感器转到机器人坐标系下
            //激光点云转到里程计坐标系下 
            /*
            * Tol表示当前点云到里程计坐标的变换
            */
            Eigen::Matrix3d Tol;
            Tol  <<   cos(mid_yaw) ,-sin(mid_yaw), mid_pose.x(),
                     sin(mid_yaw) ,  cos(mid_yaw) ,mid_pose.y(),
                     0.0   , 0.0  ,    1.0;
           Eigen::Vector3d odom_pose;
           odom_pose  << lida_x,lida_y,1.0;//1不能省略，否则少了平移部分
           odom_pose =Tol*odom_pose;
            //将点云坐标系变换到这一帧的第一个点云的世界位姿
           /*
            * Two表示第一个点到世界的坐标
            *   需要乘以逆
            */
            Eigen::Matrix3d Two;
            Two  <<  cos(base_yaw) ,-sin(base_yaw), base_pose.x(),
                     sin(base_yaw) , cos(base_yaw) ,base_pose.y(),
                     0.0   , 0.0  ,    1.0;
            Eigen::Vector3d mid_pose_;
            mid_pose_  << odom_pose.x(),odom_pose.y(),1.0;//1不能省略，否则少了平移部分
            //将激光点从世界坐标系变换到雷达坐标系下，所以是逆矩阵
            mid_pose_ = Two.inverse()*mid_pose_;
            //转化为极坐标,表示激光雷达下的坐标点
            ranges[startIndex+i] = sqrt(mid_pose_.x()*mid_pose_.x()+mid_pose_.y()*mid_pose_.y());
            angles[startIndex+i] = atan2(mid_pose_.y(),mid_pose_.x());
          }
       }
    }



    //激光雷达数据　分段线性进行插值
    //这里会调用Lidar_MotionCalibration()
    /**
     * @name Lidar_Calibration()
     * @brief 激光雷达数据　分段线性进行差值　分段的周期为5ms
     * @param ranges 激光束的距离值集合
     * @param angle　激光束的角度值集合
     * @param startTime　第一束激光的时间戳
     * @param endTime　最后一束激光的时间戳
     * @param *tf_
    */
    void Lidar_Calibration(std::vector<double>& ranges,
                           std::vector<double>& angles,
                           ros::Time startTime,
                           ros::Time endTime,
                           tf::TransformListener * tf_)
    {
        //统计激光束的数量
        int beamNumber = ranges.size();
        if(beamNumber != angles.size())
        {
            ROS_ERROR("Error:ranges not match to the angles");
            return ;
        }

        // 5ms来进行分段
        int interpolation_time_duration = 5 * 1000;

        tf::Stamped<tf::Pose> frame_start_pose;
        tf::Stamped<tf::Pose> frame_mid_pose;
        tf::Stamped<tf::Pose> frame_base_pose;
        tf::Stamped<tf::Pose> frame_end_pose;

        //起始时间 us
        double start_time = startTime.toSec() * 1000 * 1000;
        double end_time = endTime.toSec() * 1000 * 1000;
        double time_inc = (end_time - start_time) / (beamNumber - 1); // 每束激光数据的时间间隔

        //当前插值的段的起始坐标
        int start_index = 0;

        //起始点的位姿 这里要得到起始点位置的原因是　起始点就是我们的base_pose
        //所有的激光点的基准位姿都会改成我们的base_pose
        // ROS_INFO("get start pose");

        if(!getLaserPose(frame_start_pose, ros::Time(start_time /1000000.0), tf_))
        {
            ROS_WARN("Not Start Pose,Can not Calib");
            return ;
        }

        if(!getLaserPose(frame_end_pose,ros::Time(end_time / 1000000.0),tf_))
        {
            ROS_WARN("Not End Pose, Can not Calib");
            return ;
        }

        int cnt = 0;
        //基准坐标就是第一个位姿的坐标
        frame_base_pose = frame_start_pose;
        for(int i = 0; i < beamNumber; i++)
        {
            //分段线性,时间段的大小为interpolation_time_duration
            //time_inc 每两个点之间的时间间隔
            double mid_time = start_time + time_inc * (i - start_index);
            if(mid_time - start_time > interpolation_time_duration || (i == beamNumber - 1))
            {
                cnt++;

                //得到起点和终点的位姿
                //终点的位姿
                if(!getLaserPose(frame_mid_pose, ros::Time(mid_time/1000000.0), tf_))
                {
                    ROS_ERROR("Mid %d Pose Error",cnt);
                    return ;
                }

                //对当前的起点和终点进行插值
                //interpolation_time_duration中间有多少个点.
                int interp_count = i - start_index + 1;

                Lidar_MotionCalibration(frame_base_pose,//一帧最初始的位姿
                                        frame_start_pose,//第i段的起始位姿
                                        frame_mid_pose,//第i段的中间位姿
                                        ranges,
                                        angles,
                                        start_index,//表示第i段的上一段
                                        interp_count);//一段内有几个点

                //更新时间
                start_time = mid_time;
                start_index = i;
                frame_start_pose = frame_mid_pose;
            }
        }
    }

public:
    tf::TransformListener* tf_;
    ros::NodeHandle nh_;
    ros::Subscriber scan_sub_;

    pcl::PointCloud<pcl::PointXYZRGB> visual_cloud_;
};




int main(int argc,char ** argv)
{
    ros::init(argc,argv,"LidarMotionCalib");

    tf::TransformListener tf(ros::Duration(10.0));//创建tf监听器

    LidarMotionCalibrator tmpLidarMotionCalib(&tf);

    ros::spin();
    return 0;
}