public static List <List <PointDExtended> > FindAllValidCrossingPoints(List <List <SegmentExtended> > list_of_family)
{
List <List <PointDExtended> > list_of_crossing_points = new List <List <PointDExtended> >();
foreach (List <SegmentExtended> family in list_of_family)
{
List <int> list_of_case = Enumerable.Range(0, family.Count).ToList(); /// [0,1,2,3,...,n]
List <List <int> > list_of_combinations_of_the_family = Toolbox.GetKCombs(list_of_case, 2).ToList().Select(x => x.ToList()).ToList(); /// [[0,1],[0,2],[0,3],[1,2],[1,3],[2,3],...]
List <List <SegmentExtended> > list_of_parallel_combination = list_of_combinations_of_the_family.Select(
x => LineDetection.testIfSegmentArePerpendicular(family[x[0]], family[x[1]]) ? new List <SegmentExtended>()
{
family[x[0]], family[x[1]]
} : null
).ToList();
list_of_parallel_combination.RemoveAll(item => item == null);
list_of_crossing_points.Add(list_of_parallel_combination.Select(x => Toolbox.GetCrossingPointBetweenSegment(x[0], x[1])).ToList());
}
list_of_crossing_points = list_of_crossing_points.Distinct().ToList();
return(list_of_crossing_points);
}
public void ProcessLidarData(List <PolarPointRssi> polarPointRssi)
{
List <SegmentExtended> Lines = new List <SegmentExtended>();
List <PolarPointRssi> validPoint = polarPointRssi.Where(x => x.Distance <= Math.Sqrt(Math.Pow(3, 2) + Math.Pow(2, 2))).ToList();
List <PointD> validPointXY = validPoint.Select(x => Toolbox.ConvertPolarToPointD(x)).ToList();
List <PointDExtended> absolutePoints = new List <PointDExtended>();
validPointXY = ClustersDetection.ExtractClusterByDBScan(validPointXY, 0.05, 3).SelectMany(x => x.points).ToList().Select(x => Toolbox.ConvertPolarToPointD(x)).ToList().Select(x => x.Pt).ToList();
RectangleOriented best_rectangle = FindRectangle.FindMbrBoxByOverlap(validPointXY);
List <PointD> border_points = FindRectangle.FindAllBorderPoints(validPointXY, best_rectangle, 0.05);
List <PolarPointRssi> border_point_polar = border_points.Select(x => Toolbox.ConvertPointDToPolar(x)).ToList();
List <ClusterObjects> inside_clusters = ClustersDetection.ExtractClusterByDBScan(validPointXY.Where(x => border_points.IndexOf(x) == -1).ToList(), 0.045, 3);
List <PolarPointRssiExtended> processedPoints = ClustersDetection.SetColorsOfClustersObjects(inside_clusters);
List <ClusterObjects> border_clusters = ClustersDetection.ExtractClusterByDBScan(border_points, 0.05, 10);
List <SegmentExtended> iepfs_lines = new List <SegmentExtended>();
foreach (ClusterObjects c in border_clusters)
{
var iepf_border_points = LineDetection.IEPF_Algorithm(c.points.Select(x => Toolbox.ConvertPolarToPointD(x).Pt).ToList(), 0.05);
for (int i = 1; i < iepf_border_points.Count; i++)
{
iepfs_lines.Add(new SegmentExtended(iepf_border_points[i - 1], iepf_border_points[i], Color.Black, 2));
}
}
iepfs_lines = LineDetection.MergeSegmentWithLSM(iepfs_lines, 0.5, 3 * Math.PI / 180);
Lines.AddRange(iepfs_lines);
List <List <SegmentExtended> > list_of_family = LineDetection.FindFamilyOfSegment(iepfs_lines);
List <PointD> corners_points = CornerDetection.FindAllValidCrossingPoints(list_of_family).SelectMany(x => x).ToList().Select(x => x.Pt).ToList();
Tuple <PointD, PointD, PointD, PointD> corners = Toolbox.GetCornerOfAnOrientedRectangle(best_rectangle);
double thresold = 0.09;
double width = Math.Max(best_rectangle.Lenght, best_rectangle.Width);
double height = Math.Min(best_rectangle.Lenght, best_rectangle.Width);
List <SegmentExtended> rectangle_segments = FindRectangle.DrawRectangle(best_rectangle, Color.Green, 1);
//if (width >= 3 - thresold && height >= 2 - thresold)
//{
// Console.ForegroundColor = ConsoleColor.Green;
//}
//else if (width >= 3 - thresold)
//{
// Console.ForegroundColor = ConsoleColor.Blue;
//}
//else if (width > 2 + thresold)
//{
// Console.ForegroundColor = ConsoleColor.Yellow;
// if (height > 2 - thresold)
// Console.ForegroundColor = ConsoleColor.DarkYellow;
//}
//else if (width >= 2 - thresold && width <= 2 + thresold)
// Console.ForegroundColor = ConsoleColor.Red;
//else
// Console.ResetColor();
//Console.WriteLine(width + " " + height);
processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(best_rectangle.Center), 10, Color.Black));
Lines.AddRange(rectangle_segments);
//Console.WriteLine("Corners: " + FindRectangle.GetNumberOfVisibleCorners(best_rectangle));
RectangleOriented resized_rectangle = FindRectangle.ResizeRectangle(best_rectangle, thresold);
List <Location> list_of_possible_locations;
if (resized_rectangle != null)
{
Console.ForegroundColor = ConsoleColor.Green;
Lines.AddRange(FindRectangle.DrawRectangle(resized_rectangle, Color.LightGreen, 8));
processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(resized_rectangle.Center), 10, Color.Green));
list_of_possible_locations = FindRectangle.ListAllPossibleLocation(resized_rectangle);
//processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList());
}
else
{
Tuple <RectangleOriented, RectangleOriented, RectangleOriented> list_of_possible_rectangles = FindRectangle.ListResisableRectangle(best_rectangle, thresold);
Console.ForegroundColor = ConsoleColor.Yellow;
list_of_possible_locations = FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2);
//Console.ForegroundColor = ConsoleColor.Blue;
//list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item2));
//Console.ForegroundColor = ConsoleColor.Red;
//list_of_possible_locations.AddRange(FindRectangle.ListAllPossibleLocation(list_of_possible_rectangles.Item3));
//Console.ForegroundColor = ConsoleColor.Red;
//Console.WriteLine("1 ------");
//processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red : Color.DarkRed)).ToList());
//Console.ForegroundColor = ConsoleColor.Yellow;
//Console.WriteLine("2 ------");
Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item1, Color.Yellow, 4));
//processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item1.Center), 10, Color.Yellow));
Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item2, Color.Blue, 4));
//processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item2.Center), 10, Color.Blue));
Lines.AddRange(FindRectangle.DrawRectangle(list_of_possible_rectangles.Item3, Color.Red, 4));
//processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(list_of_possible_rectangles.Item3.Center), 10, Color.LightGoldenrodYellow));
}
//processedPoints.AddRange(list_of_possible_locations.Select(x => new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(x.X, x.Y)), 10, (x.Theta != 1) ? Color.Red: Color.DarkBlue)).ToList());
absolutePoints = list_of_possible_locations.Select(x => new PointDExtended(new PointD(x.X, x.Y), x.Theta > 0 ? Color.Red : Color.Blue, 10)).ToList();
Location best_location = FindRectangle.GetBestLocation(list_of_possible_locations, robotLocation);
//processedPoints.Add(new PolarPointRssiExtended(Toolbox.ConvertPointDToPolar(new PointD(best_location.X, best_location.Y)), 10, Color.DarkGreen));
OnLidarSetupRobotLocationEvent?.Invoke(this, best_location);
Console.ResetColor();
List <Cup> list_of_cups = new List <Cup>();
List <LidarObjects> list_of_objects = new List <LidarObjects>();
foreach (ClusterObjects c in inside_clusters)
{
RectangleOriented cluster_rectangle = FindRectangle.FindMbrBoxByArea(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList());
cluster_rectangle.Width += 0.1;
cluster_rectangle.Lenght += 0.1;
//Lines.AddRange(FindRectangle.DrawRectangle(cluster_rectangle, c.points[0].Color, 3));
Color color = Toolbox.ColorFromHSL((list_of_objects.Count * 0.20) % 1, 1, 0.5);
list_of_objects.Add(new LidarObjects(c.points.Select(x => Toolbox.ConvertPolarToPointD(x.Pt)).ToList(), color));
Cup cup = DetectCup(c);
// The null condition is Bad need to edit
if (cup != null)
{
list_of_cups.Add(cup);
}
}
OnProcessLidarObjectsDataEvent?.Invoke(this, list_of_objects);
// Lines.Add(DetectGlobalLine(polarPointRssi, 1d, 0d, 5d, 3, 0.2d));
OnProcessLidarLineDataEvent?.Invoke(this, Lines);
OnProcessLidarCupDataEvent?.Invoke(this, list_of_cups);
RawLidarArgs processLidar = new RawLidarArgs()
{
RobotId = robotId, LidarFrameNumber = LidarFrame, PtList = processedPoints.Select(x => x.Pt).ToList()
};
OnProcessLidarDataEvent?.Invoke(this, processLidar);
OnProcessLidarPolarDataEvent?.Invoke(this, processedPoints);
OnProcessLidarAbsoluteDataEvent?.Invoke(this, absolutePoints);
//OnProcessLidarXYDataEvent?.Invoke(this, processedPoints.Select(x => new PointDExtended(Toolbox.ConvertPolarToPointD(x), Color.Blue, 2)).ToList());
}
