SAC |
您所在的位置：网站首页 › arcaea510源点多少钱 › SAC |
SAC
|
第一次尝试，记录自己的学习历程。 在学习点云配准的过程中，对于局部点云与全局点云的配准，发现SAC-IA配准算法的效果相对较好，所以自己尝试实现SAC-IA算法。当然其中基础部分、法线和FPFH特征描述子的计算还是直接调用pcl. 算法流程首先简单介绍算法的实现流程： 分别计算源点云和目标点云的FPFH特征描述子；基于FPFH特征描述子对两个点云中的点进行匹配；随机选择 n (n >= 3) 对匹配点；通过SVD求解该匹配情况下的旋转与位移；计算此时对应的误差；重复步骤３－５，直到满足条件，将最小误差对应的旋转和位移作为最终结果。 可以看出上述步骤基本就是RANSAC算法的思想。下面结合代码详细分析每个步骤。 FPFH描述子的计算这直接调用pcl，没啥好说的，直接上代码。所使用点云的分辨率为0.2m. pcl::NormalEstimation norm_est; pcl::FPFHEstimation fpfh_est; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); norm_est.setSearchMethod(tree); norm_est.setRadiusSearch(0.4f); fpfh_est.setSearchMethod(tree); fpfh_est.setRadiusSearch(0.4f); // 源点云 pcl::PointCloud::Ptr source_normal(new pcl::PointCloud); norm_est.setInputCloud(source); norm_est.compute(*source_normal); pcl::PointCloud::Ptr source_fpfh(new pcl::PointCloud); fpfh_est.setInputCloud(source); fpfh_est.setInputNormals(source_normal); fpfh_est.compute(*source_fpfh); // 目标点云 pcl::PointCloud::Ptr target_normal(new pcl::PointCloud); norm_est.setInputCloud(target); norm_est.compute(*target_normal); pcl::PointCloud::Ptr target_fpfh(new pcl::PointCloud); fpfh_est.setInputCloud(target); fpfh_est.setInputNormals(target_normal); fpfh_est.compute(*target_fpfh); 描述子匹配该匹配的规则为每个源点云中的点匹配ｋ个目标点云中的点，即描述子的ｋ近邻。定义了如下的结构体用来保存匹配情况。 struct corrs{ int source_index; // 源点云中点的索引 // double max_distance; vector target_index; // 目标点云中点的索引 };在kd树的基础上实现的，效率高。将目标点云的FPFH描述子构建成 kd 树，再对源点云中的每个点的FPFH特征描述子进行近邻点查询。 pcl::search::KdTree fpfh_tree; fpfh_tree.setInputCloud(target_fpfh); vector index(k); vector distance(k); int len = source_fpfh->size(); vector matches(len); for(size_t i = 0; i bool valid_sample = true; id = rand() % len; for(size_t i = 0; i valid_sample = false; cnt++; break; } } if(valid_sample){ sample_index.push_back(id); cnt = 0; } if(cnt > 100) break; // 若循环100次还找不到符合要求的点，则直接退出，重新选点 } if(sample_index.size() pt1.push_back(target->at(final_match[1][i])); pt2.push_back(source->at(matches[final_match[0][i]].source_index)); } // 计算匹配点的中心 pcl::PointXYZ p1(0, 0, 0); pcl::PointXYZ p2(0, 0, 0); for(size_t i = 0; i q1[i].x = pt1[i].x - p1.x; q1[i].y = pt1[i].y - p1.y; q1[i].z = pt1[i].z - p1.z; q2[i].x = pt2[i].x - p2.x; q2[i].y = pt2[i].y - p2.y; q2[i].z = pt2[i].z - p2.z; } Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); for(size_t i = 0; i tree.nearestKSearch(*source, i, 1, nn_index, nn_distance); if(nn_distance[0] > 1.0) continue; // 对于距离太远的点，则将其排除误差，此处需要结合点云分辨率设定阈值 error += nn_distance[0]; cnt++; } return error / cnt; } 实验结果完整程序运行结果与直接调用pcl库存在一定差异，首先运行时间相对稍长一些，运行时间多出5%~15%，最终的误差也不一样。考虑到其中存在随机因素以及计算误差的函数存在差异，所以没有深究。以下图片展示了上述代码运行后得到的效果。 |
【本文地址】