Line Follower

My initial line follower is working! It was tricky working out which end of a line segment, created by best fitting points, to follow each time. The code is still not perfect since tight hair-pin corners confuse it. The main assumption is that a line only changes direction gradually. So the line follower always try to continue in a direction close to the previous heading.
Here is a sample of a vectorized line (shown in red):

To improve performance when turning sharp corners I think the algorithm needs to be able to look at the image at different levels. Perhaps it can start with a small radius to capture points and grow it a few times to get a sample of possible headings. This would probably require the erasure of “used” points from the image.

In fact maintaining multiple possible paths could make the algorithm more adaptive. This would make junction handling easy. Both routes are taken, only one has to succeed.

Here is the NextSegment function so far:

        public NextSegment (prevPoint : Point, prevLine : Line) : (Point * Point * Line)
            def points = GetColoredPointsInCircle(prevPoint)
            def (centroid, angle) = BestFit(points)

            def rCosA = Cos(angle) * _radius
            def rSinA = Sin(angle) * _radius
            def startPoint = Point(centroid.X - rCosA, centroid.Y - rSinA)
            def endPoint = Point(centroid.X + rCosA, centroid.Y + rSinA)
            def line = Line(startPoint, endPoint)

            def betterStartPoint = match (line.Intersect(prevLine))
                | Line.Intersection.Point (pt) =>
                    if (PointInLineSegment(pt, startPoint, endPoint))
                        pt    
                    else
                        prevPoint
                | Line.Intersection.Parallel =>
                    prevPoint
                | Line.Intersection.Coincident =>
                    prevPoint

            def (next,l) = if (Abs(prevLine.AngleTo(line)) > PI/2)
                (startPoint, Line(line.Point2, line.Point1))
            else
                (endPoint, line)

            _finished =  (next.DistanceTo(_end) < _radius)
            (betterStartPoint, next, l)

The code to choose the correct next point on the polyline is a bit of hack. Basically it has to flip the best fit “line” as well to ensure the heading is maintained. This block will be changed in future versions to include the multiple branching.

: 23 Nov 2006 19:57 |  Tags: Project |  Comments (0) |  Report a problem