Geometry & Recognition

RANSAC 알고리즘을 써서 주어진 2차원 점집합에서 원을 추정한다. 원을 만들기 위해서는 최소한 3점이 필요하고, 또 일직선에 있지 않아야 한다. 이렇게 만들어진 원은 세 점을 꼭짓점으로 하는 삼각형의 외접원이다. 주어진 외접원에서 크게 벗어나지 않는 inliers를 찾고, 이 inliers에 대해 최소자승법으로 원의 중심과 반지름을 다시 추정한다. 무작위로 선택된 세 점에 대해 위의 과정을 반복 시행해서 구한 원 중에서 inliers의 벗어남 편차가 최소인 것을 결과로 선택한다.
// 참고: http://en.wikipedia.org/wiki/RANSAC 
//

// [0, max_num) 사이에서 3개의 숫자를 무작위로 선택함;
void GetRandomTriplet(int max_num, int triplet [3]);

더보기

void getRandTriple(int max_num, int triplet[3]){
    // [0, max_num] select 3 numbers;
    int index = 0;
    while (index < 3) {
        BOOL newone = TRUE;
        int r = (int)((double)(rand()) / RAND_MAX * max_num);
        for (int i = 0; i < index; ++i)
            if (r == triplet[i]) { newone = FALSE; break; }
        if (newone) triplet[index++] = r;
    }   
}

// 세 점의 외접원을 구함;
int CircumCircle(double tx[3], double ty[3], double cparams[4]) ;

더보기

int circumCircle(double tx[3], double ty[3], double cparam[4]) {
    double x1 = tx[0], x2 = tx[1], x3 = tx[2];
    double y1 = ty[0], y2 = ty[1], y3 = ty[2];
    double bax = x2 - x1, bay = y2 - y1;
    double cax = x3 - x1, cay = y3 - y1;
    double E = bax * (x1 + x2) + bay * (y1 + y2);
    double F = cax * (x1 + x3) + cay * (y1 + y3);
    double G = 2. * (bax * (y3 - y2) - bay * (x3 - x2));
    if (G == 0.) return 0; //error;
    //assert(fabs(G)>small_epsilon); //to prevent collinear or degenerate case;
    cparams[0] = (cay * E - bay * F) / G; //cx;
    cparams[1] = (bax * F - cax * E) / G; //cy;
    cparams[2] = hypot(cparams[0]-x1, cparam[1]-y1); //radius;
    return 1;
}

// 3x3 선형 방정식을 푼다: A * x = b;
bool SolveLinearEQ3x3(double A [9], double bb [3], double x [3]) ;

더보기

bool SolveLinearEQ3x3(double A[9], double bb[3], double x[3]) {
    double invA[9];
    double det=(A[0]*(A[4]*A[8]-A[5]*A[7])-A[1]*(A[3]*A[8]-A[5]*A[6])+A[2]*(A[3]*A[7]-A[4]*A[6])) ;
    if (det != 0.) {
        det = 1./det;
        invA[0] = (A[4] * A[8] - A[5] * A[7]) * det;
        invA[1] = (A[2] * A[7] - A[1] * A[8]) * det;
        invA[2] = (A[1] * A[5] - A[2] * A[4]) * det;
        invA[3] = (A[5] * A[6] - A[3] * A[8]) * det;
        invA[4] = (A[0] * A[8] - A[2] * A[6]) * det;
        invA[5] = (A[2] * A[3] - A[0] * A[5]) * det;
        invA[6] = (A[3] * A[7] - A[4] * A[6]) * det;
        invA[7] = (A[1] * A[6] - A[0] * A[7]) * det;
        invA[8] = (A[0] * A[4] - A[1] * A[3]) * det;
        //
        x[0] = invA[0] * bb[0] + invA[1] * bb[1] + invA[2] * bb[2];
        x[1] = invA[3] * bb[0] + invA[4] * bb[1] + invA[5] * bb[2];
        x[2] = invA[6] * bb[0] + invA[7] * bb[1] + invA[8] * bb[2];
        return true;
    } else {
        x[0] = x[1] = x[2] = 0;
        return false;
    }
}

// x^2 + y^2 + A*x + B*y + C = 0;
// Least square fit for A, B, C;(참조: kipl.tistory.com/207)
int CircleFit_LS(std::vector<double>& xp, std::vector<double>& yp, double cparams[4]) ; 

더보기

int circleFit_LS(std::vector<double>& xp, std::vector<double>& yp, double cparams[4]) {  
    double sx = 0, sy = 0, sxx = 0, sxy = 0, syy = 0;
    double sxxx = 0, sxxy = 0, sxyy = 0, syyy = 0 ;
    for (int i = xp.size(); i-->0;) {
        double x = xp[i], y = yp[i];
        double xx = x * x, yy = y * y;
        sx += x;        sy += y;
        sxx += xx;      sxy += x*y ;       syy += yy ;
        sxxx += xx * x; sxxy += xx * y;
        sxyy += x * yy; syyy += yy * y; 
    } ;
    double A[9], b[3], sol[3];
    A[0] = sxx, A[1] = sxy, A[2] = sx,
    A[3] = sxy, A[4] = syy, A[5] = sy,
    A[6] = sx,  A[7] = sy,  A[8] = N,
    b[0] = -sxxx - sxyy,
    b[1] = -sxxy - syyy,
    b[2] = -sxx - syy;
    if (!SolveLinearEQ3x3(A, b, sol)) return 0;
    double det = sol[0] * sol[0] + sol[1] * sol[1] - 4 * sol[2];
    if (det <= 0) return 0;
    cparams[0] = -sol[0] / 2 ; //cx;
    cparams[1] = -sol[1] / 2 ; //cy;   
    cparams[2] = sqrt(det)/2.; //radius;
    return 1;
};

// 주어진 원에서 임계 거리 이내의 데이터만 골라냄;

double findInlier(std::vector<double>& xp, std::vector<double>& yp,
                double cparams[4], double dist_th,
                std::vector<double>& xinliers, std::vector<double>& yinliers) {
    xinliers.clear(); yinliers.clear();    
    double rms = 0;           // root-mean-square;
    double cx = cparams[0], cy = cparams[1], rad = cparams[2];
    for (int k = xp.size(); k-->0;) {
        double dist_dev = fabs(hypot(xp[k]-cx, yp[k]-cy)-rad)/rad ;
        if (dist_dev <= dist_th) { // collect maybe_inliers;
            xinliers.push_back(xp[k]);
            yinliers.push_back(yp[k]);
            rms += SQR(dist - rad);
        }
    }
    return sqrt(rms / xinliers.size());
}

int RansacCircleFit(std::vector<double>& xp, std::vector<double>& yp,
                    int sample_th,      // # of inliers; >= 50% of data(N), 66%; 
                    double dist_th,     // 25% of radius;   |dist-rad|/rad< dist_th
                    int max_iter,
                    double cparams[4]) {    
    double pr = double(sample_th) / double(xp.size());
    double trials99 = log(1. - 0.99)/log(1. - pow(pr, 3));
    int iter = min(max_iter, trials99);
    bool found = false;   
    double min_rms = 1.e+10;
    double cx, cy, rad;
    if (sample_th < 3) sample_th = 3;
    while (iter--) { 
        int tri[3]; 
        double tx[3], ty[3];
        getRandTriple(xp.size()-1, tri);
        for (int i = 0; i < 3; i++)
            tx[i] = xp[tri[i]], ty[i] = yp[tri[i]];
            
        if (!circumCircle(tx, ty, cparams)  
            // if three points are degenerate or on a line, discard them!
            continue ;
        std::vector<double> consensusx, consensusy;
        sdev = findInlier(xp, yp, cparams, dist_th, consensusx, consensusy);
        if (consensusx.size() >= sample_th) {          
            // estimate model params using the maybe-inliers;
            if (!circleFit_LS(consensusx, consensusy, cparams)) 
                continue; // least-square fitting fails;
            // collect real-inliers;
            double rms = findInlier(xp, yp, cparams, dist_th/2, consensusx, consensusy); 
            if (consensusx.size() < sample_th) continue;
            // estimate model params using inliers again;
            if (!circleFit_LS(consensusx, consensusy, cparams)) continue;            
            TRACE("cx = %f, cy = %f, rad=%f\n", cparams[0], cparams[1], cparams[2]);
            if (rms < min_rms) {
                min_rms = rms; found = true;
                // we need more elaborate termination conditions;
#if _DEBUG
#endif
            }
        }
    }
    return found;
};

• sample_th = 2 * N / 3;
• dist_deviate_th = 0.25;
• 파란색: 최소자승법을 이용한 피팅 (outlier의 영향을 많이 받음);
• 붉은색: RANSAC 피팅 결과 (2017.01.04 수정)

see also http://blog.naver.com/helloktk/80029898273