![]()
2D Lidar : dots => (segments & centroids) => tracking
2025.10.09 11:12
Getting Dots are the easiest step. (Many opensources are available.)
--------------------------------------------------------------------------------------------------
procedure CalculateSegmentEndpoints(var Segment: TSegment);
var
i, j: Integer;
MaxDist, Dist: Double;
P1Index, P2Index: Integer;
begin
if Segment.Count < 2 then
begin
// Segment must have at least 2 points
Segment.OneEnd := Segment.Centroid;
Segment.OtherEnd := Segment.Centroid;
Exit;
end;
MaxDist := -1.0;
P1Index := 1;
P2Index := 2;
// Iterate over all unique pairs of points
for i := 1 to Segment.Count do
begin
for j := i + 1 to Segment.Count do
begin
// Calculate squared distance to avoid expensive Sqrt for comparison
Dist := Sqr(Segment.Points[i].X - Segment.Points[j].X) +
Sqr(Segment.Points[i].Y - Segment.Points[j].Y);
if Dist > MaxDist then
begin
MaxDist := Dist;
P1Index := i;
P2Index := j;
end;
end;
end;
// The two farthest points are the best representation of the segment's endpoints
Segment.OneEnd := Segment.Points[P1Index];
Segment.OtherEnd := Segment.Points[P2Index];
end;
=====================================================
Updated UpdateTrackedObjects Procedure
You need to integrate the tracking logic into your main program structure. Since you are using a fixed array size for TSegmentList, I'll provide an updated UpdateTrackedObjects procedure that uses similar fixed-size arrays for tracking, which is more idiomatic for Object Pascal and consistent with your other code.
You will need to ensure you have the necessary tracking variables defined (as previously suggested):
const
// ... your other constants ...
MAX_ASSOCIATION_DISTANCE = 10.0; // Max distance for matching
MAX_LOST_FRAMES = 5; // How many frames an object can be missing before deletion
type
TTrackedObject = record
ID: Integer;
Centroid: TPoint;
LastSeenFrame: Integer;
FramesLost: Integer;
end;
var
// ... your other variables ...
TrackedObjects: array [1..MAX_SEGMENTS] of TTrackedObject; // Max 100 tracked objects
CurrentFrameNo: Integer = 0; // Starts at 0, incremented each cycle
NextObjectID: Integer = 1;
NumTrackedObjects: Integer = 0; // Actual count of objects being tracked
procedure UpdateTrackedObjects(
const CurrentSegments: TSegmentList;
const FoundNumSegments: Integer;
const CurrentFrame: Integer);
var
i, j, BestMatchIndex: Integer;
MinDist, Dist: Double;
NewCentroidsMatched: array [1..MAX_SEGMENTS] of Boolean;
OldObjectsMatched: array [1..MAX_SEGMENTS] of Boolean;
begin
// 1. Mark all new centroids as unmatched
for i := 1 to FoundNumSegments do
NewCentroidsMatched[i] := False;
// 2. Mark all old objects as unmatched
for j := 1 to NumTrackedObjects do
OldObjectsMatched[j] := False;
// --- Association (Greedy Nearest Neighbor) ---
// Try to match each new segment to the closest unmatched old object
for i := 1 to FoundNumSegments do // Loop through new segments
begin
MinDist := 999999.0;
BestMatchIndex := -1;
for j := 1 to NumTrackedObjects do // Loop through existing tracked objects
begin
// Only consider objects that haven't been matched in this frame yet
if not OldObjectsMatched[j] then
begin
// Calculate distance between new centroid and old object's centroid
Dist := CalculateDistance(CurrentSegments[i].Centroid, TrackedObjects[j].Centroid);
if Dist < MinDist then
begin
MinDist := Dist;
BestMatchIndex := j; // Index in the TrackedObjects array
end;
end;
end;
// --- DECISION POINT: USE MAX_ASSOCIATION_DISTANCE HERE ---
if (BestMatchIndex <> -1) and (MinDist <= MAX_ASSOCIATION_DISTANCE) then
begin
// A match is found and is within the allowed travel distance
// UPDATE THE TRACKED OBJECT
TrackedObjects[BestMatchIndex].Centroid := CurrentSegments[i].Centroid;
TrackedObjects[BestMatchIndex].LastSeenFrame := CurrentFrame;
TrackedObjects[BestMatchIndex].FramesLost := 0; // Reset lost count
// Mark both as matched (critical for 1-to-1 association)
OldObjectsMatched[BestMatchIndex] := True;
NewCentroidsMatched[i] := True;
end;
end; // End of association loop
// --- Handle Unmatched New Centroids (NEW OBJECTS) ---
for i := 1 to FoundNumSegments do
begin
if not NewCentroidsMatched[i] then
begin
// Create a new object if capacity allows
if NumTrackedObjects < MAX_SEGMENTS then
begin
Inc(NumTrackedObjects);
TrackedObjects[NumTrackedObjects].ID := NextObjectID;
TrackedObjects[NumTrackedObjects].Centroid := CurrentSegments[i].Centroid;
TrackedObjects[NumTrackedObjects].LastSeenFrame := CurrentFrame;
TrackedObjects[NumTrackedObjects].FramesLost := 0;
Inc(NextObjectID);
end;
end;
end;
// --- Handle Unmatched Old Objects (LOST OBJECTS) ---
i := 1;
while i <= NumTrackedObjects do
begin
if not OldObjectsMatched[i] then
begin
// Object was missed in this frame, increment lost count
Inc(TrackedObjects[i].FramesLost);
// Check if the object should be deleted (permanently lost)
if TrackedObjects[i].FramesLost >= MAX_LOST_FRAMES then
begin
// Delete the object by shifting the array (expensive but necessary for fixed arrays)
for j := i to NumTrackedObjects - 1 do
TrackedObjects[j] := TrackedObjects[j + 1];
Dec(NumTrackedObjects); // Decrease the total count
// Note: Do NOT Inc(i) since the next object is now at index i
end
else
begin
// Keep the object, but continue to the next index
Inc(i);
end;
end
else
begin
// Matched object, continue to the next index
Inc(i);
end;
end;
// Increment the global frame number for the next cycle
Inc(CurrentFrameNo);
end;