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()
Where did the squareLength and markerLength values come from?
squareLength=0.023444, markerLength=0.017439,
I’m having a hard time relating them to numbers that “make sense” and their ratio also doesn’t appear to be a tidy number, so I wonder if this might be at least part of the problem.
Can you share the input images to the calibration process?
Also I’ don’t thing your scale_camera_matrix_1080p_to_720p function isn’t quite right.
See this link: matlab - How does resizing an image affect the intrinsic camera matrix? - Signal Processing Stack Exchange
I think what you want is something like:
Cx = S*(Cx + 0.5) - 0.5;
Cy = S*(Cy + 0.5) - 0.5;
I would recommend trying to get it working without any scaling to start with. Once that works, you can introduce the extra complexity of scaling.
Hi Steve,
I am currently not using the scaling in the code yet. I was experimenting.
The square is measured in the robodk software. When importing a .png it doesn’t stay a consistent 20mm/15mm ratio.
Code for calibration
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.037834,
markerLength=0.028386,
dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
)
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/skha/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/skha/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
I don’t know anything about the robodk software you are using, so I’m missing a lot of context. I also don’t understand how the ratio would be changed by saving it as a PNG.
If I were in your shoes, I’d be using whatever lengths for the square / marker were specified when generating the ChAruco target. If you aren’t able to get good calibration results when using those values, stop and try to figure out why.
I’d also reproject the world points (the 3D positions of the chessboard corners) to each of your calibration input images (using the calibrated camera matrix and intrisics, along with the pose / extrinsics recovered for the corresponding image). Draw circles (using the cv::circle function) to the image for each of the projected points. I would use the subpixel drawing capability for best results. (ask if you don’t know how to do this)
You only showed 2 calibration input images. Your code has a reference to 16 as the number of images it could use, are you actually using 16 images? Are the images varied in position and orientation, or are a lot of them similar to each other?
when importing the image as png into rododk it converts the current pixel into mm. So if a 7x5 board square 30mm and marker 20 mm can vary drastically depending on the dpi of the image.
Yes, I am using all 16 pictures. I wasn’t sure if you wanted to see all 16. I am covering the entire FOV and at different angles up to 45 deg.
I felt like puzzling.
>>> 0.023444 * 1280
30.008319999999998
>>> 30 / 1280
0.0234375
markerLength remains a puzzle to me.
>>> 0.017439 / 0.023444
0.7438577034635727
**
Reprojection error: 0.05 pixels**
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=(5,7),
squareLength=0.035,
markerLength=0.021,
dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_100)
)
picture_range=17
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/skha/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/skha/PycharmProjects/GolfPutty/CalibrationFiles/{i}.png", frame)
if image_size is None:
image_size = frame.shape\[1\], frame.shape\[0\]
# Detection
frame = 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 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
board = cv2.aruco.CharucoBoard(
size=(5,7),
squareLength=0.035,
markerLength=0.021,
dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_100)
)
calibration_file = "C:/Users/skha/PycharmProjects/GolfPutty/CalibrationFiles/calibration.xml"
marker_length=.021
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 == 0: # The marker you care about
# Estimate pose of the marker
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
marker_corners[i], marker_length, mtx, dist
)
rvec, tvec = np.round(rvec, decimals=4),np.round(tvec, decimals=4)
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]
])
obj_pts = np.array([[-1, 1, 0], [1, 1, 0], [1, -1, 0], [-1, -1, 0]]) * (marker_length / 2)
img_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, mtx, dist)
# Draw reprojected points (should overlap with detected corners)
for pt in img_pts.squeeze():
cv2.circle(frame, tuple(pt.astype(int)), 5, (0, 255, 0), -1)
imgpts, _ = cv2.projectPoints(obj_points, rvec, tvec, mtx, dist)
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,50]) # 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
Pose_cam_in_robot = np.round(Pose_cam_in_robot,5)
print('Pose_cam_in_robot:',Pose_cam_in_robot)
#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()
Looking at the first image (orthogonal view), it looks like your coordinate axes are being drawn pretty close to the center of the Aruco marker (definitely not as far off as the tip of the arrow is in the second image) so my guess is the Aruco stuff is working properly, but something isn’t right with the transforms you are computing or how you are applying them. There is a log going on in the code, so I’d suggest removing as much of it as you can to start with a simple case and incrementally add things in. For example, can you start with a basic transform that has only translation, or maybe something with rotation around the Z axis only.
Without really digging in, my guess (and it’s nothing more than that!) is that you aren’t adjusting the translation vector correctly when you apply your rotations (for example when you apply the result of align_z_up())…If you can do some controlled tests and see that your error increases as you increase the rotation angle, for example, it might point you to the source of the error.
Good luck!