Fixes

examples/basic_example.py

@@ -1,4 +1,5 @@
 from vision_system.camera import Camera
+import vision_system.vision_system_utils as vision_utils
 import rclpy
 import cv2 as cv
 
@@ -18,11 +19,17 @@ def main():
 
  rgb_frame, distance_frame = camera.acquire_frames_once()
  # Do whatever you want with the frames example show them 
+ camera_info = camera.get_camera_info()
+
+ print(vision_utils.deproject_pixel_to_point([100, 150], distance_frame[100,150], camera_info))
+
  cv.imshow('Color Frame', rgb_frame)
  cv.imshow('Distance Frame', distance_frame)
  cv.waitKey(0)
  cv.destroyAllWindows()
 
+
+
  # Otherwise if you want to use the processing functio and so do nothing here call:
  # camera.process_once() instead of camera.acquire_frames_once
 

vision_system/camera.py

@@ -24,6 +24,7 @@
 from sensor_msgs.msg import CameraInfo, Image
 from vision_system.post_processing import PostProcessing
 import numpy as np
+import copy
 from typing import Optional, Tuple
 
 DEFAULT_COLOR_IMAGE_TOPIC = '/camera/color/image_raw'
@@ -300,7 +301,7 @@ def get_camera_info(self) -> Optional[CameraInfo]:
  :return: The camera info as a CameraInfo object, or None if it has not been retrieved.
  """
  if self.camera_info is not None:
- return self.camera_info.copy()
+ return copy.deepcopy(self.camera_info) 
  return None
 
  def get_frame_id(self) -> Optional[str]:

vision_system/vision_system_utils.py

@@ -18,22 +18,22 @@
 from sensor_msgs.msg import CameraInfo
 import numpy as np
 
-def deproject_pixel_to_point(self, pixel, distance, camera_info: CameraInfo):
+def deproject_pixel_to_point(pixel, distance, camera_info: CameraInfo):
  """
  Deproject a pixel to a 3D point
  :param pixel: (u, v) pixel coordinates
  :param depth: depth value
  :return: (x, y, z) 3D point
  """
- if self.camera_info is None:
+ if camera_info is None:
  return None
 
  # Camera intrinsics
- fx = self.camera_info.K[0]
- fy = self.camera_info.K[4]
- ppx = self.camera_info.K[2]
- ppy = self.camera_info.K[5]
- coeffs = [coeff for coeff in self.camera_info.D]
+ fx = camera_info.k[0]
+ fy = camera_info.k[4]
+ ppx = camera_info.k[2]
+ ppy = camera_info.k[5]
+ coeffs = [coeff for coeff in camera_info.d]
 
  u = pixel[0]
  v = pixel[1]