从零搭建无人车(1)-激光雷达数据预处理
- 2020 年 3 月 4 日
- 筆記
从零搭建无人车(1)-激光雷达数据预处理
一 原理简述
1 激光雷达产生畸变的原因
通常激光雷达在工作的时候，我们认为其每一帧激光对应一个位姿（相当于认为这帧激光的时间里机器人的坐标没变，也就是静止），但是每帧激光的时间里机器人实际上是在运动的。
2 常见的去除畸变的方法-里程计辅助方法
第一种是纯估计方法：ICP类方法，VICP。
另外一种就是里程计辅助的方法，十分常用。
我们用里程的位姿来标定激光雷达。这么说还是比较笼统，
其实就是以里程计坐标为基准通过插值来求出每一激光束对应的机器人位姿（暂且认为里程计得到的坐标准确）。
大体流程
由原始激光数据，我们可以知道每一束激光打在障碍物上的激光点距离里程计坐标系原点的距离值和角度值。
转换为距离值和角度值,去畸变之后,转化为sensor_msgs::pointcloud重新发布
去畸变流程
那么首先我们要做一个机器人运动的假设，假设机器人在一帧激光数据的时间里做匀速运动。
然后把里程计得到的坐标存储到一个队列里面，和激光雷达数据的时间对齐之后要保证里程计队列完全覆盖这一帧的扫描数据。
我们以激光雷达扫描的一帧激光数据为例，这一帧数据里有若干个激光束.
实际上我们去畸变的过程就是求解出具体每一束激光对应的机器人位姿，
求解出位姿再把这个位姿转换回扫描数据的距离值和角度值，重新发布出去。
二 代码框架
==1 主函数==
初始化LidarMotionCalib节点
ros::init(argc,argv,"LidarMotionCalib");构建一个tf监听者对象
tf::TransformListener tf(ros::Duration(10.0));构建 LidarMotionCalibration 对象 , 把tf作为构造函数的参数传入
LidarMotionCalibrator tmpLidarMotionCalib(&tf);进入回调函数
ros::spin();==2 LidarMotionCalibrator类==
先看变量这部分：
public: tf::TransformListener* tf_; ros::NodeHandle nh_; ros::Subscriber scan_sub_; ros::Publisher points_pub_;- 定义了一个tf监听者 tf_
- 定义了节点句柄 nh_
- 定义了一个订阅者（ scan_sub_），订阅 sensor_msgs::LaserScan
- 定义了一个点云数据发布者( points_pub_ ) 发布 sensor_msgs::pointcloud
==3 LidarMotionCalibrator类的构造函数与析构函数==
- 参数是tf，初始化时监听ros的坐标变换
- 订阅者 scan_sub_ 订阅名为 /scan 的 topic ，注册消息回调函数。这个地方加上引用，不是很懂
- 发布者 points_pub_ 发布类型为
LidarMotionCalibrator( tf::TransformListener* tf ) { tf_ = tf; scan_sub_ = nh_.subscribe("/scan", 10, &LidarMotionCalibrator::ScanCallBack, this); points_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/cloud", 10); } ~LidarMotionCalibrator() { if(tf_!=NULL) delete tf_; }==4 回调函数 void ScanCallBack()==
当接收到订阅的消息后，就会进入消息回调函数。
传入参数为 LaserScan
void ScanCallBack(const sensor_msgs::LaserScanPtr &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<double> angles,ranges; double lidar_angle = laserScanMsg.angle_max; for( int i = beamNum - 1; i > 0; i-- ) { double lidar_dist = laserScanMsg.ranges[i]; // 计算每个距离值对应的角度信息 lidar_angle -= laserScanMsg.angle_increment ; 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); }调用去畸变的函数
Lidar_Calibration(ranges, angles, startTime, endTime, tf_);根据机器人当前的航向角,转化为点云
tf::Stamped<tf::Pose> visualPose; if( !getLaserPose(visualPose, startTime, tf_)) { ROS_WARN("Not visualPose,Can not Calib"); return ; } // 得到机器人的yaw角度 double visualYaw = tf::getYaw(visualPose.getRotation()); //转换为 pointcloud sensor_msgs::PointCloud cloud; cloud.header.frame_id = "odom"; cloud.header.stamp = ros::Time::now(); cloud.points.resize( ranges.size() ); 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]); geometry_msgs::Point32 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 = 0.1; cloud.points.push_back(pt); }发布点云数据
points_pub_.publish(cloud);==5 得到里程计位姿的函数 bool getLaserPose()==
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_footprint"; robot_pose.stamp_ = dt; //设置为ros::Time()表示返回最近的转换关系 // get the global pose of the robot try { if ( !tf_->waitForTransform("/odom", "/base_footprint", 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: %sn", ex.what()); return false; } catch (tf::ConnectivityException& ex) { ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %sn", ex.what()); return false; } catch (tf::ExtrapolationException& ex) { ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %sn", ex.what()); return false; } return true; }==6 Lidar_Calibration()函数==
参数
- 存储激光数据的距离值和角度值的vector
- 当前帧激光的开始时间
- 当前帧激光的结束时间
- tf监听者。
void Lidar_Calibration(std::vector<double> &ranges, std::vector<double> &angles, ros::Time startTime,ros::Time endTime, tf::TransformListener *tf_)流程
- 统计激光束的数量，需要两个vector的大小一致。
int beamNumber = ranges.size(); if (beamNumber != angles.size()) { ROS_ERROR("Error:ranges not match to the angles"); return; }- 定义一个int型变量，值为5000。
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; }把基准坐标设置成得到的起始点坐标
开始进入循环：
-
遍历每一束激光，得到每一束激光的时间，如果这束激光的时间-上一个断点点的时间大于5ms就分段，计数+1。
-
仍在for循环里，得到每一个断点的里程计下的坐标，（肯定会得到，没得到就报错了）
-
通过子函数Lidar_MotionCalibration对当前段进行插值。当前段是指从start_index到i的段。
每次循环结束，start_index会+1，i不一定加几。。所以插值的就是一小段。 - 跳转到子函数Lidar_MotionCalibration。参数：基准坐标，这一段的开始坐标，这一段的结束坐标，存储角度值和距离值的vector。起始下标和里面的断点数，这里面都是2。
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); // 如果两次时间差大于 5ms 或者 i 已经到了最后一个激光束 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); //更新时间 //就是下次从当前段的mid time 插值 start_time = mid_time; start_index = i; // 下次的开始位姿就是当前插值段的最终位姿 frame_start_pose = frame_mid_pose; } }==7Lidar_MotionCalibration函数（实际进行插值的函数==
最后一个参数其实就是输出的位姿数，其实就是把每一束的激光点对应的位姿都求出来。
三 遇到的问题
1 坐标转换问题
在实现实际插值的函数时候，我们通过里程计插值得到的机器人位姿是在里程计坐标系下的（里程计坐标系是整个程序的固定坐标系）。然后我们需要把这个坐标转换到机器人的基准坐标系下，基准坐标系的原点是这帧激光第一个激光束对应的机器人位姿。拿到每束激光在里程计下的坐标首先要把它转换到基准坐标系下。
对每一小段的位姿线性插值得到的是每个激光束对应的点在里程计坐标系下的坐标。
原始数据可视化过程的坐标转换：
每个激光束的距离值和角度值——>每一束激光点在雷达坐标系下的坐标——>每一束激光点在lidar基准坐标系下的坐标——>显示
从雷达坐标系向基准坐标系转化时，转换矩阵就是由这一帧的第一个激光点时间对应的机器人位姿确定的。
这一帧的每一束激光点都是这样转化显示的，而实际上，随着机器人的运动，机器人在里程计下的位姿时刻在变化，所以显示出来的点云会有畸变。
去畸变过程的坐标转换：
激光束的距离值和角度值-——>每一束激光点在雷达坐标系下的坐标——>每个激光束的点在里程计坐标系下的坐标——>每个激光点在基准坐标系中的坐标——>计算在基准坐标系下的角度值和距离值
激光点的位姿从lidar坐标系到odom坐标系的转换，插值得到机器人在里程计坐标系中的转换。
这样每一束激光点都对应了一个转换关系。没有去畸变的时候，这个转化关系是不变的，只是把所有的点在雷达坐标系下的坐标都按着这一帧的第一个激光点对应的转化关系转换的。这样其实就是忽略了机器人在这一帧激光数据的时间内的运动问题。
就这个问题，说一下运动机器人坐标变换。几种坐标系，map、odom、base_link。odom是全局坐标系。
代码
#include <ros/ros.h> #include <tf/tf.h> #include <tf/transform_broadcaster.h> #include <tf/transform_listener.h> #include <sensor_msgs/LaserScan.h> #include <pcl-1.8/pcl/point_cloud.h> #include <pcl/point_types.h> #include <iostream> #include <dirent.h> #include <fstream> #include <iostream> #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Core> class LidarMotionCalibrator { public: LidarMotionCalibrator( tf::TransformListener* tf ) { tf_ = tf; scan_sub_ = nh_.subscribe("/scan", 10, &LidarMotionCalibrator::ScanCallBack, this); points_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/cloud", 10); } ~LidarMotionCalibrator() { if(tf_!=NULL) delete tf_; } // 处理原始数据的函数 void ScanCallBack(const sensor_msgs::LaserScanPtr &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<double> angles,ranges; double lidar_angle = laserScanMsg.angle_max; for( int i = beamNum - 1; i > 0; i-- ) { double lidar_dist = laserScanMsg.ranges[i]; // 计算每个距离值对应的角度信息 lidar_angle -= laserScanMsg.angle_increment ; 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); } Lidar_Calibration(ranges, angles, startTime, endTime, tf_); //转换为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()); //转换为 pointcloud sensor_msgs::PointCloud cloud; cloud.header.frame_id = "odom"; cloud.header.stamp = ros::Time::now(); cloud.points.resize( ranges.size() ); 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]); geometry_msgs::Point32 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 = 0.1; cloud.points.push_back(pt); } points_pub_.publish(cloud); } /** * @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_footprint"; robot_pose.stamp_ = dt; //设置为ros::Time()表示返回最近的转换关系 // get the global pose of the robot try { if ( !tf_->waitForTransform("/odom", "/base_footprint", 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: %sn", ex.what()); return false; } catch (tf::ConnectivityException& ex) { ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %sn", ex.what()); return false; } catch (tf::ExtrapolationException& ex) { ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %sn", 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) { //每个位姿进行线性插值时的步长 double beam_step = 1.0 / (beam_number - 1); //机器人的起始角度 和 最终角度 tf::Quaternion start_angle_q = frame_start_pose.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(frame_base_pose.getRotation()); //机器人的起始位姿 tf::Vector3 start_pos = frame_start_pose.getOrigin(); start_pos.setZ(0); //最终位姿 tf::Vector3 end_pos = frame_end_pose.getOrigin(); end_pos.setZ(0); //基础坐标系 tf::Vector3 base_pos = frame_base_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)); //线性插值 mid_pos = start_pos.lerp(end_pos, beam_step * i); //得到激光点在odom 坐标系中的坐标 根据 double tmp_angle; //如果激光雷达不等于无穷,则需要进行矫正. 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; } } } //激光雷达数据 分段线性进行插值 分段的周期为10ms //这里会调用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_; ros::Publisher points_pub_; }; 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; }
