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 LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30 * Math.PI / 180.0)
{
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50)
{
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0)
{
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold)
{
result.forward_match_count++;
}
else
{
result.forward_rejection_count++;
}
}
// return the result
return(result);
}
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel)
{
// construct the result object to hold stuff
LaneModelTestResult result = new LaneModelTestResult();
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// get the zero point of the lane model path
LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint;
// get the first waypoint on the RNDF path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the heading of the rndf path at its zero point and the heading of the lane model at
// the rndf's zero point
LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location);
Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector;
Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector;
double angle = Math.Acos(laneModelHeading.Dot(rndfHeading));
// check if the angle is within limits for comparing offset
if (angle < 30*Math.PI/180.0) {
// get the deviation between lane zero point and rndf zero point
result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location);
}
// now start check for how many waypoints are accepted
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) {
// check the distance along the rndf path
double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i));
// break out if we're too far along the rndf
if (rndfDistAlong > 50) {
break;
}
// get the waypoint
Coordinates waypoint = rndfPath[i];
// project on to lane path
LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint);
// check if we're too far along the lane path
double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint);
if (distAlong > lane_model_max_dist || distAlong < 0) {
break;
}
// check if the deviation
double dist = waypoint.DistanceTo(laneWaypoint.Location);
// increment appropriate counts
if (dist < lane_deviation_reject_threshold) {
result.forward_match_count++;
}
else {
result.forward_rejection_count++;
}
}
// return the result
return result;
}
