private List <Obstacle> FinalizeProcessing(List <Obstacle> trackedObstacles, List <Obstacle> untrackedObstacles, CarTimestamp timestamp)
{
List <Obstacle> finalObstacles = new List <Obstacle>(trackedObstacles.Count + untrackedObstacles.Count);
finalObstacles.AddRange(trackedObstacles);
finalObstacles.AddRange(untrackedObstacles);
Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles);
Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles);
int extraCount = 0;
if (leftObstacle != null)
{
extraCount++;
}
if (rightObstacle != null)
{
extraCount++;
}
List <int> ignoredObstacles = this.lastIgnoredObstacles;
OperationalObstacle[] uiObstacles = new OperationalObstacle[finalObstacles.Count + extraCount];
int i;
for (i = 0; i < finalObstacles.Count; i++)
{
Obstacle obs = finalObstacles[i];
obs.minSpacing = min_spacing[(int)obs.obstacleClass] + ExtraSpacing;
obs.desSpacing = des_spacing[(int)obs.obstacleClass] + ExtraSpacing;
try {
obs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(conv_polygon[(int)obs.obstacleClass], obs.AvoidancePolygon);
}
catch (Exception) {
OperationalTrace.WriteWarning("error computing minkowski convolution in finalize processing");
try {
obs.cspacePolygon = obs.AvoidancePolygon.Inflate(TahoeParams.T / 2.0 + min_spacing[(int)obs.obstacleClass]);
}
catch (Exception) {
obs.cspacePolygon = obs.AvoidancePolygon;
}
}
OperationalObstacle uiObs = new OperationalObstacle();
uiObs.age = obs.age;
uiObs.obstacleClass = obs.obstacleClass;
uiObs.poly = obs.AvoidancePolygon;
uiObs.headingValid = obs.absoluteHeadingValid;
uiObs.heading = obs.absoluteHeading;
if (ignoredObstacles != null)
{
uiObs.ignored = ignoredObstacles.Contains(obs.trackID);
}
uiObstacles[i] = uiObs;
}
if (leftObstacle != null)
{
OperationalObstacle uiObs = new OperationalObstacle();
uiObs.age = leftObstacle.age;
uiObs.obstacleClass = leftObstacle.obstacleClass;
uiObs.poly = leftObstacle.AvoidancePolygon;
uiObstacles[i++] = uiObs;
}
if (rightObstacle != null)
{
OperationalObstacle uiObs = new OperationalObstacle();
uiObs.age = rightObstacle.age;
uiObs.obstacleClass = rightObstacle.obstacleClass;
uiObs.poly = rightObstacle.AvoidancePolygon;
uiObstacles[i++] = uiObs;
}
Services.UIService.PushObstacles(uiObstacles, timestamp, "obstacles", true);
return(finalObstacles);
}
private List <Obstacle> ProcessTrackedClusters(SceneEstimatorTrackedClusterCollection clusters, Rect vehicleBox)
{
List <Obstacle> obstacles = new List <Obstacle>(clusters.clusters.Length);
// get the list of previous id's
SortedList <int, Obstacle> previousID;
if (processedObstacles != null)
{
previousID = new SortedList <int, Obstacle>(processedObstacles.obstacles.Count);
foreach (Obstacle obs in processedObstacles.obstacles)
{
if (obs != null && obs.trackID != -1 && !previousID.ContainsKey(obs.trackID))
{
previousID.Add(obs.trackID, obs);
}
}
}
else
{
previousID = new SortedList <int, Obstacle>();
}
List <Coordinates> goodPoints = new List <Coordinates>(1500);
Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
Polygon mergePolygon = mergeCircle.ToPolygon(24);
foreach (SceneEstimatorTrackedCluster cluster in clusters.clusters)
{
// ignore deleted targets
if (cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_DELETED || cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL || cluster.relativePoints == null || cluster.relativePoints.Length < 3)
{
continue;
}
Obstacle obs = new Obstacle();
obs.trackID = cluster.id;
obs.speed = cluster.speed;
obs.speedValid = cluster.speedValid;
obs.occuluded = cluster.statusFlag != SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;
// update the age
Obstacle prevTrack = null;
previousID.TryGetValue(cluster.id, out prevTrack);
goodPoints.Clear();
int numOccupancyDeleted = 0;
foreach (Coordinates pt in cluster.relativePoints)
{
if (!vehicleBox.IsInside(pt))
{
if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free)
{
occupancyDeletedCount++;
numOccupancyDeleted++;
}
else
{
goodPoints.Add(pt);
}
}
}
if (goodPoints.Count < 3)
{
continue;
}
IList <Polygon> polys;
if (obs.occuluded && numOccupancyDeleted > 0)
{
polys = WrapAndSplit(goodPoints, 1, 2.5);
}
else
{
polys = new Polygon[] { Polygon.GrahamScan(goodPoints) };
}
obs.absoluteHeadingValid = cluster.headingValid;
obs.absoluteHeading = cluster.absoluteHeading;
// set the obstacle polygon for calculate obstacle distance
Polygon obsPoly = Polygon.GrahamScan(goodPoints);
double targetDistance = GetObstacleDistance(obsPoly);
ObstacleClass impliedClass = ObstacleClass.DynamicUnknown;
switch (cluster.targetClass)
{
case SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE:
if (cluster.isStopped)
{
impliedClass = ObstacleClass.DynamicStopped;
}
else
{
impliedClass = ObstacleClass.DynamicCarlike;
}
break;
case SceneEstimatorTargetClass.TARGET_CLASS_NOTCARLIKE:
impliedClass = ObstacleClass.DynamicNotCarlike;
break;
case SceneEstimatorTargetClass.TARGET_CLASS_UNKNOWN:
impliedClass = ObstacleClass.DynamicUnknown;
break;
}
if (prevTrack == null)
{
obs.age = 1;
// we haven't seen this track before, determine what the implied class is
if (targetDistance < target_class_ignore_dist)
{
impliedClass = ObstacleClass.DynamicUnknown;
}
}
else
{
obs.age = prevTrack.age + 1;
// if we've seen this target before and we've labelled it as unknown and it is labelled as car-like now, check the distance
if (prevTrack.obstacleClass == ObstacleClass.DynamicUnknown && targetDistance < target_class_ignore_dist && obs.age < target_class_ignore_age)
{
impliedClass = ObstacleClass.DynamicUnknown;
}
}
// get the off-road percentage
double offRoadPercent = GetPercentOffRoad(obs.obstaclePolygon);
if (offRoadPercent > 0.65)
{
obs.offroadAge = obs.age;
}
// now check if we're labelling the obstacle as car-like if it has been off-road in the last second
if ((impliedClass == ObstacleClass.DynamicCarlike || impliedClass == ObstacleClass.DynamicStopped) && (obs.age - obs.offroadAge) > 10 && obs.offroadAge > 0)
{
// label as not car like
impliedClass = ObstacleClass.DynamicNotCarlike;
}
obs.obstacleClass = impliedClass;
foreach (Polygon poly in polys)
{
Obstacle newObs = obs.ShallowClone();
newObs.obstaclePolygon = poly;
// determine what to do with the cluster
if (cluster.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE && !cluster.isStopped)
{
// if the heading is valid, extrude the car polygon and predict forward
if (cluster.headingValid)
{
newObs.extrudedPolygon = ExtrudeCarPolygon(newObs.obstaclePolygon, cluster.relativeheading);
}
}
try {
newObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, newObs.AvoidancePolygon);
}
catch (Exception) {
}
obstacles.Add(newObs);
}
}
return(obstacles);
}
private List <Obstacle> ProcessUntrackedClusters(SceneEstimatorUntrackedClusterCollection clusters, List <Obstacle> trackedObstacles, Rect vehicleBox)
{
List <Obstacle> obstacles = new List <Obstacle>();
SortedList <Obstacle, List <Coordinates> > point_splits = new SortedList <Obstacle, List <Coordinates> >();
List <Coordinates> unclaimed_points = new List <Coordinates>(1500);
foreach (SceneEstimatorUntrackedCluster cluster in clusters.clusters)
{
// clear out stored variables
point_splits.Clear();
unclaimed_points.Clear();
// now determine if the point belongs to an old obstacle
ObstacleClass targetClass;
if (cluster.clusterClass == SceneEstimatorClusterClass.SCENE_EST_HighObstacle)
{
targetClass = ObstacleClass.StaticLarge;
}
else
{
targetClass = ObstacleClass.StaticSmall;
}
// only add points that are not within a tracked obstacle's extruded polygon
for (int i = 0; i < cluster.points.Length; i++)
{
Coordinates pt = cluster.points[i];
// iterate over all tracked cluster
bool did_hit = false;
if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free)
{
occupancyDeletedCount++;
did_hit = true;
}
else if (vehicleBox.IsInside(pt))
{
did_hit = true;
}
else if (trackedObstacles != null)
{
foreach (Obstacle trackedObs in trackedObstacles)
{
if (trackedObs.extrudedPolygon != null && trackedObs.extrudedPolygon.BoundingCircle.IsInside(pt) && trackedObs.extrudedPolygon.IsInside(pt))
{
did_hit = true;
break;
}
}
}
// if there was a hit, skip this point
if (!did_hit)
{
unclaimed_points.Add(pt);
}
//if (did_hit)
// continue;
//Obstacle oldObstacle = FindIntersectingCluster(pt, targetClass, previousObstacles);
//if (oldObstacle != null) {
// List<Coordinates> obstacle_points;
// if (!point_splits.TryGetValue(oldObstacle, out obstacle_points)) {
// obstacle_points = new List<Coordinates>(100);
// point_splits.Add(oldObstacle, obstacle_points);
// }
// obstacle_points.Add(pt);
//}
//else {
// unclaimed_points.Add(pt);
//}
}
// we've split up all the points appropriately
// now construct the obstacles
// we'll start with the obstacle belonging to an existing polygon
//foreach (KeyValuePair<Obstacle, List<Coordinates>> split in point_splits) {
// if (split.Value != null && split.Value.Count >= 3) {
// // the obstacle will inherit most of the properties of the old obstacle
// Obstacle obs = new Obstacle();
// obs.age = split.Key.age+1;
// obs.obstacleClass = split.Key.obstacleClass;
// // don't bother doing a split operation on these clusters -- they have already been split
// obs.obstaclePolygon = Polygon.GrahamScan(split.Value);
// obstacles.Add(obs);
// }
//}
// handle the unclaimed points
IList <Polygon> polygons = WrapAndSplit(unclaimed_points, split_area_threshold, split_length_threshold);
foreach (Polygon poly in polygons)
{
// create a new obstacle
Obstacle obs = new Obstacle();
obs.age = 1;
obs.obstacleClass = targetClass;
obs.obstaclePolygon = poly;
obstacles.Add(obs);
}
}
// test all old obstacles and see if they intersect any new obstacles
// project the previous static obstacles to the current time frame
if (processedObstacles != null)
{
try {
// get the relative transform
List <Obstacle> carryOvers = new List <Obstacle>();
Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
Polygon mergePolygon = mergeCircle.ToPolygon(24);
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, clusters.timestamp);
foreach (Obstacle prevObs in processedObstacles.obstacles)
{
if (prevObs.obstacleClass == ObstacleClass.StaticLarge || prevObs.obstacleClass == ObstacleClass.StaticSmall)
{
prevObs.obstaclePolygon = prevObs.obstaclePolygon.Transform(transform);
prevObs.age++;
if (prevObs.age < 20)
{
Coordinates centroid = prevObs.obstaclePolygon.GetCentroid();
double dist = GetObstacleDistance(prevObs.obstaclePolygon);
double angle = centroid.ArcTan;
if (dist < 30 && dist > 6 && Math.Abs(centroid.Y) < 15 && Math.Abs(angle) < Math.PI / 2.0)
{
try {
prevObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, prevObs.obstaclePolygon);
if (!TestIntersection(prevObs.mergePolygon, obstacles))
{
bool dropObstacle = false;
for (int i = 0; i < prevObs.obstaclePolygon.Count; i++)
{
Coordinates pt = prevObs.obstaclePolygon[i];
// iterate over all tracked cluster
if (vehicleBox.IsInside(pt))
{
dropObstacle = true;
}
else if (useOccupancyGrid && externalUseOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free)
{
dropObstacle = true;
}
else if (trackedObstacles != null)
{
foreach (Obstacle trackedObs in trackedObstacles)
{
if (trackedObs.obstacleClass == ObstacleClass.DynamicCarlike)
{
Polygon testPoly = trackedObs.extrudedPolygon ?? trackedObs.mergePolygon;
if (testPoly != null && testPoly.BoundingCircle.IsInside(pt) && testPoly.IsInside(pt))
{
dropObstacle = true;
break;
}
}
}
}
if (dropObstacle)
{
break;
}
}
if (!dropObstacle)
{
carryOvers.Add(prevObs);
}
}
}
catch (Exception) {
}
}
}
}
}
obstacles.AddRange(carryOvers);
}
catch (Exception) {
}
}
// create the merge polygon for all these duder
/*Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
* Polygon mergePolygon = mergeCircle.ToPolygon(24);
*
* foreach (Obstacle obs in obstacles) {
* obs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, obs.obstaclePolygon);
* }*/
return(obstacles);
}
public ObstacleCollection GetProcessedObstacles(CarTimestamp timestamp, SAUDILevel saudi)
{
List <Obstacle> transformedObstacles = new List <Obstacle>();
if (Services.StateProvider.GetVehicleState().speed < 4.5)
{
Obstacle leftObstacle = GetSideObstacle(currentLeftSideObstacles);
Obstacle rightObstacle = GetSideObstacle(currentRightSideObstacles);
if (leftObstacle != null)
{
transformedObstacles.Add(leftObstacle);
}
if (rightObstacle != null)
{
transformedObstacles.Add(rightObstacle);
}
}
ObstacleCollection processedObstacles = this.processedObstacles;
if (processedObstacles != null)
{
if (transformedObstacles.Capacity < transformedObstacles.Count + processedObstacles.obstacles.Count)
{
transformedObstacles.Capacity = transformedObstacles.Count + processedObstacles.obstacles.Count;
}
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, timestamp);
foreach (Obstacle obs in processedObstacles.obstacles)
{
if (!Settings.IgnoreTracks || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.StaticSmall)
{
Obstacle newObs = obs.ShallowClone();
if (saudi == SAUDILevel.L1)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall && Math.Abs(newObs.AvoidancePolygon.GetArea()) < L1_area_threshold)
{
continue;
}
}
else if (saudi == SAUDILevel.L2)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall)
{
continue;
}
}
else if (saudi == SAUDILevel.L3)
{
if (obs.obstacleClass == ObstacleClass.StaticSmall || obs.obstacleClass == ObstacleClass.StaticLarge || obs.obstacleClass == ObstacleClass.DynamicNotCarlike || obs.obstacleClass == ObstacleClass.DynamicUnknown)
{
continue;
}
}
if (newObs.cspacePolygon != null)
{
newObs.cspacePolygon = newObs.cspacePolygon.Transform(transform);
}
if (newObs.extrudedPolygon != null)
{
newObs.extrudedPolygon = newObs.extrudedPolygon.Transform(transform);
}
if (newObs.obstaclePolygon != null)
{
newObs.obstaclePolygon = newObs.obstaclePolygon.Transform(transform);
}
if (newObs.predictedPolygon != null)
{
newObs.predictedPolygon = newObs.predictedPolygon.Transform(transform);
}
transformedObstacles.Add(newObs);
}
}
}
return(new ObstacleCollection(timestamp, transformedObstacles));
}
