前言
什么是激光雷达的运动畸变 ?
激光雷达的一帧数据是过去一周期内形成的所有数据,数据仅有一时间戳,而非某个时刻的数据,因此在这一帧时间内的激光雷达或者其载体通常会发生运动,因此,这一帧的原点不一致,会导致一些问题,这个问题就是运动畸变
所以需要去运动畸变,也叫畸变校正
如何进行运动补偿?
运动补偿的目的就是把所有的点云补偿到某一时刻,这样就可以把本身在过去100ms内收集的点云统一到一个时间点上去 这个时间点可以是起始时刻,也可以是结束时刻,也可以是中间的任意时刻
常见的是补偿到起始时刻
Pstart = T_start_current * Pcurrent
畸变校准方法
因此运动补偿需要知道每个点时刻对应的位姿 T_start_current 通常有几种做法
1 如果有高频里程计,可以比较方便的获取每个点相对起始扫描时刻的位姿
2 如果有imu,可以方便的求出每个点相对起始点的旋转
3 如果没有其它传感器,可以使用匀速模型假设,使用上一帧间里程计的结果,作为当前两帧之间的运动,同时假设当前帧也是匀速运动,也可以估计出每个点相对起始时刻的位姿
k-1 到 k 帧 和 k到k+1帧的运动是一至的,用k-1到k帧的位姿变换当做k到k+1帧的位姿变换, 可以求到k到k+1帧的每个点的位姿变换
ALOAM是使用的纯lidar的方式,所以使用的是第3种方式
Code
将帧中间的点转到起始点坐标系下
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{功能函数的名字 : TransformToStart
形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址
调用的的时候按下面这种方式用就可以了
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//interpolation ratio
double s;
//由于kitti数据集上的lidar已经做过了运动补偿,因此这里就不做具体补偿了
if (DISTORTION)
// intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//求出了点占的时间比率
else
s = 1.0; //s = 1 说明全部补偿到点云结束的时刻s代表要转换的点在根据时间在这一帧里占的比率
SCAN_PERIOD是一帧的时间,10hz的lidar,那么周期就是0.1s
在前面的点的预处理时将 intensity 附了别的值 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
intensity的整体减去实数部分,就是时间差,那么除以周期,也就是时间占比了
有的lidar在内部做了去畸变,那么可以设置DISTORTION为1
则s为1,那么按照上面的定义,就是把全部补偿到点云结束的时刻
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 这里相当于是一个匀速模型的假设
// 把位姿变换分解成平移和旋转
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;做一个匀速模型假设,即上一帧的位姿变换,就是这帧的位姿变换
以此来求出输入点坐标系到起始时刻点坐标系的位姿变换,通过上面求的时间占比s
这里把位姿变换 分解成了 旋转 + 平移 的方式
由于四元数是一个超复数,不是简单的乘法,求它的占比用的 Eigen的slerp函数(球面线性插值)
线性插值和球面线性插值的对比,如上图
两个单位四元数之间进行插值，如左图的线性插值，得到的四元数一定不是单位四元数，我们期望对于旋转的插值应该是不改变长度的，所以显然右图球面（Slerp）插值更为合理。
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Eigen::Vector3d point(pi->x, pi->y, pi->z);//把当前点的坐标取出
Eigen::Vector3d un_point = q_point_last * point + t_point_last;//通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标上面有了旋转和平移,下面就简单了
把当前点的坐标取出
通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//将求得的转换后的坐标赋值给输出点
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;//赋值原的intensity
}将求得的转换后的坐标赋值给输出点
赋值原的intensity
将帧中间的点转到起始点坐标系下
这个是通过反变换的思想,首先把点统一到起始时刻坐标系下,再通过反变换,得到结束时刻坐标系下的点
void TransformToEnd(PointType const *const pi, PointType *const po)
{功能函数的名字 : TransformToEnd
形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址
调用的的时候按下面这种方式用就可以了
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 首先先做畸变校正,都转到起始时刻
pcl::PointXYZI un_point_tmp;//转到帧起始时刻坐标系下的点
//先统一到起始时刻
TransformToStart(pi, &un_point_tmp);首先先做畸变校正,都转到起始时刻
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点
//q_last_curr \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标q_last_curr \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移
代码的公式可以推导下:
两边乘 Re-s的逆
合并下
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//将求得的转换后的坐标赋值给输出点
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//移除掉 intensity 相对时间的信息
po->intensity = int(pi->intensity);
}将求得的转换后的坐标赋值给输出点
移除掉 intensity 相对时间的信息
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
整体代码:
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s;
//由于kitti数据集上的lidar已经做过了运动补偿,因此这里就不做具体补偿了
if (DISTORTION)
// intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//求出了点占的时间比率
else
s = 1.0; //s = 1 说明全部补偿到点云结束的时刻
//s = 1;
//所有点的操作方式都是一致的,相当于从结束时刻补偿到起始时刻
// 这里相当于是一个匀速模型的假设
// 把位姿变换分解成平移和旋转
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//求姿态的球面插值
Eigen::Vector3d t_point_last = s * t_last_curr;//求位移的线性插值
Eigen::Vector3d point(pi->x, pi->y, pi->z);//把当前点的坐标取出
Eigen::Vector3d un_point = q_point_last * point + t_point_last;//通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标
//将求得的转换后的坐标赋值给输出点
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;//赋值原的intensity
}
// transform all lidar points to the start of the next frame
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// 首先先做畸变校正,都转到起始时刻
pcl::PointXYZI un_point_tmp;//转到帧起始时刻坐标系下的点
//先统一到起始时刻
TransformToStart(pi, &un_point_tmp);
//再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点
//q_last_curr \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标
//将求得的转换后的坐标赋值给输出点
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//移除掉 intensity 相对时间的信息
po->intensity = int(pi->intensity);
}以上是 激光雷达的运动畸变以及补偿方式 的代码解析
