Camera Calibration¶

import glob

import cv2
import numpy as np


def detect_corners(
    images_path: str, board_size: tuple[int, int], square_size: float
) -> tuple[list[np.ndarray], list[np.ndarray], tuple[int, int]]:
    """Detect chessboard corners in calibration images and prepare object/image points.

    Args:
        images_path (str): Glob pattern to calibration images.
        board_size (tuple[int, int]): Number of corners in chessboard (cols, rows).
        square_size (float): Size of one square edge on the board.

    Returns:
        object_points (list[np.ndarray]): 3D points in real world space for each image.
        image_points (list[np.ndarray]): 2D points in image plane for each image.
        image_shape (tuple[int, int]): Shape of calibration images (width, height).
    """
    obj_p = np.zeros((board_size[1] * board_size[0], 3), np.float32)
    obj_p[:, :2] = np.indices((board_size[0], board_size[1])).T.reshape(-1, 2)
    obj_p *= square_size

    object_points: list[np.ndarray] = []
    image_points: list[np.ndarray] = []
    image_shape: tuple[int, int] = (0, 0)

    for fname in glob.glob(images_path):
        img = cv2.imread(fname)
        if img is None:
            continue
        if image_shape == (0, 0):
            image_shape = (img.shape[1], img.shape[0])
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        found, corners = cv2.findChessboardCorners(
            gray,
            board_size,
            cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE,
        )
        if not found:
            print(f"Warning: Chessboard not found in {fname}")
            continue

        cv2.cornerSubPix(
            gray,
            corners,
            (11, 11),
            (-1, -1),
            (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001),
        )

        object_points.append(obj_p)
        image_points.append(corners)

        cv2.drawChessboardCorners(img, board_size, corners, found)
        cv2.namedWindow("Detected Corners", cv2.WINDOW_NORMAL)
        cv2.imshow("Detected Corners", img)
        cv2.waitKey(1000)

    cv2.destroyAllWindows()

    return object_points, image_points, image_shape


def calibrate_camera(
    object_points: list[np.ndarray],
    image_points: list[np.ndarray],
    image_shape: tuple[int, int],
) -> tuple[np.ndarray, np.ndarray]:
    """Perform camera calibration to compute intrinsic matrix and distortion coefficients.

    Args:
        object_points (list[np.ndarray]): 3D world points from detect_corners().
        image_points (list[np.ndarray]): 2D image points from detect_corners().
        image_shape (tuple[int, int]): (width, height) of calibration images.

    Returns:
        camera_matrix (np.ndarray): Intrinsic parameters matrix (3x3).
        dist_coeffs (np.ndarray): Distortion coefficients (k1, k2, p1, p2, k3).
    """
    _, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
        object_points, image_points, image_shape, None, None
    )

    total_error = 0
    for i in range(len(object_points)):
        projected, _ = cv2.projectPoints(
            object_points[i], rvecs[i], tvecs[i], camera_matrix, dist_coeffs
        )
        error = cv2.norm(image_points[i], projected, cv2.NORM_L2) / len(projected)
        total_error += error
    mean_error = total_error / len(object_points)
    print(f"Calibration completed. Mean reprojection error: {mean_error:.4f}px")

    return camera_matrix, dist_coeffs


if __name__ == "__main__":
    pattern = "images/chessboard/*.jpg"
    board_size = (10, 7)
    square_size = 25
    obj_p, img_p, shape = detect_corners(pattern, board_size, square_size)
    print(f"Detected {len(obj_p)} images with corners.")
    K, d = calibrate_camera(obj_p, img_p, shape)
    np.savez("src/calib_params.npz", camera_matrix=K, dist_coeffs=d)

results = self.model.predict(
                cv_img,
                conf=self.conf,
                iou=self.iou,
                classes=[0],
                max_det=1,
                verbose=False,
            )[0]

if len(results.boxes) == 1:
    box = results.boxes[0]
    cx, cy, w, h = box.xywh[0]
    cx, cy = int(round(cx)), int(round(cy))

img_sub = message_filters.Subscriber(img_sub_topic, CompressedImage)
odom_sub = message_filters.Subscriber(odom_sub_topic, Odometry)
sync = message_filters.ApproximateTimeSynchronizer(
    [img_sub, odom_sub], queue_size=10, slop=0.05
)
sync.registerCallback(self.sync_callback)

def odom_to_transformation(msg: Odometry):
    # 平移向量
    t = np.array([
        msg.pose.pose.position.x,
        msg.pose.pose.position.y,
        msg.pose.pose.position.z
    ])

    # 四元数 (x, y, z, w)
    q = [
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w
    ]
    T = tf.transformations.quaternion_matrix(q)
    T[0:3, 3] = t

    print(f"R =\n{T[0:3, 0:3]}")
    print(f"t ={t}")

    return T

T_lidar_world = odom_to_transformation(msg)

K = np.array(rospy.get_param("~camera_intrinsics"), dtype=np.float64)
T_lidar_camera = np.array(rospy.get_param("~lidar_camera_extrinsics"), dtype=np.float64)
# world -> camera
T_world_camera = T_lidar_camera @ np.linalg.inv(T_lidar_world)
A = K @ T_world_camera[:3, :] # A: 3x4

def generate_coefficients(u, v, A):
    # Mp=b
    M = np.array(
        [
            [A[0, 0] - u * A[2, 0], A[0, 1] - u * A[2, 1], A[0, 2] - u * A[2, 2]],
            [A[1, 0] - v * A[2, 0], A[1, 1] - v * A[2, 1], A[1, 2] - v * A[2, 2]],
        ],
        dtype=np.float64,
    )
    b = -np.array([A[0, 3] - u * A[2, 3], A[1, 3] - v * A[2, 3]], dtype=np.float64)
    return M, b

M_list, b_list = zip(
    *(generate_coefficients(u, v, A) for (u, v, A) in self.deque)
)
M_ls = np.vstack(M_list)
b_ls = np.hstack(b_list)
Xw, *_ = np.linalg.lstsq(M_ls, b_ls, rcond=None)

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

d = generate_distutils_setup(
    packages=['yolo_detector'],
    package_dir={'': 'src'}
)

setup(**d)

# -*- coding: utf-8 -*-
from collections import deque

import cv2
import message_filters
import numpy as np
import rospy
import tf.transformations
from cv_bridge import CvBridge
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image, CompressedImage
from ultralytics import YOLO


def odom_to_transformation(msg: Odometry):
    R = tf.transformations.quaternion_matrix(
        [
            msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.y,
            msg.pose.pose.orientation.z,
            msg.pose.pose.orientation.w,
        ]
    )
    T = np.eye(4)
    T[:3, :3] = R[:3, :3]
    T[:3, 3] = [
        msg.pose.pose.position.x,
        msg.pose.pose.position.y,
        msg.pose.pose.position.z,
    ]
    return T


def generate_coefficients(u, v, A):
    # Mp=b
    M = np.array(
        [
            [A[0, 0] - u * A[2, 0], A[0, 1] - u * A[2, 1], A[0, 2] - u * A[2, 2]],
            [A[1, 0] - v * A[2, 0], A[1, 1] - v * A[2, 1], A[1, 2] - v * A[2, 2]],
        ],
        dtype=np.float64,
    )
    b = -np.array([A[0, 3] - u * A[2, 3], A[1, 3] - v * A[2, 3]], dtype=np.float64)
    return M, b


class YoloDetector:
    def __init__(self):
        rospy.init_node("yolo_detector", anonymous=False)

        img_sub_topic = rospy.get_param("~img_sub_topic", "/usb_cam/image_raw/compressed")
        odom_sub_topic = rospy.get_param("~odom_sub_topic", "/Odometry")
        img_pub_topic = rospy.get_param("~img_pub_topic", "/usb_cam/image_detected")

        self.img_pub = rospy.Publisher(img_pub_topic, Image, queue_size=3)

        self.K = np.array(rospy.get_param("~camera_intrinsics"), dtype=np.float64)
        self.T_lidar_camera = np.array(
            rospy.get_param("~lidar_camera_extrinsics"), dtype=np.float64
        )

        model_path = rospy.get_param("~model_path", "yolov8n.pt")
        self.model = YOLO(model_path)
        self.model.fuse()
        self.model.to("cuda")
        self.model.half()

        self.conf = rospy.get_param("~conf", 0.5)
        self.iou = rospy.get_param("~iou", 0.5)
        self.show_window = rospy.get_param("~show_window", True)

        self.deque = deque(maxlen=rospy.get_param("~deque_size", 3))

        self.last_T = None
        self.thres_distance = rospy.get_param("~thres_distance", 1)
        self.thres_angle = rospy.get_param("~thres_angle", 1)

        img_sub = message_filters.Subscriber(img_sub_topic, CompressedImage)
        odom_sub = message_filters.Subscriber(odom_sub_topic, Odometry)
        sync = message_filters.ApproximateTimeSynchronizer(
            [img_sub, odom_sub], queue_size=30, slop=rospy.get_param("~time_sync_slop", 0.1)
        )
        sync.registerCallback(self.sync_callback)

        self.bridge = CvBridge()

        rospy.spin()

    def sync_callback(self, img_msg: CompressedImage, odom_msg: Odometry):
        cv_img = self.bridge.compressed_imgmsg_to_cv2(img_msg, "bgr8")
        if cv_img is None or cv_img.size == 0:
            rospy.logwarn("Receive empty image!")
            return

        # detect at most one person
        results = self.model.predict(
            cv_img,
            conf=self.conf,
            iou=self.iou,
            classes=[0],
            max_det=1,
            verbose=False,
        )[0]
        if len(results.boxes) == 0:
            rospy.logwarn("No objects detected.")
            return
        else:
            for box in results.boxes:
                rospy.loginfo(f"YOLO result: {box.cls.item()}, {box.conf.item()}, {box.xyxy.tolist()}.")

        # publish image with bounding box
        annotated = results.plot()
        if self.show_window:
            cv2.imshow("YOLO Detection", annotated)
            cv2.waitKey(1)
        img_pub_msg = self.bridge.cv2_to_imgmsg(annotated, encoding="bgr8")
        img_pub_msg.header = img_msg.header
        self.img_pub.publish(img_pub_msg)

        # get the center pixel coordinates (u,v)
        box = results.boxes[0]
        u, v, _, _ = box.xywh[0].cpu().numpy()
        u, v = int(np.round(u)), int(np.round(v))

        T_lidar_world = odom_to_transformation(odom_msg)
        T_world_camera = self.T_lidar_camera @ np.linalg.inv(T_lidar_world)

        if self.last_T is not None:
            d_T = np.linalg.inv(self.last_T) @ T_world_camera
            d_distance = np.linalg.norm(d_T[:3, 3])
            d_R = d_T[:3, :3]
            d_angle = np.arccos(np.clip((np.trace(d_R) - 1) / 2, -1.0, 1.0)) * 180 / np.pi
            if d_distance < self.thres_distance and d_angle < self.thres_angle:
                rospy.logwarn("Skip frame: camera motion too small")
                return

        A = self.K @ T_world_camera[:3, :]
        self.deque.append((u, v, A))
        self.last_T = T_world_camera.copy()
        rospy.loginfo(f"Add matrix (u, v, A), (u, v): {(u, v)}")

        # solve the equation
        if len(self.deque) == self.deque.maxlen:
            M_list, b_list = zip(
                *(generate_coefficients(u, v, A) for (u, v, A) in self.deque)
            )
            M_ls = np.vstack(M_list)
            b_ls = np.hstack(b_list)
            # rospy.loginfo(f"Solving MX=b:\nM: {M_ls},\nb: {b_ls}")
            rospy.loginfo("Solving MX=b")

            Xw, *_ = np.linalg.lstsq(M_ls, b_ls, rcond=None)
            rospy.loginfo(f"Detected person in World Frame: {Xw}")
            Xc = T_world_camera[:3, :3] @ Xw + T_world_camera[:3, 3]
            distance = np.linalg.norm(Xc)
            rospy.loginfo(f"Detected person in Camera Frame: {Xc}")
            rospy.loginfo(f"Direction: {'forward' if Xc[2] > 0 else 'backforward'}, {'right' if Xc[0] > 0 else 'left'}, {'down' if Xc[1] > 0 else 'up'}")
            rospy.loginfo(f"Distance: {distance}")

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from yolo_detector.YoloDetector import YoloDetector

if __name__ == "__main__":
    try:
        YoloDetector()
    except rospy.ROSInterruptException as e:
        rospy.logerr(e)