手写SAC_IA实现基于FPFH描述子的点云帧间匹配

置顶 bug大湿

已于 2022-09-14 15:15:55 修改

阅读量1.2k

点赞数 2

分类专栏： 0_1SLAM 文章标签：自动驾驶 c++ 机器人

于 2022-09-08 10:26:08 首次发布

本文链接：https://blog.csdn.net/qq_42911741/article/details/126717255

版权

0_1SLAM 专栏收录该内容

30 篇文章 6 订阅

订阅专栏

1-特征点直方图(PFH)：

计算两个点的法向量的关系，设两个点的法向量为ns和nt，ns和nt的关系可以用三个角度（两个向量的旋转关系）+两点之间的欧氏距离来衡量。

过程：确定查询点pi，确定pi的搜索半径r，确在r内pi点的k近邻的所有元素集合M，计算M中的所有点的两两点间的法线关系，创建出直方图；

n个点的点云的PFH，时间复杂度O(n*k^2)；

2-快速特征点直方图(FPFH)：

先计算查询点pi的SPFH(简单点特征直方图)，即只计算查询点pi与邻域点之间的法线关系；重新确定k近邻，计算邻域点k近邻的SPFH，以下公式计算pi点的FPFH值：FPFH(pi) =

其中，k为近邻值，wk为权重，一般为距离；

n 个点的点云的FPFH，时间复杂度O(n*k)；

3-差异：PFH计算查询点精确的r范围所有点之间的关系，FPFH计算查询点r内只与近邻点的关系以及近邻点r内的关系（2r内）；

4-pcl代码：

输入点云：kitti的 64线束velo

【描述子提取】：

pcl::FPFHSignature33说明：内部创建了不相关的直方图11个（pcl默认的），特征维度3，每个特征对应一个直方图，所以是FPFHSignature33（横轴33）；

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal;   
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());  
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); 
normal.setInputCloud(inut_cloud);    
normal.setSearchMethod(tree);      
normal.setRadiusSearch(0.6);      //查询点周领域搜索半径0.6  
normal.compute(*cloud_normals);   //计算法线

//构建fpfh对象，传入 点云，法线，计算出fpfh特征        
//构建kd树搜索，速度快，FPFHSignature33： 3个角特征 × 11个直方图   
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;  
pcl::search::KdTree<PointXYZ>::Ptr tree_fpfh(new pcl::search::KdTree<pcl::PointXYZ>);       
fpfh.setInputCloud(inut_cloud);       
fpfh.setInputNormals(cloud_normals);   
fpfh.setSearchMethod(tree_fpfh);         
//    fpfh特征 
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());     
fpfh.setRadiusSearch(0.8);       // 使用半径必须大于估计法线时使用的半径       fpfh.compute(*fpfhs);

输入源点云时：

输入滤波后的点云时：

==> 点云数量对描述子提取速度影响甚大，我用原始点云12w左右，速度21s才提取完，更别说后面配准的时间，点云数目再1w左右，提取效率还算可以，0.09s（我电脑比较拉跨）。

【帧间匹配，位姿估计】：

pcl库计算normal和fpfh可以使用openMP加速：

#include <pcl/features/normal_3d_omp.h>//使用OMP需要添加的头文件
#include <pcl/features/fpfh_omp.h> //fpfh加速计算的omp(多核并行计算)
//-------------------------法向量估计-----------------------    
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>()); 
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n_est; 
n_est.setInputCloud(input_cloud);    
n_est.setNumberOfThreads(8);//设置openMP的线程数    
n_est.setSearchMethod(tree);  
n_est.setKSearch(10);   
n_est.compute(*normals);
//------------------FPFH估计-------------------------------
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(newpcl::PointCloud<pcl::FPFHSignature33>()); 
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f_est;  
f_est.setNumberOfThreads(8); //指定8核计算    
f_est.setInputCloud(input_cloud);   
f_est.setInputNormals(normals);    
f_est.setSearchMethod(tree);    
f_est.setKSearch(10);    
f_est.compute(*fpfh);

pcl库的sac_ia配准结果：（源点云较目标点云移动(x=0.5, y=0.5)）

对比上一篇文章的pcl的icp和ndt配准，此次配准结果是相当准的，但是时间很长很长！！！

RANSAC算法配准的实现步骤：[随机采样一致性]
(1)在对应点集中随机选取３个对应点对，并求解刚体变换矩阵；
(2)计算对应点集中剩余点对在此刚体变换矩阵下的距离误差，若其中一点对的距离误差小于设定的阈值误差，则该点为样本内点，否则为样本外点，同时统计样本内点的数目；
(3)重复以上步骤直至抵达迭代次数的上限。统计不同刚体变换模型下的样本内点数量，样本内点数量最多的作为最佳数学模型，保留所有样本内点，剔除样本外点之后，剩余点对作为正确的点对用于点云配准操作；

SAC_IA算法配准的实现步骤：[采样一致性初始对齐算法]

1）计算两幅点云的特征（这里我测试的是fpfh）；

2）根据特征，进行最近邻搜索，得到source特征点对应的K个target特征；

3）随机采样n(n>=3便于计算)个source点，再为每个source点匹配其K近邻的target中的点，形成n个点对；

4）进行匹配，计算R,t，重复计算，直到两幅点云的误差满足收敛条件，则输出最佳最佳变换矩阵；

SAC_IA 改进之处在于：

1）限制了采样点之间的距离；

2）不再是RANSAC直接的点与点距离的查找，而是根据特征找点；

以下是我手写的sac_ia配准代码，还请大家探讨探讨，完整代码上传到: git@github.com:qqngwzlb/pcl_registration.git

// 目标点云特征target_fpfhs，源点云特征source_fpfhs，采样点大小sample_size，输出变换矩阵T
void my_sac_ia ( pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, 
                 pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, 
                 pcl::PointCloud<pcl::FPFHSignature33>::Ptr& target_fpfhs,
                 pcl::PointCloud<pcl::FPFHSignature33>::Ptr& source_fpfhs,
                 int sample_size, Eigen::Matrix4d& T) {  

       // std::cout << "source->at: " << target_cloud->at(0) << std::endl;
       // (26.5125,5.4335,-11.2905) ==> 类型pcl::PointXYZ
// 1-查找  为每个源特征查找对应 k个近邻的目标特征  
       int K = 10;   // 设置K的个数  
       std::vector<int> idx(K);  // 创建一个 搜索后 保存 的 点的索引值 的 向量
       std::vector<float> squaredDis(K);     // 创建一个 搜索后 保存 的 点的距离平方值 的 向量     
       size_t source_size = source_fpfhs->size();
       std::vector<relate> matchSet(source_size);        // 作为左值，必须先初始化

       pcl::search::KdTree<pcl::FPFHSignature33>::Ptr fpfh_tree(new pcl::search::KdTree<pcl::FPFHSignature33>());
       fpfh_tree->setInputCloud(target_fpfhs);
       // 通过描述子找最近邻
       for (int i=0; i<source_size; ++i) { 
              // source_fpfhs[i] 与 target_fpfhs最近的K个
              fpfh_tree->nearestKSearch(*source_fpfhs, i, K, idx, squaredDis);
              matchSet.at(i).source_index = i;
              matchSet.at(i).target_index = idx; 
       } 

// 2-在matchSet中随机选择匹配点
       int sampleSize = sample_size;
       std::vector<int> final_target_matches; // 存储点对
       std::vector<int> final_source_matches; 

       srand((unsigned int)time(NULL)); 

   while (1) {
       for(;;) { 
              int temp_rand_idx = (rand() % (source_size-0)) + 0;           // [0, source_size)
              final_source_matches.push_back(matchSet[temp_rand_idx].source_index);
              if (calc_cloud_dist(source_cloud, final_source_matches, 100) == sampleSize) {
                     std::cout << "step1 over!" << std::endl;

                     for (int i = 0; i<final_source_matches.size(); ++i) {

                            std::cout <<     final_source_matches.at(i)  << std::endl;
                            std::cout << "_________________" << std::endl;
                     }
                     break;
              }
       }

       // 为选出的每个源点云的点，在目标点云的k近邻中寻找对应的一个匹配点
       for (int i=0; i<sampleSize; i++) { 

              std::vector<int> source2target_idx_vec( matchSet[final_source_matches.at(i)].target_index );

              int temp_rand_idx = (rand() % (K-0)) + 0; 
              final_target_matches.push_back( source2target_idx_vec.at(temp_rand_idx) );
              source2target_idx_vec.clear();
       }
       std::cout << "step2 over!" << std::endl;
       for (int i = 0; i<final_target_matches.size(); ++i) {

              std::cout << final_target_matches.at(i)  << std::endl;
              std::cout << "_________________" << std::endl;
       }  

// 3-求解：R，t
       std::vector<Eigen::Vector3d>  source_points, target_points;
       T = Eigen::Matrix4d::Identity();
       for (int i=0; i< sampleSize; i++) {
              target_points.push_back( Eigen::Vector3d( target_cloud->points[final_target_matches.at(i)].x, 
                                                        target_cloud->points[final_target_matches.at(i)].y, 
                                                        target_cloud->points[final_target_matches.at(i)].z) );
              source_points.push_back( Eigen::Vector3d(source_cloud->points[final_source_matches.at(i)].x, 
                                                        source_cloud->points[final_source_matches.at(i)].y, 
                                                        source_cloud->points[final_source_matches.at(i)].z) );
              std::cout << "chose source points: \n" << source_points.at(i) << std::endl;
              std::cout << "chose target points: \n" << target_points.at(i) << std::endl;
       }
       
       _3d23d_est(target_points, source_points, T);    
// 误差计算：两次变化矩阵的差值
       int error = 0.0;
       error = compute_error(source_cloud, target_cloud, T);
       if ( error < 2) {
              std::cout << "calculate finished！！！" << std::endl;
              break;
       }else {
              std::cout << "now error:\n" << error << std::endl;
       }
   }

}