·# 激光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 angledouble 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的极坐标系下

总结

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

激光SLAM第三章作业去除运动畸变相关推荐

《地球概论》（第3版）笔记第三章地球的运动
目录第三章地球的运动第六节地球的自转 301 地球自转及其证明 302 地球自转的规律性 302-1 地轴和极移 302-2 地轴进动 302-3 地球自转的周期 302-4 真太阳日与平太阳 ...
《计算机网络》第三章作业
<计算机网络>第三章作业复习题 R3 考虑在主机A和主机B之间有一条TCP连接.假设从主机A传送到主机B的TCP报文段具有源端口号x和目的端口号y.对于从主机B传送到主机A的报文段,源端 ...
【编译原理】龙书第三章作业答案
[编译原理]龙书第三章作业答案练习3.1.1:根据3.1.2节中的讨论,将下面的C++程序划分成正确的词素序列.哪些词素应该有相关联的词法值?应该具有什么值? 答案: 左列为词素,右列为值,划分如下 ...
网络安全-技术与实践 | 第三章作业布置 by cdut 赵老师
网络安全-技术与实践 | 第三章作业布置 by cdut 赵继老师 1:ARP的主要功能是将(IP)地址转换成为(物理)地址注解:ARP:Address Resolution Protocol(地址 ...
JAVA学习第三章作业
1.程序填空题: 定义一个抽象类Person,其中有一个公共的抽象方法showInfo().然后定义此抽象类的一个子类Student,包括的成员变量有姓名.学号.班级和年龄,且此类中包括两个构造方法. ...
从零开始手写VIO第三章作业（含关键点细节及思维过程）
文章目录前言·与同主题博文的不同 1.代码修改 1.1阻尼因子 µ 随着迭代变化的曲线图 1.2完成曲线y = ax^2^ + bx + c的参数估计 1.3实现其他更优秀的阻尼因子策略 2.公式推 ...
大学c语言第三章作业,华中科技大学光电子学院C语言第三章
<华中科技大学光电子学院C语言第三章>由会员分享,可在线阅读,更多相关<华中科技大学光电子学院C语言第三章(20页珍藏版)>请在装配图网上搜索. 1.第三章简单程序设计 1. ...
计算机组成原理 mov(r0)，-(sp),第三章作业
电子科技大学廖建明计算机组成原理第三章答案 2012060020029 王衡 1．某主存储器部分单元的地址码与存储器内容对应关系如下: 地址码存储内容 1000H A307H 1001H 0B3F ...
大学c语言第三章作业,c语言程序设计一章部分和第三章习题答案.doc
c语言程序设计一章部分和第三章习题答案实验报告课程名称 C语言程序设计A 实验项目编程环境认知与顺序程序设计实验仪器 PC机一台学院_____信息管理学院_______ 专业信息 ...

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