public LocalRoadModel(CarTimestamp timestamp, double modelProbability, LocalLaneModel centerLane, LocalLaneModel leftLane, LocalLaneModel rightLane) { this.timestamp = timestamp; this.modelProbability = modelProbability; this.centerLane = centerLane; this.leftLane = leftLane; this.rightLane = rightLane; }
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 ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp) { // check the lane model probability if (laneModel.Probability < lane_probability_reject_threshold) { // we're rejecting this, just return a path lane model return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth); } // 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); // iterate through the waypoints in the RNDF path and project onto the lane model // the first one that is over the threshold, we consider the waypoint before as a potential ending point LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath(); // flag indicating if any of the waypoint tests failed because of the devation was too high bool anyDeviationTooHigh = false; // number of waypoints accepted int numWaypointsAccepted = 0; // get the vehicle's position on the rndf path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; // get the vehicle's position on the lane model LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint; // get the last point we want to consider on the lane model LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist); // start walking forward through the waypoints on the rndf path // this loop will implicitly exit when we're past the end of the lane model as the waypoints // will stop being close to the lane model (GetClosestPoint returns the end point if we're past the // end of the path) for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) { // get the waypoint Coordinates rndfWaypoint = rndfPath[i]; // get the closest point on the lane model LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint); // compute the distance between the two double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location); // if this is above the deviation threshold, leave the loop if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) { // if we're at the end of the lane model path, we don't want to consider this a rejection if (laneModelClosestPoint < laneModelFarthestPoint) { // mark that at least on deviation was too high anyDeviationTooHigh = true; } break; } // increment the number of waypoint accepted numWaypointsAccepted++; // update the end point of where we're valid as the local road model was OK up to this point laneModelDeviationEndPoint = laneModelClosestPoint; } // go through and figure out how far out the variance is within tolerance LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath(); // walk forward from this point until the end of the lane mode path for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) { // check if we're within the variance toleration if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) { // we are, update the point on path laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i); } else { // we are out of tolerance, break out of the loop break; } } // now figure out everything out // determine waypoint rejection status WaypointRejectionResult waypointRejectionResult; if (laneModelDeviationEndPoint.Valid) { // if the point is valid, that we had at least one waypoint that was ok // check if any waypoints were rejected if (anyDeviationTooHigh) { // some waypoint was ok, so we know that at least one waypoint was accepted waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted; } else { // no waypoint triggered a rejection, but at least one was good waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted; } } else { // the point is not valid, so we either had no waypoints or we had all rejections if (anyDeviationTooHigh) { // the first waypoint was rejected, so all are rejected waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected; } else { // the first waypoint (if any) was past the end of the lane model waypointRejectionResult = WaypointRejectionResult.NoWaypoints; } } // criteria for determining if this path is valid: // - if some or all waypoints were accepted, than this is probably a good path // - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted // - if there were no waypoints, this is a potentially dangerous situation since we can't reject // or confirm the local road model. for now, we'll assume that it is correct but this may need to change // if we start handling intersections in this framework // - if all waypoints were rejected, than we don't use the local road model // - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of // the lane model was in tolerance // now build out the lane model ILaneModel finalLaneModel; // check if we rejected all waypoints or no lane model points satisified the variance threshold if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) { // want to just use the path lane model finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth); } else { // we'll use the lane model // need to build up the center line as well as left and right bounds // to build up the center line, use the lane model as far as we feel comfortable (limited by either variance // or by rejections) and then use the rndf lane after that. LinePath centerLine = new LinePath(); // figure out the max distance // if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) { laneModelDeviationEndPoint = laneModelFarthestPoint; } // figure out the closer of the end points LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint; bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint; // add the lane model to the center line centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint)); // create a list to hold the width expansion values List<double> widthValue = new List<double>(); // make the width expansion values the width of the path plus the 1-sigma values for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) { widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i])); } // now figure out how to add the rndf path // get the projection of the lane model end point on the rndf path LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location); // if the closest point is past the end of rndf path, then we don't want to tack anything on if (rndfPathStartPoint != rndfPath.EndPoint) { // get the last segment of the new center line Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector; // get the last point of the new center line Coordinates laneModelEndLoc = laneModelEndPoint.Location; // now figure out the distance to the next waypoint LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath(); // figure out if we're ending at a waypoint or not if (endAtWaypoint) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1); // if the distance from the start point to the next point is less than rndf_dist_min, then // use the waypont after double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint); if (dist < rndf_dist_min) { if (rndfPathStartPoint.Index < rndfPath.Count-2) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2); } else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } else { rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2); } } } else { // track the last angle we had double lastAngle = double.NaN; // walk down the rndf path until we find a valid point for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) { // advance from the start point by dist double distTemp = dist; rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp); // if the distTemp is > 0, then we're past the end of the path if (distTemp > 0) { // if we're immediately past the end, we don't want to tack anything on if (dist == rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } break; } // check the angle made by the last segment of center line and the segment // formed between the end point of the center line and this new point double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize())); // check if the angle satisfies the threshold or we're increasing the angle if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) { // break out of the loop, we're done searching break; } lastAngle = angle; } } // tack on the rndf starting at next point going to the end if (rndfNextPoint.Valid) { LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint); centerLine.AddRange(subPath); // insert the lane model end point into the sub path subPath.Insert(0, laneModelEndLoc); // get the angles List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1); // add the width of the path inflated by the angles for (int i = 0; i < angles.Count; i++) { // calculate the width expansion factor // 90 deg, 3x width // 45 deg, 1.5x width // 0 deg, 1x width double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1; // add the width value widthValue.Add(widthFactor*laneModel.Width/2); } // add the final width widthValue.Add(laneModel.Width/2); } // set the rndf path start point to be the point we used rndfPathStartPoint = rndfNextPoint; } // for now, calculate the left and right bounds the same way we do for the path lane model // TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray()); // get the next shifts for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; } LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray()); // build the final lane model finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp); } SendLaneModelToUI(finalLaneModel, rndfPathTimestamp); // output the fit result return finalLaneModel; }
private bool CheckGeneralShape(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel centerLaneModel) { // bad newz bears if (centerLaneModel.LanePath == null || centerLaneModel.LanePath.Count < 2) { return false; } // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = centerLaneModel.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; Coordinates firstWaypoint = rndfPath[rndfZeroPoint.Index+1]; // get the projection of the first waypoint onto the lane path LinePath.PointOnPath laneFirstWaypoint = laneModelPath.GetClosestPoint(firstWaypoint); // get the offset vector Coordinates offsetVector = laneFirstWaypoint.Location - firstWaypoint; // start iterating through the waypoints forward and project them onto the rndf path for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) { // get the waypoint Coordinates waypoint = rndfPath[i]; // adjust by the first waypoint offset waypoint += offsetVector; // project onto the lane model LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint); // check the distance from the zero point on the lane model if (laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint) > road_model_max_dist) { break; } // check the devation from the rndf double deviation = waypoint.DistanceTo(laneWaypoint.Location); // if the deviation is over some threshold, then we reject the model if (deviation > road_deviation_reject_threshold) { return false; } } // we got this far, so this stuff is OK return true; }