前言
隨機採樣一致性（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
完整代碼地址：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次試驗，失敗的概率是：
得到最少需要的試驗次數爲：
事實上，這個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
《機器人學中的狀態估計》
《計算機視覺中的數學方法》