def _create_depth(self, pipeline, using_nn):
'''!
Creates the depth node.
Uses the appropiate settings for the configs and existing pipeline.
Parameters:
@param pipeline: The pipeline to add the node to
@param using_nn: Whether the pipeline is using a neural network or not
'''
if self.depth:
depth_node = pipeline.createStereoDepth()
depth_node.setLeftRightCheck(True)
depth_node.setConfidenceThreshold(200)
if (not using_nn) and self._configs["depth_close"] == True: #See close -> Extended
depth_node.setExtendedDisparity(True)
depth_node.setSubpixel(False)
elif (not using_nn) and self._configs["depth_far"] == True: #See longer -> Subpixel
depth_node.setSubpixel(True)
depth_node.setExtendedDisparity(False)
else:
depth_node.setDepthAlign(dai.CameraBoardSocket.RGB)
#depth_node.setDepthAlign(dai.StereoDepthProperties.DepthAlign.CENTER)
depth_node.setSubpixel(False)
depth_node.setExtendedDisparity(False)
self.nodes[OAK_Stage.DEPTH] = depth_node
if self._configs["depth_host"] == False:
spatialLocationCalculator = pipeline.createSpatialLocationCalculator()
topLeft = dai.Point2f(0.4, 0.4)
bottomRight = dai.Point2f(0.6, 0.6)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight)
spatialLocationCalculator.setWaitForConfigInput(False)
spatialLocationCalculator.initialConfig.addROI(config)
depth_node.disparity.link(spatialLocationCalculator.inputDepth)
self.nodes["spartial_location_calculator"] = spatialLocationCalculator
spatialLocationCalculator.passthroughDepth.link(xoutDepth.input)
stereo.depth.link(spatialLocationCalculator.inputDepth)
topLeft = dai.Point2f(0.4, 0.4)
bottomRight = dai.Point2f(0.6, 0.6)
# topLeft2 = dai.Point2f(0.6, 0.4)
# bottomRight2 = dai.Point2f(0.8, 0.6)
# topLeft = dai.Point2f(0.1, 0.1)
# bottomRight = dai.Point2f(0.9, 0.9)
spatialLocationCalculator.setWaitForConfigInput(False)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight)
spatialLocationCalculator.initialConfig.addROI(config)
spatialLocationCalculator.out.link(xoutSpatialData.input)
xinSpatialCalcConfig.out.link(spatialLocationCalculator.inputConfig)
# Pipeline is defined, now we can connect to the device
with dai.Device(pipeline) as device:
device.startPipeline()
# Output queue will be used to get the depth frames from the outputs defined above
depthQueue = device.getOutputQueue(name="depth", maxSize=4, blocking=False)
spatialCalcQueue = device.getOutputQueue(name="spatialData",
maxSize=4,
blocking=False)
spatialCalcConfigInQueue = device.getInputQueue("spatialCalcConfig")
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
