第二章傳感器數據處理
第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