激光SLAM学习(2)——运动畸变的去除
目前在做一些自动驾驶相关的工作，处理部分包含了激光雷达数据的畸变去除部分内容，一直是大概知道原理，具体实现过程不是很清楚，最近又复习了深蓝学院的激光slam部分的内容，感觉收获很多，跟大家分享一下～
激光雷达去畸变问题的引出
运动过程中，激光雷达的点云信息是畸变的，通过使用底盘数据对激光雷达的数据进行补偿，去除激光雷达的畸变。
如果不使用里程计来补偿激光雷达数据，则可以使用icp或者vicp进行运动估计的去除，本质是也是进行一个匹配，迭代求解最佳的R,t, 但是存在的问题在于对于低帧率的激光数据，匀速运动假设并不成立，同时数据的预处理和状态估计过程是耦合的，在此先只分析借助odom来对激光雷达数据进行去畸变，ICP和VICP，下次可以简单分析。以下主要参考深蓝学院激光slam的课件，代码是分析的课程的作业的代码部分
基本处理步骤
已知数据
- 当前帧激光的起始时间ts，te
- 两个激光束之间的时间δt
- 里程计数据按照时间顺序存储在一个队列中，队首的时间最早
- 最早的里程计数据的时间戳＜ts
- 最早的里程计数据的时间戳＞te
求解目标
- 求解当前帧激光数据中每一个激光点对应的机器人位姿，即求解（ts，ts+δt，…，te）时刻的机器人位姿
- 根据求解的位姿把激光点转到同一坐标系下
- 重新封装一帧激光数据，发布出去
代码包含一以下几个部分：
//对原始激光数据进行处理
void ScanCallback(const champion_nav_msgs::ChampionNavLaserScanPtr& scan_msg);
/**
* @name getLaserPose()
* @brief 得到机器人在里程计坐标系中的位姿tf::Pose
* 得到dt时刻激光雷达在odom坐标系的位姿
* @param odom_pos 机器人的位姿
* @param dt dt时刻
* @param tf_
*/
bool getLaserPose(tf::Stamped &odom_pose,
ros::Time dt,
tf::TransforListener* tf_);
/**
* @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 frame_base_pose,
tf::Stamped frame_start_pose,
tf::Stamped frame_end_pose,
std::vector& ranges,
std::vector& angles,
int startIndex,
int& beam_number)
//激光雷达数据 分段线性进行插值 分段的周期为10ms
//这里会调用Lidar_MotionCalibration()
/**
* @name Lidar_Calibration()
* @brief 激光雷达数据 分段线性进行差值 分段的周期为5ms
* @param ranges 激光束的距离值集合
* @param angle 激光束的角度值集合
* @param startTime 第一束激光的时间戳
* @param endTime 最后一束激光的时间戳
* @param *tf_
*/
void Lidar_Calibration(std::vector& ranges,
std::vector& angles,
ros::Time startTime,
ros::Time endTime,
tf::TransformListener * tf_)
代码实现过程
通过在回调函数里获取原始数据，并调用Lidar_Calibration函数对lidar数据进行校准。
重点就是Lidar_Calibration函数的校准过程了，具体内容如下:
//激光雷达数据 分段线性进行插值 分段的周期为10ms
//这里会调用Lidar_MotionCalibration()
/**
* @name Lidar_Calibration()
* @brief 激光雷达数据 分段线性进行差值 分段的周期为5ms
* @param ranges 激光束的距离值集合
* @param angle 激光束的角度值集合
* @param startTime 第一束激光的时间戳
* @param endTime 最后一束激光的时间戳
* @param *tf_
*/
void Lidar_Calibration(std::vector& ranges,
std::vector& 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 frame_start_pose;
tf::Stamped frame_mid_pose;
tf::Stamped frame_base_pose;
tf::Stamped 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;
}
}
}
对每一帧所有的激光束数据进行去畸变处理
/**
* @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 frame_base_pose,
tf::Stamped frame_start_pose,
tf::Stamped frame_end_pose,
std::vector& ranges,
std::vector& angles,
int startIndex,
int& beam_number)
{
//TODO
//每个位姿进行线性插值时的步长
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;
}
//end of TODO
}
}
等同于要使里程计数据与激光数据同步，找到当前帧激光数据的临近的两个里程计数据，插值方式包括线性插值和二次插值得到当前时刻激光对应的里程计的估计值。