public void GetLaneChangeModels(LinePath startingRndfPath, double startingRndfPathWidth, int startingNumLanesLeft, int startingNumLanesRight, LinePath endingRndfPath, double endingRndfPathWidth, bool changeLeft, CarTimestamp rndfPathTimestamp, out ILaneModel startingLaneModel, out ILaneModel endingLaneModel) { LocalRoadModel localRoadModel = this.localRoadModel; // get the selected lane for the starting path int startingSelectedLane = SelectLane(localRoadModel, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp, startingNumLanesLeft, startingNumLanesRight); // calculate the number of lanes left and right for the ending lane int endingNumLanesLeft = startingNumLanesLeft; int endingNumLanesRight = startingNumLanesRight; if (changeLeft) { endingNumLanesLeft--; endingNumLanesRight++; } else { endingNumLanesLeft++; endingNumLanesRight--; } // get the selected lane for the ending path int endingSelectedLane = SelectLane(localRoadModel, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp, endingNumLanesLeft, endingNumLanesRight); // check if either is invalid or the difference does not line up with the change direction int deltaExpected = changeLeft ? 1 : -1; // starting - ending if (startingSelectedLane < 0 || endingSelectedLane < 0 || (startingSelectedLane - endingSelectedLane) != deltaExpected) { // this is some invalid stuff // we won't use either of them since we're not sure which one is truly valid startingLaneModel = GetLaneModel(localRoadModel, -1, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp); endingLaneModel = GetLaneModel(localRoadModel, -1, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp); // mark that we did reject didRejectLast = true; } else { // looks like the lane model selection was valid startingLaneModel = GetLaneModel(localRoadModel, startingSelectedLane, startingRndfPath, startingRndfPathWidth, rndfPathTimestamp); endingLaneModel = GetLaneModel(localRoadModel, endingSelectedLane, endingRndfPath, endingRndfPathWidth, rndfPathTimestamp); // mark that we did not reject didRejectLast = false; } // figure out what we want to send to the ui // for now, just send starting lane model SendLaneModelToUI(startingLaneModel, rndfPathTimestamp); }
public ILaneModel GetLaneModel(LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp, int numLanesLeft, int numLanesRight) { LocalRoadModel localRoadModel = this.localRoadModel; int selectedLane = SelectLane(localRoadModel, rndfPath, rndfPathWidth, rndfPathTimestamp, numLanesLeft, numLanesRight); // send the selected lane index (or rejection code) out Services.Dataset.ItemAs <double>("selected lane").Add(selectedLane, rndfPathTimestamp); // build the road model from the selected lane ILaneModel finalLaneModel = GetLaneModel(localRoadModel, selectedLane, rndfPath, rndfPathWidth, rndfPathTimestamp); // send to UI for display SendLaneModelToUI(finalLaneModel, rndfPathTimestamp); // return back to center return(finalLaneModel); }
private ILaneModel GetLaneModel(LocalRoadModel localRoadModel, int selectedLane, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp) { didRejectLast = false; switch (selectedLane) { case 0: return(GetLaneModel(localRoadModel.LeftLane, rndfPath, rndfPathWidth, rndfPathTimestamp)); case 1: return(GetLaneModel(localRoadModel.CenterLaneModel, rndfPath, rndfPathWidth, rndfPathTimestamp)); case 2: return(GetLaneModel(localRoadModel.RightLane, rndfPath, rndfPathWidth, rndfPathTimestamp)); } didRejectLast = true; return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth)); }
public static Object Deserialize(Stream stream, string channelName) { BinaryReader br = new BinaryReader(stream); SceneEstimatorMessageID msgtype = (SceneEstimatorMessageID)br.ReadInt32(); switch (msgtype) { case SceneEstimatorMessageID.SCENE_EST_Info: Console.WriteLine("SE Info:"); break; case SceneEstimatorMessageID.SCENE_EST_Bad: Console.WriteLine("SE BAD:"); break; case SceneEstimatorMessageID.SCENE_EST_ParticleRender: break; case SceneEstimatorMessageID.SCENE_EST_PositionEstimate: break; case SceneEstimatorMessageID.SCENE_EST_Stopline: break; case SceneEstimatorMessageID.SCENE_EST_TrackedClusters: { if (channelName != SceneEstimatorObstacleChannelNames.TrackedClusterChannelName && channelName != SceneEstimatorObstacleChannelNames.AnyClusterChannelName) { break; } int chunkNum = (int)br.ReadByte(); int numTotChunks = (int)br.ReadByte(); totPackets++; if (chunkNum == 0) //first packet... { if (expChunkNum != chunkNum) { badPackets++; } int lenToRead = (int)(br.BaseStream.Length - br.BaseStream.Position); lock (trackStorageLock) { trackedClusterStorage = new MemoryStream(65000); trackedClusterStorage.Write(br.ReadBytes(lenToRead), 0, lenToRead); } if (chunkNum == numTotChunks - 1) //we are done { expChunkNum = 0; //Console.WriteLine("Got Single Frame Packet OK " + " of " + numTotChunks); Object o = null; lock (trackStorageLock) { try { o = ProcessTrackedClusterMsg(new BinaryReader(trackedClusterStorage)); } catch (Exception ex) { Console.WriteLine("EXCEPTION: " + ex.Message); } goodPackets++; } if (o != null) { return(o); } } else { expChunkNum = 1; Console.WriteLine("Got Mulii Frame Packet " + expChunkNum + " of " + numTotChunks); } } else if (chunkNum == expChunkNum) //nth packet.... { lock (trackStorageLock) { int lenToRead = (int)(br.BaseStream.Length - br.BaseStream.Position); trackedClusterStorage.Write(br.ReadBytes(lenToRead), 0, lenToRead); } if (chunkNum == numTotChunks - 1) //we are done { expChunkNum = 0; Object o = null; lock (trackStorageLock) { Console.WriteLine("Got Mulii Frame Packet OK Len:" + trackedClusterStorage.Length + " Chunks " + numTotChunks); try { o = ProcessTrackedClusterMsg(new BinaryReader(trackedClusterStorage)); } catch (Exception ex) { Console.WriteLine("EXCEPTION: " + ex.Message); } trackedClusterStorage = new MemoryStream(65000); goodPackets++; } return(o); } else { expChunkNum++; Console.WriteLine("Got Mulii Frame Packet " + expChunkNum + " of " + numTotChunks); } } else //misaligned packet! yikes { lock (trackStorageLock) { trackedClusterStorage = new MemoryStream(); } expChunkNum = 0; badPackets++; Console.WriteLine("Bad Packet! ChunkNum: " + chunkNum + " ExpChunkNum: " + expChunkNum + " Total: " + numTotChunks); } if (totPackets % 100 == 0) { Console.WriteLine("PACKET STATS: GOOD: " + goodPackets + " BAD: " + badPackets); } break; } case SceneEstimatorMessageID.SCENE_EST_UntrackedClusters: { if (channelName != SceneEstimatorObstacleChannelNames.UntrackedClusterChannelName && channelName != SceneEstimatorObstacleChannelNames.AnyClusterChannelName) { break; } SceneEstimatorUntrackedClusterCollection ucc = new SceneEstimatorUntrackedClusterCollection(); int numClusters = br.ReadInt32(); ucc.clusters = new SceneEstimatorUntrackedCluster[numClusters]; ucc.timestamp = br.ReadDouble(); for (int i = 0; i < numClusters; i++) { int numPoints = br.ReadInt32(); ucc.clusters[i].clusterClass = (SceneEstimatorClusterClass)br.ReadInt32(); ucc.clusters[i].points = new Coordinates[numPoints]; for (int j = 0; j < numPoints; j++) { Int16 tmpx = br.ReadInt16(); Int16 tmpy = br.ReadInt16(); ucc.clusters[i].points[j].X = (double)tmpx / 100.0; ucc.clusters[i].points[j].Y = (double)tmpy / 100.0; } } if (br.BaseStream.Position != br.BaseStream.Length) { Console.WriteLine("Warning: Incomplete parse of received untracked cluster message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + "."); } return(ucc); } case (SceneEstimatorMessageID)LocalRoadModelMessageID.LOCAL_ROAD_MODEL: if ((channelName != LocalRoadModelChannelNames.LocalRoadModelChannelName) && (channelName != LocalRoadModelChannelNames.LMLocalRoadModelChannelName)) { break; } LocalRoadModel lrm = new LocalRoadModel(); lrm.timestamp = br.ReadDouble(); lrm.probabilityRoadModelValid = br.ReadSingle(); lrm.probabilityCenterLaneExists = br.ReadSingle(); lrm.probabilityLeftLaneExists = br.ReadSingle(); lrm.probabilityRightLaneExists = br.ReadSingle(); lrm.laneWidthCenter = br.ReadSingle(); lrm.laneWidthCenterVariance = br.ReadSingle(); lrm.laneWidthLeft = br.ReadSingle(); lrm.laneWidthLeftVariance = br.ReadSingle(); lrm.laneWidthRight = br.ReadSingle(); lrm.laneWidthRightVariance = br.ReadSingle(); int numCenterPoints = br.ReadInt32(); int numLeftPoints = br.ReadInt32(); int numRightPoints = br.ReadInt32(); lrm.LanePointsCenter = new Coordinates[numCenterPoints]; lrm.LanePointsCenterVariance = new float[numCenterPoints]; lrm.LanePointsLeft = new Coordinates[numLeftPoints]; lrm.LanePointsLeftVariance = new float[numLeftPoints]; lrm.LanePointsRight = new Coordinates[numRightPoints]; lrm.LanePointsRightVariance = new float[numRightPoints]; for (int i = 0; i < numCenterPoints; i++) { //fixed point conversion! Int16 x = br.ReadInt16(); Int16 y = br.ReadInt16(); UInt16 var = br.ReadUInt16(); lrm.LanePointsCenter[i] = new Coordinates((double)x / 100.0, (double)y / 100.0); lrm.LanePointsCenterVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f); } for (int i = 0; i < numLeftPoints; i++) { //fixed point conversion! Int16 x = br.ReadInt16(); Int16 y = br.ReadInt16(); UInt16 var = br.ReadUInt16(); lrm.LanePointsLeft[i] = new Coordinates((double)x / 100.0, (double)y / 100.0); lrm.LanePointsLeftVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f); } for (int i = 0; i < numRightPoints; i++) { //fixed point conversion! Int16 x = br.ReadInt16(); Int16 y = br.ReadInt16(); UInt16 var = br.ReadUInt16(); lrm.LanePointsRight[i] = new Coordinates((double)x / 100.0, (double)y / 100.0); lrm.LanePointsRightVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f); } int offset = (LocalRoadModel.MAX_LANE_POINTS * 3) - (numCenterPoints + numRightPoints + numLeftPoints); offset *= 6; //this is just the size of each "point" if ((br.BaseStream.Position + offset) != br.BaseStream.Length) { Console.WriteLine("Warning: Incomplete parse of received local road model message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + "."); } return(lrm); default: throw new InvalidDataException("Invalid Scene Estimator Message Received: " + msgtype); } return(null); }
public SceneEstimatorLRM_SERXEventArgs(LocalRoadModel lrmSE) { this.lrmSE = lrmSE; }
public SceneEstimatorLRM_LMRXEventArgs(LocalRoadModel lrmLM) { this.lrmLM = lrmLM; }
private int SelectLane(LocalRoadModel localRoadModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp, int numLanesLeft, int numLanesRight) { // check if we don't have a road model or it should be rejected bool nullRejection = localRoadModel == null; bool probRejection1 = false, probRejection2 = false, shapeRejection = false; if (localRoadModel != null) { probRejection1 = (didRejectLast && localRoadModel.ModelProbability < model_probability_accept_threshold); probRejection2 = (!didRejectLast && localRoadModel.ModelProbability < model_probability_reject_threshold); shapeRejection = !CheckGeneralShape(rndfPath, rndfPathTimestamp, localRoadModel.CenterLaneModel); } if (nullRejection || probRejection1 || probRejection2 || shapeRejection) { int rejectionCode = -4; if (nullRejection) { rejectionCode = -2; } else if (probRejection1 || probRejection2) { rejectionCode = -3; } else if (shapeRejection) { rejectionCode = -4; } return(rejectionCode); } // we decided stuff isn't completely stuff // we need to pick a lane that is the best fit // TODO: hysterysis factor--want to be in the "same lane" as last time // get the test results for each lane LaneModelTestResult leftTest = TestLane(rndfPath, rndfPathTimestamp, localRoadModel.LeftLane); LaneModelTestResult centerTest = TestLane(rndfPath, rndfPathTimestamp, localRoadModel.CenterLaneModel); LaneModelTestResult rightTest = TestLane(rndfPath, rndfPathTimestamp, localRoadModel.RightLane); // compute the lane existance probability values for left, center and right bool hasLeft = numLanesLeft > 0; bool hasRight = numLanesRight > 0; double probLeftExists = hasLeft ? 1 : extra_lane_probability; double probNoLeftExists = hasLeft ? 0 : (1 - extra_lane_probability); double probRightExists = hasRight ? 1 : extra_lane_probability; double probNoRightExists = hasRight ? 0 : (1 - extra_lane_probability); // calculate center lane probability double centerLaneExistanceProb = (probLeftExists * localRoadModel.LeftLane.Probability + probNoLeftExists * (1 - localRoadModel.LeftLane.Probability)) * (probRightExists * localRoadModel.RightLane.Probability + probNoRightExists * (1 - localRoadModel.RightLane.Probability)); // calculate left lane probability double leftLaneExistanceProb = 0.5 * (probRightExists * localRoadModel.CenterLaneModel.Probability + probNoRightExists * (1 - localRoadModel.CenterLaneModel.Probability)); // calculate right lane probability double rightLaneExistanceProb = 0.5 * (probLeftExists * localRoadModel.CenterLaneModel.Probability + probNoLeftExists * (1 - localRoadModel.CenterLaneModel.Probability)); // now figure out what to do!! // create rankings for each duder // 0 - left lane // 1 - center lane // 2 - right lane // existance vote order int[] existanceVotes = GetVoteCounts(false, existanceVoteStep, double.NaN, leftLaneExistanceProb, centerLaneExistanceProb, rightLaneExistanceProb); // deviation vote order int[] deviatationVotes = GetVoteCounts(true, deviationVoteStep, double.NaN, leftTest.rndf_deviation, centerTest.rndf_deviation, rightTest.rndf_deviation); // number agreeing vote order int[] numAgreeVotes = GetVoteCounts(false, matchVoteStep, 0, leftTest.forward_match_count, centerTest.forward_match_count, rightTest.forward_match_count); // number rejected vote order int[] numRejectedVotes = GetVoteCounts(true, rejectVoteStep, 0, leftTest.forward_rejection_count, centerTest.forward_rejection_count, rightTest.forward_rejection_count); // vote on the stuff int selectedLane = DoVoting(3, existanceVotes, deviatationVotes, numAgreeVotes, numRejectedVotes); return(selectedLane); }