RANSAC算法原理與應用(1)

前言

隨機採樣一致性(random sample consensus,RANSAC)是一種對帶有外點的數據擬合參數模型的迭代方法。

傳感器測量值可能會受多種因素干擾而不可靠,同時特徵匹配也會存在誤匹配的情況,如果不正確地檢測並剔除他們,視覺SLAM中的很多算法(如計算本質矩陣/基礎矩陣/單應矩陣、三角測量、PNP等)將會失敗。

我們將非常不可能的測量值(根據測量模型)稱爲外點(outlier)。對外點的一種常用判斷標準是(一維數據中)將超出平均值三個標準差的測量值作爲外點。

處理外點的兩種最常用方案爲:
1、隨機採樣一致性(RANSAC)

2、M估計

案例(直線RANSAC)

在幾何上,魯棒估計一條直線可描述爲:給定一組二維測量數據點,尋找一條直線使得測量點到該直線的幾何距離的平方和達到最小,即該直線最小化測量點到直線的幾何距離平方和,並且使得內點偏離該直線的距離小於 t 個單位。因此,這個問題有兩個要求:

1. 用一條直線擬合測量數據點;

2. 根據閾值 t 將測量數據分爲內點與外點;

[牛客網]擬合二維平面中的帶噪音直線,其中有不超過10%的樣本點遠離了直線,另外90%的樣本點可能有高斯噪聲的偏移

要求輸出爲

ax+by+c=0的形式
其中a > 0 且 a^2 + b^2 = 1
輸入描述:

第一個數n表示有多少個樣本點  之後n*2個數 每次是每個點的x 和y

輸出描述:

輸出a,b,c三個數,至多可以到6位有效數字

示例1

輸入

    5
    3 4
    6 8
    9 12
    15 20
    10 -10

輸出

0.800000 -0.600000 0.000000

主要參考以下:

https://www.codeproject.com/Articles/576228/Line-Fitting-in-Images-Using-Orthogonal-Linear-Reg

最小二乘原理推導

[MVG] 魯棒估計: RANSAC & 魯棒核函數

完整代碼地址:https://github.com/WinDistance/ransac

#include <random>
#include <iostream>
#include <time.h>
#include <set>
#include <cassert>
#include <limits.h>


using namespace std;
//數據點類型
struct Point2D {
	Point2D() {};
	Point2D(double X, double Y) :x(X), y(Y) {};
	double x;
	double y;
};
/**
  * @brief 線性模型
  *
  * Ax+By+C = 0;
*/
class LineModel {

	//待估計參數
	double A_, B_, C_;
public:
	LineModel() {};
	~LineModel() {};

	//使用兩個點對直線進行初始估計
	void FindLineModel(const Point2D& pts1, const Point2D& pts2) {

		A_ = pts2.y - pts1.y;
		B_ = pts1.x - pts2.x;
		C_ = (pts1.y - pts2.y) * pts2.x + (pts2.x - pts1.x) * pts2.y;
	}

	//返回點到直線的距離
	double computeDistance(const Point2D& pt) {
		return abs(A_ * pt.x + B_ * pt.y + C_) / sqrt(A_ * A_ + B_ * B_);
	}

	//模型參數輸出
	void printLineParam()
	{
		cout << "best_model.A =  "<< A_<< endl
			<<"  best_model.B =  " <<B_ <<endl
			<< " best_model.C=  "<<C_<< endl;
	}

	//利用最大的內點集重新估計模型
	//y=kx+b
	//利用最小二乘法:
	//k=(meanXY-meanX*meanY)/(meanXX-meanX*meanX)
	//b=meanY-k*meanX
	double estimateModel(vector<Point2D>& data, set<size_t>& inliers_set)
	{
		assert(inliers_set.size() >= 2);
		//求均值 means
		double meanX=0,meanY=0;
		double meanXY = 0, meanXX = 0;
		for (auto& idx : inliers_set) {
			meanX += data[idx].x;
			meanY += data[idx].y;
			meanXY += data[idx].x * data[idx].y;
			meanXX += data[idx].x * data[idx].x;

		}
		meanX /= inliers_set.size();
		meanY /= inliers_set.size();
		meanXY /= inliers_set.size();
		meanXX /= inliers_set.size();


		bool isVertical = (meanXX-meanX * meanX) == 0;
		double k = NAN;
		double b = NAN;

		if (isVertical)
		{
			A_ = 1;
			B_ = 0;
			C_ = meanX;
		}
		else
		{
			k = (meanXY - meanX * meanY) / (meanXX - meanX * meanX); 
			b = meanY - k * meanX;                                                 
			//A^2+B^2 = 1;
			//這裏要注意k的符號
			double scaleFactor = (k>=0.0?1.0:-1.0) / sqrt(1 + k * k);
			A_ = scaleFactor * k;
			B_ = -scaleFactor;
			C_ = scaleFactor * b;
		}

		//誤差計算
		double sXX, sYY, sXY;
		sXX = sYY = sXY = 0;
		for (auto& index : inliers_set) {
			Point2D point;
			point = data[index];
			sXX += (point.x - meanX) * (point.x - meanX);
			sYY += (point.y - meanY) * (point.y - meanY);
			sXY += (point.x - meanX) * (point.y - meanY);
		}
		double error = A_ * A_ * sXX + 2 * A_ * B_ * sXY + B_ * B_ * sYY;
		error /= inliers_set.size();
		return error;
	}

};



/**
* @brief 運行RANSAC算法
*
* @param[in]    data               一組觀測數據
* @param[in]    n	           適用於模型的最少數據個數
* @param[in]    maxIterations       算法的迭代次數
* @param[in]    d                   判定模型是否適用於數據集的數據數目,於求解出來的模型的內點質量或者說數據集大小的一個約束
* @param[in]    t					用於決定數據是否適應於模型的閥值
* @param[in&out]    model           自定義的待估計模型,爲該函數提供Update、computeError和Estimate三個成員函數
*                                    運行結束後,模型參數被設置爲最佳的估計值
* @param[out]    best_consensus_set    輸出一致點的索引值
* @param[out]    best_error         輸出最小損失函數
*/

int runRansac(vector<Point2D>& dataSets, int n, int maxIterations, double sigma,int d,
		LineModel& best_model, set<size_t>& inlier_sets, double& best_error) {


		int isFound = 0;           //算法成功的標誌
		int N = dataSets.size();
		set<int> maybe_inliers;    //初始隨機選取的點(的索引值)
		LineModel currentModel;
		set<size_t> maxInliers_set;  //最大內點集
		best_error = 1.7976931348623158e+308;
		default_random_engine rng(time(NULL));     //隨機數生成器
		uniform_int_distribution<int> dist(0, N - 1);  //採用均勻分佈



	     //1. 新建一個容器allIndices,生成0到N-1的數作爲點的索引
		vector<size_t> allIndices;
		allIndices.reserve(N);
		vector<size_t> availableIndices;

		for (int i = 0; i < N; i++)
		{
			allIndices.push_back(i);
		}


	     //2.這個點集是用來計算線性模型的所需的最小點集
		vector< vector<size_t> > minSets = vector< vector<size_t> >(maxIterations, vector<size_t>(n, 0));
		//隨機選點,注意避免重複選取同一個點
		for (int it = 0; it < maxIterations; it++)
		{
			availableIndices = allIndices;
			for (size_t j = 0; j < n; j++)
			{
				// 產生0到N-1的隨機數
				int randi = dist(rng);
				// idx表示哪一個索引對應的點被選中
				int idx = availableIndices[randi];

				minSets[it][j] = idx;
				//cout << "idx:" << idx << endl;
				// randi對應的索引已經被選過了,從容器中刪除
				// randi對應的索引用最後一個元素替換,並刪掉最後一個元素
				availableIndices[randi] = availableIndices.back();
				availableIndices.pop_back();
			}
		}


		//3.主循環程序,求解最大的一致點集
		vector<Point2D> pts;
		pts.reserve(n);
		for (int it = 0; it < maxIterations; it++)
		{

			for (size_t j = 0; j < n; j++)
			{
				int idx = minSets[it][j];

				pts[j] = dataSets[idx];

			}

			//cout << pts[0].x << endl << pts[1].x << endl;
			//根據隨機到的兩個點計算直線的模型
			currentModel.FindLineModel(pts[0],pts[1]);  

			//currentModel.printLineParam();
			set<size_t> consensus_set;        //選取模型後,根據誤差閾值t選取的內點(的索引值)

			//根據初始模型和閾值t選擇內點  
			// 基於卡方檢驗計算出的閾值
			const double th =sqrt(3.841*sigma*sigma);
			double current_distance_error = 0.0 ;
			double distance_error = 0.0;
			for (int i = 0; i < N; i++) 
			{
				current_distance_error= currentModel.computeDistance(dataSets[i]);
				if (current_distance_error < th) {
					consensus_set.insert(i);
				}

			}

			if (consensus_set.size() > maxInliers_set.size()) {
				maxInliers_set = consensus_set;
			}
		}

		//4.根據全部的內點重新計算模型
	        //重新在內點集上找一個誤差最小的模型
		if (maxInliers_set.size() > d) {
			double current_distance_error = best_model.estimateModel(dataSets, maxInliers_set);
			//若當前模型更好,則更新輸出量
			if (best_error < current_distance_error) {
				best_model = currentModel;
				inlier_sets = maxInliers_set;
				best_error = current_distance_error;
				isFound = 1;
			}
		}
		return isFound;
	}







int main() {

	vector<Point2D> Point_sets{ Point2D(3, 4), Point2D(6, 8), Point2D(9, 12), Point2D(15, 20), Point2D(10,-10) };
     //vector<Point2D> Point_sets{ Point2D(3, 4), Point2D(3, 7), Point2D(10,-10) };
     //vector<Point2D> Point_sets{ Point2D(3, 4), Point2D(6, 4), Point2D(9, 4), Point2D(15, 4), Point2D(10,-10) };
	int data_size = Point_sets.size();
	//2.設置輸入量
	int maxIterations = 50;   //最大迭代次數
	int n = 2;                //模型自由度
	double t = 0.01;        //用於決定數據是否適應於模型的閥值
	int d = data_size * 0.5; //判定模型是否適用於數據集的數據數目
	//3.初始化輸出量
	LineModel best_model;            //最佳線性模型
	set<size_t> best_consensus_set;  //記錄一致點索引的set
	double best_error;              //最小殘差
	//4.運行RANSAC            
	int status = runRansac(Point_sets, n, maxIterations, t, d, best_model, best_consensus_set, best_error);
	//5.輸出
	best_model.printLineParam();
	return 0;
}

RANSAC

步驟:

1. 確定求解模型 M,即確定模型參數 p,所需要的最小數據點的個數 n。由 n 個數據點組成

的子集稱爲模型 M 的一個樣本;

2. 從數據點集 D 中隨機地抽取一個樣本 J,由該樣本計算模型的一個實例 Mp (J ) ,確定與

M p(J ) 之間幾何距離< 閾值 t 的數據點所構成的集合,並記爲 S( M p (J ) ),稱爲實例 M p(J)

的一致集;

3. 如果在一致集 S( M p (J ) )中數據點的個數S( M p (J ) ) > 閾值 T,則用S( M p (J ) )重新估計模

型 M,並輸出結果;如果S( M p (J ) )<閾值 T,返回到步驟 2;

4. 經過 K 次隨機抽樣,選擇最大的一致集 S( M p (J ) ),用S( M p (J ) )重新估計模型 M,並輸出

結果。

//RANSAC的算法大致可以表述爲(來自wikipedia):

    Given:
        data – a set of observed data points
        model – a model that can be fitted to data points
        n – the minimum number of data values required to fit the model
        k – the maximum number of iterations allowed in the algorithm
        t – a threshold value for determining when a data point fits a model
        d – the number of close data values required to assert that a model fits well to data

    Return:
        bestfit – model parameters which best fit the data (or nul if no good model is found)

    iterations = 0
    bestfit = nul
    besterr = something really large
    while iterations < k {
        maybeinliers = n randomly selected values from data
        maybemodel = model parameters fitted to maybeinliers
        alsoinliers = empty set
        for every point in data not in maybeinliers {
            if point fits maybemodel with an error smaller than t
                 add point to alsoinliers
        }
        if the number of elements in alsoinliers is > d {
            % this implies that we may have found a good model
            % now test how good it is
            bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
            thiserr = a measure of how well model fits these points
            if thiserr < besterr {
                bestfit = bettermodel
                besterr = thiserr
            }
        }
        increment iterations
    }
    return bestfit

抽樣次數

樣本由從測量數據集中均勻隨機抽取的子集所構成,每個樣本所包含數據點的個數 n 是確定模

型參數所需要數據點的最小數目,例如:直線最少需要兩個數據點才能確定,即 n = 2 ;圓最少需要

3 個數據點,即 n = 3。

RANSAC是一種概率算法,爲了能確保有更好的概率找到真正的內點集合,必須實驗足夠多的次數。以下爲試驗次數的計算過程:
1、假設每次選取測量點都是相互獨立的,且每個測量點爲內點的概率均爲w,p爲經過k次試驗後成功的總體概率;
2、那麼在某次實驗中,n個隨機樣本都是內點的可能性是wn(這裏n表示爲擬合該模型需要的最少數據個數);

3、因此經過了p次試驗,失敗的概率是:
1p=(1wn)k 1-p=(1-w^n)^k
得到最少需要的試驗次數爲:
k=log(1p)log(1wn) k=\frac{log(1-p)}{log(1-w^n)}
事實上,這個k值被看作是選取不重複點的上限,因爲這個結果假設n個點都是獨立選擇的,也就是說,某個點被選定之後,它可能會被後續的迭代過程重複選定到。而數據點通常是順序選擇的。

距離閾值

如果我們希望所選取的閾值 t 使得內點被接受的概率是α ,則需要通過由內點到模型之間幾何

距離的概率分佈來計算距離閾值 t,這是非常困難的。在實際中,距離閾值通常靠經驗選取。

終止閾值

終止閾值是難以設置的問題。經驗的做法是:給出內點比例 w 的一個估計值ε ,如果一致集大

小相當於數據集的內點規模則終止。由於很難給出內點比例 w 的一個準確估計,所以經驗做法往往

不能獲得較好的估計結果。由於終止閾值僅僅是用來終止 RANSAC 的抽樣,所以通常的做法是自適應算法(終止 RANSAC 抽樣),自動更新K。

最終估計

由內點得到模型估計 M,由 M應用卡方檢驗重新劃分內點與外點;繼續這個過程直至內點集收斂。

參考文獻

https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf

https://en.wikipedia.org/wiki/Random_sample_consensus

https://blog.csdn.net/luoshixian099/article/details/50217655

https://zhuanlan.zhihu.com/p/62175983

《機器人學中的狀態估計》

《計算機視覺中的數學方法》

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章