深蓝学院激光slam 理论与实践第三章激光雷达去畸变作业习题

淩亂°似流年 2022-09-03 13:18 303阅读 0赞

1. 补充去除激光雷达运动畸变模块的代码;

第一题代码说明:  
本题目为实现一个里程计去除激光雷达运动畸变的代码模块,作业里面有两个工程:  
champion\_nav\_msgs 和 LaserUndistortion。大家需要首先编译安装 champion\_nav\_msgs,按照  
champion\_nav\_msgs 的 readme 文件执行即可,或运行命令 sudo bash install.sh,注意如果你的  
ubuntu 版本不是 kinetic,要将所有 kinetic 的地方修改成你的 ros 版本。  
程序运行过程为:  
Step1:实现 207 行 LidarMotionCalibration 函数,并用 catkin\_make 命令进行编译;  
Step2:在 LaserUndistortion 下,进行 source:source devel/setup.bash;  
Step3:运行 launch 文件:roslaunch LaserUndistortion LaserUndistortion.launch,执行本条指令的时候,必须保证没有任何 ROS 节点在运行, roscore 也要关闭;  
Step4:进入到 /bag 目录下,运行指令:rosbag play -–clock laser.bag;  
Step5:如果一切正常,则会看到 pcl 的可视化界

解题思路：

> 用插值的方法，从而得到每一个小时刻的位姿，从而去除运动畸变。

明确几个物理量：  
（1）start\_time : 激光数据开始的时刻。

（2）end\_time : endTime = startTime + ros::Duration(laserScanMsg.time\_increment \* beamNum); 激光数据结束的时刻。

（3）frame\_start\_pose : 雷达数据开始时刻，从odom获取的start\_time时刻的机器人位姿。

（4）frame\_mid\_pose : 中间某一帧雷达时刻，从odom获取的mid\_time时刻的机器人位姿。

（5）frame\_end\_pose : 雷达数据结束时刻，从odom获取的end\_time时刻的机器人位姿。

（6）frame\_base\_pose : 基准坐标，就是将所有去畸变的电云放在这个坐标系下。这里我们将第一个雷达数据所在位姿作为基准坐标。（基准位姿）

（7）interpolation\_time\_duration : 插值间隔。如果mid\_time - start\_time > interpolation\_time\_duration，则进行插值.

（8）interp\_count : 在插值间隔中有几个激光点。

坐标转换流程：

激光雷达坐标系 --> 视觉里程计坐标系 --> 基础坐标系

插值思想：  
![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70][]

每次都选取中间的一个超过插值时间的激光时间段，起点为start，终点为middle，这个段的起点终点位姿由里程计获取，可以用!tf\_->waitForTransform("/odom", “/base\_laser”, dt, ros::Duration(0.5)))等上一个，避免时间不同步的问题。对内部进行插值，有几个点插值几个，使得每个激光点都有属于自己的位姿。

全部源码

#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-1.8/pcl/point_cloud.h>
    #include <pcl-1.8/pcl/visualization/cloud_viewer.h>
    #include <pcl/point_types.h>
    
    #include <dirent.h>
    #include <fstream>
    #include <iostream>
    
    pcl::visualization::CloudViewer g_PointCloudView("PointCloud View");
    
    class LidarMotionCalibrator {
    public:
        LidarMotionCalibrator(tf::TransformListener* tf)
        {
            tf_ = tf;
            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();
            endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);
    
            // 将数据复制出来
            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 = 0, b = 0; //red 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);
            }
            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_
        */
        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 {
                if (!tf_->waitForTransform("/odom", "/base_laser", dt, ros::Duration(0.5))) // 0.15s 的时间可以修改
                {
                    ROS_ERROR("LidarMotion-Can not Wait Transform()");
                    return false;
                }
                tf_->transformPose("/odom", robot_pose, odom_pose);
            } 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
            // frame_base_pose.inverse;
            tf::Quaternion start_q = frame_start_pose.getRotation();
            tf::Quaternion end_q = frame_end_pose.getRotation();
            tf::Vector3 start_xy(frame_start_pose.getOrigin().getX(), frame_start_pose.getOrigin().getY(), 1);
            tf::Vector3 end_xy(frame_end_pose.getOrigin().getX(), frame_end_pose.getOrigin().getY(), 1);
    
            for (size_t i = startIndex; i < startIndex + beam_number; i++) {
                tf::Vector3 mid_xy = start_xy.lerp(end_xy, (i - startIndex) / (beam_number - 1));
                tf::Quaternion mid_q = start_q.slerp(end_q, (i - startIndex) / (beam_number - 1));
                tf::Transform mid_frame(mid_q, mid_xy);
                double x = ranges[i] * cos(angles[i]);
                double y = ranges[i] * sin(angles[i]);
                tf::Vector3 calib_point = frame_base_pose.inverse() * mid_frame * tf::Vector3(x, y, 1);
                ranges[i] = sqrt(calib_point[0] * calib_point[0] + calib_point[1] * calib_point[1]);
                angles[i] = atan2(calib_point[1], calib_point[0]);
            }
            //end of TODO
        }
    
        //激光雷达数据　分段线性进行插值
        //这里会调用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; // 每束激光数据的时间间隔
    
            //当前插值的段的起始坐标
            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
                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,
                        frame_mid_pose,
                        ranges,
                        angles,
                        start_index,
                        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));
    
        LidarMotionCalibrator tmpLidarMotionCalib(&tf);
    
        ros::spin();
        return 0;
    }

实验结果截图：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1][]

2. 阅读论文 Least-Squares Fitting of Two 3-D Points Sets，推导并证明已知对应点的 ICP 求解方法；

推导：

对于两组点云：

![20210807234230774.png][]

求解R和t，使得下式最小：

![20210807234244877.png][]

上式进行变换：

![20210807234253845.png][]

其中，цx 和 цp分别为X点云和P点云的质心。

转换为去除质心的形式：

![20210807234303355.png][]

展开得到

![20210807234315304.png][]

由于后面两项都有

![20210807234329333.png][]

我们可以令该项等于0，然后式子转化为

![20210807234356645.png][]

=0求解t。

上式进一步转化，得到：

![20210807234542861.png][]

测量相位差：

![20210807234826869.png][]

（2）

![20210807234835629.png][]

右图是激光雷达光束模型的联合概率分布图。

其中ZtK表示测量返回的距离值，Zmax表示激光雷达可能测量得到的最大数值。

由四种概率分布组成：

蓝色是高斯概率分布，代表实际测量距离及其不确定性。

绿色是指数分布，代表检测到动态障碍物的可行性。

红色代表随机噪声。

粉色代表错误测量的可能性。

4. 简答题，开放性答案：设计使用 IMU 去除激光雷达运动畸变的方法并回答问题。

（1）仅用 IMU 去除运动畸变可能会有哪些不足之处？

（2）在仅有 IMU 和激光雷达传感器的情况下，你会如何设计运动畸变去除方案(平移+旋转)，达到较好的畸变去除效果?

（1）不足之处：

IMU对于线加速度进行二次积分可能误差较大

1.  设计：

在每一个激光雷达数据帧内部，使用imu进行积分，得到IMU里程计。

通过时间同步，然后确定每一个激光雷达点束的在IMU里程计坐标系下面的位置

将每一个激光雷达点束都矫正到该帧起始位姿下面。

对于n时刻矫正好的激光雷达数据，使用icp算法，将n时刻与n-1时刻的矫正好的激光雷达帧进行匹配，得到激光雷达匹配的位姿。

使用这个位姿纠正imu的累计误差，然后继续矫正下一帧数据。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70]: /images/20220829/6dbba07d506f41aa92ef85f9b4214cb7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1]: /images/20220829/78bbe0670c7a475cb3987968833bb14b.png
[20210807234230774.png]: /images/20220829/1e1b17ab585e4649b39c2ebf93aa3ea1.png
[20210807234244877.png]: /images/20220829/e26c7d8f5ab9468890fa1b033c72e8ed.png
[20210807234253845.png]: /images/20220829/b856d677653b41ef996a55a3771027c3.png
[20210807234303355.png]: /images/20220829/e3525da064394a92a600833b50e9209e.png
[20210807234315304.png]: /images/20220829/56d3383428ba46a381d6fb02debdce21.png
[20210807234329333.png]: /images/20220829/c40c9f15456b4cd985cf3e58eb505a96.png
[20210807234356645.png]: /images/20220829/9c18cc23ac8145039d185705282339ea.png
[20210807234542861.png]: /images/20220829/487e10e236654fdbb4e0450eb5c0cd98.png
[20210807234826869.png]: /images/20220829/5ecd0de9ac5a4bf2b3ed0468f7c1efa5.png
[20210807234835629.png]: /images/20220829/8cc38df66cc542a1be8e71e8739bc669.png