RANSAC拟合平面(C++版本思路)

参考资料：

[1]内容P62 :https://wenku.baidu.com/view/31d9dc51b90d6c85ed3ac62c.html?pn=51
[2]C++代码：https://github.com/MRPT/mrpt/blob/mrpt-1.5/samples/ransac-demo-plane3D/test.cpp
[3]http://grunt1223.iteye.com/blog/961063
[4]https://blog.csdn.net/pi9nc/article/details/9083485
[5]https://blog.csdn.net/zouwen198317/article/details/38494149
[6]Ransac C++程序下载：http://yanivresearch.info/#software

目前拟合平面的方法和思路：

拟合平面就是估计模型参数，可以采用最小二乘进行估计模型参数，使用的工具有ceres,g2o,以及自己的实现，还有ransac的方法。

相比较最小二乘拟合模型参数，RANSAC的优势：

最小二乘是适应包含局外点在内点所有点。
ransac能得出一个仅仅用局内点计算出来的模型，局外点并不影响效果。

目前的opencv库中的ransac函数是：

目前的pcl库中的ransac函数是：

参考资料：
[1]https://www.cnblogs.com/li-yao7758258/p/6477007.html
[2]http://www.pclcn.org/study/shownews.php?lang=cn&id=62
[3]https://www.cnblogs.com/ironstark/p/4998037.html

RANSAC拟合模型参数整体思路：

//ransac思路：
//1随机选择点，对于拟合平面来说，最少需要3个点，然后计算出模型参数
//2用数据去检验估计的模型，看结果是否小于一定的阈值，得到小于阈值的点的个数，即内点个数
//3内点数量如果大于已经保存的内点数量，更新模型参数，内点较少的模型参数被遗弃，内点多的模型参数被保留
//4 步骤1和步骤3进行重复，不断迭代，从而找到内点个数最多的模型参数
//最后可以用内点做的再次估计模型参数，得到最终的模型参数
//写程序比较复杂
//能用伪代码写吗
//因为调用的库太多了

参考[5]的整体思路：

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

//ZGx
//线参数估计器的构造函数的实现
//成员变量m_deltaSquared是啥意思？
//ZGx

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为直线上任意一点
 */

//ZGx
//data中的每个元素时一个指针
//parameter:斜率和一个点,得到模型参数
//ZGx

void LineParamEstimator::estimate(std::vector<Point2D *> &data,
                                                                    std::vector<double> ¶meters)
{
    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是啥呢？parameter:斜率和一个点,得到模型参数
    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> ¶meters)
{
    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
 * 通过与已知法线的点乘结果，确定待测点与已知直线的匹配程度；结果越小则越符合，为
 * 零则表明点在直线上
 */

//ZGX
//给定数据，判断是符合该模型
//m_deltaSquared是是否符合模型的阈值
//ZGX

bool LineParamEstimator::agree(std::vector<double> ¶meters, Point2D &data)
{
    double signedDistance = parameters[0]*(data.x-parameters[2]) + parameters[1]*(data.y-parameters[3]);
    return ((signedDistance*signedDistance) < m_deltaSquared);
}


//ZGx
//ransac思路：
//1随机选择点，对于拟合曲线来说，最少需要2个点，然后计算出模型参数
//2用数据去检验估计的模型，看结果是否小于一定的阈值，得到小于阈值的点的个数，存起来
//3 步骤1和步骤2不断进行重复，找到内点个数最多的模型参数
//最后可以用内点做的再次估计模型参数，得到最终的模型参数
//写程序比较复杂
//能用伪代码写吗
//因为调用的库太多了
//ZGx

/*****************************************************************************/
template<class T, class S>
double Ransac<T,S>::compute(std::vector<S> ¶meters,
                                                      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;
}