change map_lanetoxodr. for add lane width from rtk.

src/tool/map_lanetoxodr/dialogaddroadfromrtk.cpp

@@ -97,23 +97,15 @@ void DialogAddRoadFromRTK::calcrelxy(double flon0, double flat0, double flon, do
     rely = y - y0;
 
 }
-void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
-{
-    QString str = QFileDialog::getOpenFileName(this,tr("Open RTK Data"),"",tr("Lane File(*.csv)"));
-    if(str.isEmpty())return;
 
+
+std::vector<iv::rtkdata> DialogAddRoadFromRTK::loadrtkdata(QString strfilepath,double & flon0,double & flat0,OpenDrive * pxodr)
+{
     QFile xFile;
-    xFile.setFileName(str);
-    QFileInfo fi(str);
-    QString filename = fi.fileName();
-    if(filename.contains(".txt"))
-    {
-        filename = filename.left(filename.length() -4);
-    }
+    xFile.setFileName(strfilepath);
 
-    ui->plainTextEdit->clear();
-    mvectorrtkdata.clear();
-    ui->plainTextEdit->appendPlainText(QString("Longitude\tLatitude\tHeight\trelx\trely"));
+    std::vector<iv::rtkdata> xvectorrtkdata;
+    xvectorrtkdata.clear();
 
     if(xFile.open(QIODevice::ReadWrite))
     {
@@ -138,7 +130,7 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                 strheight = strheight.left(strheight.size() -2);
                 fheight = strheight.toDouble();
 
-                calcrelxy(mlon0,mlat0,flon,flat,frelx,frely);
+                calcrelxy(flon0,flat0,flon,flat,frelx,frely);
 
                 iv::rtkdata xdata;
                 xdata.mflat = flat;
@@ -146,27 +138,25 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                 xdata.mfrelx = frelx;
                 xdata.mfrely = frely;
                 xdata.mheight = fheight;
-                if(mvectorrtkdata.size() < 1)
+                if(xvectorrtkdata.size() < 1)
                 {
                     xdata.mfrels = 0;
                 }
                 else
                 {
-                    xdata.mfrels = mvectorrtkdata[mvectorrtkdata.size()-1].mfrels
-                            +sqrt(pow(mvectorrtkdata[mvectorrtkdata.size()-1].mfrelx - frelx,2)+pow(mvectorrtkdata[mvectorrtkdata.size()-1].mfrely - frely,2));
+                    xdata.mfrels = xvectorrtkdata[xvectorrtkdata.size()-1].mfrels
+                            +sqrt(pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrelx - frelx,2)+pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrely - frely,2));
                 }
-                mvectorrtkdata.push_back(xdata);
+                xvectorrtkdata.push_back(xdata);
 
                 if(i==nstartline)
                 {
-                    if(mpxodr->GetRoadCount()<1)
+                    if(pxodr->GetRoadCount()<1)
                     {
-                        glat0 = flat;
-                        glon0 = flon;
-                        mlon0 = glon0;
-                        mlat0 = glat0;
-                        calcrelxy(mlon0,mlat0,flon,flat,frelx,frely);
-                        mvectorrtkdata.clear();
+                        flat0 = flat;
+                        flon0 = flon;
+                        calcrelxy(flon0,flat0,flon,flat,frelx,frely);
+                        xvectorrtkdata.clear();
                         iv::rtkdata xdata;
                         xdata.mflat = flat;
                         xdata.mflon = flon;
@@ -174,24 +164,151 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                         xdata.mfrely = frely;
                         xdata.mheight = fheight;
                         xdata.mfrels = 0;
-                        mvectorrtkdata.push_back(xdata);
+                        xvectorrtkdata.push_back(xdata);
 
                     }
                 }
 
-                QString strline = QString::number(flon,'f',7) + QString("\t")
-                        +QString::number(flat,'f',7)+QString("\t")
-                        +QString::number(fheight,'f',4)+QString("\t")
-                        +QString::number(frelx,'f',2) + QString("\t")
-                        +QString::number(frely,'f',2) + QString("\t");
-                ui->plainTextEdit->appendPlainText(strline);
+//                QString strline = QString::number(flon,'f',7) + QString("\t")
+//                        +QString::number(flat,'f',7)+QString("\t")
+//                        +QString::number(fheight,'f',4)+QString("\t")
+//                        +QString::number(frelx,'f',2) + QString("\t")
+//                        +QString::number(frely,'f',2) + QString("\t");
+//                ui->plainTextEdit->appendPlainText(strline);
             }
         }
 
 
  //       std::vector<iv::rtkdata> * pxvectorrtkdata = &mvectorrtkdata;
+ //       ui->pushButton_CreateRoad->setEnabled(true);
+    }
+
+    return xvectorrtkdata;
+
+
+}
+
+void DialogAddRoadFromRTK::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 = loadrtkdata(str,mlon0,mlat0,mpxodr);
+
+    if(mlon0 != glon0)
+    {
+        glon0 = mlon0;
+        glat0 = mlat0;
+    }
+
+    if(mvectorrtkdata.size()>0)
+    {
+        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_CreateRoad->setEnabled(true);
     }
+
+//    if(xFile.open(QIODevice::ReadWrite))
+//    {
+//        QByteArray ba;
+//        ba = xFile.read(xFile.size());
+//        QList<QByteArray> strlinedata=  ba.split('\n');
+//        int i;
+//        int nsize = strlinedata.size();
+//        const int nstartline = 1;
+//        for(i=nstartline;i<nsize;i++)
+//        {
+//            double flat,flon,fheight,frelx,frely;
+//            QList<QByteArray> strlistvalue = strlinedata.at(i).split(',');
+//            int ncount = strlistvalue.size();
+//            if(ncount>=4)
+//            {
+////                flon = getvaluefromdegreestring(strlistvalue.at(2));
+////                flat = getvaluefromdegreestring(strlistvalue.at(1));
+//                flon  = atof(strlistvalue.at(2).data());
+//                flat = atof(strlistvalue.at(1).data());
+//                QString strheight = strlistvalue.at(3);
+//                strheight = strheight.left(strheight.size() -2);
+//                fheight = strheight.toDouble();
+
+//                calcrelxy(mlon0,mlat0,flon,flat,frelx,frely);
+
+//                iv::rtkdata xdata;
+//                xdata.mflat = flat;
+//                xdata.mflon = flon;
+//                xdata.mfrelx = frelx;
+//                xdata.mfrely = frely;
+//                xdata.mheight = fheight;
+//                if(mvectorrtkdata.size() < 1)
+//                {
+//                    xdata.mfrels = 0;
+//                }
+//                else
+//                {
+//                    xdata.mfrels = mvectorrtkdata[mvectorrtkdata.size()-1].mfrels
+//                            +sqrt(pow(mvectorrtkdata[mvectorrtkdata.size()-1].mfrelx - frelx,2)+pow(mvectorrtkdata[mvectorrtkdata.size()-1].mfrely - frely,2));
+//                }
+//                mvectorrtkdata.push_back(xdata);
+
+//                if(i==nstartline)
+//                {
+//                    if(mpxodr->GetRoadCount()<1)
+//                    {
+//                        glat0 = flat;
+//                        glon0 = flon;
+//                        mlon0 = glon0;
+//                        mlat0 = glat0;
+//                        calcrelxy(mlon0,mlat0,flon,flat,frelx,frely);
+//                        mvectorrtkdata.clear();
+//                        iv::rtkdata xdata;
+//                        xdata.mflat = flat;
+//                        xdata.mflon = flon;
+//                        xdata.mfrelx = frelx;
+//                        xdata.mfrely = frely;
+//                        xdata.mheight = fheight;
+//                        xdata.mfrels = 0;
+//                        mvectorrtkdata.push_back(xdata);
+
+//                    }
+//                }
+
+//                QString strline = QString::number(flon,'f',7) + QString("\t")
+//                        +QString::number(flat,'f',7)+QString("\t")
+//                        +QString::number(fheight,'f',4)+QString("\t")
+//                        +QString::number(frelx,'f',2) + QString("\t")
+//                        +QString::number(frely,'f',2) + QString("\t");
+//                ui->plainTextEdit->appendPlainText(strline);
+//            }
+//        }
+
+
+// //       std::vector<iv::rtkdata> * pxvectorrtkdata = &mvectorrtkdata;
+//        ui->pushButton_CreateRoad->setEnabled(true);
+//    }
+
+
 }
 
 void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()

src/tool/map_lanetoxodr/dialogaddroadfromrtk.h

@@ -45,9 +45,13 @@ private:
 
     std::vector<iv::rtkdata> mvectorrtkdata;
 
+public:
+    static void calcrelxy(double flon0,double flat0,double flon,double flat,double & relx,double &rely);
+    static std::vector<iv::rtkdata> loadrtkdata(QString strfilepath,double & flon0,double & flat0,OpenDrive * pxodr);
+
 private:
     double getvaluefromdegreestring(QByteArray ba);
-    void calcrelxy(double flon0,double flat0,double flon,double flat,double & relx,double &rely);
+
 };
 
 #endif // DIALOGADDROADFROMRTK_H

src/tool/map_lanetoxodr/dialoglanefromrtk.cpp

@@ -0,0 +1,250 @@
+#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;
+
+    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)
+        {
+            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;
+                        }
+
+                    }
+                }
+            }
+        }
+    }
+
+    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;
+        }
+        VectorXd coeffs = polyfit(x_veh, y_veh, nrange -1);
+        double a[4];
+        for(j=0;j<4;j++)a[j] = 0;
+        for(j=0;j<nrange;j++)a[j] = coeffs[j];
+        pLane->AddWidthRecord(0,a[0],a[1],a[2],a[3]);
+    }
+
+
+    ui->pushButton_CreateLane->setEnabled(false);
+}

src/tool/map_lanetoxodr/dialoglanefromrtk.h

@@ -0,0 +1,40 @@
+#ifndef DIALOGLANEFROMRTK_H
+#define DIALOGLANEFROMRTK_H
+
+#include <QDialog>
+
+#include "OpenDrive/OpenDrive.h"
+
+#include "dialogaddroadfromrtk.h"
+
+#include <QMessageBox>
+
+namespace Ui {
+class DialogLaneFromRTK;
+}
+
+class DialogLaneFromRTK : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogLaneFromRTK(Road * pRoad,OpenDrive * pxodr,QWidget *parent = nullptr);
+    ~DialogLaneFromRTK();
+
+private slots:
+    void on_pushButton_LoadData_clicked();
+
+    void on_pushButton_CreateLane_clicked();
+
+private:
+    Ui::DialogLaneFromRTK *ui;
+
+    Road * mpRoad;
+    OpenDrive * mpxodr;
+    double mlat0;
+    double mlon0;
+
+    std::vector<iv::rtkdata> mvectorrtkdata;
+};
+
+#endif // DIALOGLANEFROMRTK_H

src/tool/map_lanetoxodr/dialoglanefromrtk.ui

@@ -0,0 +1,55 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogLaneFromRTK</class>
+ <widget class="QDialog" name="DialogLaneFromRTK">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>679</width>
+    <height>317</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QPushButton" name="pushButton_LoadData">
+   <property name="geometry">
+    <rect>
+     <x>100</x>
+     <y>22</y>
+     <width>161</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Load Data</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_CreateLane">
+   <property name="geometry">
+    <rect>
+     <x>390</x>
+     <y>21</y>
+     <width>161</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Create Lane</string>
+   </property>
+  </widget>
+  <widget class="QPlainTextEdit" name="plainTextEdit">
+   <property name="geometry">
+    <rect>
+     <x>60</x>
+     <y>70</y>
+     <width>551</width>
+     <height>211</height>
+    </rect>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -32,6 +32,7 @@ SOURCES += \
     dialogaddroadfromrtk.cpp \
     dialogeditlane.cpp \
     dialogeditroadmark.cpp \
+    dialoglanefromrtk.cpp \
     ivxodrtool.cpp \
         main.cpp \
         mainwindow.cpp \
@@ -74,6 +75,7 @@ HEADERS += \
     dialogaddroadfromrtk.h \
     dialogeditlane.h \
     dialogeditroadmark.h \
+    dialoglanefromrtk.h \
     ivxodrtool.h \
         mainwindow.h \
     rawtype.h \
@@ -112,6 +114,7 @@ FORMS += \
         dialogaddroadfromrtk.ui \
         dialogeditlane.ui \
         dialogeditroadmark.ui \
+        dialoglanefromrtk.ui \
         mainwindow.ui \
         roadeditdialog.ui \
         speeddialog.ui \

src/tool/map_lanetoxodr/roadeditdialog.cpp

@@ -426,3 +426,15 @@ void RoadEditDialog::on_pushButton_EditRoadMark_clicked()
     roadmarkd.exec();
     on_comboBox_Road_currentIndexChanged(ui->comboBox_Road->currentIndex());
 }
+
+void RoadEditDialog::on_pushButton_LaneFromRTK_clicked()
+{
+    if(mpCurRoad == 0)
+    {
+        QMessageBox::warning(this,"Warning","Not Select Road");
+        return;
+    }
+    DialogLaneFromRTK lanertk(mpCurRoad,mpxodr,this);
+    lanertk.exec();
+    on_comboBox_Road_currentIndexChanged(ui->comboBox_Road->currentIndex());
+}

src/tool/map_lanetoxodr/roadeditdialog.h

@@ -13,6 +13,7 @@
 
 #include "dialogeditlane.h"
 #include "dialogeditroadmark.h"
+#include "dialoglanefromrtk.h"
 
 namespace Ui {
 class RoadEditDialog;
@@ -42,6 +43,8 @@ private slots:
 
     void on_pushButton_EditRoadMark_clicked();
 
+    void on_pushButton_LaneFromRTK_clicked();
+
 private:
     Ui::RoadEditDialog *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/roadeditdialog.ui

@@ -246,6 +246,19 @@
     <string>Edit RoadMark</string>
    </property>
   </widget>
+  <widget class="QPushButton" name="pushButton_LaneFromRTK">
+   <property name="geometry">
+    <rect>
+     <x>600</x>
+     <y>210</y>
+     <width>211</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Lane From RTK</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -464,15 +464,33 @@ double xodrfunc::GetLineDis(GeometryLine * pline,const double x,const double y,d
 }
 
 
-int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad, GeometryBlock **pgeo, double &fdis, double &nearx, double &neary, double &nearhead, const double nearthresh,double * pfs,int * pnlane)
+namespace iv {
+struct nearoption
+{
+    Road * pRoad;
+    GeometryBlock * pgeob;
+    double fdis;
+    double nearx;
+    double neary;
+    double nearhead;
+    double fs;
+    int nlane;
+    double fgeodis;
+};
+}
+
+int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad, GeometryBlock **pgeo,
+                           double &fdis, double &nearx, double &neary, double &nearhead,
+                           const double nearthresh,double * pfs,int * pnlane,bool bnotuselane)
 {
     double dismin = std::numeric_limits<double>::infinity();
     fdis = dismin;
-    int i;
+    unsigned int i;
     *pObjRoad = 0;
+    std::vector<iv::nearoption> xvectornearopt;
     for(i=0;i<pxodr->GetRoadCount();i++)
     {
-        int j;
+        unsigned int j;
         Road * proad = pxodr->GetRoad(i);
         double nx,ny,nh,frels;
 
@@ -506,12 +524,32 @@ int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Roa
                 break;
             }
 
-            if(dis < 100)
+            double fgeodis;
+
+            fgeodis = dis;
+            if((dis < 100)&&(bnotuselane == false))
             {
                 double faccuratedis;
                 faccuratedis = GetAcurateDis(x,y,proad,frels+pg->GetS(),nx,ny,nh,&nlane);
                 if(faccuratedis < dis)dis = faccuratedis;
+
+            }
+
+            if(dis == 0)
+            {
+                iv::nearoption xopt;
+                xopt.fdis = dis;
+                xopt.fgeodis = fgeodis;
+                xopt.fs = frels +pg->GetS();
+                xopt.nearhead = nh;
+                xopt.nearx = nx;
+                xopt.neary = ny;
+                xopt.nlane = nlane;
+                xopt.pgeob = pgb;
+                xopt.pRoad = proad;
+                xvectornearopt.push_back(xopt);
             }
+
             if(dis < dismin)
             {
                 dismin = dis;
@@ -527,6 +565,30 @@ int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Roa
         }
     }
 
+    if(xvectornearopt.size() > 1)
+    {
+        double fgeodismin = 1000;
+        int nindex = 0;
+        for(i=0;i<xvectornearopt.size();i++)
+        {
+            if(xvectornearopt.at(i).fgeodis < fgeodismin)
+            {
+                fgeodismin = xvectornearopt.at(i).fgeodis;
+                nindex = i;
+            }
+        }
+        dismin = xvectornearopt.at(nindex).fdis;
+        nearx = xvectornearopt.at(nindex).nearx;
+        neary = xvectornearopt.at(nindex).neary;
+        nearhead = xvectornearopt.at(nindex).nearhead;
+        fdis = dismin;
+        *pObjRoad = xvectornearopt.at(nindex).pRoad;
+        *pgeo = xvectornearopt.at(nindex).pgeob;
+        if(pfs != 0)*pfs = xvectornearopt.at(nindex).fs;
+        if(pnlane != 0)*pnlane = xvectornearopt.at(nindex).nlane;
+
+    }
+
 
     if(fdis > nearthresh)return -1;
     return 0;

src/tool/map_lanetoxodr/xodrfunc.h

@@ -43,7 +43,7 @@ public:
     static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
                              double & neary,double & nearhead,double & frels);
     static int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
-                     double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0);
+                     double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 
     static double GetAcurateDis(const double x,const double y,Road * pRoad,const  double  s,const double nearx,
                                 const double neary,const double nearhead,int * pnlane= 0);