change tool/map_lanetoxodr for add xodrmake.cpp.

src/tool/map_lanetoxodr/OpenDrive/OpenDrive.h

@@ -47,8 +47,8 @@ private:
 	/**
 	 * Copy constructor, makes the object non-copyable
 	 */
-	OpenDrive (const OpenDrive& openDrive){};
-	const OpenDrive& operator=(const OpenDrive& rhs){};
+    OpenDrive (const OpenDrive& openDrive){};
+    const OpenDrive& operator=(const OpenDrive& rhs){};
 
 public:
 	/**

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -1571,6 +1571,20 @@ void MainWindow::onClickClearRoadLane()
 
 void MainWindow::onClickAddRoad()
 {
+    int nrtn;
+    std::string strrtn = " ";
+    std::string strroadname = mpLE_RoadName->text().toStdString();
+    nrtn = ServiceXODRMake.AddRoadFromeLanePoint(mvectorlp,mvectorroadlane,mvectorroadopposite,glon0,
+                                                 glat0,mxodr,strroadname,strrtn);
+    mpCBSelLane->clear();
+    mpCBSelOpLane->clear();
+    mvectorroadlane.clear();
+    mvectorroadopposite.clear();
+    updateCBRoad();
+    mbRefresh = true;
+    update();
+
+    return;
     if(mvectorroadlane.size() < 1)return;
 
     int nlanesize = mvectorroadlane.size();

src/tool/map_lanetoxodr/mainwindow.h

@@ -60,81 +60,9 @@
 using namespace std;
 using namespace Eigen;
 
-namespace iv {
-struct lanepoint
-{
-    double mfLat;
-    double mfLon;
-    double mfHeight;
-    int nLaneType;
-    double mfHeading;//Heading.
-    double mfDis; //Dis To First Point.
-    double  mfX;
-    double mfY;
-    double mfHdg;
-    double mfRelS = -1;  //Reletive S
-};
-
-}
-
-namespace iv {
-struct lpunit
-{
-    std::vector<iv::lanepoint> mvectorlpleft;
-    std::vector<iv::lanepoint> mvectorlpright;
-    char strlanename[256];
-};
-}
-
-namespace  iv {
-struct lanetype
-{
-    double s;
-    int ntype; //0 broken 1  solid
-};
-
-}
-
-namespace  iv {
-struct lanecoff
-{
-    double A;
-    double B;
-    double C;
-    double D;
-};
-
-}
-
-namespace iv {
-struct distogeo
-{
-  double mfs;
-  double mfdis;
-};
-}
-
-namespace iv {
+#include "rawtype.h"
 
-class lanecontact
-{
-  public:
-    int ml1;
-    int ml2;
-};
-class roadcontact
-{
-  public:
-    int  mnroad1id;
-    int  mnroad2id;
-    int mncon1; //0 start 1 end
-    int mncon2; //0 start 1 end
-
-    std::vector<iv::lanecontact> mvectorlc;
-    std::vector<iv::lanecontact> mvectorlcop;
-
-};
-}
+#include "xodrmake.h"
 
 namespace Ui {
 class MainWindow;

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -59,11 +59,13 @@ SOURCES += \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/imu.pb.cc \
     geofit.cpp \
-    circlefitting.cpp
+    circlefitting.cpp \
+    xodrmake.cpp
 
 HEADERS += \
     autoconnect.h \
         mainwindow.h \
+    rawtype.h \
     speeddialog.h \
     trafficlightdialog.h \
     trafficlightlanevaliditydialog.h \
@@ -89,7 +91,8 @@ HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/imu.pb.h \
     geofit.h \
-    circlefitting.h
+    circlefitting.h \
+    xodrmake.h
 
 FORMS += \
         mainwindow.ui \

src/tool/map_lanetoxodr/rawtype.h

@@ -0,0 +1,80 @@
+#ifndef RAWTYPE_H
+#define RAWTYPE_H
+
+namespace iv {
+struct lanepoint
+{
+    double mfLat;
+    double mfLon;
+    double mfHeight;
+    int nLaneType;
+    double mfHeading;//Heading.
+    double mfDis; //Dis To First Point.
+    double  mfX;
+    double mfY;
+    double mfHdg;
+    double mfRelS = -1;  //Reletive S
+};
+
+}
+
+namespace iv {
+struct lpunit
+{
+    std::vector<iv::lanepoint> mvectorlpleft;
+    std::vector<iv::lanepoint> mvectorlpright;
+    char strlanename[256];
+};
+}
+
+namespace  iv {
+struct lanetype
+{
+    double s;
+    int ntype; //0 broken 1  solid
+};
+
+}
+
+namespace  iv {
+struct lanecoff
+{
+    double A;
+    double B;
+    double C;
+    double D;
+};
+
+}
+
+namespace iv {
+struct distogeo
+{
+  double mfs;
+  double mfdis;
+};
+}
+
+namespace iv {
+
+class lanecontact
+{
+  public:
+    int ml1;
+    int ml2;
+};
+class roadcontact
+{
+  public:
+    int  mnroad1id;
+    int  mnroad2id;
+    int mncon1; //0 start 1 end
+    int mncon2; //0 start 1 end
+
+    std::vector<iv::lanecontact> mvectorlc;
+    std::vector<iv::lanecontact> mvectorlcop;
+
+};
+}
+
+#endif // RAWTYPE_H

src/tool/map_lanetoxodr/xodrmake.cpp

@@ -0,0 +1,913 @@
+#include "xodrmake.h"
+
+#include <memory>
+
+xodrmake::xodrmake()
+{
+
+}
+
+xodrmake& xodrmake::Inst()
+{
+    static xodrmake xxdormake;
+    return xxdormake;
+}
+
+
+int xodrmake::CreateRoadID(int ntype, OpenDrive &xxodr)
+{
+    int i;
+    bool bUsed = false;
+    int nroadidstart  = 10000;
+    if(ntype == 1)  //not create by lane  roaid
+    {
+        nroadidstart = 20000;
+    }
+
+    int nroadcount = xxodr.GetRoadCount();
+    if(nroadcount == 0)return nroadidstart;
+    int * proadid = new int[nroadcount];
+    for(i=0;i<nroadcount;i++)
+    {
+        proadid[i]=atoi(xxodr.GetRoad(i)->GetRoadId().data());
+    }
+    do
+    {
+        bUsed = false;
+    for(i=0;i<nroadcount;i++)
+    {
+        if(proadid[i] == nroadidstart)
+        {
+            nroadidstart++;
+            bUsed = true;
+        }
+    }
+    }while(bUsed);
+    delete proadid;
+    return nroadidstart;
+}
+
+int xodrmake::AddRoadFromeLanePoint(std::vector<iv::lpunit> &xvectorlp, std::vector<int> &xvectorroadlane,
+                                    std::vector<int> &xvectorroadopposite,
+                                    double flon0,double flat0,OpenDrive &xxodr, std::string strroadname,string &strerror)
+{
+
+    if(xvectorroadlane.size() < 1)
+    {
+        strerror = "not select lane.";
+        return -1;
+    }
+
+    unsigned int nlanesize = xvectorroadlane.size();
+    unsigned int noplanesize = xvectorroadopposite.size();
+    double notlinethresh = 1.0;  //When heading change more than this value, is a arc.
+
+    unsigned int nsize = xvectorlp[xvectorroadlane[0]].mvectorlpleft.size();
+    int * pntype = new int[nsize];
+    std::shared_ptr<int> ppntype; ppntype.reset(pntype);
+
+    std::vector<iv::lanepoint > * pvectorlp = &(xvectorlp[xvectorroadlane[0]].mvectorlpleft);
+    unsigned int i;
+
+    //Go  throuh point  for fit type.
+    for(i=0;i<5;i++)pntype[i] = 0;
+    for(i=5;i<(nsize-5);i++)
+    {
+        double s = pvectorlp->at(i).mfDis;
+        double head0 = pvectorlp->at(i).mfHeading;
+        unsigned int j;
+        double xcount = 0;
+        double headdifftotal = 0;
+        double headdiffavg = 0.0;
+        for(j=1;j<nsize;j++)
+        {
+            if((j>1)&&((pvectorlp->at(j).mfDis - s)>1.0))
+            {
+                break;
+            }
+            double headdiff = pvectorlp->at(j).mfHeading - head0;
+            if(headdiff > 300)headdiff = headdiff - 360;
+            if(headdiff < -300)headdiff = headdiff + 360;
+            headdifftotal = headdifftotal + headdiff;
+            xcount = xcount + 1.0;
+        }
+        if(xcount > 0)headdiffavg = headdifftotal/xcount;
+        if(fabs(headdiffavg) > (notlinethresh*2))
+        {
+            pntype[i] = 1;
+        }
+        else
+        {
+            pntype[i] = 0;
+        }
+
+    }
+    for(i=(nsize -5);i<nsize;i++)
+    {
+        pntype[i] = 0;
+    }
+
+    for(i=0;i<nsize;i++)  //Remove Few Not Line Point.
+    {
+        if(pntype[i] !=  0)
+        {
+            unsigned int j;
+            int ncount = 1;
+            for(j=(i+1);j<nsize;j++)
+            {
+                if(pntype[i] != 0)ncount++;
+                else break;
+            }
+            if(ncount < 10)pntype[i] = 0;
+            else
+            {
+                i = i+ ncount;
+            }
+        }
+    }
+    for(i=5;i<nsize;i++)
+    {
+        if(pntype[i] != pntype[i-1])
+        {
+            int j = i-1;
+            double disx = pvectorlp->at(i).mfDis;
+
+            for(j=(i-2);j>5;j--)
+            {
+                if(pntype[j] == pntype[i-1])
+                {
+                    pntype[j] = 2; //Besel
+                }
+                if(fabs(pvectorlp->at(j).mfDis - disx)>1)  //1 m besel
+                {
+                    break;
+                }
+            }
+            pntype[i-1] = 2;
+        }
+    }
+
+
+    //Calc dis  to ref  line
+    double xor0,yor0;
+    GaussProjCal(flon0,flat0,&xor0,&yor0);
+    std::vector<std::vector<iv::distogeo>> xvectordiss;
+    std::vector<std::vector<iv::distogeo>> xvectoropdiss;
+    for(i=0;i<xvectorroadlane.size();i++)
+    {
+        std::vector<iv::distogeo> xvectordistogeo;
+        int j;
+        int nsizepoint = xvectorlp[xvectorroadlane[i]].mvectorlpright.size();
+
+        bool bHaveLast = false;
+        int nLast = 0;
+        for(j=0;j<nsizepoint;j++)
+        {
+            double fdismin = 1000;
+            double fS = 0;
+            unsigned int k;
+            double x,y;
+            double x0,y0;
+            iv::distogeo xdistogeo;
+
+            int nLastBig = 0;
+            bool bFindDisMin = false;
+
+            x = xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfX;
+            y = xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfY;
+            unsigned int kstart = 0;
+            if(bHaveLast)
+            {
+                kstart= nLast;
+            }
+            for(k=kstart;k<xvectorlp[xvectorroadlane[0]].mvectorlpleft.size();k++)
+            {
+
+                x0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfX;
+                y0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfY;
+                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
+                if(fdismin >  fdis)
+                {
+                    fdismin = fdis;
+                    fS = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfDis;
+                    bFindDisMin = true;
+                    nLast = k;
+
+                }
+                if((bFindDisMin)&&(bHaveLast))
+                {
+                    if(fdis>fdismin)
+                    {
+                        nLastBig++;
+                    }
+                    else
+                    {
+                        nLastBig = 0;
+                    }
+                }
+                if(nLastBig > 10)break;
+
+
+            }
+            //            std::cout<<" k is "<<k<<std::endl;
+            if(bHaveLast == false)
+            {
+                if(fdismin<10)bHaveLast = true;
+            }
+            else
+            {
+                if(fdismin > 10)bHaveLast = false;
+            }
+
+            double fHdg = geofit::CalcHdg(x,y,xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfX,
+                    xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfY);
+            double fHdgDiff = fHdg - xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfHdg;
+            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
+            {
+                xdistogeo.mfdis = fdismin;
+                xdistogeo.mfs = fS;
+
+                xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfRelS = fS;
+            }
+            else
+            {
+                xdistogeo.mfs = -1;
+                std::cout<<"point extend."<<std::endl;
+                xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfRelS =  -1;
+            }
+            xvectordistogeo.push_back(xdistogeo);
+        }
+
+        xvectordiss.push_back(xvectordistogeo);
+    }
+
+    for(i=1;i<xvectorroadlane.size();i++)
+    {
+        std::vector<iv::distogeo> xvectordistogeo;
+        int j;
+        int nsizepoint = xvectorlp[xvectorroadlane[i]].mvectorlpleft.size();
+        bool bHaveLast = false;
+        int nLast = 0;
+        for(j=0;j<nsizepoint;j++)
+        {
+            double fdismin = 1000;
+            double fS = 0;
+            unsigned int k;
+            double x,y;
+            double x0,y0;
+            iv::distogeo xdistogeo;
+            int nLastBig = 0;
+            bool bFindDisMin = false;
+            x = xvectorlp[xvectorroadlane[i]].mvectorlpleft[j].mfX;
+            y = xvectorlp[xvectorroadlane[i]].mvectorlpleft[j].mfY;
+
+            unsigned int kstart = 0;
+            if(bHaveLast)
+            {
+                kstart= nLast;
+            }
+            for(k=kstart;k<xvectorlp[xvectorroadlane[0]].mvectorlpleft.size();k++)
+            {
+                x0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfX;
+                y0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfY;
+                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
+                if(fdismin >  fdis)
+                {
+                    fdismin = fdis;
+                    fS = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfDis;
+                    bFindDisMin = true;
+                    nLast = k;
+
+                }
+                if((bFindDisMin)&&(bHaveLast))
+                {
+                    if(fdis>fdismin)
+                    {
+                        nLastBig++;
+                    }
+                    else
+                    {
+                        nLastBig = 0;
+                    }
+                }
+                if(nLastBig > 10)break;
+            }
+            //           std::cout<<" k is "<<k<<std::endl;
+            if(bHaveLast == false)
+            {
+                if(fdismin<10)bHaveLast = true;
+            }
+            else
+            {
+                if(fdismin > 10)bHaveLast = false;
+            }
+
+            double fHdg = geofit::CalcHdg(x,y,xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfX,
+                    xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfY);
+            double fHdgDiff = fHdg - xvectorlp[xvectorroadlane[i]].mvectorlpleft[j].mfHdg;
+            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
+            {
+                xdistogeo.mfdis = fdismin;
+                xdistogeo.mfs = fS;
+
+                xvectorlp[xvectorroadlane[i]].mvectorlpleft[j].mfRelS = fS;
+            }
+            else
+            {
+                xdistogeo.mfs = -1;
+                std::cout<<"point extend."<<std::endl;
+                xvectorlp[xvectorroadlane[i]].mvectorlpleft[j].mfRelS = -1;
+            }
+            xvectordistogeo.push_back(xdistogeo);
+
+        }
+
+        xvectordiss.push_back(xvectordistogeo);
+    }
+
+
+    for(i=0;i<xvectorroadopposite.size();i++)
+    {
+        std::vector<iv::distogeo> xvectordistogeo;
+        int j;
+        int nsizepoint = xvectorlp[xvectorroadopposite[i]].mvectorlpleft.size();
+        bool bHaveLast = false;
+        int nLast = 0;
+        for(j=0;j<nsizepoint;j++)
+        {
+            double fdismin = 1000;
+            double fS = 0;
+            int k;
+            double x,y;
+            double x0,y0;
+            iv::distogeo xdistogeo;
+            int nLastBig = 0;
+            bool bFindDisMin = false;
+            x = xvectorlp[xvectorroadopposite[i]].mvectorlpleft[j].mfX;
+            y = xvectorlp[xvectorroadopposite[i]].mvectorlpleft[j].mfY;
+
+            int nrefsize = xvectorlp[xvectorroadlane[0]].mvectorlpleft.size();
+            int kstart = nrefsize-1;
+            if(bHaveLast)
+            {
+                kstart= nLast;
+            }
+
+            for(k=kstart;k>=0;k--)
+            {
+                x0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfX;
+                y0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfY;
+                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
+                if(fdismin >  fdis)
+                {
+                    fdismin = fdis;
+                    fS = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfDis;
+                    bFindDisMin = true;
+                    nLast = k;
+
+                }
+                if((bFindDisMin)&&(bHaveLast))
+                {
+                    if(fdis>fdismin)
+                    {
+                        nLastBig++;
+                    }
+                    else
+                    {
+                        nLastBig = 0;
+                    }
+                }
+                if(nLastBig > 10)break;
+            }
+            //           std::cout<<" k is "<<k<<std::endl;
+            if(bHaveLast == false)
+            {
+                if(fdismin<10)bHaveLast = true;
+            }
+            else
+            {
+                if(fdismin > 10)bHaveLast = false;
+            }
+
+            double fHdg = geofit::CalcHdg(x,y,xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfX,
+                    xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfY);
+            double fHdgDiff = fHdg - xvectorlp[xvectorroadopposite[i]].mvectorlpleft[j].mfHdg;
+            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
+            {
+                xdistogeo.mfdis = fdismin;
+                xdistogeo.mfs = fS;
+
+                xvectorlp[xvectorroadopposite[i]].mvectorlpleft[j].mfRelS = fS;
+            }
+            else
+            {
+                xdistogeo.mfs = -1;
+                std::cout<<"point extend."<<std::endl;
+                xvectorlp[xvectorroadopposite[i]].mvectorlpleft[j].mfRelS = -1;
+            }
+
+            xvectordistogeo.push_back(xdistogeo);
+
+        }
+
+        xvectoropdiss.push_back(xvectordistogeo);
+    }
+
+    for(i=0;i<xvectorroadopposite.size();i++)
+    {
+        std::vector<iv::distogeo> xvectordistogeo;
+        int j;
+        int nsizepoint = xvectorlp[xvectorroadopposite[i]].mvectorlpright.size();
+        bool bHaveLast = false;
+        int nLast = 0;
+        for(j=0;j<nsizepoint;j++)
+        {
+            double fdismin = 1000;
+            double fS = 0;
+            int k;
+            double x,y;
+            double x0,y0;
+            iv::distogeo xdistogeo;
+            int nLastBig = 0;
+            bool bFindDisMin = false;
+            x = xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfX;
+            y = xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfY;
+
+            int nrefsize = xvectorlp[xvectorroadlane[0]].mvectorlpleft.size();
+            int kstart = nrefsize-1;
+            if(bHaveLast)
+            {
+                kstart= nLast;
+            }
+
+            for(k=kstart;k>=0;k--)
+            {
+                x0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfX;
+                y0 = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfY;
+                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
+                if(fdismin >  fdis)
+                {
+                    fdismin = fdis;
+                    fS = xvectorlp[xvectorroadlane[0]].mvectorlpleft[k].mfDis;
+                    bFindDisMin = true;
+                    nLast = k;
+
+                }
+                if((bFindDisMin)&&(bHaveLast))
+                {
+                    if(fdis>fdismin)
+                    {
+                        nLastBig++;
+                    }
+                    else
+                    {
+                        nLastBig = 0;
+                    }
+                }
+                if(nLastBig > 10)break;
+            }
+            //           std::cout<<" k is "<<k<<std::endl;
+            if(bHaveLast == false)
+            {
+                if(fdismin<10)bHaveLast = true;
+            }
+            else
+            {
+                if(fdismin > 10)bHaveLast = false;
+            }
+
+            double fHdg = geofit::CalcHdg(x,y,xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfX,
+                    xvectorlp[xvectorroadlane[0]].mvectorlpleft[nLast].mfY);
+            double fHdgDiff = fHdg - xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfHdg;
+            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
+            {
+                xdistogeo.mfdis = fdismin;
+                xdistogeo.mfs = fS;
+
+                xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfRelS = fS;
+            }
+            else
+            {
+                xdistogeo.mfs = -1;
+                std::cout<<"point extend."<<std::endl;
+                xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfRelS = -1;
+            }
+            xvectordistogeo.push_back(xdistogeo);
+
+        }
+
+        xvectoropdiss.push_back(xvectordistogeo);
+    }
+
+    //Get Lane Type
+    std::vector<std::vector<iv::lanetype>> xvevelanetype;
+    std::vector<std::vector<iv::lanetype>> xvectoroplanetype;
+    for(i=0;i<nlanesize;i++)
+    {
+        std::vector<iv::lanetype> xvelanetype;
+        int j;
+        int nlasttype;
+        iv::lanetype xlt;
+        xlt.s = 0;
+        xlt.ntype = 0;
+        int nsizelp = xvectorlp[xvectorroadlane[i]].mvectorlpright.size();
+        if(nsizelp > 0)
+        {
+            xlt.ntype = xvectorlp[xvectorroadlane[i]].mvectorlpright[0].nLaneType;
+            nlasttype = xlt.ntype;
+        }
+        for(j=1;j<nsizelp;j++)
+        {
+            int nlt = xvectorlp[xvectorroadlane[i]].mvectorlpright[j].nLaneType;
+            if((nlt != nlasttype)&&(xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfRelS != -1))
+            {
+                xvelanetype.push_back(xlt);
+                xlt.ntype = nlt;
+                xlt.s = xvectorlp[xvectorroadlane[i]].mvectorlpright[j].mfRelS;
+            }
+            nlasttype = nlt;
+        }
+        xvelanetype.push_back(xlt);
+        xvevelanetype.push_back(xvelanetype);
+    }
+
+    for(i=0;i<noplanesize;i++)
+    {
+        std::vector<iv::lanetype> xvelanetype;
+        int j;
+        int nlasttype;
+        iv::lanetype xlt;
+        xlt.s = 0;
+        xlt.ntype = 0;
+        int nsizelp = xvectorlp[xvectorroadopposite[i]].mvectorlpright.size();
+        if(nsizelp > 0)
+        {
+            xlt.ntype = xvectorlp[xvectorroadopposite[i]].mvectorlpright[nsizelp-1].nLaneType;
+            nlasttype = xlt.ntype;
+        }
+        for(j=(nsizelp-2);j>=0;j--)
+        {
+            int nlt = xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].nLaneType;
+            if((nlt != nlasttype)&&(xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfRelS != -1))
+            {
+                xvelanetype.push_back(xlt);
+                xlt.ntype = nlt;
+                xlt.s = xvectorlp[xvectorroadopposite[i]].mvectorlpright[j].mfRelS;
+            }
+            nlasttype = nlt;
+        }
+        xvelanetype.push_back(xlt);
+        xvectoroplanetype.push_back(xvelanetype);
+    }
+
+    //Get Lane Mark Width
+    std::vector<double> xvectormarkwidth;
+    std::vector<double> xvectoravgdis;
+    for(i=0;i<xvectordiss.size();i++)
+    {
+        double fdis = 0;
+        unsigned int j;
+        int ncount = 0;
+        for(j=0;j<xvectordiss[i].size();j++)
+        {
+            if(xvectordiss[i].at(j).mfs !=  -1)
+            {
+                fdis = fdis + xvectordiss[i].at(j).mfdis;
+                ncount++;
+            }
+        }
+        if(ncount>0)fdis = fdis/ncount;
+        xvectoravgdis.push_back(fdis);
+    }
+
+    for(i=1;i<xvectorroadlane.size();i++)
+    {
+        xvectormarkwidth.push_back(xvectoravgdis[xvectorroadlane.size()+i-1] - xvectoravgdis[i-1]);
+    }
+
+    std::vector<double> xvectoropmarkwidth;
+    std::vector<double> xvectoropavgdis;
+
+    for(i=0;i<xvectoropdiss.size();i++)
+    {
+        double fdis = 0;
+        int j;
+        int ncount = 0;
+        int nsize = xvectoropdiss[i].size();
+        for(j=0;j<nsize;j++)
+        {
+            if(xvectoropdiss[i].at(j).mfs !=  -1)
+            {
+                fdis = fdis + xvectoropdiss[i].at(j).mfdis;
+                ncount++;
+            }
+        }
+        if(ncount>0)fdis = fdis/ncount;
+        xvectoropavgdis.push_back(fdis);
+    }
+
+    if(xvectoropavgdis.size()>0)xvectoropmarkwidth.push_back(xvectoropavgdis[0]);
+    for(i=1;i<xvectorroadopposite.size();i++)
+    {
+        xvectoropmarkwidth.push_back(xvectoropavgdis[i] - xvectoropavgdis[i+xvectorroadopposite.size()-1]);
+    }
+
+    //Get Lane Width
+    std::vector<std::vector<iv::distogeo>> xvectorlanewidth;
+    std::vector<std::vector<iv::distogeo>> xvectoroplanewidth;
+    std::vector<iv::distogeo> xlanewidth;
+    xlanewidth.clear();
+    unsigned int j;
+    for(j=0;j<xvectordiss[0].size();j++)
+    {
+        if(xvectordiss[0].at(j).mfs !=  -1)xlanewidth.push_back(xvectordiss[0].at(j));
+    }
+    xvectorlanewidth.push_back(xlanewidth);
+    for(i=1;i<xvectorroadlane.size();i++)
+    {
+        xlanewidth.clear();
+        for(j=0;j<xvectordiss[i].size();j++)
+        {
+            if((xvectordiss[i].at(j).mfs!=-1)&&(xvectordiss[i-1+nlanesize].at(j).mfs!=-1))
+            {
+                iv::distogeo xdisg;
+                xdisg = xvectordiss[i].at(j);
+                xdisg.mfdis = xdisg.mfdis - xvectordiss[i-1+nlanesize].at(j).mfdis + xvectormarkwidth[i-1];
+                xlanewidth.push_back(xdisg);
+            }
+        }
+        xvectorlanewidth.push_back(xlanewidth);
+    }
+    xlanewidth.clear();
+    if(xvectoropmarkwidth.size()>0)
+    {
+        if(xvectoropmarkwidth[0] >  0.4)
+        {
+            unsigned int nsize = xvectoropdiss[0].size();
+            for(j=0;j<nsize;j++)
+            {
+                if(xvectoropdiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectoropdiss[0].at(j));
+            }
+            xvectoroplanewidth.push_back(xlanewidth);
+            xlanewidth.clear();
+            nsize = xvectoropdiss[noplanesize].size();
+            for(j=0;j<nsize;j++)
+            {
+                if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
+                {
+                    iv::distogeo xdisg;
+                    xdisg = xvectoropdiss[noplanesize].at(j);
+                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis ;
+                    xlanewidth.push_back(xdisg);
+                }
+
+            }
+            xvectoroplanewidth.push_back(xlanewidth);
+        }
+        else
+        {
+            nsize = xvectoropdiss[noplanesize].size();
+            for(j=0;j<nsize;j++)
+            {
+                if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
+                {
+                    iv::distogeo xdisg;
+                    xdisg = xvectoropdiss[noplanesize].at(j);
+                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis + xvectoropmarkwidth[0];
+                    xlanewidth.push_back(xdisg);
+                }
+
+            }
+            xvectoroplanewidth.push_back(xlanewidth);
+        }
+        for(i=1;i<noplanesize;i++)
+        {
+            xlanewidth.clear();
+            nsize = xvectoropdiss[noplanesize+i].size();
+            for(j=0;j<nsize;j++)
+            {
+                if((xvectoropdiss[noplanesize+i].at(j).mfs != -1)&&(xvectoropdiss[i].at(j).mfs != -1))
+                {
+                    iv::distogeo xdisg;
+                    xdisg = xvectoropdiss[noplanesize+i].at(j);
+                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[i].at(j).mfdis + xvectoropmarkwidth[i];
+                    xlanewidth.push_back(xdisg);
+                }
+            }
+            xvectoroplanewidth.push_back(xlanewidth);
+
+        }
+
+    }
+
+    std::vector<iv::lanecoff> xvectorlanecoff;
+    std::vector<iv::lanecoff> xvectoroplanecoff;
+    for(i=0;i<nlanesize;i++)
+    {
+        unsigned int N = xvectorlanewidth[i].size();
+        VectorXd x_veh(N);
+        VectorXd y_veh(N);
+        for(j=0;j<N;j++)
+        {
+            x_veh[j] = xvectorlanewidth[i].at(j).mfs;
+            y_veh[j] =xvectorlanewidth[i].at(j).mfdis;
+        }
+        VectorXd coeffs = polyfit(x_veh, y_veh, 3);
+        iv::lanecoff xlanecoff;
+        xlanecoff.A = coeffs[0];
+        xlanecoff.B = coeffs[1];
+        xlanecoff.C = coeffs[2];
+        xlanecoff.D = coeffs[3];
+        xvectorlanecoff.push_back(xlanecoff);
+    }
+
+    for(i=0;i<xvectoroplanewidth.size();i++)
+    {
+        unsigned int N = xvectoroplanewidth[i].size();
+        VectorXd x_veh(N);
+        VectorXd y_veh(N);
+        for(j=0;j<N;j++)
+        {
+            x_veh[j] = xvectoroplanewidth[i].at(j).mfs;
+            y_veh[j] =xvectoroplanewidth[i].at(j).mfdis;
+        }
+        VectorXd coeffs = polyfit(x_veh, y_veh, 3);
+        iv::lanecoff xlanecoff;
+        xlanecoff.A = coeffs[0];
+        xlanecoff.B = coeffs[1];
+        xlanecoff.C = coeffs[2];
+        xlanecoff.D = coeffs[3];
+        xvectoroplanecoff.push_back(xlanecoff);
+    }
+
+    iv::lanecoff xlaneheightcoff;
+    unsigned int M = xvectorlp[0].mvectorlpleft.size();
+    VectorXd x_vehhg(M);
+    VectorXd y_vehhg(M);
+    for(j=0;j<M;j++)
+    {
+        x_vehhg[j] = xvectorlp[0].mvectorlpleft.at(j).mfDis;
+        y_vehhg[j] =xvectorlp[0].mvectorlpleft.at(j).mfHeight;
+    }
+    VectorXd coeffs = polyfit(x_vehhg, y_vehhg, 3);
+    xlaneheightcoff.A = coeffs[0];
+    xlaneheightcoff.B = coeffs[1];
+    xlaneheightcoff.C = coeffs[2];
+    xlaneheightcoff.D = coeffs[3];
+
+
+
+    geofit xgeofit;
+
+    unsigned int N = pvectorlp->size();
+    VectorXd x_veh(N);
+    VectorXd y_veh(N);
+    VectorXi t_veh(N);
+
+
+    double x0,y0;
+    GaussProjCal(flon0,flat0,&x0,&y0);
+
+    for(j=0;j<N;j++)
+    {
+        double x,y;
+
+        GaussProjCal(pvectorlp->at(j).mfLon,pvectorlp->at(j).mfLat,&x,&y);
+        x_veh[j] = x - x0;
+        y_veh[j] = y - y0;
+        t_veh[j] = pntype[j];
+    }
+
+    std::vector<geobase> xvectorgeo = xgeofit.getgeo(x_veh,y_veh,t_veh);
+
+    //    OpenDrive od;
+    //    std::string mapx = "map";
+    //    od.SetHeader(1,1,mapx,1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
+
+    j= 0;
+    double xroadlen = 0;
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+        xroadlen = xroadlen + xvectorgeo[j].mfLen;
+    }
+
+    xxodr.AddRoad(strroadname.data(),xroadlen, QString::number(CreateRoadID(0,xxodr)).toStdString(),"-1");
+    Road * p = xxodr.GetRoad(xxodr.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);
+    Lane * pLL;
+    pLS->SetS(0);
+    pLS->AddLane(0,0,"none",false);
+    pLL = pLS->GetLane(0);
+    if(noplanesize != xvectoroplanewidth.size())
+    {
+        pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+    }
+    for(i=0;i<nlanesize;i++)
+    {
+        pLS->AddLane(-1,(i+1)*(-1),"driving",false,false);
+        pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
+
+        pLL->AddWidthRecord(0,xvectorlanecoff[i].A,xvectorlanecoff[i].B,
+                            xvectorlanecoff[i].C,xvectorlanecoff[i].D);
+
+
+
+        unsigned int j;
+        for(j=0;j<xvevelanetype[i].size();j++)
+        {
+            std::string strlanetype = "broken";
+            if(xvevelanetype[i].at(j).ntype != 0)
+            {
+                strlanetype = "solid";
+            }
+            pLL->AddRoadMarkRecord(xvevelanetype[i].at(j).s,strlanetype,"standard","standard",0.15,"false");
+        }
+
+    }
+
+    for(i=0;i<xvectoroplanewidth.size();i++)
+    {
+        pLS->AddLane(1,(i+1)*(1),"driving",false,false);
+        pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
+        if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
+        {
+            pLL->SetType("shoulder");
+            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+        }
+
+        pLL->AddWidthRecord(0,xvectoroplanecoff[i].A,xvectoroplanecoff[i].B,
+                            xvectoroplanecoff[i].C,xvectoroplanecoff[i].D);
+
+        unsigned int j;
+        int index = i;
+        if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
+        {
+            continue;
+        }
+        if(noplanesize != xvectoroplanewidth.size())
+        {
+            index = i-1;
+        }
+        else
+            index = i;
+        for(j=0;j<xvectoroplanetype[index].size();j++)
+        {
+            std::string strlanetype = "broken";
+            if(xvectoroplanetype[index].at(j).ntype != 0)
+            {
+                strlanetype = "solid";
+            }
+            pLL->AddRoadMarkRecord(xvectoroplanetype[index].at(j).s,strlanetype,"standard","standard",0.15,"false");
+        }
+
+    }
+
+    return 0;
+}

src/tool/map_lanetoxodr/xodrmake.h

@@ -0,0 +1,37 @@
+#ifndef XODRMAKE_H
+#define XODRMAKE_H
+
+#include <vector>
+
+#include "rawtype.h"
+
+#include "geofit.h"
+
+#include "gnss_coordinate_convert.h"
+
+#include "OpenDrive/OpenDrive.h"
+
+class xodrmake
+{
+public:
+    xodrmake();
+
+public:
+    static xodrmake& Inst();
+
+public:
+
+    int AddRoadFromeLanePoint(std::vector<iv::lpunit> & xvectorlp,std::vector<int>  & xvectorroadlane,
+                               std::vector<int> & xvectorroadopposite,double flon0,double flat0,
+                              OpenDrive & xxodr,std::string strroadname,std::string & strerror);
+
+private:
+    int CreateRoadID(int ntype,OpenDrive & xxodr);
+
+
+
+};
+
+#define ServiceXODRMake xodrmake::Inst()
+
+#endif // XODRMAKE_H