STD三角描述子代码解析

---

一、算法介绍

中文博客：CSDN
源代码：STD

摘要——在这项工作中，我们提出了一种新颖的全局描述符，称为稳定三角形描述符（STD），用于三维地点识别。对于一个三角形，其形状由边长或夹角唯一确定。此外，三角形的形状对刚性变换完全不变。基于这一特性，我们首先设计了一种算法，从三维点云中高效提取局部关键点，并将这些关键点编码为三角形描述符。然后，通过匹配点云之间描述符的边长（以及其他一些信息）来实现地点识别。通过描述符匹配对获得的点对应关系可以进一步用于几何验证，从而大大提高地点识别的准确性。在我们的实验中，我们将所提出的系统与其他先进系统（即M2DP、ScanContext）在公共数据集（即KITTI、NCLT和Complex-Urban）以及我们自收集的数据集（使用非重复扫描的固态激光雷达）上进行了广泛比较。所有定量结果表明，STD在适应性方面更强，并且在精度上有显著提升。

算法框架

二、代码解析

在关键帧中提取关键点后，构建了一个k-D树，并为每个点搜索20个邻近点来形成三角形描述符。具有相同边长的冗余描述符将被消除。每个三角形描述符包含三个顶点 $p_1,p_2$ 和 $p_3$，以及投影法线向量 $n_1,n_2$ 和 $n_3$。此外，三角形的顶点按照边长升序排列（见图3）。我们总结三角形描述符 $\Delta$ 包含以下内容：

• $p1,p2,p3$：三个顶点，
• $n1,n2,n3$：三个投影法线向量，
• $l_{12},l_{23},l_{13}$：三条边，且 $l_{12}\le l_{23}\le l_{13}$，
• $q$：三角形的质心，
• $k$：与描述符对应的帧编号。
  除了描述符，我们还将保存从这个关键帧中提取的所有 $n$ 个平面 $\Pi =\left({\pi }_1,{\pi }_2,\dots ,{\pi }_n\right)$，以备后续的几何验证步骤使用。对应的代码为：

1.三角描述子定义

// 定义三角形描述符结构体STDesc，用于存储三角形的几何和属性信息
typedef struct STDesc {
    
    
  // 三角形的三条边长，从短到长依次排列
  Eigen::Vector3d side_length_;

  // 顶点之间的投影角度（即法向量之间的夹角）
  Eigen::Vector3d angle_;

  // 三角形质心坐标
  Eigen::Vector3d center_;

  // 该描述符所属的帧编号
  unsigned int frame_id_;

  // 三角形的三个顶点坐标
  Eigen::Vector3d vertex_A_;
  Eigen::Vector3d vertex_B_;
  Eigen::Vector3d vertex_C_;

  // 每个顶点的附加信息，例如强度值（intensity）
  Eigen::Vector3d vertex_attached_;
} STDesc;

2.平面的计算

代码首先初始化了协方差矩阵 $C$，中心点 $\vec{P}$ 和法向量 $\vec{N}$。然后，它遍历所有的体素点，计算协方差矩阵和中心点。使用特征值分解来找到协方差矩阵的三个特征向量和特征值，这些特征向量代表了数据的主方向，而特征值代表了这些方向上的分散程度。最小的特征值对应的特征向量被认为是平面的法向量，因为平面上的点在这个方向上的分散程度最小。如果最小特征值小于一个预设的阈值，那么认为这些点确实构成了一个平面，并计算平面的半径和截距。最后，根据这些计算结果设置平面模型的属性。

1. 初始化协方差矩阵 $C$ 和中心点 $\vec{P}$ 为零向量，法向量 $\vec{N}$ 也为零向量。
   $$C=0,\vec{P}=0,\vec{N}=0$$

2. 计算点的数量 $n$。

3. 对于每个点 ${\vec{p}}_i$ 在体素点集合中：

   • 累加协方差矩阵 $C$。
     $$C=C+{\vec{p}}_i{\vec{p}}_i^T$$
   • 累加中心点 $\vec{P}$。
     $$\vec{P}=\vec{P}+{\vec{p}}_i$$

4. 计算平均中心点 $\vec{P}$。
   $$\vec{P}=\frac{\vec{P}}{n}$$

5. 计算协方差矩阵 $C$。
   $$C=\frac{C}{n}-\vec{P}{\vec{P}}^T$$

6. 对协方差矩阵 $C$ 进行特征值分解，得到特征向量 $E$ 和特征值 $\Lambda$。
   $$E,\Lambda =\text{EigenSolver}(C)$$

7. 提取实数部分的特征值 ${\Lambda }_{\text{real}}$。
   $${\Lambda }_{\text{real}}=\text{Re}(\Lambda )$$

8. 找到最小和最大的特征值对应的索引 $\text{min}$ 和 $\text{max}$。
   $$\text{min}=\arg \min ({\Lambda }_{\text{real}}),\text{max}=\arg \max ({\Lambda }_{\text{real}})$$

9. 如果最小特征值小于平面检测阈值 $\text{plane\_detection\_thre}$：

   • 设置法向量 $\vec{N}$ 为对应最小特征值的特征向量。
     $$\vec{N}=E_{\text{real}}(:,\text{min})$$
   • 设置最小特征值为 ${\Lambda }_{\text{min}}$。
     $${\Lambda }_{\text{min}}={\Lambda }_{\text{real}}(\text{min})$$
   • 计算半径 $r$ 为最大特征值的平方根。
     $$r=\sqrt{{\Lambda }_{\text{real}}(\text{max})}$$
   • 设置平面标志 $\text{is\_plane}$ 为真。
     $$\text{is\_plane}=\text{true}$$
   • 计算截距 $b$。
     $$b=-(\vec{N}(0)\cdot \vec{P}(0)+\vec{N}(1)\cdot \vec{P}(1)+\vec{N}(2)\cdot \vec{P}(2))$$
   • 设置平面中心点 ${\vec{P}}_{\text{center}}$ 的坐标和法向量。
     $${\vec{P}}_{\text{center}}.x=\vec{P}(0),{\vec{P}}_{\text{center}}.y=\vec{P}(1),{\vec{P}}_{\text{center}}.z=\vec{P}(2)$$
     $${\vec{P}}_{\text{center}}.norma{l}_x=\vec{N}(0),{\vec{P}}_{\text{center}}.norma{l}_y=\vec{N}(1),{\vec{P}}_{\text{center}}.norma{l}_z=\vec{N}(2)$$
   • 否则，设置平面标志 $\text{is\_plane}$ 为假。
     $$\text{is\_plane}=\text{false}$$

// 定义一个函数 init_plane，用于初始化平面模型
void OctoTree::init_plane() {
    
    
  // 初始化协方差矩阵为零矩阵
  plane_ptr_->covariance_ = Eigen::Matrix3d::Zero();
  // 初始化平面的中心点为零向量
  plane_ptr_->center_ = Eigen::Vector3d::Zero();
  // 初始化平面的法向量为零向量
  plane_ptr_->normal_ = Eigen::Vector3d::Zero();
  // 记录体素点的数量
  plane_ptr_->points_size_ = voxel_points_.size();
  // 初始化平面的半径为0
  plane_ptr_->radius_ = 0;
  // 遍历所有的体素点
  for (auto pi : voxel_points_) {
    
    
    // 累加每个点的外积，用于计算协方差矩阵
    plane_ptr_->covariance_ += pi * pi.transpose();
    // 累加每个点的坐标，用于计算中心点
    plane_ptr_->center_ += pi;
  }
  // 计算平均中心点
  plane_ptr_->center_ = plane_ptr_->center_ / plane_ptr_->points_size_;
  // 计算协方差矩阵，减去中心点的贡献
  plane_ptr_->covariance_ = plane_ptr_->covariance_ / plane_ptr_->points_size_ -
                            plane_ptr_->center_ * plane_ptr_->center_.transpose();
  // 对协方差矩阵进行特征值分解
  Eigen::EigenSolver<Eigen::Matrix3d> es(plane_ptr_->covariance_);
  // 获取特征向量和特征值
  Eigen::Matrix3cd evecs = es.eigenvectors();
  Eigen::Vector3cd evals = es.eigenvalues();
  // 提取特征值的实数部分
  Eigen::Vector3d evalsReal = evals.real();
  // 找到最小和最大特征值的索引
  Eigen::Matrix3d::Index evalsMin, evalsMax;
  evalsReal.rowwise().sum().minCoeff(&evalsMin);
  evalsReal.rowwise().sum().maxCoeff(&evalsMax);
  // 计算中间特征值的索引
  int evalsMid = 3 - evalsMin - evalsMax;
  // 如果最小特征值小于预设的平面检测阈值，则认为找到了一个平面
  if (evalsReal(evalsMin) < config_setting_.plane_detection_thre_) {
    
    
    // 设置平面的法向量为对应最小特征值的特征向量
    plane_ptr_->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin),
                          evecs.real()(2, evalsMin);
    // 记录最小特征值
    plane_ptr_->min_eigen_value_ = evalsReal(evalsMin);
    // 计算平面的半径为最大特征值的平方根
    plane_ptr_->radius_ = sqrt(evalsReal(evalsMax));
    // 设置平面标志为真
    plane_ptr_->is_plane_ = true;
    // 计算平面的截距
    plane_ptr_->intercept_ = -(plane_ptr_->normal_(0) * plane_ptr_->center_(0) +
                               plane_ptr_->normal_(1) * plane_ptr_->center_(1) +
                               plane_ptr_->normal_(2) * plane_ptr_->center_(2));
    // 设置平面中心点的坐标和法向量
    plane_ptr_->p_center_.x = plane_ptr_->center_(0);
    plane_ptr_->p_center_.y = plane_ptr_->center_(1);
    plane_ptr_->p_center_.z = plane_ptr_->center_(2);
    plane_ptr_->p_center_.normal_x = plane_ptr_->normal_(0);
    plane_ptr_->p_center_.normal_y = plane_ptr_->normal_(1);
    plane_ptr_->p_center_.normal_z = plane_ptr_->normal_(2);
  } else {
    
    
    // 如果最小特征值不小于阈值，则认为没有找到平面
    plane_ptr_->is_plane_ = false;
  }
}

3.提取角点、特征点

图 5. (a) 边界体素中的点用黄色表示。 (b) 这些点投影到相邻平面上（蓝色点）。 © 平面图像，其中每个像素表示边界体素中点到平面的最大距离（以厘米为单位）。如果一个点在其 5×5 邻域内具有最大像素值，它将被视为关键点（红色点）。

（1）建立连接关系build_connection

函数 STDescManager::build_connection 主要用于构建一个体素映射中各个体素之间的连接关系。具体功能如下：

1. 遍历体素映射：函数遍历输入的 voxel_map，该映射将 VOXEL_LOC 类型的位置映射到对应的 OctoTree 对象。

2. 检查平面体素：对于每个体素，如果其对应的 OctoTree 对象的 plane_ptr_->is_plane_ 属性为 true，则认为该体素代表一个平面。

3. 遍历相邻方向：对于每个平面体素，函数遍历其六个可能的相邻方向（上、下、左、右、前、后）。

4. 查找相邻体素：对于每个方向，函数在 voxel_map 中查找对应的相邻体素。

5. 建立连接：

   • 如果相邻体素存在，并且也代表一个平面，则根据两个平面的法向量是否接近（通过范数比较）来决定是否建立连接。
   • 如果法向量的差或和的范数小于预设阈值 config_setting_.plane_merge_normal_thre_，则认为两个平面相近，可以合并，并建立双向连接。
   • 如果相邻体素不代表一个平面，则只将当前体素指向相邻体素，但不形成双向连接。

6. 标记连接状态：函数更新 OctoTree 对象的 connect_、is_check_connect_ 和 connect_tree_ 属性来记录连接状态和指向相邻体素的指针。

通过这个函数，可以构建一个体素之间的连接网络，为后续的处理（如平面合并、特征提取等）提供基础。

void STDescManager::build_connection(  
    std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map)  
{
    
      
  // 遍历体素映射中的每一个元素  
  for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++)  
  {
    
      
    // 如果当前八叉树代表一个平面  
    if (iter->second->plane_ptr_->is_plane_)   
    {
    
      
      OctoTree *current_octo = iter->second; // 获取当前八叉树的指针  
        
      // 遍历当前八叉树的6个相邻方向  
      for (int i = 0; i < 6; i++)  
      {
    
      
        VOXEL_LOC neighbor = iter->first; // 复制当前体素的位置作为相邻体素的位置  
          
        // 根据i的值设置相邻体素的位置  
        // i的0-2分别代表x, y, z轴的正方向，3-5分别代表负方向  
        if (i == 0) {
    
     neighbor.x += 1; }  
        else if (i == 1) {
    
     neighbor.y += 1; }  
        else if (i == 2) {
    
     neighbor.z += 1; }  
        else if (i == 3) {
    
     neighbor.x -= 1; }  
        else if (i == 4) {
    
     neighbor.y -= 1; }  
        else if (i == 5) {
    
     neighbor.z -= 1; }  
          
        // 在体素映射中查找相邻体素  
        auto near = voxel_map.find(neighbor);  
          
        // 如果没有找到相邻体素  
        if (near == voxel_map.end())   
        {
    
      
          // 标记当前八叉树在i方向上的连接为未连接且已检查  
          current_octo->is_check_connect_[i] = true;  
          current_octo->connect_[i] = false;  
        }   
        else   
        {
    
      
          // 如果当前八叉树在i方向上还未检查过连接  
          if (!current_octo->is_check_connect_[i])   
          {
    
      
            OctoTree *near_octo = near->second; // 获取相邻八叉树的指针  
              
            // 标记当前八叉树和相邻八叉树在相应方向上的连接为已检查  
            current_octo->is_check_connect_[i] = true;  
            int j = (i >= 3) ? (i - 3) : (i + 3); // 计算相邻八叉树对应的反向索引  
            near_octo->is_check_connect_[j] = true;  
              
            // 如果相邻八叉树也代表一个平面  
            if (near_octo->plane_ptr_->is_plane_)   
            {
    
      
              // 计算两个平面法向量的差和和  
              Eigen::Vector3d normal_diff = current_octo->plane_ptr_->normal_ - near_octo->plane_ptr_->normal_;  
              Eigen::Vector3d normal_add = current_octo->plane_ptr_->normal_ + near_octo->plane_ptr_->normal_;  
                
              // 如果法向量的差或和的范数小于某个阈值，则认为两个平面相近，可以合并  
              if (normal_diff.norm() < config_setting_.plane_merge_normal_thre_ ||  
                  normal_add.norm() < config_setting_.plane_merge_normal_thre_)   
              {
    
      
                // 标记两个八叉树在相应方向上的连接为已连接，并互相指向对方  
                current_octo->connect_[i] = true;  
                near_octo->connect_[j] = true;  
                current_octo->connect_tree_[i] = near_octo;  
                near_octo->connect_tree_[j] = current_octo;  
              }   
              else   
              {
    
      
                // 否则，标记为未连接  
                current_octo->connect_[i] = false;  
                near_octo->connect_[j] = false;  
              }  
            }   
            else   
            {
    
      
              // 如果相邻八叉树不代表一个平面，则只将当前八叉树指向相邻八叉树，但不形成双向连接  
              current_octo->connect_[i] = false;  
              near_octo->connect_[j] = true;  
              near_octo->connect_tree_[j] = current_octo;  
            }  
          }  
        }  
      }  
    }  
  }  
}

（2）角点提取 (STDescManager::corner_extractor)

函数 STDescManager::corner_extractor 旨在从给定的体素映射和点云数据中提取角点。其主要步骤和功能如下：

1. 初始化临时点云：创建一个临时点云 prepare_corner_points 用于存储准备提取的角点。

2. 定义体素偏移量：为了避免不同视角导致的体素切割不一致，定义了一个体素偏移量的列表 voxel_round，包含-1到1的x、y、z方向上的偏移量。

3. 遍历体素映射：遍历输入的体素映射 voxel_map，对于每个不包含平面的体素，获取其位置和对应的 OctoTree 指针。

4. 检查体素连接：对于每个体素，遍历其六个方向的连接，如果找到有连接的体素且该连接体素代表一个平面，则进行进一步处理。

5. 投影和角点提取：

   • 将当前体素及其周围偏移体素投影到相邻平面上。
   • 使用 extract_corner 函数在投影点上提取角点，并将提取的角点添加到 prepare_corner_points 点云中。

6. 非极大值抑制：对准备的角点进行非极大值抑制，以减少角点数量并增强角点的显著性。

7. 选择角点：

   • 如果准备的角点数量小于配置的最大角点数量，则直接将这些角点赋值给输出的角点云 corner_points。
   • 如果准备的角点数量大于配置的最大角点数量，则根据角点的强度进行排序，并选择前 maximum_corner_num_ 个角点作为最终的角点。

void STDescManager::corner_extractor(  
    std::unordered_map<VOXEL_LOC, OctoTree *> &voxel_map, // 输入的体素映射，键是体素位置，值是OctoTree指针  
    const pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud, // 输入的点云数据  
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points) {
    
     // 输出的角点云数据  
  
  pcl::PointCloud<pcl::PointXYZINormal>::Ptr prepare_corner_points(  
      new pcl::PointCloud<pcl::PointXYZINormal>); // 用于准备角点的临时点云  
  
  // 避免由于不同视角导致的体素切割不一致  
  std::vector<Eigen::Vector3i> voxel_round; // 存储体素周围偏移量的向量  
  for (int x = -1; x <= 1; x++) {
    
     // 遍历x方向的偏移量  
    for (int y = -1; y <= 1; y++) {
    
     // 遍历y方向的偏移量  
      for (int z = -1; z <= 1; z++) {
    
     // 遍历z方向的偏移量  
        Eigen::Vector3i voxel_inc(x, y, z); // 创建当前偏移量的Eigen向量  
        voxel_round.push_back(voxel_inc); // 将偏移量添加到列表中  
      }  
    }  
  }  
  for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) 
  {
    
     // 遍历体素映射  
    if (!iter->second->plane_ptr_->is_plane_)
    {
    
     // 如果当前体素不包含平面  
      VOXEL_LOC current_position = iter->first; // 获取当前体素的位置  
      OctoTree *current_octo = iter->second; // 获取当前OctoTree指针  
      int connect_index = -1; // 初始化连接索引  
      for (int i = 0; i < 6; i++) 
      {
    
     // 遍历6个方向的连接  
        if (current_octo->connect_[i]) 
        {
    
     // 如果当前方向有连接  
          connect_index = i; // 记录连接索引  
          OctoTree *connect_octo = current_octo->connect_tree_[connect_index]; // 获取连接的OctoTree  
          bool use = false; // 初始化使用标志  
          for (int j = 0; j < 6; j++) 
          {
    
     // 遍历连接OctoTree的6个方向的连接  
            if (connect_octo->is_check_connect_[j]) 
            {
    
     // 如果当前方向需要检查连接  
              if (connect_octo->connect_[j])
              {
    
     // 如果该方向有连接  
                use = true; // 设置使用标志为true  
              }  
            }  
          }  
          // 如果没有相邻平面，跳过  
          if (use == false) 
          {
    
       
            continue;  
          }  
          // 只投影包含超过10个点的体素  
          if (current_octo->voxel_points_.size() > 10)
          {
    
      
            Eigen::Vector3d projection_normal =  
                current_octo->connect_tree_[connect_index]->plane_ptr_->normal_; // 获取投影平面的法向量  
            Eigen::Vector3d projection_center =  
                current_octo->connect_tree_[connect_index]->plane_ptr_->center_; // 获取投影平面的中心点  
            std::vector<Eigen::Vector3d> proj_points; // 存储投影点的向量  
            // 将边界体素和附近体素投影到相邻平面上  
            for (auto voxel_inc : voxel_round) 
            {
    
      
              VOXEL_LOC connect_project_position = current_position; // 获取当前体素位置  
              connect_project_position.x += voxel_inc[0]; // 应用x方向偏移  
              connect_project_position.y += voxel_inc[1]; // 应用y方向偏移  
              connect_project_position.z += voxel_inc[2]; // 应用z方向偏移  
              auto iter_near = voxel_map.find(connect_project_position); // 在体素映射中查找投影位置  
              if (iter_near != voxel_map.end()) 
              {
    
     // 如果找到了投影位置  
                bool skip_flag = false; // 初始化跳过标志  
                if (!voxel_map[connect_project_position]  
                         ->plane_ptr_->is_plane_) {
    
     // 如果投影位置不包含平面  
                  if (voxel_map[connect_project_position]->is_project_) 
                  {
    
     // 如果已经投影过  
                    for (auto normal : voxel_map[connect_project_position]  
                                           ->proj_normal_vec_) {
    
     // 遍历已投影的法向量  
                      Eigen::Vector3d normal_diff = projection_normal - normal; // 计算法向量差异  
                      Eigen::Vector3d normal_add = projection_normal + normal; // 计算法向量和  
                      // 检查是否重复投影  
                      if (normal_diff.norm() < 0.5 || normal_add.norm() < 0.5) 
                      {
    
      
                        skip_flag = true; // 设置跳过标志为true  
                      }  
                    }  
                  }  
                  if (skip_flag) 
                  {
    
      
                    continue; // 如果需要跳过，继续下一次循环  
                  }  
                  // 将投影点添加到列表中，并标记为已投影  
                  for (size_t j = 0; j < voxel_map[connect_project_position]  
                                             ->voxel_points_.size();  
                       j++) {
    
      
                    proj_points.push_back(  
                        voxel_map[connect_project_position]->voxel_points_[j]);  
                    voxel_map[connect_project_position]->is_project_ = true;  
                    voxel_map[connect_project_position]  
                        ->proj_normal_vec_.push_back(projection_normal);  
                  }  
                }  
              }  
            }  
            // 在这里进行2D投影和角点提取  
            pcl::PointCloud<pcl::PointXYZINormal>::Ptr sub_corner_points(  
                new pcl::PointCloud<pcl::PointXYZINormal>); // 创建临时角点点云  
            extract_corner(projection_center, projection_normal, proj_points,  
                           sub_corner_points); // 提取角点  
            for (auto pi : sub_corner_points->points) {
    
     // 将提取的角点添加到准备角点的点云中  
              prepare_corner_points->push_back(pi);  
            }  
          }  
        }  
      }  
    }  
  }  

  //强度处理
  non_maxi_suppression(prepare_corner_points); // 进行非极大值抑制  
  // 根据配置设置的最大角点数量，选择是否直接赋值或进行排序选择  
  if (config_setting_.maximum_corner_num_ > prepare_corner_points->size())
  {
    
      
    corner_points = prepare_corner_points; // 如果准备角点的数量小于最大数量，直接赋值  
  } 
  else 
  {
    
      
    std::vector<std::pair<double, int>> attach_vec; // 存储角点强度和索引的向量  
    for (size_t i = 0; i < prepare_corner_points->size(); i++) 
    {
    
     // 遍历准备角点的点云  
      attach_vec.push_back(std::pair<double, int>(prepare_corner_points->points[i].intensity, i)); // 将角点强度和索引添加到向量中  
    }  
    std::sort(attach_vec.begin(), attach_vec.end(), attach_greater_sort); // 按强度降序排序  
    for (size_t i = 0; i < config_setting_.maximum_corner_num_; i++) 
    {
    
     // 根据最大角点数量选择角点  
      corner_points->points.push_back(prepare_corner_points->points[attach_vec[i].second]);  
    }  
  }  
}

（3）角点提取核心函数extract_corner

该函数STDescManager::extract_corner主要执行以下任务：它根据提供的投影中心和法线计算出用于定义投影平面的参数，并根据这些参数将3D点投影到2D平面上。函数通过梯度计算识别出潜在的角点，并排除那些可能是直线上的部分。它将这些角点从2D平面重新映射回3D空间，并存储在输出的点云对象中，以便后续处理。这个过程涉及到距离阈值的设定、2D坐标的转换、梯度的计算以及角点的筛选和重新投影。

1. 初始化和参数准备：

   • 获取投影图像的分辨率 $\text{resolution}$、最小距离阈值 $\text{dis\_threshold\_min}$ 和最大距离阈值 $\text{dis\_threshold\_max}$。
   • 提取投影法线的系数 $\text{A}=\text{proj\_normal}[0]$，$\text{B}=\text{proj\_normal}[1]$，$\text{C}=\text{proj\_normal}[2]$，并计算 $\text{D}=-(\text{A}\cdot \text{proj\_center}[0]+\text{B}\cdot \text{proj\_center}[1]+\text{C}\cdot \text{proj\_center}[2])$。

2. 计算点到平面的距离和投影：

   • 计算每个点到投影平面的距离 $\text{dis}=|x\cdot \text{A}+y\cdot \text{B}+z\cdot \text{C}+\text{D}|$，并过滤超出阈值的点。
   • 对有效的点计算其在投影平面上的投影坐标，并转换为2D坐标。

3. 确定2D点集边界和分割：

   • 找出2D点集的边界，计算分割的数量，并初始化存储图像数据的容器。

4. 分配点到分割并计算均值坐标和梯度：

   • 将2D点分配到对应的分割中，并计算每个分割的均值坐标和计数。
   • 计算每个分割的梯度，用于后续的角点检测。

5. 提取角点并过滤直线：

   • 在每个分割中寻找梯度最大的点作为角点，并过滤掉直线上的点。

6. 将角点重新投影到3D空间并输出：

   • 将2D角点坐标重新投影到3D空间，并添加到输出点云中。对于每个角点，计算其在3D空间中的坐标 $\text{coord}=py\cdot \text{x\_axis}+px\cdot \text{y\_axis}+\text{proj\_center}$。

void STDescManager::extract_corner(
    const Eigen::Vector3d &proj_center, const Eigen::Vector3d proj_normal,
    const std::vector<Eigen::Vector3d> proj_points,
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points)
{
    
    

  // 从配置中获取投影图像的分辨率、最小和最大距离阈值
  double resolution = config_setting_.proj_image_resolution_;
  double dis_threshold_min = config_setting_.proj_dis_min_;
  double dis_threshold_max = config_setting_.proj_dis_max_;

  // 提取投影法线的系数
  double A = proj_normal[0];
  double B = proj_normal[1];
  double C = proj_normal[2];
  double D = -(A * proj_center[0] + B * proj_center[1] + C * proj_center[2]);

  // 定义x轴向量，并根据投影法线的方向调整x轴向量
  Eigen::Vector3d x_axis(1, 1, 0);
  if (C != 0)
  {
    
    
    x_axis[2] = -(A + B) / C;
  }
  else if (B != 0)
  {
    
    
    x_axis[1] = -A / B;
  }
  else
  {
    
    
    x_axis[0] = 0;
    x_axis[1] = 1;
  }
  x_axis.normalize();                                 // 归一化x轴向量
  Eigen::Vector3d y_axis = proj_normal.cross(x_axis); // 计算与投影法线正交的y轴向量
  y_axis.normalize();                                 // 归一化y轴向量

  // 计算2D坐标转换所需的系数
  double ax = x_axis[0];
  double bx = x_axis[1];
  double cx = x_axis[2];
  double dx = -(ax * proj_center[0] + bx * proj_center[1] + cx * proj_center[2]);
  double ay = y_axis[0];
  double by = y_axis[1];
  double cy = y_axis[2];
  double dy = -(ay * proj_center[0] + by * proj_center[1] + cy * proj_center[2]);

  // 存储2D点的容器
  std::vector<Eigen::Vector2d> point_list_2d;
  for (size_t i = 0; i < proj_points.size(); i++)
  {
    
    
    double x = proj_points[i][0];
    double y = proj_points[i][1];
    double z = proj_points[i][2];
    double dis = fabs(x * A + y * B + z * C + D); // 计算点到平面的距离
    if (dis < dis_threshold_min || dis > dis_threshold_max)
    {
    
    
      continue; // 根据距离阈值过滤点
    }
    Eigen::Vector3d cur_project; // 当前点在投影平面上的投影

    // 计算点在投影平面上的投影坐标
    cur_project[0] = (-A * (B * y + C * z + D) + x * (B * B + C * C)) / (A * A + B * B + C * C);
    cur_project[1] = (-B * (A * x + C * z + D) + y * (A * A + C * C)) / (A * A + B * B + C * C);
    cur_project[2] = (-C * (A * x + B * y + D) + z * (A * A + B * B)) / (A * A + B * B + C * C);
    pcl::PointXYZ p; // PCL点类型
    p.x = cur_project[0];
    p.y = cur_project[1];
    p.z = cur_project[2];
    double project_x = cur_project[0] * ay + cur_project[1] * by + cur_project[2] * cy + dy; // 计算2D坐标
    double project_y = cur_project[0] * ax + cur_project[1] * bx + cur_project[2] * cx + dx;
    Eigen::Vector2d p_2d(project_x, project_y); // 存储2D坐标
    point_list_2d.push_back(p_2d);              // 添加到2D点容器中
  }

  // 找出2D点集的边界
  double min_x = 10;
  double max_x = -10;
  double min_y = 10;
  double max_y = -10;
  if (point_list_2d.size() <= 5)
  {
    
    
    return; // 如果2D点的数量太少，则直接返回
  }
  for (auto pi : point_list_2d)
  {
    
    
    if (pi[0] < min_x)
    {
    
    
      min_x = pi[0];
    }
    if (pi[0] > max_x)
    {
    
    
      max_x = pi[0];
    }
    if (pi[1] < min_y)
    {
    
    
      min_y = pi[1];
    }
    if (pi[1] > max_y)
    {
    
    
      max_y = pi[1];
    }
  }

  // 根据分辨率和边界计算分割的数量
  int segmen_base_num = 5;
  double segmen_len = segmen_base_num * resolution;
  int x_segment_num = (max_x - min_x) / segmen_len + 1;
  int y_segment_num = (max_y - min_y) / segmen_len + 1;
  int x_axis_len = (int)((max_x - min_x) / resolution + segmen_base_num);
  int y_axis_len = (int)((max_y - min_y) / resolution + segmen_base_num);

  // 初始化用于存储图像数据的容器
  std::vector<Eigen::Vector2d> img_container[x_axis_len][y_axis_len];
  double img_count_array[x_axis_len][y_axis_len] = {
    
    0};
  double gradient_array[x_axis_len][y_axis_len] = {
    
    0};
  double mean_x_array[x_axis_len][y_axis_len] = {
    
    0};
  double mean_y_array[x_axis_len][y_axis_len] = {
    
    0};
  for (int x = 0; x < x_axis_len; x++)
  {
    
    
    for (int y = 0; y < y_axis_len; y++)
    {
    
    
      img_count_array[x][y] = 0;
      mean_x_array[x][y] = 0;
      mean_y_array[x][y] = 0;
      gradient_array[x][y] = 0;
      std::vector<Eigen::Vector2d> single_container;
      img_container[x][y] = single_container;
    }
  }

  // 分配点到分割中，并计算每个分割的均值坐标和计数
  for (size_t i = 0; i < point_list_2d.size(); i++)
  {
    
    
    int x_index = (int)((point_list_2d[i][0] - min_x) / resolution);
    int y_index = (int)((point_list_2d[i][1] - min_y) / resolution);
    mean_x_array[x_index][y_index] += point_list_2d[i][0];
    mean_y_array[x_index][y_index] += point_list_2d[i][1];
    img_count_array[x_index][y_index]++;
    img_container[x_index][y_index].push_back(point_list_2d[i]);
  }

  // 计算梯度
  for (int x = 0; x < x_axis_len; x++)
  {
    
    
    for (int y = 0; y < y_axis_len; y++)
    {
    
    
      double gradient = 0;
      int cnt = 0;
      int inc = 1;
      for (int x_inc = -inc; x_inc <= inc; x_inc++)
      {
    
    
        for (int y_inc = -inc; y_inc <= inc; y_inc++)
        {
    
    
          int xx = x + x_inc;
          int yy = y + y_inc;
          if (xx >= 0 && xx < x_axis_len && yy >= 0 && yy < y_axis_len)
          {
    
    
            if (xx != x || yy != y)
            {
    
    
              if (img_count_array[xx][yy] >= 0)
              {
    
    
                gradient += img_count_array[x][y] - img_count_array[xx][yy];
                cnt++;
              }
            }
          }
        }
      }
      if (cnt != 0)
      {
    
    
        gradient_array[x][y] = gradient * 1.0 / cnt;
      }
      else
      {
    
    
        gradient_array[x][y] = 0;
      }
    }
  }
  // 通过梯度提取角点
  std::vector<int> max_gradient_vec;         // 存储最大梯度值的向量
  std::vector<int> max_gradient_x_index_vec; // 存储最大梯度对应的x坐标索引的向量
  std::vector<int> max_gradient_y_index_vec; // 存储最大梯度对应的y坐标索引的向量

  // 在每个分割中寻找梯度最大的点
  for (int x_segment_index = 0; x_segment_index < x_segment_num; x_segment_index++)
  {
    
    
    for (int y_segment_index = 0; y_segment_index < y_segment_num; y_segment_index++)
    {
    
    
      double max_gradient = 0;        // 初始化最大梯度值
      int max_gradient_x_index = -10; // 初始化最大梯度的x坐标索引
      int max_gradient_y_index = -10; // 初始化最大梯度的y坐标索引

      // 在当前分割的每个子区域内寻找梯度最大的点
      for (int x_index = x_segment_index * segmen_base_num; x_index < (x_segment_index + 1) * segmen_base_num; x_index++)
      {
    
    
        for (int y_index = y_segment_index * segmen_base_num; y_index < (y_segment_index + 1) * segmen_base_num; y_index++)
        {
    
    
          // 如果当前点的梯度值大于已知的最大梯度值，则更新最大梯度值及其坐标索引
          if (img_count_array[x_index][y_index] > max_gradient)
          {
    
    
            max_gradient = img_count_array[x_index][y_index];
            max_gradient_x_index = x_index;
            max_gradient_y_index = y_index;
          }
        }
      }

      // 如果最大梯度值大于配置的角点阈值，则认为该点是角点
      if (max_gradient >= config_setting_.corner_thre_)
      {
    
    
        max_gradient_vec.push_back(max_gradient);
        max_gradient_x_index_vec.push_back(max_gradient_x_index);
        max_gradient_y_index_vec.push_back(max_gradient_y_index);
      }
    }
  }

  // 过滤掉直线
  // 计算直线或非直线
  std::vector<Eigen::Vector2i> direction_list; // 存储方向向量的向量
  Eigen::Vector2i d(0, 1);                     // 方向向量(0,1)
  direction_list.push_back(d);
  d << 1, 0; // 方向向量(1,0)
  direction_list.push_back(d);
  d << 1, 1; // 方向向量(1,1)
  direction_list.push_back(d);
  d << 1, -1; // 方向向量(1,-1)
  direction_list.push_back(d);

  // 对于每个最大梯度值对应的点，检查其是否为直线
  for (size_t i = 0; i < max_gradient_vec.size(); i++)
  {
    
    
    bool is_add = true; // 标记是否添加该点作为角点
    for (int j = 0; j < 4; j++)
    {
    
                                                                                  // 检查四个方向
      Eigen::Vector2i p(max_gradient_x_index_vec[i], max_gradient_y_index_vec[i]); // 当前点的坐标
      Eigen::Vector2i p1 = p + direction_list[j];                                  // 沿方向j的点
      Eigen::Vector2i p2 = p - direction_list[j];                                  // 沿方向-j的点
      int threshold = img_count_array[p[0]][p[1]] / 2;                             // 阈值

      // 如果沿方向j和-j的点的梯度值都大于阈值，则认为当前点可能是直线上的点
      if (img_count_array[p1[0]][p1[1]] >= threshold &&
          img_count_array[p2[0]][p2[1]] >= threshold)
      {
    
    
        // is_add = false; // 注释掉，因为我们不在这里直接修改is_add
      }
      else
      {
    
    
        continue; // 如果不是直线，则跳过
      }
    }

    // 如果当前点不是直线，则将其添加到角点集合中
    if (is_add)
    {
    
    
      double px = mean_x_array[max_gradient_x_index_vec[i]][max_gradient_y_index_vec[i]] / img_count_array[max_gradient_x_index_vec[i]][max_gradient_y_index_vec[i]]; // 计算平均x坐标
      double py = mean_y_array[max_gradient_x_index_vec[i]][max_gradient_y_index_vec[i]] / img_count_array[max_gradient_x_index_vec[i]][max_gradient_y_index_vec[i]]; // 计算平均y坐标

      // 将2D角点重新投影到3D空间
      Eigen::Vector3d coord = py * x_axis + px * y_axis + proj_center;
      pcl::PointXYZINormal pi; // PCL点类型，用于存储3D角点信息
      pi.x = coord[0];
      pi.y = coord[1];
      pi.z = coord[2];
      pi.intensity = max_gradient_vec[i];  // 角点强度设置为梯度值
      pi.normal_x = proj_normal[0];        // 法线x分量
      pi.normal_y = proj_normal[1];        // 法线y分量
      pi.normal_z = proj_normal[2];        // 法线z分量
      corner_points->points.push_back(pi); // 将3D角点添加到输出点云中
    }
  }
  return; // 函数结束
}

4.三角描述符计算过程(STDescManager::build_stdesc)

1. 参数初始化
   定义比例因子 $\text{scale}$，近邻数量 $\text{near\_num}$，边长最大/最小阈值：

   $$\text{scale}=\frac{1.0}{\text{config\_setting\_}.std\_side\_resolution\_}$$
   $$\text{near\_num}=\text{config\_setting\_}.descriptor\_near\_num\_$$
   $$\text{max\_dis\_threshold}=\text{config\_setting\_}.descriptor\_max\_len\_$$
   $$\text{min\_dis\_threshold}=\text{config\_setting\_}.descriptor\_min\_len\_$$

2. 邻近点搜索
   使用 k-D 树查找每个角点的最近 near_num 个邻居点，形成多个三角形组合。

3. 三角形顶点与边长计算
   对于每个组合的三个点 $p1$、$p2$ 和 $p3$，计算三角形的边长 $a$、$b$、$c$：

   $$a=\sqrt{(p1.x-p2.x{)}^2+(p1.y-p2.y{)}^2+(p1.z-p2.z{)}^2}$$
   $$b=\sqrt{(p1.x-p3.x{)}^2+(p1.y-p3.y{)}^2+(p1.z-p3.z{)}^2}$$
   $$c=\sqrt{(p3.x-p2.x{)}^2+(p3.y-p2.y{)}^2+(p3.z-p2.z{)}^2}$$

4. 边长筛选与排序

   • 筛选出满足最大/最小阈值条件的三角形。
   • 按照边长顺序将 $a$、$b$、$c$ 排序，使 $a\le b\le c$。

5. 三角形描述符构建

   • 顶点排序与法向量计算：根据边长和点法向量，定义三角形的三个顶点 $A$、$B$、$C$ 及其对应的法向量。

   • 质心计算：
     $$\text{center}=\frac{A+B+C}{3}$$

   • 边长存储：
     $$\text{side\_length}=\left(\text{scale}\cdot a,\text{scale}\cdot b,\text{scale}\cdot c\right)$$

   • 投影角度计算：
     计算相邻法向量的点积，得到三角形的角度属性：
     $$\text{angle}[0]=|5\cdot \text{normal\_1}\cdot \text{normal\_2}|$$
     $$\text{angle}[1]=|5\cdot \text{normal\_1}\cdot \text{normal\_3}|$$
     $$\text{angle}[2]=|5\cdot \text{normal\_3}\cdot \text{normal\_2}|$$

6. 结果存储
   生成的三角形描述符 single_descriptor 包含顶点、质心、边长、角度和 frame_id，将其添加到 stds_vec 中。

// 函数STDescManager::build_stdesc：用于构建三角形描述符
void STDescManager::build_stdesc(
    const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &corner_points, // 输入关键点
    std::vector<STDesc> &stds_vec) {
    
     // 输出三角形描述符的向量
  stds_vec.clear(); // 清空输出向量，准备存储新的描述符

  // 配置参数
  double scale = 1.0 / config_setting_.std_side_resolution_; // 描述符的边长缩放因子
  int near_num = config_setting_.descriptor_near_num_; // 每个点附近的邻居点数
  double max_dis_threshold = config_setting_.descriptor_max_len_; // 边长的最大阈值
  double min_dis_threshold = config_setting_.descriptor_min_len_; // 边长的最小阈值

  std::unordered_map<VOXEL_LOC, bool> feat_map; // 用于存储已存在的三角形描述符
  pcl::KdTreeFLANN<pcl::PointXYZINormal>::Ptr kd_tree(new pcl::KdTreeFLANN<pcl::PointXYZINormal>); // k-D树用于最近邻搜索
  kd_tree->setInputCloud(corner_points); // 将关键点集合设置为kd树输入

  std::vector<int> pointIdxNKNSearch(near_num); // 存储最近邻点的索引
  std::vector<float> pointNKNSquaredDistance(near_num); // 存储最近邻点的平方距离

  // 遍历每个关键点，找到附近的20个邻近点构成三角形描述符
  for (size_t i = 0; i < corner_points->size(); i++) {
    
    
    pcl::PointXYZINormal searchPoint = corner_points->points[i]; // 当前搜索点
    // 执行最近邻搜索
    if (kd_tree->nearestKSearch(searchPoint, near_num, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
    
    
      // 遍历组合邻近点，以构造三角形
      for (int m = 1; m < near_num - 1; m++) {
    
    
        for (int n = m + 1; n < near_num; n++) {
    
    
          // 选取三角形的三个顶点p1、p2、p3
          pcl::PointXYZINormal p1 = searchPoint;
          pcl::PointXYZINormal p2 = corner_points->points[pointIdxNKNSearch[m]];
          pcl::PointXYZINormal p3 = corner_points->points[pointIdxNKNSearch[n]];

          // 计算法线向量差值与和
          Eigen::Vector3d normal_inc1(p1.normal_x - p2.normal_x, p1.normal_y - p2.normal_y, p1.normal_z - p2.normal_z);
          Eigen::Vector3d normal_inc2(p3.normal_x - p2.normal_x, p3.normal_y - p2.normal_y, p3.normal_z - p2.normal_z);
          Eigen::Vector3d normal_add1(p1.normal_x + p2.normal_x, p1.normal_y + p2.normal_y, p1.normal_z + p2.normal_z);
          Eigen::Vector3d normal_add2(p3.normal_x + p2.normal_x, p3.normal_y + p2.normal_y, p3.normal_z + p2.normal_z);

          // 计算边长a, b, c
          double a = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
          double b = sqrt(pow(p1.x - p3.x, 2) + pow(p1.y - p3.y, 2) + pow(p1.z - p3.z, 2));
          double c = sqrt(pow(p3.x - p2.x, 2) + pow(p3.y - p2.y, 2) + pow(p3.z - p2.z, 2));

          // 判断边长是否在阈值范围内，不在则跳过
          if (a > max_dis_threshold || b > max_dis_threshold || c > max_dis_threshold ||
              a < min_dis_threshold || b < min_dis_threshold || c < min_dis_threshold) {
    
    
            continue;
          }

          // 按边长对顶点重新排序，使得 a <= b <= c
          double temp;
          Eigen::Vector3d A, B, C;
          Eigen::Vector3i l1, l2, l3, l_temp;
          l1 << 1, 2, 0;
          l2 << 1, 0, 3;
          l3 << 0, 2, 3;
          if (a > b) {
    
     temp = a; a = b; b = temp; l_temp = l1; l1 = l2; l2 = l_temp; }
          if (b > c) {
    
     temp = b; b = c; c = temp; l_temp = l2; l2 = l3; l3 = l_temp; }
          if (a > b) {
    
     temp = a; a = b; b = temp; l_temp = l1; l1 = l2; l2 = l_temp; }

          // 计算体素位置并检查重复
          pcl::PointXYZ d_p;
          d_p.x = a * 1000;
          d_p.y = b * 1000;
          d_p.z = c * 1000;
          VOXEL_LOC position((int64_t)d_p.x, (int64_t)d_p.y, (int64_t)d_p.z);
          auto iter = feat_map.find(position);
          if (iter == feat_map.end()) {
    
     // 新的三角形，构建描述符
            Eigen::Vector3d vertex_attached;
            if (l1[0] == l2[0]) {
    
     A << p1.x, p1.y, p1.z; normal_1 << p1.normal_x, p1.normal_y, p1.normal_z; vertex_attached[0] = p1.intensity; }
            else if (l1[1] == l2[1]) {
    
     A << p2.x, p2.y, p2.z; normal_1 << p2.normal_x, p2.normal_y, p2.normal_z; vertex_attached[0] = p2.intensity; }
            else {
    
     A << p3.x, p3.y, p3.z; normal_1 << p3.normal_x, p3.normal_y, p3.normal_z; vertex_attached[0] = p3.intensity; }

            if (l1[0] == l3[0]) {
    
     B << p1.x, p1.y, p1.z; normal_2 << p1.normal_x, p1.normal_y, p1.normal_z; vertex_attached[1] = p1.intensity; }
            else if (l1[1] == l3[1]) {
    
     B << p2.x, p2.y, p2.z; normal_2 << p2.normal_x, p2.normal_y, p2.normal_z; vertex_attached[1] = p2.intensity; }
            else {
    
     B << p3.x, p3.y, p3.z; normal_2 << p3.normal_x, p3.normal_y, p3.normal_z; vertex_attached[1] = p3.intensity; }

            if (l2[0] == l3[0]) {
    
     C << p1.x, p1.y, p1.z; normal_3 << p1.normal_x, p1.normal_y, p1.normal_z; vertex_attached[2] = p1.intensity; }
            else if (l2[1] == l3[1]) {
    
     C << p2.x, p2.y, p2.z; normal_3 << p2.normal_x, p2.normal_y, p2.normal_z; vertex_attached[2] = p2.intensity; }
            else {
    
     C << p3.x, p3.y, p3.z; normal_3 << p3.normal_x, p3.normal_y, p3.normal_z; vertex_attached[2] = p3.intensity; }

            // 保存描述符信息
            STDesc single_descriptor;
            single_descriptor.vertex_A_ = A;
            single_descriptor.vertex_B_ = B;
            single_descriptor.vertex_C_ = C;
            single_descriptor.center_ = (A + B + C) / 3; // 质心
            single_descriptor.vertex_attached_ = vertex_attached; // 顶点附加信息
            single_descriptor.side_length_ << scale * a, scale * b, scale * c; // 边长
            single_descriptor.angle_[0] = fabs(5 * normal_1.dot(normal_2));
            single_descriptor.angle_[1] = fabs(5 * normal_1.dot(normal_3));
            single_descriptor.angle_[2] = fabs(5 * normal_3.dot(normal_2));
            single_descriptor.frame_id_ = current_frame_id_; // 帧编号
            feat_map[position] = true; // 记录已处理位置
            stds_vec.push_back(single_descriptor); // 存储描述
            }
           }