I am working on determining the relative pose between two cameras using fiducial markers and transformation matrices. I have two cameras:
Front Camera, and Behind Camera with opposite directions
I placed two ArUco markers (Marker2 and Marker4) in the scene and computed their transformations relative to both cameras and relative to eachothers using another external cameras and some more markers. Based on the transformation chain in the diagram below, I derived the following equation to compute the transformation TTT from the Front Camera to the Behind Camera: T_front_to_behind = T_behind_to_marker4_inv @ T_marker2_to_marker4_inv @ T_front_to_marker2_inv
Transformations Explanation:
- T1T_1T1​ is the transformation from Marker4 to the Behind Camera.
- T2T_2T2​ is the transformation from Marker2 to the Front Camera.
- T3T_3T3​ is the transformation from Marker4 to Marker2.
- T4T_4T4​ (not explicitly used in the equation) represents the transformation from the Front Camera to the Behind Camera.
I used OpenCV’s cv::aruco.estimatePoseSingleMarkers() to extract the pose of each marker in the camera coordinate system. Now, I need to properly compute TTT using matrix inversions and multiplications.
My Questions:
- Does my transformation equation correctly represent the transformation from the Front Camera to the Behind Camera?
- Should I consider the inverse of the transformations differently, or is my approach valid?
- Are there any OpenCV functions that can simplify this process?
- How should I handle potential errors in the marker detections to improve accuracy?
Any suggestions or insights would be greatly appreciated. Thanks in advance!
import cv2
import numpy as np
import glob
import os
# ======================== CONFIGURATION ======================== #
# Define directories for front and behind camera images
front_image_directory = r'C:\Users\AR07180\Documents\ZED\Phase2-front2behind images\FrontImages'
behind_image_directory = r'C:\Users\AR07180\Documents\ZED\Phase2-front2behind images\BehindImages'
output_directory = r'C:\Users\AR07180\Documents\ZED\Phase2-front2behind images'
os.makedirs(output_directory, exist_ok=True)
# Define ArUco dictionary
aruco_dict_type = cv2.aruco.DICT_6X6_250
aruco_dict = cv2.aruco.getPredefinedDictionary(aruco_dict_type)
# Initialize detector parameters
parameters = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
# Real-world marker size in meters
marker_length = 0.16
# Camera intrinsic parameters (modify if needed)
camera_params = {
"behind": {
"matrix": np.array([[1065.91, 0, 1101.56], [0, 1066.58, 616.181], [0, 0, 1]], dtype=np.float32),
"distortion": np.array([-0.0724717, 0.0593418, 0.000271638, -0.000149516, -0.0257159], dtype=np.float32).reshape(-1, 1)
},
"front": {
"matrix": np.array([[1030.85, 0, 1080.42], [0, 1032.11, 605.77], [0, 0, 1]], dtype=np.float32),
"distortion": np.array([-0.0689, 0.0546, 0.000221, -0.000127, -0.0238], dtype=np.float32).reshape(-1, 1)
}
}
# Marker IDs
marker_ids = {
"reference": 4, # Reference marker
"front_visible": [2], # Front camera sees marker 2
"behind_visible": [4] # Behind camera sees marker 4
}
# Precomputed relative transformations from Marker 4 to Marker 2
relative_transformations = {
4: { # Reference marker
2: np.array([
[0.79821804, -0.07060428, -0.59821651, 2.89099771],
[0.15864749, 0.98268568, 0.09570695, -0.27526049],
[0.58110148, -0.17130056, 0.79559863, 0.28990722],
[0, 0, 0, 1]
])
}
}
# ========================== FUNCTION DEFINITIONS ========================= #
def log_message(message):
"""Prints and logs a message."""
print(message)
def detect_markers_and_compute_poses(image_directory, camera_name):
""" Detects ArUco markers in images and computes their 3D poses. """
camera_matrix = camera_params[camera_name]["matrix"]
dist_coeffs = camera_params[camera_name]["distortion"]
marker_poses = {}
image_files = sorted(glob.glob(os.path.join(image_directory, '*.png')))
if not image_files:
log_message(f"[ERROR] No images found for {camera_name} camera.")
return {}
log_message(f"[INFO] Processing {len(image_files)} images for {camera_name} camera.")
for image_path in image_files:
log_message(f"[DEBUG] Processing image: {os.path.basename(image_path)}")
image = cv2.imread(image_path)
if image is None:
log_message(f"[ERROR] Failed to load image: {image_path}")
continue
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, _ = detector.detectMarkers(gray)
if ids is not None:
log_message(f"[INFO] Detected {len(ids)} marker(s): {ids.flatten()} in {camera_name}")
for j, marker_id in enumerate(ids.flatten()):
marker_corners_2d = corners[j].reshape(-1, 2)
try:
success, rvec, tvec = cv2.solvePnP(
np.array([
[-marker_length / 2, marker_length / 2, 0],
[marker_length / 2, marker_length / 2, 0],
[marker_length / 2, -marker_length / 2, 0],
[-marker_length / 2, -marker_length / 2, 0]
], dtype=np.float32),
marker_corners_2d,
camera_matrix,
dist_coeffs
)
if success:
R, _ = cv2.Rodrigues(rvec)
T = np.hstack((R, tvec.reshape(3, 1)))
T = np.vstack((T, [0, 0, 0, 1]))
marker_poses[marker_id] = T
except cv2.error as e:
log_message(f"[ERROR] solvePnP failed for Marker {marker_id}: {str(e)}")
return marker_poses
def compute_camera_transformation(front_poses, behind_poses):
""" Computes the transformation from the front camera to the behind camera. """
reference_marker = marker_ids["reference"]
front_marker_id = marker_ids["front_visible"][0]
if reference_marker not in behind_poses or front_marker_id not in front_poses:
log_message(f"[ERROR] Required markers not detected.")
return None
log_message("[INFO] Computing transformation from front to behind camera.")
try:
# Transformation from behind camera to marker 4
T_behind_to_marker4 = behind_poses[reference_marker]
# Transformation from front camera to marker 2
T_front_to_marker2 = front_poses[front_marker_id]
# Relative transformation from marker 2 to marker 4
T_marker2_to_marker4 = relative_transformations[reference_marker][front_marker_id]
# Compute the inverses
T_marker2_to_marker4_inv = np.linalg.inv(T_marker2_to_marker4)
T_front_to_marker2_inv = np.linalg.inv(T_front_to_marker2)
T_behind_to_marker4_inv = np.linalg.inv(T_behind_to_marker4)
# Compute transformation from front camera to behind camera
T_front_to_behind = T_behind_to_marker4_inv @ T_marker2_to_marker4_inv @ T_front_to_marker2_inv
log_message("[INFO] Computed transformation from front camera to behind camera.")
log_message(f"[DEBUG] Transformation Matrix:\n{T_front_to_behind}")
return T_front_to_behind
except np.linalg.LinAlgError as e:
log_message(f"[ERROR] Matrix inversion failed: {str(e)}")
return None
# ========================== MAIN EXECUTION ========================== #
log_message("\n=== Starting Front-to-Behind Camera Calibration Process ===")
front_marker_poses = detect_markers_and_compute_poses(front_image_directory, "front")
behind_marker_poses = detect_markers_and_compute_poses(behind_image_directory, "behind")
T_front_to_behind = compute_camera_transformation(front_marker_poses, behind_marker_poses)
cv2.destroyAllWindows()