/// <summary>
/// Scan segments
/// </summary>
/// <param name="realPose">Real pose (to use for scanning)</param>
/// <param name="estimatedPose">Estimated pose (to use to store in segments)</param>
/// <param name="segments"></param>
private void ScanSegments(Vector3 realPose, Vector3 estimatedPose, out List <ScanSegment> segments) //J'ai l'impression que c'est là qu'il simule le LIDAR
{
Random rnd = new Random();
float scanAngle = (MathF.PI * 2) / numScanPoints;
ScanSegment scanSegment = new ScanSegment()
{
Pose = estimatedPose,
IsLast = true
};
for (float angle = 0.0f; angle < MathF.PI * 2; angle += scanAngle) //ici c'est la boucle pour le range du LIDAR
{
float lidarAngle = angle + realPose.Z;
if (field.RayTrace(realPose.ToVector2(), lidarAngle, maxScanDist, out float hit))
{
hit += ((float)rnd.Next(-100, 100) / 100.0f) * measureError;
scanSegment.Rays.Add(new Ray(angle, hit));
}
}
segments = new List <ScanSegment>()
{
scanSegment
};
}
/// <summary>
/// Scan segments
/// </summary>
/// <param name="realPose">Real pose (to use for scanning)</param>
/// <param name="estimatedPose">Estimated pose (to use to store in segments)</param>
/// <param name="segments"></param>
private void ScanSegments(Vector3 realPose, Vector3 estimatedPose, out List <ScanSegment> segments)
{
Random rnd = new Random();
float scanAngle = (MathF.PI * 2) / numScanPoints;
ScanSegment scanSegment = new ScanSegment()
{
Pose = estimatedPose,
IsLast = true
};
for (float angle = 0.0f; angle < MathF.PI * 2; angle += scanAngle)
{
float lidarAngle = angle + realPose.Z;
if (field.RayTrace(realPose.ToVector2(), lidarAngle, maxScanDist, out float hit))
{
hit += ((float)rnd.Next(-100, 100) / 100.0f) * measureError;
scanSegment.Rays.Add(new Ray(angle, hit));
}
}
segments = new List <ScanSegment>()
{
scanSegment
};
}
/// <summary>
/// "Draw" scan segment.
/// </summary>
/// <param name="segment">Scan segment</param>
private void DrawScan(ScanSegment segment)
{
foreach (Ray ray in segment.Rays)
{
Vector2 endPos = new Vector2(
ray.Radius * MathF.Cos(ray.Angle),
ray.Radius * MathF.Sin(ray.Angle));
Line line = new Line()
{
X1 = segment.Pose.X,
Y1 = segment.Pose.Y,
X2 = segment.Pose.X + endPos.X,
Y2 = segment.Pose.Y + endPos.Y,
Stroke = Brushes.Red,
StrokeThickness = 0.05
};
DrawArea.Children.Add(line);
}
}
/// <summary>
/// "Draw" scan segment.
/// </summary>
/// <param name="segment">Scan segment</param>
private void DrawScan(ScanSegment segment) //fonction pas apelée, je me demande si on peut pas l'enlever
// car pas appelée
{
foreach (Ray ray in segment.Rays)
{
Vector2 endPos = new Vector2(
ray.Radius * MathF.Cos(ray.Angle),
ray.Radius * MathF.Sin(ray.Angle));
Line line = new Line()
{
X1 = segment.Pose.X,
Y1 = segment.Pose.Y,
X2 = segment.Pose.X + endPos.X,
Y2 = segment.Pose.Y + endPos.Y,
Stroke = Brushes.Red,
StrokeThickness = 0.05
};
DrawArea.Children.Add(line);
}
}
