change tool/map_lanetoxodr for add fromrtk data fit.

src/tool/map_lanetoxodr/dialogaddroadfromrtk.cpp

@@ -3,6 +3,14 @@
 
 #include <QFileDialog>
 
+#include "geofit.h"
+#include "circlefitting.h"
+#include <QMessageBox>
+
+#include "mainwindow.h"
+
+extern MainWindow * gw;
+
 extern double glon0 ;
 extern double glat0;
 
@@ -103,6 +111,7 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
     }
 
     ui->plainTextEdit->clear();
+    mvectorrtkdata.clear();
     ui->plainTextEdit->appendPlainText(QString("Longitude\tLatitude\tHeight\trelx\trely"));
 
     if(xFile.open(QIODevice::ReadWrite))
@@ -127,6 +136,14 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
 
                 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;
+                mvectorrtkdata.push_back(xdata);
+
                 if(i==2)
                 {
                     if(mpxodr->GetRoadCount()<1)
@@ -136,6 +153,15 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                         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;
+                        mvectorrtkdata.push_back(xdata);
+
                     }
                 }
 
@@ -149,3 +175,185 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
         }
     }
 }
+
+void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
+{
+    if(mvectorrtkdata.size()<2)
+    {
+        QMessageBox::warning(this,"Warning","Not Load Data.",QMessageBox::YesAll);
+        return;
+    }
+
+    double LINE_ERROR = 0.1;
+
+
+    std::vector<geobase> xvectorgeo;
+
+    bool bComplete = false;
+    int j;
+    int ncurpos = 0;
+    int nrange = mvectorrtkdata.size();
+    while(bComplete == false)
+    {
+
+        VectorXd x_veh(nrange);
+        VectorXd y_veh(nrange);
+        for(j=0;j<nrange;j++)
+        {
+            x_veh[j] = mvectorrtkdata.at(j+ncurpos).mfrelx;
+            y_veh[j] = mvectorrtkdata.at(j+ncurpos).mfrely;
+        }
+
+        bool bArcOk = false;
+        bool bLineOk = false;
+        double dismax = 0;
+        QPointF sp,ep;
+        double fR,flen;
+        double fhdgstart,fhdgend;
+        if(nrange >= 3)
+        {
+
+            geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);
+
+            if(dismax < LINE_ERROR)
+            {
+                bArcOk = true;
+                std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            }
+        }
+
+
+        auto coeffs = polyfit(x_veh, y_veh, 1);
+
+        dismax = 0;
+        for(j=0;j<nrange;j++)
+        {
+            double A = coeffs[1];
+            double B = -1;
+            double C = coeffs[0];
+            double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
+            if(dis>dismax)dismax = dis;
+        }
+        if(dismax < LINE_ERROR)
+        {
+            bLineOk = true;
+            std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+        }
+
+
+
+        if((nrange <= 2)||bLineOk||bArcOk)
+        {
+            if(bLineOk)
+            {
+                std::cout<<"use line"<<std::endl;
+                geobase xgeo;
+                xgeo.mnStartPoint = ncurpos;
+                xgeo.mnEndPoint = ncurpos + nrange -1 ;
+
+                double x0,y0,x1,y1;
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
+                        y_veh[0],x0,y0);
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
+                        y_veh[nrange - 1],x1,y1);
+                xgeo.mfA = coeffs[1];
+                xgeo.mfB = -1;
+                xgeo.mfC = coeffs[0];
+                xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
+                xgeo.mfX = x0;
+                xgeo.mfY = y0;
+                xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
+                xgeo.mnType = 0;
+                xvectorgeo.push_back(xgeo);
+            }
+            else
+            {
+                if(bArcOk)
+                {
+                    std::cout<<"use arc"<<std::endl;
+                    geobase xgeo;
+                    xgeo.mnStartPoint = ncurpos;
+                    xgeo.mnEndPoint = ncurpos + nrange -1;
+                    xgeo.mfHdg = fhdgstart;
+                    xgeo.mfHdgStart = fhdgstart;
+                    xgeo.mfHdgEnd = fhdgend;
+                    xgeo.mfX = sp.x();
+                    xgeo.mfY = sp.y();
+                    xgeo.mfLen = flen;
+                    xgeo.mnType = 1;
+                    xgeo.mfEndX = ep.x();
+                    xgeo.mfEndY = ep.y();
+                    xgeo.mR = fR;
+                    xvectorgeo.push_back(xgeo);
+                }
+            }
+            //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            ncurpos = ncurpos + nrange -1;
+            nrange = mvectorrtkdata.size() - ncurpos;
+
+        }
+        else
+        {
+            nrange = nrange/2;
+            if(nrange<2)nrange = 2;
+        }
+        if(ncurpos>=(mvectorrtkdata.size()-1))bComplete = true;
+    }
+
+
+    std::cout<<"complete "<<std::endl;
+
+
+    double xroadlen = 0;
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+        xroadlen = xroadlen + xvectorgeo[j].mfLen;
+    }
+
+    mpxodr->AddRoad("",xroadlen, 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);
+    Lane * pLL;
+    pLS->SetS(0);
+    pLS->AddLane(0,0,"none",false);
+    pLL = pLS->GetLane(0);
+}

src/tool/map_lanetoxodr/dialogaddroadfromrtk.h

@@ -34,6 +34,8 @@ public:
 private slots:
     void on_pushButton_LoadData_clicked();
 
+    void on_pushButton_CreateRoad_clicked();
+
 private:
     Ui::DialogAddRoadFromRTK *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/geofit.cpp

@@ -923,4 +923,10 @@ int geofit::CreateBezier(double xstart, double ystart, double hdg_start, double
     return 0;
 }
 
+geofit & geofit::Inst()
+{
+    static geofit x;
+    return x;
+}
+
 

src/tool/map_lanetoxodr/geofit.h

@@ -72,6 +72,8 @@ public:
     std::vector<geobase> getlinegeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);
     std::vector<geobase> getarcgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);
 
+    static geofit & Inst();
+
 };
 
 #endif // GEOFIT_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -17,11 +17,14 @@ double glat0 = 39.1364713;
 //double glat0 = 39;
 double ghdg0 = 360;
 
+MainWindow * gw;
+
 
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainWindow)
 {
+    gw = this;
     ui->setupUi(this);
 
     mnfac = 1;

src/tool/map_lanetoxodr/mainwindow.h

@@ -314,6 +314,9 @@ private:
 
     void *    mpagpsimu;
 
+public:
+    int CreateRoadID(int ntype = 0); //Create Road ID for new road. default:road  1:not create by lane road.
+
 private:
     void UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
@@ -337,7 +340,7 @@ private:
 //    OpenDrive mxodr;
     Road CreateRoad(Eigen::VectorXd xvals, Eigen::VectorXd yvals);
 
-    int CreateRoadID(int ntype = 0); //Create Road ID for new road. default:road  1:not create by lane road.
+
     int CreateJunctionID();
 
     int GetEndPoint(Road * proad,double & x, double & y, double & hdg);