feat(perception): lane/path edge detector (Issue #339)
Adds Canny+Hough+bird-eye perspective pipeline for detecting left/right
path edges from the forward camera. Pure-Python helper (_path_edges.py)
is fully tested; ROS2 node publishes PathEdges on /saltybot/path_edges.
- saltybot_scene_msgs/msg/PathEdges.msg — new message
- saltybot_scene_msgs/CMakeLists.txt — register PathEdges.msg
- saltybot_bringup/_path_edges.py — PathEdgeConfig, PathEdgesResult,
build/apply_homography, canny_edges,
hough_lines, classify_lines,
average_line, warp_segments,
process_frame
- saltybot_bringup/path_edges_node.py — ROS2 node (sensor_msgs/Image →
PathEdges, parameters for all
tunable Canny/Hough/birdseye params)
- test/test_path_edges.py — 38 tests, 38 passing
- setup.py — add path_edges console_script
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
79505579b1
commit
bd9cb6da35
@ -0,0 +1,379 @@
|
||||
"""
|
||||
_path_edges.py — Lane/path edge detection via Canny + Hough + bird-eye
|
||||
perspective transform (no ROS2 deps).
|
||||
|
||||
Algorithm
|
||||
---------
|
||||
1. Crop the bottom `roi_frac` of the BGR image to an ROI.
|
||||
2. Convert ROI to grayscale → Gaussian blur → Canny edge map.
|
||||
3. Run probabilistic Hough (HoughLinesP) on the edge map to find
|
||||
line segments.
|
||||
4. Filter segments by slope: near-horizontal lines (|slope| < min_slope)
|
||||
are discarded as ground noise; the remaining lines are classified as
|
||||
left edges (negative slope) or right edges (positive slope).
|
||||
5. Average each class into a single dominant edge line and extrapolate it
|
||||
to span the full ROI height.
|
||||
6. Apply a bird-eye perspective homography (computed from a configurable
|
||||
source trapezoid in the ROI) to warp all segment endpoints to a
|
||||
top-down view.
|
||||
7. Return a PathEdgesResult with all data.
|
||||
|
||||
Coordinate conventions
|
||||
-----------------------
|
||||
All pixel coordinates in PathEdgesResult are in the ROI frame:
|
||||
origin = top-left of the bottom-half ROI
|
||||
y = 0 → roi_top of the full image
|
||||
y increases downward; x increases rightward.
|
||||
|
||||
Bird-eye homography
|
||||
-------------------
|
||||
The homography H is computed via cv2.getPerspectiveTransform from four
|
||||
source points (fractional ROI coords) to four destination points in a
|
||||
square output image of size `birdseye_size`. Default source trapezoid
|
||||
assumes a forward-looking camera at ~35–45° tilt above a flat surface.
|
||||
|
||||
Public API
|
||||
----------
|
||||
PathEdgeConfig dataclass — all tunable parameters with sensible defaults
|
||||
PathEdgesResult NamedTuple — all outputs of process_frame()
|
||||
build_homography(src_frac, roi_w, roi_h, birdseye_size)
|
||||
→ 3×3 float64 homography
|
||||
apply_homography(H, points_xy) → np.ndarray (N, 2) warped points
|
||||
canny_edges(bgr_roi, low, high, ksize) → uint8 edge map
|
||||
hough_lines(edge_map, threshold, min_len, max_gap) → List[(x1,y1,x2,y2)]
|
||||
classify_lines(lines, min_slope) → (left, right)
|
||||
average_line(lines, roi_height) → Optional[(x1,y1,x2,y2)]
|
||||
warp_segments(lines, H) → List[(bx1,by1,bx2,by2)]
|
||||
process_frame(bgr, cfg) → PathEdgesResult
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import List, NamedTuple, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ── Config ────────────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class PathEdgeConfig:
|
||||
# ROI
|
||||
roi_frac: float = 0.50 # bottom fraction of image used as ROI
|
||||
|
||||
# Preprocessing
|
||||
blur_ksize: int = 5 # Gaussian blur kernel (odd integer)
|
||||
canny_low: int = 50 # Canny low threshold
|
||||
canny_high: int = 150 # Canny high threshold
|
||||
|
||||
# Hough
|
||||
hough_rho: float = 1.0 # distance resolution (px)
|
||||
hough_theta: float = math.pi / 180.0 # angle resolution (rad)
|
||||
hough_threshold: int = 30 # minimum votes
|
||||
min_line_len: int = 40 # minimum segment length (px)
|
||||
max_line_gap: int = 20 # maximum gap within a segment (px)
|
||||
|
||||
# Line classification
|
||||
min_slope: float = 0.3 # |slope| below this → discard (near-horizontal)
|
||||
|
||||
# Bird-eye perspective — source trapezoid as fractions of (roi_w, roi_h)
|
||||
# Default: wide trapezoid for forward-looking camera at ~40° tilt
|
||||
birdseye_src: List[List[float]] = field(default_factory=lambda: [
|
||||
[0.40, 0.05], # top-left (near horizon)
|
||||
[0.60, 0.05], # top-right (near horizon)
|
||||
[0.95, 0.95], # bottom-right (near robot)
|
||||
[0.05, 0.95], # bottom-left (near robot)
|
||||
])
|
||||
birdseye_size: int = 400 # square bird-eye output image side (px)
|
||||
|
||||
|
||||
# ── Result ────────────────────────────────────────────────────────────────────
|
||||
|
||||
class PathEdgesResult(NamedTuple):
|
||||
lines: List[Tuple[float, float, float, float]] # ROI coords
|
||||
left_lines: List[Tuple[float, float, float, float]]
|
||||
right_lines: List[Tuple[float, float, float, float]]
|
||||
left_edge: Optional[Tuple[float, float, float, float]] # None if absent
|
||||
right_edge: Optional[Tuple[float, float, float, float]]
|
||||
birdseye_lines: List[Tuple[float, float, float, float]]
|
||||
birdseye_left: Optional[Tuple[float, float, float, float]]
|
||||
birdseye_right: Optional[Tuple[float, float, float, float]]
|
||||
H: np.ndarray # 3×3 homography (ROI → bird-eye)
|
||||
roi_top: int # y-offset of ROI in the full image
|
||||
|
||||
|
||||
# ── Homography ────────────────────────────────────────────────────────────────
|
||||
|
||||
def build_homography(
|
||||
src_frac: List[List[float]],
|
||||
roi_w: int,
|
||||
roi_h: int,
|
||||
birdseye_size: int,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Compute a perspective homography from ROI pixel coords to a square
|
||||
bird-eye image.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
src_frac : four [fx, fy] pairs as fractions of (roi_w, roi_h)
|
||||
roi_w, roi_h : ROI dimensions in pixels
|
||||
birdseye_size : side length of the square output image
|
||||
|
||||
Returns
|
||||
-------
|
||||
H : (3, 3) float64 homography matrix; maps ROI px → bird-eye px.
|
||||
"""
|
||||
src = np.array(
|
||||
[[fx * roi_w, fy * roi_h] for fx, fy in src_frac],
|
||||
dtype=np.float32,
|
||||
)
|
||||
S = float(birdseye_size)
|
||||
dst = np.array([
|
||||
[S * 0.25, 0.0],
|
||||
[S * 0.75, 0.0],
|
||||
[S * 0.75, S],
|
||||
[S * 0.25, S],
|
||||
], dtype=np.float32)
|
||||
|
||||
H = cv2.getPerspectiveTransform(src, dst)
|
||||
return H.astype(np.float64)
|
||||
|
||||
|
||||
def apply_homography(H: np.ndarray, points_xy: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Apply a 3×3 homography to an (N, 2) float array of 2-D points.
|
||||
|
||||
Returns
|
||||
-------
|
||||
(N, 2) float32 array of transformed points.
|
||||
"""
|
||||
if len(points_xy) == 0:
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
|
||||
pts = np.column_stack([points_xy, np.ones(len(points_xy))]) # (N, 3)
|
||||
warped = (H @ pts.T).T # (N, 3)
|
||||
warped /= warped[:, 2:3] # homogenise
|
||||
return warped[:, :2].astype(np.float32)
|
||||
|
||||
|
||||
# ── Edge detection ────────────────────────────────────────────────────────────
|
||||
|
||||
def canny_edges(bgr_roi: np.ndarray, low: int, high: int, ksize: int) -> np.ndarray:
|
||||
"""
|
||||
Convert a BGR ROI to a Canny edge map.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
bgr_roi : uint8 BGR image (the bottom-half ROI)
|
||||
low : Canny lower threshold
|
||||
high : Canny upper threshold
|
||||
ksize : Gaussian blur kernel size (must be odd ≥ 1)
|
||||
|
||||
Returns
|
||||
-------
|
||||
uint8 binary edge map (0 or 255), same spatial size as bgr_roi.
|
||||
"""
|
||||
grey = cv2.cvtColor(bgr_roi, cv2.COLOR_BGR2GRAY)
|
||||
if ksize >= 3 and ksize % 2 == 1:
|
||||
grey = cv2.GaussianBlur(grey, (ksize, ksize), 0)
|
||||
return cv2.Canny(grey, low, high)
|
||||
|
||||
|
||||
# ── Hough lines ───────────────────────────────────────────────────────────────
|
||||
|
||||
def hough_lines(
|
||||
edge_map: np.ndarray,
|
||||
threshold: int = 30,
|
||||
min_len: int = 40,
|
||||
max_gap: int = 20,
|
||||
rho: float = 1.0,
|
||||
theta: float = math.pi / 180.0,
|
||||
) -> List[Tuple[float, float, float, float]]:
|
||||
"""
|
||||
Run probabilistic Hough on an edge map.
|
||||
|
||||
Returns
|
||||
-------
|
||||
List of (x1, y1, x2, y2) tuples in the edge_map pixel frame.
|
||||
Empty list when no lines are found.
|
||||
"""
|
||||
raw = cv2.HoughLinesP(
|
||||
edge_map,
|
||||
rho=rho,
|
||||
theta=theta,
|
||||
threshold=threshold,
|
||||
minLineLength=min_len,
|
||||
maxLineGap=max_gap,
|
||||
)
|
||||
if raw is None:
|
||||
return []
|
||||
return [
|
||||
(float(x1), float(y1), float(x2), float(y2))
|
||||
for x1, y1, x2, y2 in raw[:, 0]
|
||||
]
|
||||
|
||||
|
||||
# ── Line classification ───────────────────────────────────────────────────────
|
||||
|
||||
def classify_lines(
|
||||
lines: List[Tuple[float, float, float, float]],
|
||||
min_slope: float = 0.3,
|
||||
) -> Tuple[List, List]:
|
||||
"""
|
||||
Classify Hough segments into left-edge and right-edge candidates.
|
||||
|
||||
In image coordinates (y increases downward):
|
||||
- Left lane lines have NEGATIVE slope (upper-right → lower-left)
|
||||
- Right lane lines have POSITIVE slope (upper-left → lower-right)
|
||||
- Near-horizontal segments (|slope| < min_slope) are discarded
|
||||
|
||||
Returns
|
||||
-------
|
||||
(left_lines, right_lines)
|
||||
"""
|
||||
left: List = []
|
||||
right: List = []
|
||||
for x1, y1, x2, y2 in lines:
|
||||
dx = x2 - x1
|
||||
if abs(dx) < 1e-6:
|
||||
continue # vertical — skip to avoid division by zero
|
||||
slope = (y2 - y1) / dx
|
||||
if slope < -min_slope:
|
||||
left.append((x1, y1, x2, y2))
|
||||
elif slope > min_slope:
|
||||
right.append((x1, y1, x2, y2))
|
||||
return left, right
|
||||
|
||||
|
||||
# ── Line averaging ────────────────────────────────────────────────────────────
|
||||
|
||||
def average_line(
|
||||
lines: List[Tuple[float, float, float, float]],
|
||||
roi_height: int,
|
||||
) -> Optional[Tuple[float, float, float, float]]:
|
||||
"""
|
||||
Average a list of collinear Hough segments into one representative line,
|
||||
extrapolated to span the full ROI height (y=0 → y=roi_height-1).
|
||||
|
||||
Returns None if the list is empty or the averaged slope is near-zero.
|
||||
"""
|
||||
if not lines:
|
||||
return None
|
||||
|
||||
slopes: List[float] = []
|
||||
intercepts: List[float] = []
|
||||
for x1, y1, x2, y2 in lines:
|
||||
dx = x2 - x1
|
||||
if abs(dx) < 1e-6:
|
||||
continue
|
||||
m = (y2 - y1) / dx
|
||||
b = y1 - m * x1
|
||||
slopes.append(m)
|
||||
intercepts.append(b)
|
||||
|
||||
if not slopes:
|
||||
return None
|
||||
|
||||
m_avg = float(np.mean(slopes))
|
||||
b_avg = float(np.mean(intercepts))
|
||||
if abs(m_avg) < 1e-6:
|
||||
return None
|
||||
|
||||
y_bot = float(roi_height - 1)
|
||||
y_top = 0.0
|
||||
x_bot = (y_bot - b_avg) / m_avg
|
||||
x_top = (y_top - b_avg) / m_avg
|
||||
return (x_bot, y_bot, x_top, y_top)
|
||||
|
||||
|
||||
# ── Bird-eye segment warping ──────────────────────────────────────────────────
|
||||
|
||||
def warp_segments(
|
||||
lines: List[Tuple[float, float, float, float]],
|
||||
H: np.ndarray,
|
||||
) -> List[Tuple[float, float, float, float]]:
|
||||
"""
|
||||
Warp a list of line-segment endpoints through the homography H.
|
||||
|
||||
Returns
|
||||
-------
|
||||
List of (bx1, by1, bx2, by2) in bird-eye pixel coordinates.
|
||||
"""
|
||||
if not lines:
|
||||
return []
|
||||
pts = np.array(
|
||||
[[x1, y1] for x1, y1, _, _ in lines] +
|
||||
[[x2, y2] for _, _, x2, y2 in lines],
|
||||
dtype=np.float32,
|
||||
)
|
||||
warped = apply_homography(H, pts)
|
||||
n = len(lines)
|
||||
return [
|
||||
(float(warped[i, 0]), float(warped[i, 1]),
|
||||
float(warped[i + n, 0]), float(warped[i + n, 1]))
|
||||
for i in range(n)
|
||||
]
|
||||
|
||||
|
||||
# ── Main entry point ──────────────────────────────────────────────────────────
|
||||
|
||||
def process_frame(bgr: np.ndarray, cfg: PathEdgeConfig) -> PathEdgesResult:
|
||||
"""
|
||||
Run the full lane/path edge detection pipeline on one BGR frame.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
bgr : uint8 BGR image (H × W × 3)
|
||||
cfg : PathEdgeConfig
|
||||
|
||||
Returns
|
||||
-------
|
||||
PathEdgesResult with all detected edges in ROI + bird-eye coordinates.
|
||||
"""
|
||||
h, w = bgr.shape[:2]
|
||||
roi_top = int(h * (1.0 - cfg.roi_frac))
|
||||
roi = bgr[roi_top:, :]
|
||||
roi_h, roi_w = roi.shape[:2]
|
||||
|
||||
# Build perspective homography (ROI px → bird-eye px)
|
||||
H = build_homography(cfg.birdseye_src, roi_w, roi_h, cfg.birdseye_size)
|
||||
|
||||
# Edge detection
|
||||
edges = canny_edges(roi, cfg.canny_low, cfg.canny_high, cfg.blur_ksize)
|
||||
|
||||
# Hough line segments (in ROI coords)
|
||||
lines = hough_lines(
|
||||
edges,
|
||||
threshold = cfg.hough_threshold,
|
||||
min_len = cfg.min_line_len,
|
||||
max_gap = cfg.max_line_gap,
|
||||
rho = cfg.hough_rho,
|
||||
theta = cfg.hough_theta,
|
||||
)
|
||||
|
||||
# Classify and average
|
||||
left_lines, right_lines = classify_lines(lines, cfg.min_slope)
|
||||
left_edge = average_line(left_lines, roi_h)
|
||||
right_edge = average_line(right_lines, roi_h)
|
||||
|
||||
# Warp all segments to bird-eye
|
||||
birdseye_lines = warp_segments(lines, H)
|
||||
birdseye_left = (warp_segments([left_edge], H)[0] if left_edge else None)
|
||||
birdseye_right = (warp_segments([right_edge], H)[0] if right_edge else None)
|
||||
|
||||
return PathEdgesResult(
|
||||
lines = lines,
|
||||
left_lines = left_lines,
|
||||
right_lines = right_lines,
|
||||
left_edge = left_edge,
|
||||
right_edge = right_edge,
|
||||
birdseye_lines = birdseye_lines,
|
||||
birdseye_left = birdseye_left,
|
||||
birdseye_right = birdseye_right,
|
||||
H = H,
|
||||
roi_top = roi_top,
|
||||
)
|
||||
@ -0,0 +1,164 @@
|
||||
"""
|
||||
path_edges_node.py — ROS2 node for lane/path edge detection (Issue #339).
|
||||
|
||||
Subscribes
|
||||
----------
|
||||
/camera/color/image_raw (sensor_msgs/Image)
|
||||
|
||||
Publishes
|
||||
---------
|
||||
/saltybot/path_edges (saltybot_scene_msgs/PathEdges)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
roi_frac (float, default 0.50) bottom fraction of image as ROI
|
||||
blur_ksize (int, default 5) Gaussian blur kernel size (odd)
|
||||
canny_low (int, default 50) Canny lower threshold
|
||||
canny_high (int, default 150) Canny upper threshold
|
||||
hough_threshold (int, default 30) minimum Hough votes
|
||||
min_line_len (int, default 40) minimum Hough segment length (px)
|
||||
max_line_gap (int, default 20) maximum Hough gap (px)
|
||||
min_slope (float, default 0.3) |slope| below this → discard
|
||||
birdseye_size (int, default 400) bird-eye square image side (px)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Header
|
||||
|
||||
from saltybot_scene_msgs.msg import PathEdges
|
||||
|
||||
from ._path_edges import PathEdgeConfig, process_frame
|
||||
|
||||
|
||||
class PathEdgesNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('path_edges_node')
|
||||
|
||||
# Declare parameters
|
||||
self.declare_parameter('roi_frac', 0.50)
|
||||
self.declare_parameter('blur_ksize', 5)
|
||||
self.declare_parameter('canny_low', 50)
|
||||
self.declare_parameter('canny_high', 150)
|
||||
self.declare_parameter('hough_threshold', 30)
|
||||
self.declare_parameter('min_line_len', 40)
|
||||
self.declare_parameter('max_line_gap', 20)
|
||||
self.declare_parameter('min_slope', 0.3)
|
||||
self.declare_parameter('birdseye_size', 400)
|
||||
|
||||
self._sub = self.create_subscription(
|
||||
Image,
|
||||
'/camera/color/image_raw',
|
||||
self._image_cb,
|
||||
10,
|
||||
)
|
||||
self._pub = self.create_publisher(PathEdges, '/saltybot/path_edges', 10)
|
||||
|
||||
self.get_logger().info('PathEdgesNode ready — subscribing /camera/color/image_raw')
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _build_config(self) -> PathEdgeConfig:
|
||||
p = self.get_parameter
|
||||
return PathEdgeConfig(
|
||||
roi_frac = p('roi_frac').value,
|
||||
blur_ksize = p('blur_ksize').value,
|
||||
canny_low = p('canny_low').value,
|
||||
canny_high = p('canny_high').value,
|
||||
hough_threshold = p('hough_threshold').value,
|
||||
min_line_len = p('min_line_len').value,
|
||||
max_line_gap = p('max_line_gap').value,
|
||||
min_slope = p('min_slope').value,
|
||||
birdseye_size = p('birdseye_size').value,
|
||||
)
|
||||
|
||||
def _image_cb(self, msg: Image) -> None:
|
||||
# Convert ROS Image → BGR numpy array
|
||||
bgr = self._ros_image_to_bgr(msg)
|
||||
if bgr is None:
|
||||
return
|
||||
|
||||
cfg = self._build_config()
|
||||
result = process_frame(bgr, cfg)
|
||||
|
||||
out = PathEdges()
|
||||
out.header = msg.header
|
||||
out.line_count = len(result.lines)
|
||||
out.roi_top = result.roi_top
|
||||
|
||||
# Flat float32 arrays: [x1,y1,x2,y2, x1,y1,x2,y2, ...]
|
||||
out.segments_px = [v for seg in result.lines for v in seg]
|
||||
out.segments_birdseye_px = [v for seg in result.birdseye_lines for v in seg]
|
||||
|
||||
# Left edge
|
||||
if result.left_edge is not None:
|
||||
x1, y1, x2, y2 = result.left_edge
|
||||
out.left_x1, out.left_y1 = float(x1), float(y1)
|
||||
out.left_x2, out.left_y2 = float(x2), float(y2)
|
||||
out.left_detected = True
|
||||
else:
|
||||
out.left_detected = False
|
||||
|
||||
if result.birdseye_left is not None:
|
||||
bx1, by1, bx2, by2 = result.birdseye_left
|
||||
out.left_birdseye_x1, out.left_birdseye_y1 = float(bx1), float(by1)
|
||||
out.left_birdseye_x2, out.left_birdseye_y2 = float(bx2), float(by2)
|
||||
|
||||
# Right edge
|
||||
if result.right_edge is not None:
|
||||
x1, y1, x2, y2 = result.right_edge
|
||||
out.right_x1, out.right_y1 = float(x1), float(y1)
|
||||
out.right_x2, out.right_y2 = float(x2), float(y2)
|
||||
out.right_detected = True
|
||||
else:
|
||||
out.right_detected = False
|
||||
|
||||
if result.birdseye_right is not None:
|
||||
bx1, by1, bx2, by2 = result.birdseye_right
|
||||
out.right_birdseye_x1, out.right_birdseye_y1 = float(bx1), float(by1)
|
||||
out.right_birdseye_x2, out.right_birdseye_y2 = float(bx2), float(by2)
|
||||
|
||||
self._pub.publish(out)
|
||||
|
||||
@staticmethod
|
||||
def _ros_image_to_bgr(msg: Image) -> np.ndarray | None:
|
||||
"""Convert a sensor_msgs/Image to a uint8 BGR numpy array."""
|
||||
enc = msg.encoding.lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
|
||||
if enc in ('rgb8', 'bgr8', 'mono8'):
|
||||
channels = 1 if enc == 'mono8' else 3
|
||||
try:
|
||||
img = data.reshape((msg.height, msg.width, channels))
|
||||
except ValueError:
|
||||
return None
|
||||
if enc == 'rgb8':
|
||||
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
|
||||
elif enc == 'mono8':
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
return img
|
||||
|
||||
# Unsupported encoding
|
||||
return None
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = PathEdgesNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -53,6 +53,8 @@ setup(
|
||||
'person_reid = saltybot_bringup.person_reid_node:main',
|
||||
# Dynamic obstacle velocity estimator (Issue #326)
|
||||
'obstacle_velocity = saltybot_bringup.obstacle_velocity_node:main',
|
||||
# Lane/path edge detector (Issue #339)
|
||||
'path_edges = saltybot_bringup.path_edges_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
364
jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py
Normal file
364
jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py
Normal file
@ -0,0 +1,364 @@
|
||||
"""
|
||||
test_path_edges.py — pytest tests for _path_edges.py (no ROS2 required).
|
||||
|
||||
Tests cover:
|
||||
- build_homography: output shape, identity-like mapping
|
||||
- apply_homography: empty input, single point, batch
|
||||
- canny_edges: output shape, dtype, uniform image produces no edges
|
||||
- hough_lines: empty edge map returns []
|
||||
- classify_lines: slope filtering, left/right split
|
||||
- average_line: empty → None, single line, multi-line average
|
||||
- warp_segments: empty list, segment endpoint ordering
|
||||
- process_frame: smoke test on synthetic image
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from saltybot_bringup._path_edges import (
|
||||
PathEdgeConfig,
|
||||
PathEdgesResult,
|
||||
apply_homography,
|
||||
average_line,
|
||||
build_homography,
|
||||
canny_edges,
|
||||
classify_lines,
|
||||
hough_lines,
|
||||
process_frame,
|
||||
warp_segments,
|
||||
)
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _solid_bgr(h: int = 100, w: int = 200, color=(128, 128, 128)) -> np.ndarray:
|
||||
img = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
img[:] = color
|
||||
return img
|
||||
|
||||
|
||||
def _default_H(roi_w: int = 200, roi_h: int = 100) -> np.ndarray:
|
||||
cfg = PathEdgeConfig()
|
||||
return build_homography(cfg.birdseye_src, roi_w, roi_h, cfg.birdseye_size)
|
||||
|
||||
|
||||
# ── build_homography ──────────────────────────────────────────────────────────
|
||||
|
||||
class TestBuildHomography:
|
||||
def test_shape(self):
|
||||
H = _default_H()
|
||||
assert H.shape == (3, 3)
|
||||
|
||||
def test_dtype(self):
|
||||
H = _default_H()
|
||||
assert H.dtype == np.float64
|
||||
|
||||
def test_bottom_left_maps_near_left(self):
|
||||
"""Bottom-left source trapezoid point should map near left of bird-eye."""
|
||||
cfg = PathEdgeConfig()
|
||||
H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size)
|
||||
# src[3] = [0.05, 0.95] → near bottom-left of ROI → should map to x≈100 (centre-left), y≈400
|
||||
src_pt = np.array([[0.05 * 200, 0.95 * 100]], dtype=np.float32)
|
||||
dst = apply_homography(H, src_pt)
|
||||
assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.25, abs=5)
|
||||
assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5)
|
||||
|
||||
def test_bottom_right_maps_near_right(self):
|
||||
cfg = PathEdgeConfig()
|
||||
H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size)
|
||||
# src[2] = [0.95, 0.95] → bottom-right → maps to x≈300, y≈400
|
||||
src_pt = np.array([[0.95 * 200, 0.95 * 100]], dtype=np.float32)
|
||||
dst = apply_homography(H, src_pt)
|
||||
assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.75, abs=5)
|
||||
assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5)
|
||||
|
||||
|
||||
# ── apply_homography ──────────────────────────────────────────────────────────
|
||||
|
||||
class TestApplyHomography:
|
||||
def test_empty_input(self):
|
||||
H = _default_H()
|
||||
out = apply_homography(H, np.empty((0, 2), dtype=np.float32))
|
||||
assert out.shape == (0, 2)
|
||||
|
||||
def test_single_point_roundtrip(self):
|
||||
"""Warping then inverse-warping should recover the original point."""
|
||||
H = _default_H(200, 100)
|
||||
H_inv = np.linalg.inv(H)
|
||||
pt = np.array([[50.0, 40.0]], dtype=np.float32)
|
||||
warped = apply_homography(H, pt)
|
||||
unwarped = apply_homography(H_inv, warped)
|
||||
np.testing.assert_allclose(unwarped, pt, atol=1e-3)
|
||||
|
||||
def test_batch_output_shape(self):
|
||||
H = _default_H()
|
||||
pts = np.random.rand(5, 2).astype(np.float32) * 100
|
||||
out = apply_homography(H, pts)
|
||||
assert out.shape == (5, 2)
|
||||
assert out.dtype == np.float32
|
||||
|
||||
|
||||
# ── canny_edges ───────────────────────────────────────────────────────────────
|
||||
|
||||
class TestCannyEdges:
|
||||
def test_output_shape(self):
|
||||
roi = _solid_bgr(80, 160)
|
||||
edges = canny_edges(roi, low=50, high=150, ksize=5)
|
||||
assert edges.shape == (80, 160)
|
||||
|
||||
def test_output_dtype(self):
|
||||
roi = _solid_bgr()
|
||||
edges = canny_edges(roi, low=50, high=150, ksize=5)
|
||||
assert edges.dtype == np.uint8
|
||||
|
||||
def test_uniform_image_no_edges(self):
|
||||
"""A solid-colour image should produce no edges."""
|
||||
roi = _solid_bgr(80, 160, color=(100, 100, 100))
|
||||
edges = canny_edges(roi, low=50, high=150, ksize=5)
|
||||
assert edges.max() == 0
|
||||
|
||||
def test_strong_edge_detected(self):
|
||||
"""A sharp horizontal boundary should produce edges."""
|
||||
roi = np.zeros((100, 200, 3), dtype=np.uint8)
|
||||
roi[50:, :] = 255 # sharp boundary at y=50
|
||||
edges = canny_edges(roi, low=20, high=60, ksize=3)
|
||||
assert edges.max() == 255
|
||||
|
||||
def test_ksize_even_skips_blur(self):
|
||||
"""Even ksize should not crash (blur is skipped for even kernels)."""
|
||||
roi = _solid_bgr()
|
||||
edges = canny_edges(roi, low=50, high=150, ksize=4)
|
||||
assert edges.shape == (100, 200)
|
||||
|
||||
|
||||
# ── hough_lines ───────────────────────────────────────────────────────────────
|
||||
|
||||
class TestHoughLines:
|
||||
def test_empty_edge_map(self):
|
||||
edge_map = np.zeros((100, 200), dtype=np.uint8)
|
||||
lines = hough_lines(edge_map, threshold=30, min_len=40, max_gap=20)
|
||||
assert lines == []
|
||||
|
||||
def test_returns_list_of_tuples(self):
|
||||
"""Draw a diagonal line and verify hough returns tuples of 4 floats."""
|
||||
edge_map = np.zeros((100, 200), dtype=np.uint8)
|
||||
cv2.line(edge_map, (0, 0), (199, 99), 255, 2)
|
||||
lines = hough_lines(edge_map, threshold=10, min_len=20, max_gap=5)
|
||||
assert isinstance(lines, list)
|
||||
if lines:
|
||||
assert len(lines[0]) == 4
|
||||
assert all(isinstance(v, float) for v in lines[0])
|
||||
|
||||
def test_line_detected_on_drawn_segment(self):
|
||||
"""A drawn line segment should be detected."""
|
||||
edge_map = np.zeros((200, 400), dtype=np.uint8)
|
||||
cv2.line(edge_map, (50, 100), (350, 150), 255, 2)
|
||||
lines = hough_lines(edge_map, threshold=10, min_len=30, max_gap=10)
|
||||
assert len(lines) >= 1
|
||||
|
||||
|
||||
# ── classify_lines ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestClassifyLines:
|
||||
def test_empty_returns_two_empty_lists(self):
|
||||
left, right = classify_lines([], min_slope=0.3)
|
||||
assert left == []
|
||||
assert right == []
|
||||
|
||||
def test_negative_slope_goes_left(self):
|
||||
# slope = (y2-y1)/(x2-x1) = (50-0)/(0-100) = -0.5 → left
|
||||
lines = [(100.0, 0.0, 0.0, 50.0)]
|
||||
left, right = classify_lines(lines, min_slope=0.3)
|
||||
assert len(left) == 1
|
||||
assert right == []
|
||||
|
||||
def test_positive_slope_goes_right(self):
|
||||
# slope = (50-0)/(100-0) = 0.5 → right
|
||||
lines = [(0.0, 0.0, 100.0, 50.0)]
|
||||
left, right = classify_lines(lines, min_slope=0.3)
|
||||
assert left == []
|
||||
assert len(right) == 1
|
||||
|
||||
def test_near_horizontal_discarded(self):
|
||||
# slope = 0.1 → |slope| < 0.3 → discard
|
||||
lines = [(0.0, 0.0, 100.0, 10.0)]
|
||||
left, right = classify_lines(lines, min_slope=0.3)
|
||||
assert left == []
|
||||
assert right == []
|
||||
|
||||
def test_vertical_line_skipped(self):
|
||||
# dx ≈ 0 → skip
|
||||
lines = [(50.0, 0.0, 50.0, 100.0)]
|
||||
left, right = classify_lines(lines, min_slope=0.3)
|
||||
assert left == []
|
||||
assert right == []
|
||||
|
||||
def test_mixed_lines(self):
|
||||
lines = [
|
||||
(100.0, 0.0, 0.0, 50.0), # slope -0.5 → left
|
||||
(0.0, 0.0, 100.0, 50.0), # slope +0.5 → right
|
||||
(0.0, 0.0, 100.0, 5.0), # slope +0.05 → discard
|
||||
]
|
||||
left, right = classify_lines(lines, min_slope=0.3)
|
||||
assert len(left) == 1
|
||||
assert len(right) == 1
|
||||
|
||||
|
||||
# ── average_line ──────────────────────────────────────────────────────────────
|
||||
|
||||
class TestAverageLine:
|
||||
def test_empty_returns_none(self):
|
||||
assert average_line([], roi_height=100) is None
|
||||
|
||||
def test_single_line_extrapolated(self):
|
||||
# slope=0.5, intercept=0: x = y/0.5 = 2y
|
||||
# At y=99: x=198; at y=0: x=0
|
||||
lines = [(0.0, 0.0, 200.0, 100.0)] # slope = 100/200 = 0.5
|
||||
result = average_line(lines, roi_height=100)
|
||||
assert result is not None
|
||||
x_bot, y_bot, x_top, y_top = result
|
||||
assert y_bot == pytest.approx(99.0, abs=1)
|
||||
assert y_top == pytest.approx(0.0, abs=1)
|
||||
|
||||
def test_two_parallel_lines_averaged(self):
|
||||
# Both have slope=1.0, intercepts -50 and +50 → avg intercept=0
|
||||
# x = (y - 0) / 1.0 = y
|
||||
lines = [
|
||||
(50.0, 0.0, 100.0, 50.0), # slope=1, intercept=-50
|
||||
(0.0, 50.0, 50.0, 100.0), # slope=1, intercept=50
|
||||
]
|
||||
result = average_line(lines, roi_height=100)
|
||||
assert result is not None
|
||||
x_bot, y_bot, x_top, y_top = result
|
||||
assert y_bot == pytest.approx(99.0, abs=1)
|
||||
# avg intercept = 0, m_avg=1 → x_bot = 99
|
||||
assert x_bot == pytest.approx(99.0, abs=2)
|
||||
|
||||
def test_vertical_only_returns_none(self):
|
||||
# dx == 0 → skip → no slopes → None
|
||||
lines = [(50.0, 0.0, 50.0, 100.0)]
|
||||
assert average_line(lines, roi_height=100) is None
|
||||
|
||||
def test_returns_four_tuple(self):
|
||||
lines = [(0.0, 0.0, 100.0, 50.0)]
|
||||
result = average_line(lines, roi_height=100)
|
||||
assert result is not None
|
||||
assert len(result) == 4
|
||||
|
||||
|
||||
# ── warp_segments ─────────────────────────────────────────────────────────────
|
||||
|
||||
class TestWarpSegments:
|
||||
def test_empty_returns_empty(self):
|
||||
H = _default_H()
|
||||
result = warp_segments([], H)
|
||||
assert result == []
|
||||
|
||||
def test_single_segment_returns_one_tuple(self):
|
||||
H = _default_H(200, 100)
|
||||
lines = [(10.0, 10.0, 90.0, 80.0)]
|
||||
result = warp_segments(lines, H)
|
||||
assert len(result) == 1
|
||||
assert len(result[0]) == 4
|
||||
|
||||
def test_start_and_end_distinct(self):
|
||||
"""Warped segment endpoints should be different from each other."""
|
||||
H = _default_H(200, 100)
|
||||
lines = [(10.0, 10.0, 190.0, 90.0)]
|
||||
result = warp_segments(lines, H)
|
||||
bx1, by1, bx2, by2 = result[0]
|
||||
# The two endpoints should differ
|
||||
assert abs(bx1 - bx2) + abs(by1 - by2) > 1.0
|
||||
|
||||
def test_batch_preserves_count(self):
|
||||
H = _default_H(200, 100)
|
||||
lines = [(0.0, 0.0, 10.0, 10.0), (100.0, 0.0, 90.0, 50.0)]
|
||||
result = warp_segments(lines, H)
|
||||
assert len(result) == 2
|
||||
|
||||
|
||||
# ── process_frame ─────────────────────────────────────────────────────────────
|
||||
|
||||
class TestProcessFrame:
|
||||
def _lane_image(self, h: int = 480, w: int = 640) -> np.ndarray:
|
||||
"""
|
||||
Create a synthetic BGR image with two diagonal lines representing
|
||||
left and right lane edges in the bottom half.
|
||||
"""
|
||||
img = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
roi_top = h // 2
|
||||
# Left edge: from bottom-left area upward to the right (negative slope in image coords)
|
||||
cv2.line(img, (80, h - 10), (240, roi_top + 10), (255, 255, 255), 4)
|
||||
# Right edge: from bottom-right area upward to the left (positive slope in image coords)
|
||||
cv2.line(img, (w - 80, h - 10), (w - 240, roi_top + 10), (255, 255, 255), 4)
|
||||
return img
|
||||
|
||||
def test_returns_named_tuple(self):
|
||||
bgr = _solid_bgr(120, 240)
|
||||
cfg = PathEdgeConfig()
|
||||
result = process_frame(bgr, cfg)
|
||||
assert isinstance(result, PathEdgesResult)
|
||||
|
||||
def test_roi_top_correct(self):
|
||||
bgr = np.zeros((200, 400, 3), dtype=np.uint8)
|
||||
cfg = PathEdgeConfig(roi_frac=0.5)
|
||||
result = process_frame(bgr, cfg)
|
||||
assert result.roi_top == 100
|
||||
|
||||
def test_uniform_image_no_lines(self):
|
||||
bgr = _solid_bgr(200, 400, color=(80, 80, 80))
|
||||
cfg = PathEdgeConfig()
|
||||
result = process_frame(bgr, cfg)
|
||||
assert result.lines == []
|
||||
assert result.left_edge is None
|
||||
assert result.right_edge is None
|
||||
assert result.left_lines == []
|
||||
assert result.right_lines == []
|
||||
|
||||
def test_homography_matrix_shape(self):
|
||||
bgr = _solid_bgr(200, 400)
|
||||
result = process_frame(bgr, PathEdgeConfig())
|
||||
assert result.H.shape == (3, 3)
|
||||
|
||||
def test_birdseye_segments_same_count(self):
|
||||
"""birdseye_lines and lines must have the same number of segments."""
|
||||
bgr = self._lane_image()
|
||||
result = process_frame(bgr, PathEdgeConfig(hough_threshold=10, min_line_len=20))
|
||||
assert len(result.birdseye_lines) == len(result.lines)
|
||||
|
||||
def test_lane_image_detects_edges(self):
|
||||
"""Synthetic lane image should detect at least one of left/right edge."""
|
||||
bgr = self._lane_image()
|
||||
cfg = PathEdgeConfig(
|
||||
roi_frac=0.5,
|
||||
canny_low=30,
|
||||
canny_high=100,
|
||||
hough_threshold=10,
|
||||
min_line_len=20,
|
||||
max_line_gap=15,
|
||||
)
|
||||
result = process_frame(bgr, cfg)
|
||||
assert (result.left_edge is not None) or (result.right_edge is not None)
|
||||
|
||||
def test_segments_px_flat_array_length(self):
|
||||
"""segments_px-equivalent length must be 4 × line_count."""
|
||||
bgr = self._lane_image()
|
||||
cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20)
|
||||
result = process_frame(bgr, cfg)
|
||||
assert len(result.lines) * 4 == sum(4 for _ in result.lines)
|
||||
|
||||
def test_left_right_lines_subset_of_all_lines(self):
|
||||
"""left_lines + right_lines must be a subset of all lines."""
|
||||
bgr = self._lane_image()
|
||||
cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20)
|
||||
result = process_frame(bgr, cfg)
|
||||
all_set = set(result.lines)
|
||||
for seg in result.left_lines:
|
||||
assert seg in all_set
|
||||
for seg in result.right_lines:
|
||||
assert seg in all_set
|
||||
@ -25,6 +25,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
# Issue #326 — dynamic obstacle velocity estimator
|
||||
"msg/ObstacleVelocity.msg"
|
||||
"msg/ObstacleVelocityArray.msg"
|
||||
# Issue #339 — lane/path edge detector
|
||||
"msg/PathEdges.msg"
|
||||
DEPENDENCIES std_msgs geometry_msgs vision_msgs builtin_interfaces
|
||||
)
|
||||
|
||||
|
||||
48
jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg
Normal file
48
jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg
Normal file
@ -0,0 +1,48 @@
|
||||
# PathEdges.msg — Lane/path edge detection result (Issue #339)
|
||||
#
|
||||
# All pixel coordinates are in the ROI frame:
|
||||
# origin = top-left of the bottom-half ROI crop
|
||||
# y=0 → roi_top of the full image; y increases downward; x increases rightward.
|
||||
#
|
||||
# Bird-eye coordinates are in the warped top-down perspective image.
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
# --- Raw Hough segments (ROI frame) ---
|
||||
# Flat array of (x1, y1, x2, y2) tuples; length = 4 * line_count
|
||||
float32[] segments_px
|
||||
|
||||
# Same segments warped to bird-eye view; length = 4 * line_count
|
||||
float32[] segments_birdseye_px
|
||||
|
||||
# Number of Hough line segments detected
|
||||
uint32 line_count
|
||||
|
||||
# --- Dominant left edge (ROI frame) ---
|
||||
float32 left_x1
|
||||
float32 left_y1
|
||||
float32 left_x2
|
||||
float32 left_y2
|
||||
bool left_detected
|
||||
|
||||
# --- Dominant left edge (bird-eye frame) ---
|
||||
float32 left_birdseye_x1
|
||||
float32 left_birdseye_y1
|
||||
float32 left_birdseye_x2
|
||||
float32 left_birdseye_y2
|
||||
|
||||
# --- Dominant right edge (ROI frame) ---
|
||||
float32 right_x1
|
||||
float32 right_y1
|
||||
float32 right_x2
|
||||
float32 right_y2
|
||||
bool right_detected
|
||||
|
||||
# --- Dominant right edge (bird-eye frame) ---
|
||||
float32 right_birdseye_x1
|
||||
float32 right_birdseye_y1
|
||||
float32 right_birdseye_x2
|
||||
float32 right_birdseye_y2
|
||||
|
||||
# y-offset of ROI in the full image (pixels from top)
|
||||
uint32 roi_top
|
||||
Loading…
x
Reference in New Issue
Block a user