B-spline 곡선에 Least-Squares Data Fitting

차선 추적을 위해서는 먼저 이미지에서 차선을 추출하고 추출한 차선으로 차선 모델을 갱신하는 과정을 거칩니다. 차선 추적에 대해서는  "Lane Detection and tracking using B-Snake"라는 논문에서 잘 설명되어 있습니다. 여기서는 이미지에서 추출한 차선 데이터(이미지 상의 점들)를 차선 모델(B-spline 곡선으로 표현)에 Least-Squares 방법으로 피팅 하는 방법에 관한 것입니다.

 

다음 문서를 참고하십시오.

SplineFitting.pdf

0.08MB

 

다음은 Least-Squares 방법으로 B-spline 곡선에  fitting 하는 함수입니다. P가 피팅 되어야 할 셈플 점들이고 Q가 B-spline의 제어 점들(control points) 입니다.

inline dMatrix CubicBSplineBasis (sPoint P[], int n_P, int n_Q)
{
    dMatrix N(n_P, n_Q);
    N.null ();

    double m = (n_Q - 1.)/(n_P - 1.);

    for (int r=0; r<n_P; r++) {
        double ct = m*r;
        int c = (int)ct;
        double t = ct - c;

        int c_m1 = (c - 1 < 0      ) ? 0 : c - 1;
        int c_p0 = c + 0;
        int c_p1 = (c + 1 > n_Q - 1) ? n_Q - 1 : c + 1;
        int c_p2 = (c + 2 > n_Q - 1) ? n_Q - 1 : c + 2;

        double tt = t*t;
        double ttt = tt * t;

        N(r,c_m1) +=  (-1*ttt + 3*tt - 3*t + 1)/6.;
        N(r,c_p0) +=  ( 3*ttt - 6*tt + 0*t + 4)/6.;
        N(r,c_p1) +=  (-3*ttt + 3*tt + 3*t + 1)/6.;
        N(r,c_p2) +=  ( 1*ttt + 0*tt + 0*t + 0)/6.;
    }

    return N;
}

inline void LeastSquareBSplineFitting (sPoint P[], int n_P, sPoint Q[], int n_Q)
{
    dMatrix N = CubicBSplineBasis (P, n_P, n_Q);
    dMatrix P_(n_P, 2);

    for (int i=0; i<n_P; ++i) {
        P_(i,0) = P[i].x;
        P_(i,1) = P[i].y;
    }

    dMatrix Q_ = !(~N*N)*~N*P_;

    for (int i=0; i<n_Q; ++i) {
        Q[i].x = Q_(i,0);
        Q[i].y = Q_(i,1);
    }
}

 

다음은 상기 소스코드가 포함된 VC++ 2008 프로젝트 파일입니다.

spline_fitting.zip

0.12MB

 

아래 동영상은 B-spline 곡선의 제어 점 수를 2개에서 셈플 점 수까지 늘여가면서 피팅되는 B-spline 곡선을 그린 것입니다. 초록색 곡선은 Catmull Rom Spline 곡선입니다. 빨간색 점들은 셈플 점들이고 파란색 점들은 B-spline 곡선의 제어 점들입니다. 파란색 곡선은 피팅된 B-spline 곡선입니다.