深蓝学院激光SLAM第二期学习笔记

第二章传感器数据处理

 

第1节 里程计运动模型及标定

 

根据相对位置求绝对位姿：

若为二维点，[x, y, 1] = [R, t; 0, 1]*[del_x, del_y, 1]，这里的xy表示的点是没有方向的

若为带方向的三维点，上述公式是没法用的，不能用向量而应该用位姿矩阵来表示T = T0*delta_T;

记住：带方向的位姿，一定先转化成T再进行运算

[R, t; 0, 1]  <=> [RT, -RT*t; 0, 1]

 

运动模型：从w_l, w_r获得v, w

航迹推算：从上一时刻绝对坐标和局部的相对坐标，推算下一时刻绝对坐标

线性最小二乘：超定方程Ax=b，A=[a1 a2 ... an]向量空间无法表示b，无解只有比较好的近似解，Ax*-b与该空间垂直，与任意一个a垂直，建立方程，推导得通解(ATA)逆（AT）b.

 

里程计标定（使用激光雷达）：1、直接法。假设里程计给出的相对位移与激光之间存在线性的变换，求解该变换的9个参数；2、模型法。标定的是模型中的参数，两个*的半径、轮间距。

 

作业：找不到nav_core头文件，这个是navigation stack的一部分，sudo apt-get install ros-kinectic-navigation；

找不到CSM，The C(anonical) Scan Matcher (CSM) is a pure C implementation of a very fast variation of ICP using a point-to-line metric optimized for range-finder scan matching. 解决方案见https://github.com/AndreaCensi/csm sudo apt-get install ros-kinetic-csm

 

第2节 激光雷达畸变校正

 

雷达模型

光束模型：击中、被挡住提前返回、无穷远、完全随机，需要多次划线算法，杂乱环境下不好用，位姿（主要是角度）的微小变化导致期望的巨大变化

似然场模型：对障碍物膨胀，实际上csm就是

雷达畸变：

靠近墙壁时，墙壁畸变旋转的方向与雷达旋转方向相同；原理时，方向相反。

 

畸变校正方法：纯估计方法、里程计辅助方法、融合法

1、ICP,不知道匹配关系，因而不能一步到位求解出R和t，需要迭代<=>EM期望最大化；VICP基于匀速运动假设。位姿的线性插值，LOAM也是这种思想；状态估计和去畸变耦合；

2、IMU和里程计的选择，IMU线性太差，里程计位置和角度都比较好。实际上就是用IMU为激光插值，先选一部分直接插值，其他的激光再按照匀速运动插值，相当于分段函数；

3、融合：里程计->VICP->里程计->...

 

作业：主要思想，beam与beam之间时间间隔足够大，分段(start, end)，两个断点的位姿通过里程计（高频）插值得到，假设该段内有n个点（包含两端），那么分割成n-1个段。插值得到每一个beam对应的机器人位姿后，将该beam的<r, theta>变成欧式坐标，再投影到世界坐标系下，再投影到base坐标系（整帧的第一个beam）下，再转换成极坐标，这样就将所有的beam修正到了base的极坐标系下。

tf::Stamped<tf::Pose> visualPose;

visualPose.getRotation()获得四元数头发：：Quaternion，有.slerp(end, i*step)求解角度插值的方法

上述n个点分成n-1段，分别插值为start.slerp(end, 0*(1/n-1)), start.slerp(end, 1*(1/n-1)),...., start.slerp(end, 1)共n个点

double visualYaw = tf::getYaw(visualPose.getRotation());//可以进一步从四元数获得yaw角

tf::Vector3 = visualPose.getOrigin();//获得位置坐标

visualPose.getOrigin().getX();//等同于[0]索引

current_pose = start_pose.lerp(end_pose, step*i);//位置的插值

对于坐标变换T*v不知道怎么实现，这里是直接将T拆分开求解的

此外还有tfFuzzyZero()函数，tfNormalizeAngle()函数

如何定义自己的message格式，这个还没有看，有时间学习一下

 

第3节 前端1

 

ICP、PL-ICP、NICP、IMLS-ICP精度越来越高，计算量越来越大

1、如果已知匹配，实际上可以一步到位求解R和t，就是因为不知道，所以需要迭代

ICP在求解R和t的过程中可以求出解析解，速度要比迭代求解快

2、PLICP，对于直线，上一次看到一个点，由于离散性，下一次看到两侧的两个点。二阶收敛，精度高，特别是结构化环境中，对初值敏感，因而一般与里程计或者CSM结合使用。好像有解析解？

3、NICP匹配中除了欧式距离，还考虑法向量、曲率等曲面特征，误差项也考虑了法向量的角度差，方法就是把点从p扩展维度到<p, n>。法向量的求解，最小特征值对应的特征向量的方向，曲率lamda1/(lamda1+lambda2)。没有解析解，需要用LM迭代求解。

4、IMLS-ICP。PLICP相当于是分段光滑，这里相当于拟合出了曲线。隐式，求不出来点，但可以知道距离。

对于每一个点q，寻找附近点pi的集合P，它们可以拟合出一个面，为了求解q在面上的投影点，一个很自然的想法就是要知道高度h和投影点处的法向n。对于法向可以直接用最近的点的法向代替。对于高度，可以根据每一个pi及ni拟合出一个小平面，计算q与每一个小平面的距离hi，但是需要加权，权重就是pi在以q为中心的一个正态分布上的值，也就是说离q越近，权重越高，因此只有q一定范围内pi是有意义的，也就是moving的含义。至此，就求解出来了高度h和法向n，很容易反向求解出q在隐式曲面上的投影点。

 

作业：

根据点云估计法向，推导简化，可以减小计算量。涉及特征分解：

/**

* @brief IMLSICPMatcher::ComputeNormal

* 计算法向量

* @param nearPoints 某个点周围的所有激光点

* @return

*/

Eigen::Vector2d IMLSICPMatcher::ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)

{

Eigen::Vector2d normal;



//TODO

Eigen::Vector2d mean = Eigen::Vector2d::Zero();

Eigen::Matrix2d sigma = Eigen::Matrix2d::Zero();

for(auto& p : nearPoints){

mean += p;

sigma += p*p.transpose();//把vector严格视为n行1列的matrix即可

}

mean /= nearPoints.size();

sigma = sigma/nearPoints.size()-mean*mean.transpose();//不确定这里的推导有没有问题，看结果

Eigen::EigenSolver<Eigen::Matrix2d> es(sigma);//Eigen::EigenSolver是一个模板，需要指定矩阵的类型

auto eigen_value = es.pseudoEigenvalueMatrix();//矩阵可逆效果与.eigenvalues()一样；不可逆只有这个可以得到正确结果

auto eigen_vectors = es.pseudoEigenvectors();

if(eigen_value(0,0) < eigen_value(1, 1)){//此外还有.minCoeff()求最小值（也可以返回索引），.rowwise()可以降维。

normal = eigen_vectors.col(0);

}else{

normal = eigen_vectors.col(1);

}

//end of TODO

return normal;

}

 

height = projSum/(weightSum+0.000000001);//安全

 

第4节 前端2

基于优化的方法=基于势场的方法，不需要匹配点

高斯牛顿、NDT、CSM+BB(Branch Bound)

1、高斯牛顿

拉格朗日插值法，把插值多项式表示成基函数的线性组合

双线性插值，两个变量u, v

本质就是，非线性方程无法求解，但可以推导出在局部有意义的导数。为了最小化误差函数，需要求解一个最优的deltaT，给定该点处导数的数值解，高斯牛顿法就可以给出该处出发合适的步长和方向，即deltaT。误差可以不用二次方么？

2、NDT

牛顿法：求解f(x)的最小值，令g(x)=f'(x)=0去求解x，但是x也求不出来解析解，只能迭代：

g(x+delta)=g(x)+g'(x)*delta=0，然后求解出了delta，叠加到x上，这时的x在局部看来，可以让g(x)=0，即f(x)最小。由于非线性，需要再继续迭代，不断寻找新的最优位姿。这里的g'(x)即f(x)导数的导数，即H矩阵。

3、CSM

 

作业总结：

1、已知odom1、odom2、laser1的位姿，估计laser1的位姿。这里有个假设，即laser与odom的中心是重合的，这样laser2相对于laser1的位姿，即odom2相对于odometer1的位姿。如此，根据odom之间的相对位姿，估计出来一个不错的，laser之间的相对位姿。这里有个数学上的逻辑，在对有方向的三维点进行坐标变换时有两种方式：

（1）

          [cos, -sin, x,        [cosd, -sind, xd,

           sin, cos, y,    *    sind, cosd, yd,

           0,     0,    1]         0,      0,        1]

这种方式得到了位姿矩阵，非常方便。

（2）

         [cos, -sin, 0,        [xd,               [x

           sin, cos, 0,    *    yd,         +     y

           0,     0,    1]         thetad]        theta]

这种方式将位姿向量转换成位姿向量，当需要使用向量时非常方便。反过来变换也是很明显的。

//进行优化．
        //初始解为上一帧激光位姿+运动增量
        Eigen::Vector3d deltaPose = nowPose - m_odomPath.back();
        deltaPose(2) = GN_NormalizationAngle(deltaPose(2));//现在一直odom之间的变化量，想求laser之间的变化量

        Eigen::Matrix3d R_laser = Eigen::Matrix3d::Zero();
        double theta = m_prevLaserPose(2);
        R_laser << cos(theta), -sin(theta), 0, 
                   sin(theta),  cos(theta), 0,
                        0,          0,      1;

        Eigen::Matrix3d R_odom = Eigen::Matrix3d::Zero();
        theta = m_odomPath.back()(2);
        R_odom << cos(theta), -sin(theta), 0, 
                  sin(theta),  cos(theta), 0,
                       0,          0,      1;
        Eigen::Vector3d finalPose = m_prevLaserPose + R_laser * R_odom.transpose() * deltaPose;//这里应该假设了odom和laser的中心重合
        finalPose(2) = GN_NormalizationAngle(finalPose(2));
        //final为根据odom估计处的laser的位姿
        GaussianNewtonOptimization(map,finalPose,nowPts);

解释一下，先求解odom2与odom1在世界坐标系下的差值，再将xy投影到odom1坐标系下，即求解得到了odom2在odom1下的坐标向量delta。基于odom与laser重合的假设，laser2相对于laser1的坐标也是delta，因此再进行一次变换就得到了laser2的世界坐标。这也是高斯牛顿法优化的起点。

2、作业中地图的逻辑是，世界坐标系和地图坐标系的（0， 0）均位于左下角。（x, y）处占用概率由四舍五入得到地图坐标查表得到，周围整数坐标由floor函数取整得到。地图问题需要再深入考虑下。在地图上求解答到导数的时候，要注意这里的导数单位是/格，需要除以resolution才能得到世界坐标系意义下的导数；地图需要不断判断是否越界；

3、1*2的vecor乘以2*3的matrix好像不可以，应该是数据结构上的问题，都是用matrix就没问题了。matrix*vector好像没问题。

4、

now_pose[2] = GN_NormalizationAngle(now_pose[2]);

在对位姿进行叠加的时候，需要注意判断角度的范围，控制在[-pi, pi]之间。

 

第5节 后端

 

激光SLAM中优化的节点一般只包含机器人位姿，不包含环境特征。

后端是一个标准的非线性最小二乘问题：

[z1,  z2, z3, ..., zn] = f([x1, x2, x3, ..., xn])

我觉得很重要的一点在于要认识到所有的x不是割裂的，他们都是整个系统状态的一部分，而不是n个独立的自变量，即状态向量的概念。

而每一次的观测zi是对整个状态向量的不完全观测

总体的误差函数，f(x)=sigma fi(x)，因此对x的雅克比矩阵自然而言也应该是fi(x)对x的雅克比矩阵之和。

作业总结：

1、搞清楚矩阵的维度，特别是H, b， 最后要求解的是H * delx = -b，千万别忽略了这里的负号。

另外线性方程的求解，使用：

dx = -H.colPivHouseholderQr.solve(b);

没能成功，原因在于colPivHouseholderQr是函数，而不是属性，这点一定要注意！应为：

dx = -H.colPivHouseholderQr().solve(b);或者

dx = -H.lu（）.solve(b);

H.inverse()*b没有优势，原因在于H的维度高，而且稀疏性没有利用起来；

2、感性认知，需要知道每一个constraint对应的节点xi, yi坐标，以及观测zi和sigma，才能求解相应的误差ei，及ei的雅克比Ai, Bi，而他们又是H和b更新的主要组成部分；

3、代码中有几点需要后面学习：（1）读文本（2）发布markarray