C++ 算法學習四（直線、拋物線擬合）

好久沒寫博客了，忘了怎麼開場，哈哈，小編在從事車道線檢測，以及機器學習算法線性迴歸時都用到了線性擬合與多項式擬合，其實可以直接通過opencv的API接口也可實現，具體可見

polynomial_curve_fit(points,1, A);

但是嵌入式平臺木有opencv, 於是就詳細研究了一下其中數學原理，用C++ 在嵌入式平臺直接實現了一下；
先上結果圖吧

直線擬合

曲線拋物線擬合

下面我詳細記錄下擬合過程：
（1）
對於一元線性迴歸模型, 假設從總體中獲取了n組觀察值（X1，Y1），（X2，Y2）， …，（Xn，Yn）。對於平面中的這n個點，可以使用無數條曲線來擬合。要求樣本回歸函數儘可能好地擬合這組值。綜合起來看，這條直線處於樣本數據的中心位置最合理。 選擇最佳擬合曲線的標準可以確定爲：使總的擬合誤差（即總殘差）達到最小。有以下三個標準可以選擇：

    （1）用“殘差和最小”確定直線位置是一個途徑。但很快發現計算“殘差和”存在相互抵消的問題。
    （2）用“殘差絕對值和最小”確定直線位置也是一個途徑。但絕對值的計算比較麻煩。
    （3）最小二乘法的原則是以“殘差平方和最小”確定直線位置。用最小二乘法除了計算比較方便外，
    得到的估計量還具有優良特性。這種方法對異常值非常敏感。

    最常用的是普通最小二乘法（ Ordinary  Least Square，OLS）：所選擇的迴歸模型應該使所有
    觀察值的殘差平方和達到最小。（Q爲殘差平方和）

直線方程如下

代碼實現（直線與拋物線寫在一起，註釋#define DEBUG爲直線）

#include <iostream>
#include <stdio.h>
#include <vector>
#include <valarray>
#include <cmath>
#include <opencv2/opencv.hpp>
//#define DEBUG     //直線擬合，關閉則拋物線擬合
using namespace cv;
using namespace std;

//////////////最小二乘法///////////////////

vector<float >least_square_line(vector<Point2i>,int n);

//////////////////////////////////////////

//////////////////////////最小二乘法直線擬合/////////////////////////////////////
vector<float >least_square_line(vector<Point2i>points,int n)
{

    vector<float >result;
    ////////////////////////////////a = (n*C - B*D) / (n*A - B*B)////////////////////////
    /// //////////////////////////// b = (A*D - B*C) / (n*A - B*B)////////////////////
    float A = 0;   //x的平方求和
    float B = 0;   //x求和
    float C = 0;   //xy求和
    float D = 0;   //y求和
    float E = 0;    //樣本個數
    //cout<<"A"<<endl;
    for (int i = 0; i <= n; i++)
    {
        A = A + pow(points[i].x, 2);

        B = B + points[i].x;

        C = C + points[i].x * points[i].y;
        //cout<<C<<endl;
        D = D + points[i].y;
        E = n;

    }
    float a, b, temp = 0;
    if (E * A - B * B == 0) {
        a = 1;
        b = 0;

        cout << "斜率不存在 " << endl;
    }
    else {
        a = (E * C - B * D) / (E * A - B * B);
        b = (A * D - B * C) / (E * A - B * B);

        cout<<"a"<<a<<endl<<"b"<<b<<endl;

    }
    result.push_back(a);
    result.push_back(b);
    return result;
}
////////////////////////////////////////////////////////////////////////////////


//最小二乘擬合相關函數定義
////////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////
///////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////
///////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////
double sum(vector<double> Vnum, int n);

double MutilSum(vector<double> Vx, vector<double> Vy, int n);

double RelatePow(vector<double> Vx, int n, int ex);

double RelateMutiXY(vector<double> Vx, vector<double> Vy, int n, int ex);

void EMatrix(vector<double> Vx, vector<double> Vy, int n, int ex, double coefficient[]);

void CalEquation(int exp, double coefficient[]);

double F(double c[],int l,int m);

double Em[6][4];
////////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////

//累加
double sum(vector<double> Vnum, int n)
{
    double dsum = 0;
    for (int i = 0; i < n; i++) {
        dsum += Vnum[i];
    }
    return dsum;
}

//乘積和
double MutilSum(vector<double> Vx, vector<double> Vy, int n)
{
    double dMultiSum=0;
    for (int i=0; i<n; i++)
    {
        dMultiSum+=Vx[i]*Vy[i];
    }
    return dMultiSum;
}

//ex次方和
double RelatePow(vector<double> Vx, int n, int ex)
{
    double ReSum=0;
    for (int i=0; i<n; i++)
    {
        ReSum+=pow(Vx[i],ex);
    }
    return ReSum;
}

//x的ex次方與y的乘積的累加
double RelateMutiXY(vector<double> Vx, vector<double> Vy, int n, int ex)
{
    double dReMultiSum=0;
    for (int i=0; i<n; i++)
    {
        dReMultiSum+=pow(Vx[i],ex)*Vy[i];
    }
    return dReMultiSum;
}

//計算方程組的增廣矩陣
void EMatrix(vector<double> Vx, vector<double> Vy, int n, int ex, double coefficient[])
{
    for (int i=1; i<=ex; i++)
    {
        for (int j=1; j<=ex; j++)
        {
            Em[i][j]=RelatePow(Vx,n,i+j-2);
        }
        Em[i][ex+1]=RelateMutiXY(Vx,Vy,n,i-1);
    }
    Em[1][1]=n; CalEquation(ex,coefficient);
}

//求解方程
void CalEquation(int exp, double coefficient[])
{
    for(int k=1;k<exp;k++) //消元過程
    {
        for(int i=k+1;i<exp+1;i++)
        {
            double p1=0;
            if(Em[k][k]!=0)
                p1=Em[i][k]/Em[k][k];
            for(int j=k;j<exp+2;j++)
                Em[i][j]=Em[i][j]-Em[k][j]*p1;
        }
    }
    coefficient[exp]=Em[exp][exp+1]/Em[exp][exp];
    for(int l=exp-1;l>=1;l--)
        //回代求解
        coefficient[l]=(Em[l][exp+1]-F(coefficient,l+1,exp))/Em[l][l];
}


//供CalEquation函數調用
double F(double c[],int l,int m)
{
    double sum=0;
    for(int i=l;i<=m;i++)
        sum+=Em[l-1][i]*c[i];
    return sum;
}
///////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////
///////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////
///////////////////////////最小二乘法曲線你擬合////////////////////////////////////////////////////


int main()
{

    cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC3);

    vector<Point2i> points;

    vector<float >slope;

    cv::Vec4f line_para;

    float num;
#ifdef DEBUG
    float  x[10]={0,10,20,30,40,50,60,70,80,90};
    float z[10]={0,20.2,40.1,65,83,100.1,118,136,157,176};

    for(int l=0;l<=9;l++)
    {
        circle(image,Point2f(x[l],z[l]),3,Scalar(255,0,0),1,8);
        Point2f pt(x[l],z[l]);
        points.push_back(pt);
    }


    slope=least_square_line(points,10);
    fitLine(points,line_para,cv::DIST_L2, 0, 1e-2, 1e-2);

    ///////////////////////////////////////////////////////////////////////
    //////////////////////////opencv  fitline/////////////////////////////////////
    float k=line_para[1]/line_para[0];
    cv::Point point1, point2,point0;
    point0.x = line_para[2];
    point0.y = line_para[3];
    //計算直線的端點(y = k(x - x0) + y0)
    cv::Point point3, point4;
    point3.x = 0;
    point3.y = k * (0 - point0.x) + point0.y;
    point4.x = 640;
    point4.y = k * (640 - point0.x) + point0.y;
    cv::line(image, point3, point4, cv::Scalar(255, 0, 0), 2, 8, 0);
    //////////////////////////opencv  fitline/////////////////////////////////////



    //////////////////////////最小二乘法/////////////////////////////////////
    point1.x = 0;
    point1.y = slope[1];
    point2.y = 480;
    point2.x =  (480 - slope[1]) /slope[0];
    cout<<point1;
    cout<<point2;

    cv::line(image, point1, point2, cv::Scalar(0, 0, 255), 2, 8, 0);

    //////////////////////////最小二乘法/////////////////////////////////////
#else
    double  y[10]={0,40,50,60,70,80,190};
    double h[10]={0,64.2,100.1,144,192,254,377};

    double coffocoent[5];
    memset(coffocoent,0, sizeof(double)*5);
    vector<double >vx,vy;
    for (int k = 0; k <7 ; ++k)
    {
        circle(image,Point2f(y[k],h[k]),3,Scalar(0,255,0),3,8);
        vx.push_back(y[k]);
        vy.push_back(h[k]);
    }
    double time1 = static_cast<double>(getTickCount());  //記錄起始時間
        EMatrix(vx,vy,7,3,coffocoent);
    time1=((double)getTickCount()-time1)/getTickFrequency();   //計算程序運行時間
    cout<<"此方法運行時間爲："<<time1<<"秒"<<endl;

    vector<Point>points_fitted;
   for (int p = 0; p <5; ++p) {
        cout<<coffocoent[p]<<endl;

    }
    printf("擬合方程爲：y = %lf + %lfx + %lfx^2 \n",coffocoent[1],coffocoent[2],coffocoent[3]);

    Mat_<double >A(3,1);
    A.at<double>(0,0)=coffocoent[1];
    A.at<double>(1,0)=coffocoent[2];
    A.at<double>(2,0)=coffocoent[3];

   for (int x = 0; x < 640; x++)
   {
       double y = A.at<double>(0, 0) + A.at<double>(1, 0) * x + A.at<double>(2, 0) * std::pow(x, 2) ;

       points_fitted.push_back(cv::Point(x,y));
   }
       cv::polylines(image, points_fitted, false, cv::Scalar(0, 255, 255), 2, 8, 0);
#endif
       cv::imshow("image", image);
       cv::waitKey(0);

       return 0;
   }