RANSAC直線フィットアルゴリズム

1.参考文献
 

RANSAC       

2.アルゴリズム実装

#include 
#include 
#include 
#include 
#include 
// Date: 2018-01-09
// Author: HSW
//

// RANSAC       
//      
//
using namespace std;

typedef struct st_Point
{
    double x;
    double y;
}st_Point;

//   RANSAC       
// pstData:          
// dataCnt:      
// lineParameterK:      
// lineParameterB:      
// minCnt:   (  )             
// maxIterCnt:       
// maxErrorThreshold:       
// consensusCntThreshold:          
// modelMeanError:     
//    ：   0        ，            
int ransacLiner(st_Point* pstData, int dataCnt, int minCnt, double maxIterCnt, int consensusCntThreshold, double maxErrorThreshod, double& A, double& B, double& C, double& modelMeanError)
{
    default_random_engine rng;
    uniform_int_distribution uniform(0, dataCnt - 1);
    rng.seed(10); //        
    set selectIndexs;     //        
    vector selectPoints;      //     
    set consensusIndexs;  //           
    double line_A = 0;
    double line_B = 0;
    double line_C = 0;
    modelMeanError = 0;
    int isNonFind  = 1;
    unsigned int      bestConsensusCnt = 0; //             
    int iter = 0;
    while(iter < maxIterCnt)
    {
        selectIndexs.clear();
        selectPoints.clear();

        // Step1:     minCnt  
        while(1)
        {
            unsigned int index = uniform(rng);
            selectIndexs.insert(index);
            if(selectIndexs.size() == minCnt)
            {
                break;
            }
        }

        // Step2:          (y2 - y1)*x - (x2 - x1)*y + (y2 - y1)x2 - (x2 - x1)y2= 0
        set::iterator selectIter = selectIndexs.begin();
        while(selectIter != selectIndexs.end())
        {
            unsigned int index = *selectIter;
            selectPoints.push_back(pstData[index]);
            selectIter++;
        }
        double deltaY = (selectPoints[1]).y - (selectPoints[0]).y;
        double deltaX = (selectPoints[1]).x - (selectPoints[0]).x;
        line_A = deltaY;
        line_B = -deltaX;
        line_C = -deltaY * (selectPoints[0]).x + deltaX * (selectPoints[0]).y;

        // Step3:       :        
        int dataIter = 0;
        double meanError = 0;
        set tmpConsensusIndexs;
        while(dataIter < dataCnt)
        {
            double distance =  (line_A * pstData[dataIter].x + line_B * pstData[dataIter].y + line_C) / sqrt(line_A*line_A + line_B*line_B);
            distance = distance > 0 ? distance : -distance;
            if(distance < maxErrorThreshod)
            {
                tmpConsensusIndexs.insert(dataIter);
            }
            meanError += distance;
            dataIter++;
        }

        // Step4:      :                  +         
        if(tmpConsensusIndexs.size() >= bestConsensusCnt && tmpConsensusIndexs.size() >= consensusCntThreshold)
        {
            bestConsensusCnt = consensusIndexs.size();  //              
            modelMeanError = meanError / dataCnt;
            consensusIndexs.clear();
            consensusIndexs = tmpConsensusIndexs;        //          
            A = line_A; 
            B = line_B; 
            C = line_C; 
            isNonFind = 0;
        }
        iter++;
    }
    return isNonFind;
}

#define MAX_LINE_CORRECT_POINT_CNT   (40)
#define MAX_LINE_NOISE_POINT_CNT     (5)

int main()
{
    st_Point dataPoints[MAX_LINE_CORRECT_POINT_CNT + MAX_LINE_NOISE_POINT_CNT];
    memset(dataPoints, 0, sizeof(dataPoints));
    int iter;
    for(iter = 0; iter < MAX_LINE_CORRECT_POINT_CNT; ++iter)
    {
        dataPoints[iter].x = iter;
        dataPoints[iter].y = iter*2 + 5; // y = 2 * x + 5   
    }
    int totalCnt = MAX_LINE_CORRECT_POINT_CNT + MAX_LINE_NOISE_POINT_CNT;
    for(iter = MAX_LINE_CORRECT_POINT_CNT; iter < totalCnt; ++iter)
    {
        dataPoints[iter].x = iter;
        dataPoints[iter].y = iter*2 + 1; // y = 2 * x + 1   
    }
    double A = 0;
    double B = 0;
    double C = 0;
    double meanError = 0;
    //       
    if(!ransacLiner(dataPoints, totalCnt, 2, 20, 35, 0.1, A, B, C, meanError))
    {
        cout << "A =         " << A         << endl;
        cout << "B =         " << B         << endl;
        cout << "C =         " << C         << endl;
        cout << "meanError = " << meanError << endl;
    }
    else
    {
        cout << "RANSAC Failed " << endl;
    }
    //     
    if(!ransacLiner(dataPoints, totalCnt, 2, 50, 35, 0.1, A, B, C, meanError))
    {
        cout << "A =         " << A         << endl;
        cout << "B =         " << B         << endl;
        cout << "C =         " << C         << endl;
        cout << "meanError = " << meanError << endl;
    }
    else
    {
        cout << "RANSAC Failed " << endl;
    }
    return 0;
}