[Question] Camera calibration (eye to hand) in simulation get wrong results.

[SantiDiazC]

Hi,
I am working on a simple environment to test camera calibration (eye to hand). I used a similar approach as in this example:
https://github.com/andyzeng/visual-pushing-grasping/blob/master/calibrate.py

where I just control the robot to the grid points using the DifferentialIKController.

so basically I collect the points from a chessboard with the camera using rgb and depth images and the tcp coordinates to later make the calibration. The camera is set to convention="world".
My set up is: robot (ur5) to the origin and a

front view camera  with pos=(1.9, 0, 1.2), rot= (0,-0.24, 0, 1)

The problem is when calculating the camera transformation the translation is almost same as in the simulation

camera to world:  [ 1.87515023 -0.01142518  1.17738762]

however, the rotation matrix is wrong, it seems like there is some rotation or change in the axis:

Reference Rotation Matrix: 
 [[ 0.89107413 -0.         -0.45385779]
 [ 0.          1.         -0.        ]
 [ 0.45385779  0.          0.89107413]]

camera rotation:  [[ 0.01442082  0.99988884  0.        ]
 [ 0.49407605  0.         -0.86941025]
 [-0.86929908  0.01440935 -0.49407638]]

camera quaternion:  [ 0.61357978  0.60766878 -0.35358656  0.35949277]

which ofc yields to a completely different rotation (pointing to the sky).

So I am wondering what I am missing to make it work.

Thank you in advance

[RandomOakForest]

Thanks for posting this. It would be best if you let us know the code from which you output each matrix and vector. Camera parameters may use row-major matrices, and may not correspond to your setup for calibration. How do you obtain the "Reference Rotation Matrix" and the "camera to world" vector?

[SantiDiazC]

@RandomOakForest Thank you for your reply,

After Studied a little more I solved my problem, but I think it is important to share this for people that may have the same problem and when setting their real environment as their simulation or vice-versa (camera pose and data compatibility). I still have a question (remarked at the end of this comment)

The problem arise since the camera I defined on a Direct RL environment was defined like this:

camera = CameraCfg(
            prim_path="/World/envs/env_.*/camera",
            offset=CameraCfg.OffsetCfg(
                pos=(1.9, 0.0, 1.2), 
                rot=(0, -0.24, 0, 1), 
                convention = "world",
            ), 

We set the convention to world in the camera class config, however, in the camera class constructor it is transform to convention='opengl':

if self.cfg.spawn is not None:
            # compute the rotation offset
            rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32).unsqueeze(0)
            rot_offset = convert_camera_frame_orientation_convention(
                rot, origin=self.cfg.offset.convention, target="opengl"
            )
            .
            .
            .
            self.cfg.spawn.func(
                            self.cfg.prim_path, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=rot_offset
                        )

so when you check on the UI for the camera transform the orientation has a different quaternion than the one given in the config (it is in opengl convention).

So far this was the first issue I encounter (what is the convention for the orientation in the UI).

Now the calibration process is like this:

    while simulation_app.is_running():
        with torch.inference_mode():
            # reset
            if count % 12200 == 0:
                count = 0
                env.reset()
                console.print("-" * 80, style="info")
                console.print("[INFO]: Resetting environment...", style="info")
                calib_point_id = 0

            if count % 30 == 0:
                # move to next calibration point
                 # Find checkerboard center
                checkerboard_size = (10,6)
                refine_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
                camera_color_img = env.camera.data.output['rgb'][0].clone().cpu().numpy() #(num_envs, H, W, 4)
                camera_depth_img = env.camera.data.output['depth'][0].clone().cpu().numpy() #(num_envs, H, W, 4)
                bgr_color_data = cv2.cvtColor(camera_color_img, cv2.COLOR_RGB2BGR)
                gray_data = cv2.cvtColor(bgr_color_data, cv2.COLOR_RGB2GRAY)
                cam_intrinsics = env.camera.data.intrinsic_matrices[0].clone().cpu().numpy()
                checkerboard_found, corners = cv2.findChessboardCorners(gray_data, checkerboard_size, None, cv2.CALIB_CB_ADAPTIVE_THRESH)

                if checkerboard_found:
                    corners_refined = cv2.cornerSubPix(gray_data, corners, (3,3), (-1,-1), refine_criteria)

                    # Get observed checkerboard center 3D point in camera space
                    checkerboard_pix = np.round(corners_refined[4,0,:]).astype(int)
                    checkerboard_z = camera_depth_img[checkerboard_pix[1]][checkerboard_pix[0]]
                    console.print("checkerboard pixel z: ", checkerboard_z, style="info")
                    checkerboard_x = np.multiply(checkerboard_pix[0]-cam_intrinsics[0][2],checkerboard_z/cam_intrinsics[0][0])
                    checkerboard_y = np.multiply(checkerboard_pix[1]-cam_intrinsics[1][2],checkerboard_z/cam_intrinsics[1][1])
                    if checkerboard_z == 0:
                        continue

                    # Save calibration point and observed checkerboard center
                    observed_pts.append([checkerboard_x,checkerboard_y,checkerboard_z])
                    tool_position = calib_grid_pts[calib_point_id] # + checkerboard_offset_from_tool
                    measured_pts.append(tool_position)
                    observed_pix.append(checkerboard_pix)
                    # Draw and display the corners
                    vis = cv2.drawChessboardCorners(bgr_color_data, checkerboard_size, corners_refined, checkerboard_found)
                    # vis = cv2.drawChessboardCorners(bgr_color_data, (1,1), corners_refined[4,:,:], checkerboard_found)
                    cv2.imwrite('%06d.png' % len(measured_pts), vis)
                    cv2.imshow('Calibration',vis)
                    cv2.waitKey(10)

                # next calibration point inside the grid
                calib_point_id = (calib_point_id + 1) % calib_grid_pts.shape[0]
                
            if calib_point_id >= 100:

                measured_pts = np.asarray(measured_pts)
                observed_pts = np.asarray(observed_pts)
                observed_pix = np.asarray(observed_pix)
                world2camera = np.eye(4)
                np.save('measured_pts.npy', measured_pts)
                np.save('observed_pts.npy', observed_pts)
                np.save('observed_pix.npy', observed_pix)
                np.save('cam_intrinsics.npy', cam_intrinsics)


                
                env.close()
                simulation_app.close()
                break
            # set calibration point and move the robot using IK controller
            calib_point = calib_grid_pts[calib_point_id,:]
            joint_efforts = torch.zeros((env.robot_dof_targets.shape[0], env.robot_dof_targets.shape[1] + 1), device=env.device) # no action
            joint_efforts[:, 0:3] = torch.tensor(calib_point, device=env.device).repeat(joint_efforts.shape[0], 1)
            joint_efforts[:, 3:7] = torch.tensor([ 1, 0, 0, 0 ], device=env.device).repeat(joint_efforts.shape[0], 1)
            # step the environment
            obs, rew, terminated, truncated, info = env.step(joint_efforts)
 
            count += 1

    # close the environment
    env.close()

This is pretty much the same on the example code I shared before, but only changing the control of the UR5 robot arm to the differential IK controller in Isaaclab. The calibration process is also same as in their code.

Now the second Issue is divided in two sub issues lets say: the first one is the convention for quaternions used on isaaclab/sim and the convention used on the calibration process (in this case I used scipy to obtain the quaternion from the matrix obtained in the calibration). isaaclab uses (w, x, y, z) convention and scipy by default uses (x, y, z , w).

The second sub issue is that the camera calibration uses opencv to obtain the "observed points" and "observed pixel points". Opencv has a orientation convention similar to the OpenGL convention but rotated 180 degrees along the x axis:

So the orientation quaternion obtained in the calibration process has to be rotated 180 on the x axis to go back to the OpenGL convention.

So in summary if we set the calibration process like this, and taking into account all the transformations and changes in convention along the process, to obtain the same orientation quaternion we set on the config of the camera we need to do the following:

calibration matrix -> orientation quaternion (opencv convention) -> change xyzw to wxyz convention-> apply 180 on x to convert to opengl convention -> convert from opengl to world convention

In code It would be something like this:

from scipy.spatial.transform import Rotation as R
from omni.isaac.lab.utils.math import (
    convert_camera_frame_orientation_convention,
    convert_quat,
    euler_xyz_from_quat,
    quat_from_euler_xyz,
    quat_mul,
)

# obtain camera to world matrix and the respective quaternion from calibration
camera_pose = np.linalg.inv(world2camera) # world2camera is the calibration matrix obtained using the code above (link)
R_matrix = camera_pose[:3, :3]
t = camera_pose[:3, 3]
R_matrix = np.where(np.abs(R_matrix) < 1e-2, 0, R_matrix) # to reduce the precision (easier to check)
r = R.from_matrix(R_matrix)
quat = r.as_quat()

quat = convert_quat(quat) # from xyzw convention to wxyz (isaac)

quat_world = convert_camera_frame_orientation_convention(
    quat,
    origin="opengl",  # camera output (OpenGL camera frame)
    target="world"      # expected by Isaac Lab world frame
) # convert opengl to world convention

roll_correction = quat_from_euler_xyz(torch.tensor([-np.pi], dtype=torch.float32),
                                       torch.tensor([0.0], dtype=torch.float32), 
                                       torch.tensor([0.0], dtype=torch.float32))

quat_world = quat_mul(quat_world, roll_correction) #apply 180 degrees rotation in x
console.print("quat: ", quat, style="info")

console.print("quat_world: ", quat_world, style="info") #check if match with the camera config

that returns:

quat_world:  tensor([[-6.6433e-07, -2.4472e-01,  2.6813e-06,  9.6959e-01]])

Which is the same as the one defined in the camera config class previously.

I have to remark that the 180 rotation on x was done after changing the convention from opengl2world, if you want to do it before it has to be around the z axis. I am still not sure what it is like that, but I am guessing it is because the measured points (tool from the robot) are in world convention, but I am not completly sure about it. If someone knows please let me know.

Also If you consider I can share the full code (including the Direct RL envirionment and UR5 with the chessboard asset) later as a tutorial or a contribution to the project examples, since I think it is useful to people on how to work with the different conventions for rotation in Isaaclab and how to use it to connect your simulation and real environment sensors.

[HariKrishnan06082k]

A tutorial would be nice if not at least the code. Thanks for the explanation I was having similar problem when I tried it earlier.

[RandomOakForest]

Thanks for following up. This is a good example. I'll move the post to our Discussions under show and tell. Great work!