AR Vision

1. Experiment Objective

Use OpenCV to implement an augmented reality effect — locate the camera pose via a checkerboard and overlay 3D virtual objects onto the camera image.

2. Experiment Principle

1. Key Terms

2. Technical Architecture

Terminal 1: start agent + camera + joint models
Terminal 2:  start AR vision

Data Flow: ESP32 camera → image subscription → grayscale conversion → findChessboardCorners detects the 6×9 checkerboard corners → solvePnPRansac solves the camera pose → pre-defined 3D model points are projected to the 2D image via the camera intrinsics → cv2.line / cv2.fillPoly draw the 3D shapes → real-time display. Key presses switch between 12 kinds of 3D shapes.

3. Core Topic Reference

Firmware Uplink (sensor data)

ROS2 Internal Topics

3. Experiment Steps

1. Hardware Preparation

2. Start Agent + Camera + Joint Model (Terminal 1)

xxxxxxxxxx
ros2 launch microros_v2_bringup car_base.launch.py

3. Start AR Vision (Terminal 2)

xxxxxxxxxx
ros2 launch microros_v2_opencv_visual_pkg simple_ar.launch.py

4. Operation Instructions

Workflow: Grayscale conversion → findChessboardCorners detects the 6×9 checkerboard → cornerSubPix sub-pixel refinement → solvePnPRansac solves the pose → the point sets of the 12 3D shapes are projected to the 2D image via the camera intrinsics → draw lines and polygons.

Remote shape switching: The node also subscribes to /Graphics_topic (std_msgs/String). You can switch shapes by directly specifying a shape name through a ROS2 topic, for example:

xxxxxxxxxx
ros2 topic pub /Graphics_topic std_msgs/msg/String "data: 'Ball'"

The camera intrinsics are factory-calibrated and stored in camera.yaml.

4. Experiment Observations

• Place the checkerboard in the camera's field of view; colored 3D virtual shapes are overlaid and follow the checkerboard pose in real time
• Press f/F to cycle through the 12 different 3D shapes in order

5. Program Analysis

Source code paths:

• Launch: ~/microros_ws/src/microros_v2_opencv_visual_pkg/launch/simple_ar.launch.py
• Node: ~/microros_ws/src/microros_v2_opencv_visual_pkg/microros_v2_opencv_visual_pkg/simple_ar.py

1. Core Node Analysis

SimpleARNode locates the camera pose through checkerboard detection, projects the pre-defined 3D model points to the 2D image, and draws them.

​x
# Source file: ~/microros_ws/src/microros_v2_opencv_visual_pkg/microros_v2_opencv_visual_pkg/simple_ar.py
class SimpleARNode(Node):
    def __init__(self):
        super().__init__('simple_ar_node')
        self.bridge = CvBridge()
​
        # OpenCV display window configuration
        self.window_name = 'Simple AR'
        self.display_width = 960
        self.display_height = 720
        cv.namedWindow(self.window_name, cv.WINDOW_NORMAL)
        cv.resizeWindow(self.window_name, self.display_width, self.display_height)
​
        # Publish the AR-processed image
        self.pub_img = self.create_publisher(Image, '/simpleAR/camera', 1)
​
        # Subscribe to the shape switching topic
        self.sub_graphics = self.create_subscription(
            String, '/Graphics_topic', self.choose_graphics, 1)
​
        # Subscribe to the raw image topic
        self.subscription = self.create_subscription(
            Image, '/camera/image_raw',
            self.image_callback,
            QoSProfile(history=QoSHistoryPolicy.KEEP_LAST, depth=1,
                       reliability=QoSReliabilityPolicy.BEST_EFFORT,
                       durability=QoSDurabilityPolicy.VOLATILE))
​
        # Current shape index and mirror flag
        self.index = 0
        self.mirror = False
​
        # Predefined vertex sets of the 12 3D shapes (partial examples)
        self.graphics = [
            "Triangle", "Rectangle", "Parallelogram", "WindMill",
            "TableTennisTable", "Ball", "Arrow", "Knife",
            "Desk", "Bench", "Stickman", "ParallelBars"
        ]
        self.Graphics = self.graphics[self.index]
​
        # Checkerboard inner corner specification 6×9
        self.patternSize = (6, 9)
​
        # 3D model vertex coordinates (78 points define the vertices and edges of the 12 shapes)
        self.axis = np.float32([
            [0, 0, -1], [0, 8, -1], [5, 8, -1], [5, 0, -1],
            [1, 2, -1], [1, 6, -1], [4, 2, -1], [4, 6, -1],
            # ... 78 3D coordinate points in total
        ])
​
        # Checkerboard 3D world coordinates (6×9=54 corners)
        self.objectPoints = np.zeros((6 * 9, 3), np.float32)
        self.objectPoints[:, :2] = np.mgrid[0:6, 0:9].T.reshape(-1, 2)
​
        # Load camera intrinsics (from camera.yaml)
        config_path = (
            '~/microros_ws/src/'
            'microros_v2_opencv_visual_pkg/config'
        )
        yaml_path = f'{config_path}/camera.yaml'
        if os.path.exists(yaml_path):
            with open(yaml_path, 'r') as f:
                calib_data = yaml.safe_load(f)
            self.cameraMatrix = np.array(
                calib_data['camera_matrix']['data'],
                dtype=np.float64).reshape(3, 3)
            self.distCoeffs = np.array(
                calib_data['distortion_coefficients']['data'],
                dtype=np.float64).reshape(1, -1)
            self.camera_ready = True
​
    def image_callback(self, msg):
        """Image callback: image conversion → checkerboard detection → PnP pose estimation → 3D projection → drawing"""
        try:
            frame = self.bridge.imgmsg_to_cv2(
                msg, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().error(f'Image decode failed: {e}')
            return
​
        if self.mirror:
            frame = cv.flip(frame, 1)
​
        action = cv.waitKey(10) & 0xFF
        frame = self.process(frame, action)
        cv.imshow(self.window_name, frame)
​
        if action == ord('q') or action == 113:
            self.get_logger().info('Exiting the application...')
            rclpy.shutdown()
​
    def process(self, img, action):
        """Core processing pipeline: key switching → grayscale → corner detection → sub-pixel refinement → PnP → 3D projection"""
        if action == ord('f') or action == ord('F'):
            self.graphics_update()
​
        if not self.camera_ready:
            return img  # show the original image only when the camera is not calibrated
​
        gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
​
        # Checkerboard corner detection (6×9 inner corners, with adaptive threshold + fast detection flags)
        retval, corners = cv.findChessboardCorners(
            gray, self.patternSize, None,
            flags=cv.CALIB_CB_ADAPTIVE_THRESH
            + cv.CALIB_CB_NORMALIZE_IMAGE
            + cv.CALIB_CB_FAST_CHECK)
​
        if retval:
            # Sub-pixel corner refinement (30 iterations, precision 0.001)
            corners = cv.cornerSubPix(
                gray, corners, (11, 11), (-1, -1),
                (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001))
​
            # PnP + RANSAC robust pose estimation
            retval, rvec, tvec, inliers = cv.solvePnPRansac(
                self.objectPoints, corners,
                self.cameraMatrix, self.distCoeffs)
​
            if retval:
                # Project the 78 3D model points onto the 2D image
                image_points, jacobian = cv.projectPoints(
                    self.axis, rvec, tvec,
                    self.cameraMatrix, self.distCoeffs)
                # Draw according to the currently selected shape
                img = self.draw(img, corners, image_points)
​
        self.pub_img.publish(self.bridge.cv2_to_imgmsg(img, encoding='bgr8'))
        return img

Key logic notes:

• Checkerboard detection: findChessboardCorners uses the CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_FAST_CHECK flags for adaptive thresholding and fast detection, which suits real-time AR scenarios
• Sub-pixel refinement: cornerSubPix improves corner coordinate accuracy to the sub-pixel level (0.001 px), reducing pose estimation error
• PnP + RANSAC: solvePnPRansac uses the RANSAC algorithm for robust pose estimation, tolerating some misdetected or occluded corners
• 3D→2D projection: projectPoints projects the 78 pre-defined 3D vertices onto the 2D plane using the camera intrinsics and pose — the core operation of AR
• 12 shapes: the draw() method uses cv.drawContours / cv.line / cv.circle to draw the 2D projected outlines of the different 3D shapes based on the current value of self.Graphics

2. Key Parameters

Source code path: ~/microros_ws/src/microros_v2_opencv_visual_pkg/microros_v2_opencv_visual_pkg/simple_ar.py

This node declares no ROS2 parameters (simple_ar.launch.py has no DeclareLaunchArgument, and the source has no self.declare_parameter()); the core configuration consists of hard-coded constants:

To modify the checkerboard specification or the camera calibration file, you need to edit the corresponding lines directly in the source code.

3. Node Description

6. Frequently Asked Questions