public ApproximatedLine(Line line, LineDetectionSettings settings)
{
Settings = settings;
lines = new List <Line> {
line
};
}
public static void Execute(Transform meshParent, LineDetectionSettings settings)
{
var roadRenderers = GetRoadRenderers();
var correctionNeeded = settings.lineSource == LineDetectionSettings.LineSource.CorrectedHdMap || settings.generateLineSensorData;
var corrector = correctionNeeded ? new MapLineAlignmentCorrector(settings) : null;
try
{
for (var i = 0; i < roadRenderers.Count; ++i)
{
var road = roadRenderers[i];
var progress = (float)i / roadRenderers.Count;
EditorUtility.DisplayProgressBar("Detecting lines", $"Processing road {i + 1} of {roadRenderers.Count}", progress);
var mf = road.GetComponent <MeshFilter>();
var tex = road.sharedMaterial.GetTexture(AlbedoTexProp);
var imagePath = Path.Combine(Application.dataPath, "..", AssetDatabase.GetAssetPath(tex));
var uvToWorldScale = LineUtils.EstimateUvToWorldScale(mf, tex);
var lines = DetectLines(imagePath, uvToWorldScale);
var approximatedLines = ProcessUvLines(lines, settings);
var worldSpaceSegments = UvToWorldMapper.CalculateWorldSpaceNew(approximatedLines, mf, uvToWorldScale);
ProcessWorldLines(worldSpaceSegments, settings, out var worldSpaceSnappedSegments);
if (correctionNeeded)
{
corrector.AddSegments(worldSpaceSegments);
}
if (settings.lineSource == LineDetectionSettings.LineSource.IntensityMap)
{
BuildWorldObject(worldSpaceSegments, road.transform, meshParent);
}
}
if (correctionNeeded)
{
var debugCorrectedSegments = corrector.Process();
var lineDataOverride = meshParent.gameObject.AddComponent <LaneLineOverride>();
lineDataOverride.SetData(debugCorrectedSegments);
}
}
finally
{
EditorUtility.ClearProgressBar();
}
}
private static void ProcessWorldLines(List <SegmentedLine3D> segments, LineDetectionSettings settings, out List <SegmentedLine3D> connectedSegments)
{
var changed = true;
var safety = 0;
var dist = settings.worldSpaceSnapDistance;
var angle = settings.worldSpaceSnapAngle;
while (changed)
{
if (safety++ > 20000)
{
throw new Exception("Broken infinite loop");
}
changed = false;
for (var i = 0; i < segments.Count - 1; ++i)
{
for (var j = i + 1; j < segments.Count; ++j)
{
if (segments[i].TryMerge(segments[j], dist, angle))
{
segments.Remove(segments[j]);
changed = true;
break;
}
}
if (changed)
{
break;
}
}
}
foreach (var segment in segments)
{
var color = Random.ColorHSV(0, 1, 0.9f, 1, 0.7f, 1.0f);
segment.color = color;
segment.SnapSegments();
}
connectedSegments = ConnectSegments(segments, settings);
}
public MapLineAlignmentCorrector(LineDetectionSettings settings)
{
this.settings = settings;
}
private static List <SegmentedLine3D> ConnectSegments(List <SegmentedLine3D> originalSegments, LineDetectionSettings settings)
{
var result = originalSegments.Select(x => x.Clone()).ToList();
var distanceThreshold = settings.worldDottedLineDistanceThreshold;
var angleThreshold = settings.worldSpaceSnapAngle;
for (var i = 0; i < result.Count; ++i)
{
var bestMatchIndex = -1;
var bestMatchDist = float.MaxValue;
var bestMatchUseStartA = false;
var bestMatchUseStartB = false;
for (var j = i + 1; j < result.Count; ++j)
{
FindClosestEnds(result[i], result[j], out var useStartA, out var useStartB);
var iPos = useStartA ? result[i].Start : result[i].End;
var jPos = useStartB ? result[j].Start : result[j].End;
var iVec = useStartA ? -result[i].StartVectorXZ : result[i].EndVectorXZ;
var jVec = useStartB ? result[j].StartVectorXZ : -result[j].EndVectorXZ;
var ijDist = Vector3.Distance(iPos, jPos);
if (Mathf.Abs(iPos.y - jPos.y) > 2f)
{
continue;
}
if (ijDist > distanceThreshold || Angle(iVec, jVec) > angleThreshold)
{
continue;
}
var joinVec = iPos - jPos;
var joinVecXZ = new Vector2(joinVec.x, joinVec.z);
if (Angle(joinVecXZ, jVec) > 0.5f * angleThreshold || Angle(joinVecXZ, iVec) > 0.5f * angleThreshold)
{
continue;
}
if (ijDist < bestMatchDist)
{
bestMatchDist = ijDist;
bestMatchIndex = j;
bestMatchUseStartA = useStartA;
bestMatchUseStartB = useStartB;
}
}
if (bestMatchIndex == -1)
{
continue;
}
if (bestMatchUseStartA)
{
result[i].Invert();
}
if (!bestMatchUseStartB)
{
result[bestMatchIndex].Invert();
}
result[i].lines.Add(new Line3D(result[i].End, result[bestMatchIndex].Start));
result[i].lines.AddRange(result[bestMatchIndex].lines);
result.RemoveAt(bestMatchIndex);
i = -1;
}
return(result);
}
public ApproximatedLine(List <Line> lines, LineDetectionSettings settings)
{
Settings = settings;
this.lines = new List <Line>();
this.lines.AddRange(lines);
}
private static List <ApproximatedLine> ProcessUvLines(List <Line> lines, LineDetectionSettings settings)
{
var approximatedLines = new List <ApproximatedLine>();
foreach (var line in lines)
{
var assigned = false;
foreach (var approxLine in approximatedLines)
{
if (approxLine.TryAddLine(line))
{
assigned = true;
break;
}
}
if (!assigned)
{
approximatedLines.Add(new ApproximatedLine(line, settings));
}
}
var changed = true;
var safety = 0;
while (changed)
{
if (safety++ > 20000)
{
throw new Exception("Broken infinite loop");
}
changed = false;
for (var i = 0; i < approximatedLines.Count - 1; ++i)
{
for (var j = i + 1; j < approximatedLines.Count; ++j)
{
if (approximatedLines[i].TryMerge(approximatedLines[j]))
{
approximatedLines.Remove(approximatedLines[j]);
changed = true;
break;
}
}
if (changed)
{
break;
}
}
}
for (var i = 0; i < approximatedLines.Count - 1; ++i)
{
for (var j = i + 1; j < approximatedLines.Count; ++j)
{
if (approximatedLines[j].lines.Count > 1)
{
continue;
}
if (approximatedLines[i].TryMergeIgnoreAngle(approximatedLines[j]))
{
approximatedLines.Remove(approximatedLines[j]);
}
}
}
for (var i = 0; i < approximatedLines.Count; ++i)
{
approximatedLines[i].Recalculate();
if (approximatedLines[i].BestFitLine.Length > settings.maxLineSegmentLength)
{
LineUtils.SplitByLength(approximatedLines[i], out var a, out var b);
approximatedLines.RemoveAt(i--);
approximatedLines.Add(a);
approximatedLines.Add(b);
}
}
for (var i = 0; i < approximatedLines.Count; ++i)
{
var line = approximatedLines[i];
if (!line.IsValid)
{
LineUtils.RemovePastThresholdLines(line);
}
if (!line.IsValid)
{
approximatedLines.RemoveAt(i--);
}
}
return(approximatedLines);
}
