public void SetCurrentValue(SceneEstimatorUntrackedClusterCollection value, string label)
{
if (value == null || value.clusters == null)
{
clusterPoints = null;
return;
}
PointF[][] newPoints = new PointF[value.clusters.Length][];
boundingRects = new RectangleF[value.clusters.Length];
for (int i = 0; i < value.clusters.Length; i++)
{
SceneEstimatorUntrackedCluster cluster = value.clusters[i];
if (cluster.points == null || cluster.points.Length == 0)
{
newPoints[i] = new PointF[0];
boundingRects[i] = RectangleF.Empty;
}
float minX = float.MaxValue, minY = float.MaxValue;
float maxX = float.MinValue, maxY = float.MinValue;
newPoints[i] = new PointF[cluster.points.Length];
for (int j = 0; j < cluster.points.Length; j++)
{
PointF pt = Utility.ToPointF(cluster.points[j]);
newPoints[i][j] = pt;
if (pt.X < minX)
{
minX = pt.X;
}
if (pt.X > maxX)
{
maxX = pt.X;
}
if (pt.Y < minY)
{
minY = pt.Y;
}
if (pt.Y > maxY)
{
maxY = pt.Y;
}
}
// prevent the rectangle from being "Empty" because it has zero length/width
if (maxX == minX)
{
maxX = minX + box_size;
}
if (maxY == minY)
{
maxY = minY + box_size;
}
boundingRects[i] = new RectangleF(minX, minY, maxX - minX, maxY - minY);
}
clusterPoints = newPoints;
}
public void GetHits(IList <Polygon> obstacles, Coordinates vehicleLoc, double vehicleHeading, SceneEstimatorUntrackedClusterCollection clusters)
{
// for now, only support the Scan sensor type
if (sensorType != SensorType.Scan)
{
throw new NotSupportedException("Only Scan sensor type is supported at the moment");
}
Matrix3 transform = Matrix3.Rotation(-vehicleHeading) * Matrix3.Translation(-vehicleLoc.X, -vehicleLoc.Y);
// for each obstacle, get the bounding circle, see if it's in range
List <Polygon> possibleTargets = new List <Polygon>(obstacles.Count);
for (int i = 0; i < obstacles.Count; i++)
{
Circle boundingCircle = obstacles[i].BoundingCircle;
double minDist = boundingCircle.center.DistanceTo(vehicleLoc) - boundingCircle.r;
if (minDist < range)
{
possibleTargets.Add(obstacles[i].Transform(transform));
}
}
// now start doing the actual rays
int numRays = (int)Math.Round((leftAngularExtent - rightAngularExtent) / beamResolution);
double beamStep = (leftAngularExtent - rightAngularExtent) / numRays;
numRays++;
HitData[] rays = new HitData[numRays];
for (int rayNum = 0; rayNum < numRays; rayNum++)
{
double angle = rightAngularExtent + rayNum * beamStep;
Coordinates unitVec = Coordinates.FromAngle(angle);
LineSegment l = new LineSegment(Coordinates.Zero, unitVec * range);
rays[rayNum].line = l;
rays[rayNum].distance = double.MaxValue;
rays[rayNum].obstacleIndex = -1;
rays[rayNum].pt = Coordinates.NaN;
rays[rayNum].unitVec = unitVec;
}
// go through and intersect with each polygon
for (int rayInd = 0; rayInd < numRays; rayInd++)
{
HitData hd = rays[rayInd];
for (int i = 0; i < possibleTargets.Count; i++)
{
Coordinates[] pts;
double[] K;
if (possibleTargets[i].Intersect(hd.line, out pts, out K))
{
// find the min distance
double minK = K[0];
int minKind = 0;
for (int j = 1; j < K.Length; j++)
{
if (K[j] < minK)
{
minK = K[j];
minKind = j;
}
}
// get the minimum point
double dist = minK * range;
if (dist < hd.distance)
{
hd.distance = dist;
hd.obstacleIndex = i;
hd.pt = pts[minKind];
}
}
}
rays[rayInd] = hd;
}
Random rand = new Random();
// we have all the hits, form into clusters
int indPrev = -1;
List <Coordinates> clusterPoints = new List <Coordinates>();
List <SceneEstimatorUntrackedCluster> newClusters = new List <SceneEstimatorUntrackedCluster>();
for (int rayNum = 0; rayNum < numRays; rayNum++)
{
HitData hd = rays[rayNum];
if (hd.obstacleIndex != indPrev && clusterPoints.Count > 0)
{
// test if any points are within 10 meters
bool noneTooClose = true;
if (noneTooClose)
{
SceneEstimatorUntrackedCluster cluster = new SceneEstimatorUntrackedCluster();
cluster.points = clusterPoints.ToArray();
newClusters.Add(cluster);
clusterPoints.Clear();
}
}
indPrev = hd.obstacleIndex;
if (hd.obstacleIndex == -1)
{
continue;
}
// add noise to the point
double noisyRange = hd.distance + (2 * rand.NextDouble() - 1) * .25;
Coordinates pt = hd.unitVec * noisyRange;
clusterPoints.Add(pt);
}
if (indPrev != -1 && clusterPoints.Count > 0)
{
SceneEstimatorUntrackedCluster cluster = new SceneEstimatorUntrackedCluster();
cluster.points = clusterPoints.ToArray();
newClusters.Add(cluster);
}
SceneEstimatorUntrackedCluster[] allClusters;
if (clusters.clusters != null && clusters.clusters.Length > 0)
{
allClusters = new SceneEstimatorUntrackedCluster[clusters.clusters.Length + newClusters.Count];
Array.Copy(clusters.clusters, allClusters, clusters.clusters.Length);
newClusters.CopyTo(allClusters, clusters.clusters.Length);
}
else
{
allClusters = newClusters.ToArray();
}
clusters.clusters = allClusters;
}