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