Skip to menu

Robotics with Object Pascal

Rover

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;

 
 
Tune_Params_4_Object_Tracking1.png

 

Tune_Params_4_Object_Tracking2.png

 

Tune_Params_4_Segmentation.png