def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
try:
rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
except TypeError:
# get None for rgb_image
print("trackMouse(): Didn't get RGB value")
return
hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
hsv = hsv_image[y, x]
# print(hsv_image)
# self.ui.rdoutRGB.setText("({},{},{})".format(rgb[0], rgb[1], rgb[2]))
self.ui.rdoutRGB.setText("({},{},{})".format(
hsv[0], hsv[1], hsv[2]))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if (self.kinect.kinectCalibrated):
z_w = .1236 * np.tan(z / 2842.5 + 1.1863) - 0.94
xy_world = self.sm.worldCoordinates(x, y)
self.ui.rdoutMouseWorld.setText(
"(%.3f,%.3f,%.3f)" %
(xy_world[0], xy_world[1], xy_world[2]))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
"""color calibration"""
#color = self.kinect.colorDetector(x,y)
#self.kinect.colorCalibration(x,y)
#self.kinect.block_detection_verification(x,y)
# map real world
real_x = self.kinect.real_coord[x][y][0]
real_y = self.kinect.real_coord[x][y][1]
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
real_z = self.kinect.convertDepthtomm(z)
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if self.kinect.kinectCalibrated == True:
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" %
(real_x, real_y, real_z))
else:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
def trackMouse(self):
"""
Mouse position presentation in GUI
After implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self,QCursor.pos()).x()
y = QWidget.mapFromGlobal(self,QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if(self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
mouse_coord = [x,y,1]
# affine = [[1,1,1],[1,1,1],[0,0,1]]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z))
if (self.kinect.kinectCalibrated):
# Z = self.kinect.worldHeight - 0.1236 * 1000 * np.tan(z/2842.5 + 1.1863)
# world_coord = np.matmul(self.kinect.convert_to_world, mouse_coord)
world_coord = self.kinect.world_coord(x,y)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (world_coord[0],world_coord[1],world_coord[2]))
else:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if not self.kinect.kinectCalibrated:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
w_x, w_y, w_z = self.kinect.toWorldCoord(x, y, z)
self.ui.rdoutMouseWorld.setText("(%.1f,%.1f,%.1f)" %
(w_x, w_y, w_z))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
#convert camera data to depth in mm
depth = 1000 * 0.1236 * np.tan(z / 2842.5 + 1.1863)
# if self.kinect.kinectCalibrated == True :
world_frame = depth * np.dot(self.kinect.projection, [x, y, 1])
#To convert depth to IK convention
world_frame[2] = -world_frame[2] + 939
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.0f)" %
(world_frame[0], world_frame[1], world_frame[2]))
self.kinect.world_frame = world_frame # use this variable in click and grab
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
d = self.kinect.currentDepthFrame[y][x] # depth value
d = np.clip(d, 0, 2**10 - 1)
Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863)
# print Zc
# print('----control station intrinsic_matrix')
# print self.kinect.intrinsic_matrix
XYZ_camera = Zc * np.matmul(
np.linalg.inv(self.kinect.intrinsic_matrix),
np.array([x, y, 1]))
# print('----control station co_eff_camera_2_world')
# print self.kinect.co_eff_camera_2_world
W = np.matmul(self.kinect.co_eff_camera_2_world,
np.append(XYZ_camera, 1))
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, d))
# W = np.matmul(self.sm.coeff_rgb_2_world, np.array([x, y, 1]))
#linear fitting
# d = 2047-z
# 2047 - 718 --> 0
# 2047 - 705 --> 38
# 2047 - 688 --> 38*2
# 2047 - 668 --> 38*3
# 2047 - 646 --> 38*4
# 2047 - 624 --> 38*5
# 2047 - 598 --> 38*6
# 2047 - 570 --> 38*7
# 2047 - 538 --> 38*8
# 2047 - 501 --> 38*9
# 2047 - 462 --> 38*10
# need better calibration function
# W[2] = (8.00506778e-06)*d**3-(3.79099906e-02)*d**2 + (6.08296089e+01)*d - (3.26712433e+04)
#W[2] = (1.46565657e+00)*d - (1.91508256e+03)
#W[2] = (-4.15178471e-08)*d**4 + (2.49769770e-04)*d**3 - (5.65159066e-01)*d**2 + (5.71205622e+02)*d - (2.17696573e+05)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" %
(W[0], W[1], W[2]))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self,QCursor.pos()).x()
y = QWidget.mapFromGlobal(self,QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if self.kinect.calibrated == True:
mouse = np.array([[x], [y], [1]], dtype=float)
depth = (0.1236 * np.tan((z) / 2842.5 + 1.1863))
camera = depth * np.matmul(inv(self.kinect.camera_matrix),
mouse)
points = np.array([[camera[0]], [camera[1]], [1]])
world = np.matmul(self.kinect.affineB.astype(float),
points.astype(float))
#world = np.matmul(self.kinect.affineB.astype(float),mouse)
#depth = -2.5333*z + 1836.7
board_depth = 0.1236 * np.tan(718 / 2842.5 + 1.1863)
#depth = (1-depth/board_depth)*1000
depth = (board_depth - depth) * 1000
"""
correction_factor = 0.000626
world[0]=world[0]*(1-correction_factor*depth)
world[1]=world[1]*(1-correction_factor*depth)
"""
#self.ui.rdoutMouseWorld.setText("(%3.2f,%3.2f,%3.2f)" % (camera[0],camera[1],camera[2]))
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.0f)" % (world[0], world[1], depth))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self,QCursor.pos()).x()
y = QWidget.mapFromGlobal(self,QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if(self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z))
PointCameraFrm = self.kinect.ConvertImagePointToCameraFrame(np.array([x,y]))
PointWorldFrm = self.kinect.ConvertCameraFrametoWorlFrame(PointCameraFrm)
#self.ui.rdoutMouseWorld.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(%.3f,%.3f,%.3f)" % (PointWorldFrm[0],PointWorldFrm[1],PointWorldFrm[2]))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self,QCursor.pos()).x()
y = QWidget.mapFromGlobal(self,QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if(self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z))
if (self.kinect.kinectCalibrated):
zw = self.kinect.depthcalibration(z)
xwyw = self.kinect.pixeltoworldcoordinates(np.array([x,y,1]), z)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (xwyw[0],xwyw[1],zw))
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
hsv = hsv_image[y, x]
# self.ui.rdoutRGB.setText("({},{},{})".format(*rgb))
# self.ui.rdoutRGB.setText("({},{},{})".format(*hsv))
self.ui.rdoutRGB.setText(hue_to_classification(hsv[0]))
print(hsv[0])
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
count = 0
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
# posesall = self.rexarm.get_positions()
# endeffectorpos = FK_dh(posesall,0)
# if path.exists("traj_fast_not_smooth.txt"):
# with open('traj_fast_not_smooth.txt','a') as f:
# f.write(str(self.rexarm.get_wrist_pose())+'\n')
# else :
# with open('traj_fast_not_smooth.txt','w') as f:
# f.write("Traj Not Smooth\n")
else:
# Subtracting the X and Y distance corresponding to image origin frame to find cursor location with reference to imae frame
x = x - MIN_X
y = y - MIN_Y
# Checking if the Kinect depth camera is producing output
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
# Display the x,y (pixels), z (10 bit number) coordinates
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
# Checking if the calibration has been done
if self.sm.calibration_state() == True:
#############################################
# CAMERA FRAME TO DEPTH FRAME #
#############################################
# Taking in the pixel values in camera frame and transforming to the kinect depth frame
pixel_value = np.array([x, y])
# Converting 10 bit depth to real distance using provided analytical function
z = self.kinect.currentDepthFrame[int(
pixel_value.item(1))][int(pixel_value.item(0))]
Z = 12.36 * np.tan(float(z) / 2842.5 + 1.1863)
# 95 cm marks the z location of the base plane wrt to the camera. Subtracting 95 to measure +z from the base plane
Z_modified = 95 - Z
#############################################
# CAMERA FRAME TO WORLD FRAME #
#############################################
# Extracting the origin of the camera frame (Following 4 quadrant system)
pix_center = self.sm.pixel_center_loc()
# X and Y locations in the RGB space in pixels with (0,0) at the robot base center
x = x - pix_center.item(0)
y = pix_center.item(1) - y
# Taking in the pixel values in camera frame and transforming to the world frame
pixel_value = np.array([x, y])
pixel_value = np.transpose(pixel_value)
# Extracting the affine matrix computed during camera calibration
affine = self.sm.return_affine()
affine = affine[0:2, 0:2]
# World x,y location corresponding to iamge frame x,y location
world_value = np.matmul(affine, pixel_value)
#############################################
# SOLVE PNP #
#############################################
# rot,trans = self.sm.return_solvepnp()
# cam = self.sm.return_intrinsic()
# xyz_c = Z*rgb_pt.T
# xyz_c = np.linalg.inv(cam).dot(xyz_c)
# xyz_c = xyz_c - trans
# world_value = xyz_c*rot
# -0.197*float(z) + 142.772
# self.kinect.detectBlocksInDepthImage()
# self.kinect.processVideoFrame()
# Displaying the World X,Y and Z coordinates in GUI
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.1f)" %
(world_value.item(0), world_value.item(1), Z_modified))
# self.sm.WC = [world_value.item(0)*10,world_value.item(1)*10,ZZ*10]
else:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
