基于Aloam对特征匹配及雷达里程计的学习

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$这样的形式，送入到优化器进行计算。
分两类：
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$ 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}$的做法，因为初始时刻:

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}$时刻
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。