calibrate.py

import cv2
import numpy as np
import os

import rospy

from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError


# Chessboard dimensions
number_of_squares_X = 12  # Number of chessboard squares along the x-axis
number_of_squares_Y = 9   # Number of chessboard squares along the y-axis

# Number of interior corners
nX = number_of_squares_X - 1  # Number of interior corners along x-axis
nY = number_of_squares_Y - 1  # Number of interior corners along y-axis

# Square side length in meters
square_size = 0.035  # 35mm = 0.035m

# Termination criteria for the iterative corner sub-pixel algorithm
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# Prepare object points (0,0,0), (1,0,0), (2,0,0) ....,(nX-1,nY-1,0)
objp = np.zeros((nY * nX, 3), np.float32)
objp[:, :2] = np.mgrid[0:nX, 0:nY].T.reshape(-1, 2)
objp = objp * square_size

# Arrays to store object points and image points from all images
objpoints = []  # 3D points in real world space
imgpoints = []  # 2D points in image plane

# Get images from the Astra camera
cap = cv2.VideoCapture(0)  # 0 is the default camera index

# Create a directory to save calibration images
cal_img_dir = "calibration_images"
if not os.path.exists(cal_img_dir):
    os.makedirs(cal_img_dir)


rospy.init_node("camera calibrate")

bridge = CvBridge()

img = None

def image_callback(data):
    global img
    try:
        img = bridge.imgmsg_to_cv2(data, "bgr8")
        print("tttt")
    except CvBridgeError as e:
        rospy.logerr("CvBridge Error: {0}".format(e))


image_sub = rospy.Subscriber(
    "/xtion/rgb/camera_raw", Image, image_callback
)


img_count = 0
while True:
    # ret, img = cap.read()
    print("0000")
    # rgb_image_raw = rospy.wait_for_message("/xtion/rgb/image_raw", Image, timeout=10)
    # img = bridge.imgmsg_to_cv2(rgb_image_raw, "bgr8")
    if img is not None:
        bridge.imgmsg_to_cv2(img, "bgr8")

        # if not ret:
        #     print("Failed to capture image")
        #     break

        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # Find the chess board corners
        ret, corners = cv2.findChessboardCorners(gray, (nX, nY), None)

        print("1111")

        cv2.imshow('img', img)

        # If found, add object points and image points
        if ret:
            print("22222")
            objpoints.append(objp)
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
            imgpoints.append(corners2)

            # Draw and display the corners
            cv2.drawChessboardCorners(img, (nX, nY), corners2, ret)
            cv2.imshow('img', img)

            # Save the image if 's' is pressed
            key = cv2.waitKey(1) & 0xFF
            if key == ord('s'):
                img_name = os.path.join(cal_img_dir, f"cal_img_{img_count}.png")
                cv2.imwrite(img_name, img)
                img_count += 1
                print(f"Image {img_count} saved")

        # Press 'q' to exit the loop
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

# Release the camera and close all windows
cap.release()
cv2.destroyAllWindows()

# Calibrate the camera
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

print(f"Camera matrix:\n{mtx}")
print(f"Distortion coefficients:\n{dist}")

# Save the calibration parameters
np.savez("calibration_params.npz", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)