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