import numpy as np

def compute_add_s(ground_truth_pose, predicted_pose, model_points):
    """
    Calculate the ADD-S metric for 6D pose comparison.
    
    Parameters:
    ground_truth_pose: 4x4 transformation matrix for ground truth pose
    predicted_pose: 4x4 transformation matrix for predicted pose
    model_points: Nx3 array of 3D points representing the object model
    
    Returns:
    ADD-S score (average minimum distance between transformed point sets)
    """
    # Convert to homogeneous coordinates
    ones_column = np.ones((model_points.shape[0], 1))
    homogeneous_points = np.hstack((model_points, ones_column))
    
    # Transform points using ground truth pose
    ground_truth_transformed = (ground_truth_pose @ homogeneous_points.T).T[:, :3]
    
    # Transform points using predicted pose
    predicted_transformed = (predicted_pose @ homogeneous_points.T).T[:, :3]
    
    # Calculate minimum distances between corresponding points
    # Using broadcasting to compute all pairwise distances
    point_differences = ground_truth_transformed[:, None, :] - predicted_transformed[None, :, :]
    pairwise_distances = np.linalg.norm(point_differences, axis=2)
    min_distances = np.min(pairwise_distances, axis=1)
    
    # Compute average minimum distance
    add_s_score = np.mean(min_distances)
    
    return add_s_score

Example Usage

# Define transformation matrices
actual_transform = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
])

estimated_transform = np.array([
    [0.999, 0, 0.035, 0.1],
    [0, 1, 0, 0.2],
    [-0.035, 0, 0.999, 0.3],
    [0, 0, 0, 1]
])

# Define model vertices
model_vertices = np.array([
    [0, 0, 0],
    [1, 0, 0],
    [0, 1, 0],
    [0, 0, 1]
])

# Compute ADD-S metric
add_s_result = compute_add_s(actual_transform, estimated_transform, model_vertices)
print(f"ADD-S Score: {add_s_result}")

Implementation Details

1. Functon Definition

The compute_add_s function accepts three parameters:

• ground_truth_pose: A 4×4 transformation matrix representing the actual object pose
• predicted_pose: A 4×4 transformation matrix representing the estimated pose
• model_points: An N×3 array containing 3D coordinates of the object's model vertices

2. Homogeneous Coordinate Conversion

ones_column = np.ones((model_points.shape[0], 1))
homogeneous_points = np.hstack((model_points, ones_column))

This step converts 3D Cartesian coordinates to homogeneous coordinates by appending a fourth dimension with value 1. This conversion enables proper application of 4×4 transformation matrices, which combine rotation and translation operations.

3. Point Transformasion

ground_truth_transformed = (ground_truth_pose @ homogeneous_points.T).T[:, :3]

This operation transforms model points from they local coordinate system to the global coordinate system using the ground truth pose matrix:

1. Transpose homogeneous points to shape [4, N] for matrix multiplication
2. Apply transformation matrix via matrix multiplication
3. Transpose back to shape [N, 4]
4. Extract first three columns to obtain transformed 3D coordinates

The same process is repeated using the predicted pose matrix.

4. Distance Calculation

point_differences = ground_truth_transformed[:, None, :] - predicted_transformed[None, :, :]
pairwise_distances = np.linalg.norm(point_differences, axis=2)
min_distances = np.min(pairwise_distances, axis=1)

This section computes the ADD-S metric through several operations:

Broadcasting for Pairwise Differences:

• ground_truth_transformed[:, None, :] reshapes to [N, 1, 3]
• predicted_transformed[None, :, :] reshapes to [1, N, 3]
• The subtraction creates a [N, N, 3] array containing all vector differences

Distance Computation:

• np.linalg.norm(..., axis=2) calculates Euclidean distances for all point pairs
• np.min(..., axis=1) finds the minimum distance for each ground truth point

5. Metric Computation

add_s_score = np.mean(min_distances)

The final ADD-S score is the arithmetic mean of all minimum distances, providing a single scalar value representing pose estimation accuracy.

Broadcasting Mechanism

NumPy's broadcasting enables efficient computation with out explicit loops. When arrays have different shapes, NumPy automatically expands dimensions to make them compatible for element-wise operations.

Example:

# Original arrays
a = np.array([[1], [2], [3]])  # Shape: (3, 1)
b = np.array([0, 1, 2])        # Shape: (3,)

# Broadcasting expands both to (3, 3)
result = a + b
# Result: [[1, 2, 3],
#          [2, 3, 4],
#          [3, 4, 5]]

In the ADD-S calculation, broadcasting efficiently computes all pairwise distances between transformed point sets, significantly improving computational performance compared to nested loops.