第二章传感器数据处理
第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
