在計算機視覺領域廣泛的使用各種不同的采樣一致性參數估計算法用於排除錯誤的樣本，樣本不同對應的應用不同，例如剔除錯誤的配准點對，分割出處在模型上的點集，PCL中以隨機采樣一致性算法（RANSAC）為核心，同時實現了五種類似與隨機采樣一致形算法的隨機參數估計算法，例如隨機采樣一致性算法（RANSAC）最大似然一致性算法（MLESAC），最小中值方差一致性算法（LMEDS）等，所有估計參數算法都符合一致性原則。在PCL中設計的采樣一致性算法的應用主要就是對點雲進行分割，根據設定的不同的幾個模型，估計對應的幾何參數模型的參數，在一定容許的范圍內分割出在模型上的點雲。
（1）RANSAC隨機采樣一致性算法的介紹
RANSAC是“RANdom SAmple Consensus（隨機抽樣一致）”的縮寫。它可以從一組包含“局外點”的觀測數據集中，通過迭代方式估計數學模型的參數。它是一種不確定的算法——它有一定的概率得出一個合理的結果；為了提高概率必須提高迭代次數。
數 據分兩種：有效數據（inliers）和無效數據（outliers）。偏差不大的數據稱為有效數據，偏差大的數據是無效數據。如果有效數據占大多數，無 效數據只是少量時，我們可以通過最小二乘法或類似的方法來確定模型的參數和誤差；如果無效數據很多（比如超過了50%的數據都是無效數據），最小二乘法就 失效了，我們需要新的算法
一個簡單的例子是從一組觀測數據中找出合適的2維直線。假設觀測數據中包含局內點和局外點，其中局內點近似的被直線所通過，而局外點遠離於直線。簡單的最 小二乘法不能找到適應於局內點的直線，原因是最小二乘法盡量去適應包括局外點在內的所有點。相反，RANSAC能得出一個僅僅用局內點計算出模型，並且概 率還足夠高。但是，RANSAC並不能保證結果一定正確，為了保證算法有足夠高的合理概率，我們必須小心的選擇算法的參數。
左圖：包含很多局外點的數據集 右圖：RANSAC找到的直線（局外點並不影響結果）
概述
RANSAC算法的輸入是一組觀測數據，一個可以解釋或者適應於觀測數據的參數化模型，一些可信的參數。
RANSAC通過反復選擇數據中的一組隨機子集來達成目標。被選取的子集被假設為局內點，並用下述方法進行驗證：
1.有一個模型適應於假設的局內點，即所有的未知參數都能從假設的局內點計算得出。
2.用1中得到的模型去測試所有的其它數據，如果某個點適用於估計的模型，認為它也是局內點。
3.如果有足夠多的點被歸類為假設的局內點，那么估計的模型就足夠合理。
4.然后，用所有假設的局內點去重新估計模型，因為它僅僅被初始的假設局內點估計過。
5.最后，通過估計局內點與模型的錯誤率來評估模型。
算法
偽碼形式的算法如下所示：
輸入：
data —— 一組觀測數據
model —— 適應於數據的模型
n —— 適用於模型的最少數據個數
k —— 算法的迭代次數
t —— 用於決定數據是否適應於模型的閥值
d —— 判定模型是否適用於數據集的數據數目
輸出：
best_model —— 跟數據最匹配的模型參數（如果沒有找到好的模型，返回null）
best_consensus_set —— 估計出模型的數據點
best_error —— 跟數據相關的估計出的模型錯誤
iterations = 0
best_model = null
best_consensus_set = null
best_error = 無窮大
while ( iterations < k )
maybe_inliers = 從數據集中隨機選擇n個點
maybe_model = 適合於maybe_inliers的模型參數
consensus_set = maybe_inliers
for ( 每個數據集中不屬於maybe_inliers的點 ）
if ( 如果點適合於maybe_model，且錯誤小於t ）
將點添加到consensus_set
if （ consensus_set中的元素數目大於d ）
已經找到了好的模型，現在測試該模型到底有多好
better_model = 適合於consensus_set中所有點的模型參數
this_error = better_model究竟如何適合這些點的度量
if ( this_error < best_error )
我們發現了比以前好的模型，保存該模型直到更好的模型出現
best_model = better_model
best_consensus_set = consensus_set
best_error = this_error
增加迭代次數
返回 best_model, best_consensus_set, best_error
(2)最小中值法（LMedS）
LMedS的做法很簡單，就是從樣本中隨機抽出N個樣本子集，使用最大似然（通常是最小二乘）對每個子集計算模型參數和該模型的偏差，記錄該模型參 數及子集中所有樣本中偏差居中的那個樣本的偏差（即Med偏差），最后選取N個樣本子集中Med偏差最小的所對應的模型參數作為我們要估計的模型參數。
在PCL中sample_consensus模塊支持的幾何模型
- SACMODEL_PLANE - used to determine plane models. The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d]
- SACMODEL_LINE - used to determine line models. The six coefficients of the line are given by a point on the line and the direction of the line as: [point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]
- SACMODEL_CIRCLE2D - used to determine 2D circles in a plane. The circle's three coefficients are given by its center and radius as: [center.x center.y radius]
- SACMODEL_CIRCLE3D - used to determine 3D circles in a plane. The circle's seven coefficients are given by its center, radius and normal as: [center.x, center.y, center.z, radius, normal.x, normal.y, normal.z]
- SACMODEL_SPHERE - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]
- SACMODEL_CYLINDER - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]
- SACMODEL_CONE - used to determine cone models. The seven coefficients of the cone are given by a point of its apex, the axis direction and the opening angle, as: [apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]
- SACMODEL_TORUS - not implemented yet
- SACMODEL_PARALLEL_LINE - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to SACMODEL_LINE .
- SACMODEL_PERPENDICULAR_PLANE - a model for determining a plane perpendicular to an user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .
- SACMODEL_PARALLEL_LINES - not implemented yet
- SACMODEL_NORMAL_PLANE - a model for determining plane models using an additional constraint: the surface normals at each inlier point has to be parallel to the surface normal of the output plane, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .
- SACMODEL_PARALLEL_PLANE - a model for determining a plane parallel to an user-specified axis, within a maximum specified angular deviation. SACMODEL_PLANE .
- SACMODEL_NORMAL_PARALLEL_PLANE defines a model for 3D plane segmentation using additional surface normal constraints. The plane must lie parallel to a user-specified axis. SACMODEL_NORMAL_PARALLEL_PLANE therefore is equivalent to SACMODEL_NORMAL_PLANE + SACMODEL_PARALLEL_PLANE. The plane coefficients are similar to SACMODEL_PLANE .
(2)PCL中Sample_consensus模塊及類的介紹
PCL中Sample_consensus庫實現了隨機采樣一致性及其泛化估計算法，例如平面，柱面，等各種常見的幾何模型，用不同的估計算法和不同的幾何模型自由的結合估算點雲中隱含的具體幾何模型的系數，實現對點雲中所處的幾何模型的分割，線，平面，柱面 ，和球面都可以在PCL 庫中實現，平面模型經常被用到常見的室內平面的分割提取中， 比如牆，地板，桌面，其他模型常應用到根據幾何結構檢測識別和分割物體中，一共可以分為兩類：一類是針對采樣一致性及其泛化函數的實現，一類是幾個不同模型的具體實現，例如：平面，直線，圓球等
pcl::SampleConsensusModel< PointT >是隨機采樣一致性估計算法中不同模型實現的基類，所有的采樣一致性估計模型都繼承與此類，定義了采樣一致性模型的相關的一般接口，具體實現由子類完成，其繼承關系：
類成員的介紹
Public Member Functions |
|
| SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false) | |
| SampleConsensusModel類的構造函數，cloud為輸入點雲對象的指針，indices為算法使用點雲索引向量，如果設置random=true則用當前時間初始化隨機化函數的種子否則使用12345作為種子 | |
| virtual void | getSamples (int &iterations, std::vector< int > &samples) |
| 獲取一組隨機采樣點數據以點雲中點的索引方式存儲到samples，iterations為迭代次數 | |
| virtual bool | computeModelCoefficients (const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)=0 |
| 純虛函數檢查給定的點雲索引樣本samples能否一個有效的模型， | |
| virtual void | optimizeModelCoefficients (const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)=0 |
| 優化初始估計的模型參數，inliers設定的局內點，model_coefficients初始估計的模型的系數，optimized_coefficients優化后的模型系數 | |
| virtual void | getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)=0 |
| 計算點雲中所有點到給定模型中model_coefficients的距離，存儲到distances | |
| virtual void | selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)=0 |
| 從點雲中選擇所有到給定模型model_coefficients的距離小於給定的閥值 threshold 的點為局內點，inliers存儲最終輸出的局內點 | |
| virtual int | countWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold)=0 |
| 統計點雲到給定模型model_coefficients距離小於閥值的點的個數 | |
| virtual void | projectPoints (const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)=0 |
| 將局內點inliner投影到model_coefficients上創建一組新的點雲projected_points， | |
| （太多了 太耽誤時間了）************************** | |
（2）pcl::SampleConsensus< T > 是采樣一致性算法的基類
Public Member Functions |
|
| SampleConsensus (const SampleConsensusModelPtr &model, double threshold, bool random=false) | |
| 其中model設置隨機采樣性算法使用的模型，threshold 閥值 | |
| void | setSampleConsensusModel (const SampleConsensusModelPtr &model) |
| Set the Sample Consensus model to use. | |
| void | setDistanceThreshold (double threshold) |
| Set the distance to model threshold. | |
| double | getDistanceThreshold () |
| Get the distance to model threshold, as set by the user. | |
| void | setMaxIterations (int max_iterations) |
| Set the maximum number of iterations. | |
| int | getMaxIterations () |
| Get the maximum number of iterations, as set by the user. | |
| void | setProbability (double probability) |
| Set the desired probability of choosing at least one sample free from outliers. | |
| double | getProbability () |
| Obtain the probability of choosing at least one sample free from outliers, as set by the user. | |
| virtual bool | computeModel (int debug_verbosity_level=0)=0 |
| Compute the actual model. | |
| virtual bool | refineModel (const double sigma=3.0, const unsigned int max_iterations=1000) |
| Refine the model found. | |
| void | getRandomSamples (const boost::shared_ptr< std::vector< int > > &indices, size_t nr_samples, std::set< int > &indices_subset) |
| Get a set of randomly selected indices. | |
| void | getModel (std::vector< int > &model) |
（3）pcl::RandomizedMEstimatorSampleConsensus< PointT > 實現了RANSAC算法，RANSAC算法適用與處理數據點中局內點比例比較大的情況，科快速的進行局外點的剔除。
class pcl::SampleConsensusModelCircle2D< PointT >實現采樣一致性 計算二位平面圓周模型
class pcl::SampleConsensusModelCone< PointT, PointNT > 實現采樣一致性計算的三維椎體模型
太多了
| class | pcl::LeastMedianSquares< PointT > |
| LeastMedianSquares represents an implementation of the LMedS (Least Median of Squares) algorithm. | |
| class | pcl::MaximumLikelihoodSampleConsensus< PointT > |
| MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood Estimator SAmple Consensus) algorithm, as described in: "MLESAC: A new robust estimator with application to estimating image geometry", P.H.S. | |
| class | pcl::MEstimatorSampleConsensus< PointT > |
| MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) algorithm, as described in: "MLESAC: A new robust estimator with application to estimating image geometry", P.H.S. | |
| class | pcl::ProgressiveSampleConsensus< PointT > |
| RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm, as described in: "Matching with PROSAC – Progressive Sample Consensus", Chum, O. | |
| class | pcl::RandomSampleConsensus< PointT > |
| RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm, as described in: "Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography", Martin A. | |
| class | pcl::RandomizedMEstimatorSampleConsensus< PointT > |
| RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator SAmple Consensus) algorithm, which basically adds a Td,d test (see RandomizedRandomSampleConsensus) to an MSAC estimator (see MEstimatorSampleConsensus). | |
| class | pcl::RandomizedRandomSampleConsensus< PointT > |
| RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RAndom SAmple Consensus), as described in "Randomized RANSAC with Td,d test", O. | |
| class | pcl::SampleConsensusModelNormalParallelPlane< PointT, PointNT > |
| SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional surface normal constraints. | |
| class | pcl::SampleConsensusModelNormalSphere< PointT, PointNT > |
| SampleConsensusModelNormalSphere defines a model for 3D sphere segmentation using additional surface normal constraints. | |
| class | pcl::SampleConsensusModelParallelLine< PointT > |
| SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular constraints. |
代碼實例 random_sample_consensus.cpp
#include <iostream> #include <pcl/console/parse.h> #include <pcl/filters/extract_indices.h> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/sample_consensus/ransac.h> #include <pcl/sample_consensus/sac_model_plane.h> #include <pcl/sample_consensus/sac_model_sphere.h> #include <pcl/visualization/pcl_visualizer.h> #include <boost/thread/thread.hpp> boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) { // -------------------------------------------- // -----Open 3D viewer and add point cloud----- // -------------------------------------------- boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); //viewer->addCoordinateSystem (1.0, "global"); viewer->initCameraParameters (); return (viewer); } /****************************************************************************************************************** 對點雲進行初始化，並對其中一個點雲填充點雲數據作為處理前的的原始點雲，其中大部分點雲數據是基於設定的圓球和平面模型計算 而得到的坐標值作為局內點，有1/5的點雲數據是被隨機放置的組委局外點。 *****************************************************************************************************************/ int main(int argc, char** argv) { // 初始化點雲對象 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); //存儲源點雲 pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); //存儲提取的局內點 // 填充點雲數據 cloud->width = 500; //填充點雲數目 cloud->height = 1; //無序點雲 cloud->is_dense = false; cloud->points.resize (cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size (); ++i) { if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0) { //根據命令行參數用x^2+y^2+Z^2=1設置一部分點雲數據，此時點雲組成1/4個球體作為內點 cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0); if (i % 5 == 0) cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); //此處對應的點為局外點 else if(i % 2 == 0) cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y)); else cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y)); } else { //用x+y+z=1設置一部分點雲數據，此時地拿雲組成的菱形平面作為內點 cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0); if( i % 2 == 0) cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); //對應的局外點 else cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y); } } std::vector<int> inliers; //存儲局內點集合的點的索引的向量 //創建隨機采樣一致性對象 pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud)); //針對球模型的對象 pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud)); //針對平面模型的對象 if(pcl::console::find_argument (argc, argv, "-f") >= 0) { //根據命令行參數，來隨機估算對應平面模型，並存儲估計的局內點 pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p); ransac.setDistanceThreshold (.01); //與平面距離小於0.01 的點稱為局內點考慮 ransac.computeModel(); //執行隨機參數估計 ransac.getInliers(inliers); //存儲估計所得的局內點 } else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 ) { //根據命令行參數 來隨機估算對應的圓球模型，存儲估計的內點 pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s); ransac.setDistanceThreshold (.01); ransac.computeModel(); ransac.getInliers(inliers); } // 復制估算模型的所有的局內點到final中 pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final); // 創建可視化對象並加入原始點雲或者所有的局內點 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0) viewer = simpleVis(final); else viewer = simpleVis(cloud); while (!viewer->wasStopped ()) { viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } return 0; }
運行結果：
在沒有任何參數的情況下，三維窗口顯示創建的原始點雲（含有局內點和局外點），如圖所示，很明顯這是一個帶有噪聲的菱形平面，噪聲點是立方體，自己要是我們在產生點雲是生成的是隨機數生在（0，1）范圍內。
./random_sample_consensus
./random_sample_consensus -f
./random_sample_consensus -sf
未完待續****************************************************8