Camera Calibration
Kalibrasi kamera untuk proyeksi 3D dan lokalisasi akurat
Camera Calibration
Kalibrasi kamera penting untuk mengkonversi posisi pixel ke koordinat 3D dunia nyata. Tanpa kalibrasi, estimasi jarak bola bisa meleset jauh meskipun deteksi pixel sudah benar.
Intrinsic Parameters
Matrix intrinsik mendeskripsikan properti internal kamera. Nilai ini dipakai untuk mengubah koordinat pixel menjadi arah sinar (ray) dalam ruang 3D.
┌ ┐
│ fx 0 cx │
K = │ 0 fy cy │
│ 0 0 1 │
└ ┘
fx, fy = focal length (pixels)
cx, cy = principal point (image center)Calibration dengan OpenCV
Purpose: Menghitung intrinsic matrix dan distortion coefficients dari checkerboard.
Inputs: path gambar checkerboard, ukuran board, ukuran kotak.
Outputs: mtx (intrinsic), dist (distortion), rvecs, tvecs.
Steps:
- Bangun koordinat 3D papan checkerboard.
- Deteksi corner di setiap image.
- Kalibrasi kamera dari pasangan 3D ke 2D. Pitfalls: jumlah gambar terlalu sedikit atau sudut terbatas membuat hasil bias. Validation: reprojection error kecil dan undistort terlihat lurus.
import cv2
import numpy as np
import glob
def calibrate_camera(images_path, board_size=(9, 6), square_size=0.025):
"""Calibrate camera using checkerboard pattern."""
# Prepare object points
objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2)
objp *= square_size
objpoints = [] # 3D points
imgpoints = [] # 2D points
images = glob.glob(images_path)
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, board_size, None)
if ret:
# Refine corners
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
(cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
objpoints.append(objp)
imgpoints.append(corners)
# Calibrate
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None
)
return mtx, dist, rvecs, tvecsExtrinsic Parameters
Posisi dan orientasi kamera relatif terhadap frame referensi (robot base). Ini penting karena kamera bergerak mengikuti head, sehingga pose kamera berubah setiap saat.
Purpose: Menghitung pose kamera dari sudut head pan dan tilt.
Inputs: sudut pan, tilt, dan offset kamera ke head.
Outputs: rotasi R dan translasi t.
Steps:
- Hitung matriks rotasi per sumbu.
- Gabungkan rotasi sesuai urutan kinematika.
- Gunakan offset kamera sebagai translasi.
Pitfalls: urutan rotasi terbalik menghasilkan posisi kamera salah.
Validation: arah
camera_optical_framesesuai visualisasi TF.
def get_camera_pose(robot_head_tilt, robot_head_pan, camera_offset):
"""Get camera pose from head angles."""
# Rotation matrices
R_tilt = rotation_matrix_x(robot_head_tilt)
R_pan = rotation_matrix_z(robot_head_pan)
R = R_pan @ R_tilt
t = camera_offset # [x, y, z] offset dari head
return R, tPixel to World Projection
Bagian ini menjelaskan bagaimana koordinat pixel diproyeksikan ke lapangan. Hasil ini dipakai untuk menghitung jarak dan posisi objek secara geometris.
Ground Plane Intersection
Purpose: Memproyeksikan pixel ke koordinat lapangan di bidang ground.
Inputs: pixel (u, v), camera_matrix, rotasi R, translasi t.
Outputs: titik (x, y) di ground atau None.
Steps:
- Normalisasi pixel ke koordinat kamera.
- Bentuk arah ray di dunia.
- Hitung titik potong dengan ground. Pitfalls: salah definisi sumbu membuat hasil terbalik. Validation: titik hasil berada di depan kamera dan masuk akal dengan posisi robot.
def pixel_to_ground(u, v, camera_matrix, R, t, ground_height=0):
"""Project pixel to ground plane."""
# Inverse camera matrix
K_inv = np.linalg.inv(camera_matrix)
# Pixel in normalized camera coordinates
p_norm = K_inv @ np.array([u, v, 1])
# Ray direction in world frame
ray_dir = R.T @ p_norm
# Camera position in world
cam_pos = -R.T @ t
# Intersect with ground (z = ground_height)
if ray_dir[2] == 0:
return None
t_intersect = (ground_height - cam_pos[2]) / ray_dir[2]
if t_intersect < 0: # Behind camera
return None
world_point = cam_pos + t_intersect * ray_dir
return world_point[:2] # Return x, y on groundBall 3D Position
Purpose: Estimasi jarak bola dari ukuran radius di image.
Inputs: radius_pixels, camera_matrix, ball_diameter.
Outputs: jarak bola dari kamera (meter).
Steps:
- Ambil
fxdari intrinsic. - Hitung jarak dengan model pinhole. Pitfalls: bola tidak bulat sempurna atau occlusion membuat radius bias. Validation: jarak terukur mendekati ground truth pada jarak tertentu.
def ball_position_from_size(radius_pixels, camera_matrix, ball_diameter=0.065):
"""Estimate ball distance from apparent size."""
fx = camera_matrix[0, 0]
distance = (ball_diameter * fx) / (2 * radius_pixels)
return distanceDistortion Correction
Purpose: Menghapus distorsi lensa sebelum deteksi dan proyeksi.
Inputs: image, camera_matrix, dist_coeffs.
Outputs: image yang sudah undistort.
Steps:
- Hitung matrix baru dengan FOV optimal.
- Jalankan
undistort. - Crop ke ROI valid. Pitfalls: crop mengubah ukuran image; sesuaikan downstream pipeline. Validation: garis lurus di scene terlihat lurus setelah undistort.
def undistort_image(image, camera_matrix, dist_coeffs):
"""Remove lens distortion."""
h, w = image.shape[:2]
# Get optimal camera matrix
new_mtx, roi = cv2.getOptimalNewCameraMatrix(
camera_matrix, dist_coeffs, (w, h), 1, (w, h)
)
# Undistort
undistorted = cv2.undistort(image, camera_matrix, dist_coeffs, None, new_mtx)
# Crop
x, y, w, h = roi
undistorted = undistorted[y:y+h, x:x+w]
return undistortedComplete Calibration Pipeline
Purpose: Membungkus workflow load, save, undistort, dan proyeksi. Inputs: path file kalibrasi dan image. Outputs: image undistort dan koordinat world. Steps:
- Load atau hitung kalibrasi.
- Simpan kalibrasi per kamera.
- Gunakan untuk undistort dan proyeksi pixel. Pitfalls: file kalibrasi tertukar antar kamera membuat hasil meleset. Validation: hasil proyeksi stabil di beberapa posisi bola.
class CameraCalibrator:
def __init__(self, camera_matrix=None, dist_coeffs=None):
self.camera_matrix = camera_matrix
self.dist_coeffs = dist_coeffs
def load_calibration(self, filepath):
"""Load calibration from file."""
data = np.load(filepath)
self.camera_matrix = data['camera_matrix']
self.dist_coeffs = data['dist_coeffs']
def save_calibration(self, filepath):
"""Save calibration to file."""
np.savez(filepath,
camera_matrix=self.camera_matrix,
dist_coeffs=self.dist_coeffs)
def undistort(self, image):
"""Undistort image."""
return undistort_image(image, self.camera_matrix, self.dist_coeffs)
def pixel_to_world(self, u, v, R, t):
"""Project pixel to world coordinates."""
return pixel_to_ground(u, v, self.camera_matrix, R, t)ROS 2 Camera Info
Purpose: Membuat message CameraInfo agar node lain tahu parameter kamera.
Inputs: camera_matrix, dist_coeffs, width, height.
Outputs: sensor_msgs/msg/CameraInfo.
Steps:
- Isi ukuran image.
- Set model distorsi dan param K, D. Pitfalls: ukuran image tidak cocok dengan stream membuat proyeksi salah. Validation: node downstream tidak error saat membaca CameraInfo.
from sensor_msgs.msg import CameraInfo
def create_camera_info(camera_matrix, dist_coeffs, width, height):
"""Create ROS CameraInfo message."""
msg = CameraInfo()
msg.width = width
msg.height = height
msg.distortion_model = 'plumb_bob'
msg.d = dist_coeffs.flatten().tolist()
msg.k = camera_matrix.flatten().tolist()
return msgResources
Practice
- Calibrate webcam menggunakan checkerboard pattern
- Undistort gambar dan bandingkan
- Project ball pixel position ke ground coordinates