[95kvdko]

Hi everyone,

I'm doing a project where i want to click on a single point of an image and get the coordinates (x, y, z) of that point in real life. Then recreate on robodk the scenario of the camera and move a robotic arm (ur5e of universal robots) to that point. The thing is that if i print the coordinates (x, y, z) that i get from the camera, i think they make sense, but when i use them to create a target for robodk and move the arm there, they don't match with the point where should it go.

This is the code that i'm using.

Code:

import cv2                                # state of the art computer vision algorithms library
import numpy as np                        # fundamental package for scientific computing
import imageio                      
import matplotlib.pyplot as plt           # 2D plotting library producing publication quality figures
import pyrealsense2 as rs
import win32gui
import win32api
import time
import pyautogui
import tkinter
import tkinter.messagebox
from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations

# Any interaction with RoboDK must be done through
RL = Robolink()

top = tkinter.Tk()

points = rs.points()
pipe = rs.pipeline()
config = rs.config()
profile = pipe.start(config)

colorizer = rs.colorizer()
align = rs.align(rs.stream.color)
state_left = win32api.GetKeyState(0x01)  # Left button up = 0 or 1. Button down = -127 or -128
device = profile.get_device() # type: rs.device
depth_sensor = device.first_depth_sensor() # type: rs.depth_sensor


if depth_sensor.supports(rs.option.depth_units):
 depth_sensor.set_option(rs.option.depth_units,0.0001)

robot = RL.Item('UR5e')
frame = RL.Item('Frame 2')
Target = RL.AddTarget('Target 2', frame)

try:
   while True:
      frames = pipe.wait_for_frames()
      frames = align.process(frames)
      depth_frame = frames.get_depth_frame()
      color_frame = frames.get_color_frame()
      if not depth_frame: continue
      #print(width,height)
      depth = np.asanyarray(colorizer.colorize(depth_frame).get_data())
      color = np.asanyarray(color_frame.get_data())
      images = np.hstack((color, depth))
         #cv2.namedWindow('RGB_View', cv2.WINDOW_NORMAL)
      #cv2.setWindowProperty('RGB_View', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
      cv2.imshow('RGB_View', depth)
      depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
      #flags, hcursor, (x,y) = win32gui.GetCursorInfo()
      x, y = win32api.GetCursorPos()
      #print(x, y)
      pixel_coord = [x, y]
      keypressed = win32api.GetKeyState(0x01)
      key = cv2.waitKey(1)
      # Press esc or 'q' to close the image window
      if key & 0xFF == ord('q') or key == 27:
          cv2.destroyAllWindows()
          break
      #Calculate distance
      if keypressed!=state_left:
       state_left = keypressed
       print(keypressed)
       if keypressed < 0:
        if (0<x<632 and 32<y<512):
         dist_to_center = depth_frame.get_distance(x,y)
         dist_to_center = round(dist_to_center, 4)
         depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, pixel_coord, dist_to_center)
         depth_point = [i * 100 for i in depth_point]
         depth_point = [round(i, 5) for i in depth_point]
         #depth_point = ( ", ".join( repr(e) for e in depth_point))
         #with open('Coordinates.txt', 'w') as f:
          # f.write("%s\n" % depth_point)
         #f.close()
         print(depth_point)
         print('The camera is facing an object:',dist_to_center,'meters away')
         Target.setPose(Offset(eye(), depth_point[0], depth_point[1], depth_point[2], 0, 0, 0))
         robot.MoveJ(Target)
         time.sleep(0.7)

finally:
   pipe.stop()

If someone knows how to get the rotation of the camera, it would be very useful too.

Thanks in advance,

Regards.

[Jeremy]

This thread has been moved to the API section.

[Albert]

You should first calibrate the position of the camera in your cell. 
For example, you can add a new coordinate system called "Camera Eye" that represents the position of the camera.

Are you holding the camera with the robot? If so, you should properly place the camera with respect to the robot (also known as Hand-eye calibration).

If the camera is static you could take 3 or more reference points to position your camera (common points for the camera and the robot).

For example, to retrieve the absolute XYZ coordinates of a target you can do something like this:

Code:

# Get the camera coordinate system item
camera_eye = RDK.Item("Camera Eye")

# Calculate absolute XYZ position clicked from the camera in absolute coordinates:
xyz_camera = [depth_point[0], depth_point[1], depth_point[2]]
xyz_abs = camera_eye.PoseAbs()*xyz_camera

You can then move to this position doing something like this:

Code:

# Take the current robot position as a full pose (you can then use this orientation)
target_pose = robot.Pose()

# Calculate absolute target coordinates (world/common reference frame)
#target_pose_abs = target.Parent().PoseAbs() * target_pose
target_pose_abs = target.PoseAbs() # previous line does the same

# Force XYZ absolute coordinates for this pose (with respect to the World reference frame)
target_pose_abs.setPos(xyz_abs)

# Set the pose of the target given the absolute position (the relative target is calculated accordingly)
target.setPoseAbs(target_pose_abs)

# Move to the target
robot.MoveJ(target)

[Sam]

If someone is looking for a way to programmatically determine the camera pose in RoboDK, we added an example of camera pose estimation using OpenCV and ChAruCo board in our Python examples, see https://robodk.com/doc/en/PythonAPI/exam...amera-pose.