change map_lanetoxodr,complete nds data.

src/tool/map_lanetoxodr/dialogaddroadfromnds.cpp

@@ -1,11 +1,22 @@
 #include "dialogaddroadfromnds.h"
 #include "ui_dialogaddroadfromnds.h"
 
-DialogAddRoadFromNDS::DialogAddRoadFromNDS(QWidget *parent) :
+
+#include <QFileDialog>
+#include <QMessageBox>
+
+DialogAddRoadFromNDS::DialogAddRoadFromNDS(OpenDrive * pxodr,QWidget *parent) :
     QDialog(parent),
     ui(new Ui::DialogAddRoadFromNDS)
 {
+    mpxodr = pxodr;
     ui->setupUi(this);
+    connect(this,SIGNAL(ndsproccomplete(int)),this,SLOT(onNDSComplete(int)));
+    mpTimer = new QTimer(this);
+    connect(mpTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
+    ui->progressBar->setRange(0,100);
+    ui->progressBar->setValue(0);
+    setWindowTitle("Road From NDS");
 }
 
 DialogAddRoadFromNDS::~DialogAddRoadFromNDS()
@@ -13,7 +24,41 @@ DialogAddRoadFromNDS::~DialogAddRoadFromNDS()
     delete ui;
 }
 
+void DialogAddRoadFromNDS::onNDSComplete(int nrtn)
+{
+    QString strres;
+    if(nrtn ==0)strres = "Successfully";
+    else strres = "Fail. Code = "+QString::number(nrtn);
+
+    if(nrtn == 0)QMessageBox::information(this,"NDS",strres,QMessageBox::YesAll);
+    else QMessageBox::warning(this,"NDS",strres, QMessageBox::YesAll);
+    onTimer();
+    mpTimer->stop();
+    ui->pushButton->setEnabled(true);
+}
+
 void DialogAddRoadFromNDS::on_pushButton_clicked()
 {
+    QString strfilevehicle = QFileDialog::getOpenFileName(this,tr("Open vehicle file"),"",tr("Vehicle File(*.csv)"));
+    if(strfilevehicle.isEmpty())return;
+    QString strfileline = QFileDialog::getOpenFileName(this,tr("Open line file"),"",tr("line File(*.csv)"));
+    if(strfileline.isEmpty())return;
 
+    mndsproc.mnProc = 0;
+    mndsproc.mstrState = "Initialzing";
+    ui->pushButton->setEnabled(false);
+    mpthread = new std::thread(&DialogAddRoadFromNDS::updateProgress,this,strfileline,strfilevehicle);
+    mpTimer->start(100);
+}
+
+void DialogAddRoadFromNDS::updateProgress(QString strfileline,QString strfilevehicle)
+{
+    int nrtn = mndsproc.ProcNDSData(strfileline.toStdString(),strfilevehicle.toStdString(),mpxodr);
+    emit ndsproccomplete(nrtn);
+}
+
+void DialogAddRoadFromNDS::onTimer()
+{
+    ui->progressBar->setValue(mndsproc.mnProc);
+    ui->lineEdit_State->setText(mndsproc.mstrState.data());
 }

src/tool/map_lanetoxodr/dialogaddroadfromnds.h

@@ -2,6 +2,12 @@
 #define DIALOGADDROADFROMNDS_H
 
 #include <QDialog>
+#include <QTimer>
+#include "OpenDrive/OpenDrive.h"
+
+#include "ndsdataproc.h"
+
+#include <thread>
 
 namespace Ui {
 class DialogAddRoadFromNDS;
@@ -12,14 +18,27 @@ class DialogAddRoadFromNDS : public QDialog
     Q_OBJECT
 
 public:
-    explicit DialogAddRoadFromNDS(QWidget *parent = nullptr);
+    explicit DialogAddRoadFromNDS(OpenDrive * pxodr,QWidget *parent = nullptr);
     ~DialogAddRoadFromNDS();
 
+    void updateProgress(QString strfileline,QString strfilevehicle);
+
 private slots:
     void on_pushButton_clicked();
+    void onNDSComplete(int nrtn);
+    void onTimer();
+
+signals:
+    void ndsproccomplete(int);
 
 private:
     Ui::DialogAddRoadFromNDS *ui;
+    OpenDrive * mpxodr;
+
+    NDSDataProc mndsproc;
+
+    std::thread * mpthread;
+    QTimer * mpTimer;
 };
 
 #endif // DIALOGADDROADFROMNDS_H

src/tool/map_lanetoxodr/dialogaddroadfromnds.ui

@@ -16,8 +16,8 @@
   <widget class="QPushButton" name="pushButton">
    <property name="geometry">
     <rect>
-     <x>140</x>
-     <y>160</y>
+     <x>50</x>
+     <y>40</y>
      <width>191</width>
      <height>41</height>
     </rect>
@@ -26,6 +26,42 @@
     <string>From NDS</string>
    </property>
   </widget>
+  <widget class="QProgressBar" name="progressBar">
+   <property name="geometry">
+    <rect>
+     <x>50</x>
+     <y>200</y>
+     <width>461</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="value">
+    <number>24</number>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_State">
+   <property name="geometry">
+    <rect>
+     <x>180</x>
+     <y>120</y>
+     <width>261</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>50</x>
+     <y>127</y>
+     <width>91</width>
+     <height>30</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>State:</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/geofit.cpp

@@ -959,6 +959,11 @@ int geofit::CreateBezier(double xstart, double ystart, double hdg_start, double
     v[0] = b0;v[1] = b1;v[2] = b2;v[3] = b3;
     std::cout<<" u0:"<<u[0]<<std::endl;
 
+    *flen = s;
+//    if(s> 1000)
+//    {
+//        std::cout<<" s : "<<s<<std::endl;
+//    }
     return 0;
 }
 
@@ -985,6 +990,7 @@ std::vector<geobase> geofit::CreateBezierGeo(double startx, double starty, doubl
     xgeobezier.mfX = startx;
     xgeobezier.mfY = starty;
     xgeobezier.mnType = 2;
+    xgeobezier.mfLen = fblen;
     }
     else
     {

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -88,7 +88,7 @@ MainWindow::MainWindow(QWidget *parent) :
 
     setWindowTitle("Create Map From Lane Info");
 
-    NDSDataProc::ProcNDSData("/home/yuchuli/下载/nds-sync-line.csv","/home/yuchuli/下载/nds-sync-vehicle.csv");
+//    NDSDataProc::ProcNDSData("/home/yuchuli/下载/nds-sync-line.csv","/home/yuchuli/下载/nds-sync-vehicle.csv");
 
 }
 
@@ -5848,6 +5848,6 @@ void MainWindow::on_actionEdit_Road_Noavoid_triggered()
 
 void MainWindow::on_actionAdd_Road_From_NDS_triggered()
 {
-    DialogAddRoadFromNDS rnd(this);
+    DialogAddRoadFromNDS rnd(&mxodr, this);
     rnd.exec();
 }

src/tool/map_lanetoxodr/ndsdataproc.cpp

@@ -2,9 +2,13 @@
 
 #include <QFile>
 #include "gnss_coordinate_convert.h"
+#include <QDateTime>
 
 #include "geofit.h"
 #include "circlefitting.h"
+#include "mainwindow.h"
+
+extern MainWindow * gw;
 
 NDSDataProc::NDSDataProc()
 {
@@ -16,19 +20,25 @@ NDSDataProc::NDSDataProc()
 //-2 can't open vehicle data file
 //-3 no valid line data.
 //-4 no valid vehicle data.
-int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
+int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath,OpenDrive * mpxodr)
 {
+    mnProc = 0;
+    mstrState = "Loading Data.";
     QFile xFileline;
     QFile xFilevehicle;
     xFileline.setFileName(strlinepath.data());
     xFilevehicle.setFileName(strvehiclepath.data());
     if(!xFileline.open(QIODevice::ReadOnly))
     {
+        mnProc = 100;
+        mstrState = " Can't Read Line File. ";
         return -1;
     }
     if(!xFilevehicle.open(QIODevice::ReadOnly))
     {
         xFileline.close();
+        mnProc = 100;
+        mstrState = " Can't Read Vehicle File. ";
         return -2;
     }
 
@@ -70,6 +80,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
     {
         xFileline.close();
         xFilevehicle.close();
+        mnProc = 100;
+        mstrState = " No Line Data. ";
         return -3;
     }
 
@@ -109,15 +121,20 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 
     if(xvectorvehicle.size()<2)
     {
+        mnProc = 100;
+        mstrState = " No Vehicle Data. ";
         return -4 ;
     }
 
 
     double flon0,flat0;
 
-    flon0 = xvectorvehicle[0].lon;
-    flat0 = xvectorvehicle[0].lat;
-
+    if(mpxodr->GetRoadCount() == 0)
+    {
+        flon0 = xvectorvehicle[0].lon;
+        flat0 = xvectorvehicle[0].lat;
+        mpxodr->SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,flat0,flon0,360);
+    }
     double x0,y0;
     GaussProjCal(flon0,flat0,&x0,&y0);
 
@@ -125,6 +142,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
     int nlinecount = xvectorline.size();
     int nvehiclecount = xvectorvehicle.size();
 
+    mnProc = 10;
+    mstrState = " Calulation x y. ";
     for(i=0;i<nvehiclecount;i++)
     {
         iv::nds_vehicle * p;
@@ -148,6 +167,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 
     int nlinenow = 0;
 
+    mnProc = 20;
+    mstrState = " Get Vehicle And Line Relation. ";
     for(i=0;i<nvehiclecount;i++)
     {
         if(nlinenow >= nlinecount)
@@ -206,6 +227,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
     double fdismin = 100000;
     double fdismax = 0;
 
+
     int nvlsize = xvectorvl.size();
     fS = 0;
     for(i=1;i<nvlsize;i++)
@@ -213,6 +235,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
         iv::nds_vehicle v1 = xvectorvehicle[xvectorvl[i-1].mvehindex];
         iv::nds_vehicle v2 = xvectorvehicle[xvectorvl[i].mvehindex];
         double fdis = sqrt(pow(v1.frel_x - v2.frel_x,2) + pow(v1.frel_y - v2.frel_y,2));
+        xvectorvehicle[xvectorvl[i].mvehindex].fdistolast = fdis;
         fS = fS + fdis;
         if(fdis<fdismin)fdismin = fdis;
         if(fdis>fdismax)fdismax = fdis;
@@ -227,6 +250,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
         if(fhdg<0)fhdg = fhdg + 2.0*M_PI;
         pv1->fhdg = fhdg;
         iv::nds_line * pline1 = &xvectorline[xvectorvl[i].mvectorlineindex[0]];
+        pv1->flanewidth = pline1->lanewidth;
         iv::nds_line * pline2 = NULL;
         if(xvectorvl[i].mvectorlineindex.size()>=2)
         {
@@ -256,6 +280,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
     }
 
 
+    mnProc = 30;
+    mstrState = " Fitting Geo. ";
 
     std::cout<<" fdis "<<fS<<" min "<<fdismin<<" max "<<fdismax<<std::endl;
 
@@ -324,7 +350,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 
         if((nrange <= 2)||bLineOk||bArcOk)
         {
-            if(bLineOk)
+            if((bLineOk)||(nrange<=2))
             {
                 std::cout<<"use line"<<std::endl;
                 geobase xgeo;
@@ -345,24 +371,12 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                 xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
                 xgeo.mnType = 0;
                 fStartX = x0;
-                fStartX = y0;
+                fStartY = y0;
                 fStartHdg = xgeo.mfHdg;
                 fEndX = x1;
                 fEndY = y1;
-                fEndHdg = fStartHdg;
-//                if(bFirst)
-//                {
-//                    xgeo.mfX = x_veh[0];
-//                    xgeo.mfY = y_veh[0];
-//                    bFirst = false;
-//                }
-//                else
-//                {
-//                    xgeo.mfX = fXLast;
-//                    xgeo.mfY = fYLast;
-//                }
-//                fXLast = xgeo.mfX + xgeo.mfLen * cos(xgeo.mfHdg);
-//                fYLast = xgeo.mfY + xgeo.mfLen * sin(xgeo.mfHdg);
+                fEndHdg = xgeo.mfHdg;
+
                 xvectorgeo.push_back(xgeo);
             }
             else
@@ -389,23 +403,6 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                     fEndX = xgeo.mfEndX;
                     fEndY = xgeo.mfEndY;
                     fEndHdg = xgeo.mfHdgEnd;
-//                    if(bFirst)
-//                    {
-//                        xgeo.mfX = x_veh[0];
-//                        xgeo.mfY = y_veh[0];
-//                        xgeo.mfEndX = ep.x() + (x_veh[0] - sp.x());
-//                        xgeo.mfEndY = ep.y() + (y_veh[0] - sp.y());
-//                        bFirst = false;
-//                    }
-//                    else
-//                    {
-//                        xgeo.mfX = fXLast;
-//                        xgeo.mfY = fYLast;
-//                        xgeo.mfEndX = ep.x() + (fXLast - sp.x());
-//                        xgeo.mfEndY = ep.y() + (fYLast - sp.y());
-//                    }
-//                    fXLast = xgeo.mfEndX;
-//                    fYLast = xgeo.mfEndY;
                     xvectorgeo.push_back(xgeo);
                 }
             }
@@ -417,6 +414,8 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                                                                           fLastHdg,fStartX,fStartY,fStartHdg);
                 if(xvectorgeobe.size()>0)
                 {
+                    xvectorgeobe[0].mnStartPoint = ncurpos-1;
+                    xvectorgeobe[0].mnEndPoint = ncurpos;
                     xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
                 }
             }
@@ -439,14 +438,298 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
                 nrange = nrange -1;
             if(nrange<2)nrange = 2;
         }
-        if(ncurpos>=(xvectorvl.size()-1))bComplete = true;
+        if(ncurpos>=(int)(xvectorvl.size()-1))
+        {
+            if(ncurpos == (int)(xvectorvl.size()-1))
+            {
+                std::cout<<"Last section, 2 Points, use line."<<std::endl;
+                VectorXd x_veh1(2);
+                VectorXd y_veh1(2);
+                x_veh1[0] = fLastX;
+                y_veh1[0] = fLastY;
+                x_veh1[1] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
+                y_veh1[1] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].fy;
+                auto coeffs = polyfit(x_veh1, y_veh1, 1);
+
+                geobase xgeo;
+                xgeo.mnStartPoint = ncurpos-1;
+                xgeo.mnEndPoint = ncurpos;
+
+                xgeo.mfA = coeffs[1];
+                xgeo.mfB = -1;
+                xgeo.mfC = coeffs[0];
+                xgeo.mfHdg = geofit::Inst().CalcHdg(x_veh1[0],y_veh1[0],x_veh1[1],y_veh1[1]);
+                xgeo.mfX = x_veh1[0];
+                xgeo.mfY = y_veh1[0];
+                xgeo.mfLen = sqrt(pow(x_veh1[1]-x_veh1[0],2)+pow(y_veh1[1]-y_veh1[0],2));
+                xgeo.mnType = 0;
+                xvectorgeo.push_back(xgeo);
+            }
+            bComplete = true;
+        }
+    }
+
+    double snow = 0;
+    for(i=0;i<(int)xvectorgeo.size();i++)
+    {
+        geobase * pgeo = &xvectorgeo[i];
+//        std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
+        double tems = 0;
+        for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
+        {
+            xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
+            tems = tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
+        }
+        snow = snow + pgeo->mfLen;
+    }
+
+
+    mnProc = 90;
+    mstrState = " Fitting Width and Height. ";
+    //Width
+    std::vector<iv::widthabcd> xvectorwidthabcd;
+    bComplete = false;
+    ncurpos = 0;
+    nrange = xvectorvl.size();
+    double flanewidtherror = 0.1; //1cm
+    while(bComplete == false)
+    {
+        double ele_coff[4];
+        for(j=0;j<4;j++)ele_coff[j] = 0;
+        if(nrange>0)ele_coff[0] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].flanewidth;
+        int M = nrange;
+        VectorXd x_vehhg(M);
+        VectorXd y_vehhg(M);
+        for(j=0;j<M;j++)
+        {
+            x_vehhg[j] = xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            y_vehhg[j] =xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].flanewidth;
+        }
+        int MX = 3;
+        if(M<4)MX = M -1;
+        if(MX>3)MX = 3;
+        if(MX>0)
+        {
+            VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
+            for(j=0;j<=MX;j++)
+            {
+                ele_coff[j] = coeffs[j];
+            }
+        }
+        double ferror = 0;
+        for(j=0;j<M;j++)
+        {
+            double s = x_vehhg[j];
+            double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
+            if(fvalue>ferror)ferror = fvalue;
+        }
+        if(ferror>flanewidtherror)
+        {
+            if(nrange>10)nrange = nrange/2;
+            else nrange = nrange -1;
+        }
+        else
+        {
+            iv::widthabcd xabcd;
+            xabcd.s = xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            xabcd.A = ele_coff[0];
+            xabcd.B = ele_coff[1];
+            xabcd.C = ele_coff[2];
+            xabcd.D = ele_coff[3];
+            xvectorwidthabcd.push_back(xabcd);
+            ncurpos = ncurpos + nrange-1;
+            nrange = xvectorvl.size() - ncurpos;
+        }
+
+        if(ncurpos>=(int)(xvectorvl.size()-1))
+        {
+            bComplete = true;
+        }
+    }
+
+    std::vector<iv::eleabcd> xvectoreleabcd;
+    bComplete = false;
+    ncurpos = 0;
+    nrange = xvectorvl.size();
+    double feleerror = 0.5; //1cm
+    while(bComplete == false)
+    {
+        double ele_coff[4];
+        for(j=0;j<4;j++)ele_coff[j] = 0;
+        if(nrange>0)ele_coff[0] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].height;
+        int M = nrange;
+        VectorXd x_vehhg(M);
+        VectorXd y_vehhg(M);
+        for(j=0;j<M;j++)
+        {
+            x_vehhg[j] = xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            y_vehhg[j] =xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].height;
+        }
+        int MX = 3;
+        if(M<4)MX = M -1;
+        if(MX>3)MX = 3;
+        if(MX>0)
+        {
+            VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
+            for(j=0;j<=MX;j++)
+            {
+                ele_coff[j] = coeffs[j];
+            }
+        }
+        double ferror = 0;
+        for(j=0;j<M;j++)
+        {
+            double s = x_vehhg[j];
+            double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
+            if(fvalue>feleerror)ferror = fvalue;
+        }
+        if(ferror>flanewidtherror)
+        {
+            if(nrange>10)nrange = nrange/2;
+            else nrange = nrange -1;
+        }
+        else
+        {
+            iv::eleabcd xabcd;
+            xabcd.s = xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            xabcd.A = ele_coff[0];
+            xabcd.B = ele_coff[1];
+            xabcd.C = ele_coff[2];
+            xabcd.D = ele_coff[3];
+            xvectoreleabcd.push_back(xabcd);
+            ncurpos = ncurpos + nrange-1;
+            nrange = xvectorvl.size() - ncurpos;
+        }
+
+        if(ncurpos>=(int)(xvectorvl.size()-1))
+        {
+            bComplete = true;
+        }
     }
 
+    std::cout<<" last s: "<<xvectorvehicle[xvectorvl[nvlsize-1].mvehindex].s<<std::endl;
+
     std::cout<<" geo size "<<xvectorgeo.size()<<std::endl;
 
 
     std::cout<<"complete "<<std::endl;
 
+    mpxodr->AddRoad("",xvectorvehicle[xvectorvl[nvlsize-1].mvehindex].s, QString::number(gw->CreateRoadID()).toStdString(),"-1");
+    Road * p = mpxodr->GetRoad(mpxodr->GetRoadCount() - 1);
+
+  //  p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
+
+    double s = 0;
+    j= 0;
+//        for(j=0;j<4;j++)
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+
+        p->AddGeometryBlock();
+        GeometryBlock * pgb = p->GetGeometryBlock(j);
+
+        geobase * pline;
+        geobase * pbez;
+        geobase * parc;
+
+        switch(xvectorgeo[j].mnType)
+        {
+        case 0:
+            pline = &xvectorgeo[j];
+            pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+            break;
+        case 1:
+            parc = &xvectorgeo[j];
+            pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+            break;
+        case 2:
+            pbez = &xvectorgeo[j];
+            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+            pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
+                                       pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
+                                       pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
+                                       pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
+            break;
+        }
+        s = s + xvectorgeo[j].mfLen;
+    }
+
+    p->AddLaneSection(0);
 
+    LaneSection * pLS = p->GetLaneSection(0);
+
+    pLS->AddLane(0,0,"none",false);
+    pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
+    Lane * pLane = pLS->GetLastRightLane();
+    for(j=0;j<(int)xvectorwidthabcd.size();j++)
+    {
+        iv::widthabcd * pwa = &xvectorwidthabcd[j];
+        pLane->AddWidthRecord(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
+    }
+
+    for(j=0;j<(int)xvectoreleabcd.size();j++)
+    {
+        iv::eleabcd * pwa = &xvectoreleabcd[j];
+        p->AddElevation(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
+    }
+
+    int noldtype = 0;
+    int ntype = 0;
+    Lane * pCenterLane = pLS->GetLastCenterLane();
+    if(nvlsize > 0)
+    {
+        ntype = xvectorvehicle[xvectorvl[0].mvehindex].nleftlanetype;
+        pCenterLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[0].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
+        noldtype = ntype;
+    }
+    for(i=1;i<(int)(nvlsize-1);i++)
+    {
+        int ntype = xvectorvehicle[xvectorvl[i].mvehindex].nleftlanetype;
+        if(ntype!= noldtype )
+        {
+            pCenterLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[i].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
+            noldtype = ntype;
+        }
+    }
+
+    noldtype = 0;
+    ntype = 0;
+    if(nvlsize > 0)
+    {
+        ntype = xvectorvehicle[xvectorvl[0].mvehindex].nrightlanetype;
+        pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[0].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
+        noldtype = ntype;
+    }
+    for(i=1;i<(int)(nvlsize-1);i++)
+    {
+        int ntype = xvectorvehicle[xvectorvl[i].mvehindex].nrightlanetype;
+        if(ntype!= noldtype )
+        {
+            pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[i].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
+            noldtype = ntype;
+        }
+    }
+
+
+    mnProc = 100;
+    mstrState = " Complete. ";
     return 0;
 }
+
+std::string NDSDataProc::roadmarktypetostr(int ntype)
+{
+    switch (ntype) {
+    case 1:
+        return "none";
+        break;
+    case 2:
+        return "broken";
+        break;
+    case 3:
+        return "solid";
+        break;
+    default:
+        return "none";
+        break;
+    }
+}

src/tool/map_lanetoxodr/ndsdataproc.h

@@ -5,6 +5,7 @@
 #include <iostream>
 #include <vector>
 #include "gnss_coordinate_convert.h"
+#include "OpenDrive/OpenDrive.h"
 
 
 namespace iv {
@@ -27,6 +28,10 @@ struct  nds_vehicle
     int nrightlanetype=1;
     double flanewidth = 0;
 
+    double s=0;
+
+    double fdistolast = 0;
+
 };
 
 struct nds_line
@@ -46,6 +51,24 @@ struct nds_vl
     std::vector<int> mvectorlineindex;
 };
 
+struct widthabcd
+{
+    double s;
+    double A;
+    double B;
+    double C;
+    double D;
+};
+
+struct eleabcd
+{
+    double s;
+    double A;
+    double B;
+    double C;
+    double D;
+};
+
 }
 
 class NDSDataProc
@@ -54,11 +77,17 @@ public:
     NDSDataProc();
 
 public:
-    static int ProcNDSData(std::string strlinepath,std::string strvehiclepath);
+    int ProcNDSData(std::string strlinepath,std::string strvehiclepath,OpenDrive * mpxodr);
+    std::string roadmarktypetostr(int ntype);
 
 private:
     double mflon0;
     double mflat0;
+public:
+    int mnProc = 0;
+    std::string mstrState;
+
+
 };
 
 #endif // NDSDATAPROC_H

src/tool/map_lanetoxodr/nds拟合.md

@@ -0,0 +1,24 @@
+
+
+## 1.数据导入
+导入line数据和vehicle数据。
+
+## 2.转换成相对坐标
+将所有经纬度数据转换成大地坐标，然后减去原点的大地坐标，得到相对坐标。
+
+## 3.数据筛选
+首选筛掉定位不对的数据。 然后给每一个vehicle数据寻找对应的line数据并加入，如果没有line数据，则去掉。将line数据偏离中心点距离和车道宽度数据加入到vehicle数据内。
+
+## 4.参考点计算
+根据车道宽度信息、偏离中心距离、相对坐标计算参考点坐标。
+
+## 5.参考线拟合
+a)初始化当前点为0，拟合区间为所有点。
+b)使用直线和圆弧进行拟合，如果其中一个拟合误差小于0.1米，跳到d。
+c)缩减拟合区间，回到b
+d)用贝塞尔曲线拟合当前几何起始点和上一段几何的终点，将贝塞尔曲线和当前几何增加进来。
+e)将当前点前移当前区间个点。拟合区间为剩余点。 如果拟合区间不为0，跳到b。
+f)完成
+
+## 6.高度和宽度拟合
+高度和宽度拟合以3次多项式方式拟合，和参考线拟合的流程相同，只是当前点前移为当前区间减1。