modularization/src/tool/map_lanetoxodr/dialoglanefromrtk.cpp

#include "dialoglanefromrtk.h"
#include "ui_dialoglanefromrtk.h"

#include <QFileDialog>

#include "xodrfunc.h"
#include "geofit.h"

extern double glon0 ;
extern double glat0;

DialogLaneFromRTK::DialogLaneFromRTK(Road * pRoad,OpenDrive * pxodr,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogLaneFromRTK)
{
    ui->setupUi(this);
    mpRoad = pRoad;
    mpxodr = pxodr;
    mlon0 = glon0;
    mlat0 = glat0;
    ui->pushButton_CreateLane->setEnabled(false);

}

DialogLaneFromRTK::~DialogLaneFromRTK()
{
    delete ui;
}

void DialogLaneFromRTK::on_pushButton_LoadData_clicked()
{
    QString str = QFileDialog::getOpenFileName(this,tr("Open RTK Data"),"",tr("Lane File(*.csv)"));
    if(str.isEmpty())return;

    QFile xFile;
    xFile.setFileName(str);
    QFileInfo fi(str);
//    QString filename = fi.fileName();
//    if(filename.contains(".txt"))
//    {
//        filename = filename.left(filename.length() -4);
//    }

    ui->plainTextEdit->clear();
    mvectorrtkdata.clear();
    ui->plainTextEdit->appendPlainText(QString("Longitude\tLatitude\tHeight\trelx\trely"));

    mvectorrtkdata = DialogAddRoadFromRTK::loadrtkdata(str,mlon0,mlat0,mpxodr);


    if(mvectorrtkdata.size()>0)
    {
        unsigned int i;
        for(i=0;i<mvectorrtkdata.size();i++)
        {
        QString strline = QString::number(mvectorrtkdata.at(i).mflon,'f',7) + QString("\t")
                +QString::number(mvectorrtkdata.at(i).mflat,'f',7)+QString("\t")
                +QString::number(mvectorrtkdata.at(i).mheight,'f',4)+QString("\t")
                +QString::number(mvectorrtkdata.at(i).mfrelx,'f',2) + QString("\t")
                +QString::number(mvectorrtkdata.at(i).mfrely,'f',2) + QString("\t");
        ui->plainTextEdit->appendPlainText(strline);
        }

        ui->pushButton_CreateLane->setEnabled(true);
    }
}


namespace iv {
struct rtklanedis
{
    double fdis;
    double fhdgdir;
    double fs;
    double frealdis;
    double flanedis;
};
}

void DialogLaneFromRTK::on_pushButton_CreateLane_clicked()
{
    unsigned int i;

    Road * pRoad = 0;
    GeometryBlock * pgeob;
    double fdis,nearx,neary,hdg;
    double fs;
    int nlane;

    pRoad = mpRoad;
    std::vector<iv::rtklanedis> xvectorrktlanedis;

    for(i=0;i<mvectorrtkdata.size();i++)
    {
//        if(xodrfunc::GetNearPoint(mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely,mpxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,100,&fs,&nlane,true) == 0)
//        {
        if(xodrfunc::GetNearPointAtRoad(mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely,pRoad,&pgeob,fdis,nearx,neary,hdg,100,&fs,&nlane,true) == 0)
        {
            double fhdgdir = geofit::CalcHdg(nearx,neary,
                                             mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely);
            fhdgdir  = fhdgdir - hdg;
            while(fhdgdir<0)fhdgdir = fhdgdir + 2.0*M_PI;
            while(fhdgdir>=2.0*M_PI)fhdgdir = fhdgdir - 2.0*M_PI;
            iv::rtklanedis xlanedis;
            xlanedis.fdis = fdis;
            xlanedis.fhdgdir = fhdgdir;
            xlanedis.fs = fs;
            xvectorrktlanedis.push_back(xlanedis);
            qDebug("dis is %f dir is %f",fdis,fhdgdir);
        }
    }

    double favghdg = 0;
    if(xvectorrktlanedis.size() < 1)
    {
        QMessageBox::warning(this,"Warning","Data Error.Maybe these data is not this road.",QMessageBox::YesAll);
        return;
    }
    for(i=0;i<xvectorrktlanedis.size();i++)
    {
        favghdg = favghdg + xvectorrktlanedis.at(i).fhdgdir;
    }

    favghdg = favghdg/xvectorrktlanedis.size();

    double flanedir = M_PI*3.0/2.0;
    bool brightlane = true;   //R
    int nidmark = -1;
    if(favghdg <M_PI)
    {
        brightlane = false;
        flanedir = M_PI/2.0;
        nidmark = 1;
    }

    for(i=0;i<xvectorrktlanedis.size();i++)
    {
        double fxdis = xvectorrktlanedis.at(i).fdis * cos(xvectorrktlanedis.at(i).fhdgdir - flanedir);
        qDebug("dis is %f dir is %f xdis is %f",xvectorrktlanedis.at(i).fdis,
               xvectorrktlanedis.at(i).fhdgdir,
               fxdis);
        xvectorrktlanedis.at(i).frealdis = fxdis;
    }

    LaneSection * pLS = 0;
    if(pRoad->GetLaneSectionCount() == 0)
    {
        pRoad->AddLaneSection(0);
        pLS = pRoad->GetLaneSection(0);
        for(i=0;i<xvectorrktlanedis.size();i++)
        {
            xvectorrktlanedis.at(i).flanedis = xvectorrktlanedis.at(i).frealdis;
        }
    }
    else
    {
        if(pRoad->GetLaneSectionCount() == 1)pLS = pRoad->GetLaneSection(0);
        else
        {
            unsigned int j;
            for(j=0;j<pRoad->GetLaneSectionCount();j++)
            {
                if(xvectorrktlanedis.at(0).fs>= pRoad->GetLaneSection(j)->GetS())
                {
                    pLS = pRoad->GetLaneSection(j);
                    break;
                }
            }
        }

        for(i=0;i<xvectorrktlanedis.size();i++)
        {

            if(pLS->GetLaneCount() > 0)
            {
                unsigned int j;
                for(j=0;j<pLS->GetLaneCount();j++)
                {
                    if(pLS->GetLane(j)->GetId() * nidmark > 0)
                    {
                        Lane * pLane = pLS->GetLane(j);
                        LaneWidth * pLaneWidth = 0;
                        unsigned int k;
                        for(k=0;k<(pLane->GetLaneWidthCount()-1);k++)
                        {
                            if(xvectorrktlanedis.at(i).fs > (pLane->GetLaneWidth(k+1)->GetS()+pLS->GetS()))
                            {
                                pLaneWidth = pLane->GetLaneWidth(k);
                                break;
                            }
                        }
                        if(pLaneWidth == 0)
                        {
                            if(pLane->GetLaneWidthCount()>0)
                            {
                                pLaneWidth = pLane->GetLaneWidth(0);
                            }
                        }

                        if(pLaneWidth != 0)
                        {
                            double fss = xvectorrktlanedis.at(i).fs;
                            fss = fss - pLS->GetS();
                            xvectorrktlanedis.at(i).fs = fss;
                            double fedis = pLaneWidth->GetA() + pLaneWidth->GetB() * fss
                                    +pLaneWidth->GetC()*fss*fss
                                    +pLaneWidth->GetD()*fss*fss*fss;
                            xvectorrktlanedis.at(i).frealdis = xvectorrktlanedis.at(i).frealdis - fedis;
                        }

                    }
                }
            }
        }
    }

    double favgrealdis = 0;

    for(i=0;i<xvectorrktlanedis.size();i++)
    {
        favgrealdis = favgrealdis + xvectorrktlanedis.at(i).frealdis;
    }

    if(xvectorrktlanedis.size() > 0)favgrealdis = favgrealdis/xvectorrktlanedis.size();

    if(favgrealdis<0.1)
    {
        char strout[255];
        snprintf(strout,255,"lane average width is %f, very small,do you want Create Lane?",favgrealdis);
        int nrtn = QMessageBox::warning(this,"Warning",QString(strout),QMessageBox::Yes,QMessageBox::No);
        if(nrtn == QMessageBox::No)
        {
            return;
        }
    }

    Lane * pLane;
    if(brightlane)
    {
        pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
        pLane = pLS->GetLastRightLane();
    }
    else
    {
        pLS->AddLane(1,(1)*(pLS->GetLeftLaneCount()+1),"driving",false);
        pLane = pLS->GetLastLeftLane();
    }


    int nrange = xvectorrktlanedis.size();
    if(nrange == 1)
    {
        pLane->AddWidthRecord(0,xvectorrktlanedis.at(0).frealdis,0,0,0);
    }
    else
    {
        VectorXd x_veh(nrange);
        VectorXd y_veh(nrange);
        unsigned int j;
        for(j=0;j<nrange;j++)
        {
            x_veh[j] = xvectorrktlanedis.at(j).fs;
            y_veh[j] = xvectorrktlanedis.at(j).frealdis;
        }
        int nnum = 3;
        if(nrange<4)nnum = nrange -1;
        VectorXd coeffs = polyfit(x_veh, y_veh, nnum);
        double a[4];
        for(j=0;j<4;j++)a[j] = 0;
        for(j=0;j<nnum;j++)a[j] = coeffs[j];
        pLane->AddWidthRecord(0,a[0],a[1],a[2],a[3]);
    }


    ui->pushButton_CreateLane->setEnabled(false);
}