Converting Distorted Images to Undistorted Images

Camera lenses introduce distortion to images, particularly at the edges, causing straight lines to appear curved. Undistorting images requires the camera's intrinsic parameters and distortion coefficients.

The general process for undistorting images involves:

1. Obtaining the camera's intrinsic matrix and distortion coefficients
2. Computing an optimal new camera matrix to minimize information loss
3. Creating undistortion mapping matrices
4. Applying these maps to remap pixels from the distorted to undistorted image

In our implementation, we used OpenCV with ROS on a Duckiebot with these key steps:

1. Retrieved camera parameters from the ROS camera_info topic
2. Used cv2.getOptimalNewCameraMatrix() to calculate an optimal camera matrix
3. Created mapping matrices once with cv2.initUndistortRectifyMap()
4. Applied the undistortion to each new image using cv2.remap()
5. Cropped the result using the region of interest (ROI) to remove black borders

This method is more efficient than using cv2.undistort() for each frame, as the computational heavy lifting is done only once during initialization.

Fig 1.1a Distorted Camera Image

Fig 1.1b Undistorted Camera Image

Image Preparation for Processing

Before applying our main computer vision algorithms, we performed several pre-processing steps to optimize the images:

• Resized the image from its original 640x480 resolution to half its size (320x240) to improve processing speed
• Cropped the top half of the image to focus only on the relevant road area
• Applied a Gaussian blur using OpenCV's cv2.GaussianBlur() function to reduce noise and detail

These pre-processing steps significantly improved the performance of our subsequent computer vision operations by reducing computational load and focusing only on the relevant portions of the image. We are able to run our nodes at a frequency of 20 frames/second.

Line and Color Detection

We implemented a robust color detection system to identify colored lane lines on the road. The process involved several key steps:

How to detect lines and colors?

1. First, we captured sample images of red, green, and blue lane lines using the Duckiebot's camera
2. We created a custom HSV calibration tool with sliders to precisely determine optimal HSV ranges for each color


Fig 1.3a Red Color Mask



Fig 1.3b Green Color Mask



Fig 1.3c Blue Color Mask

3. Converted camera frames from BGR to HSV color space using cv2.cvtColor()
4. Applied color masking with cv2.inRange() using our calibrated HSV thresholds
5. Enhanced detection reliability by applying dilation to fill gaps in the masks
6. Found and filtered contours using cv2.findContours() and area thresholding

How contour detection works

Contour detection is a crucial step that identifies the boundaries of objects in binary images:

• After creating a binary mask with our HSV color filtering, contour detection identifies connected regions in this mask
• The cv2.findContours() function traces the boundaries of white areas in the binary mask
• Each contour is represented as a sequence of points defining the object's outline
• We used cv2.RETR_TREE to retrieve all contours in a hierarchical structure
• The cv2.CHAIN_APPROX_SIMPLE parameter reduces memory usage by storing only endpoint coordinates
• We then filtered contours by area (>300 pixels) to eliminate noise and focus on substantial lane markings

How to get lane dimensions?

• Used cv2.boundingRect() to extract bounding box coordinates (x, y, width, height) around detected contours
• Stored these dimensions in a structured data format for each detected color lane
• Visualized dimensions by drawing rectangles around detected lanes and annotating with size information

How to develop a robust line and color detection algorithm?

Our robust color detection approach incorporated several key principles:

• Precise calibration: Using our custom HSV calibration tool to fine-tune color ranges for our specific lighting conditions
• Noise reduction: Applying appropriate pre-processing and filtering incorrect detections by area threshold along with dilation
• Anticipating the environment in which colors are detected:Since our Duckiebot is on the ground, cropping the top half to not detect any colors above the road was an important strategy
• Choosing the right contour: Sometimes, multiple contours can be detected in close proximity, we choose the largest contour

This approach enabled reliable detection of colored lane lines, providing critical input for the upcoming navigation system.

Fig 1.3d Red Lane Detection with bounding boxes and dimensions

Fig 1.3e Green Lane Detection with bounding boxes and dimensions

Fig 1.3f Blue Lane Detection with bounding boxes and dimensions

Integration of Vision and Control

We implemented a unified approach to autonomous navigation and LED control based on the visual lane detection:

• Created a dedicated navigation node to handle both movement commands and LED signaling
• Used the detection results from our color detection system to inform navigation decisions
• Implemented LED control to provide visual feedback about the currently detected lane
• Published Twist2D movement commands directly to the car_cmd_switch_node/cmd topic
• Controlled the robot by setting linear velocity (v) and angular velocity (omega) parameters
• This approach differed from Exercise 2 by directly manipulating chassis movement rather than using wheel ticks

Color-Specific Behaviors & Distance Estimation

We successfully implemented the required behaviors for each colored lane (blue: stopping, signaling right, and turning 90° right; red: stopping and driving straight; green: stopping, signaling left, and turning 90° left).

A key innovation in our implementation was using the Duckiebot's extrinsic calibration parameters to accurately determine the distance to each lane:

• Used the homography matrix from the bot's calibration to project pixel coordinates to ground coordinates
• Specifically, mapped the bottom center point of each lane's bounding box to its real-world ground distance
• This approach enabled precise distance-based behaviors, allowing the bot to reliably stop at the required 30cm distance from each lane
• Computed the ground projection using the following transformation:

ground_point = H @ image_point  # Where H is the homography matrix 
# X,Y coordinates in ground frame 
ground_x, ground_y = ground_point[0], ground_point[1]

ROS Service Implementation

To enable modular and on-demand execution of color-specific behaviors, we implemented a ROS service architecture:

• Created a custom service in the lane detection node with the navigation node as the client
• This allowed us to trigger different color behaviors separately and on demand
• The service provided a comprehensive understanding of ROS service/client patterns

# LaneInfo.srv 
# Request 
int8 RED=1 
int8 GREEN=2
int8 BLUE=3
int8 color
---
# Response
bool detected
int32 x
int32 y
int32 width
int32 height
int32 area
float32 distance

By combining accurate distance estimation with our color detection system, the Duckiebot could reliably execute the appropriate behavior for each lane color while maintaining precise positioning relative to the lanes.

Fig 1.5a: Duckiebot approaches the red line from 30cm distance, stops for 3-5 seconds, then proceeds straight ahead for at least 30cm

Fig 1.5b: Duckiebot approaches the green line from 30cm distance, stops for 3-5 seconds, signals using left LEDs, then turns 90 degrees to the left

Fig 1.5c: Duckiebot approaches the blue line from 30cm distance, stops for 3-5 seconds, signals using right LEDs, then turns 90 degrees to the right

Integration and Optimization Considerations

Throughout the development of this project, we identified several key insights regarding system integration:

How to integrate computer vision, LED control, and wheel movement nodes?

• Using ROS services or topics to establish clear communication between detection and control nodes
• Ensuring consistent coordinate frames between vision data and control commands
• Centralizing behavior logic in the navigation node with color detection as a service
• Alternatively, we could make the color detection node a publisher that publishes lane results like distance and dimensions to a topic to which navigation node subscribes to

How to optimize integration and handle delays?

• Reducing image resolution for faster processing
• Processing only regions of interest rather than the entire image
• Adding simple smoothing filters to control commands to reduce jitter

How does camera frequency and control update rate impact performance?

• In our implementation, both rates were set to 20Hz which worked well for our application
• Matching rates helped maintain system stability during transitions between behaviors
• This frequency provided sufficient responsiveness while staying within processing capabilities

Lane Following with Control Systems

This section explores different control strategies for enabling autonomous lane following in the Duckiebot.

Lane Detection Implementation

Building on our approach from Part 1, we implemented lane detection for yellow dotted center lines and white solid outer lines:

• Applied similar HSV color filtering techniques to isolate yellow and white lanes
• Used ground projection via homography to convert detected lane positions to real-world distances
• Published detection results to a custom ROS topic using our own message format
• Enabled the lane controller node to subscribe to these lane positions for feedback control

Fig 2.1: Duckiebot following lane for 1.5m using P controller

Fig 2.2: Duckiebot following lane for 1.5m using PD controller with reduced oscillation

Fig 2.3: Duckiebot following lane for 1.5m using PID controller with improved stability

Control System Implementation

Our error calculation approach focused on maintaining the Duckiebot in the center of the lane:

• When both lanes were detected, error was calculated as the addition of distances (yellow lane +y, white lane -y)
• For balanced positioning, the net error should be zero (equidistant from both lanes)
• When only one lane was detected, we adjusted the error to maintain approximately 10cm from that lane
• This error value directly influenced the angular velocity (omega) for steering control
• After tuning, we settled on gains of: Kp = 30, Kd = 0.7, Ki = 0.1

Comparison of Controller Types

What are the pros and cons of P, PD, and PID controllers?

What is the error calculation method for your controller?

Our error calculation used the ground-projected distances to the lanes:

# When both lanes detected: 
error = yellow_lane_distance + white_lane_distance
# When only yellow lane detected:
error = yellow_lane_distance - desired_distance (10cm)
# When only white lane detected:
error = white_lane_distance - desired_distance (-10cm)

This approach provided a signed error value that represented the robot's deviation from the lane center, where zero indicated perfect centering between lanes.
The control output (p/pd/pid) is obtained as a function of this error and directly assigned to the angular velocity, omega.

How does the derivative ("D") term in the PD controller impact control logic? Is it beneficial?

In theory, the derivative term helps dampen oscillations and provides faster response to changing errors. In our implementation:

• We expected the D term to significantly reduce oscillations
• While we did observe some dampening effect, the difference wasn't as dramatic as anticipated
• Spolier alert: The D term is actually crucial for curved paths and our values for part 3 changed quite a bit
• For our short 1.5m test track, the improvements were definitely present though

How does the integral ("I") term in the PID controller affect performance? Was it useful for your robot?

The integral term had mixed effects on our system:

• We kept the integral gain very small (Ki = 0.1) after testing
• Higher values consistently caused noticeable overshooting
• The small value helped address minor systematic biases
• Overall, this term had the smallest impact on our controller's performance

What methods can be used to tune the controller's parameters effectively?

We used a combination of approaches to tune our PID parameters:

1. Started with manual tuning: implemented P control first (took the most time), then added D, and finally I
2. Used iterative testing: made small adjustments based on observed performance
3. We later realized we could have applied the Ziegler-Nichols method: identified the ultimate gain where oscillations began, then calculated initial parameters. We will keep it in mind for next time
4. It was taking a long time for us to set rebuild the bot for each gain value. We managed to find an alternative starting dts-gui-tools which provided a docker container where we copied our catkin package to. We ran catkin build on it and were able to run the package simply using the rosrun command while passing the gain values as command line arguments. (see alt_lane_following_controller.py in our repo)

The final parameters (Kp = 30, Kd = 0.7, Ki = 0.1) provided a good balance between responsive steering and stable lane following across the 1.5-meter test distance.