提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
前言
这是视觉工坊的苏赟老师的LVI-SAM下的课程作业讲解记录,方便以后复习(如果会复习的话哈哈哈)。仅供学习交流,侵删。作业是老师改的代码框架然后挖空的形式,挺用心的,有需要作业说明和代码包的评论下我看到了会私发bdu云链接,就不在这里发出来了,感觉不太好。
一、作业要求
基于所给的 SLAM 代码框架,实现利用里程计进行激光点云运动畸变去除的子模块,改善建图效果。只需要完善 scanRegistration.cpp 文件中的 RemoveLidarDistortion 函数,在start 与 end 之间添加代码,完成点云运动畸变去除函数,利用里程计结果对每个点进行校正。
二、求解思路
看到输入有:cloud_in,cloud_out,dqlc,dtlc,开辟了堆区的空间用引用的方式使得输入cloud_in的点云进行畸变矫正并输出cloud_out。dqlc是四元数形式,dtlc是旋转矩阵形式。后面的调用中,可以看到实际的输入是laserCloudFilter, laserCloudUndis, dq_odom, dt_odom, 找到dq_odom, dt_odom对应的定义:
dq_odom = q_odom_curr.conjugate() * q_odom_last;
dt_odom = q_odom_curr.conjugate() * (t_odom_last - t_odom_curr);
可以发现增量dq_odom是q_odom_curr.conjugate()即运动当前时刻的逆乘以上一时刻,以终点为参考点,计算与上一时刻的运动变换,即qwj->qjw×qwi=qji,j是当前时刻,i是上一时刻,所以是向前指的,这个相对关系很重要,即目标是将当前激光点云转到扫描终点坐标系下,扫描终点即当前扫描时刻作为原点,这里是作者怕我们弄错贴心的写出来了,不然是要写在去畸变里面的。
void RemoveLidarDistortion(const pcl::PointCloud<PointType>::Ptr &cloud_in, pcl::PointCloud<PointType>::Ptr &cloud_out, const Eigen::Quaterniond &dqlc, const Eigen::Vector3d &dtlc)
{
// 作业:
// TODO: 完成点云运动畸变去除函数,利用里程计结果对每个点进行校正;
// start:
*cloud_out = *cloud_in;
// end.
}
//后面的调用函数
RemoveLidarDistortion(laserCloudFilter, laserCloudUndis, dq_odom, dt_odom);
有了当前的点云和运动之后,开始去畸变:要计算当前激光点云中每一个点相对于扫描终点的时间比例,也就是雷达扫描的角度比例。注意这里的激光雷达扫描是顺时针旋转的,而用的坐标系是右手定则,yaw角是逆时针旋转的,所以角度要加负号;第一个点对应起始角度。
最终代码如下:核心思想就是知道起始、终止时刻,并用当前扫描时间求比例插值。
注意点:
1、注意旋转四元数的方向。
2、注意防止起始时刻越过终止时刻等此类的操作。
void RemoveLidarDistortion(const pcl::PointCloud<PointType>::Ptr &cloud_in, pcl::PointCloud<PointType>::Ptr &cloud_out, const Eigen::Quaterniond &dqlc, const Eigen::Vector3d &dtlc)
{
// 作业:
// TODO: 完成点云运动畸变去除函数,利用里程计结果对每个点进行校正;
// start:
int cloudSize = cloud_in->size();
float startOri = -atan2(cloud_in->points[0].y,cloud_in->points[0].x);
float endOri = -atan2(cloud_in->points[cloudSize-1].y,cloud_in->points[cloud_in->points-1].x);
if(endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
bool halfPassed = false;
PointType p_in , p_out;
Eigen::Vector3d startP , endP;
size_t j = 0;//typedef unsigned long long size_t
//开始遍历咯
for (int i=0 ; i < cloudSize ; i++)
{
p_in = cloud_in->point[i];
float ori = -atan2(p_in.y , p_in.x);
//也是防止过头了这一系列奇奇怪怪的东西
if (!halfPassed)
{
if (ori < startOri - M_PI/2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
(
ori -= 2 * M_PI;
)
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
if (ori > endOri)
{
ori -= 2 * M_PI;
}
}
//算比例啦
float ratio = (ori - startOri) / (endOri - startOri);
//因为是换到终点,所以要1-
float s = 1.0 - ratio;
//Eigen::Quaterniond::Identity().slerp(t, q_last_curr) 能够实现四元数球面插值。
//t ∈ [ 0 , 1 ] t \in [0,1]t∈[0,1]为插值点。q_last_curr为两帧之间的旋转四元数,即在两帧之间的旋转线性插入旋转四元数。
Eigen::Quaterniond delta_qlc = Eigen::Quaterniond::Identity().slerp(s,dqlc).normalized();
const Eigen::Vector3d delta_Plc = s * dtlc;//平移直接乘以比例就好
endP = delta_qlc * Eigen::Vector3d(p_in.x,p_in.y,p_in.z) + delta_Plc;
p_out = p_in;
p_out.x = endP.x();
p_out.y = endP.y();
p_out.z = endP.z();
j++;
cloud_out->push_back(p_out);
// end.
}