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 point_to_image(self, bb_data, circle_color=(0, 255, 0)):
"""Draws a 3D point on the color image.
Converts the given 3D point back to a pixel
and draws it on the color image as a small circle.
Parameters
----------
bb_data : tuple
A sextuple that contains the x, y, z
and the respective dimensions of a bounding box.
Only the first three elements (x, y, z) are needed.
circle_color : tuple, optional
by default green (0, 255, 0)
"""
pixel = rs2.rs2_project_point_to_pixel(self.intrinsics, bb_data[:3])
if self.debug:
rospy.loginfo("BB median center xy:" + str(round(pixel[0])) + " " +
str(round(pixel[1])))
cv2.circle(
self.color_image,
(
round(pixel[0]),
round(pixel[1]),
),
5,
circle_color,
2,
)
def torso3DToPixel(self,
depth_intrin,
position,
toRobRotation=np.eye(3),
toRobTranslation=np.array([0, 0, 0])):
position = np.dot(np.linalg.inv(toRobRotation),
(position - toRobTranslation))
return rs.rs2_project_point_to_pixel(depth_intrin, position.tolist())
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 compute_feedback_vector(depth_frame: rs.depth_frame, pos3d: (float, float,
float)):
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
x, y = rs.rs2_project_point_to_pixel(depth_intrin, pos3d)
plane_depth = depth_frame.as_depth_frame().get_distance(int(x),
int(y)) * 1000
if plane_depth > pos3d[2]:
return np.array((0, 0, 0))
dist_beyond = pos3d[2] - plane_depth
return dist_beyond * compute_normal_vector(depth_frame, x, y)
def depth_distance(self, x, y):
depth_intrin = self.depth_intrin
#ix, iy = self.ix, self.iy
#depth_scale = self.depth_sensor.get_depth_scale()
udist = self.depth_frame.get_distance(x, y) #in meters
point = rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], udist)
point_array = np.asanyarray(point)
point_to_pixel = rs.rs2_project_point_to_pixel(depth_intrin, point)
print(point)
print(point_to_pixel)
return point_array
def generate_frame_flat_surface(width: float, height: float, distance: float, intrinsics: rs2.intrinsics) -> np.ndarray:
"""
:param width: The width of the flat surface in meters
:param height: The height of the flat surface in meters
:param distance: The distance of the object from the camera
:param intrinsics: The intrinsics of the camera that the object should be 'captured' from
:return: The frame that should have been captured if that surface was captured from that camera from that distance
"""
frame = np.zeros(shape=(intrinsics.height, intrinsics.width))
max_j, max_i = rs2.rs2_project_point_to_pixel(intrinsics, [width / 2, height / 2, distance])
max_i, max_j = int(min(max_i, intrinsics.height)), int(min(max_j, intrinsics.width))
for i in range(intrinsics.height - max_i, max_i):
for j in range(intrinsics.width - max_j, max_j):
frame[i][j] = distance
return frame
def convert_pc_to_frame(pc: np.ndarray, intrinsics: rs2.intrinsics) -> np.ndarray:
"""
:param pc: A point cloud
:param intrinsics: The intrinsics of the camera that the object should be 'captured' from
:return: The frame that would give that point cloud if it was captured from that camera
"""
assert len(pc.shape) == 2
assert pc.shape[1] == 3
frame = np.zeros(shape=(intrinsics.width, intrinsics.height))
for (x, y, z) in pc:
j, i = rs2.rs2_project_point_to_pixel(intrinsics, [x, y, z])
# Maybe should use int(i),int(j) because of collisions
i, j = round(i), round(j)
frame[i][j] = z
return frame
def deproject_ros_point_to_frame_point(target_point, frame_set, camera_frame):
'''
Converts a 3-D camera-relative point to a 2-D pixel coordinate in frame
@param target_point - ROS point that is being converted.
@param frame_set - the latest VisionFrameSet object
@param camera_frame - the TF frame that represents the camera's frame of reference
@return the 3-D point in ROS coordinates: +x forward, +y left, +z up
'''
global cameraInfo
target_point_cam_rel = transform_to_frame(target_point, camera_frame)
if not target_point_cam_rel:
return None
rs2_point = [
# convert from right-handed z-up to right-handed z-forward
-target_point_cam_rel.point.y * 1000,
-target_point_cam_rel.point.z * 1000,
target_point_cam_rel.point.x * 1000,
]
return tuple(
rs.rs2_project_point_to_pixel(cameraInfo.get_color_intrinsics(),
rs2_point))
def project(self, Pc):
'''
Project a 3D camera coordinates point Pc [X Y Z] into the picture frame coordinates pixel (x,y)
'''
return tuple([ int(x) for x in rs.rs2_project_point_to_pixel(self.intrinsics, list(Pc)) ])
def markerprocess(self):
# Capture the frame first
color_image, depth_image, depth_frame, color_frame = self._capture()
# Aruco marker part
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv2.aruco.detectMarkers(
color_image, self.dictionary, parameters=self.parameters)
aruco_list = {}
# centre= {}
result_center = {}
# orient_centre= {}
if markerIds is not None:
# Print corners and ids to the console
# result=zip(markerIds, markerCorners)
for k in range(len(markerCorners)):
temp_1 = markerCorners[k]
temp_1 = temp_1[0]
temp_2 = markerIds[k]
temp_2 = temp_2[0]
aruco_list[temp_2] = temp_1
key_list = aruco_list.keys()
font = cv2.FONT_HERSHEY_SIMPLEX
# print(key_list)
for key in key_list:
dict_entry = aruco_list[key]
centre = dict_entry[0] + dict_entry[1] + dict_entry[
2] + dict_entry[3]
centre[:] = [int(x / 4) for x in centre]
# orient_centre = centre + [0.0,5.0]
centre = tuple(centre)
result_center[key] = centre
# orient_centre = tuple((dict_entry[0]+dict_entry[1])/2)
cv2.circle(color_image, centre, 1, (0, 0, 255), 8)
# Compute distance when matching the conditions
# print(result_center)
# print(len(result_center))
if len(result_center) < 4:
print("No enough marker detected")
if len(result_center) >= 4:
# To avoid keyerror
# start = time.time()
try:
# Moving object localization marker
x_id0 = result_center[0][0]
y_id0 = result_center[0][1]
p_0 = [x_id0, y_id0]
# Target object localization marker
# Single ID-5
x_id5 = result_center[5][0]
y_id5 = result_center[5][1]
p_5 = [x_id5, y_id5]
# Dual ID-4 and ID-3
x_id4 = result_center[4][0]
y_id4 = result_center[4][1]
p_4 = [x_id4, y_id4]
x_id3 = result_center[3][0]
y_id3 = result_center[3][1]
p_3 = [x_id3, y_id3]
# Deproject pixel to 3D point
point_0 = self.pixel2point(depth_frame, p_0)
point_5 = self.pixel2point(depth_frame, p_5)
point_4 = self.pixel2point(depth_frame, p_4)
point_3 = self.pixel2point(depth_frame, p_3)
# Calculate target point
point_target = [
point_4[0] + point_3[0] - point_5[0],
point_4[1] + point_3[1] - point_5[1],
point_4[2] + point_3[2] - point_5[2]
]
# Compute distance
# dis=distance_3dpoints(point_target,point_0)
# Display target and draw a line between them
color_intrin = color_frame.profile.as_video_stream_profile(
).intrinsics
target_pixel = rs.rs2_project_point_to_pixel(
color_intrin, point_target)
target_pixel[0] = int(target_pixel[0])
target_pixel[1] = int(target_pixel[1])
cv2.circle(color_image, tuple(target_pixel), 1,
(0, 0, 255), 8)
cv2.line(color_image, tuple(p_0), tuple(target_pixel),
(0, 255, 0), 2)
# Euclidean distance
dis_obj2target = self.distance_3dpoints(
point_0, point_target)
dis_obj2target_goal = dis_obj2target * np.sin(
np.arccos(0.02 / dis_obj2target))
# print(dis_obj2target_goal)
# images = cv2.aruco.drawDetectedMarkers(images, markerCorners, borderColor=(0, 0, 255))
# end = time.time()
# print(str(end-start))
except KeyError:
print("Keyerror!!!")
# Outline all of the markers detected in our image
# images = cv2.aruco.drawDetectedMarkers(images, markerCorners, borderColor=(0, 0, 255))
# self.images = color_image
self.images = cv2.aruco.drawDetectedMarkers(color_image,
markerCorners,
borderColor=(0, 0, 255))
return self.images
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
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
# 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)
cv2.waitKey(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
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_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[
0] < 480 and i_top < 300000:
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 point_projection(self, pixel_3d):
depth_intrin = self.filtered.profile.as_video_stream_profile(
).intrinsics
return rs.rs2_project_point_to_pixel(depth_intrin, pixel_3d)
def start_pipeline():
save_path = "./out"
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)
pipeline.start(config)
print('[INFO] Starting pipeline')
profile = pipeline.get_active_profile()
# profile = pipeline.start(config)
depth_profile = rs.video_stream_profile(profile.get_stream(
rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
print("[INFO] Width: ", w, "Height: ", h)
fx, fy, = depth_intrinsics.fx, depth_intrinsics.fy
print("[INFO] Focal length X: ", fx)
print("[INFO] Focal length Y: ", fy)
ppx, ppy = depth_intrinsics.ppx, depth_intrinsics.ppy
print("[INFO] Principal point X: ", ppx)
print("[INFO] Principal point Y: ", ppy)
# Identifying depth scaling factor
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
pc = rs.pointcloud()
decimate = rs.decimation_filter()
spatial = rs.spatial_filter
hole = rs.hole_filling_filter
temporal = rs.temporal_filter()
colorizer = rs.colorizer()
align_to = rs.stream.color
align = rs.align(align_to)
frame_counter = 0
try:
print("[INFO] Starting pipeline stream with frame {:d}".format(
frame_counter))
while True:
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
aligned_depth_frame = aligned_frames.get_depth_frame()
aligned_color_frame = aligned_frames.get_color_frame()
depth_colormap = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
color_image = np.asanyarray(aligned_color_frame.get_data())
pts = pc.calculate(aligned_depth_frame)
pc.map_to(aligned_color_frame)
if not aligned_depth_frame or not aligned_color_frame:
print("No depth or color frame, try again...")
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data())
images = np.hstack(
(cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB), depth_colormap))
cv2.imshow(str(frame_counter), images)
key = cv2.waitKey(1)
if key == ord("d"):
landmarks = landmark_detector(color_image)
for n, px in landmarks.items():
z = aligned_depth_frame.get_distance(px[0], px[1])
# Test Landmarks px = [width, height]
pt = rs.rs2_deproject_pixel_to_point(
depth_intrinsics, [px[0], px[1]], z)
w, h = rs.rs2_project_point_to_pixel(depth_intrinsics, pt)
print("Original Pixel:", px)
print("Deprojected Pixel:", [w, h])
return px, w, h
except Exception as ex:
print(ex)
finally:
pipeline.stop()
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
point_5 = pixel2point(depth_frame, p_5)
point_4 = pixel2point(depth_frame, p_4)
point_3 = pixel2point(depth_frame, p_3)
# Calculate target point
point_target = [
point_4[0] + point_3[0] - point_5[0],
point_4[1] + point_3[1] - point_5[1],
point_4[2] + point_3[2] - point_5[2]
]
# Compute distance
# dis=distance_3dpoints(point_target,point_0)
# Display target and draw a line between them
color_intrin = color_frame.profile.as_video_stream_profile(
).intrinsics
target_pixel = rs.rs2_project_point_to_pixel(
color_intrin, point_target)
target_pixel[0] = int(target_pixel[0])
target_pixel[1] = int(target_pixel[1])
cv2.circle(images, tuple(target_pixel), 1,
(0, 0, 255), 8)
cv2.line(images, tuple(p_0), tuple(target_pixel),
(0, 255, 0), 2)
# Euclidean distance
dis_obj2target = distance_3dpoints(
point_0, point_target)
dis_obj2target_goal = dis_obj2target * np.sin(
np.arccos(0.02 / dis_obj2target))
print(dis_obj2target_goal)
useful_color_image = color_image
