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);
}
