Hi,
I am trying to using RODODK for OpenCV simulation.
However, I am having trouble figuring out why the position is off by a few mm in the x and y.
The reprojection is .15 which should be really good. Is my math wrong?
import cv2
import numpy as np
import cv2.aruco as aruco
from robodk.robomath import Mat
from robodk.robolink import *
board = cv2.aruco.CharucoBoard(
size=(7,5),
squareLength=0.023444,
markerLength=0.017439,
dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_50)
)
picture_range=16
RDK= Robolink()
Poses = [RDK.Item(f'Target {i}') for i in range(1, picture_range)]
for i in range(1, picture_range):
globals()[f"Pose_{i}"] = RDK.Item(f"Target {i}")
camera= RDK.Item('Cali')
robot = RDK.Item("Doosan Robotics H2515")
robot.setSpeed(1000) # mm/s
robot.setAcceleration(1000) # mm/s²
if not camera.Valid():
print('CAMERA NOT FOUND')
exit()
calibration_file = "C:/Users//PycharmProjects/GolfPutty/CalibrationFiles/calibration.xml"
def chaUco_Calibration(calibration_file):
# Detection setup
# Calibration data collection
all_corners = []
all_ids = []
image_size = None
calibration_complete = False
detector = aruco.CharucoDetector(board)
while True:
for i, pose in enumerate(Poses):
robot.MoveJ(pose)
# Get frame
bytes_img = RDK.Cam2D_Snapshot("", camera)
if not bytes_img:
continue
nparr = np.frombuffer(bytes_img, np.uint8)
frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
cv2.imwrite(f"C:/Users//PycharmProjects/GolfPutty/CalibrationFiles/snap_{i}.png", frame)
if image_size is None:
image_size = frame.shape[1], frame.shape[0]
# Detection
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect ChArUco board
charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(frame)
if charuco_ids is not None:
frame = cv2.aruco.drawDetectedMarkers(frame, marker_corners, marker_ids)
frame = cv2.aruco.drawDetectedCornersCharuco(frame, charuco_corners, charuco_ids)
#display_text = "Press 'c' to capture calibration frame"
if charuco_ids is not None:
frame = cv2.aruco.drawDetectedMarkers(frame, marker_corners, marker_ids)
frame = cv2.aruco.drawDetectedCornersCharuco(frame, charuco_corners, charuco_ids)
#key = cv2.waitKey(1)
all_corners.append(charuco_corners)
all_ids.append(charuco_ids)
print(f"Captured frame {len(all_corners)}")
# Calibrate when enough frames (press 'f' to force)
key = cv2.waitKey(1)
if ((len(all_corners) >= picture_range)) and not calibration_complete:
print("Calibrating...")
ret, mtx, dist, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco(
all_corners, all_ids, board, image_size, None, None)
if ret < 1: # Good reprojection error
fs = cv2.FileStorage(calibration_file, cv2.FILE_STORAGE_WRITE)
if not fs.isOpened():
print("Error: Unable to open file for writing. Check file path and permissions.")
exit(1) # Stop execution
fs.write("Camera_Matrix", mtx)
fs.write("Distortion_Coefficients", dist)
print("Calibration complete and uploaded to RoboDK!")
print(f"Reprojection error: {ret:.2f} pixels")
return
else:
print(f"Poor calibration (error: {ret:.2f} px). Capture more frames.")
# Display info
#cv2.putText(frame, display_text, (10, 30),
# cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
#cv2.putText(frame, f"Frames captured: {len(all_corners)}/15", (10, 60),
# cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.imshow('ChArUco Calibration', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
if __name__ == "__main__":
chaUco_Calibration(calibration_file) # Only runs when executed directly
import cv2
import cv2.aruco as aruco
import numpy as np
from robodk.robolink import *
from robodk.robomath import * # RoboDK Math functions
# Use your board's dictionary
import numpy as np
import cv2
def resize_1080p_to_720p(frame):
#return (cv2.resize(frame, (1280, 720), interpolation=cv2.INTER_LINEAR))
return frame
def align_z_up(rvec):
# Convert rotation vector to rotation matrix
R, _ = cv2.Rodrigues(rvec)
# Desired Z axis in world/camera coordinates
z_up = np.array([0, 0, 1], dtype=float)
# Keep the current X axis as close as possible to original
x_axis = R[:, 0]
# Recompute Y as cross product to keep orthogonal
y_axis = np.cross(z_up, x_axis)
y_axis /= np.linalg.norm(y_axis)
# Recompute X as orthogonal to both Y and Z
x_axis = np.cross(y_axis, z_up)
x_axis /= np.linalg.norm(x_axis)
# Build the new rotation matrix
R_new = np.column_stack((x_axis, y_axis, z_up))
# Convert back to rotation vector
rvec_new, _ = cv2.Rodrigues(R_new)
return rvec_new, R_new
def scale_camera_matrix_1080p_to_720p(mtx_1080p):
scale_x = 1280 / 1920
scale_y = 720 / 1080
mtx_720p = mtx_1080p.copy()
mtx_720p[0, 0] *= scale_x # fx
mtx_720p[1, 1] *= scale_y # fy
mtx_720p[0, 2] *= scale_x # cx
mtx_720p[1, 2] *= scale_y # cy
return mtx_1080p
board = cv2.aruco.CharucoBoard(
size=(7,5),
squareLength=0.023444,
markerLength=0.017439,
dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_50)
)
calibration_file = "C:/Users//PycharmProjects/GolfPutty/CalibrationFiles/calibration.xml"
marker_length=0.017439
RDK=Robolink()
camera= RDK.Item('Cali')
robot = RDK.Item("Doosan Robotics H2515")
fs = cv2.FileStorage(calibration_file, cv2.FILE_STORAGE_READ)
mtx = fs.getNode("Camera_Matrix").mat()
dist = fs.getNode("Distortion_Coefficients").mat()
fs.release()
Starting_pose= RDK.Item('Start')
Marker_pose= RDK.Item('markers')
robot.MoveJ(Starting_pose)
detector = aruco.CharucoDetector(board)
#zero_dist = np.zeros_like(dist)
#dist=zero_dist
while True:
bytes_img = RDK.Cam2D_Snapshot("", camera)
nparr = np.frombuffer(bytes_img, np.uint8)
frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(frame)
if marker_ids is not None:
aruco.drawDetectedMarkers(frame, marker_corners, marker_ids)
# Iterate through all detected markers
for i, marker_id in enumerate(marker_ids.flatten()):
if marker_id == 15: # The marker you care about
# Estimate pose of the marker
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
marker_corners[i], marker_length, mtx, dist
)
print("marker_length:", marker_length)
# Draw axis on that marker
cv2.drawFrameAxes(frame, mtx, dist, rvec, tvec, 0.02)
tvec, rvec = np.round(tvec, 7), np.round(rvec, 7)
# Display coordinates
x, y , z = tvec[0][0]*1000
cv2.putText(
frame,
f"Marker {marker_id}: X={x:.3f}m Y={y:.3f}m Z={z:.3f}m",
(10, 30),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(0, 255, 0),
2
)
print(f"Marker {marker_id} coordinates: {x:.3f}, {y:.3f}, {z:.3f}")
obj_points = 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]
])
imgpts, _ = cv2.projectPoints(obj_points, rvec, tvec, mtx, dist)
for pt in imgpts:
cv2.circle(frame, tuple(pt.ravel().astype(int)), 5, (0, 0, 255), -1)
cv2.imshow("Marker Pose", frame)
cv2.waitKey(5000)
#cv2.imshow("Marker Pose", frame)
# Convernt Camera to Base
Robot_Pose = robot.Pose()
R_flange = Robot_Pose.Rot33()
T_flange = np.array(Robot_Pose.Pos()) # mm
# --- Camera offset in flange coordinates ---
cam_offset_translation = np.array([0, 0,-100]) # mm from flange origin
cam_offset_rotation = np.eye(3) # adjust if tilted
# --- Flange → Camera transform ---
Pose_flange_to_cam = np.eye(4)
Pose_flange_to_cam[:3, :3] = cam_offset_rotation
Pose_flange_to_cam[:3, 3] = cam_offset_translation
# --- Base → Flange transform ---
Pose_base_to_flange = np.eye(4)
Pose_base_to_flange[:3, :3] = R_flange
Pose_base_to_flange[:3, 3] = T_flange
# --- Base → Camera transform ---
Pose_cam_in_robot = Pose_base_to_flange @ Pose_flange_to_cam
#Figure Out Object Position in Camera
# Convert ArUco to Camera Coordinates
#-------------------------------------------------
R_matrix, _ = cv2.Rodrigues(rvec)
tvec_mm = tvec.flatten()*1000 # meters → mm
rvec_aligned, R_aligned = align_z_up(rvec)
Pose_obj_in_cam = np.eye(4)
# 3. Build object pose in camera frame (4x4 matrix)
Pose_obj_in_cam[:3, :3] = R_aligned
# Returns a vector that the object is perpendicular to the camera.
Pose_obj_in_cam[:3, 3] = tvec_mm
#-------------------------------------------------
#Pose_obj_in_robot=Pose_cam_in_robot_rounded@Pose_obj_in_cam_rounded
Pose_obj_in_robot = np.matmul(Pose_cam_in_robot, Pose_obj_in_cam)
robot_pose_mat = Mat(Pose_obj_in_robot.tolist())
print("robot_pose_mat:",robot_pose_mat)
cv2.imshow("Marker Pose", frame)
robot.setSpeed(1000) # mm/s
robot.setAcceleration(1000) # mm/s²
# 🔹 Ensure the robot retains the current rotation while setting the new position
Marker_pose.setPose(robot_pose_mat)
robot.MoveJ(Marker_pose)
# Move the robot to the marker pose
robot.WaitMove()
exit()
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
