change tool/map_lanetoxodr. change when use rtk update road, and add use rtk fit elevation.

src/tool/map_lanetoxodr/dialogaddroadfromrtk.cpp

@@ -22,6 +22,7 @@ DialogAddRoadFromRTK::DialogAddRoadFromRTK(OpenDrive * pxodr,double lon0,double
     mlat0 = lat0;
     mlon0 = lon0;
     ui->setupUi(this);
+    ui->pushButton_CreateRoad->setEnabled(false);
 }
 
 DialogAddRoadFromRTK::~DialogAddRoadFromRTK()
@@ -121,7 +122,8 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
         QList<QByteArray> strlinedata=  ba.split('\n');
         int i;
         int nsize = strlinedata.size();
-        for(i=1;i<nsize;i++)
+        const int nstartline = 1;
+        for(i=nstartline;i<nsize;i++)
         {
             double flat,flon,fheight,frelx,frely;
             QList<QByteArray> strlistvalue = strlinedata.at(i).split(',');
@@ -142,9 +144,18 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                 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==1)
+                if(i==nstartline)
                 {
                     if(mpxodr->GetRoadCount()<1)
                     {
@@ -160,6 +171,7 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                         xdata.mfrelx = frelx;
                         xdata.mfrely = frely;
                         xdata.mheight = fheight;
+                        xdata.mfrels = 0;
                         mvectorrtkdata.push_back(xdata);
 
                     }
@@ -173,6 +185,10 @@ void DialogAddRoadFromRTK::on_pushButton_LoadData_clicked()
                 ui->plainTextEdit->appendPlainText(strline);
             }
         }
+
+
+ //       std::vector<iv::rtkdata> * pxvectorrtkdata = &mvectorrtkdata;
+        ui->pushButton_CreateRoad->setEnabled(true);
     }
 }
 
@@ -356,4 +372,31 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
     pLS->SetS(0);
     pLS->AddLane(0,0,"none",false);
     pLL = pLS->GetLane(0);
+
+    double ele_coff[4];
+    for(j=0;j<4;j++)ele_coff[j] = 0;
+    if(mvectorrtkdata.size()>0)ele_coff[0] = mvectorrtkdata[0].mheight;
+    int M = mvectorrtkdata.size();
+    VectorXd x_vehhg(M);
+    VectorXd y_vehhg(M);
+    for(j=0;j<M;j++)
+    {
+        x_vehhg[j] = mvectorrtkdata[j].mfrels;
+        y_vehhg[j] =mvectorrtkdata[j].mheight;
+    }
+    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];
+        }
+    }
+
+    p->AddElevation(0,ele_coff[0],ele_coff[1],ele_coff[2],ele_coff[3]);
+
+    ui->pushButton_CreateRoad->setEnabled(false);
 }

src/tool/map_lanetoxodr/dialogaddroadfromrtk.h

@@ -19,6 +19,7 @@ struct rtkdata
     double mheight;
     double mfrelx;
     double mfrely;
+    double mfrels;
 
 };
 }

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -4817,4 +4817,8 @@ void MainWindow::on_actionAdd_Road_From_RTK_triggered()
     arfrd.exec();
     mpLELat0->setText(QString::number(glat0,'f',7));
     mpLELon0->setText(QString::number(glon0,'f',7));
+    updateCBRoad();
+    mbRefresh = true;
+    update();
+
 }