LOAM源代码分析附公式推导之LaserOdometry
LOAM源代码分析附公式推导之LaserOdometry
- 更新日志
- 6. 之LaserOdometry
-
- 6.1 参数配置
- 6.2 话题订阅
- 6.3 IMU数据处理
- 6.4 点云数据处理
-
- 6.4.1 点云数据
- 6.4.2 公式推导
-
- 6.4.2.1 帧间运动
- 6.4.2.2 GN非线性优化
- 6.4.2.3 累计运动
- 6.4.2.4 IMU信息修正
- 6.4.3 坐标变换
- 6.4.4 迭代优化
-
- 6.4.4.1 通用设置
- 6.4.4.2 边缘点处理
- 6.4.4.3 平面点处理
- 6.4.5 求解运动
- 6.4.6 运动累计与优化
- 6.4.7 结果发布
- 更新日志
更新日志
同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。
- 2020-12-07
整理完成LaserOdometry节点内容
最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！ - 2020-10-11
整理完成MultiScanRegistration节点内容，地址请转到LOAM源代码分析附公式推导之MultiScanRegistration - 2020-10-09
第一次编辑，初版
6. 之LaserOdometry
6.1 参数配置
默认设置如下
- 每次激光扫描周期 scanPeriod = 0.1s
- 输入与输出激光帧的比值 ioRatio = 2
- 最大迭代次数 maxIterations = 25
explicit LaserOdometry(float scanPeriod = 0.1, uint16_t ioRatio = 2, size_t maxIterations = 25);
还有其他可配置参数及默认值
- 迭代优化时平移量阈值 deltaTAbort = 0.1
- 迭代优化时旋转量阈值 deltaRAbort = 0.1
6.2 话题订阅
发布的话题如下，平面点、边缘点、全部点云以及里程计信息
// advertise laser odometry topics
_pubLaserCloudCornerLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
_pubLaserCloudSurfLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
_pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
_pubLaserOdometry = node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);
对于发布的里程计信息/laser_odom_to_init，坐标系定义如下
LaserOdometry::LaserOdometry(float scanPeriod, uint16_t ioRatio, size_t maxIterations):
BasicLaserOdometry(scanPeriod, maxIterations),
_ioRatio(ioRatio)
{
// initialize odometry and odometry tf messages
_laserOdometryMsg.header.frame_id = "/camera_init";
_laserOdometryMsg.child_frame_id = "/laser_odom";
_laserOdometryTrans.frame_id_ = "/camera_init";
_laserOdometryTrans.child_frame_id_ = "/laser_odom";
}
订阅的话题，主要是MultiScanRegistration中发布的数据。
// subscribe to scan registration topics
_subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);
_subCornerPointsLessSharp = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this);
_subSurfPointsFlat = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this);
_subSurfPointsLessFlat = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this);
_subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this);
_subImuTrans = node.subscribe<sensor_msgs::PointCloud2>
("/imu_trans", 5, &LaserOdometry::imuTransHandler, this);
6.3 IMU数据处理
首先是IMU数据的回调函数，转换数据格式，使用了pcl::PointXYZ，与前面一致。
void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg)
{
_timeImuTrans = imuTransMsg->header.stamp;
pcl::PointCloud<pcl::PointXYZ> imuTrans;
pcl::fromROSMsg(*imuTransMsg, imuTrans);
updateIMU(imuTrans);
_newImuTrans = true;
}
进一步更新IMU数据信息，参考之前的分析5.4.4 打包IMU数据，有
获取点云中
第一个点时对应的激光姿态角_imuStart，获取点云中最后一个点时对应的激光姿态角_imuCur
获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动位置偏移开始点对应的IMU局部坐标系imuShiftFromStart
获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动速度偏移开始点对应的IMU局部坐标系imuVelocityFromStart
void BasicLaserOdometry::updateIMU(pcl::PointCloud<pcl::PointXYZ> const& imuTrans)
{
assert(4 == imuTrans.size());
_imuPitchStart = imuTrans.points[0].x;
_imuYawStart = imuTrans.points[0].y;
_imuRollStart = imuTrans.points[0].z;
_imuPitchEnd = imuTrans.points[1].x;
_imuYawEnd = imuTrans.points[1].y;
_imuRollEnd = imuTrans.points[1].z;
_imuShiftFromStart = imuTrans.points[2];
_imuVeloFromStart = imuTrans.points[3];
}
这样就OK了，接下来就是考虑如何使用它了，在6.4 点云数据处理中再详细解释吧。
6.4 点云数据处理
6.4.1 点云数据
对于不同话题下的点云数据处理大同小异，这里只简单说一个就行
- 1.获取点云数据的时间
_timeSurfPointsFlat - 2.转换点云格式
fromROSMsg - 3.去除无效点
removeNaNFromPointCloud - 4.设置标志位
_newSurfPointsFlat
void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg)
{
_timeSurfPointsFlat = surfPointsFlatMsg->header.stamp;
surfPointsFlat()->clear();
pcl::fromROSMsg(*surfPointsFlatMsg, *surfPointsFlat());
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*surfPointsFlat(), *surfPointsFlat(), indices);
_newSurfPointsFlat = true;
}
得到的点云数据分别为
pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsSharp; ///< sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsFlat; ///< flat surface points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsLessFlat; ///< less flat surface points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloud; ///< full resolution cloud
每次对点云数据处理时需要保证同时接受到最新的数据
bool LaserOdometry::hasNewData()
{
return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat &&
_newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans &&
fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005;
}
对最新的点云数据进行处理BasicLaserOdometry::process()，如何处理的呢？我们下一个小节展开，这也是激光里程计最核心的部分。
void LaserOdometry::process()
{
if (!hasNewData())
return;// waiting for new data to arrive...
reset();// reset flags, etc.
BasicLaserOdometry::process();
publishResult();
}
6.4.2 公式推导
6.4.2.1 帧间运动
使用3.3 坐标变换的定义，继续推导。
对于一帧点云数据，有第一个点 p s p_s ps和最后一个点 p e p_e pe，以及任意时刻的点 p t p_t pt，
首先将任意时刻 t t t的点重新投影到统一时刻 t s t_s ts得到对应的点 p ~ s t \widetilde{p}_{st} p st，因此有关系
p t = R s : t p ~ s t + T s : t p_t=R_{s:t}\widetilde{p}_{st}+T_{s:t} pt=Rs:tp st+Ts:t
进一步得到
p ~ s t = R s : t − 1 ( p t − T s : t ) \widetilde{p}_{st}=R_{s:t}^{-1}(p_t-T_{s:t}) p st=Rs:t−1(pt−Ts:t)
可以看到，我们需要求解的就是每个时刻 t t t对应的变换关系 R s : t R_{s:t} Rs:t和 T s : t T_{s:t} Ts:t，根据匀速运动假设，实际只需要求解第一个点和最后一个点的变换关系 R s : e R_{s:e} Rs:e和 T s : e T_{s:e} Ts:e，并且在迭代优化中使用的是矩阵 R s : t − 1 R_{s:t}^{-1} Rs:t−1，考虑将该矩阵元素使用欧拉角表示。
这里考虑清楚为什么使用 R s : t − 1 R_{s:t}^{-1} Rs:t−1矩阵，而不是 R s : t R_{s:t} Rs:t矩阵。
那么，根据坐标系定义，使用左乘、旋转坐标轴、先Z再X后Y得到旋转矩阵 R Z X Y R_{ZXY} RZXY，有
R s : t − 1 = R Z X Y = R r y R r x R r z = [ c y c z − s x s y s z c y s z + s x s y c z − s y c x − c x s z c x c z s x s y c z + s x c y s z s y s z − s x c y c z c y c x ] R_{s:t}^{-1}=R_{ZXY}=R_{ry}R_{rx}R_{rz}= \begin{bmatrix} cycz-sxsysz& cysz+sxsycz &-sycx \\ -cxsz & cxcz& sx \\ sycz+sxcysz & sysz-sxcycz & cycx \end{bmatrix} Rs:t−1=RZXY=RryRrxRrz=⎣⎡cycz−sxsysz−cxszsycz+sxcyszcysz+sxsyczcxczsysz−sxcycz−sycxsxcycx⎦⎤
那么，对于矩阵 R s : t R_{s:t} Rs:t，有
R s : t = ( R Z X Y ) − 1 = R − r z R − r x R − r y R_{s:t}=(R_{ZXY})^{-1}=R_{-rz}R_{-rx}R_{-ry} Rs:t=(RZXY)−1=R−rzR−rxR−ry
那么对于优化的目标可以简述为
寻找最优的
旋转矩阵R s : t − 1 R_{s:t}^{-1} Rs:t−1和平移量T s : t T_{s:t} Ts:t，使得所有点云特征点到特征区域的距离最小。
那么，对于边缘点、平面点均有下面等式
f ( p ~ s t ) = f ( R s : t − 1 ( p t − T s : t ) ) = f ( R s : t − 1 ， p t ， T s : t ) = f ( R s : t − 1 ， T s : t ) = d f(\widetilde{p}_{st}) =f(R_{s:t}^{-1}(p_t-T_{s:t}))=f(R_{s:t}^{-1}，p_t，T_{s:t})=f(R_{s:t}^{-1}，T_{s:t})= d f(p st)=f(Rs:t−1(pt−Ts:t))=f(Rs:t−1，pt，Ts:t)=f(Rs:t−1，Ts:t)=d
定义 R s : t − 1 R_{s:t}^{-1} Rs:t−1对应的欧拉角分别为 r x , r y , r z rx,ry,rz rx,ry,rz即是
R s : t − 1 = R Z X Y = R r y R r x R r z R_{s:t}^{-1}=R_{ZXY}=R_{ry}R_{rx}R_{rz} Rs:t−1=RZXY=RryRrxRrz
T s : t T_{s:t} Ts:t分量分别为 t x , t y , t z t_x,t_y,t_z tx,ty,tz，即是
T s : t = [ t x ; t y ; t z ] T_{s:t}=[t_x;t_y;t_z] Ts:t=[tx;ty;tz]
进一步有
∂ f ∂ ( r x , r y , r z , t x , t y , t z ) = [ ∂ f ∂ r x , ∂ f ∂ r y , ∂ f ∂ r z , ∂ f ∂ t x , ∂ f ∂ t y , ∂ f ∂ t z ] \frac{\partial{f}}{\partial{(rx,ry,rz,t_x,t_y,t_z)}}=[ \frac{\partial{f}}{\partial{rx}},\frac{\partial{f}}{\partial{ry}},\frac{\partial{f}}{\partial{rz}}, \frac{\partial{f}}{\partial{t_x}},\frac{\partial{f}}{\partial{t_y}},\frac{\partial{f}}{\partial{t_z}}] ∂(rx,ry,rz,tx,ty,tz)∂f=[∂rx∂f,∂ry∂f,∂rz∂f,∂tx∂f,∂ty∂f,∂tz∂f]
现在以 r x rx rx, t x t_x tx举例说明，其他分量 r y , r z , t y , t z ry,rz,t_y,t_z ry,rz,ty,tz等同理
∂ f ∂ r x = ∂ f ∂ p ~ s t ∂ p ~ s t r x = [ l a , l b , l c ] ∂ R s : t − 1 ∂ r x ( p t − T s : t ) = [ l a , l b , l c ] ∂ R s : t − 1 ∂ r x p t − [ l a , l b , l c ] ∂ R s : t − 1 ∂ r x T s : t \frac{\partial{f}}{\partial{rx}}= \frac{\partial{f}}{\partial{\widetilde{p}_{st}}} \frac{\partial{\widetilde{p}_{st}}}{rx}= [la,lb,lc] \frac{\partial{R_{s:t}^{-1}}}{\partial{rx}} (p_t-T_{s:t})= [la,lb,lc] \frac{\partial{R_{s:t}^{-1}}}{\partial{rx}} p_t-[la,lb,lc] \frac{\partial{R_{s:t}^{-1}}}{\partial{rx}}T_{s:t} ∂rx∂f=∂p st∂frx∂p st=[la,lb,lc]∂rx∂Rs:t−1(pt−Ts:t)=[la,lb,lc]∂rx∂Rs:t−1pt−[la,lb,lc]∂rx∂Rs:t−1Ts:t
∂ f ∂ t x = ∂ f ∂ p ~ s t ∂ p ~ s t t x = [ l a , l b , l c ] ∂ [ R s : t − 1 ( p t − T s : t ) ] ∂ t x = [ l a , l b , l c ] R s : t − 1 [ − 1 ; 0 ; 0 ] \frac{\partial{f}}{\partial{t_x}}= \frac{\partial{f}}{\partial{\widetilde{p}_{st}}} \frac{\partial{\widetilde{p}_{st}}}{t_x}= [la,lb,lc] \frac{\partial{[R_{s:t}^{-1}(p_t-T_{s:t})]}}{\partial{t_x}}= [la,lb,lc]R_{s:t}^{-1}[-1;0;0] ∂tx∂f=∂p st∂ftx∂p st=[la,lb,lc]∂tx∂[Rs:t−1(pt−Ts:t)]=[la,lb,lc]Rs:t−1[−1;0;0]
式中， ∂ f ∂ p ~ s t \frac{\partial{f}}{\partial{\widetilde{p}_{st}}} ∂p st∂f为梯度方向，对于点和直线，自然是垂直直线的方向，对于点和平面，自然是法向量方向。
式中， ∂ R s : t − 1 ∂ r x \frac{\partial{R_{s:t}^{-1}}}{\partial{rx}} ∂rx∂Rs:t−1就是矩阵对欧拉角的微分，推导如下
∂ R s : t − 1 ∂ r x = ∂ R Z X Y ∂ r x = ∂ [ c y c z − s x s y s z c y s z + s x s y c z − s y c x − c x s z c x c z s x s y c z + s x c y s z s y s z − s x c y c z c y c x ] ∂ r x = [ − c x s y s z c x s y c z s x s y s x s z − s x c z c x c x c y s z − c x c y c z − c y s x ] \frac{\partial{R_{s:t}^{-1}}}{\partial{rx}}= \frac{\partial{R_{ZXY}}}{\partial{rx}}= \frac{\partial{\begin{bmatrix} cycz-sxsysz& cysz+sxsycz &-sycx \\ -cxsz & cxcz& sx \\ sycz+sxcysz & sysz-sxcycz & cycx \end{bmatrix}}}{\partial{rx}}= \begin{bmatrix} -cxsysz& cxsycz &sxsy \\ sxsz & -sxcz& cx \\ cxcysz & -cxcycz & -cysx \end{bmatrix} ∂rx∂Rs:t−1=∂rx∂RZXY=∂rx∂⎣⎡cycz−sxsysz−cxszsycz+sxcyszcysz+sxsyczcxczsysz−sxcycz−sycxsxcycx⎦⎤=⎣⎡−cxsyszsxszcxcyszcxsycz−sxcz−cxcyczsxsycx−cysx⎦⎤
6.4.2.2 GN非线性优化
根据GN非线性优化有下面的等式，每次求解得到的是解增量，因此需要保证初值较为准确。
min x 1 2 ∣ ∣ F ( x ) ∣ ∣ 2 2 \min_{x}\frac{1}{2}||F(x)||^2_2 xmin21∣∣F(x)∣∣22
F ( x + Δ x ) ≈ F ( x ) + J ( x ) Δ x F(x+\Delta{x})\approx{F(x)+J(x)\Delta{x}} F(x+Δx)≈F(x)+J(x)Δx
J ( x ) T J ( x ) Δ x = − J ( x ) T F ( x ) J(x)^TJ(x)\Delta{x}=-J(x)^TF(x) J(x)TJ(x)Δx=−J(x)TF(x)
式中， J ( x ) J(x) J(x)为 F ( x ) F(x) F(x)关于 x x x的一阶导数。也就是 J = ∂ F ∂ ( r x , r y , r z , t x , t y , t z ) J=\frac{\partial{F}}{\partial{(rx,ry,rz,t_x,t_y,t_z)}} J=∂(rx,ry,rz,tx,ty,tz)∂F， F F F是由一系列 f f f组成的方程组
6.4.2.3 累计运动
根据之前的推导，实际求解的是矩阵 R s : t − 1 R_{s:t}^{-1} Rs:t−1中的值，在使用时需要添加负号、更改变换顺序才能得到最终的 R s : t R_{s:t} Rs:t。
我们最后使用的是 R s : t R_{s:t} Rs:t，也就是时刻不断增加时对应的变换关系，而不是 R s : t − 1 R_{s:t}^{-1} Rs:t−1
那么有累计运动_transformSum(对应 R r z R r x R r y R_{rz}R_{rx}R_{ry} RrzRrxRry)和帧间运动_transform(对应 R r y R r x R r z R_{ry}R_{rx}R_{rz} RryRrxRrz)，对于旋转部分，有
R s : t − 1 = R Z X Y = R r y R r x R r z R_{s:t}^{-1}=R_{ZXY}=R_{ry}R_{rx}R_{rz} Rs:t−1=RZXY=RryRrxRrz
R s : t = ( R Z X Y ) − 1 = R − r z R − r x R − r y R_{s:t}=(R_{ZXY})^{-1}=R_{-rz}R_{-rx}R_{-ry} Rs:t=(RZXY)−1=R−rzR−rxR−ry
R Y X Z = [ c y c z + s x s y s z c x s z − s y c z + s x c y s z − c y s z + s x s y c z c x c z s y s z + s x c y c z c x s y − s x c x c y ] R_{YXZ}= \begin{bmatrix} cycz+sxsysz& cxsz &-sycz+sxcysz \\ -cysz+sxsycz & cxcz& sysz+sxcycz \\ cxsy & -sx & cxcy \end{bmatrix} RYXZ=⎣⎡cycz+sxsysz−cysz+sxsyczcxsycxszcxcz−sx−sycz+sxcyszsysz+sxcyczcxcy⎦⎤
因此可以得到
R ′ = R t r a n s f o r m R t r a n s f o r m S u m = [ c y c z + s x s y s z c x s z − s y c z + s x c y s z − c y s z + s x s y c z c x c z s y s z + s x c y c z c x s y − s x c x c y ] [ c y c z + s x s y s z c x s z − s y c z + s x c y s z − c y s z + s x s y c z c x c z s y s z + s x c y c z c x s y − s x c x c y ] R'=R_{transform}R_{transformSum}= \begin{bmatrix} cycz+sxsysz& cxsz &-sycz+sxcysz \\ -cysz+sxsycz & cxcz& sysz+sxcycz \\ cxsy & -sx & cxcy \end{bmatrix} \begin{bmatrix} cycz+sxsysz& cxsz &-sycz+sxcysz \\ -cysz+sxsycz & cxcz& sysz+sxcycz \\ cxsy & -sx & cxcy \end{bmatrix} R′=RtransformRtransformSum=⎣⎡cycz+sxsysz−cysz+sxsyczcxsycxszcxcz−sx−sycz+sxcyszsysz+sxcyczcxcy⎦⎤⎣⎡cycz+sxsysz−cysz+sxsyczcxsycxszcxcz−sx−sycz+sxcyszsysz+sxcyczcxcy⎦⎤
可以得到更新后的累计旋转角度 r x , r y , r z rx,ry,rz rx,ry,rz，这里举例说明 r x rx rx
r x = − a r c s i n ( R 3 , 2 ′ ) = − a r c s i n ( c x s y ∗ c x s z − s x ∗ c x c z − c x c y ∗ s x ) rx = -arcsin(R'_{3,2})=-arcsin(cxsy*cxsz-sx*cxcz-cxcy*sx) rx=−arcsin(R3,2′)=−arcsin(cxsy∗cxsz−sx∗cxcz−cxcy∗sx)
r y = − a r c t a n ( R 3 , 1 ′ / R 3 , 3 ′ ) ry = -arctan(R'_{3,1}/R'_{3,3}) ry=−arctan(R3,1′/R3,3′)
r z = − a r c t a n ( R 1 , 2 ′ / R 2 , 2 ′ ) rz = -arctan(R'_{1,2}/R'_{2,2}) rz=−arctan(R1,2′/R2,2′)
6.4.2.4 IMU信息修正
pluginIMURotation使用的是IMU信息对估计的旋转进一步优化
目前已有信息
- rx, ry, rz存放使用
激光点云计算的累计的从0时刻到tend的旋转 w e R l ^{e}_{w}R_{l} weRl - imu测量的得到点云中
第一个点时旋转角度 w s R i ^{s}_{w}R_{i} wsRi - imu测量的得到点云中
最后一个点时旋转角度 w e R i ^{e}_{w}R_{i} weRi
因此有修正值
w e R l ′ = w e R i × i n v ( w s R i ) × w e R l ^{e}_{w}R'_{l}={^{e}_{w}R_{i}}×inv({^{s}_{w}R_{i}})×{^{e}_{w}R_{l}} weRl′=weRi×inv(wsRi)×weRl
w e R l = R Y X Z , w e R i = R Y X Z , i n v ( w s R i ) = R Z X Y {^{e}_{w}R_{l}}=R_{YXZ},{^{e}_{w}R_{i}}=R_{YXZ},inv({^{s}_{w}R_{i}})=R_{ZXY} weRl=RYXZ,weRi=RYXZ,inv(wsRi)=RZXY
w e R l ′ = w e R i ∗ i n v ( w s R i ) ∗ w e R l = [ c y c z + s x s y s z c x s z − s y c z + s x c y s z − c y s z + s x s y c z c x c z s y s z + s x c y c z c x s y − s x c x c y ] × [ c y c z + s x s y s z − c y s z + s x s y c z s y c x c x s z c x c z − s x − s y c z + s x c y s z s y s z + s x c y c z c y c x ] × [ c y c z + s x s y s z c x s z − s y c z + s x c y s z − c y s z + s x s y c z c x c z s y s z + s x c y c z c x s y − s x c x c y ] ^{e}_{w}R'_{l}={^{e}_{w}R_{i}}*inv({^{s}_{w}R_{i}})*{^{e}_{w}R_{l}}= \begin{bmatrix} cycz+sxsysz& cxsz &-sycz+sxcysz \\ -cysz+sxsycz & cxcz& sysz+sxcycz \\ cxsy & -sx & cxcy \end{bmatrix}× \begin{bmatrix} cycz+sxsysz& -cysz+sxsycz &sycx \\ cxsz & cxcz& -sx \\ -sycz+sxcysz & sysz+sxcycz & cycx \end{bmatrix}× \begin{bmatrix} cycz+sxsysz& cxsz &-sycz+sxcysz \\ -cysz+sxsycz & cxcz& sysz+sxcycz \\ cxsy & -sx & cxcy \end{bmatrix} weRl′=weRi∗inv(wsRi)∗weRl=⎣⎡cycz+sxsysz−cysz+sxsyczcxsycxszcxcz−sx−sycz+sxcyszsysz+sxcyczcxcy⎦⎤×⎣⎡cycz+sxsyszcxsz−sycz+sxcysz−cysz+sxsyczcxczsysz+sxcyczsycx−sxcycx⎦⎤×⎣⎡cycz+sxsysz−cysz+sxsyczcxsycxszcxcz−sx−sycz+sxcyszsysz+sxcyczcxcy⎦⎤
因此可以得到角度 r x , r y , r z rx,ry,rz rx,ry,rz，这里以 r x rx rx为例
r x = − a r c s i n ( − s x ) = − a r c s i n [ w e R l ′ ( 3 , 2 ) ] ≈ − a r c s i n ( [ — — — — — — c x s y × ( c y c z + s x s y s z ) − s x × ( c x s z ) + c x c y × ( − s y c z + s x c y s z ) c x s y × ( − c y s z + s x s y c z ) − s x × ( c x c z ) + c x c y × ( s y s z + s x c y c z ) c x s y × ( s y c x ) − s x × ( − s x ) + c x c y × ( c y c x ) ] × [ — c x s z — — c x c z — — − s x — ] ) rx = -arcsin(-sx)=-arcsin[^{e}_{w}R'_{l}(3,2)]\approx -arcsin(\begin{bmatrix} —& — & — \\ — & — & — \\ cxsy×(cycz+sxsysz)-sx×(cxsz)+cxcy×(-sycz+sxcysz) & cxsy×(-cysz+sxsycz)-sx×(cxcz)+cxcy×(sysz+sxcycz) & cxsy×(sycx)-sx×(-sx)+cxcy×(cycx) \end{bmatrix}× \begin{bmatrix} —& cxsz &— \\ — & cxcz& — \\ — & -sx & — \end{bmatrix}) rx=−arcsin(−sx)=−arcsin[weRl′(3,2)]≈−arcsin(⎣⎡——cxsy×(cycz+sxsysz)−sx×(cxsz)+cxcy×(−sycz+sxcysz)——cxsy×(−cysz+sxsycz)−sx×(cxcz)+cxcy×(sysz+sxcycz)——cxsy×(sycx)−sx×(−sx)+cxcy×(cycx)⎦⎤×⎣⎡———cxszcxcz−sx———⎦⎤)
可以自己对照着看一下公式，就是比较繁琐。
不到长城非好汉，唯览紫禁生无憾。@2020-12-07
6.4.3 坐标变换
这里对需要使用的坐标变换进行简单分析
注意两个变量
累计运动_transformSum(对应 R r z R r x R r y R_{rz}R_{rx}R_{ry} RrzRrxRry)和帧间运动_transform(对应 R r y R r x R r z R_{ry}R_{rx}R_{rz} RryRrxRrz)
- 累计运动
_transformSum(对应 R r z R r x R r y R_{rz}R_{rx}R_{ry} RrzRrxRry)在代码中体现为
rotateYXZ(-ry,-rx,-rz)
世界坐标系－－－－－－－－－－－－>>>>>激光坐标系
世界坐标系<<<<<－－－－－－－－－－－－激光坐标系
rotateZXY(rz,rx,ry)
- 帧间运动
_transform(对应 R r y R r x R r z R_{ry}R_{rx}R_{rz} RryRrxRrz)在代码中体现为
rotateYXZ(ry,rx,rz)
第一个点激光坐标系－－－－－－－－－－－－>>>>>最后一个点激光坐标系
第一个点激光坐标系<<<<<－－－－－－－－－－－－最后一个点激光坐标系
rotateZXY(-rz,-rx,-ry)
首先是transformToStart。
p ~ s t = R s : t − 1 ( p t − T s : t ) \widetilde{p}_{st}=R_{s:t}^{-1}(p_t-T_{s:t}) p st=Rs:t−1(pt−Ts:t)
//变换到 第一个点坐标系 inv(R)(Xt - T)= X
void BasicLaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po)
{
// 相对第一个点的时间偏移
float s = (1.f / _scanPeriod) * (pi.intensity - int(pi.intensity));
po.x = pi.x - s * _transform.pos.x();
po.y = pi.y - s * _transform.pos.y();
po.z = pi.z - s * _transform.pos.z();
po.intensity = pi.intensity;
Angle rx = -s * _transform.rot_x.rad();
Angle ry = -s * _transform.rot_y.rad();
Angle rz = -s * _transform.rot_z.rad();
rotateZXY(po, rz, rx, ry);
}
然后是transformToEnd
p t = R s : t p ~ s t + T s : t p_t=R_{s:t}\widetilde{p}_{st}+T_{s:t} pt=Rs:tp st+Ts:t
- 变换到
第一个点坐标系 inv(R )(Xt - T)= X - 变换到
最后一个点坐标系 Xt= RX + T - 对
非匀速运动进行恢复 - 使用
IMU测量信息补偿
//注意这里添加了之前没有考虑的非匀速运动信息_imuShiftFromStart
size_t BasicLaserOdometry::transformToEnd(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud)
{
size_t cloudSize = cloud->points.size();
for (size_t i = 0; i < cloudSize; i++)
{
pcl::PointXYZI& point = cloud->points[i];
float s = (1.f / _scanPeriod) * (point.intensity - int(point.intensity));
//变换到 第一个点坐标系 inv(R)(Xt - T)= X
point.x -= s * _transform.pos.x();
point.y -= s * _transform.pos.y();
point.z -= s * _transform.pos.z();
point.intensity = int(point.intensity);
Angle rx = -s * _transform.rot_x.rad();
Angle ry = -s * _transform.rot_y.rad();
Angle rz = -s * _transform.rot_z.rad();
rotateZXY(point, rz, rx, ry);
//变换到 最后一个点坐标系 Xt= RX + T
rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z);
//下面是对非匀速运动进行恢复。
point.x += _transform.pos.x() - _imuShiftFromStart.x();
point.y += _transform.pos.y() - _imuShiftFromStart.y();
point.z += _transform.pos.z() - _imuShiftFromStart.z();
// 下面是使用IMU测量信息补偿
rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);
rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);
}
return cloudSize;
}
6.4.4 迭代优化
6.4.4.1 通用设置
首先确保系统已经初始化
if (!_systemInited)
{
_cornerPointsLessSharp.swap(_lastCornerCloud);
_surfPointsLessFlat.swap(_lastSurfaceCloud);
_lastCornerKDTree.setInputCloud(_lastCornerCloud);
_lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
_transformSum.rot_x += _imuPitchStart;
_transformSum.rot_z += _imuRollStart;
_systemInited = true;
return;
}
根据之前的推导5.4.4 打包IMU数据，得到了点云数据中的速度偏移 _imuVeloFromStart，因此去除非匀速运动信息，得到估计运动的初值。
_frameCount++;
_transform.pos -= _imuVeloFromStart * _scanPeriod;
只有提取的边缘点和平面点数量足够多时才会进行运动估计
if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
{
//运动估计
}
根据之前的分析LOAM SLAM之论文原理解读，这里设置最近邻索引Vector和欧式距离Vector，并进行变量初始化。
- 边缘点需要
2个最近邻点 - 平面点需要
3个最近邻点
std::vector<int> pointSearchInd(1);
std::vector<float> pointSearchSqDis(1);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices);
size_t cornerPointsSharpNum = _cornerPointsSharp->points.size();
size_t surfPointsFlatNum = _surfPointsFlat->points.size();
_pointSearchCornerInd1.resize(cornerPointsSharpNum);
_pointSearchCornerInd2.resize(cornerPointsSharpNum);
_pointSearchSurfInd1.resize(surfPointsFlatNum);
_pointSearchSurfInd2.resize(surfPointsFlatNum);
_pointSearchSurfInd3.resize(surfPointsFlatNum);
运动估计时需要满足下面的任意条件
- 最大迭代次数为
_maxIterations
for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++)
{
// 迭代优化
}
- 运动估计值
收敛
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
pow(rad2deg(matX(1, 0)), 2) +
pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
pow(matX(4, 0) * 100, 2) +
pow(matX(5, 0) * 100, 2));
if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
break;
部分变量说明
pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudOri; // < point selection 运动估计时使用的所有特征点：包含平面点和边缘点
pcl::PointCloud<pcl::PointXYZI>::Ptr _coeffSel; // < point selection coefficients 运动估计时特征点对应的系数：包含平面点和边缘点
以及选取的特征点，有最近邻、次近邻等，在使用时详细说明吧
// pointSel当前特征点 tripod1，2,3最近邻点，次近邻点，三近邻点
pcl::PointXYZI pointSel, pointProj,tripod1,tripod2,tripod3;
6.4.4.2 边缘点处理
这里需要对每个边缘点寻找最近邻的点，然后求取点到直线的距离，并且为6.4.2.1 帧间运动方程组中的系数做准备，这个时候需要干一件事，那就是将点云数据进行坐标系变换，统一变换到第一个点对应的坐标系。
那么，就开始了！
首先变换点云到开始点坐标系，在转换中使用了_transform，因此每一次迭代后，得到的变换结果也是不一样的。
transformToStart(_cornerPointsSharp->points[i], pointSel);
每5次迭代重新提取特征点
- 使用
KD树 _lastCornerKDTree查找最近邻点索引pointSearchInd和对应的欧式距离pointSearchSqDis - 得到
最近邻点的Scan索引 即是激光发射束id closestPointScan - 通过对
点云索引的增加、减少，在相邻激光束查找次近邻点，注意空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据 - 得到当前特征点在激光束中的
最近邻点_pointSearchCornerInd1、次近邻点_pointSearchCornerInd2
// 每5次迭代重新提取特征点
if (iterCount % 5 == 0)
{
pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices);
// 使用KD树查找最近邻点以及对应的距离
_lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
// 距离需要满足最低的阈值要求
if (pointSearchSqDis[0] < 25)
{
closestPointInd = pointSearchInd[0];
// 得到最近邻点的Scan索引 即是激光发射束id
int closestPointScan = int(_lastCornerCloud->points[closestPointInd].intensity);
float pointSqDis, minPointSqDis2 = 25;
// 在相邻激光束查找次近邻点
// _lastCornerCloud中同一线束上的点索引相近
// 点云索引增加的线束中查找
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
{
// 空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据
if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5)
{
break;
}
// 计算两个三维点的距离，欧式距离
pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
// 得到相邻激光束中次近邻点，更新距离minPointSqDis2 坐标minPointInd2
if (int(_lastCornerCloud->points[j].intensity) > closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
// 点云索引增加的线束中查找
for (int j = closestPointInd - 1; j >= 0; j--)
{
// 空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据
if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5)
{
break;
}
// 计算两个三维点的距离，欧式距离
pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
// 得到相邻激光束中次近邻点，更新距离minPointSqDis2 坐标minPointInd2
if (int(_lastCornerCloud->points[j].intensity) < closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
}
// 得到当前特征点在激光束中的最近邻点_pointSearchCornerInd1、次近邻点_pointSearchCornerInd2
_pointSearchCornerInd1[i] = closestPointInd;
_pointSearchCornerInd2[i] = minPointInd2;
}
如果成功找到次近邻点索引，那么可以找到边缘线特征，有下面
叉乘的具体公式为
a=(a1,b1,c1)，向量b=(a2,b2,c2)，
则向量a×向量b=
| i j k |
|a1 b1 c1|
|a2 b2 c2|
=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)
对应代码里
i -- pointSel
j -- tripod1
l -- tripod2
// 成功找到次近邻点索引
if (_pointSearchCornerInd2[i] >= 0)
{
// 分别对应点 i , j
tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]];
tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]];
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = tripod1.x;
float y1 = tripod1.y;
float z1 = tripod1.z;
float x2 = tripod2.x;
float y2 = tripod2.y;
float z2 = tripod2.z;
// 注意向量的方向 z －y x
// ijl平行四边形的面积
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
// jl的长度
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// ijl平面法向量和jl方向向量叉乘，注意单位向量。
// 叉乘的ｘ分量 由点远离直线向外，d增大的方向
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
// 叉乘的y分量
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// 叉乘的z分量
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// 面积/高 = 点到线的距离
float ld2 = a012 / l12; // Eq. (2)
// TODO: Why writing to a variable that's never read?
pointProj = pointSel;
pointProj.x -= la * ld2;
pointProj.y -= lb * ld2;
pointProj.z -= lc * ld2;
float s = 1;
// 距离d越大，权重s越小
if (iterCount >= 5)
{
s = 1 - 1.8f * fabs(ld2);
}
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
//不考虑d为0，不考虑距离d过大的点
if (s > 0.1 && ld2 != 0)
{
_laserCloudOri->push_back(_cornerPointsSharp->points[i]);
_coeffSel->push_back(coeff);
}
}
6.4.4.3 平面点处理
这里需要对每个平面点寻找最近邻、次近邻(与最近邻位于同一线束)以及第三近邻的点，然后求取点到平面的距离，并且为6.4.2.1 帧间运动方程组中的系数做准备，这个时候需要干一件事，那就是将点云数据进行坐标系变换，统一变换到第一个点对应的坐标系。
那么，就开始了！
首先变换点云到开始点坐标系，在转换中使用了_transform，因此每一次迭代后，得到的变换结果也是不一样的。
transformToStart(_surfPointsFlat->points[i], pointSel);
每5次迭代重新提取特征点
- 使用
KD树 _lastSurfaceKDTree查找最近邻点索引pointSearchInd和对应的欧式距离pointSearchSqDis - 得到
最近邻点的Scan索引 即是激光发射束id closestPointScan - 通过对
点云索引的增加、减少，在相邻激光束查找次近邻点和第三近邻点，注意空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据 - 得到当前特征点在激光束中的
最近邻点_pointSearchCornerInd1、次近邻点_pointSearchCornerInd2、第三近邻点_pointSearchSurfInd3
if (iterCount % 5 == 0)
{
// 使用KD树查找最近邻点以及对应的距离
_lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
// 距离需要满足最低的阈值要求
if (pointSearchSqDis[0] < 25)
{
closestPointInd = pointSearchInd[0];
// 得到最近邻点的Scan索引 即是激光发射束id
int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity);
float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
// 在相邻激光束查找次近邻点
// 点云索引增加的线束中查找
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
{
// 空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据
if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5)
{
break;
}
// 计算两个三维点的距离，欧式距离
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
// 得到相邻激光束中次近邻点，与当前点位于同一激光束，更新距离minPointSqDis2 坐标minPointInd2
if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
else
{
// 不同线束则作为 第三近邻点 更新距离minPointSqDis3 坐标minPointInd3
if (pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
// 点云索引减少的线束中查找
for (int j = closestPointInd - 1; j >= 0; j--)
{
// 空间相邻的激光束，id相差为2,因此这里使用了2.5作为判断依据
if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5)
{
break;
}
// 计算两个三维点的距离，欧式距离
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
// 得到相邻激光束中次近邻点，与当前点位于同一激光束，更新距离minPointSqDis2 坐标minPointInd2
if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
else
{
// 不同线束则作为 第三近邻点 更新距离minPointSqDis3 坐标minPointInd3
if (pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
}
// 得到当前特征点在激光束中的最近邻点_pointSearchCornerInd1、
// 次近邻点（同一线束中）_pointSearchCornerInd2、第三近邻点（相邻线束）_pointSearchSurfInd3
_pointSearchSurfInd1[i] = closestPointInd;
_pointSearchSurfInd2[i] = minPointInd2;
_pointSearchSurfInd3[i] = minPointInd3;
}
如果成功找到次近邻点、第三近邻点对应的索引，那么可以找到平面特征关系
点对应关系为
i --- pointSel
j --- tripod1
l --- tripod2
m --- tripod3
叉乘公式a x b=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)
实际代码中公式不是完全一一对应，但是也能够很好的理解
// 没有找到次近邻，第三近邻点的话 索引为-1
if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0)
{
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];
//jlm形成的平面法向量 x -y z 由点远离平面的方向，距离增大的方向
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
- (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
- (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y)
- (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
//法向量 点乘j点坐标 取负号 是中间量
float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
//法向量的模值，底面积
float ps = sqrt(pa * pa + pb * pb + pc * pc);
//归一化
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
//得到距离， 立体面积/低面积 = 高，距离d
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; //Eq. (3)??
// TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
// 距离d越大，权重s越小
if (iterCount >= 5)
{
s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel));
}
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
//不考虑pd2为0，不考虑距离pd2过大的点
if (s > 0.1 && pd2 != 0)
{
_laserCloudOri->push_back(_surfPointsFlat->points[i]);
_coeffSel->push_back(coeff);
}
}
6.4.5 求解运动
通过特征点以及特征点和特征区域的关系，可以构建方程组（6.4.2.1 帧间运动），每次迭代时求解该方程组得到运动增量（6.4.2.2 GN非线性优化），最终运动估计收敛。运动求解时使用了防止非线性优化解退化的方法，详见LOAM SLAM原理之防止非线性优化解退化。
对所有满足要求的特征点可以构建方程得到方程组系数矩阵，参考6.4.2.1 帧间运动
// 估计运动时，使用的特征点数量足够
int pointSelNum = _laserCloudOri->points.size();
if (pointSelNum < 10)
{
continue;
}
Eigen::Matrix<float, Eigen::Dynamic, 6> matA(pointSelNum, 6);
Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, pointSelNum);
Eigen::Matrix<float, 6, 6> matAtA;
Eigen::VectorXf matB(pointSelNum);
Eigen::Matrix<float, 6, 1> matAtB;
Eigen::Matrix<float, 6, 1> matX;
for (int i = 0; i < pointSelNum; i++)
{
const pcl::PointXYZI& pointOri = _laserCloudOri->points[i];
coeff = _coeffSel->points[i];
float s = 1;
float srx = sin(s * _transform.rot_x.rad());
float crx = cos(s * _transform.rot_x.rad());
float sry = sin(s * _transform.rot_y.rad());
float cry = cos(s * _transform.rot_y.rad());
float srz = sin(s * _transform.rot_z.rad());
float crz = cos(s * _transform.rot_z.rad());
float tx = s * _transform.pos.x();
float ty = s * _transform.pos.y();
float tz = s * _transform.pos.z();
// 前面已经推导了，可以参考等式一起看 **6.4.2.1 帧间运动**
float arx = (-s * crx*sry*srz*pointOri.x + s * crx*crz*sry*pointOri.y + s * srx*sry*pointOri.z
+ s * tx*crx*sry*srz - s * ty*crx*crz*sry - s * tz*srx*sry) * coeff.x
+ (s*srx*srz*pointOri.x - s * crz*srx*pointOri.y + s * crx*pointOri.z
+ s * ty*crz*srx - s * tz*crx - s * tx*srx*srz) * coeff.y
+ (s*crx*cry*srz*pointOri.x - s * crx*cry*crz*pointOri.y - s * cry*srx*pointOri.z
+ s * tz*cry*srx + s * ty*crx*cry*crz - s * tx*crx*cry*srz) * coeff.z;
float ary = ((-s * crz*sry - s * cry*srx*srz)*pointOri.x
+ (s*cry*crz*srx - s * sry*srz)*pointOri.y - s * crx*cry*pointOri.z
+ tx * (s*crz*sry + s * cry*srx*srz) + ty * (s*sry*srz - s * cry*crz*srx)
+ s * tz*crx*cry) * coeff.x
+ ((s*cry*crz - s * srx*sry*srz)*pointOri.x
+ (s*cry*srz + s * crz*srx*sry)*pointOri.y - s * crx*sry*pointOri.z
+ s * tz*crx*sry - ty * (s*cry*srz + s * crz*srx*sry)
- tx * (s*cry*crz - s * srx*sry*srz)) * coeff.z;
float arz = ((-s * cry*srz - s * crz*srx*sry)*pointOri.x + (s*cry*crz - s * srx*sry*srz)*pointOri.y
+ tx * (s*cry*srz + s * crz*srx*sry) - ty * (s*cry*crz - s * srx*sry*srz)) * coeff.x
+ (-s * crx*crz*pointOri.x - s * crx*srz*pointOri.y
+ s * ty*crx*srz + s * tx*crx*crz) * coeff.y
+ ((s*cry*crz*srx - s * sry*srz)*pointOri.x + (s*crz*sry + s * cry*srx*srz)*pointOri.y
+ tx * (s*sry*srz - s * cry*crz*srx) - ty * (s*crz*sry + s * cry*srx*srz)) * coeff.z;
float atx = -s * (cry*crz - srx * sry*srz) * coeff.x + s * crx*srz * coeff.y
- s * (crz*sry + cry * srx*srz) * coeff.z;
float aty = -s * (cry*srz + crz * srx*sry) * coeff.x - s * crx*crz * coeff.y
- s * (sry*srz - cry * crz*srx) * coeff.z;
float atz = s * crx*sry * coeff.x - s * srx * coeff.y - s * crx*cry * coeff.z;
float d2 = coeff.intensity;
matA(i, 0) = arx;
matA(i, 1) = ary;
matA(i, 2) = arz;
matA(i, 3) = atx;
matA(i, 4) = aty;
matA(i, 5) = atz;
matB(i, 0) = -0.05 * d2;
}
求取非线性优化解的增量，参考6.4.2.2 GN非线性优化
matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;
matX = matAtA.colPivHouseholderQr().solve(matAtB);
防止非线性优化解退化时的处理，只对第一次迭代进行处理
if (iterCount == 0)
{
Eigen::Matrix<float, 1, 6> matE;
Eigen::Matrix<float, 6, 6> matV;
Eigen::Matrix<float, 6, 6> matV2;
Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
matE = esolver.eigenvalues().real();
matV = esolver.eigenvectors().real();
//这里有点问题，matV中列向量是特征向量，因此需要转置．
//
// matV2 = matV;
matV2 = matV.transpose();
isDegenerate = false;
float eignThre[6] = { 10, 10, 10, 10, 10, 10 };
for (int i = 0; i < 6; i++)
{
if (matE(0, i) < eignThre[i])
{
for (int j = 0; j < 6; j++)
{
matV2(i, j) = 0;
}
isDegenerate = true;
}
else
{
break;
}
}
// matP = matV.inverse() * matV2;
//需要进行转置
matP = matV.transpose().inverse() * matV2;
}
if (isDegenerate)
{
Eigen::Matrix<float, 6, 1> matX2(matX);
matX = matP * matX2;
}
更新解的分量，收敛时直接退出，否则继续进行迭代
// 更新解分量
_transform.rot_x = _transform.rot_x.rad() + matX(0, 0);
_transform.rot_y = _transform.rot_y.rad() + matX(1, 0);
_transform.rot_z = _transform.rot_z.rad() + matX(2, 0);
_transform.pos.x() += matX(3, 0);
_transform.pos.y() += matX(4, 0);
_transform.pos.z() += matX(5, 0);
// 判断解是否有效
if (!pcl_isfinite(_transform.rot_x.rad())) _transform.rot_x = Angle();
if (!pcl_isfinite(_transform.rot_y.rad())) _transform.rot_y = Angle();
if (!pcl_isfinite(_transform.rot_z.rad())) _transform.rot_z = Angle();
if (!pcl_isfinite(_transform.pos.x())) _transform.pos.x() = 0.0;
if (!pcl_isfinite(_transform.pos.y())) _transform.pos.y() = 0.0;
if (!pcl_isfinite(_transform.pos.z())) _transform.pos.z() = 0.0;
// 解的增量
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
pow(rad2deg(matX(1, 0)), 2) +
pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
pow(matX(4, 0) * 100, 2) +
pow(matX(5, 0) * 100, 2));
// 增量过小，可以考虑停止迭代
if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
break;
6.4.6 运动累计与优化
在代码中，使用相邻帧点云得到的估计运动使用_transform表示，而累计的估计运动使用_transformSum表示。因此需要将帧间运动和累计运动融合更新里程计信息，具体如何更新的呢，就接着向下看吧。
根据之前的推导6.4.2.1 帧间运动，实际求解的是矩阵 R s : t − 1 R_{s:t}^{-1} Rs:t−1中的值，在使用时需要添加负号、更改变换顺序才能得到最终的 R s : t R_{s:t} Rs:t。下面函数的具体分析参考6.4.2.3 累计运动
Angle rx, ry, rz;
accumulateRotation(_transformSum.rot_x,
_transformSum.rot_y,
_transformSum.rot_z,
-_transform.rot_x,
-_transform.rot_y.rad() * 1.05,
-_transform.rot_z,
rx, ry, rz);
详细解算过程为
// c为Sum，l为Cur
// 先Y再X后Z，左乘旋转坐标轴矩阵 从《世界》坐标系转换到《激光》坐标系
// Rcur*Rsum然后根据矩阵元素求解角度
void BasicLaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz,
Angle lx, Angle ly, Angle lz,
Angle &ox, Angle &oy, Angle &oz)
{
float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin()
- cx.cos()*cz.cos()*lx.sin()
- lx.cos()*ly.cos()*cx.sin();
ox = -asin(srx);
float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin())
+ lx.cos()*ly.sin()*(cy.cos()*cz.cos() + cx.sin()*cy.sin()*cz.sin())
+ lx.cos()*ly.cos()*cx.cos()*cy.sin();
float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos()
- lx.cos()*ly.sin()*(cz.cos()*cy.sin() - cy.cos()*cx.sin()*cz.sin())
- lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin());
oy = atan2(srycrx / ox.cos(), crycrx / ox.cos());
float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin())
+ cx.cos()*cz.sin()*(ly.cos()*lz.cos() + lx.sin()*ly.sin()*lz.sin())
+ lx.cos()*cx.cos()*cz.cos()*lz.sin();
float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos()
- cx.cos()*cz.sin()*(ly.cos()*lz.sin() - lz.cos()*lx.sin()*ly.sin())
- cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin());
oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos());
}
之前使用了IMU测量的非匀速运动偏移量_imuShiftFromStart，见5.4.2 近似匀速运动模型部分。因此得到真实运动，此为激光坐标系下的表示。
Vector3 v(_transform.pos.x() - _imuShiftFromStart.x(),
_transform.pos.y() - _imuShiftFromStart.y(),
_transform.pos.z() * 1.05 - _imuShiftFromStart.z());
进一步得到世界坐标系下的平移量，并累计。
rotateZXY(v, rz, rx, ry);
Vector3 trans = _transformSum.pos - v;
使用IMU测量信息对估计的旋转进行修正，参考6.4.2.4 IMU信息修正
//(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
//rx, ry, rz存放累计的到tend的旋转，imu分别存放到开始和到结束的旋转，是imu的测量数据。
//目的：纠正精确化rx, ry, rz的值，imuEnd*inv(imuStart)*Sum表示在估计的里程计中附加IMU测量的旋转量。
pluginIMURotation(rx, ry, rz,
_imuPitchStart, _imuYawStart, _imuRollStart,
_imuPitchEnd, _imuYawEnd, _imuRollEnd,
rx, ry, rz);
详细解算过程为
void BasicLaserOdometry::pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz,
const Angle& blx, const Angle& bly, const Angle& blz,
const Angle& alx, const Angle& aly, const Angle& alz,
Angle &acx, Angle &acy, Angle &acz)
{
float sbcx = bcx.sin();
float cbcx = bcx.cos();
float sbcy = bcy.sin();
float cbcy = bcy.cos();
float sbcz = bcz.sin();
float cbcz = bcz.cos();
float sblx = blx.sin();
float cblx = blx.cos();
float sbly = bly.sin();
float cbly = bly.cos();
float sblz = blz.sin();
float cblz = blz.cos();
float salx = alx.sin();
float calx = alx.cos();
float saly = aly.sin();
float caly = aly.cos();
float salz = alz.sin();
float calz = alz.cos();
// 参考**6.4.2.4 IMU信息修正**
float srx = -sbcx * (salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly)
- cbcx * cbcz*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
- calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
- cbcx * sbcz*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
- calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz);
acx = -asin(srx);
float srycrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
- calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
- (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
- calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz)
+ cbcx * sbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly);
float crycrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
- calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz)
- (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
- calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
+ cbcx * cbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly);
acy = atan2(srycrx / acx.cos(), crycrx / acx.cos());
float srzcrx = sbcx * (cblx*cbly*(calz*saly - caly * salx*salz) - cblx * sbly*(caly*calz + salx * saly*salz) + calx * salz*sblx)
- cbcx * cbcz*((caly*calz + salx * saly*salz)*(cbly*sblz - cblz * sblx*sbly)
+ (calz*saly - caly * salx*salz)*(sbly*sblz + cbly * cblz*sblx)
- calx * cblx*cblz*salz)
+ cbcx * sbcz*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz)
+ (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz)
+ calx * cblx*salz*sblz);
float crzcrx = sbcx * (cblx*sbly*(caly*salz - calz * salx*saly) - cblx * cbly*(saly*salz + caly * calz*salx) + calx * calz*sblx)
+ cbcx * cbcz*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx)
+ (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly)
+ calx * calz*cblx*cblz)
- cbcx * sbcz*((saly*salz + caly * calz*salx)*(cblz*sbly - cbly * sblx*sblz)
+ (caly*salz - calz * salx*saly)*(cbly*cblz + sblx * sbly*sblz)
- calx * calz*cblx*sblz);
acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos());
}
因此得到最终的里程计信息
//得到最终的激光里程计信息，但是依然很粗略的结果
// Xw = R' Xl +T'矩阵为 ， 平移量直接相加
// 从《激光》坐标系到《世界》坐标系，角度取《负》号 ，先Z再X后Y，左乘 旋转坐标轴
_transformSum.rot_x = rx;
_transformSum.rot_y = ry;
_transformSum.rot_z = rz;
_transformSum.pos = trans;
变换点云坐标，为下一次运动估计做准备
transformToEnd(_cornerPointsLessSharp);
transformToEnd(_surfPointsLessFlat);
具体内容如下，推导见6.4.3 坐标变换
//注意这里添加了之前没有考虑的非匀速运动信息_imuShiftFromStart
size_t BasicLaserOdometry::transformToEnd(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud)
{
size_t cloudSize = cloud->points.size();
for (size_t i = 0; i < cloudSize; i++)
{
pcl::PointXYZI& point = cloud->points[i];
float s = (1.f / _scanPeriod) * (point.intensity - int(point.intensity));
//变换到 第一个点坐标系 inv(R)(Xt - T)= X
point.x -= s * _transform.pos.x();
point.y -= s * _transform.pos.y();
point.z -= s * _transform.pos.z();
point.intensity = int(point.intensity);
Angle rx = -s * _transform.rot_x.rad();
Angle ry = -s * _transform.rot_y.rad();
Angle rz = -s * _transform.rot_z.rad();
rotateZXY(point, rz, rx, ry);
//变换到 最后一个点坐标系 Xt= RX + T
rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z);
//下面是对非匀速运动进行恢复。
point.x += _transform.pos.x() - _imuShiftFromStart.x();
point.y += _transform.pos.y() - _imuShiftFromStart.y();
point.z += _transform.pos.z() - _imuShiftFromStart.z();
// 下面是使用IMU测量信息补偿
rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);
rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);
}
return cloudSize;
}
设置其他参数
// 为下一次 运动估计 做准备
_cornerPointsLessSharp.swap(_lastCornerCloud);
_surfPointsLessFlat.swap(_lastSurfaceCloud);
lastCornerCloudSize = _lastCornerCloud->points.size();
lastSurfaceCloudSize = _lastSurfaceCloud->points.size();
if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
{
_lastCornerKDTree.setInputCloud(_lastCornerCloud);
_lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
}
6.4.7 结果发布
最终将得到的里程计信息进行发布，使用坐标变换节点对其处理，得到连续的运动估计信息。
发布里程计信息_laserOdometryMsg
// publish odometry transformations
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum().rot_z.rad(),
-transformSum().rot_x.rad(),
-transformSum().rot_y.rad());
_laserOdometryMsg.header.stamp = _timeSurfPointsLessFlat;
_laserOdometryMsg.pose.pose.orientation.x = -geoQuat.y;
_laserOdometryMsg.pose.pose.orientation.y = -geoQuat.z;
_laserOdometryMsg.pose.pose.orientation.z = geoQuat.x;
_laserOdometryMsg.pose.pose.orientation.w = geoQuat.w;
_laserOdometryMsg.pose.pose.position.x = transformSum().pos.x();
_laserOdometryMsg.pose.pose.position.y = transformSum().pos.y();
_laserOdometryMsg.pose.pose.position.z = transformSum().pos.z();
_pubLaserOdometry.publish(_laserOdometryMsg);
发布tf变换格式的里程计信息
_laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat;
_laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
_laserOdometryTrans.setOrigin(tf::Vector3(transformSum().pos.x(), transformSum().pos.y(), transformSum().pos.z()));
_tfBroadcaster.sendTransform(_laserOdometryTrans);
对于点云数据，根据输入输出频率_ioRatio关系发布
// publish cloud results according to the input output ratio
if (_ioRatio < 2 || frameCount() % _ioRatio == 1)
{
ros::Time sweepTime = _timeSurfPointsLessFlat;
publishCloudMsg(_pubLaserCloudCornerLast, *lastCornerCloud(), sweepTime, "/camera");
publishCloudMsg(_pubLaserCloudSurfLast, *lastSurfaceCloud(), sweepTime, "/camera");
transformToEnd(laserCloud()); // transform full resolution cloud to sweep end before sending it
publishCloudMsg(_pubLaserCloudFullRes, *laserCloud(), sweepTime, "/camera");
}
持续更新中，望批评指正！有疑惑的地方欢迎共同探讨！@2020-12-07
更新日志
同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。
- 2020-12-07
整理完成LaserOdometry节点内容
最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！ - 2020-10-11
整理完成MultiScanRegistration节点内容，地址请转到LOAM源代码分析附公式推导之MultiScanRegistration - 2020-10-09
第一次编辑，初版