def get_boundary_in_color(roi_2D, Trans, extrinsics_device, color_internal):
roi_color = np.zeros((3, 4)) #roi_2D[x-,x+,y-,y+]
roi_color[0, :2] = roi_2D[0]
roi_color[0, 2:4] = roi_2D[1]
roi_color[1, [0, 3]] = roi_2D[2]
roi_color[1, [1, 2]] = roi_2D[3]
roi_color[2, :] = 0
transform = Transformation(Trans[4], Trans[5])
roi_color = transform.apply_transformation(roi_color)
roi_color = roi_color.transpose().tolist()
color_pixel = []
for point in roi_color:
cloud_point = rs.rs2_transform_point_to_point(extrinsics_device, point)
color_pixel.append(
rs.rs2_project_point_to_pixel(color_internal, cloud_point))
color_boundary_point = np.row_stack(color_pixel).T
print(np.shape(color_boundary_point))
# transform = Transformation(Trans[2],Trans[3].flatten())
# roi_point = transform.apply_transformation(roi_color)
# color_boundary_point=get_point_3D_2D(roi_point,Trans[0])
# print(np.shape(color_boundary_point))
return color_boundary_point
def is_disturbance(plane, contour, depth_frame, depth_intrin,
depth_to_color_extrin):
distance_sum = 0
i = 0
print('contour', len(contour))
for each in contour:
if depth_frame.get_distance(int(each[1]), int(each[0])) != 0:
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, [each[1], each[0]],
depth_frame.get_distance(int(each[1]), int(each[0])))
color_point = rs.rs2_transform_point_to_point(
depth_to_color_extrin, depth_point)
if math.sqrt(plane[0] * plane[0] + plane[1] * plane[1] +
plane[2] * plane[2]) != 0:
distance = abs(color_point[0] * plane[0] + color_point[1] *
plane[1] + color_point[2] * plane[2] +
plane[3]) / math.sqrt(plane[0] * plane[0] +
plane[1] * plane[1] +
plane[2] * plane[2])
else:
distance = 0
continue
distance_sum += distance
i += 1
if i != 0:
average_distance = distance_sum / i
print(average_distance)
else:
print("the points' distance to camera are all 0 ")
average_distance = 1
if average_distance < 0.01:
return True
else:
return False
def get_object_points(color_frame, depth_frame):
"""Short summary.
Parameters
----------
color_frame : type
Description of parameter `color_frame`.
depth_frame : type
Description of parameter `depth_frame`.
Returns
-------
type
Description of returned object.
"""
pc = rs.pointcloud()
pc.map_to(color_frame)
pointcloud = pc.calculate(depth_frame)
v = pointcloud.get_vertices()
verts = np.asanyarray(v).view(np.float32).reshape(-1, 3)
cloud = pcl.PointCloud(verts)
filtered_cloud = do_passthrough_filter(point_cloud=cloud,
name_axis='z',
min_axis=0.0,
max_axis=0.6)
downsampled_cloud = do_voxel_grid_filter(point_cloud=filtered_cloud,
LEAF_SIZE=0.004)
table_cloud, objects_cloud = do_ransac_plane_segmentation(
downsampled_cloud, max_distance=0.01)
# project_inliers = objects_cloud.make_ProjectInliers()
# project_inliers.set_model_type(pcl.SACMODEL_NORMAL_PARALLEL_PLANE)
# plane = project_inliers.filter()
colorless_cloud = XYZRGB_to_XYZ(objects_cloud)
clusters = get_clusters(objects_cloud,
tolerance=0.02,
min_size=100,
max_size=25000)
colored_points = get_cloud_clusters(clusters, colorless_cloud)
# Create a cloud with each cluster of points having the same color
clusters_cloud = pcl.PointCloud()
clusters_cloud.from_list(colored_points)
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
Pixel_Coord = []
for data in clusters_cloud:
if np.round(data[2], 2) > 0.1:
color_point = rs.rs2_transform_point_to_point(
depth_to_color_extrin, list(data))
Pixel_Coord.append(
rs.rs2_project_point_to_pixel(color_intrin, color_point))
# pcl.save(downsampled_cloud, "12.ply")
# if clusters_cloud.size > 0:
# pcl.save(clusters_cloud, "13.ply")
return Pixel_Coord
def project_color_pix_to_depth(color_intrin, depth_intrin, color_to_depth_extrin, depth_scale, color_pix):
color_point = rs.rs2_deproject_pixel_to_point(color_intrin, color_pix, 1.0 / depth_scale)
depth_point = rs.rs2_transform_point_to_point(color_to_depth_extrin, color_point)
depth_pix = rs.rs2_project_point_to_pixel(depth_intrin, depth_point)
depth_pix = [int(x) for x in depth_pix]
return depth_pix
def point_transform(point, depth_frame, rois, depth_intrin,
depth_to_color_extrin):
left_point = [0, 0]
for i in range(5, 100):
left_point = [point[0] + i, point[1] - i]
if not point_in_mask(left_point, rois) and depth_frame.get_distance(
int(left_point[0]), int(left_point[1])) != 0:
break
if left_point != [0, 0]:
depth_point1 = rs.rs2_deproject_pixel_to_point(
depth_intrin, left_point,
depth_frame.get_distance(int(left_point[0]), int(left_point[1])))
color_point1 = rs.rs2_transform_point_to_point(depth_to_color_extrin,
depth_point1)
else:
color_point1 = [0, 0, 0]
print('point_transform fail')
return color_point1
def calculate_boundingbox_points(pipeline, pipeline_profile, point_cloud,
Trans, extrinsics_device, color_internal):
dimensions = []
color_images = []
depth_value0 = point_cloud[2].reshape(720, 1280)
if len(depth_value0 > 0.001) > 500:
depth_value1 = np.int0(
np.where(depth_value0 < 0.005, 0, depth_value0) * 1000)
mask = np.where(depth_value1 > 0, 255, depth_value1).astype(np.uint8)
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
contours_0 = []
# 寻找满足的轮
for j in range(len(contours)):
cnt = contours[j]
area = cv2.contourArea(cnt)
length = cv2.arcLength(cnt, True)
if area > 100 and length > 200:
contours_0.append(contours[j])
#cv2.drawContours(mask,contours_0,-1,[255],3)
for i in range(len(contours_0)):
index = []
for j in range(len(contours_0[i])):
x = contours_0[i][j][0][0]
y = contours_0[i][j][0][1]
index.append(x + y * 1280)
hull = cv2.convexHull(contours_0[i])
angle_0 = cv2.minAreaRect(hull)[2]
point_cloud0 = point_cloud[:, index]
if point_cloud0.shape[1] > 100:
coord = np.c_[point_cloud0[0, :],
point_cloud0[1, :]].astype('float32')
min_area_rectangle = cv2.minAreaRect(coord)
angle_1 = min_area_rectangle[2]
bounding_box_world_2d = cv2.boxPoints(min_area_rectangle)
# print("debug111")
# print(bounding_box_world_2d)
height = max(point_cloud0[
2, :]) # - min(point_cloud[2, :]) + depth_threshold
height_array = np.array([[-height], [-height], [-height],
[-height], [0], [0], [0], [0]])
bounding_box_world_3d = np.column_stack((np.row_stack(
(bounding_box_world_2d, bounding_box_world_2d)),
height_array))
trans = Transformation(Trans[4], Trans[5])
bounding_box_device_3d = trans.apply_transformation(
bounding_box_world_3d.transpose()) #
# print("debug222")
# print(bounding_box_device_3d)
# trans = Transformation(Trans[2],Trans[3])
# bounding_box_color_image_point=trans.apply_transformation(bounding_box_device_3d)
# print("debug333")
# print(bounding_box_color_image_point) #
#
# color_pixel=get_point_3D_2D(bounding_box_color_image_point, Trans[0]).T
# color_images.append(color_pixel)
# print("debug444")
# print(color_pixel)
#
bounding_box_device_3d = bounding_box_device_3d.transpose(
).tolist()
color_pixel = []
for bounding_box_point in bounding_box_device_3d:
bounding_box_color_image_point = rs.rs2_transform_point_to_point(
extrinsics_device, bounding_box_point)
color_pixel.append(
rs.rs2_project_point_to_pixel(
color_internal, bounding_box_color_image_point))
color_image = np.row_stack(color_pixel)
color_images.append(color_image)
dimensions.append([
min_area_rectangle[1][0], min_area_rectangle[1][1], height,
angle_0
])
#
# cv2.imshow("depth11", mask)
# cv2.waitKey(1)
return color_images, dimensions
else:
return color_images, dimensions
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold = 0.01):
"""
Calculate the top and bottom bounding box corner points for the point cloud in the image coordinates of the color imager of the realsense device
Parameters:
-----------
point_cloud : ndarray
The (3 x N) array containing the pointcloud information
calibration_info_devices : dict
keys: str
Serial number of the device
values: [transformation_devices, intrinsics_devices, extrinsics_devices]
transformation_devices: Transformation object
The transformation object containing the transformation information between the device and the world coordinate systems
intrinsics_devices: rs.intrinscs
The intrinsics of the depth_frame of the realsense device
extrinsics_devices: rs.extrinsics
The extrinsics between the depth imager 1 and the color imager of the realsense device
depth_threshold : double
The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used
Following the right-hand coordinate system, if the object is placed on the chessboard plane, the height of the object will increase along the negative Z-axis
Return:
----------
bounding_box_points_color_image : dict
The bounding box corner points in the image coordinate system for the color imager
keys: str
Serial number of the device
values: [points]
points: list
The (8x2) list of the upper corner points stacked above the lower corner points
length : double
The length of the bounding box calculated in the world coordinates of the pointcloud
width : double
The width of the bounding box calculated in the world coordinates of the pointcloud
height : double
The height of the bounding box calculated in the world coordinates of the pointcloud
"""
# Calculate the dimensions of the filtered and summed up point cloud
# Some dirty array manipulations are gonna follow
if point_cloud.shape[1] > 500:
# Get the bounding box in 2D using the X and Y coordinates
coord = np.c_[point_cloud[0,:], point_cloud[1,:]].astype('float32')
min_area_rectangle = cv2.minAreaRect(coord)
bounding_box_world_2d = cv2.boxPoints(min_area_rectangle)
# Caculate the height of the pointcloud
height = max(point_cloud[2,:]) - min(point_cloud[2,:]) + depth_threshold
# Get the upper and lower bounding box corner points in 3D
height_array = np.array([[-height], [-height], [-height], [-height], [0], [0], [0], [0]])
bounding_box_world_3d = np.column_stack((np.row_stack((bounding_box_world_2d,bounding_box_world_2d)), height_array))
# Get the bounding box points in the image coordinates
bounding_box_points_color_image={}
for (device, calibration_info) in calibration_info_devices.items():
# Transform the bounding box corner points to the device coordinates
bounding_box_device_3d = calibration_info[0].inverse().apply_transformation(bounding_box_world_3d.transpose())
# Obtain the image coordinates in the color imager using the bounding box 3D corner points in the device coordinates
color_pixel=[]
bounding_box_device_3d = bounding_box_device_3d.transpose().tolist()
for bounding_box_point in bounding_box_device_3d:
bounding_box_color_image_point = rs.rs2_transform_point_to_point(calibration_info[2], bounding_box_point)
color_pixel.append(rs.rs2_project_point_to_pixel(calibration_info[1][rs.stream.color], bounding_box_color_image_point))
bounding_box_points_color_image[device] = np.row_stack( color_pixel )
return bounding_box_points_color_image, min_area_rectangle[1][0], min_area_rectangle[1][1], height
else :
return {},0,0,0
def calculate_boundingbox_points(point_cloud,
calibration_info_devices,
depth_threshold=0.01):
"""
Calculate the top and bottom bounding box corner points for the point cloud in the image coordinates of the color imager of the realsense device
Parameters:
-----------
point_cloud : ndarray
The (3 x N) array containing the pointcloud information
calibration_info_devices : dict
keys: str
Serial number of the device
values: [transformation_devices, intrinsics_devices, extrinsics_devices]
transformation_devices: Transformation object
The transformation object containing the transformation information between the device and the world coordinate systems
intrinsics_devices: rs.intrinscs
The intrinsics of the depth_frame of the realsense device
extrinsics_devices: rs.extrinsics
The extrinsics between the depth imager 1 and the color imager of the realsense device
depth_threshold : double
The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used
Following the right-hand coordinate system, if the object is placed on the chessboard plane, the height of the object will increase along the negative Z-axis
Return:
----------
bounding_box_points_color_image : dict
The bounding box corner points in the image coordinate system for the color imager
keys: str
Serial number of the device
values: [points]
points: list
The (8x2) list of the upper corner points stacked above the lower corner points
length : double
The length of the bounding box calculated in the world coordinates of the pointcloud
width : double
The width of the bounding box calculated in the world coordinates of the pointcloud
height : double
The height of the bounding box calculated in the world coordinates of the pointcloud
"""
# Calculate the dimensions of the filtered and summed up point cloud
# Some dirty array manipulations are gonna follow
if point_cloud.shape[1] > 500:
# Get the bounding box in 2D using the X and Y coordinates
coord = np.c_[point_cloud[0, :], point_cloud[1, :]].astype('float32')
min_area_rectangle = cv2.minAreaRect(coord)
bounding_box_world_2d = cv2.boxPoints(min_area_rectangle)
# Caculate the height of the pointcloud
height = max(point_cloud[2, :]) - min(
point_cloud[2, :]) + depth_threshold
# Get the upper and lower bounding box corner points in 3D
height_array = np.array([[-height], [-height], [-height], [-height],
[0], [0], [0], [0]])
bounding_box_world_3d = np.column_stack((np.row_stack(
(bounding_box_world_2d, bounding_box_world_2d)), height_array))
# Get the bounding box points in the image coordinates
bounding_box_points_color_image = {}
for (device, calibration_info) in calibration_info_devices.items():
# Transform the bounding box corner points to the device coordinates
bounding_box_device_3d = calibration_info[0].inverse(
).apply_transformation(bounding_box_world_3d.transpose())
# Obtain the image coordinates in the color imager using the bounding box 3D corner points in the device coordinates
color_pixel = []
bounding_box_device_3d = bounding_box_device_3d.transpose().tolist(
)
for bounding_box_point in bounding_box_device_3d:
bounding_box_color_image_point = rs.rs2_transform_point_to_point(
calibration_info[2], bounding_box_point)
color_pixel.append(
rs.rs2_project_point_to_pixel(
calibration_info[1][rs.stream.color],
bounding_box_color_image_point))
bounding_box_points_color_image[device] = np.row_stack(color_pixel)
return bounding_box_points_color_image, min_area_rectangle[1][
0], min_area_rectangle[1][1], height
else:
return {}, 0, 0, 0
color_to_depth_extrin = aligned_color_frame.profile.get_extrinsics_to(
aligned_color_depth_frame.profile)
# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print('depth_scale:', depth_scale)
#depth_scale = depth_sensor.get_option(rs.option.depth_units)
# Map depth to color
color_pixel = [396, 270]
pixel_distance_in_meters = aligned_depth_frame.get_distance(
color_pixel[0], color_pixel[1])
#pixel_distance_in_meters=aligned_color_depth_frame.get_distance(color_pixel[0],color_pixel[1])
print("pixel distance in meters:", pixel_distance_in_meters)
color_point = rs.rs2_deproject_pixel_to_point(color_intrin, color_pixel,
pixel_distance_in_meters)
depth_point = rs.rs2_transform_point_to_point(color_to_depth_extrin,
color_point)
depth_pixel = rs.rs2_project_point_to_pixel(depth_intrin, depth_point)
pipe.stop()
print('depth_intrinsic', depth_intrin)
print('color_intrinsic', color_intrin)
print('depth and color extrinsic', color_to_depth_extrin)
print('depth_pixel', depth_pixel)
print('depth_point', depth_point)
print('color_pixel', color_pixel)
print('color_point', color_point)
cv2.namedWindow('Align Example', cv2.WINDOW_NORMAL)
#cv2.imshow('Align Example', images)
cv2.imshow('Align Example', aligned_color_image)
cv2.namedWindow('depth example', cv2.WINDOW_NORMAL)
cv2.imshow('depth example', aligned_color_depth_image)
def __detect(self, save_img=False):
# Declare pointcloud object, for calculating pointclouds and texture mappings
pc = rs.pointcloud()
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)
frames = self.__pipeline.wait_for_frames()
aligned_frames = align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
# time.sleep(8)
img_color = np.array(color_frame.get_data())
img_depth = np.array(depth_frame.get_data())
cv2.imwrite(opt.source, img_color)
# detection section
img_size = (
320, 192
) if ONNX_EXPORT else opt.img_size # (320, 192) or (416, 256) or (608, 352) for (height, width)
out, source, weights, half, view_img, save_txt = opt.output, opt.source, opt.weights, opt.half, opt.view_img, opt.save_txt
webcam = source == '0' or source.startswith(
'rtsp') or source.startswith('http') or source.endswith('.txt')
# Initialize
device = torch_utils.select_device(
device='cpu' if ONNX_EXPORT else opt.device)
if os.path.exists(out):
shutil.rmtree(out) # delete output folder
os.makedirs(out) # make new output folder
# Initialize model
model = Darknet(opt.cfg, img_size)
# Load weights
attempt_download(weights)
if weights.endswith('.pt'): # pytorch format
model.load_state_dict(
torch.load(weights, map_location=device)['model'])
else: # darknet format
load_darknet_weights(model, weights)
# Second-stage classifier
classify = False
if classify:
modelc = torch_utils.load_classifier(name='resnet101',
n=2) # initialize
modelc.load_state_dict(
torch.load('weights/resnet101.pt',
map_location=device)['model']) # load weights
modelc.to(device).eval()
# Eval mode
model.to(device).eval()
# Fuse Conv2d + BatchNorm2d layers
# model.fuse()
# Export mode
if ONNX_EXPORT:
model.fuse()
img = torch.zeros((1, 3) + img_size) # (1, 3, 320, 192)
f = opt.weights.replace(opt.weights.split('.')[-1],
'onnx') # *.onnx filename
torch.onnx.export(model, img, f, verbose=False, opset_version=11)
# Validate exported model
import onnx
model = onnx.load(f) # Load the ONNX model
onnx.checker.check_model(model) # Check that the IR is well formed
print(onnx.helper.printable_graph(model.graph)
) # Print a human readable representation of the graph
return
# Half precision
half = half and device.type != 'cpu' # half precision only supported on CUDA
if half:
model.half()
# Set Dataloader
vid_path, vid_writer = None, None
if webcam:
view_img = True
torch.backends.cudnn.benchmark = True # set True to speed up constant image size inference
dataset = LoadStreams(source, img_size=img_size)
else:
save_img = True
dataset = LoadImages(source, img_size=img_size)
# Get names and colors
names = load_classes(opt.names)
colors = [[random.randint(0, 255) for _ in range(3)]
for _ in range(len(names))]
# Run inference
t0 = time.time()
_ = model(torch.zeros(
(1, 3, img_size, img_size),
device=device)) if device.type != 'cpu' else None # run once
for path, img, im0s, vid_cap in dataset:
img = torch.from_numpy(img).to(device)
img = img.half() if half else img.float() # uint8 to fp16/32
img /= 255.0 # 0 - 255 to 0.0 - 1.0
if img.ndimension() == 3:
img = img.unsqueeze(0)
# Inference
t1 = torch_utils.time_synchronized()
pred = model(img, augment=opt.augment)[0]
t2 = torch_utils.time_synchronized()
# to float
if half:
pred = pred.float()
# Apply NMS
pred = non_max_suppression(pred,
opt.conf_thres,
opt.iou_thres,
multi_label=False,
classes=opt.classes,
agnostic=opt.agnostic_nms)
# Apply Classifier
if classify:
pred = apply_classifier(pred, modelc, img, im0s)
# Process detections
for i, det in enumerate(pred): # detections per image
if webcam: # batch_size >= 1
p, s, im0 = path[i], '%g: ' % i, im0s[i]
else:
p, s, im0 = path, '', im0s
save_path = str(Path(out) / Path(p).name)
s += '%gx%g ' % img.shape[2:] # print string
position_result = geometry_msgs.msg.PointStamped()
if det is not None and len(det):
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(img.shape[2:], det[:, :4],
im0.shape).round()
# Print results
for c in det[:, -1].unique():
n = (det[:, -1] == c).sum() # detections per class
s += '%g %ss, ' % (n, names[int(c)]) # add to string
# stop_num = 0
# print('c = ',len(det))
target_detected = False
# Write results
for *xyxy, conf, cls in det:
# c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3]))
# c0 = ((int(xyxy[0])+int(xyxy[2]))/2, (int(xyxy[1])+int(xyxy[3]))/2)
if save_txt: # Write to file
with open(save_path + '.txt', 'a') as file:
file.write(
('%g ' * 6 + '\n') % (*xyxy, cls, conf))
if save_img or view_img: # Add bbox to image
label = '%s %.2f' % (names[int(cls)], conf)
plot_one_box(xyxy,
im0,
label=label,
color=colors[int(cls)])
# print(c0)
if int(cls) == self.targetclass:
x_center = int((int(xyxy[0]) + int(xyxy[2])) / 2)
y_center = int((int(xyxy[1]) + int(xyxy[3])) / 2)
# Intrinsics and Extrinsics
depth_intrin = depth_frame.profile.as_video_stream_profile(
).intrinsics
color_intrin = color_frame.profile.as_video_stream_profile(
).intrinsics
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
# Depth scale: units of the values inside a depth frame, how to convert the value to units of 1 meter
depth_sensor = self.__pipe_profile.get_device(
).first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# Map depth to color
depth_pixel = [240, 320] # Random pixel
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, depth_pixel, depth_scale)
color_point = rs.rs2_transform_point_to_point(
depth_to_color_extrin, depth_point)
color_pixel = rs.rs2_project_point_to_pixel(
color_intrin, color_point)
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
vtx = np.array(points.get_vertices())
tex = np.array(points.get_texture_coordinates())
pix_num = 1280 * y_center + x_center
point_cloud_value = [
np.float(vtx[pix_num][0]),
np.float(vtx[pix_num][1]),
np.float(vtx[pix_num][2])
]
print('point_cloud_value:', point_cloud_value,
names[int(cls)], int(cls))
if np.float(vtx[pix_num][2]) > 0.11:
position_result.header.frame_id = 'camera_color_optical_frame'
position_result.point.x = np.float(
vtx[pix_num][0])
position_result.point.y = np.float(
vtx[pix_num][1])
position_result.point.z = np.float(
vtx[pix_num][2])
self.__target_position_pub_.publish(
position_result) # publish the result
target_detected = True
rospy.loginfo('The target has been detected!')
break # only the target class
# os.system('rostopic pub -1 /goal_pose geometry_msgs/PointStamped [0,[0,0],zed_left_camera_optical_frame] [%s,%s,%s]' %(np.float(vtx[pix_num][0]),np.float(vtx[pix_num][1]),np.float(vtx[pix_num][2]))
# os.system('rostopic pub -1 /start_plan std_msgs/Bool 1')
# else:
# os.system('rostopic pub -1 /start_plan std_msgs/Bool 0')
# print("Can't estimate point cloud at this position.")
# stop_num += 1
# if stop_num >= len(det):
# os.system('rostopic pub -1 /start_plan std_msgs/Bool 0')
if not target_detected:
position_result.header.frame_id = 'empty'
self.__target_position_pub_.publish(
position_result) # publish failure topic
rospy.logwarn('Fail to detect the target!')
else:
position_result.header.frame_id = 'empty'
self.__target_position_pub_.publish(
position_result) # publish failure topic
rospy.logwarn('Fail to detect any target!')
# Print time (inference + NMS)
print('%sDone. (%.3fs)' % (s, t2 - t1))
# Stream results
if view_img:
cv2.imshow(p, im0)
if cv2.waitKey(1) == ord('q'): # q to quit
raise StopIteration
# Save results (image with detections)
if save_img:
if dataset.mode == 'images':
cv2.imwrite(save_path, im0)
else:
if vid_path != save_path: # new video
vid_path = save_path
if isinstance(vid_writer, cv2.VideoWriter):
vid_writer.release(
) # release previous video writer
fps = vid_cap.get(cv2.CAP_PROP_FPS)
w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
vid_writer = cv2.VideoWriter(
save_path, cv2.VideoWriter_fourcc(*opt.fourcc),
fps, (w, h))
vid_writer.write(im0)
if save_txt or save_img:
print('Results saved to %s' % os.getcwd() + os.sep + out)
if platform == 'darwin': # MacOS
os.system('open ' + out + ' ' + save_path)
print('Done. (%.3fs)' % (time.time() - t0))
return
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
depth_sensor = pipe_profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# Map depth to color
depth_pixel = [240, 320] # Random pixel
# depth_pixel = [0,100]
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel,
depth_scale)
color_point = rs.rs2_transform_point_to_point(depth_to_color_extrin,
depth_point)
color_pixel = rs.rs2_project_point_to_pixel(color_intrin, color_point)
# print ('depth: ',color_point)
# print ('depth: ',color_pixel)
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
vtx = np.asanyarray(points.get_vertices())
tex = np.asanyarray(points.get_texture_coordinates())
img_color, top, bottle, right, left = circle_fit.my_fun(img_color)
print(
"------------------------------------------------------------------------------------------"
)
i_top = 640 * int(top[1]) + int(top[0])
print("i_top = ", i_top)
if top[1] > 0 and top[1] < 640 and top[0] > 0 and top[
def point_to_another_coord(extrinsics, deprojected_coordinates):
new_point = rs.rs2_transform_point_to_point(extrinsics,
deprojected_coordinates)
return new_point
def find_plane(points, depth_frame, rois, depth_intrin, depth_to_color_extrin):
left_point = [0, 0]
right_point = [0, 0]
third_point = [0, 0]
#left_point
for i in range(15, 100):
left_point = [points['corner2'][0] + i, points['corner2'][1] - i]
if not point_in_mask(left_point, rois) and depth_frame.get_distance(
int(left_point[0]), int(left_point[1])) != 0:
break
if left_point != [0, 0]:
depth_point1 = rs.rs2_deproject_pixel_to_point(
depth_intrin, left_point,
depth_frame.get_distance(int(left_point[0]), int(left_point[1])))
color_point1 = rs.rs2_transform_point_to_point(depth_to_color_extrin,
depth_point1)
else:
color_point1 = [0, 0, 0]
print('color_point1_transform fail')
#right_point
for i in range(15, 100):
right_point = [points['corner3'][0] - i, points['corner3'][1] - i]
if not point_in_mask(right_point, rois) and depth_frame.get_distance(
int(right_point[0]), int(right_point[1])) != 0:
break
if right_point != [0, 0]:
depth_point2 = rs.rs2_deproject_pixel_to_point(
depth_intrin, right_point,
depth_frame.get_distance(int(right_point[0]), int(right_point[1])))
color_point2 = rs.rs2_transform_point_to_point(depth_to_color_extrin,
depth_point2)
else:
color_point2 = [0, 0, 0]
print('color_point2_transform fail')
#third_point
"""
for i in range(15,200):
third_point=[points['corner1'][0]+i,points['corner1'][1]+i]
if not point_in_mask(third_point,rois) and depth_frame.get_distance(int(third_point[0]),int(third_point[1]))!=0:
break
if third_point!=[0,0]:
depth_point3=rs.rs2_deproject_pixel_to_point(depth_intrin, third_point, depth_frame.get_distance(int(third_point[0]),int(third_point[1])))
color_point3=rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point2)
else:
color_point3=[0,0,0]
print('color_point3_transform fail')
"""
if left_point != [0, 0] and right_point != [0, 0]:
for i in range(50, 200):
third_point = [
int((right_point[0] + left_point[0]) / 2),
int((right_point[1] + left_point[1]) / 2) - i
]
if not point_in_mask(
third_point, rois) and depth_frame.get_distance(
int(third_point[0]), int(third_point[1])) != 0:
break
if third_point != [0, 0]:
depth_point3 = rs.rs2_deproject_pixel_to_point(
depth_intrin, third_point,
depth_frame.get_distance(int(third_point[0]),
int(third_point[1])))
color_point3 = rs.rs2_transform_point_to_point(
depth_to_color_extrin, depth_point3)
else:
color_point3 = [0, 0, 0]
print('color_point3_transform fail')
else:
print('in depth_image,we can not find left or right point')
if left_point != [0, 0] and right_point != [0, 0
] and third_point != [0, 0]:
L1 = [
color_point2[0] - color_point1[0],
color_point2[1] - color_point1[1],
color_point2[2] - color_point1[2]
]
L2 = [
color_point3[0] - color_point1[0],
color_point3[1] - color_point1[1],
color_point3[2] - color_point1[2]
]
A = L1[1] * L2[2] - L1[2] * L2[1]
B = L1[2] * L2[0] - L1[0] * L2[2]
C = L1[0] * L2[1] - L1[1] * L2[0]
D = (A * color_point1[0] + B * color_point1[1] +
C * color_point1[2]) * (-1)
print("平面的参数", A, B, C, D)
return [A, B, C, D]
else:
return [0, 0, 0, 0]
def extract_based_pixel2(self, color_pixel):
self.depth_to_color_extrin = self.depth.profile.get_extrinsics_to(
self.color.profile)
self.color_to_depth_extrin = self.color.profile.get_extrinsics_to(
self.depth.profile)
# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
depth_sensor = self.profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
color_pixel1 = [color_pixel[0], color_pixel[1]]
color_pixel2 = [
color_pixel[0] + color_pixel[2], color_pixel[1] + color_pixel[3]
]
left_top_color_point = rs.rs2_deproject_pixel_to_point(
self.color_intrin, color_pixel1, 1.0)
left_top_depth_point = rs.rs2_transform_point_to_point(
self.color_to_depth_extrin, left_top_color_point)
left_top_depth_pixel = rs.rs2_project_point_to_pixel(
self.depth_intrin, left_top_depth_point)
right_down_color_point = rs.rs2_deproject_pixel_to_point(
self.color_intrin, color_pixel2, 1.0)
right_down_depth_point = rs.rs2_transform_point_to_point(
self.color_to_depth_extrin, right_down_color_point)
right_down_depth_pixel = rs.rs2_project_point_to_pixel(
self.depth_intrin, right_down_depth_point)
depth_image = np.asanyarray(self.depth.get_data())
height, width = depth_image.shape
lx = int(left_top_depth_pixel[0])
ly = int(left_top_depth_pixel[1])
rx = int(right_down_depth_pixel[0])
ry = int(right_down_depth_pixel[1])
index_max = np.argmax(depth_image[lx:rx + 1, ly:ry + 1])
index_min = np.argmin(depth_image[lx:rx + 1, ly:ry + 1])
print('index_max', index_max)
print('index_min', index_min)
index_max = lx * width + ly + index_max
index_min = lx * width + ly + index_min
pixel_max = [int(index_max / width), int(index_max % width)]
pixel_min = [int(index_min / width), int(index_min % width)]
scale1 = depth_image[lx, ly]
scale2 = depth_image[rx, ry]
scale3 = depth_image[pixel_max[0], pixel_max[1]]
scale4 = depth_image[pixel_min[0], pixel_min[1]]
print('scale1', scale1)
print('scale2', scale2)
print('scale3', scale3)
print('scale4', scale4)
point1 = rs.rs2_deproject_pixel_to_point(self.depth_intrin,
left_top_depth_pixel, scale1)
point2 = rs.rs2_deproject_pixel_to_point(self.depth_intrin,
right_down_depth_pixel,
scale2)
point3 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, pixel_max,
scale3)
point4 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, pixel_min,
scale4)
l1 = min(point1[0], point2[0]) * depth_scale
r1 = max(point1[0], point2[0]) * depth_scale
l2 = min(point1[1], point2[1]) * depth_scale
r2 = max(point1[1], point2[1]) * depth_scale
l3 = min(point1[2], point2[2]) * depth_scale
r3 = max(point1[2], point2[2]) * depth_scale
bounding_cube = [l1, r1, l2, r2, l3, r3]
depth_bbox = [
left_top_depth_pixel[0], left_top_depth_pixel[1],
right_down_depth_pixel[0], right_down_depth_pixel[1]
]
print(depth_bbox)
self.show_depth_bounding(depth_bbox)
# return [left_top_depth_pixel[0],left_top_depth_pixel[1],right_down_depth_pixel[0],right_down_depth_pixel[1]]
return bounding_cube
def extract_based_pixel(self, color_pixel):
self.depth_to_color_extrin = self.depth.profile.get_extrinsics_to(
self.color.profile)
self.color_to_depth_extrin = self.color.profile.get_extrinsics_to(
self.depth.profile)
# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
depth_sensor = self.profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# depth = depth_frame.get_distance(j, i)
# depth_point = rs.rs2_deproject_pixel_to_point(
# depth_intrin, [j, i], depth)
# text = "%.5lf, %.5lf, %.5lf\n" % (
# depth_point[0], depth_point[1], depth_point[2])
color_pixel1 = [color_pixel[0], color_pixel[1]]
color_pixel2 = [
color_pixel[0] + color_pixel[2], color_pixel[1] + color_pixel[3]
]
left_top_color_point = rs.rs2_deproject_pixel_to_point(
self.color_intrin, color_pixel1, 1.0)
left_top_depth_point = rs.rs2_transform_point_to_point(
self.color_to_depth_extrin, left_top_color_point)
left_top_depth_pixel = rs.rs2_project_point_to_pixel(
self.depth_intrin, left_top_depth_point)
right_down_color_point = rs.rs2_deproject_pixel_to_point(
self.color_intrin, color_pixel2, 1.0)
right_down_depth_point = rs.rs2_transform_point_to_point(
self.color_to_depth_extrin, right_down_color_point)
right_down_depth_pixel = rs.rs2_project_point_to_pixel(
self.depth_intrin, right_down_depth_point)
depth_image = np.asanyarray(self.depth.get_data()).T
height, width = depth_image.shape
lx = int(left_top_depth_pixel[0])
ly = int(left_top_depth_pixel[1])
rx = int(right_down_depth_pixel[0])
ry = int(right_down_depth_pixel[1])
depth_bbox = [
left_top_depth_pixel[0], left_top_depth_pixel[1],
right_down_depth_pixel[0], right_down_depth_pixel[1]
]
print(color_pixel)
print(depth_bbox)
self.show_depth_bounding(depth_bbox)
scale1 = depth_image[lx, ly]
scale2 = depth_image[rx, ry]
scale_max = np.max(depth_image[lx:rx, ly:ry])
scale_mean = np.median(depth_image[lx:rx, ly:ry])
#scale_mean = np.mean(depth_image[lx:rx,ly:ry])
#rs.depth_frame.get_distance(x, y)
high = max(scale1, scale2)
low = min(scale1, scale2)
num = (rx - lx) * (ry - ly)
points = np.zeros([num, 3]).astype(np.float32)
index = 0
for i in range(lx, rx):
for j in range(ly, ry):
scale = depth_image[i, j]
if scale > 0.01 and scale < scale_mean * 1.1:
point = rs.rs2_deproject_pixel_to_point(
self.depth_intrin, [i, j], scale)
points[index, 0] = point[0]
points[index, 1] = point[1]
points[index, 2] = point[2]
index = index + 1
points = points[0:index, :] * depth_scale
#print(point.shape)
# centroid = np.mean(points, axis=0, keepdims=True)
# furthest_distance = np.amax(np.sqrt(np.sum((points - centroid) ** 2, axis=-1)), keepdims=True)
#
# points = (points - centroid) / furthest_distance
# points[:, 1:3] = -points[:, 1:3]
return points
