@toni.sm Thanks for replying. The camera_pose
is from
def get_camera_pose(self):
"""Get camera pose.
Returns:
camera_pose: 7d array
"""
stage = omni.usd.get_context().get_stage()
prim = stage.GetPrimAtPath(self._viewport_window.get_active_camera())
pose = omni.usd.get_world_transform_matrix(prim)
rot_mat = np.array(pose.ExtractRotationMatrix())
trans = np.array(pose.ExtractTranslation())
camera_pose_matrix = np.zeros([4,4])
camera_pose_matrix[:3,:3] = rot_mat
camera_pose_matrix[:3,-1] = trans
camera_pose_matrix[-1,-1] = 1
# rotate 180 degree in x axis for convention
camera_pose = np.dot(
camera_pose_matrix,
pose_to_4x4(np.array([0.0, 0.0, 0.0, 180.0 / 180.0 * np.pi, 0.0, 0.0])),
)
camera_pose[:3,-1] = camera_pose[:3,-1] * get_stage_units()
return camera_pose
And the camK
is from
def get_camK(self, flag="mat"):
"""Get camera intrinsics in for mat 3x3 matrix or list of size 4.
Args:
flag: by default return 3x3 matrix, if set to list, return size-4 list, if set to
raw, return raw realsense data.
Returns:
camera intrinsic.
"""
camera_params = self._sd_helper.get_camera_params(self._viewport_window)
near, far = camera_params["clipping_range"]
width = self.config.width
height = self.config.height
horiz_aperture = camera_params["horizontal_aperture"]
vert_aperture = horiz_aperture * height / width
focal_length = camera_params["focal_length"]
stage = omni.usd.get_context().get_stage()
prim = stage.GetPrimAtPath(self._viewport_window.get_active_camera())
focal_length = prim.GetAttribute("focalLength").Get()
horizontal_aperture = prim.GetAttribute("horizontalAperture").Get()
vertical_aperture = prim.GetAttribute("verticalAperture").Get()
width, height = self._viewport_window.get_texture_resolution()
fx = width * focal_length / horizontal_aperture
fy = height * focal_length / vertical_aperture
cx = width * 0.5
cy = height * 0.5
print("real camK:", np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]]))
camK = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
return camK
The kp_poses
are fetched from
kp_poses = []
for i in range(4):
kp_poses.append(self.get_object_pose("/waterproof/Sphere_0{}".format(i)))
def get_object_pose(self, prim_path):
"""Get object pose.
Args:
prim_path: prim path of object
Returns:
pose: 7-dim array of position and orientation quarternion
"""
prim = get_prim_at_path(prim_path)
xprim = XFormPrim(prim.GetPath())
position, orientation = xprim.get_world_pose()
position *= get_stage_units()
pose = np.concatenate((position, orientation))
return pose