change odtolanelet.

src/map/odtolanelet/odtolanelet.cpp

@@ -55,14 +55,28 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
         return -1;
     }
 
-    iv::RoadSample * pSample = new iv::RoadSample(pRoad);
+    std::shared_ptr<iv::RoadSample> pSample_ptr = std::shared_ptr<iv::RoadSample>(new iv::RoadSample(pRoad));
+ //   iv::RoadSample * pSample = new iv::RoadSample(pRoad);
+
+    std::vector<iv::RoadPoint> * pvectorRoadPoint = pSample_ptr->GetRoadPointVector();
+
+    unsigned int nRPSize = static_cast<unsigned int>(pvectorRoadPoint->size());
+
+    unsigned int i;
+
+    for(i=0;i<nRPSize;i++)
+    {
+        unsigned int j;
+//        pvectorRoadPoint->at(i).mvectorLanePoint
+    }
+
+    return  0;
 
     std::vector<lanelet::LineString3d> xvectorlinestring;
     std::vector<std::string> xvectortype;
     std::vector<std::string> xvectorsubtype;
 
     unsigned int nLSCount = pRoad->GetLaneSectionCount();
-    unsigned int i;
 
     unsigned nleftcount = 0;
     unsigned nrightcount = 0;

src/map/odtolanelet/roadsample.cpp

@@ -38,10 +38,14 @@ int RoadSample::SampleRoad(Road * pRoad)
             break;
         }
 
+        std::vector<double> xvectorFeaturePoint = GetFeaturePoint(pLS);
+
         double fLastRefX,fLastRefY,fLastRefZ,fLastRefHdg,fLastOffsetValue,fLastS;
         bool bHaveLast = false;
         std::vector<LanePoint> xvectorLastLanePoint;
 
+        double sstart_Section = pLS->GetS();
+
         double send_Section = pLS->GetS();
         if(i == (nLSCount -1))
         {
@@ -65,16 +69,22 @@ int RoadSample::SampleRoad(Road * pRoad)
 
             if(fabs(send_Section - fS)<0.1)
             {
-                if(i == (nLSCount - 1))
-                {
-                    fS = send_Section;
-                }
-                else
-                {
-                    break; //If Not Last Section, use Next Section Start Value.
-                }
+                fS = send_Section;  //If Section End,Same Point
+//                if(i == (nLSCount - 1))
+//                {
+//                    fS = send_Section;
+//                }
+//                else
+//                {
+//                    fS = send_Section;
+//                }
             }
 
+            bool bIsCrossFeature = false;
+
+            if((fS - sstart_Section)>fStep*0.5)
+                bIsCrossFeature = IsCrossFeaturePoint(xvectorFeaturePoint,fS,fLastS);
+
             RoadPoint xRP;
             xRP.ms = fS;
             xRP.mfRefX = fRefX;
@@ -111,12 +121,47 @@ int RoadSample::SampleRoad(Road * pRoad)
 
             if((bInserPoint == false)&&(bHaveLast))
             {
-                if(IsMarkChange(xvectorLanePoint,xvectorLastLanePoint))
+
+                if(bIsCrossFeature)
                 {
+                    xRP.mvectorLanePoint = xvectorLanePoint;
+                    unsigned int k;
+                    unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
+                    for(k=0;k<ksize;k++)
+                    {
+                        xRP.mvectorLanePoint[k].mstrroadmarkcolor = mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mstrroadmarkcolor;
+                        xRP.mvectorLanePoint[k].mstrroadmarktype = mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mstrroadmarktype;
+                    }
+                    mvectorRoadPoint.push_back(xRP);
                     bInserPoint = true;
                     nSec++;
                     xRP.nSecNum = nSec;
                 }
+
+//                if(IsMarkChange(xvectorLanePoint,xvectorLastLanePoint))
+//                {
+//                    bInserPoint = true;
+//                    nSec++;
+//                    xRP.nSecNum = nSec;
+//                    mvectorRoadPoint.push_back(mvectorRoadPoint[mvectorRoadPoint.size() -1]);
+//                    mvectorRoadPoint[mvectorRoadPoint.size()-1].nSecNum = nSec;
+//                }
+            }
+
+            if((bInserPoint == false)&&(bHaveLast))
+            {
+
+                xRP.mvectorLanePoint = xvectorLanePoint;
+                unsigned int k;
+                unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
+                for(k=0;k<ksize;k++)
+                {
+                    if(fabs(xRP.mvectorLanePoint[k].mfLaneWidth - mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mfLaneWidth )>0.1)
+                    {
+                        bInserPoint = true;
+                        break;
+                    }
+                }
             }
 
 
@@ -203,6 +248,7 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
         xLanePoint.mstrroadmarktype = xRoadMark.GetType();
         xLanePoint.nLane = static_cast<int>(i);
         xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+        xLanePoint.mfLaneWidth = xLeftWidth[nLeftLaneCount-1 -i];
         xvectorLanePoint.push_back(xLanePoint);
     }
 
@@ -213,6 +259,7 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
     xLanePoint.mstrroadmarktype = xRoadMark.GetType();
     xLanePoint.nLane = static_cast<int>(i);
     xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+    xLanePoint.mfLaneWidth = 0.0;
     xvectorLanePoint.push_back(xLanePoint);
 
     for(i=0;i<nRightLaneCount;i++)
@@ -222,6 +269,7 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
         xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
         xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
         xLanePoint.mstrroadmarktype = xRoadMark.GetType();
+        xLanePoint.mfLaneWidth = xRightWidth[i];
         xLanePoint.nLane = static_cast<int>(i);
         xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
         xvectorLanePoint.push_back(xLanePoint);
@@ -266,6 +314,74 @@ std::vector<RoadPoint> * RoadSample::GetRoadPointVector()
     return  &mvectorRoadPoint;
 }
 
+std::vector<double> RoadSample::GetFeaturePoint(LaneSection * pLS)
+{
+    unsigned int i;
+    unsigned int nlanecount = pLS->GetLaneCount();
+    std::vector<double> xvectorS;
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        if(pLane != NULL)
+        {
+            unsigned int nMarkCount = pLane->GetLaneRoadMarkCount();
+            unsigned int j;
+            for(j=0;j<nMarkCount;j++)
+            {
+                LaneRoadMark * pMark = pLane->GetLaneRoadMark(j);
+                if(fabs(pMark->GetS())>0.001)
+                {
+                    xvectorS.push_back(pMark->GetS());
+                }
+            }
+        }
+    }
+
+    std::vector<double> xvectorFeature;
+    if(xvectorS.size() == 0)return xvectorFeature;
+    while(xvectorS.size()>0)
+    {
+        unsigned int indexmin = 0;
+        unsigned int nsize = static_cast<unsigned int >(xvectorS.size());
+        for(i=1;i<nsize;i++)
+        {
+            if(xvectorS[i] < xvectorS[indexmin])
+            {
+                indexmin = i;
+            }
+        }
+        if(xvectorFeature.size() == 0)
+        {
+            xvectorFeature.push_back(xvectorS[indexmin]);
+        }
+        else
+        {
+            if(fabs(xvectorS[indexmin] - xvectorFeature[xvectorFeature.size()-1]) > 0.001)
+            {
+                xvectorFeature.push_back(xvectorS[indexmin]);
+            }
+        }
+    }
+
+    return xvectorFeature;
+}
+
+bool RoadSample::IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast)
+{
+    unsigned int nSize = static_cast<unsigned int>(xvectorFeature.size());
+    unsigned int i;
+    for(i=0;i<nSize;i++)
+    {
+        if(((fSLast - xvectorFeature[i])<-0.001)&&((fS+0.1-xvectorFeature[i])>0))
+        {
+            fS = xvectorFeature[i];
+            return true;
+        }
+    }
+
+    return false;
+}
+
 }
 
 

src/map/odtolanelet/roadsample.h

@@ -73,6 +73,10 @@ private:
     int SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::vector<LanePoint> & xvectorLanePoint);
 
     bool IsMarkChange(std::vector<LanePoint> & xvectorLastLanePoint,std::vector<LanePoint> & xvectorLanePoint);
+
+    std::vector<double> GetFeaturePoint(LaneSection * pLS);
+
+    bool IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast);
 };
 
 }