public static Rect Union(Rect l, Rect r)
{
double left = Math.Min(l.x, r.x);
double right = Math.Max(l.Right, r.Right);
double bottom = Math.Min(l.y, r.y);
double top = Math.Max(l.Top, r.Top);
return new Rect(left, bottom, right-left, top-bottom);
}
public bool Overlaps(Rect r)
{
// pre-calculate left, bottom, right, top of this and right rect
double tl = x, tb = y, tr = x+width, tt = y+height;
double rl = r.x, rb = r.y, rr = r.x+r.width, rt = r.y+r.height;
if (tl <= rl && tb <= rb && tr >= rr && tt >= rt) {
// r is fully contained in this rectangle
return true;
}
else if (tl >= rl && tb >= rb && tr <= rr && tt <= rt) {
// this rectangle is fully contained in r
return true;
}
else if (((rl >= tl && rl <= tr) || (rr >= tl && rr <= tr)) && ((rb >= tb && rb <= tt) || (rt >= tb && rt <= tt))) {
return true;
}
return false;
}
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;
}
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 bool DetermineReverseRecommended(IList<Obstacle> obstacles)
{
if (obstacles == null)
return false;
// determine if there are obstacles in front of us
Rect frontRect = new Rect(0, -(TahoeParams.T + 1)/2, TahoeParams.VL + 4, TahoeParams.T + 1);
foreach (Obstacle obs in obstacles) {
// iterate through each point
foreach (Coordinates pt in obs.AvoidancePolygon) {
if (frontRect.IsInside(pt)) {
return true;
}
}
}
if (IsOffRoad())
return true;
return false;
}
