modularization/src/tool/map_lanetoxodr/circlefitting.cpp

#include "circlefitting.h"

CircleFitting::CircleFitting(QList<Data*>idataset)
{
    this->PrimiDataSet.clear();
    this->resultList.clear();
    this->Xc = 0.00;
    this->Yc = 0.00;
    this->R = 0.00;
    for(int i=0;i<idataset.size();i++)
        this->PrimiDataSet<<idataset[i];
    this->fittingData();
}

CircleFitting::~CircleFitting(void)
{
}

void CircleFitting::fittingData()
{
    ////原始数据量
    int N = this->PrimiDataSet.size();
    ////定义用于拟合的参数
    double D,C,E,G,H;
    ////原始数据X点总和
    double sum_X_Raw = 0.00;
    ////原始数据Z点总和
    double sum_Z_Raw = 0.00;
    ////原始数据X的平方和
    double sum_XSquare_Raw = 0.00;
    ////原始数据Z的平方和
    double sum_ZSquare_Raw = 0.00;
    ////原始数据X的立方和
    double sum_XCube_Raw = 0.00;
    ////原始数据Z的立方和
    double sum_ZCube_Raw = 0.00;
    ////原始数据xzz和
    double sum_XZZ_Raw = 0.00;
    ////原始数据xz和
    double sum_XZ_Raw = 0.00;
    ////原始数据xxz和
    double sum_XXZ_Raw = 0.00;

    for(int i = 0;i < this->PrimiDataSet.size();i++)
    {
        sum_X_Raw += PrimiDataSet[i]->x;
        sum_Z_Raw += PrimiDataSet[i]->y;
        sum_XSquare_Raw += PrimiDataSet[i]->x*PrimiDataSet[i]->x;
        sum_ZSquare_Raw += PrimiDataSet[i]->y*PrimiDataSet[i]->y;
        sum_XCube_Raw += PrimiDataSet[i]->x*PrimiDataSet[i]->x*PrimiDataSet[i]->x;
        sum_ZCube_Raw += PrimiDataSet[i]->y*PrimiDataSet[i]->y*PrimiDataSet[i]->y;
        sum_XZ_Raw +=  PrimiDataSet[i]->x*PrimiDataSet[i]->y;
        sum_XZZ_Raw += PrimiDataSet[i]->x*PrimiDataSet[i]->y*PrimiDataSet[i]->y;
        sum_XXZ_Raw += PrimiDataSet[i]->x*PrimiDataSet[i]->x*PrimiDataSet[i]->y;
    }
    D = N*sum_XZ_Raw - sum_X_Raw*sum_Z_Raw;
    C = N*sum_XSquare_Raw-sum_X_Raw*sum_X_Raw;
    E = N*sum_XCube_Raw+N*sum_XZZ_Raw-(sum_XSquare_Raw+sum_ZSquare_Raw)*sum_X_Raw;
    G = N*sum_ZSquare_Raw-sum_Z_Raw*sum_Z_Raw;
    H = N*sum_ZCube_Raw+N*sum_XXZ_Raw-(sum_XSquare_Raw+sum_ZSquare_Raw)*sum_Z_Raw;

    double a = (H*D-E*G)/(C*G-D*D);
    double b = (H*C-E*D)/(D*D-G*C);
    double c = -((sum_XSquare_Raw+sum_ZSquare_Raw)+a*sum_X_Raw+b*sum_Z_Raw)/N;

    double A = -0.5*1.00000*a;
    double B = -0.5*1.00000*b;
    double R = 0.5*1.00000*sqrt(a*a+b*b-4*c);
    this->resultList<<A<<B<<R;
    //qDebug()<<"CircleFitting: ("<<A<<","<<B<<") R="<<R;
}

QList<double> CircleFitting::getResultList()
{
    return this->resultList;
}