1、激光雷达运动畸变的原因：
绿色箭头表示激光雷达旋转方向，黑色箭头表示激光雷达运动方向。显然，我们可以看出每个激光束的距离数据都是在不同位姿下采集而来，如橙色、灰色、蓝色点位姿所示；然而，一般激光雷达驱动封装数据时，却默认一帧激光雷达数据的所有激光束是在同一位姿下、瞬时获得的，也就是所有激光束都是从橙色点获得的数据，这样实际使用的数据和真实数据有明显差距
2、畸变去除原理：
原理：在一帧激光雷达数据中，为每个激光束的原点都找到一个近似的里程计位姿，并且认为该激光束是在该里程计位姿上进行数据采集的，得到这些基础数据之后，进行系列的坐标变换，把所有激光点的数据都变换到第一束激光原点的坐标系下，然后再封装数据，这样就能极大的减小畸变。
3.1、畸变去除步骤：
a.读取.bag文件中激光雷达数据，同时上传里程计数据，进行时间校准。
b.在里程计坐标系下进行线性近似和线性差值，找到激光点近似的在里程计坐标下的位姿和里程计坐标系与激光雷达坐标系间角度。
c.将所有激光束从近似的激光雷达坐标系变换到里程计坐标系上（激光雷达坐标系—>里程计坐标系）；
d.最后将所有激光束从里程计坐标系变换到基准坐标系上、封装数据（里程计坐标系—>基准坐标系）。
3.1、时间同步、数据对齐
若一帧激光数据的t_s、t_e（开始、结束）时刻在缓存的里程计数据里没有准确对应的位姿，则需要使用 waitForTransform() 函数来等待 tf 的坐标转换线程得到你想要的时间点的坐标转换数据，代码中实现方式如下：
tf_->waitForTransform(“odom”, “laser_link”, dt, ros::Duration(0.5))
ros::Duration(0.5) ： 为 waitForTransform() 函数的结束条件：最多等待 0.5 秒，如果提前得到了坐标的转换信息，直接结束等待。3.2、分段线性近似、线性插值
这里先假设机器人在一帧激光数据采集过程中是匀加速、匀速、匀减速运动中一种或者多种混合都行。机器人的位姿是关于时间t的分段函数，如下图所示；这里只给出了机器人匀减速运动的解释，其他运动方式都是一样的处理方式。
3.3 、坐标转换流程
激光雷达坐标系—>轮式里程计坐标系—>基准坐标系
在上面的插值完成后，在近似位姿的激光雷达坐标系下将每个激光点变换到里程计坐标系下，再将每个激光点的数据从里程计坐标系下变换到基准坐标系下（绿色A坐标系）。
这里是将分段①（一帧数据的第一个分段）进行线性插值和坐标变换的过程放大化了，方便理解，如上图所示。
odom蓝色坐标系：机器人里程计坐标系
绿色坐标系：运动过程中的激光雷达坐标系，若干。
A点绿色坐标系：采集第一束激光数据时刻的激光雷达坐标系，这里也当做基准坐标系。
B点绿色坐标系：该分段中采集某一个激光束BD时刻的激光雷达坐标系。
A、C两点：近似线性分段的起始和结束点，A、C坐标系和坐标是从tf树获得，分别是激光雷达坐标系、以及坐标系的里程计位姿。
AC橙色虚直线：对AC弧线运动过程的线性近似，这里是放大后的效果图。
D点：激光束BD的终点。
B点蓝色坐标系：和里程计坐标方向一致、相互平行的坐标系。
激光雷达驱动实际采集到的数据：如B点绿色坐标系下的激光束BD的数据（距离和角度），该数据存储在/scan中，但存储数据时认为该数据是在A点绿色坐标系下采集的，所以存在运动畸变。
很明显，我们要将D点从B点绿色坐标系变换到A点绿色坐标系，如从黑色BD到绿色AD，此时我们就获得了畸变情况下在基准坐标下的D点的数据。于是就完成了一个激光点的去运动畸变过程，其他同理。
这里实现的C++代码流程是：
（1）把该插值B对应的激光点D的距离和角度，在近似的激光雷达坐标系（如绿色B坐标系）的XY轴计算出分量；
（2）通过旋转矩阵，将绿色B坐标系下的激光点D旋转到蓝色B坐标系，在通过平移将蓝色B坐标系下的激光点D平移到里程计坐标系下，经过计算得出激光点D在里程计坐标系下的位姿；
（3）同样，通过旋转+平移，将里程计坐标系下的D点变换到基准坐标系（绿色A坐标系）下，这样就得到激光点D在绿色A坐标系下的XY轴分量。
（4）根据XY轴分量根据sqrt(dx2+dy2)算出激光点距离，atan2(dy,dx)算出角度，对于所有激光束都是按照上述步骤处理。
4、 坐标变换涉及的数学知识
从B点绿色坐标系将激光点D变换到odom坐标系；
算法步骤：
第一步、计算本分段初始位姿和最终位姿在基准坐标系下的位姿。
第二步、获取第一步两个位姿的角度和位移
第三步、对当前帧每个点的角度和位移分别用slerp和lerp进行插值。
第四步、将插值结果转换到基准坐标系下并输出
#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/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#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();
cout<<"beamNum:"<<beamNum<<endl;//363数据
//laserScanMsg.time_increment：每两个激光时间间隔
endTime = startTime + ros::Duration(laserScanMsg.time_increment * (beamNum - 1));
cout<<"laserScanMsg.time_increment :"<<laserScanMsg.time_increment <<endl;
// 将数据复制出来
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;
/*这个getLaserPose（）函数作用是：让激光雷达的startTime和里程计的时间对齐，
通过TF变换得到在里程计下的位姿点*/
if(!getLaserPose(visualPose, startTime, tf_))
{
ROS_WARN("Not visualPose,Can not Calib");
return ;
}
//visualPose存储转换成里程计的位姿
//getYaw航偏角yaw
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;
//angles是激光雷达坐标系下x/y方向上的值
double x = ranges[i] * cos(angles[i]);
double y = ranges[i] * sin(angles[i]);
pcl::PointXYZRGB pt;// pt.x/ pt.y odom坐标系基点坐标系
/*visualYaw-激光雷达旋转到里程计的角度；
visualPose-某一激光点所在的里程计坐标；pt表示在第一个点下的里程计
详解2D激光雷达运动畸变去除-白茶>清欢*/
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()表示返回最近的转换关系
cout<<"odom_pose1："<<odom_pose.getOrigin().getX()<<endl;
// get the global pose of the robot
try
{
//让激光雷达和里程计的起始点对齐，这个函数书堵塞函数，延迟0.5s
if(!tf_->waitForTransform("/odom", "/base_laser", dt, ros::Duration(0.5))) // 0.15s 的时间可以修改
{
ROS_ERROR("LidarMotion-Can not Wait Transform()");
return false;
}
cout<<"odom_pose2："<<odom_pose.getOrigin().getX()<<endl;
//通过TF播发的关系和订阅TF关系获得激光雷达到里程计的转换关系
tf_->transformPose("/odom", robot_pose, odom_pose);
cout<<"robot_pose："<<robot_pose.getOrigin().getX()<<endl;
cout<<"odom_pose3："<<odom_pose.getOrigin().getX()<<endl;
}
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)
{
//该分段中，在里程计坐标系下，laser_link位姿的起始角度和结束角度，四元数表示
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));
//得到第i个激光束的角度插值，线性插值需要步长、起始和结束数据,与该激光点坐标系和里程计坐标系的夹角
tf::Quaternion mid_q = start_q.slerp(end_q, (i - startIndex) / (beam_number - 1));
tf::Transform mid_frame(mid_q, mid_xy);
//cout<<mid_frame.getRotation()<<endl;
//计算激光雷达距离在激光雷达坐标系下的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]);
}
}
//激光雷达数据 分段线性进行插值
//这里会调用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_)
{
cout<<"endTime1:"<<endTime<<endl;
cout<<"startTime1:"<<startTime<<endl;
//统计激光束的数量
int beamNumber = ranges.size();
cout<<"beamNumber2:"<<beamNumber<<endl;
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 - 1); // 每束激光数据的时间间隔
//当前插值的段的起始坐标
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 ;
}
cout<<"frame_start_pose"<<frame_start_pose.getOrigin().getX();
if(!getLaserPose(frame_end_pose,ros::Time(end_time / 1000000.0),tf_))
{
ROS_WARN("Not End Pose, Can not Calib");
return ;
}
cout<<"frame_end_pose"<<frame_end_pose.getOrigin().getX();
int cnt = 0;
//基准坐标就是第一个位姿的坐标
frame_base_pose = frame_start_pose;
for(int i = 0; i < beamNumber; i++)
{
//分段线性,时间段的大小为interpolation_time_duration,也就是说5ms分段一次
//time_inc-每个激光束时间间隔，mid_time依次向前移动一个激光束的时间
double mid_time = start_time + time_inc * (i - start_index);
cout<<"time_inc"<<time_inc<<endl;
//从i=1往后才能进入循环
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));//次/10s自动订阅TF中的消息，然后调用回调函数。
LidarMotionCalibrator tmpLidarMotionCalib(&tf);
ros::spin();
return 0;
}效果图：
文章参考：(2条消息) 详解2D激光雷达运动畸变去除_「小白学移动机器人」一个专注分享移动机器人相关知识的公众号！-CSDN博客
https://blog.csdn.net/zhao_ke_xue/article/details/105734833作者的文章写得具体，把激光雷达畸变纠正的算法过程描述很详细，但是最终的效果有待完善；本文是结合该博客后整理，最终的效果看着还不错，哈啊哈
扫一扫
339
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
