I have been using Isaac Sim for camera simulation and encountered an interesting issue regarding the projection of 3D points onto the image plane using the intrinsic matrix (K matrix).
Problem Description: When attempting to project 3D points (specifically the edges of an ArUco cube) onto the image plane using the provided K matrix, I observed a discrepancy in the Y-direction. The X-direction projections appear accurate, but the Y-direction projections seem to be incorrect.
Investigation and Temporary Solution: Upon further investigation, I found that modifying the K matrix by setting fy equal to fx (fy = fx) resolves the issue, resulting in accurate projections in both X and Y directions.
**Code Snippet **
import carb
import omni
import numpy as np
from omni.isaac.sensor import Camera
import omni.isaac.core.utils.numpy as np_utils
import omni.graph.core as og
import omni.replicator.core as rep
import omni.syntheticdata._syntheticdata as sd
class IsaacSimCamera:
def __init__(self, camera_details: dict = None):
self._camera_details = camera_details
self._render_product_path = None
self.U_I_TRANSFORM = np.array([[0, -1, 0, 0], [0, 0, 1, 0], [-1, 0, 0, 0], [0, 0, 0, 1]])
self.init_camera()
def init_camera(self):
self._camera_details['orientation'] = np_utils.convert(self._camera_details['orientation'])
world_i_cam_i_R = np_utils.quats_to_rot_matrices(self._camera_details['orientation'])
i_u_R = np_utils.create_tensor_from_list(
self.U_I_TRANSFORM[:3, :3].tolist(), dtype="float32"
)
self._camera_details['orientation'] = np_utils.rot_matrices_to_quats(np_utils.matmul(world_i_cam_i_R, i_u_R))
position = self._camera_details['position']
position[2] = position[2]+100
self.camera = Camera(
prim_path=self._camera_details['prim_path'],
position=position,
frequency=self._camera_details['frequency'],
resolution=self._camera_details['resolution'],
orientation=self._camera_details['orientation']
)
self.camera.set_focal_length(self._camera_details['focalLength']/10)
self.camera.set_clipping_range(self._camera_details['clippingRange'][0]/10,self._camera_details['clippingRange'][1]/10)
self.camera.set_focus_distance(self._camera_details['focusDistance']/10)
self.camera.set_horizontal_aperture(self._camera_details['horizontalAperture']/10)
self.camera.set_vertical_aperture(self._camera_details['verticalAperture']/10)
print("Intrinsic matrix of Camera: {} : {}".format(self._camera_details["prim_path"],self.camera.get_intrinsics_matrix()))
self._render_product_path = self.camera._render_product_path
def publish_camera_info(self, freq):
render_product = self._render_product_path
step_size = int(60 / freq)
topic_name = self._camera_details['prim_path'] + "_camera_info"
queue_size = 1
node_namespace = ""
frame_id = self._camera_details['prim_path'].split("/")[-1]
stereo_offset = [0.0, 0.0]
writer = rep.writers.get("ROS2PublishCameraInfo")
writer.initialize(
frameId=frame_id,
nodeNamespace=node_namespace,
queueSize=queue_size,
topicName=topic_name,
stereoOffset=stereo_offset,
)
writer.attach([render_product])
gate_path = omni.syntheticdata.SyntheticData._get_node_path(
"PostProcessDispatch" + "IsaacSimulationGate", render_product
)
og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
def publish_pointcloud_from_depth(self, freq):
render_product = self._render_product_path
step_size = int(60 / freq)
topic_name = self._camera_details['prim_path'] + "_pointcloud"
queue_size = 1
node_namespace = ""
frame_id = self._camera_details['prim_path'].split("/")[-1]
rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(
sd.SensorType.DistanceToImagePlane.name
)
writer = rep.writers.get(rv + "ROS2PublishPointCloud")
writer.initialize(
frameId=frame_id,
nodeNamespace=node_namespace,
queueSize=queue_size,
topicName=topic_name
)
writer.attach([render_product])
gate_path = omni.syntheticdata.SyntheticData._get_node_path(
rv + "IsaacSimulationGate", render_product
)
og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
def publish_rgb(self, freq):
render_product = self._render_product_path
step_size = int(60 / freq)
topic_name = self._camera_details['prim_path'] + "_rgb"
queue_size = 1
node_namespace = ""
frame_id = self._camera_details['prim_path'].split("/")[-1]
rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(sd.SensorType.Rgb.name)
writer = rep.writers.get(rv + "ROS2PublishImage")
writer.initialize(
frameId=frame_id,
nodeNamespace=node_namespace,
queueSize=queue_size,
topicName=topic_name
)
writer.attach([render_product])
gate_path = omni.syntheticdata.SyntheticData._get_node_path(
rv + "IsaacSimulationGate", render_product
)
og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
def publish_depth(self, freq):
render_product = self._render_product_path
step_size = int(60 / freq)
topic_name = self._camera_details['prim_path'] + "_depth"
queue_size = 1
node_namespace = ""
frame_id = self._camera_details['prim_path'].split("/")[-1]
rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(
sd.SensorType.DistanceToImagePlane.name
)
writer = rep.writers.get(rv + "ROS2PublishImage")
writer.initialize(
frameId=frame_id,
nodeNamespace=node_namespace,
queueSize=queue_size,
topicName=topic_name
)
writer.attach([render_product])
gate_path = omni.syntheticdata.SyntheticData._get_node_path(
rv + "IsaacSimulationGate", render_product
)
og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
pass
This provides me the camera matrix K as
[[1662.76, 0.0, 960.0],
[0.0, 1424.19, 600.0],
[0.0, 0.0, 1.0]]
and then using the following code I am trying to project the points onto the camera
import math
import cv2
image = cv2.imread("/content/saved_image (1).jpg")
three_d_locations_pos = [[-5, 5, 45],
[5, 5, 45],
[5, -5, 45],
[-5, -5, 45],
]
obj_points = np.array(three_d_locations_pos, dtype=np.float32)
rvec = np.array([math.pi, 0, 0], dtype=np.float32)
tvec = np.zeros((3,), dtype=np.float32)
camera_matrix = np.array([[1662.76, 0.0, 960.0],
[0.0, 1424.19, 600.0],
[0.0, 0.0, 1.0]], dtype=np.float32)
img_points_reprojected, _ = cv2.projectPoints(obj_points, rvec, tvec, camera_matrix, None)
# print(img_points_reprojected)
plt.scatter(img_points_reprojected[:, 0, 0], img_points_reprojected[:, 0, 1], color='yellow', s=50, label='Reprojected Points')
plt.imshow(image)
From this I get this image
But the projections look fine when I am using fy = fx in the K matrix.
This means that the K matrix provided by the Library is incorrect? or am I getting something wrong here?