ALoam使用了ceres庫進行優化，得到優化後的裡程計資訊。
學習總結：
1、明确知道要優化什麼：
我們的目的是要知道目前雷達的姿态與上一幀的點雲姿态相比的變化，即q_last_curr,t_last_curr.
注意：讀作curr到last的q，也就是curr在last坐标系下的旋轉和位置（t可以直接看做是位置）。
如果這兩個量估計的很準确的話，那麼将curr的點雲中的每個點進行：q_last_curr*p_curr+t_last_curr會對應上last坐标系下的點。
在這裡，是落在last坐标系下的某edge線上。
2、如何組織資料：
為了進行最小二乘的優化，得知道對應（比對）關系，才能得到類似 ( y − h ( x ) ) 2 (y-h(x))^2 (y−h(x))2這樣的形式，送入到優化器進行計算。
分兩類：
1）點與線的比對
2）點與面的比對
2.1、點與線的比對
含義是目前點落在找到的線上面。
Aloam（loam）的做法是按照論文中來找哪個點對應哪條線的：
2.1.1 将點雲投影回本點雲幀開始時刻的姿态上
對應目前幀的任何一個點雲，首先投影到本點雲幀開始時刻的姿态上。怎麼投呢？就是如何知道目前時刻相對于本點雲開始時刻的姿态變化？這裡用到了上一次估計的值q_last_curr和t_last_curr，用這兩個作為整個點雲幀内開始時刻和結束時刻的相對姿态變化，然後根據要處理的點雲的時刻與起始時刻的時間距離做線性插值，利用插值的相對姿态變化，就可以将該點投到對應的起始時刻的坐标系下了。
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
//s = 1;
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;
Eigen::Vector3d point(pi->x, pi->y, pi->z);
Eigen::Vector3d un_point = q_point_last * point + t_point_last;
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
2.1.2 對目前幀的某個投影到本目前幀開始時刻的點雲，找上一幀對應的線
那就有一個問題了，目前幀的點雲投影到了開始時刻，那上一幀的點雲呢？如果它們也是投影到他們所在點雲幀的開始時刻的話，還是不是在一個近似的坐标系下的。
即，存下來的上一幀的點雲，應該是投影到他們所在的點雲幀的結束時刻的坐标系上，這樣，跟下一幀的起始時刻的坐标系理論上是一個。
論文作者是這樣說的：
**Let t k t_k tk be the starting time of a sweep k. At the end of each sweep, the point cloud perceived during the sweep,
**
從上面可以看到：
處理過的點雲，都會投影到結束時刻的坐标系下，而目前收到的點雲資料會投影到起始時刻的坐标系下，用的是前面的姿态，是以這是一個估計，是初值，也是我們需要優化的目标。
進行了坐标系的基本對準後，就可以搜尋對應關系了。
對于目前幀的任何一個投影後的edge點i，在上一幀投影後的點集裡去搜尋最近的edge點，假設找到的為j，如果i與j的位置足夠接近，則在這個j所在的scanid的上下範圍内繼續找第二近的點l。
找到了了(j,l),則建構一條直線方程，将點i到(j,l)上的投影當做一個殘差。
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
對于每個點都進行以上操作，那就構成了很多個殘差項。
2.2 面點的搜尋
找法是類似的。
找最近點j，然後在j的同scan上，找最近點l，然後在不同的scan上找第三個最近點m，則(j,l,m)構成一個平面。
點到面的距離理論上要為0.
以下是總的找比對的算法示意圖：
看一下總的求姿态變化的算法描述：
3、細節補充
1、對于第一個得到的點雲幀的處理：
不做任何的所謂姿态變化處理，直接将點雲幀當做是可以使用的。是以就沒有所謂的投影到 t k + 1 t_{k+1} tk+1的做法，因為初始時刻:
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t)
第二幀來的時候，還是會用到這個q_last_curr,t_last_curr，來估計在起始時刻的坐标，此時等于沒有估計。從這裡來看，如果第一幀與第二幀姿态差别特别大的話，導緻搜尋不到正确的對應點，那麼估計的姿态就會錯了。
是以起始時刻，得要慢着點移動。
應該是在任何時刻，兩幀之間的姿态變化以及幀内的姿态變化都不能太過劇烈。因為用的上一幀與上上幀的姿态估計，來進行勻速補償。任何一個環節超了，都會導緻比對失敗。
2、投影到 t k + 1 t_{k+1} tk+1時刻
ALoam的代碼中，被注釋掉了。
4、優化部分
用ceres庫進行。
得到優化後的q_last_curr,t_last_curr，然後更新：
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
注意：
在ceres中，優化的不是直接的q_last_curr,t_last_curr，而是
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
但由于使用了Eigen::Map的機制，他們是共用記憶體的：
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);
是以，優化了para_q,para_t就是等同于優化了q_last_curr、t_last_curr。