從零搭建無人車(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; }
