#include "trace2vectormap.h" #include #include #include "math/gnss_coordinate_convert.h" trace2vectormap::trace2vectormap(iv::trace2vectormap_param xparam) { mparam = xparam; } void trace2vectormap::llh2xyy(double flon, double flat, double fhead, double & x, double & y, double & fyaw, double flon0, double flat0) { double x_o,y_o; double x_now,y_now; GaussProjCal(flon0,flat0,&x_o,&y_o); GaussProjCal(flon,flat,&x_now,&y_now); x = x_now - x_o; y = y_now - y_o; fyaw = (90 - fhead)*M_PI/180.0; } int trace2vectormap::convert() { iv::trace2vectormap_param xparam = mparam; QFile xFile; xFile.setFileName(xparam.strtracefilepath.data()); if(!xFile.open(QIODevice::ReadOnly)) { return -1; } QByteArray ba = xFile.readAll(); QList baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n'); int nline = baline.size(); int i; int npid = 1; std::vector xvectorpoint; std::vector xvectornode; std::vector xvectorlane; std::vector xvectorline; std::vector xvectorwhiteline; std::vector xvectordtlane; std::vector xvectorarea; std::vector xvectorwayarea; iv::vectormap::point xlastpoint; bool bhavelast = false; for(i=0;i badata = baline[i].split('\t'); QStringList badata = x.split(QRegExp("[\t ,;]")); if(badata.size()>=10) { double flon = QString(badata[1]).toDouble(); double flat = QString(badata[2]).toDouble(); double fheading = QString(badata[5]).toDouble(); int nspeedmode = QString(badata[7]).toInt(); double fspeed = xparam.fVelLim; std::map::iterator it = xparam.mapmodevel.find(nspeedmode); if(it !=xparam.mapmodevel.end()) { fspeed = it->second; } // std::cout<<"lat: "<=0.5) { npid++; xvectorpoint.push_back(xpoint); xlastpoint = xpoint; } } } } if(xvectorpoint.size()<2)return 0; bool bcircle = false; iv::vectormap::point xpointstart,xpointend; xpointstart = xvectorpoint[0]; xpointend = xvectorpoint[xvectorpoint.size() -1]; double fdis = sqrt(pow(xpointstart.bx-xpointend.bx,2)+pow(xpointstart.ly - xpointend.ly,2)); if(fdis<10.0) { double fyawdiff = xpointstart.fyaw - xpointend.fyaw; while(fyawdiff<0)fyawdiff = fyawdiff + 2.0*M_PI; while(fyawdiff>=(2.0*M_PI))fyawdiff = fyawdiff - 2.0*M_PI; if((fyawdiff<0.1)||(fyawdiff>(2.0*M_PI -0.1))) { std::cout<<" circle mode "<::iterator it = xparam.mapmodevel.find() snprintf(strline,1000,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%d,%d\n", xvectorlane[i].lnid,xvectorlane[i].did,xvectorlane[i].blid, xvectorlane[i].flid,xvectorlane[i].bnid,xvectorlane[i].fnid, 0,0,0,0, 0,0,0,0, 1,xvectorlane[i].lcnt,xvectorlane[i].lno,0, xvectorlane[i].fspeed/3.6,xvectorlane[i].fspeed/3.6,0,1); xFileLane.write(strline); } xFileLane.close(); QFile xFiledtLane; xFiledtLane.setFileName(strfolder + "/dtlane.csv"); if(!xFiledtLane.open(QIODevice::ReadWrite)) { qDebug(" dtlane file open fail."); return -6; } snprintf(strline,1000,"DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW\n"); xFiledtLane.write(strline); for(i=0;i<(int)xvectordtlane.size();i++) { snprintf(strline,1000,"%d,%f,%d,%f,%f,%f,%f,%f,%f,%f\n", xvectordtlane[i].did,xvectordtlane[i].Dist,xvectordtlane[i].pid, xvectordtlane[i].Dir,xvectordtlane[i].Apara,xvectordtlane[i].r, xvectordtlane[i].slope,xvectordtlane[i].cant,xvectordtlane[i].LW, xvectordtlane[i].RW); xFiledtLane.write(strline); } xFiledtLane.close(); QFile xFileArea; xFileArea.setFileName((strfolder + "/area.csv")); if(!xFileArea.open(QIODevice::ReadWrite)) { std::cout<<" Area File open fail."<