Robot Vision and Visual Servoing

Module: Computer Vision | Difficulty: Premium

Image-Based Visual Servoing (IBVS)

where is current feature, is desired, is interaction matrix.

Interaction Matrix

Position-Based Visual Servoing (PBVS)

where is the pseudoinverse of the interaction matrix.

Camera Projection

import numpy as np

class ImageBasedVisualServoing:
    def __init__(self, camera_intrinsics, lambda_gain=0.5):
        self.K = camera_intrinsics
        self.lambda_ = lambda_gain

    def interaction_matrix_2d(self, point_2d, depth):
        x, y = point_2d
        Z = depth
        Le = np.array([
            [-1/Z, 0, x/Z, x*y, -(1+x**2), y],
            [0, -1/Z, y/Z, 1+y**2, -x*y, -x]
        ])
        return Le

    def compute_velocity(self, current_features, desired_features,
                         depths):
        e = current_features - desired_features
        Le_all = []
        for i in range(len(current_features)):
            Le = self.interaction_matrix_2d(
                current_features[i], depths[i])
            Le_all.append(Le)
        Le = np.vstack(Le_all)
        v = -self.lambda_ * np.linalg.pinv(Le) @ e
        return v

class PositionBasedVisualServoing:
    def __init__(self, camera_intrinsics):
        self.K = camera_intrinsics

    def pose_from_features(self, points_2d, points_3d,
                           depths):
        T_est = np.eye(4)
        for i, (p2, p3) in enumerate(
                zip(points_2d, points_3d)):
            direction = np.linalg.inv(self.K) @ np.array(
                [p2[0], p2[1], 1.0])
            T_est[:3, 3] = p3 - direction * depths[i]
        return T_est

    def compute_velocity(self, current_pose, desired_pose,
                         lambda_gain=0.5):
        error = np.linalg.inv(desired_pose) @ current_pose
        v = -lambda_gain * error[:3, 3]
        w = -lambda_gain * np.array([
            error[2, 1] - error[1, 2],
            error[0, 2] - error[2, 0],
            error[1, 0] - error[0, 1]]) / 2
        return np.concatenate([v, w])

Research Insight: Deep visual servoing replaces hand-crafted feature extraction and interaction matrix computation with end-to-end learned policies. These approaches can directly map raw images to robot velocities, bypassing the need for explicit camera calibration and 3D reconstruction. The key advantage is robustness to occlusion and appearance changes, which cause classical IBVS to fail. However, learned policies require extensive simulation-to-real-world domain adaptation, as sim-to-real gaps in visual appearance significantly affect performance.