·# 激光SLAM第三章作业去除运动畸变

一、原理

1、由于曲线可以通过分段来近似

二、题目介绍

1、题目描述

题目描述

1、本次的作业为实现一个里程计去除激光雷达运动畸变的模块
2、本次的作业里面有两个工程:champion_nav_msgs和LaserUndistortion；大家需要首先编译安装champion_nav_msgs，按照champion_nav_msgs的readme文件执行即可，注意根据自己ubuntu的不同版本做修改。

2、解题步骤

1.实现LidarMotionCalibration（插值并转化坐标系）函数，并进行编译
2.在LaserUndistortion下，进行source：source devel/setup.bash
3.运行launch文件，执行本条指令的时候，必须保证没有任何ros节点在运行，roscore也要关闭。
4.进入到 /bag目录下，运行指令：rosbag play –clock odom.bag。
5.如果一切正常，则会看到pcl的可视化界面，当可视化界面中存在数据的时候，按R键即可看到结果。红色为畸变矫正前，绿色为畸变矫正后。

三、代码框架

1、将原始激光数据转化到矫正需要的数据

2、进行矫正Lidar_Calibration函数

3、将矫正前后的数据转化到pcl中显示

四、函数解析

1、函数名

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)

2、函数参数

 * @param frame_base_pose       标定完毕之后的基准坐标系
 * @param frame_start_pose      本分段第一个激光点对应的位姿
 * @param frame_end_pose        本分段最后一个激光点对应的位姿
 * @param ranges                激光数据－－距离
 * @param angles                激光数据－－角度
 * @param startIndex            本分段第一个激光点在激光帧中的下标
 * @param beam_number           本分段的激光点数量

3、函数代码

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
        //每个位姿进行线性插值时的步长
        double beam_step = 1.0 / (beam_number - 1);

        //机器人的起始角度和最终角度
        tf::Quaternion start_angle_q = frame_start_pose.getRotation();//getRotation()为获取四元数
        tf::Quaternion end_angle_q =  frame_end_pose.getRotation();

        //转换到弧度
        double start_angle_r = tf::getYaw(start_angle_q);
        double base_angle_r = tf::getYaw(end_angle_q);

        //机器人的起始位姿
        tf::Vector3 start_pos = frame_start_pose.getOrigin();//getOrigin()为获取平移向量
        start_pos.setZ(0);

        //最终位姿
        tf::Vector3 end_pos = frame_end_pose.getOrigin();
        end_pos.setZ(0);

        //基础坐标系
        tf::Vector3 base_pos = frame_end_pose.getOrigin();
        base_pos.setZ(0);

        double mid_angle;
        tf::Vector3 mid_pos;
        tf::Vector3 mid_point;

        double lidar_angle,lidar_dist;

        //插值计算每个点对应的位姿
        for (int i = 0; i < beam_number; ++i) {
            //角度插值
            mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q,beam_step * i));
			//slerp：球面线性插值方法
            //lerp：线性插值
            mid_pos = start_pos.lerp(end_pos,beam_step * i);

            //得到激光点在odom 坐标系中的坐标
            double tmp_angle;

            //如果range不等于无穷,则需要进行矫正
            //tfFuzzyZero()函数？
            if(tfFuzzyZero(ranges[startIndex + i]) == false)
            {
                //计算对应的激光点在odom坐标系中的坐标
                //得到这帧激光束距离和夹角
                lidar_dist = ranges[startIndex+i];
                lidar_angle = angles[startIndex+i];

                //激光雷达坐标系下的坐标
                double laser_x,laser_y;
                laser_x = lidar_dist * cos(lidar_angle);
                laser_y = lidar_dist * sin(lidar_angle);

                //里程计坐标系下的坐标
                double odom_x,odom_y;
                odom_x = laser_x * cos(mid_angle) - laser_y * sin(mid_angle) + mid_pos.x();
                odom_y = laser_x * sin(mid_angle) + laser_y * cos(mid_angle) + mid_pos.y();

                //转换到类型中去
                mid_point.setValue(odom_x,odom_y,0);

                //把在odom坐标系中的激光数据点 转换到 基础坐标系
                double x0,y0,a0,s,c;
                x0 = base_pos.x();
                y0 = base_pos.y();
                a0 = base_angle_r;
                s = sin(a0);
                c = cos(a0);
                /*
                 * 把base转换到odom 为[c -s x0
                 *                    s  c y0
                 *                    0  0 1]
                 * 把odom转换到base为 [c  s -x0*c-y0*s
                 *                    -s c x0*s-y0*c
                 *                    0  0 1]
                 */
                double tmp_x,tmp_y;
                tmp_x = mid_point.x()*c + mid_point.y() * s -x0*c -y0*s;
                tmp_y = mid_point.x()*s + mid_point.y() *c +x0*s - y0*c;
                mid_point.setValue(tmp_x,tmp_y,0);

                //然后计算以起始坐标为起点的 dist angle
                double dx,dy;
                dx = (mid_point.x());
                dy = (mid_point.y());
                lidar_dist = sqrt(dx*dx + dy*dy);
                lidar_angle = atan2(dy,dx);

                //激光雷达被矫正
                ranges[startIndex+i] = lidar_dist;
                angles[startIndex+i] = lidar_angle;//基础坐标系下激光的距离和角度值

            }
            //如果等于无穷,则随便计算一下角度
            else
            {
                //激光角度
                lidar_angle = angles[startIndex+i];

                //里程计坐标系的角度
                tmp_angle = mid_angle + lidar_angle;
                tmp_angle = tfNormalizeAngle(tmp_angle);

                //如果数据非法 则只需要设置角度就可以了。把角度换算成start_pos坐标系内的角度
                lidar_angle = tfNormalizeAngle(tmp_angle - start_angle_r);

                angles[startIndex+i] = lidar_angle;
            }
        }
       //end of TODO
    }

主要思路：分段进行插值，得到每一束激光束对应的机器人位姿。并将该beam变成欧式坐标，再投影到世界坐标系下，再投影到base坐标系（整帧的第一个beam）下，再转换成极坐标，这样就将所有的beam修正到了base的极坐标系下

总结

参考了众多博客，学习了运动畸变去除的代码实现，但是代码中许多函数均未使用过，很不熟悉，自己实现起来还是很困难，需要在这方面继续努力。