change map_laentoxodr. coding nds data convert.

src/tool/map_lanetoxodr/ndsdataproc.cpp

@@ -3,6 +3,9 @@
 #include <QFile>
 #include "gnss_coordinate_convert.h"
 
+#include "geofit.h"
+#include "circlefitting.h"
+
 NDSDataProc::NDSDataProc()
 {
 
@@ -32,6 +35,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 
     std::vector<iv::nds_line> xvectorline;
     std::vector<iv::nds_vehicle> xvectorvehicle;
+    std::vector<iv::nds_vl> xvectorvl;
 
     QByteArray ba = xFileline.readAll();
     QList<QByteArray> baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
@@ -94,7 +98,7 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
             xvehicle.height = badata[7].toDouble();
             xvehicle.heading = badata[9].toDouble();
             if(xvehicle.heading<0)xvehicle.heading = xvehicle.heading + 360;
-            if(xvehicle.locationmode == 2)xvectorvehicle.push_back(xvehicle);
+            if(xvehicle.locationmode > 0)xvectorvehicle.push_back(xvehicle);
         }
 
     }
@@ -150,6 +154,9 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
             break;
         }
 
+        iv::nds_vl xvl;
+        xvl.mvehindex = i;
+
         while(xvectorvehicle[i].localtime > xvectorline[nlinenow].localtime)
         {
             nlinenow++;
@@ -157,11 +164,219 @@ int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 
         while(xvectorvehicle[i].localtime == xvectorline[nlinenow].localtime)
         {
-            std::cout<<" veh: "<<i<<" line: "<<nlinenow<<std::endl;
+            xvl.mvectorlineindex.push_back(nlinenow);
+//            std::cout<<" veh: "<<i<<" line: "<<nlinenow<<std::endl;
             nlinenow++;
         }
+        xvectorvl.push_back(xvl);
+
+    }
+
+    //Erase no line info point.
+//    std::cout<<" vl size "<<xvectorvl.size()<<std::endl;
+    i= 0;
+    while(i<(int)xvectorvl.size())
+    {
+        if(xvectorvl[i].mvectorlineindex.size() == 0)
+        {
+            xvectorvl.erase(xvectorvl.begin()+i);
+            continue;
+        }
+        i++;
+    }
+
+
+    //Erase dis<0.5 point
+    i=1;
+    while(i<(int)xvectorvl.size())
+    {
+        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));
+        if(fdis <  0.5)
+        {
+            xvectorvl.erase(xvectorvl.begin()+i);
+            continue;
+        }
+        i++;
+    }
+//    std::cout<<" vl size "<<xvectorvl.size()<<std::endl;
+
+    double fdismin = 100000;
+    double fdismax = 0;
+
+    int nvlsize = xvectorvl.size();
+    fS = 0;
+    for(i=1;i<nvlsize;i++)
+    {
+        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));
+        fS = fS + fdis;
+        if(fdis<fdismin)fdismin = fdis;
+        if(fdis>fdismax)fdismax = fdis;
+        if(fdis> 50)std::cout<<" index "<<xvectorvl[i].mvehindex<<" dis "<<fdis<<" time "<<xvectorvehicle[xvectorvl[i].mvehindex].localtime<<std::endl;
+//        if(fdis<0.6)std::cout<<" index "<<xvectorvl[i].mvehindex<<" dis "<<fdis<<" time "<<xvectorvehicle[xvectorvl[i].mvehindex].localtime<<std::endl;
+    }
+
+
+
+    std::cout<<" fdis "<<fS<<" min "<<fdismin<<" max "<<fdismax<<std::endl;
+
+    double LINE_ERROR = 0.10;
+
+
+    std::vector<geobase> xvectorgeo;
+
+    bool bComplete = false;
+//    int j;
+    int ncurpos = 0;
+    int nrange = xvectorvl.size();
+//    double fXLast;
+//    double fYLast;
+//    bool bFirst = true;
+    while(bComplete == false)
+    {
+
+        VectorXd x_veh(nrange);
+        VectorXd y_veh(nrange);
+        for(j=0;j<nrange;j++)
+        {
+            x_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].frel_x;//mvectorrtkdata.at(j+ncurpos).mfrelx;
+            y_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].frel_y;//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;
+//                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);
+                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;
+//                    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);
+                }
+            }
+            //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            ncurpos = ncurpos + nrange -1;
+            nrange = xvectorvl.size() - ncurpos;
+
+        }
+        else
+        {
+            nrange = nrange/2;
+            if(nrange<2)nrange = 2;
+        }
+        if(ncurpos>=(xvectorvl.size()-1))bComplete = true;
     }
 
+    std::cout<<" geo size "<<xvectorgeo.size()<<std::endl;
+
+
+    std::cout<<"complete "<<std::endl;
+
+
     return 0;
 }

src/tool/map_lanetoxodr/ndsdataproc.h

@@ -3,6 +3,7 @@
 
 #include <string>
 #include <iostream>
+#include <vector>
 #include "gnss_coordinate_convert.h"
 
 
@@ -31,6 +32,12 @@ struct nds_line
     double lanecurv;
 };
 
+struct nds_vl
+{
+    int mvehindex;
+    std::vector<int> mvectorlineindex;
+};
+
 }
 
 class NDSDataProc