void BehaviorPrediction::DoOneStep(const std::vector& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map)
{
  if(!m_bUseFixedPrediction && maxDeceleration !=0)
    m_PredictionDistance = -pow(currPose.v, 2)/(maxDeceleration);

  ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);//提取軌跡/m_TrackedObjects
  CalculateCollisionTimes(minSpeed);

  if(m_bParticleFilter)
  {
    ParticleFilterSteps(m_ParticleInfo_II);//微粒過濾步驟
  }
}

void BehaviorPrediction::ExtractTrajectoriesFromMap(const std::vector& curr_obj_list,RoadNetwork& map, std::vector& old_obj_list)
{
  PlannerH planner;
  m_temp_list_ii.clear();//存放當(dāng)前幀的障礙物列表

  std::vectordelete_me_list = old_obj_list;//m_ParticleInfo_II，起始為空

  for(unsigned int i=0; i< curr_obj_list.size(); i++)
  {
    bool bMatch = false;
    for(unsigned int ip=0; ip< old_obj_list.size(); ip++)//遍歷舊的障礙物列表，是否找到與當(dāng)前新障礙物對應(yīng)的舊障礙物
    {
      if(old_obj_list.at(ip)->obj.id == curr_obj_list.at(i).id)//如果有
      {
        bool bFound = false;
        for(unsigned int k=0; k< m_temp_list_ii.size(); k++)//遍歷當(dāng)前障礙物表m_temp_list_ii，是否存在障礙物與舊障礙物相同
        {
          if(m_temp_list_ii.at(k) == old_obj_list.at(ip))//若有，不加入m_temp_list_ii
          {
            bFound = true;
            break;
          }
        }

        if(!bFound)//若m_temp_list_ii沒有找到對應(yīng)的障礙物，把new_obj加入m_temp_list_ii
        {
          old_obj_list.at(ip)->obj = curr_obj_list.at(i);
          m_temp_list_ii.push_back(old_obj_list.at(ip));
        }

        DeleteFromList(delete_me_list, old_obj_list.at(ip));

        old_obj_list.erase(old_obj_list.begin()+ip);
        bMatch = true;
        break;
      }
    }

    if(!bMatch)//如果old_obj_list.at(ip)->obj.id ！=curr_obj_list.at(i).id，curr_obj_list.at(i)加入 m_temp_list_ii
    {
      ObjParticles* pNewObj = new  ObjParticles();
      pNewObj->obj = curr_obj_list.at(i);
      m_temp_list_ii.push_back(pNewObj);
    }
  }

  DeleteTheRest(delete_me_list);
  old_obj_list.clear();
  old_obj_list = m_temp_list_ii;

  //m_PredictedObjects.clear();遍歷每一個障礙物生成多條預(yù)測軌跡
  for(unsigned int ip=0; ip< old_obj_list.size(); ip++)
  {
    PredictCurrentTrajectory(map, old_obj_list.at(ip));
    //m_PredictedObjects.push_back(old_obj_list.at(ip)->obj);
    old_obj_list.at(ip)->MatchTrajectories();
  }

}

double PlannerH::PredictTrajectoriesUsingDP(const WayPoint& startPose, std::vectorclosestWPs, const double& maxPlanningDistance, std::vector>& paths, const bool& bFindBranches , const bool bDirectionBased, const bool pathDensity)
{
  vector>tempCurrentForwardPathss;
  vectorall_cell_to_delete;
  vectorglobalPath;

  vectorpLaneCells;
  vectorunique_lanes;
  std::vectorpath;
  //遍歷當(dāng)前障礙物的每一個可行最近點
  for(unsigned int j = 0 ; j< closestWPs.size(); j++)
  {
    pLaneCells.clear();
    //從最近點開始用dp開始搜索，遍歷獲得幾條路徑
    int nPaths =  PlanningHelpers::PredictiveIgnorIdsDP(closestWPs.at(j), maxPlanningDistance, all_cell_to_delete, pLaneCells, unique_lanes);
    for(unsigned int i = 0; i< pLaneCells.size(); i++)
    {
      path.clear();
      //回溯路經(jīng)給path
      PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), closestWPs.at(j), globalPath, path, tempCurrentForwardPathss);
     //遍歷獲得的路徑上每個點，找到對應(yīng)的 unique_lanes，沒找到，加入
      for(unsigned int k = 0; k< path.size(); k++)
      {
        bool bFoundLaneID = false;
        //unique_lanes起始為空，判斷unique_lanes中是否存在path.at(k).laneId，如存在，不加入unique_lanes中
        for(unsigned int l_id = 0 ; l_id< unique_lanes.size(); l_id++)
        {
          if(unique_lanes.at(k).laneId == unique_lanes.at(l_id))
          {
            bFoundLaneID = true;
            break;
          }
        }

        if(!bFoundLaneID)
          unique_lanes.push_back(path.at(k).laneId);//存放當(dāng)前path中所有的不同path.at(k).laneId
      }

      if(path.size()>0)
      {//把障礙物位置加入path中，設(shè)置屬性
        path.insert(path.begin(), startPose);
        if(!bDirectionBased)
          path.at(0).pos.a = path.at(1).pos.a;

        path.at(0).beh_state = path.at(1).beh_state = PlannerHNS::BEH_FORWARD_STATE;
        path.at(0).laneId = path.at(1).laneId;

        PlanningHelpers::FixPathDensity(path, pathDensity);
        PlanningHelpers::SmoothPath(path,0.4,0.3,0.1);
        PlanningHelpers::CalcAngleAndCost(path);
        paths.push_back(path);
      }
    }
  }

  if(bDirectionBased && bFindBranches)
  {
    WayPoint p1, p2;
    if(paths.size()>0 && paths.at(0).size() >0)
      p2 = p1 = paths.at(0).at(0);
    else
      p2 = p1 = startPose;

    double branch_length = maxPlanningDistance*0.5;

    p2.pos.y = p1.pos.y + branch_length*0.4*sin(p1.pos.a);
    p2.pos.x = p1.pos.x + branch_length*0.4*cos(p1.pos.a);

    vectorl_branch;
    vectorr_branch;
   //手工生成分支，以p1, p2,為起點，branch_length為距離生成終點
    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_RIGHT_DIR,r_branch);
    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_LEFT_DIR, l_branch);

    PlanningHelpers::FixPathDensity(l_branch, pathDensity);
    PlanningHelpers::SmoothPath(l_branch,0.4,0.3,0.1);
    PlanningHelpers::CalcAngleAndCost(l_branch);

    PlanningHelpers::FixPathDensity(r_branch, pathDensity);
    PlanningHelpers::SmoothPath(r_branch,0.4,0.3,0.1);
    PlanningHelpers::CalcAngleAndCost(r_branch);

    paths.push_back(l_branch);
    paths.push_back(r_branch);
  }

  DeleteWaypoints(all_cell_to_delete);

  return paths.size();
}

int PlanningHelpers::PredictiveIgnorIdsDP(WayPoint* pStart, const double& DistanceLimit,
    vector& all_cells_to_delete,vector& end_waypoints, std::vector& lanes_ids)
{
  if(!pStart) return 0;

    vector>nextLeafToTrace;

    WayPoint* pZero = 0;
    WayPoint* wp    = new WayPoint();
    *wp = *pStart;
    wp->cost = 0;
    wp->pLeft = 0;
    wp->pRight = 0;
    nextLeafToTrace.push_back(make_pair(pZero, wp));//當(dāng)前搜索列表
    all_cells_to_delete.push_back(wp);

    double     distance     = 0;
    end_waypoints.clear();
    double     nCounter     = 0;

    while(nextLeafToTrace.size()>0)//當(dāng)前搜索列表不為0
    {
      nCounter++;//搜尋次數(shù)

      WayPoint* pH   = nextLeafToTrace.at(0).second;//當(dāng)前搜索列表第一個元素，設(shè)為當(dāng)前搜索元素

      assert(pH != 0);  // 如果 pH == 0，則程序在此終止，下面的程序都不會執(zhí)行
<<<<<<< HEAD
      nextLeafToTrace.erase(nextLeafToTrace.begin()+0);//從當(dāng)前搜索列表中除去當(dāng)前搜索元素

      for(unsigned int i =0; i< pH->pFronts.size(); i++)//遍歷當(dāng)前元素的下一個元素
=======
      nextLeafToTrace.erase(nextLeafToTrace.begin()+0);
      // All points in front of pH
      for(unsigned int i =0; i< pH->pFronts.size(); i++)
>>>>>>>1e30b21e93b7e9bd87ade267651cd491da401bd6
      {
        if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))//若存在，且不再all_cells_to_delete中
        {
          if(pH->cost< DistanceLimit)//當(dāng)前點距離代價小于DistanceLimit
          {
            wp = new WayPoint();
            *wp = *pH->pFronts.at(i);
           //計算代價
            double d = distance2points(wp->pos, pH->pos);
            distance += d;
            wp->cost = pH->cost + d;
            wp->pBacks.push_back(pH);
            wp->pLeft = 0;
            wp->pRight = 0;

            bool bFoundLane = false;
            // lanes_ids起始為空，遍歷lanes_ids，判斷是否存在laneid與wp->laneId相同，若存在，不加入 nextLeafToTrace。
            for(unsigned int k = 0 ; k< lanes_ids.size(); k++)
            {
              if(wp->laneId == lanes_ids.at(k))
              {
                bFoundLane = true;
                break;
              }
            }
            // 如果在 lanes_ids 里找不到 wp 所在的 laneId
            if(!bFoundLane)
              nextLeafToTrace.push_back(make_pair(pH, wp));
            all_cells_to_delete.push_back(wp);
          }
          else//當(dāng)前點距離代價大于DistanceLimit
          // 如果超出搜索距離
          {
            end_waypoints.push_back(pH);//加入end_waypoints
          }
        }
      }
    }

    while(nextLeafToTrace.size()!=0)
      nextLeafToTrace.pop_back();
    //closed_nodes.clear();

    return end_waypoints.size();
}

你是否還在尋找穩(wěn)定的海外服務(wù)器提供商？創(chuàng)新互聯(lián)www.cdcxhl.cn海外機(jī)房具備T級流量清洗系統(tǒng)配攻擊溯源，準(zhǔn)確流量調(diào)度確保服務(wù)器高可用性，企業(yè)級服務(wù)器適合批量采購，新人活動首月15元起，快前往官網(wǎng)查看詳情吧