Hi @becketps
I mean that AprilTag detection uses Computer Vision processing to estimate the distance from the target, not the stereo depth information.
I recommend you read the documentation of the Isaac ROS April Tag detector to learn more about how it works.
Yes, you can, but it’s not as precise as the AprilTag detection, which is based on Computer Vision processing.
Hi,
you are totally right. I tested it today but there I got exactly your distance problem. I already tried to correct it like this with that:
But it was wrong. I got again an error of 1cm. In x and y it depends now sometimes under 8mm and till 1cm in x and y. The calibration is now ok same values with 40 images.
It varries and the tag is relative to the base also not totally constant estimated. I think it has to do with the point clound. 10mm difference also there the case in one x, y or z.
if self._depth_img is not None and self._cam_info is not None:
try:
tf_cam_tag = self.tf_buffer.lookup_transform(
'zed2i_left_camera_frame_optical', self.tag_frame,
rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1))
tx = tf_cam_tag.transform.translation.x
ty = tf_cam_tag.transform.translation.y
tz = tf_cam_tag.transform.translation.z
fx = self._cam_info.k[0]; fy = self._cam_info.k[4]
cx = self._cam_info.k[2]; cy = self._cam_info.k[5]
px = int(tx / tz * fx + cx)
py = int(ty / tz * fy + cy)
h, w = self._depth_img.shape
px = max(15, min(px, w - 15))
py = max(15, min(py, h - 15))
patch = self._depth_img[py-15:py+15, px-15:px+15]
valid = patch[np.isfinite(patch) & (patch > 0.1) & (patch < 3.0)]
if len(valid) > 10:
depth_m = float(np.median(valid))
z_error = depth_m - tz
if abs(z_error) < 0.04:
approach_dir = T_base_tag[:3, 2] / np.linalg.norm(T_base_tag[:3, 2])
T_base_tag[:3, 3] += z_error * approach_dir
self.get_logger().info(
f'Depth-Korrektur: {z_error*1000:+.1f}mm',
throttle_duration_sec=2.0)
else:
self.get_logger().warn(
f'Depth-Korrektur zu groß ({z_error*1000:+.1f}mm) – ignoriert',
throttle_duration_sec=2.0)
except Exception as e:
self.get_logger().warn(f'Depth-Korrektur fehlgeschlagen: {e}',
throttle_duration_sec=5.0)
Hi @becketps,
Thanks for sharing the snippet. I think the depth correction step you added is unfortunately working against you, and that’s why the residual error is not going away.
approach_dir = T_base_tag[:3, 2] / np.linalg.norm(T_base_tag[:3, 2]) T_base_tag[:3, 3] += z_error * approach_dir
The AprilTag pose returned by Isaac ROS is a visual PnP estimate expressed in the left optical frame (REP-103, OpenCV convention).
The error you observe at 70 cm is mostly lateral (X/Y in optical frame), not range. Projecting the depth residual along the tag’s approach axis in the base frame mixes three different reference frames and ends up moving X and Y of the final pose as a side effect of a Z-only depth measurement. If you really want to fuse the ZED depth, do it as a scalar update on t_optical[2] before the chain to base, with inverse-variance weighting (PnP variance grows linearly with range, ZED depth variance grows quadratically — so depth helps mostly close-up and should be down-weighted further out). Don’t touch X and Y from depth.
Two more things worth checking before you blame the algorithm:
Σ qq^T with the largest eigenvalue) for the rotation part, and per-axis median + MAD outlier rejection for the translation part.For the realistic accuracy you can expect: with the factory calibration of the ZED2i, an 8 cm AprilTag rigidly mounted on a flat board, a calipered tag size, and proper SE(3) averaging, ~3 mm 1-σ lateral at 70 cm is a reasonable ceiling. If you need better, the next lever is a larger tag (10–12 cm), not a different camera or recalibration.
One last thing on the calibration frame, just to be sure: since Isaac ROS AprilTag publishes in zed2i_left_camera_optical_frame, the hand-eye calibration must have been solved against that same frame. If by mistake the runtime reads tag poses in optical frame but the hand-eye matrix was solved against zed2i_left_camera_frame (or vice-versa), you get exactly the kind of stable few-cm offset you reported, because the two frames differ by a fixed ~90°.
Let me know how it goes after these checks.
I checked that so with the tag size everything is ok. I did it like you said there but still in y direction 8mm depending on the run. X is ok and z also sometimes vary
if self.get_parameter('depth_fusion').value \
and self._depth_img is not None \
and self._cam_info is not None:
try:
tf_cam_tag = self.tf_buffer.lookup_transform(
'zed2i_left_camera_frame_optical', self.tag_frame,
rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1))
tx = tf_cam_tag.transform.translation.x
ty = tf_cam_tag.transform.translation.y
tz = tf_cam_tag.transform.translation.z # PnP-Range
fx = self._cam_info.k[0]; fy = self._cam_info.k[4]
cx = self._cam_info.k[2]; cy = self._cam_info.k[5]
px = int(tx / tz * fx + cx)
py = int(ty / tz * fy + cy)
h, w = self._depth_img.shape
px = max(15, min(px, w - 15))
py = max(15, min(py, h - 15))
patch = self._depth_img[py-15:py+15, px-15:px+15]
valid = patch[np.isfinite(patch) & (patch > 0.1) & (patch < 3.0)]
if len(valid) > 10:
depth_m = float(np.median(valid))
if self.get_parameter('depth_fusion_weighted').value:
# Inverse-Variance-Gewichtung
sigma_pnp = 0.01 * tz
sigma_depth = 0.002 * tz**2
w_pnp = 1.0 / sigma_pnp**2
w_depth = 1.0 / sigma_depth**2
z_fused = (w_pnp * tz + w_depth * depth_m) / (w_pnp + w_depth)
z_error = z_fused - tz
else:
# Direktes Update (kein Gewichten)
z_error = depth_m - tz
if abs(z_error) < 0.04:
# ✅ Nur Z im optischen Frame korrigieren, X/Y bleiben unangetastet
tf_cam_tag.transform.translation.z += z_error
# Korrigiertes cam→tag in base→tag umrechnen
T_cam_tag_corr = tf_to_matrix(tf_cam_tag)
tf_base_cam = self.tf_buffer.lookup_transform(
self.base_frame,
'zed2i_left_camera_frame_optical',
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=0.1))
T_base_cam = tf_to_matrix(tf_base_cam)
T_base_tag = T_base_cam @ T_cam_tag_corr
mode = "gewichtet" if self.get_parameter('depth_fusion_weighted').value \
else "direkt"
self.get_logger().info(
f'Depth-Korrektur ({mode}): {z_error*1000:+.1f}mm',
throttle_duration_sec=2.0)
else:
self.get_logger().warn(
f'Depth-Korrektur zu groß ({z_error*1000:+.1f}mm) – ignoriert',
throttle_duration_sec=2.0)
except Exception as e:
self.get_logger().warn(
f'Depth-Korrektur fehlgeschlagen: {e}',
throttle_duration_sec=5.0)
Hi @becketps,
The code itself now looks right; Z is updated in the optical frame and the chain to base is rebuilt cleanly. But before tuning the fusion any further, I think you’re chasing the wrong axis.
Depth fusion cannot fix a lateral (Y) error.
The tf_cam_tag.transform.translation.z += z_error step only moves the pose along the camera optical axis. It mathematically cannot reduce a residual in optical-frame X or Y. So the ~8 mm you still see in Y is not a depth problem, and no amount of variance tuning will touch it.
The fact that Z now “sometimes varies” after enabling fusion is also expected: you’ve replaced a stable PnP range with a depth sample taken on a flat low-texture white surface, where neural stereo interpolates from surrounding context. You’ve effectively traded a small bias for stochastic noise. If PnP Z was fine before, I’d disable depth fusion, or at least make σ_depth much more realistic;0.002 * tz² (≈1 mm at 70 cm) is far too optimistic for a flat textureless target; 0.01 * tz² is closer to reality.
Where a stable Y offset at 70 cm actually comes from
In order of likelihood:
wpg_tcp). A constant offset between the URDF TCP and the actual physical tool point produces exactly this kind of fixed lateral bias. Worth re-checking the gripper offsets from the UR15 flange.Clean isolation test
To separate hand-eye from PnP: fix the tag in the workspace, then visually servo the TCP in image space until the projected tag center lands exactly on (cx, cy). Measure the physical offset between TCP and the tag center with a ruler. If you get ~8 mm in the same direction as before, the issue is with calibration /TCP definition, not the detector.
Let me know what the solver reports for the angular residual and what the margin measurement on the printed tag looks like.
I have printed myself a calibration pin which I put on the robot flange. Also the Calibration on the robot tells me that it is 280mm in front of the Flange. Thats also my real TCP. Therefore the TCP is right. TCP is oriented in z direction to approch the marker and the x and y in lateral way.
One question to point 2:
Why does the white space play a role when the tag has only the origin there? I have a cut machine and can make it similar. I already did like you see.
That here is my setup. Are you sure that it makes sense to measure here anything from the TCP till the tag? The TCP ist right adjusted 280mm in front of flange
When I see the tag in the Middle where x and y counts with reference to robot ur15 base frame
When I place the TCP in the Middle of the marker:
Then Second test after your Hint 1 2 3
ros2 run tf2_ros tf2_echo ur15_base wpg_tcp
When the Tag middle is in the picture
At time 1778534018.811993699
When the TCP is in the middle on the tag:
It still now results in 8mm to the right and 2mm to high.
Here you said that the calibration has to be performed against the left optical frame. However, earlier the recommendation was to use the frame that is specified in the URDF. How do these two statements fit together?
The camera_link frame always has to be used in the URDF, because that is the physical frame where the camera is mounted to the robot. The left_camera_frame_optical cannot be assigned a separate pose in the URDF, since it is an internal frame that is already defined relative to the camera frame by the ZED driver.
So during hand-eye calibration with a ZED camera in ROS 2, the URDF joint should indeed use zed2i_camera_link as the child link, not zed2i_left_camera_frame_optical.
Why?
The ZED SDK publishes its own internal TF tree starting at zed2i_camera_link. The optical frame zed2i_left_camera_frame_optical is a child of that. If you set zed2i_left_camera_frame_optical as the URDF joint child, zed2i_camera_link becomes a second root link with no parent, which causes MoveIt to crash with:
Two root links found: [world] and [zed2i_camera_link]
Correct approach:
calibrateHandEye gives you T_tcp_optical (transform from TCP to optical frame)T_tcp_camera_link using the ZED-internal transform: T_tcp_camera_link = T_tcp_optical @ inv(T_camera_link_optical)
where T_camera_link_optical comes from:
bash
ros2 run tf2_ros tf2_echo zed2i_camera_link zed2i_left_camera_frame_optical
T_tcp_camera_link values go into the URDF:xml
<joint name="zed2i_calibration_joint" type="fixed">
<parent link="wpg_tcp"/>
<child link="zed2i_camera_link"/>
<origin xyz="x y z" rpy="r p y"/>
</joint>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_gripper">
<xacro:include filename="$(find wpg_description)/urdf/weiss_gripper.xacro"/>
<xacro:include filename="$(find wpg_description)/urdf/zed2i_camera.xacro"/>
<!-- Args immer auf Top-Level! -->
<xacro:arg name="standalone" default="false"/>
<xacro:arg name="camera_model" default="zed2i"/>
<xacro:arg name="tf_prefix" default="wpg_"/>
<xacro:arg name="quickchanger" default="true"/>
<xacro:arg name="sensor" default="false"/>
<xacro:arg name="finger_kit" default="120"/>
<!-- MACRO Version -->
<!-- <xacro:macro name="weiss_gripper_with_camera" params="parent_link tf_prefix camera_model:=none">
<xacro:weiss_gripper parent_link="${parent_link}" tf_prefix="${tf_prefix}"/> -->
<xacro:macro name="weiss_gripper_with_camera" params="
parent_link
tf_prefix
camera_model:=none
quickchanger:=true
sensor:=false
finger_kit:=120
">
<!-- ← Das fehlte! -->
<xacro:weiss_gripper
parent_link="${parent_link}"
tf_prefix="${tf_prefix}"
quickchanger="${quickchanger}"
sensor="${sensor}"
finger_kit="${finger_kit}"/>
<xacro:if value="${camera_model == 'zed2i'}">
<!-- <xacro:zed2i_mount parent_link="${tf_prefix}flange_mount"/> -->
<xacro:zed2i_mount parent_link="${tf_prefix}tcp"/>
</xacro:if>
</xacro:macro>
</robot>
And here for ZED Camera
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find zed_wrapper)/urdf/zed_macro.urdf.xacro" />
<xacro:macro name="zed2i_mount" params="parent_link">
<xacro:property name="cam_x" value="-0.003622"/>
<xacro:property name="cam_y" value="0.153387"/>
<xacro:property name="cam_z" value="-0.209547"/>
<xacro:property name="cam_roll" value="2.864207"/>
<xacro:property name="cam_pitch" value="-1.531465"/>
<xacro:property name="cam_yaw" value="1.868415"/>
<xacro:zed_camera name="zed2i" model="zed2i" enable_gnss="false" custom_baseline="0">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:zed_camera>
<!-- Parent ist wpg_tcp – Kalibrierung wurde von wpg_tcp aus gemessen -->
<joint name="zed2i_calibration_joint" type="fixed">
<parent link="wpg_tcp"/>
<!-- <child link="zed2i_left_camera_frame_optical"/> -->
<child link="zed2i_camera_link"/>
<origin xyz="${cam_x} ${cam_y} ${cam_z}" rpy="${cam_roll} ${cam_pitch} ${cam_yaw}"/>
</joint>
</xacro:macro>
</robot>
Hi @becketps,
Yes, your reasoning is correct, and the two recommendations are fully consistent, they just live at different layers.
What’s actually happening
calibrateHandEye() returns a transform in whatever camera frame produced the marker observations. Since Isaac ROS AprilTag publishes poses in zed2i_left_camera_frame_optical, the OpenCV solver gives you T_tcp_optical. That’s the “physically meaningful” calibration result; it’s the one that, when chained with the AprilTag detector output, gives consistent geometry.
The URDF is a separate concern: it describes the physical mounting of the camera, and the only rigid link the ZED driver allows you to attach to is zed2i_camera_link. The driver itself publishes the internal sub-tree camera_link → left_camera_frame → left_camera_optical_frame as static transforms (defined by zed_macro.urdf.xacro). You cannot, and should not, try to override those.
So your conversion is exactly right:
T_tcp_camera_link = T_tcp_optical · inv(T_camera_link_optical)
where T_camera_link_optical is the static transform published by the ZED driver and read with:
ros2 run tf2_ros tf2_echo zed2i_camera_link zed2i_left_camera_frame_optical
That value is constant per model, for the ZED 2i it’s essentially a fixed ~−90° pitch plus a small lateral offset to the left optical center, so you only need to compute it once.
A small sanity check
After you publish your URDF with the converted T_tcp_camera_link, verify the round-trip:
ros2 run tf2_ros tf2_echo wpg_tcp zed2i_left_camera_frame_optical
That should reproduce, within numerical noise, the original T_tcp_optical returned by calibrateHandEye(). If it doesn’t, the conversion has a sign or order issue somewhere.
Closing the loop on the residual error
Coming back to your previous test (post #26), 8 mm to the right and 2 mm too high after all of this is consistent and stable in one direction, which is exactly the signature of an angular residual in the hand-eye calibration, not a frame-conversion mistake. A 0.6–0.7° error around the optical axis projects to ~8 mm at 70 cm, right in the range you’re seeing.
The question I asked in #25 that’s still open: what’s the angular residual reported by the hand-eye solver, and how diverse were the 40 calibration poses in rotation? If they were mostly translations of the TCP with similar orientations, the rotation part of the calibration is essentially underdetermined and you can have several tenths of a degree of error with a very small reprojection number. Re-running with poses that include ≥30° rotations around each TCP axis usually closes the lateral residual.
If you can share the solver’s per-axis residual breakdown (or even just the angular RMS), we can confirm this is where the remaining 8 mm comes from.
ros2 run tf2_ros tf2_echo wpg_tcp zed2i_left_camera_frame_optical
[INFO] [1778619611.648021214] [tf2_echo]: Waiting for transform wpg_tcp → zed2i_left_camera_frame_optical: Invalid frame ID “wpg_tcp” passed to canTransform argument target_frame - frame does not exist
At time 0.0
may function estimate
def compute_angular_residual(self, results):
"""
Berechnet den Winkel-Residual der Hand-Eye Kalibrierung.
Laut Support: >0.6° → lateraler Fehler von ~7mm bei 70cm
Formel: für jedes Pose-Paar prüfen ob A·X = X·B gilt
Residual = mittlere Winkelabweichung in Grad
"""
self.get_logger().info('── Winkel-Residual Analyse ──────────────────')
for method_name, T_x in results:
R_x = T_x[:3, :3]
t_x = T_x[:3, 3]
angle_errors = []
trans_errors = []
n = len(self.robot_poses)
for i in range(n):
for j in range(i + 1, n):
# A = relative Gripper-Bewegung
T_a = np.linalg.inv(self.robot_poses[i]) @ self.robot_poses[j]
# B = relative Kamera-Bewegung
T_b = np.linalg.inv(self.camera_poses[i]) @ self.camera_poses[j]
# ── NEU: Paare mit zu großer Rotation überspringen ──
rel_angle = np.degrees(
Rotation.from_matrix(T_a[:3,:3]).magnitude())
if rel_angle > 90.0:
continue
# Prüfe: R_a · R_x = R_x · R_b
lhs = T_a[:3, :3] @ R_x
rhs = R_x @ T_b[:3, :3]
# Rotationsfehler als Winkel
R_err = lhs @ rhs.T
# Winkel aus Rotationsmatrix
trace = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
angle_deg = np.degrees(np.arccos(trace))
angle_errors.append(angle_deg)
# Translationsfehler
lhs_t = T_a[:3, :3] @ t_x + T_a[:3, 3]
rhs_t = R_x @ T_b[:3, 3] + t_x
trans_errors.append(np.linalg.norm(lhs_t - rhs_t) * 1000)
if not angle_errors:
self.get_logger().warn(
f' [{method_name}] Keine Pose-Paare unter 90° – '
f'zu viel Rotation in den Samples!')
continue
mean_angle = np.mean(angle_errors)
max_angle = np.max(angle_errors)
mean_trans = np.mean(trans_errors)
# Bewertung laut Support
if mean_angle < 0.3:
quality = '✓ sehr gut'
elif mean_angle < 0.6:
quality = '~ ok'
else:
quality = '✗ zu groß → Kalibrierung wiederholen!'
# Lateraler Fehler bei 70cm abschätzen
lateral_mm = np.tan(np.radians(mean_angle)) * 700
self.get_logger().info(f' [{method_name}]')
self.get_logger().info(f' Winkel-Residual: {mean_angle:.3f}° (max {max_angle:.3f}°) {quality}')
self.get_logger().info(f' Trans-Residual: {mean_trans:.2f}mm')
self.get_logger().info(f' → Lateraler Fehler bei 70cm: ~{lateral_mm:.1f}mm')
self.get_logger().info('─────────────────────────────────────────────')
# Warnung wenn Rotationsdiversität zu gering
self._check_rotation_diversity()
2b
I ran another calibration with 40 samples and better rotational diversity. The translation result is again very stable and consistent with previous runs:
xyz = [-0.0064, 0.1556, -0.2072]
rpy = [2.9489, -1.5345, 1.7910] rad
Reprojection error: 0.0213px (excellent)
However the angular residual formula still outputs ~95–101° for all methods:
[Park] Angular residual: 101.322° (max 178.852°)
[Horaud] Angular residual: 101.322° (max 178.851°)
[Tsai-Lenz] Angular residual: 95.667° (max 175.183°)
[Andreff] Angular residual: 101.322° (max 178.851°)
[Daniilidis] Angular residual: 101.323° (max 178.851°)
Rotation diversity:
Max rotation between poses: 179.4°
Mean rotation between poses: 70.8°
My question: The translation converges stably across 4+ independent calibration runs to essentially the same value. The reprojection error is 0.0213px. But the angular residual formula always returns ~95–101° with max ~179°, which seems physically impossible.
Is there a bug in my formula? Or is the 179.4° max rotation between poses the actual problem — causing a 180° flip ambiguity that breaks the residual calculation entirely?
Please tell me if something is missing.
I have now in x direction to the side 6mm and in y direction 4mm difference.
I found and fixed a bug in my angular residual formula. The B matrix was computed in the wrong direction:
python# Wrong (old):
T_b = np.linalg.inv(camera_poses[i]) @ camera_poses[j]
T_b = camera_poses[i] @ np.linalg.inv(camera_poses[j])
After the fix, the residual now shows physically meaningful values:
[Park] Angular residual: 0.424° → estimated lateral error ~5.2mm at 70cm
[Horaud] Angular residual: 0.423° → estimated lateral error ~5.2mm
Sanity check after URDF update:
ros2 run tf2_ros tf2_echo wpg_tcp zed2i_left_camera_frame_optical
Translation: [0.055, 0.169, -0.216]
RPY (deg): [-0.027, 0.385, -178.754]
This matches the calibrated T_tcp_optical closely. The round-trip conversion is correct.
Remaining lateral error is 6mm in X and 4mm
. Is further improvement possible or is this within the expected accuracy for this setup?