newConfig = False
key = cv2.waitKey(1)
if key == ord('q'):
break
elif key == ord('w'):
if topLeft.y - stepSize >= 0:
topLeft.y -= stepSize
bottomRight.y -= stepSize
newConfig = True
elif key == ord('a'):
if topLeft.x - stepSize >= 0:
topLeft.x -= stepSize
bottomRight.x -= stepSize
newConfig = True
elif key == ord('s'):
if bottomRight.y + stepSize <= 1:
topLeft.y += stepSize
bottomRight.y += stepSize
newConfig = True
elif key == ord('d'):
if bottomRight.x + stepSize <= 1:
topLeft.x += stepSize
bottomRight.x += stepSize
newConfig = True
if newConfig:
config.roi = dai.Rect(topLeft, bottomRight)
cfg = dai.SpatialLocationCalculatorConfig()
cfg.addROI(config)
spatialCalcConfigInQueue.send(cfg)
def get3DPosition(self, point_x,point_y, size):
'''!
Copmutes the 3D postiion of a point in the image.
Parameters:
@param point_x: x coordinate of the point
@param point_y: y coordinate of the point
@param size: size of the image of the coordinates
'''
try:
self._depth_frame
except:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
point_x *= self._depth_frame.shape[0]/size[0]
point_y *= self._depth_frame.shape[1]/size[1]
x_pixel = np.array([point_x,point_y,1.0],dtype=np.float64)
point_x = int(point_x)
point_y = int(point_y)
x_space = np.zeros(3,dtype=np.float64)
n_pixel = 0
color_calib_size = self.camera_calibration_size[OAK_Stage.COLOR]
color_intrinsic = self.camera_intrinsics[OAK_Stage.COLOR]
scale = [self._depth_frame.shape[0]/color_calib_size[0], self._depth_frame.shape[1]/color_calib_size[1]]
K = color_intrinsic.copy()
K[0] *= scale[0]
K[1] *= scale[1]
k_inv = np.linalg.inv(K)
n_point = self._configs["depth_roi_size"]
x_min = point_x-(n_point//2)-1
x_max = point_x+(n_point//2)
y_min = point_y-(n_point//2)-1
y_max = point_y+(n_point//2)
if x_min < 0:
x_min = 0
if y_min < 0:
y_min = 0
if x_max >= self._depth_frame.shape[0]:
x_max = self._depth_frame.shape[0]
if y_max >= self._depth_frame.shape[1]:
y_max = self._depth_frame.shape[1]
if self._configs["depth_host"] == False:
topLeft = dai.Point2f(x_min, y_min)
bottomRight = dai.Point2f(x_max, y_max)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight).normalize(width=self._depth_frame.shape[1], height=self._depth_frame.shape[0])
cfg = dai.SpatialLocationCalculatorConfig()
cfg.addROI(config)
self._oak_input_queue["spatialConfig"].send(cfg)
spatialdata = self._spatial_queue.get()
x_space = np.zeros(3, dtype = np.float64)
x_space[0] = spatialdata[0].spatialCoordinates.x
x_space[1] = spatialdata[0].spatialCoordinates.y
x_space[2] = spatialdata[0].spatialCoordinates.z
return []
samples = self._depth_frame[x_min:x_max,y_min:y_max].flatten()
samples = samples[samples != 0]
samples = samples[samples>100]
if samples.shape[0] == 0:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
z = 0
if self._configs["depth_point_mode"] == "median":
z = np.median(samples)
elif self._configs["depth_point_mode"] == "mean":
z = np.mean(samples)
elif self._configs["depth_point_mode"] == "min":
samples = np.sort(samples)
z = np.mean(samples[:int(samples.shape[0]*0.1)])
elif self._configs["depth_point_mode"] == "max":
samples = np.sort(samples)
z = np.median(samples[int(samples.shape[0]*0.9):])
else:
hist, edge = np.histogram(samples, bins=1000, range=(0.0,5000.0), density=True)
a = []
for i in range(edge.shape[0]-1):
a.append((edge[i]+edge[i+1])/2)
edge = np.array(a)
best_z = 0
best_z_inliers = 0
for i in range(16):
index = int(np.random.rand(1)[0]*samples.shape[0])
z_test = samples[index]
if i == 0:
z_test = np.median(samples)
if i == 1:
z_test = np.mean(samples)
z_min = z_test-100.0
z_max = z_test+100.0
mask = np.logical_and(edge>z_min,edge<z_max)
values = edge[mask]
weights = hist[np.where(mask)]
if np.sum(weights) == 0:
continue
#z_test = np.average(values, weights=weights)
n_inlier = values.shape[0]
if n_inlier > best_z_inliers:
best_z_inliers = n_inlier
best_z = z_test
z = best_z
if z == 0:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
x_space = k_inv @ (z*x_pixel)
return x_space/1000.0