一、算法原理
RANSAC算法介绍
二、代码实现
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 | #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <boost/thread/thread.hpp> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/registration/correspondence_estimation.h> #include <pcl/registration/correspondence_rejection_sample_consensus.h>//使用随机样本一致性来识别inliers using namespace std; int main(int argc, char** argv) { // 加载第一次扫描点云数据作为目标云 pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ>("A3 - Cloud.pcd", *target) == -1) { PCL_ERROR("读取目标点云失败 \n"); return (-1); } cout << "从目标点云中读取 " << target->size() << " 个点" << endl; // 加载从新视角得到的第二次扫描点云数据作为源点云 pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ>("B3 - Cloud.pcd", *source) == -1) { PCL_ERROR("读取源标点云失败 \n"); return (-1); } cout << "从源点云中读取 " << source->size() << " 个点" << endl; //---------初始化对象获取匹配点对---------------------- pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>core; core.setInputSource(source); core.setInputTarget(target); boost::shared_ptr<pcl::Correspondences> correspondence_all(new pcl::Correspondences); core.determineCorrespondences(*correspondence_all);//确定输入点云与目标点云之间的对应关系 //------------RANSAC筛选内点---------------------------- boost::shared_ptr<pcl::Correspondences> correspondence_inlier(new pcl::Correspondences); pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> ransac; ransac.setInputSource(source); ransac.setInputTarget(target); ransac.setMaximumIterations(200);//设置最大迭代次数 ransac.setInlierThreshold(0.05);//设置对应点之间的最大距离 //ransac.setRefineModel(true);//指定是否应该使用inliers的方差在内部细化模型 ransac.getRemainingCorrespondences(*correspondence_all, *correspondence_inlier); //-----------输出必要信息到控制台------------------------ cout << "ransac前的匹配点对有：" << correspondence_all->size() << "对" << endl; cout << "ransac后的匹配点对有：" << correspondence_inlier->size() << "对" << endl; cout << "ransac前的匹配点对：\n "; for (int i = 0; i < correspondence_all->size(); i++) { cout << i << "\tindex_match:\t" << correspondence_all->at(i).index_match << "\tindex_query:\t" << correspondence_all->at(i).index_query << "\n"; } cout << "ransac后的匹配点对：\n "; for (int i = 0; i < correspondence_inlier->size(); i++) { cout << i << "\tindex_match:\t" << correspondence_inlier->at(i).index_match << "\tindex_query:\t" << correspondence_inlier->at(i).index_query << "\n"; } boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云")); viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色 // 对目标点云着色可视化 (red). pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0); viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); // 对源点云着色可视化 (green). pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(source, 0, 255, 0); viewer->addPointCloud<pcl::PointXYZ>(source, input_color, "input cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud"); //对应关系可视化 viewer->addCorrespondences<pcl::PointXYZ>(source, target, *correspondence_inlier, "correspondence"); //viewer->initCameraParameters(); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return 0; } |
三、结果展示
