RANSAC算法（RANdom SAmple Consensus隨機抽樣一致）

它可以從一組包含“局外點”的觀測數據集中，通過迭代方式估計數學模型的參數。它是一種不確定的算法——它有一定的概率得出一個合理的結果；爲了提高概率必須提高迭代次數。該算法最早由Fischler和Bolles於1981年提出。

光看文字還是太抽象了，我們再用圖描述

RANSAC的基本假設是：
（1）數據由“局內點”組成，例如：數據的分佈可以用一些模型參數來解釋；
（2）“局外點”是不能適應該模型的數據；
（3）除此之外的數據屬於噪聲。

而下圖二里面、藍色部分爲局內點，而紅色部分就是局外點，而這個算法要算出的就是藍色部分那個模型的參數

（圖二）

RANSAC算法的輸入是一組觀測數據，一個可以解釋或者適應於觀測數據的參數化模型，一些可信的參數。

在上圖二中  左半部分灰色的點爲觀測數據，一個可以解釋或者適應於觀測數據的參數化模型 我們可以在這個圖定義爲一條直線，如y=kx + b；

一些可信的參數指的就是指定的局內點範圍。而k，和b就是我們需要用RANSAC算法求出來的

RANSAC通過反覆選擇數據中的一組隨機子集來達成目標。被選取的子集被假設爲局內點，並用下述方法進行驗證：

　　1.有一個模型適應於假設的局內點，即所有的未知參數都能從假設的局內點計算得出。
　　2.用1中得到的模型去測試所有的其它數據，如果某個點適用於估計的模型，認爲它也是局內點。
     3.如果有足夠多的點被歸類爲假設的局內點，那麼估計的模型就足夠合理。
     4.然後，用所有假設的局內點去重新估計模型，因爲它僅僅被初始的假設局內點估計過。
     5.最後，通過估計局內點與模型的錯誤率來評估模型。
這個過程被重複執行固定的次數，每次產生的模型要麼因爲局內點太少而被捨棄，要麼因爲比現有的模型更好而被選用。

這個算法用圖二的例子說明就是先隨機找到內點，計算k1和b1，再用這個模型算其他內點是不是也滿足y=k1x+b2，評估模型

再跟後面的兩個隨機的內點算出來的k2和b2比較模型評估值，不停迭代最後找到最優點

 

我再用圖一的模型說明一下RANSAC算法

（圖1）

RANSAC算法的輸入是一組觀測數據，一個可以解釋或者適應於觀測數據的參數化模型，一些可信的參數。

模型對應的是空間中一個點雲數據到另外一個點雲數據的旋轉以及平移。

第一步隨機得到的是一個點雲中的點對作 ，利用其不變特徵（兩點距離，兩點法向量夾角）作爲哈希表的索引值搜索另一個點雲中的一對對應點對，然後計算得到旋轉及平移的參數值。

然後適用變換，找到其他局內點，並在找到局內點之後重新計算旋轉及平移爲下一個狀態。

然後迭代上述過程，找到最終的位置

其中觀測數據就是PB，一個可以解釋或者適應於觀測數據的參數化模型是 四元數旋轉，並平移

可信的參數是兩個點對的不變特徵（兩點距離，兩點法向量夾角）

 

也就是說用RANSAC算法是 從PB找一個隨機的點對計算不變特徵，找目標點雲PR裏特徵最像的來匹配，計算qR和qT

 

 

RANSAC算法成立的條件裏主要是先要有一個模型和確定的特徵，用確定的特徵計算模型的具體參數

關於算法的源代碼，Ziv Yaniv曾經寫一個不錯的C++版本，我在關鍵處增補了註釋：

#include <math.h>
#include "LineParamEstimator.h"

LineParamEstimator::LineParamEstimator(double delta) : m_deltaSquared(delta*delta) {}
/*****************************************************************************/
/*
 * Compute the line parameters  [n_x,n_y,a_x,a_y]
 * 通過輸入的兩點來確定所在直線，採用法線向量的方式來表示，以兼容平行或垂直的情況
 * 其中n_x,n_y爲歸一化後，與原點構成的法線向量，a_x,a_y爲直線上任意一點
 */
void LineParamEstimator::estimate(std::vector<Point2D *> &data,
                                                                    std::vector<double> &parameters)
{
    parameters.clear();
    if(data.size()<2)
        return;
    double nx = data[1]->y - data[0]->y;
    double ny = data[0]->x - data[1]->x;// 原始直線的斜率爲K，則法線的斜率爲-1/k
    double norm = sqrt(nx*nx + ny*ny);

    parameters.push_back(nx/norm);
    parameters.push_back(ny/norm);
    parameters.push_back(data[0]->x);
    parameters.push_back(data[0]->y);
}
/*****************************************************************************/
/*
 * Compute the line parameters  [n_x,n_y,a_x,a_y]
 * 使用最小二乘法，從輸入點中擬合出確定直線模型的所需參量
 */
void LineParamEstimator::leastSquaresEstimate(std::vector<Point2D *> &data,
                                                                                            std::vector<double> &parameters)
{
    double meanX, meanY, nx, ny, norm;
    double covMat11, covMat12, covMat21, covMat22; // The entries of the symmetric covarinace matrix
    int i, dataSize = data.size();

    parameters.clear();
    if(data.size()<2)
        return;

    meanX = meanY = 0.0;
    covMat11 = covMat12 = covMat21 = covMat22 = 0;
    for(i=0; i<dataSize; i++) {
        meanX +=data[i]->x;
        meanY +=data[i]->y;

        covMat11    +=data[i]->x * data[i]->x;
        covMat12    +=data[i]->x * data[i]->y;
        covMat22    +=data[i]->y * data[i]->y;
    }

    meanX/=dataSize;
    meanY/=dataSize;

    covMat11 -= dataSize*meanX*meanX;
        covMat12 -= dataSize*meanX*meanY;
    covMat22 -= dataSize*meanY*meanY;
    covMat21 = covMat12;

    if(covMat11<1e-12) {
        nx = 1.0;
            ny = 0.0;
    }
    else {      //lamda1 is the largest eigen-value of the covariance matrix
               //and is used to compute the eigne-vector corresponding to the smallest
               //eigenvalue, which isn't computed explicitly.
        double lamda1 = (covMat11 + covMat22 + sqrt((covMat11-covMat22)*(covMat11-covMat22) + 4*covMat12*covMat12)) / 2.0;
        nx = -covMat12;
        ny = lamda1 - covMat22;
        norm = sqrt(nx*nx + ny*ny);
        nx/=norm;
        ny/=norm;
    }
    parameters.push_back(nx);
    parameters.push_back(ny);
    parameters.push_back(meanX);
    parameters.push_back(meanY);
}
/*****************************************************************************/
/*
 * Given the line parameters  [n_x,n_y,a_x,a_y] check if
 * [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
 * 通過與已知法線的點乘結果，確定待測點與已知直線的匹配程度；結果越小則越符合，爲
 * 零則表明點在直線上
 */
bool LineParamEstimator::agree(std::vector<double> &parameters, Point2D &data)
{
    double signedDistance = parameters[0]*(data.x-parameters[2]) + parameters[1]*(data.y-parameters[3]);
    return ((signedDistance*signedDistance) < m_deltaSquared);
}

RANSAC尋找匹配的代碼如下：

/*****************************************************************************/
template<class T, class S>
double Ransac<T,S>::compute(std::vector<S> &parameters,
                                                      ParameterEsitmator<T,S> *paramEstimator ,
                                                    std::vector<T> &data,
                                                    int numForEstimate)
{
    std::vector<T *> leastSquaresEstimateData;
    int numDataObjects = data.size();
    int numVotesForBest = -1;
    int *arr = new int[numForEstimate];// numForEstimate表示擬合模型所需要的最少點數，對本例的直線來說，該值爲2
    short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
    short *bestVotes = new short[numDataObjects];  //one if data[i] agrees with the best model, otherwise zero


              //there are less data objects than the minimum required for an exact fit
    if(numDataObjects < numForEstimate)
        return 0;
        // 計算所有可能的直線，尋找其中誤差最小的解。對於100點的直線擬合來說，大約需要100*99*0.5=4950次運算，複雜度無疑是龐大的。一般採用隨機選取子集的方式。
    computeAllChoices(paramEstimator,data,numForEstimate,
                                        bestVotes, curVotes, numVotesForBest, 0, data.size(), numForEstimate, 0, arr);

       //compute the least squares estimate using the largest sub set
    for(int j=0; j<numDataObjects; j++) {
        if(bestVotes[j])
            leastSquaresEstimateData.push_back(&(data[j]));
    }
        // 對局內點再次用最小二乘法擬合出模型
    paramEstimator->leastSquaresEstimate(leastSquaresEstimateData,parameters);

    delete [] arr;
    delete [] bestVotes;
    delete [] curVotes;

    return (double)leastSquaresEstimateData.size()/(double)numDataObjects;
}

在模型確定以及最大迭代次數允許的情況下，RANSAC總是能找到最優解。經過我的實驗，對於包含80%誤差的數據集，RANSAC的效果遠優於直接的最小二乘法。