---

---

参考中文注释：github

1. scanRegistration

1.1 激光点的扫描角度与归类

// 获取点云的大小
int cloudSize = laserCloudIn.points.size();

// 计算点云第一个点的起始方位角
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);

// 计算点云最后一个点的终止方位角，并加上2π以确保角度为正值
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                      laserCloudIn.points[cloudSize - 1].x) +
               2 * M_PI;

// 调整终止角度，确保终止角度与起始角度的差值在合理范围内
if (endOri - startOri > 3 * M_PI)
{
    
    
    endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
    
    
    endOri += 2 * M_PI;
}

// 初始化变量
bool halfPassed = false; // 标记是否超过半圈
int count = cloudSize;   // 初始化有效点计数
PointType point;         // 定义点类型
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); // 初始化扫描线点云容器

// 遍历点云中的每个点
for (int i = 0; i < cloudSize; i++)
{
    
    
    // 获取当前点的坐标
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;

    // 计算当前点的垂直角度（以度为单位）
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
    int scanID = 0; // 初始化扫描线ID

    // 根据扫描线数量计算扫描线ID
    if (N_SCANS == 16)
    {
    
    
        scanID = int((angle + 15) / 2 + 0.5); // 计算16线激光雷达的扫描线ID
        if (scanID > (N_SCANS - 1) || scanID < 0) // 检查扫描线ID是否超出范围
        {
    
    
            count--; // 有效点计数减1
            continue; // 跳过当前点
        }
    }
    else if (N_SCANS == 32)
    {
    
    
        scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); // 计算32线激光雷达的扫描线ID
        if (scanID > (N_SCANS - 1) || scanID < 0) // 检查扫描线ID是否超出范围
        {
    
    
            count--; // 有效点计数减1
            continue; // 跳过当前点
        }
    }
    else if (N_SCANS == 64)
    {
    
       
        if (angle >= -8.83) // 判断角度范围
            scanID = int((2 - angle) * 3.0 + 0.5); // 计算64线激光雷达的扫描线ID（上半部分）
        else
            scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); // 计算64线激光雷达的扫描线ID（下半部分）

        // 使用[0, 50]范围内的扫描线ID，超出范围的点视为异常点
        if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
        {
    
    
            count--; // 有效点计数减1
            continue; // 跳过当前点
        }
    }
    else
    {
    
    
        printf("wrong scan number\n"); // 输出错误信息
        ROS_BREAK(); // 中断程序
    }

    // 计算当前点的方位角
    float ori = -atan2(point.y, point.x);

    // 根据是否超过半圈调整方位角
    if (!halfPassed)
    {
    
     
        if (ori < startOri - M_PI / 2) // 如果当前点的方位角小于起始方位角减去π/2
        {
    
    
            ori += 2 * M_PI; // 加上2π
        }
        else if (ori > startOri + M_PI * 3 / 2) // 如果当前点的方位角大于起始方位角加上3π/2
        {
    
    
            ori -= 2 * M_PI; // 减去2π
        }

        if (ori - startOri > M_PI) // 如果当前点的方位角与起始方位角的差值大于π
        {
    
    
            halfPassed = true; // 标记为超过半圈
        }
    }
    else
    {
    
    
        ori += 2 * M_PI; // 加上2π
        if (ori < endOri

• 起始与结束角度：用于定义扫描区域的角度。
• 扫描ID计算：根据激光点的角度确定其属于哪个扫描层。
• 旋转角度调整：确保激光点的角度在正确的范围内。
• 相对时间计算：用于计算每个点在扫描周期中的相对位置。

1. 起始和结束角度计算：
   起始角度 $startOri$ 和结束角度 $endOri$ 的计算如下：

   • 起始角度：
     $$startOri=-\text{atan2}(y_0,x_0)$$
   • 结束角度：
     $$endOri=-\text{atan2}(y_{\text{last}},x_{\text{last}})+2\pi$$

   其中 $x_0$, $y_0$ 是第一个点的坐标，$x_{\text{last}}$, $y_{\text{last}}$ 是最后一个点的坐标。角度差如果大于 $3\pi$，将结束角度减去 $2\pi$；如果小于 $\pi$，将结束角度加上 $2\pi$。

2. 扫描角度判断与点分类：
   根据激光点的高度角（$z$ 轴与 $x$, $y$ 平面夹角）计算扫描的 $scanID$，不同的扫描仪（例如16, 32, 64线）采用不同的公式将激光点归类到扫描层（scan layer）。

   • 对于16线扫描仪（$N_{\text{SCANS}}=16$）：
     $$scanID=\lfloor \frac{\text{angle}+15}{2}+0.5\rfloor$$

   • 对于32线扫描仪（$N_{\text{SCANS}}=32$）：
     $$scanID=\lfloor \frac{\text{angle}+\frac{92.0}{3.0}}{\frac{3.0}{4.0}}\rfloor$$

   • 对于64线扫描仪（$N_{\text{SCANS}}=64$）：
     如果 $angle\ge -8.83$：
     $$scanID=\lfloor (2-\text{angle})\times 3+0.5\rfloor$$
     否则：
     $$scanID=\lfloor \frac{N_{\text{SCANS}}}{2}+(-8.83-\text{angle})\times 2+0.5\rfloor$$

   注意：如果计算得到的 $scanID$ 超出有效范围（例如大于最大扫描层或小于0），该点会被丢弃。

3. 激光点旋转角度调整：
   对于每个点，计算旋转角度 $\text{ori}$，并根据起始角度和结束角度调整其旋转角度范围。如果点的旋转角度位于起始角度和结束角度之间，计算该点在扫描周期中的相对时间（$relTime$）：
   $$relTime=\frac{\text{ori}-\text{startOri}}{\text{endOri}-\text{startOri}}$$

   扫描二维码关注公众号，回复： 17550130 查看本文章

4. 点云强度设置与保存：
   设置每个点的强度为 $scanID$ 和相对时间的线性组合：
   $$\text{point.intensity}=\text{scanID}+\text{scanPeriod}\times \text{relTime}$$

   然后，将点保存到相应的扫描层（laserCloudScans[scanID]）。

5. 输出有效点数量：
   输出有效点云的数量：
   $$\text{cloudSize}=\text{count}$$

---

1.2 特征点提取与分类

步骤 1: 点云预处理与扫描区间定义

    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    {
    
     
        scanStartInd[i] = laserCloud->size() + 5;// 记录每个scan的开始index，忽略前5个点
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;// 记录每个scan的结束index，忽略后5个点，开始和结束处的点云scan容易产生不闭合的“接缝”，对提取edge feature不利
    }

对于每个扫描（scan），记录每个扫描的开始和结束索引，忽略前后5个点，以避免开始和结束处产生接缝：

• 扫描的开始索引：
  $$\text{scanStartInd}[i]=\text{laserCloud}->\text{size}()+5$$

• 扫描的结束索引：
  $$\text{scanEndInd}[i]=\text{laserCloud}->\text{size}()-6$$

步骤 2: 计算每个点的曲率

    for (int i = 5; i < cloudSize - 5; i++)
    {
    
     
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;// 点有没有被选选择为feature点
        cloudLabel[i] = 0;// Label 2: corner_sharp
                          // Label 1: corner_less_sharp, 包含Label 2
                          // Label -1: surf_flat
                          // Label 0: surf_less_flat， 包含Label -1，因为点太多，最后会降采样
    }

曲率的计算公式为：
$$\text{cloudCurvature}[i]=\sum _{j=-5}^5(p_j-p_i{)}^2$$
其中 $p_j$ 为当前点周围的邻域点，$p_i$ 是当前点，$p_j$ 和 $p_i$ 的坐标差异计算得到该点的曲率。此处对于每个点计算其曲率，若曲率大于某个阈值（如 $0.1$），则可以作为特征点进行进一步提取。

步骤 3: 提取 Corner 和 Surface 特征

for (int i = 0; i < N_SCANS; i++)// 按照scan的顺序提取4种特征点
    {
    
    
        if( scanEndInd[i] - scanStartInd[i] < 6)// 如果该scan的点数少于7个点，就跳过
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)// 将该scan分成6小段执行特征检测
        {
    
    
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan的起始index
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan的结束index

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 根据曲率有小到大对subscan的点进行sort
            t_q_sort += t_tmp.toc();

            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)// 从后往前，即从曲率大的点开始提取corner feature
            {
    
    
                int ind = cloudSortInd[k]; 

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)// 如果该点没有被选择过，并且曲率大于0.1
                {
    
    
                    largestPickedNum++;
                    if (largestPickedNum <= 2)// 该subscan中曲率最大的前2个点认为是corner_sharp特征点
                    {
    
                            
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)// 该subscan中曲率最大的前20个点认为是corner_less_sharp特征点
                    {
    
                            
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
    
    
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;// 标记该点被选择过了

                    // 与当前点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
                    for (int l = 1; l <= 5; l++)
                    {
    
    
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
    
    
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
    
    
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
    
    
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // 提取surf平面feature，与上述类似，选取该subscan曲率最小的前4个点为surf_flat
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
    
    
                int ind = cloudSortInd[k];

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {
    
    

                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)
                    {
    
     
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++)
                    {
    
     
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
    
    
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
    
    
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
    
    
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

每个点的曲率用于确定其特征标签。

• Corner特征点提取:

  • 对于每个子扫描（subscan），从曲率最大值开始提取 $2$ 个角点（corner_sharp），接下来提取曲率较大的 $20$ 个点（corner_less_sharp）。
  • 条件：
    $$\text{曲率}>0.1$$

• Surface特征点提取:

  • 对于每个子扫描，提取曲率最小的 $4$ 个点作为平面点（surf_flat）。
  • 如果曲率小于 $0.1$，则认为该点为平面特征点。

步骤 4: 降采样

        // 最后对该scan点云中提取的所有surf_less_flat特征点进行降采样，因为点太多了
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

对于平面点（surf_less_flat），使用体素滤波器（VoxelGrid）进行降采样：

• 设置体素的大小：
  $$\text{VoxelSize}=0.2,0.2,0.2$$
• 使用降采样后的点云来构建最终的 surfPointsLessFlat。

2. laserOdometry

2.1 基于最近邻原理建立Corner特征点之间的关联

1. 变换当前帧的特征点到上一帧的坐标系:

// 对激光雷达点进行畸变校正
void TransformToStart(PointType const *const pi, PointType *const po)
{
    
    
    // 插值比例
    // 计算插值比例，用于处理激光雷达数据的时间畸变
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; // 如果存在畸变，根据点的强度值计算插值比例
    else
        s = 1.0; // 如果没有畸变，插值比例为1，即不进行插值
    //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;
}

将当前帧的 corner_sharp 特征点（记为点 $O_{\text{cur}}$）从当前帧的 Lidar 坐标系变换到上一帧的 Lidar 坐标系下（记为点 $O$），这样可以通过寻找最近邻来建立对应关系。

变换公式：
$$O=T_{\text{cur}\to \text{prev}}(O_{\text{cur}})$$

3. 寻找最近邻点（点 $O$ 对应的点 $A$）:

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧
                                                                                                        // 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）

4. 寻找第二个最近邻点（点 $O$ 对应的点 $B$）:
   在沿扫描线递增和递减的方向上分别寻找两个最近邻点 $A$ 和 $B$：

   • 递增方向：
     $$\min \text{distance}(O,B)=\text{distance}(O,A)+\Delta \text{distance}$$
   • 递减方向：
     $$\min \text{distance}(O,B)=\text{distance}(O,A)-\Delta \text{distance}$$

5. 验证两个最近邻点有效性：

                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点A有效
                        {
    
    
                            closestPointInd = pointSearchInd[0];
                            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
                            // 寻找点O的另外一个最近邻的点（记为点B） in the direction of increasing scan line
                            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)// laserCloudCornerLast 来自上一帧的corner_less_sharp特征点,由于提取特征时是
                            {
    
                                                                                       // 按照scan的顺序提取的，所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
                                // if in the same scan line, continue
                                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// intensity整数部分存放的是scanID
                                    continue;

                                // if not in nearby scans, end the loop
                                if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                        (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                        (laserCloudCornerLast->points[j].z - pointSel.z);

                                if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效,，更新点B
                                {
    
    
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }

                            // 寻找点O的另外一个最近邻的点B in the direction of decreasing scan line
                            for (int j = closestPointInd - 1; j >= 0; --j)
                            {
    
    
                                // if in the same scan line, continue
                                if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                                    continue;

                                // if not in nearby scans, end the loop
                                if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                        (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                        (laserCloudCornerLast->points[j].z - pointSel.z);

                                if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效，更新点B
                                {
    
    
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }

如果两个最近邻点有效，则可以构造残差项。通过点 $O$ 和点 $A$，$B$ 构造点到直线的距离残差：
$$\text{residual}(O,A,B)=\text{distance}(O,\text{line}(A,B))$$
7. 残差函数与优化：

struct LidarEdgeFactor
{
    
    
	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
					Eigen::Vector3d last_point_b_, double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {
    
    }

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
    
    
		Eigen::Matrix<T, 3, 1> cp{
    
    T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpa{
    
    T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
		Eigen::Matrix<T, 3, 1> lpb{
    
    T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
		Eigen::Quaternion<T> q_last_curr{
    
    q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{
    
    T(1), T(0), T(0), T(0)};
		// 考虑运动补偿，ktti点云已经补偿过所以可以忽略下面的对四元数slerp插值以及对平移的线性插值
		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{
    
    T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;
		// Odometry线程时，下面是将当前帧Lidar坐标系下的cp点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线的残差距离
		// Mapping线程时，下面是将当前帧Lidar坐标系下的cp点变换到world坐标系下，然后在world坐标系下计算点到线的残差距离
		lp = q_last_curr * cp + t_last_curr;

		// 点到线的计算如下图所示
		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
		Eigen::Matrix<T, 3, 1> de = lpa - lpb;

		// 最终的残差本来应该是residual[0] = nu.norm() / de.norm(); 为啥也分成3个，我也不知
		// 道，从我试验的效果来看，确实是下面的残差函数形式，最后输出的pose精度会好一点点，这里需要
		// 注意的是，所有的residual都不用加fabs，因为Ceres内部会对其求 平方 作为最终的残差项
		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
									   const Eigen::Vector3d last_point_b_, const double s_)
	{
    
    
		return (new ceres::AutoDiffCostFunction<
				LidarEdgeFactor, 3, 4, 3>(
//					             ^  ^  ^
//					             |  |  |
//			      残差的维度 ____|  |  |
//			 优化变量q的维度 _______|  |
//			 优化变量t的维度 __________|
			new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

使用点 $O$，$A$，$B$ 计算点到直线的距离残差，构造 Ceres 优化的残差块：
$$\text{Cost Function}=\text{LidarEdgeFactor}(O,A,B,s)$$
(1) 特征点匹配（点 $O$ 与点 $A$, $B$）
对于每个 corner_sharp 特征点 $O$，找到上一帧中最近的两个点 $A$ 和 $B$，并使用它们来构造点到直线的距离残差。

(2) 插值系数 $s$ 计算
运动补偿系数 $s$ 用于调整当前点的位置，以补偿由于扫描失真造成的影响。

(3)计算残差
对于点 $O$ 和最近邻的两个点 $A$ 和 $B$，构造点到直线的残差。具体步骤如下：

1. 计算四元数和平移的变换：
   使用四元数 $q$ 和平移向量 $t$ 对当前点 $O$ 进行变换，得到变换后的点 $lp$：
   $$lp=q\cdot O+t$$

2. 点到线的距离：
   计算变换后的点 $lp$ 到线 $AB$ 的距离。使用向量叉积和点积计算距离：
   $$\mathbf{nu}=(lp-A)\times (lp-B)$$
   $$\mathbf{de}=A-B$$

   最终的残差表示为：
   $$\text{residual}=\frac{\mathbf{nu}}{\Vert \mathbf{de}\Vert }$$

   该残差包含了三个分量：$residual[0]$, $residual[1]$, $residual[2]$，分别表示三维空间中的距离分量。在 Ceres 优化中，使用上述残差函数作为目标函数，并将其加入到优化问题中。每次迭代通过最小化该残差来优化激光雷达的位姿。

• 残差函数：使用 LidarEdgeFactor 创建残差函数，并将其添加到 Ceres 问题中：
  $$\text{Cost Function}=\text{LidarEdgeFactor}(O,A,B,s)$$

• 优化变量：优化的变量包括旋转四元数 $q$ 和平移向量 $t$，通过 Ceres 优化库对这些变量进行调整。

最终结果:优化过程中，通过计算点 $O$ 到线 $AB$ 的残差，并不断调整优化变量 $q$ 和 $t$，以最小化残差，从而提高位姿估计的精度。

2.2 基于最近邻原理建立面特征点之间的关联

// 计算Odometry线程中点到面的残差距离
struct LidarPlaneFactor
{
    
    
	LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
					 Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
		  last_point_m(last_point_m_), s(s_)
	{
    
    
		// 点l、j、m就是搜索到的最近邻的3个点，下面就是计算出这三个点构成的平面ljlm的法向量
		ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
		// 归一化法向量
		ljm_norm.normalize();
	}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
    
    

		Eigen::Matrix<T, 3, 1> cp{
    
    T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpj{
    
    T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
		//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
		//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
		Eigen::Matrix<T, 3, 1> ljm{
    
    T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
		Eigen::Quaternion<T> q_last_curr{
    
    q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{
    
    T(1), T(0), T(0), T(0)};
		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{
    
    T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;
		lp = q_last_curr * cp + t_last_curr;

		// 计算点到平面的残差距离，如下图所示
		residual[0] = (lp - lpj).dot(ljm);

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
									   const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
									   const double s_)
	{
    
    
		return (new ceres::AutoDiffCostFunction<
				LidarPlaneFactor, 1, 4, 3>(
//				 	              ^  ^  ^
//			 		              |  |  |
//			       残差的维度 ____|  |  |
//			  优化变量q的维度 _______|  |
//		 	  优化变量t的维度 __________|
			new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	Eigen::Vector3d ljm_norm;
	double s;
};

点到平面的残差计算（LidarPlaneFactor）

LidarPlaneFactor 结构体用于计算激光点到由三个点（$l$, $j$, $m$）构成的平面的残差。在Odometry线程中，点到平面的残差用于优化激光雷达的位姿。

1. 初始化平面法向量
给定三个点 $l$, $j$, $m$，首先计算这三点构成的平面的法向量 $\mathbf{n}$：
$$\mathbf{n}=({\mathbf{p}}_j-{\mathbf{p}}_l)\times ({\mathbf{p}}_j-{\mathbf{p}}_m)$$
其中，${\mathbf{p}}_l$, ${\mathbf{p}}_j$, 和 ${\mathbf{p}}_m$ 是上一帧的三个点，$\times$ 表示向量叉积。

然后，对法向量进行归一化处理：
$${\mathbf{n}}_{\text{normalized}}=\frac{\mathbf{n}}{\Vert \mathbf{n}\Vert }$$

2. 插值和旋转计算
通过四元数 $q$ 和平移向量 $t$ 对当前点进行旋转和平移，从当前帧坐标系变换到上一帧坐标系（或世界坐标系）：
$${\mathbf{p}}_{\text{last}}=q\cdot {\mathbf{p}}_{\text{curr}}+t$$
其中，${\mathbf{p}}_{\text{curr}}$ 是当前点，$q$ 是旋转四元数，$t$ 是平移向量。

3. 计算点到平面的残差
   点到平面的距离由点到平面法向量的点积计算得出：
   $$\text{residual}=({\mathbf{p}}_{\text{last}}-{\mathbf{p}}_j)\cdot {\mathbf{n}}_{\text{normalized}}$$
   其中，${\mathbf{p}}_{\text{last}}$ 是变换后的点，${\mathbf{p}}_j$ 是平面上的一个参考点，${\mathbf{n}}_{\text{normalized}}$ 是归一化后的平面法向量。

4. Ceres 优化与残差函数
在 Ceres 优化中，残差函数 $f(\mathbf{q},\mathbf{t})$ 被表示为点到平面距离：
$$f(\mathbf{q},\mathbf{t})=({\mathbf{p}}_{\text{last}}-{\mathbf{p}}_j)\cdot {\mathbf{n}}_{\text{normalized}}$$
通过最小化残差，优化目标是调整旋转四元数 $q$ 和平移向量 $t$，从而最小化点到平面的距离，进而优化激光雷达的位姿。

5. Ceres 创建残差函数
LidarPlaneFactor 使用 Ceres 的自动微分功能，构造残差函数，并将其添加到优化问题中：
$$\text{Cost Function}=\text{LidarPlaneFactor}(O,A,B,s)$$

---