def _prep_configuration(
self,
color_frame_size=None,
color_fps=None,
depth_frame_size=None,
depth_fps=None,
):
config = rs.config()
# only use these two formats
color_format = rs.format.yuyv
depth_format = rs.format.z16
config.enable_stream(
rs.stream.depth,
depth_frame_size[0],
depth_frame_size[1],
depth_format,
depth_fps,
)
config.enable_stream(
rs.stream.color,
color_frame_size[0],
color_frame_size[1],
color_format,
color_fps,
)
return config
def is_streaming(device_id):
try:
c = rs.config()
c.enable_device(device_id) # device_id is in fact the serial_number
p = rs.pipeline()
p.start(c)
p.stop()
return False
except RuntimeError:
return True
def _get_default_config(self):
config = rs.config() # default config is RGB8, we want YUYV
config.enable_stream(
rs.stream.color,
DEFAULT_COLOR_SIZE[0],
DEFAULT_COLOR_SIZE[1],
rs.format.yuyv,
DEFAULT_COLOR_FPS,
)
config.enable_stream(
rs.stream.depth,
DEFAULT_DEPTH_SIZE[0],
DEFAULT_DEPTH_SIZE[1],
rs.format.z16,
DEFAULT_DEPTH_FPS,
)
return config
def pyreassenseCap():
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.imshow('depth_image',depth_image)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
finally:
# Stop streaming
pipeline.stop()
def __init__(self, context, pipeline_configuration):
"""
Class to manage the Intel RealSense devices
Parameters:
-----------
context : rs.context()
The context created for using the realsense library
pipeline_configuration : rs.config()
The realsense library configuration to be used for the application
"""
assert isinstance(context, type(rs.context()))
assert isinstance(pipeline_configuration, type(rs.config()))
self._context = context
self._available_devices = enumerate_connected_devices(context)
self._enabled_devices = {}
self._config = pipeline_configuration
self._frame_counter = 0
def enable_imu_device(self, serial_no):
self.pipeline = rs.pipeline()
cfg = rs.config()
cfg.enable_device(serial_no)
try:
self.pipeline.start(cfg)
except Exception as e:
print('ERROR: ', str(e))
return False
# self.sync_imu_by_this_stream = rs.stream.any
active_imu_profiles = []
active_profiles = dict()
self.imu_sensor = None
for sensor in self.pipeline.get_active_profile().get_device().sensors:
for pr in sensor.get_stream_profiles():
if pr.stream_type() == rs.stream.gyro and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if pr.stream_type() == rs.stream.accel and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if self.imu_sensor:
break
if not self.imu_sensor:
print('No IMU sensor found.')
return False
print('\n'.join(['FOUND %s with fps=%s' % (str(ap[0]).split('.')[1].upper(), ap[1].fps()) for ap in active_profiles.items()]))
active_imu_profiles = list(active_profiles.values())
if len(active_imu_profiles) < 2:
print('Not all IMU streams found.')
return False
self.imu_sensor.stop()
self.imu_sensor.close()
self.imu_sensor.open(active_imu_profiles)
self.imu_start_loop_time = time.time()
self.imu_sensor.start(self.imu_callback)
# Make the device use the original IMU values and not already calibrated:
if self.imu_sensor.supports(rs.option.enable_motion_correction):
self.imu_sensor.set_option(rs.option.enable_motion_correction, 0)
return True
def run_demo():
# Define some constants
resolution_width = 1280 # pixels
resolution_height = 720 # pixels
frame_rate = 15 # fps
dispose_frames_for_stablisation = 30 # frames
chessboard_width = 6 # squares
chessboard_height = 9 # squares
square_size = 0.0253 # meters
try:
# Enable the streams from all the intel realsense devices
rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, resolution_width,
resolution_height, rs.format.z16, frame_rate)
rs_config.enable_stream(rs.stream.infrared, 1, resolution_width,
resolution_height, rs.format.y8, frame_rate)
rs_config.enable_stream(rs.stream.color, resolution_width,
resolution_height, rs.format.bgr8, frame_rate)
# Use the device manager class to enable the devices and get the frames
device_manager = DeviceManager(rs.context(), rs_config)
device_manager.enable_all_devices()
# Allow some frames for the auto-exposure controller to stablise
for frame in range(dispose_frames_for_stablisation):
frames = device_manager.poll_frames()
assert (len(device_manager._available_devices) > 0)
"""
1: Calibration
Calibrate all the available devices to the world co-ordinates.
For this purpose, a chessboard printout for use with opencv based calibration process is needed.
"""
# Get the intrinsics of the realsense device
intrinsics_devices = device_manager.get_device_intrinsics(frames)
# Set the chessboard parameters for calibration
chessboard_params = [chessboard_height, chessboard_width, square_size]
# Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
calibrated_device_count = 0
while calibrated_device_count < len(device_manager._available_devices):
frames = device_manager.poll_frames()
pose_estimator = PoseEstimation(frames, intrinsics_devices,
chessboard_params)
transformation_result_kabsch = pose_estimator.perform_pose_estimation(
)
object_point = pose_estimator.get_chessboard_corners_in3d()
calibrated_device_count = 0
for device in device_manager._available_devices:
if not transformation_result_kabsch[device][0]:
print(
"Place the chessboard on the plane where the object needs to be detected.."
)
else:
calibrated_device_count += 1
# Save the transformation object for all devices in an array to use for measurements
transformation_devices = {}
chessboard_points_cumulative_3d = np.array([-1, -1, -1]).transpose()
for device in device_manager._available_devices:
transformation_devices[device] = transformation_result_kabsch[
device][1].inverse()
points3D = object_point[device][2][:, object_point[device][3]]
points3D = transformation_devices[device].apply_transformation(
points3D)
chessboard_points_cumulative_3d = np.column_stack(
(chessboard_points_cumulative_3d, points3D))
# Extract the bounds between which the object's dimensions are needed
# It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
chessboard_points_cumulative_3d = np.delete(
chessboard_points_cumulative_3d, 0, 1)
roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)
print(
"Calibration completed... \nPlace the box in the field of view of the devices..."
)
"""
2: Measurement and display
Measure the dimension of the object using depth maps from multiple RealSense devices
The information from Phase 1 will be used here
"""
# Enable the emitter of the devices
device_manager.enable_emitter(True)
# Load the JSON settings file in order to enable High Accuracy preset for the realsense
device_manager.load_settings_json("./HighResHighAccuracyPreset.json")
# Get the extrinsics of the device to be used later
extrinsics_devices = device_manager.get_depth_to_color_extrinsics(
frames)
# Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
calibration_info_devices = defaultdict(list)
for calibration_info in (transformation_devices, intrinsics_devices,
extrinsics_devices):
for key, value in calibration_info.items():
calibration_info_devices[key].append(value)
# Continue acquisition until terminated with Ctrl+C by the user
while 1:
# Get the frames from all the devices
frames_devices = device_manager.poll_frames()
# Calculate the pointcloud using the depth frames from all the devices
point_cloud = calculate_cumulative_pointcloud(
frames_devices, calibration_info_devices, roi_2D)
# Get the bounding box for the pointcloud in image coordinates of the color imager
bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(
point_cloud, calibration_info_devices)
# Draw the bounding box points on the color image and visualise the results
visualise_measurements(frames_devices,
bounding_box_points_color_image, length,
width, height)
except KeyboardInterrupt:
print("The program was interupted by the user. Closing the program...")
finally:
device_manager.disable_streams()
cv2.destroyAllWindows()
def collectTestingX():
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 #1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# 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)
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# frames.get_depth_frame() is a 640x360 depth image
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame(
) # aligned_depth_frame is a 640x480 depth image
color_frame = aligned_frames.get_color_frame()
depth_image = np.asanyarray(aligned_depth_frame.get_data())
depth_image = removeBackground(depth_image)
depth = frames.get_depth_frame()
closest = 10000
for y in range(480):
for x in range(640):
curr = depth.get_distance(x, y)
closest = curr if curr < closest and curr != 0 else closest
color_image = np.asanyarray(color_frame.get_data())
# Remove background - Set pixels further than clipping_distance to grey
grey_color = 153
depth_image_3d = np.dstack(
(depth_image, depth_image,
depth_image)) #depth image is 1 channel, color is 3 channels
bg_removed = np.where(
(depth_image_3d > clipping_distance) | (depth_image_3d <= 0),
grey_color, color_image)
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
outputDataToFileStructure(depth_colormap, bg_removed)
pipeline.stop()
return (depth_colormap, closest)
def grid():
d_pub = rospy.Publisher('depth_frame', Twist, queue_size=10)
i_pub = rospy.Publisher('rgb_frame', Image, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
try:
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 15)
# Start streaming
pipeline.start(config)
# filters
hole_filling = rs.hole_filling_filter()
# get camera intrinsics
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
col_length = int(640*(1.0/8.0))
row_length = int(480*(1.0/5.0))
bridge = CvBridge()
while not rospy.is_shutdown():
# This call waits until a new coherent set of frames is available on a devicepip3 install opencv-python
# Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable
print('Getting frame data now')
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
# depth_frame = hole_filling.process(depth_frame)
if not depth_frame or not color_frame:
continue
depth_image = np.asanyarray(depth_frame.get_data())
rgb_image = np.asanyarray(color_frame.get_data())
left_image = depth_image[ 2*row_length:3*row_length , 1*col_length:2*col_length ]
left_distances = depth_scale*left_image
left_distances_filtered = left_distances[left_distances > 0]
center_image = depth_image[ 2*row_length: 3*row_length, 3*col_length:5*col_length]
center_distances = depth_scale*center_image
center_distances_filtered = center_distances[center_distances > 0]
right_image = depth_image[ 2*row_length:3*row_length, 6*col_length:7*col_length ]
right_distances = depth_scale*right_image
right_distances_filtered = right_distances[right_distances > 0]
# right_distances_projected = right_distances_filtered*math.sin(rad(42.6))
mean = Twist()
mean.linear.x = np.mean(left_distances_filtered)
mean.linear.y = np.mean(center_distances_filtered)
mean.linear.z = np.mean(right_distances_filtered)
"""
if DRAW_GRID:
color_image = np.asanyarray(color_frame.get_data())
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
grid(color_image, w_portion, h_portion, 255, 255, 2)
images = np.hstack((color_image, depth_colormap))
cv2.imshow('RealSense', images)
cv2.waitKey(1)
"""
d_pub.publish(mean)
i_pub.publish(bridge.cv2_to_imgmsg(rgb_image, "rgb8"))
rate.sleep()
pipeline.stop()
except Exception as e:
print('except : ', e)
pass
def main():
window_name = 'capture'
result_name = 'perspective'
USE_REAL_CAMERA = False
try:
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
if USE_REAL_CAMERA:
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
else:
config.enable_device_from_file(BAG_FILE)
# Start streaming
pipeline.start(config)
cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
cv2.moveWindow(window_name, 0, 0)
cv2.namedWindow(result_name, cv2.WINDOW_AUTOSIZE)
cv2.moveWindow(result_name, 640, 0)
cnt = 0
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
color_image = np.asanyarray(color_frame.get_data())
#depth_image = np.asanyarray(depth_frame.get_data())
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
#depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Grab new intrinsics (may be changed by decimation)
depth_intrinsics = rs.video_stream_profile(depth_frame.profile).get_intrinsics()
original = color_image.copy()
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
ret, crnrs = cv2.findChessboardCorners(color_image, SIZE, cv2.CALIB_CB_FAST_CHECK)
#cv2.drawChessboardCorners(color_image, SIZE, crnrs, ret)
# draw point of corners
if ret:
fourc = ck.get_poi(crnrs)
ck.draw_poi(color_image, fourc)
fourxyz = get_poi_rs_dist(fourc, depth_frame, depth_intrinsics)
msg0 = "{:.2f} x {:.2f} mm".format(ck.px_3d_dist(fourxyz[0], fourxyz[1]), ck.px_3d_dist(fourxyz[1], fourxyz[2]))
ck.put_mytext(color_image, msg0)
# perfrom warp perspective distortion
if ret:
cppts = ck.get_ppts(crnrs)
M = ck.get_perspective_mat(cppts, DST_W, DST_H)
perspective = cv2.warpPerspective(color_image, M, (DST_W, DST_H), cv2.INTER_LINEAR)
cv2.imshow(result_name, perspective)
if False and ret:
p0 = crnrs[0][0]
p1 = crnrs[1][0]
print("p0:{} p1:{}".format(p0, p1))
#dist_from_rs = depth_frame.get_distance(int(marker[0][0]), int(marker[0][1]))
dist_from_rs = depth_frame.get_distance(p0[0], p0[1])
dist_from_rs *= 1000
#print('marker:{} dist_rs:{}'.format(marker, dist_from_rs))
# cv2.putText(color_image, "%4.0fmm" % dist,
# (color_image.shape[1] - 500, color_image.shape[0] - 60),
# cv2.FONT_HERSHEY_SIMPLEX,
# 1.2, (0, 255, 0), 3)
cv2.putText(color_image, "%4dmm" % dist_from_rs,
(color_image.shape[1] - 500, color_image.shape[0] - 20),
cv2.FONT_HERSHEY_SIMPLEX,
1.1, (0, 255, 255), 3)
# show a frame
cv2.imshow(window_name, color_image)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 0x1B:
break
elif key & 0xFF == ord('s'):
fn = 'result-{:02d}.png'.format(cnt)
print('save result to {}'.format(fn))
cv2.imwrite(fn, color_image)
cv2.imwrite('origin-{:02d}.png'.format(cnt), original)
cnt += 1
elif key == 0x20:
cv2.waitKey(0)
finally:
cv2.destroyAllWindows()
# Stop streaming
pipeline.stop()
import math
import time
import pyrealsense2 as rs
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
pipe.start(cfg)
def quaternion_to_euler(x, y, z, w):
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = math.degrees(math.asin(t2))
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
Z = math.degrees(math.atan2(t3, t4))
return X, Y, Z
while True:
frames = pipe.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
Alpha, Beta, Gamma = quaternion_to_euler(data.rotation.x,
data.rotation.y,
data.rotation.z,
def detect():
source, weights, view_img, imgsz,save_vidio,detect_image,detect_vidio,webcam,dist_thres_lim,savevid_f = opt.source, opt.weights, opt.view_img,\
opt.img_size,opt.save_vidio,opt.detect_img,opt.detect_vidio,\
opt.detect_webcam,opt.dist_thres_lim,opt.savevid_f
set_logging()
device = select_device(opt.device)
half = device.type != 'cpu' # half precision only supported on CUDA
# Load model
model = attempt_load(weights, map_location=device) # load FP33 model
stride = int(model.stride.max()) # model stride
imgsz = check_img_size(imgsz, s=stride) # check img_size
if half:
model.half() # to FP16
pipe = rs.pipeline()
cfg = rs.config()
if not webcam:
cfg.enable_device_from_file(file_name=source)
cfg.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)
cfg.enable_stream(rs.stream.color, 848, 480, rs.format.rgb8, 30)
pipe.start(cfg)
a = 0
names = model.module.names if hasattr(model, 'module') else model.names
colors = [[random.randint(0, 255) for _ in range(3)] for _ in names]
# Run inference
if device.type != 'cpu':
model(
torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(
next(model.parameters()))) # run once
start = time.time()
colorizer = rs.colorizer()
hole_filling = rs.hole_filling_filter()
try:
while True:
if detect_image:
im0s, depth, colorized_depth = detects_img(
pipe, hole_filling, colorizer)
if detect_vidio:
im0s, depth, colorized_depth = detects_vidio(
pipe, hole_filling, colorizer)
img = read_image(im0s)
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 = time_synchronized()
pred = model(img, augment=opt.augment)[0]
# Apply NMS
pred = non_max_suppression(pred,
opt.conf_thres,
opt.iou_thres,
classes=opt.classes,
agnostic=opt.agnostic_nms)
# all_coords = []
# t3 = time.time()
for _, det in enumerate(pred):
if len(det):
det = det.cpu()
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(img.shape[2:], det[:, :4],
im0s.shape).round()
for *xyxy, conf, cls in reversed(det):
depth_temp = depth[int(xyxy[1]):int(xyxy[3]),
int(xyxy[0]):int(xyxy[2])].astype(float)
depth_temp = depth_temp * 0.001
dist, _, _, _ = cv2.mean(depth_temp)
dist_temp = Decimal(dist).quantize(Decimal('0.000'))
y_mid = (int(xyxy[1]) + int(xyxy[3])) / 2
x_mid = (xyxy[0] + xyxy[2]) / 2
len_x = np.cumsum(
depth[int(y_mid),
int(xyxy[0]):int(xyxy[2])])[-1] * 0.00165
len_y = np.cumsum(depth[int(xyxy[1]):int(xyxy[3]),
int(x_mid)])[-1] * 0.00165
label1 = str(dist_temp) + "m" + str(
int(len_x)) + "mm" + ',' + str(int(len_y)) + "mm"
# all_coords.append(xyxy)
label = f'{names[int(cls)]} {conf:.2f}'
plot_one_box(xyxy,
im0s,
label=label,
color=colors[int(cls)],
line_thickness=3)
# plot_dots_on_people(xyxy, im0s)
plot_one_box(xyxy,
colorized_depth,
label=label1,
color=colors[int(cls)],
line_thickness=2)
# distancing_all(all_coords, im0s,depth=depth,dist_thres_lim=dist_thres_lim)
images = np.hstack((cv2.cvtColor(im0s, cv2.COLOR_RGB2BGR),
cv2.cvtColor(colorized_depth,
cv2.COLOR_RGB2BGR)))
a += 1
# if a %1 ==0:
print("\r>>>FPS:{:.2f}<<< ".format(a / (time.time() - start)),
end="")
if view_img:
cv2.imshow("RealSense", images)
# t6 = time.time()
# print("cv2imshow() time :" + str(t6 - t4))
# 按下“q”键停止q
if cv2.waitKey(1) & 0xFF == ord('q'):
# cv2.destroyAllWindows()
break
if detect_image:
cv2.imwrite("out_img.png", images)
break
if save_vidio:
# if save_vidio and a!=savevid_f:
if a != savevid_f:
fps, w, h = 30, images.shape[1], images.shape[0]
# save_path += '.mp4'
if a == 1:
vid_writer = cv2.VideoWriter(
"test.mp4", cv2.VideoWriter_fourcc(*'mp4v'), fps,
(w, h))
vid_writer.write(images)
# print("\r video [{}/{}] save path{} ".format(a,300,save_vidio),end="")
else:
print(" Done,vidio save to{}, time{:.2f}".format(
"./test.mp4",
time.time() - start))
# vid_writer.release()
cv2.destroyAllWindows()
break
# t7 =time.time()
# print('cv2.waitKey()' + str(t7 - t6))
finally:
# pipe.stop()
pass
# print(util.serial_number(pipeline.start(config).get_device()))
#
# show_multi_cam(pipelines)
#
# finally:
# # Start streaming from both cameras
# for pipeline, _ in pipelines:
# pipeline.stop()
# capture_one_cam(cams_ids[0])
pc: rs2.pointcloud = rs2.pointcloud()
# points = rs2.points()
pipe: rs2.pipeline = rs2.pipeline()
config: rs2.config = rs2.config()
config.enable_stream(rs2.stream.color, 640, 480, rs2.format.rgb8, 30)
config.enable_stream(rs2.stream.depth, 640, 480, rs2.format.z16, 30)
pipe.start(config)
while True:
frames: rs2.composite_frame = pipe.wait_for_frames()
color: rs2.video_frame = frames.get_color_frame()
depth: rs2.depth_frame = frames.get_depth_frame()
if (not depth) or (not color):
continue
points: rs2.points = pc.calculate(depth)
color = frames.get_color_frame()
pc.map_to(color)
vertices = np.asanyarray(points.get_vertices())
import cv2
import time
# check the number of connected devices
devices_ = rs.context().query_devices()
print('Found {} connected devices'.format(len(devices_)))
serial_numbers = []
for dev_ in devices_:
serial_numbers.append(dev_.get_info(rs.camera_info.serial_number))
pipelines = []
configs = []
for idx in range(len(devices_)):
pipelines.append(rs.pipeline())
configs.append(rs.config())
configs[idx].enable_device(serial_numbers[idx])
configs[idx].enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
pipelines[idx].start(configs[idx])
save_image = False
current_idx = None
try:
while True:
for idx in range(len(devices_)):
current_idx = idx
frames = pipelines[idx].wait_for_frames()
depth_frame = frames.get_depth_frame()
if not depth_frame:
print('No data received')
continue
def camThread(LABELS, results, frameBuffer, camera_mode, camera_width,
camera_height, background_transparent_mode, background_img):
global fps
global detectfps
global lastresults
global framecount
global detectframecount
global time1
global time2
global cam
global window_name
global depth_scale
global align_to
global align
# Configure depth and color streams
# Or
# Open USB Camera streams
if camera_mode == 0:
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
align_to = rs.stream.color
align = rs.align(align_to)
window_name = "RealSense"
elif camera_mode == 1:
cam = cv2.VideoCapture(0)
if cam.isOpened() != True:
print("USB Camera Open Error!!!")
sys.exit(0)
cam.set(cv2.CAP_PROP_FPS, 30)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, camera_width)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_height)
window_name = "USB Camera"
cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
while True:
t1 = time.perf_counter()
# 0:= RealSense Mode
# 1:= USB Camera Mode
if camera_mode == 0:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
if frameBuffer.full():
frameBuffer.get()
color_image = np.asanyarray(color_frame.get_data())
elif camera_mode == 1:
# USB Camera Stream Read
s, color_image = cam.read()
if not s:
continue
if frameBuffer.full():
frameBuffer.get()
frames = color_image
height = color_image.shape[0]
width = color_image.shape[1]
frameBuffer.put(color_image.copy())
res = None
if not results.empty():
res = results.get(False)
detectframecount += 1
imdraw = overlay_on_image(frames,
res,
LABELS,
camera_mode,
background_transparent_mode,
background_img,
depth_scale=depth_scale,
align=align)
lastresults = res
else:
imdraw = overlay_on_image(frames,
lastresults,
LABELS,
camera_mode,
background_transparent_mode,
background_img,
depth_scale=depth_scale,
align=align)
cv2.imshow(window_name, cv2.resize(imdraw, (width, height)))
if cv2.waitKey(1) & 0xFF == ord('q'):
# Stop streaming
if pipeline != None:
pipeline.stop()
sys.exit(0)
## Print FPS
framecount += 1
if framecount >= 15:
fps = "(Playback) {:.1f} FPS".format(time1 / 15)
detectfps = "(Detection) {:.1f} FPS".format(detectframecount /
time2)
framecount = 0
detectframecount = 0
time1 = 0
time2 = 0
t2 = time.perf_counter()
elapsedTime = t2 - t1
time1 += 1 / elapsedTime
time2 += elapsedTime
def run_demo():
# Define some constants
resolution_width = 1280 # pixels
resolution_height = 720 # pixels
frame_rate = 15 # fps
dispose_frames_for_stablisation = 30 # frames
chessboard_width = 6 # squares
chessboard_height = 9 # squares
square_size = 0.0253 # meters
try:
# Enable the streams from all the intel realsense devices
rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate)
rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)
# Use the device manager class to enable the devices and get the frames
device_manager = DeviceManager(rs.context(), rs_config)
device_manager.enable_all_devices()
# Allow some frames for the auto-exposure controller to stablise
for frame in range(dispose_frames_for_stablisation):
frames = device_manager.poll_frames()
assert( len(device_manager._available_devices) > 0 )
"""
1: Calibration
Calibrate all the available devices to the world co-ordinates.
For this purpose, a chessboard printout for use with opencv based calibration process is needed.
"""
# Get the intrinsics of the realsense device
intrinsics_devices = device_manager.get_device_intrinsics(frames)
# Set the chessboard parameters for calibration
chessboard_params = [chessboard_height, chessboard_width, square_size]
# Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
calibrated_device_count = 0
while calibrated_device_count < len(device_manager._available_devices):
frames = device_manager.poll_frames()
pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params)
transformation_result_kabsch = pose_estimator.perform_pose_estimation()
object_point = pose_estimator.get_chessboard_corners_in3d()
calibrated_device_count = 0
for device in device_manager._available_devices:
if not transformation_result_kabsch[device][0]:
print("Place the chessboard on the plane where the object needs to be detected..")
else:
calibrated_device_count += 1
# Save the transformation object for all devices in an array to use for measurements
transformation_devices={}
chessboard_points_cumulative_3d = np.array([-1,-1,-1]).transpose()
for device in device_manager._available_devices:
transformation_devices[device] = transformation_result_kabsch[device][1].inverse()
points3D = object_point[device][2][:,object_point[device][3]]
points3D = transformation_devices[device].apply_transformation(points3D)
chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) )
# Extract the bounds between which the object's dimensions are needed
# It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1)
roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)
print("Calibration completed... \nPlace the box in the field of view of the devices...")
"""
2: Measurement and display
Measure the dimension of the object using depth maps from multiple RealSense devices
The information from Phase 1 will be used here
"""
# Enable the emitter of the devices
device_manager.enable_emitter(True)
# Load the JSON settings file in order to enable High Accuracy preset for the realsense
device_manager.load_settings_json("./HighResHighAccuracyPreset.json")
# Get the extrinsics of the device to be used later
extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames)
# Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
calibration_info_devices = defaultdict(list)
for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices):
for key, value in calibration_info.items():
calibration_info_devices[key].append(value)
# Continue acquisition until terminated with Ctrl+C by the user
while 1:
# Get the frames from all the devices
frames_devices = device_manager.poll_frames()
# Calculate the pointcloud using the depth frames from all the devices
point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D)
# Get the bounding box for the pointcloud in image coordinates of the color imager
bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices )
# Draw the bounding box points on the color image and visualise the results
visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height)
except KeyboardInterrupt:
print("The program was interupted by the user. Closing the program...")
finally:
device_manager.disable_streams()
cv2.destroyAllWindows()
def __init__(self):
camera_id = str(rospy.get_param('~camera_id'))
color_width = rospy.get_param('~color_width')
color_high = rospy.get_param('~color_high')
charuco_row = rospy.get_param('~charuco_row')
charuco_col = rospy.get_param('~charuco_col')
square_length = rospy.get_param('~square_length')
marker_length = rospy.get_param('~marker_length')
self.cnd = 0
self.frameId = 0
self.pipeline = rs.pipeline()
rs_config = rs.config()
rs_config.enable_stream(rs.stream.color, color_width, color_high,
rs.format.bgr8, 30)
self.pipeline.start(rs_config)
# Constant parameters used in Aruco methods
self.aruco_param = aruco.DetectorParameters_create()
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
# Create grid board object we're using in our stream
self.charuco_board = aruco.CharucoBoard_create(
squaresX=charuco_col,
squaresY=charuco_row,
squareLength=square_length,
markerLength=marker_length,
dictionary=self.aruco_dict)
# Check for camera calibration data
config = ConfigParser.ConfigParser()
config.optionxform = str
rospack = rospkg.RosPack()
self.curr_path = rospack.get_path('hand_eye')
config.read(self.curr_path + '/config/camera_' + str(camera_id) +
'_internal.ini')
internal_name = 'Internal_' + str(color_width) + '_' + str(color_high)
b00 = float(config.get(internal_name, "Key_1_1"))
b01 = float(config.get(internal_name, "Key_1_2"))
b02 = float(config.get(internal_name, "Key_1_3"))
b10 = float(config.get(internal_name, "Key_2_1"))
b11 = float(config.get(internal_name, "Key_2_2"))
b12 = float(config.get(internal_name, "Key_2_3"))
b20 = float(config.get(internal_name, "Key_3_1"))
b21 = float(config.get(internal_name, "Key_3_2"))
b22 = float(config.get(internal_name, "Key_3_3"))
self.cameraMatrix = np.mat([[b00, b01, b02], [b10, b11, b12],
[b20, b21, b22]])
# c_x = 643.47548083
# c_y = 363.67742746
# f_x = 906.60886808
# f_y = 909.34831447
# k_1 = 0.16962942
# k_2 = -0.5560001
# p_1 = 0.00116353
# p_2 = -0.00122694
# k_3 = 0.52491878
# c_x = 649.007507324219
# c_y = 356.122222900391
# f_x = 922.76806640625
# f_y = 923.262023925781
# self.cameraMatrix = np.array([[f_x, 0, c_x],
# [0, f_y, c_y],
# [0, 0, 1]])
# self.distCoeffs = np.array([k_1, k_2, p_1, p_2, k_3])
self.distCoeffs = np.array([0.0, 0, 0, 0, 0])
# Create vectors we'll be using for rotations and translations for postures
self.rvecs = None
self.tvecs = None
self.rvecs_arr = np.zeros((3, NUMBER))
self.tvecs_arr = np.zeros((3, NUMBER))
self.cam = None
self.QueryImg = None
self.init_server()
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
self.QueryImg = np.asanyarray(color_frame.get_data())
def create_pipeline(config):
"""Sets up the pipeline to extract depth and rgb frames
Arguments:
config {dict} -- A dictionary mapping for configuration. see default.yaml
Returns:
tuple -- pipeline, pointcloud, decimate, filters(list), colorizer (not used)
"""
# Ensure device is connected
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:
logging.error("No connected Intel Realsense Device!")
sys.exit(1)
if config['advanced']:
logging.info(
"Attempting to enter advanced mode and upload JSON settings file")
load_setting_file(ctx, devices, config['advanced'])
# Configure streams
pipeline = rs.pipeline()
rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, config['depth']['width'],
config['depth']['height'], rs.format.z16,
config['depth']['framerate'])
# other_stream, other_format = rs.stream.infrared, rs.format.y8
rs_config.enable_stream(rs.stream.color, config['color']['width'],
config['color']['height'], rs.format.rgb8,
config['color']['framerate'])
# Start streaming
pipeline.start(rs_config)
profile = pipeline.get_active_profile()
# Processing blocks
filters = []
decimate = None
align = rs.align(rs.stream.color)
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
# Decimation
if config.get("filters").get("decimation"):
filt = config.get("filters").get("decimation")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
decimate = rs.decimation_filter(**filt)
# Spatial
if config.get("filters").get("spatial"):
filt = config.get("filters").get("spatial")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.spatial_filter(**filt)
filters.append(my_filter)
# Temporal
if config.get("filters").get("temporal"):
filt = config.get("filters").get("temporal")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.temporal_filter(**filt)
filters.append(my_filter)
process_modules = (align, depth_to_disparity, disparity_to_depth, decimate)
# Create colorizer and point cloud
# colorizer = rs.colorizer(2)
pc = rs.pointcloud()
intrinsics = get_intrinsics(pipeline, rs.stream.color)
proj_mat = create_projection_matrix(intrinsics)
return pipeline, pc, process_modules, filters, proj_mat
def realsense_behaviour():
# set show_depth to True to show the rgb and depth frames together
show_depth = True
# realsense min and max distance
minDist = 0.3
maxDist = 4.5
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
try:
while True:
# skip 5 first frames to give Auto-Exposure time to adjust
for i in range(5):
pipeline.wait_for_frames()
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
# Access color component of the frame
color = np.asanyarray(color_frame.get_data())
# Access depth component
colorizer = rs.colorizer()
colorizer_depth = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
# Depth and rgb image alignment
align = rs.align(rs.stream.color)
frames = align.process(frames)
aligned_depth_frame = frames.get_depth_frame()
colorized_depth = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
valX = np.array([30, 319, 609])
valY = np.array([30, 239, 449])
dist = np.zeros((3, 3))
valid_matrix = np.zeros((3, 3))
for xCount in range(0, 3):
for yCount in range(0, 3):
distance = aligned_depth_frame.get_distance(
valX[xCount], valY[yCount])
if (distance < 0.3):
distance = np.inf
valid_matrix[yCount, xCount] = 0
else:
valid_matrix[yCount, xCount] = 1
dist[yCount, xCount] = distance
cv2.circle(color, (valX[xCount], valY[yCount]), 5,
(0, 0, 255), 2)
cv2.circle(colorized_depth, (valX[xCount], valY[yCount]),
5, (0, 255, 0), 2)
# print("\033c")
# print(dist)
# Find index of valid values
validIdxL = np.nonzero(valid_matrix[0:3, 0])
validIdxR = np.nonzero(valid_matrix[0:3, 2])
if (((len(validIdxL[0])) > 1 and len(validIdxR[0])) > 1):
angle = (np.mean(dist[validIdxL, 0]) -
np.mean(dist[validIdxR, 2])) * 180 / np.pi
value = 1 / np.amin(dist)
# saturation
if value > 1 / minDist: # saturation to max speed
value = 3.
elif value < 1 / maxDist: # saturation to min speed
value = 0
else:
angle = 0
value = 0
# print(angle, value)
vect = [angle, -value]
# Fill the rs_queue
global_vars.queue_rs.put(vect)
# Show the two frames together:
if show_depth:
images = np.hstack((color, colorized_depth))
else:
images = color
cv2.imshow('RealSense', images)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
break
# Create a vector based on depth data points
finally:
# Stop streaming
pipeline.stop()
def disable_streams(self):
self._config.disable_all_streams()
"""
_____ _ _
|_ _|___ ___ | |_ (_) _ __ __ _
| | / _ \/ __|| __|| || '_ \ / _` |
| || __/\__ \| |_ | || | | || (_| |
|_| \___||___/ \__||_||_| |_| \__, |
|___/
"""
if __name__ == "__main__":
try:
c = rs.config()
c.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
c.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
c.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)
c.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
device_manager = DeviceManager(rs.context(), c)
device_manager.enable_all_devices()
for k in range(150):
frames = device_manager.poll_frames()
device_manager.enable_emitter(True)
device_extrinsics = device_manager.get_depth_to_color_extrinsics(
frames)
finally:
device_manager.disable_streams()
return file_paths
# import pcl
# from pcl import pcl_visualization
CV2_LBUTTON_FLAG = False
# Configure depth and color streams...
# ...from Camera 1
pipelines = []
configs = []
profiles = []
pipeline_1 = rs.pipeline()
config_1 = rs.config()
config_1.enable_device('816612061111')
config_1.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config_1.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# ...from Camera 2
pipeline_2 = rs.pipeline()
config_2 = rs.config()
config_2.enable_device('816612061344')
config_2.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config_2.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# Start streaming from both cameras
pipeline_1.start(config_1)
pipeline_2.start(config_2)
def main():
ctx = rs.context()
devices = ctx.query_devices()
device = devices[0]
# print device info
#print('advanced_mode: %s' % device.get_info(rs.camera_info.advanced_mode));
print('asic_serial_number: %s' %
device.get_info(rs.camera_info.asic_serial_number))
print('camera_locked: %s' % device.get_info(rs.camera_info.camera_locked))
print('debug_op_code: %s' % device.get_info(rs.camera_info.debug_op_code))
print('firmware_update_id: %s' %
device.get_info(rs.camera_info.firmware_update_id))
print('firmware_version: %s' %
device.get_info(rs.camera_info.firmware_version))
print('name: %s' % device.get_info(rs.camera_info.name))
print('physical_port: %s' % device.get_info(rs.camera_info.physical_port))
print('product_id: %s' % device.get_info(rs.camera_info.product_id))
print('product_line: %s' % device.get_info(rs.camera_info.product_line))
#print('recommended_firmware_version: %s' % device.get_info(rs.camera_info.recommended_firmware_version));
print('serial_number: %s' % device.get_info(rs.camera_info.serial_number))
#print('usb_type_descriptor: %s' % device.get_info(rs.camera_info.usb_type_descriptor));
filters = dict()
filters['align'] = rs.align(rs.stream.color)
filters['spatial'] = rs.spatial_filter()
filters['spatial'].set_option(rs.option.filter_magnitude, 5)
filters['spatial'].set_option(rs.option.filter_smooth_alpha, 1)
filters['spatial'].set_option(rs.option.filter_smooth_delta, 50)
filters['spatial'].set_option(rs.option.holes_fill, 3)
filters['temporal'] = rs.temporal_filter()
filters['hole'] = rs.hole_filling_filter()
filters['disparity'] = rs.disparity_transform(True)
filters['depth'] = rs.disparity_transform(False)
config = rs.config()
config.enable_device(device.get_info(rs.camera_info.serial_number))
config.enable_stream(rs.stream.depth, IMG_WIDTH, IMG_HEIGHT, rs.format.z16,
30)
config.enable_stream(rs.stream.color, IMG_WIDTH, IMG_HEIGHT,
rs.format.bgr8, 30)
pipeline = rs.pipeline()
if exists('captured'): rmtree('captured')
if exists('images'): rmtree('images')
mkdir('captured')
mkdir('images')
sequence = 0
while True:
ch = input('press to capture')
pipeline.start(config)
for i in range(5):
pipeline.wait_for_frames()
frames = pipeline.wait_for_frames()
alignment = filters['align'].process(frames)
depth_frame = alignment.get_depth_frame()
#depth_frame = filters['disparity'].process(depth_frame);
#depth_frame = filters['spatial'].process(depth_frame);
#depth_frame = filters['temporal'].process(depth_frame);
#depth_frame = filters['depth'].process(depth_frame);
depth_frame = filters['hole'].process(depth_frame)
color_frame = alignment.get_color_frame()
if not depth_frame or not color_frame:
print("failed to capture!")
continue
depth_image = np.asanyarray(depth_frame.get_data())
print(np.min(depth_image), np.max(depth_image))
mask = masked(depth_image.copy())
color_image = np.asanyarray(color_frame.get_data())
cv2.imwrite(
join('captured',
str(sequence).zfill(3) + '_mask.png'),
cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03),
cv2.COLORMAP_JET))
cv2.imwrite(join('captured',
str(sequence).zfill(3) + '.png'), color_image)
masked_color = color_image.copy()
masked_color[mask == CLIPPED_HIGH] = np.zeros((color_image.shape[-1]))
cv2.imwrite(join('images',
str(sequence).zfill(3) + '.png'), masked_color)
pipeline.stop()
sequence += 1
model.load_weights(weightsfile)
model.net_info["height"] = 160
inp_dim = int(model.net_info["height"])
assert inp_dim % 32 == 0
assert inp_dim > 32
if CUDA:
model.cuda()
model.eval()
# Setup Realsense pipeline
pipe = rs.pipeline()
configure = rs.config()
width = 640; height = 480;
configure.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30)
configure.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30)
dec_filter = rs.decimation_filter () # Decimation - reduces depth frame density
spat_filter = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing
temp_filter = rs.temporal_filter() # Temporal - reduces temporal noise
pipe.start(configure)
align_to = rs.stream.color
align = rs.align(align_to)
while(1):
# temp = pipe.wait_for_frames()
# aligned_frames = align.process(temp)
# depth_frame = aligned_frames.get_depth_frame()
def manager():
options = {
'model': 'cfg/yolo.cfg',
'load': 'bin/yolo.weights',
'threshold': 0.5,
'gpu': 0.5
}
tfnet = TFNet(options)
pipe_mngr = rs.pipeline()
config = rs.config()
profile = pipe_mngr.start(config)
frames_mngr = pipe_mngr.wait_for_frames()
color_mngr = frames_mngr.get_color_frame()
depth_mngr = frames_mngr.get_depth_frame()
capture_mngr = cv2.VideoCapture(0)
capture_mngr.set(cv2.CAP_PROP_FRAME_WIDTH, 480)
capture_mngr.set(cv2.CAP_PROP_FRAME_HEIGHT, 640)
for p in range(0, 5):
time.sleep(1)
print(p + 1)
i = 0
iteration = 0
align_to = rs.stream.color
align = rs.align(align_to)
while True:
print("\nMODE IS: ", i)
iteration += 1
if i == 1:
#time.sleep(0.25)
if not depth_mngr: continue
frames_mngr = pipe_mngr.wait_for_frames()
depth_mngr = frames_mngr.get_depth_frame()
align_to = rs.stream.color
align = rs.align(align_to)
color_f = frames_mngr.get_color_frame()
colorr = np.asanyarray(color_f.get_data())
i = realsense(depth_mngr, profile, frames_mngr, align)
print("Iterations ", iteration)
elif i == 0:
#refresh depth output
########
color_image = np.asanyarray(color_mngr.get_data())
depth_image = np.asanyarray(depth_mngr.get_data())
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.05), cv2.COLORMAP_JET)
images = depth_colormap
cv2.namedWindow('Manager', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Depth_RealSense', images)
cv2.waitKey(1)
########
i = yolo(capture_mngr, tfnet)
print("Iterations ", iteration)
elif i == 2:
print("PRESSED 'Q'")
print("Iterations ", iteration)
break
def main():
global handin
global currentState
# img_rows, img_cols, maxFrames = 32, 32, 100
img_rows, img_cols, maxFrames = 50, 50, 55
depthFrame = 0
cameraHeightR = 480
cameraWidthR = 848
# cameraWidthR = 640
frameRateR = 60
# frameRateR = 30
# crop parameter
xupam = 350
yupam = 200
xdpam = 250
ydpam = 300
#1,3,4,5,8,9,12,13,14,15
classGest = ['1', '12', '13', '14', '15', '3', '4', '5', '8', '9']
nameGest = [
'call', 'scroll up', 'scroll down', 'right', 'leftback', "like",
'play/pause', 'close', 'click', 'down back'
]
#classGest = ['11', '12', '13','4','8']
#nameGest = ['back', 'scroll up', 'scroll down', 'close 4', 'click 8']
delayGest = 5
delayBol = False
backgroundRemove = True
boxCounter = True
# load the model and weight
json_file = open('3dcnnresult/33/3dcnnmodel.json', 'r')
loaded_model_json = json_file.read()
json_file.close()
loaded_model = model_from_json(loaded_model_json)
# load weights into new model
loaded_model.load_weights("3dcnnresult/33/3dcnnmodel.hd5")
print("Loaded model from disk")
loaded_model.compile(loss=categorical_crossentropy,
optimizer='rmsprop',
metrics=['accuracy'])
conf = loaded_model.model.get_config()
shapeInput, ap = loaded_model.model.get_layer(name="dense_2").input_shape
shapeOutput, sp = loaded_model.model.get_layer(name="dense_2").output_shape
weightDense2 = loaded_model.model.get_layer(
name="dense_2").get_weights()[0]
weightDense22I = loaded_model.model.get_layer(
name="dense_2").get_weights()[1]
weightDense22A = loaded_model.model.get_layer(
name="dense_2").get_weights()[1]
# load the FSM
ytfsmfile = "youtube_fsm.txt"
ytFsmTable = loadFSM(ytfsmfile)
updatedWeight = False
updatedWeight2 = False
''''
print("========================New weight I ==================================")
print(weightDense22I)
print("========================New weight A ==================================")
print(weightDense22A)
'''
#print(newWeight[0])
#NweightDense2 = loaded_model.model.get_layer(name="dense_2").get_weights()[0]
#modelConfig = loaded_model.get_config()
#print(modelConfig)
# setup cv face detection
face_cascade = cv2.CascadeClassifier(
'haarcascades/haarcascade_frontalface_alt2.xml')
# setup Realsense
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR,
rs.format.z16, frameRateR)
config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR,
rs.format.bgr8, frameRateR)
# if using background removal
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# print "Depth Scale is: " , depth_scale
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 # 1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# for text purpose
imgTxt = np.zeros((480, 400, 3), np.uint8)
txt = "OpenCV"
txtLoad = "["
txtDelay = "["
txtRecord = "Capture"
txtDel = "Delay"
txtProbability = "0%"
font = cv2.FONT_HERSHEY_SIMPLEX
framearray = []
ctrDelay = 0
channel = 1
gestCatch = False
gestStart = False
startTime = 0
endTime = 0
x, y, w, h = 0, 0, 0, 0
align_to = rs.stream.color
align = rs.align(align_to)
num_frames = 0
im_width, im_height = (848, 480)
# max number of hands we want to detect/track
num_hands_detect = 1
score_thresh = 0.37
try:
while True:
dataImg = []
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
#print(scores)
num_frames += 1
image_np = color_image
try:
image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
except:
print("Error converting to RGB")
boxes, scores = detector_utils.detect_objects(
image_np, detection_graph, sess)
for i in range(num_hands_detect):
if (scores[i] > score_thresh):
handin = True
#print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
break
if handin == True:
gestStart = True
else:
gestStart = False
if (backgroundRemove == True):
# Remove background - Set pixels further than clipping_distance to grey
# grey_color = 153
grey_color = 0
depth_image_3d = np.dstack(
(depth_image, depth_image, depth_image))
# depth image is 1 channel, color is 3 channels
bg_removed = np.where((depth_image_3d > clipping_distance) |
(depth_image_3d <= 0), grey_color,
color_image)
color_image = bg_removed
draw_image = color_image
else:
draw_image = color_image
# face detection here
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
if gestCatch == False:
faces = face_cascade.detectMultiScale(gray, 1.1, 2)
#print("face: ",len(faces))
ctr = 0
idxFace = -1
minDist = 9999
if len(faces) > 0:
for f in faces:
xh, yh, wh, hh = f
farea = wh * hh
midxf = int(xh + (wh * 0.5))
midyf = int(yh + (hh * 0.5))
depth_imageS = depth_image * depth_scale
deptRef = depth_imageS.item(midyf, midxf)
if deptRef <= minDist:
idxFace = ctr
minDist = deptRef
ctr = ctr + 1
#print("id face", idxFace)
if idxFace >= 0:
x, y, w, h = faces[idxFace]
cv2.rectangle(draw_image, (x, y), (x + w, y + h),
(255, 0, 0), 2)
# crop the face then pass to resize
midx = int(x + (w * 0.5))
midy = int(y + (h * 0.5))
xUp = (x - (w * 3))
yUp = (y - (h * 1.5))
xDn = ((x + w) + (w * 1))
yDn = ((y + h) + (h * 2))
if xUp < 1: xUp = 0
if xDn >= cameraWidthR: xDn = cameraWidthR
if yUp < 1: yUp = 0
if yDn >= cameraHeightR: yDn = cameraHeightR
if handin == False:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
(xDn.__int__(), yDn.__int__()), (0, 0, 255), 2)
else:
cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
(xDn.__int__(), yDn.__int__()), (0, 255, 0), 2)
cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10,
(255, 0, 0))
# region of interest
roi_gray = gray[yUp.__int__():yDn.__int__(),
xUp.__int__():xDn.__int__()]
#print(cv2.useOptimized())
'''''
stateText = "State " + str(currentState)
cv2.putText(imgTxt, stateText, (10, 200), font, 1, (255, 255, 255), 2,
cv2.LINE_AA)
avtext = "Available Gesture"
cv2.putText(imgTxt, avtext, (10, 230), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
for i in range(0, len(availableGest)):
availGestText = "G" + availableGest[i] + ", "
cv2.putText(imgTxt, availGestText, (10 + (i * 80), 260), font, 1, (255, 255, 255), 2,
cv2.LINE_AA)
'''
# if handin == True and depthFrame==10:
# depth_imageS = depth_image * depth_scale
# deptRef = depth_imageS.item(midy, midx)
# boxColor = color_image.copy()
# checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image,handin)
# find the depth of middle point of face
if backgroundRemove == True and gestCatch == False:
#if backgroundRemove == True:
e1 = cv2.getTickCount()
depth_imageS = depth_image * depth_scale
deptRef = depth_imageS.item(midy, midx)
# print(clipping_distance)
clipping_distance = (deptRef + 0.2) / depth_scale
boxColor = color_image.copy()
#handin = checkhandIn(boxCounter, deptRef, xUp, yUp, xDn, yDn, depth_imageS, draw_image)
##handin = checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image)
#print(handin)
e2 = cv2.getTickCount()
times = (e2 - e1) / cv2.getTickFrequency()
#print(times)
#handin = False
##if handin == True:
## gestStart = True
##else:
## gestStart = False
# print("gest start = ", gestStart)
if delayBol == False and gestStart == True:
# your code execution
if depthFrame < maxFrames:
frame = cv2.resize(roi_gray, (img_rows, img_cols))
framearray.append(frame)
depthFrame = depthFrame + 1
txtLoad = txtLoad + "["
gestCatch = True
# print(depthFrame)
if depthFrame == maxFrames:
dataImg.append(framearray)
xx = np.array(dataImg).transpose((0, 2, 3, 1))
X = xx.reshape(
(xx.shape[0], img_rows, img_cols, maxFrames, channel))
X = X.astype('float32')
print('X_shape:{}'.format(X.shape))
#==================== Update the Weight =======================================
newWeightI = []
newWeightA = []
# find the available gesture in the current state
availableGest = getGesture(currentState, ytFsmTable)
print(availableGest)
availG, ignoreG = translateAvailableGest(
availableGest, classGest)
print(availG)
print(ignoreG)
if updatedWeight:
weightI = manipWeight(weightDense22I, ignoreG, 1000)
newWeightI.append(weightDense2)
newWeightI.append(weightI)
if updatedWeight2:
maxClass = 10
weightA = manipWeight(weightDense22A, availG, 1000)
newWeightA.append(weightDense2)
newWeightA.append(weightA)
#=================================================================================
if updatedWeight == False and updatedWeight2 == False:
newWeightI.append(weightDense2)
newWeightI.append(weightDense22A)
loaded_model.model.get_layer(
name="dense_2").set_weights(newWeightI)
if handin == True:
# do prediction
resc = loaded_model.predict_classes(X)[0]
res = loaded_model.predict_proba(X)[0]
# find the result of prediction gesture
resultC = classGest[resc]
nameResultG = nameGest[resc]
for a in range(0, len(res)):
print("Gesture {}: {} ".format(
str(nameGest[a]), str(res[a] * 100)))
else:
resultC = 0
nameResultG = "not enough frame recorded"
print(
"==============================================================="
)
if updatedWeight2:
loaded_model.model.get_layer(
name="dense_2").set_weights(newWeightA)
# do prediction
resc2 = loaded_model.predict_classes(X)[0]
res2 = loaded_model.predict_proba(X)[0]
# find the result of prediction gesture
resultC2 = classGest[resc2]
nameResultG2 = nameGest[resc2]
for a in range(0, len(res2)):
print("Gesture {}: {} ".format(
str(nameGest[a]), str(res2[a] * 100)))
# show text of gesture result
imgTxt = np.zeros((480, 400, 3), np.uint8)
#txt = "Gesture-" + str(resultFsm)
if updatedWeight2:
if res2[resc2] > res[resc]:
txt = "ignored gesture"
act = -1
else:
txt = nameResultG
act = resultC
else:
txt = nameResultG
act = resultC
print(act)
# check with FSM for finding the next state
currentState = getNextState(currentState, act, ytFsmTable)
print(currentState)
framearray = []
#dataImg = []
txtLoad = ""
depthFrame = 0
gestCatch = False
handin = False
delayBol = True
cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txtRecord, (10, 50), font, 1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txt, (10, 160), font, 2, (255, 255, 255),
2, cv2.LINE_AA)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
# depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# print(delayBol)
if delayBol == True:
ctrDelay = ctrDelay + 1
txtDelay = txtDelay + "["
txtDel = "Delay"
cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1,
(255, 255, 255), 2, cv2.LINE_AA)
cv2.putText(imgTxt, txtDel, (10, 100), font, 1,
(255, 255, 255), 2, cv2.LINE_AA)
if ctrDelay == delayGest:
ctrDelay = 0
txtDelay = ""
delayBol = False
gestCatch = False
handin = False
gestStart = False
draw_image = cv2.flip(draw_image, 1)
# Stack both images horizontally
images = np.hstack((draw_image, imgTxt))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
finally:
# Stop streaming
pipeline.stop()
def __init__(self, width, height, fps, flip, display, record, lab_monitor, specified_devices,
snapshot_timer, num_snapshots, snapshot_interval, snapshot_mode, save_params):
"""
Manage the RealSense output streams
"""
self.width = width # Resolution
self.height = height # Resolution
self.fps = fps # Frame rate
self.flip = flip # True to flip cameras vertically when in the living lab
self.display = display # True to enable display windows
self.record = record # True to enable video writers
self.lab_monitor = lab_monitor # Adjust display window sizes for control room or lab monitors (i.e. high / low resolution monitors)
self.specified_devices = specified_devices # A list of specified camera serial numbers to be run
self.snapshot_mode = snapshot_mode # True to take pictures instead of videos
self.snapshot_timer = snapshot_timer # Countdown in seconds till picture taken
self.num_snapshots = num_snapshots # How many pictures to take
self.snapshot_interval = snapshot_interval # Time in seconds between pictures
self.save_params = save_params
assert width > 0, "Invalid width resolution"
assert height > 0, "Invalid height resolution"
assert fps > 0, "Invalid frames per second (fps)"
if self.record:
assert not self.snapshot_mode, "Should only use record when in video mode (not snapshot mode)"
if self.snapshot_mode:
assert not self.record, "Should only use record when in video mode (not snapshot mode)"
assert self.num_snapshots > 0
assert self.snapshot_interval >= 0
assert self.snapshot_timer >= 0
# Prepare specified configuration (depth & infrared not implemented)
self.rs_config = rs.config() # For initialising RealSense
self.rs_config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps)
# Load the RealSense devices
self.device_manager = DeviceManager(rs.context(), self.rs_config)
if len(self.specified_devices) > 0:
self.device_manager.enable_specified_devices(self.specified_devices)
else:
self.device_manager.enable_all_devices()
assert (len(self.device_manager._available_devices) > 0), \
"Camera initialisation failed; there are no available devices"
print(f"Specified devices {self.specified_devices}")
self.enabled_devices = []
for (serial, device) in self.device_manager._enabled_devices.items():
self.enabled_devices.append(serial)
for device in self.specified_devices:
assert device in self.enabled_devices, "Specified device is not connected / failed to connect"
# Set window positions for known cameras based on integer tiling in a 3x3 grid, roughly reflecting their
# positions in the lab:
if self.lab_monitor:
self.default_win_size = (int(1920/3), int(1080/3)) # Good for 1920x1080 display, e.g. monitor in control room
else:
self.default_win_size = (int(1920/5), int(1080/5)) # Good for Living Lab TV when using low resolution.
self.default_win_pos = {
'2': (2, 2), # Right, bottom
'3': (2, 1), # Middle, bottom
'4': (2, 0), # Right, top
'5': (1, 2), # Middle, bottom
'6': (1, 1), # Middle, middle
'7': (0, 2), # Left, bottom
'8': (0, 0), # Left, top
}
# Convert window positions to pixels
for key in self.default_win_pos:
self.default_win_pos[key] = tuple([x * self.default_win_size[i] for i, x in enumerate(self.default_win_pos[key])])
# This allows simplified window naming and consistent window positioning between runs.
# If cameras are moved around in the lab, these should be changed.
self.cam_names = {
"830112071467": '5',
"830112071329": '4',
"831612070394": '2',
"831612071422": '3',
"831612071440": '8',
"831612071526": 'X'
}
self.display_windows = {}
self.video_writers = {}
# Configure the output directory for recordings
now = datetime.datetime.now()
if not os.path.exists(os.path.join(os.getcwd(), "recordings")):
os.mkdir(os.path.join(os.getcwd(), "recordings"))
if self.record or self.save_params or self.snapshot_mode:
self.output_directory = os.path.join(os.getcwd(), "recordings", now.strftime('20%y-%m-%d-%H-%M'))
if os.path.exists(self.output_directory):
while os.path.exists(self.output_directory):
self.output_directory += "a"
os.mkdir(self.output_directory)
assert os.path.exists(self.output_directory)
# Wait for exposure to balance
print("Warming up")
self.warm_up()
# Write each cameras parameters to json file
if self.save_params:
self.save_intrinsics(self.output_directory, self.device_manager.poll_frames())
print("Loaded lab manager " + str(self.width) + " " + str(self.height) + " " + str(self.fps))
# Start streaming
if self.snapshot_mode:
self.snapshot()
elif self.display or self.record:
self.stream()
else:
return None
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--save", default=True, action='store_true', help="if save the pos")
parser.add_argument("--out_dir", default="data/t265_pos", help="dir to save the data")
opt = parser.parse_args()
time_str = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(time.time()))
if not os.path.exists(opt.out_dir) and opt.save:
os.makedirs(opt.out_dir)
if opt.save:
fout = open(os.path.join(opt.out_dir, time_str + ".txt"), "w")
# init the pipeline
pipeline = rs.pipeline()
conf = rs.config()
conf.enable_stream(rs.stream.pose)
profile = pipeline.start(conf)
try:
while True:
frames = pipeline.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
if opt.save:
save_time = time.time()
pos = data.translation
fout.write("{:.4f}\tx\t{:.4f}\ty\t{:.4f}\tz\t{:.4f}\n".format(save_time, pos.x, pos.y, pos.z))
if pose.frame_number % 100 == 0:
def main(argv):
size, file_cnt = read_parameters(argv)
pipeline = rs.pipeline()
config = rs.config()
# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
auto_calibrated_device = rs.auto_calibrated_device(device)
if not auto_calibrated_device:
print("The connected device does not support auto calibration")
return
# config.enable_stream()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
conf = pipeline.start(config)
calib_dev = rs.auto_calibrated_device(conf.get_device())
def on_chip_calib_cb(progress):
print(". ")
while True:
try:
input = raw_input(
"Please select what the operation you want to do\nc - on chip calibration\nt - tare calibration\ng - get the active calibration\nw - write new calibration\ne - exit\n"
)
if input == 'c':
print("Starting on chip calibration")
new_calib, health = calib_dev.run_on_chip_calibration(
5000, file_cnt, on_chip_calib_cb)
print("Calibration completed")
print("health factor = ", health)
if input == 't':
print("Starting tare calibration")
ground_truth = float(
raw_input("Please enter ground truth in mm\n"))
new_calib, health = calib_dev.run_tare_calibration(
ground_truth, 5000, file_cnt, on_chip_calib_cb)
print("Calibration completed")
print("health factor = ", health)
if input == 'g':
calib = calib_dev.get_calibration_table()
print("Calibration", calib)
if input == 'w':
print("Writing the new calibration")
calib_dev.set_calibration_table(new_calib)
calib_dev.write_calibration()
if input == 'e':
pipeline.stop()
return
print("Done\n")
except Exception as e:
pipeline.stop()
print(e)
except:
pipeline.stop()
print("A different Error")
def main():
# First we set up the cameras to start streaming
# Define some constants
resolution_width = 1280 # pixels
resolution_height = 720 # pixels
frame_rate = 30 # fps
dispose_frames_for_stablisation = 30 # frames
# Enable the streams from all the intel realsense devices
rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, resolution_width,
resolution_height, rs.format.z16, frame_rate)
rs_config.enable_stream(rs.stream.infrared, 1, resolution_width,
resolution_height, rs.format.y8, frame_rate)
rs_config.enable_stream(rs.stream.color, resolution_width,
resolution_height, rs.format.bgr8, frame_rate)
# Use the device manager class to enable the devices and get the frames
device_manager = DeviceManager(rs.context(), rs_config)
device_manager.enable_all_devices()
device_manager._available_devices.sort()
device_list = device_manager._available_devices
# Allow some frames for the auto-exposure controller to stablise
for frame in range(dispose_frames_for_stablisation):
frames = device_manager.poll_frames_keep()
assert (len(device_manager._available_devices) > 0)
#Then we calibrate the images
# Get the intrinsics of the realsense device
intrinsics_devices = device_manager.get_device_intrinsics(frames)
# Set the charuco board parameters for calibration
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
charuco_width = 8
charuco_height = 5
square_length = 0.03425
marker_length = .026
coordinate_dimentions = 3
charuco_board = aruco.CharucoBoard_create(charuco_width, charuco_height,
square_length, marker_length,
aruco_dict)
# Set the charuco board parameters for robot
aruco_dict_r = aruco.Dictionary_get(aruco.DICT_4X4_250)
charuco_width_r = 3
charuco_height_r = 3
square_length_r = 0.0311
marker_length_r = .0247
charuco_board_robot = aruco.CharucoBoard_create(charuco_width_r,
charuco_height_r,
square_length_r,
marker_length_r,
aruco_dict_r)
# Set the charuco board parameters for robot
aruco_dict_ro = aruco.Dictionary_get(aruco.DICT_5X5_100)
charuco_width_ro = 3
charuco_height_ro = 3
square_length_ro = 0.0311
marker_length_ro = .0247
charuco_board_robot_2 = aruco.CharucoBoard_create(charuco_width_ro,
charuco_height_ro,
square_length_ro,
marker_length_ro,
aruco_dict_ro)
# Decide if you want to use previous calibration or make a new one
calibration_decision = input(
'To use previous calibration, press "y". For new calibration press any key \n'
)
if calibration_decision != 'y':
# Choose amount of frames to average
correct_input = False
while not correct_input:
the_input = input(
"Write amount of frames you want to use to calibrate \n")
try:
amount_frames = int(the_input)
if amount_frames > 0:
correct_input = True
else:
print("Frame count has to be positive")
except ValueError:
print('Input has to be int')
# Make a dict to store all images for calibration
frame_dict = {}
transform_dict = {}
rms_dict = {}
for from_device in device_list:
transform_dict[from_device] = {}
rms_dict[from_device] = {}
for to_device in device_list:
transform_dict[from_device][to_device] = {}
rms_dict[from_device][to_device] = np.inf
print("Starting to take images in 5 seconds")
time.sleep(5)
devices_stitched = False
while not devices_stitched:
print("taking new set of images")
for frame_count in range(amount_frames):
print("taking image")
print(amount_frames - frame_count, "images left")
frames = device_manager.poll_frames_keep()
time.sleep(0.5)
frame_dict[frame_count] = {}
for device in device_list:
ir_frame = np.asanyarray(
frames[device][(rs.stream.infrared, 1)].get_data())
depth_frame = np.asanyarray(
post_process_depth_frame(
frames[device][rs.stream.depth]).get_data())
frame_dict[frame_count][device] = {
'ir_frame': ir_frame,
'depth_frame': depth_frame
}
del frames
# Make transfer matrices between all possible cameras
for idx, from_device in enumerate(device_list[:-1]):
for to_device in device_list[idx + 1:]:
if to_device != from_device:
temp_transform, temp_rms = get_transformation_matrix_wout_rsobject(
frame_dict, [from_device, to_device],
intrinsics_devices, charuco_board)
if temp_rms < rms_dict[from_device][to_device]:
rms_dict[from_device][to_device] = temp_rms
rms_dict[to_device][from_device] = temp_rms
transform_dict[from_device][
to_device] = temp_transform
transform_dict[to_device][
from_device] = temp_transform.inverse()
#Use Djikstra's to fin shortest path and check if all cameras are connected
transformation_matrices = least_error_transfroms(
transform_dict, rms_dict)
if transformation_matrices != 0:
devices_stitched = True
# Prints rms error between camera transforms
test = matrix_viewer(rms_dict)
print(test)
# Turns transformation matrices into Transfomation objects
transformation_devices = {}
for matrix in transformation_matrices:
the_matirx = transformation_matrices[matrix]
transformation_devices[matrix] = Transformation(
the_matirx[:3, :3], the_matirx[:3, 3])
# Saves calibration transforms if the user wants to
save_calibration = input(
'Press "y" if you want to save calibration \n')
if save_calibration == "y":
calibration_name = input
saved = False
while not saved:
name = input(
"Type in name of file to save. remember to end name with '.npy' \n"
)
try:
np.save(name, transformation_matrices)
saved = True
except:
print(
"could not save, try another name and remember '.npy' in the end"
)
frame_dict.clear()
else:
correct_filename = False
while not correct_filename:
file_name = input('Type in calibration file name \n')
try:
transfer_matirices_save = np.load(file_name, allow_pickle=True)
transfer_matrices = transfer_matirices_save[()]
correct_filename = True
except:
print('No such file in directory: "', file_name, '"')
transformation_devices = {}
for matrix in transfer_matrices:
the_matrix = transfer_matrices[matrix]
transformation_devices[matrix] = Transformation(
the_matrix[:3, :3], the_matrix[:3, 3])
print("Calibration completed...")
# Enable the emitter of the devices and extract serial numbers to identify cameras
device_manager.enable_emitter(True)
key_list = device_manager.poll_frames().keys()
while True:
visualisation = input(
'Presss "1" for RMS error, "2" for chessboard visualisation and "3" for 3d pointcloud and "4" for robot to camea calibration\n'
)
if visualisation == '1':
calculate_avg_rms_error(device_list, device_manager,
transformation_devices, charuco_board,
intrinsics_devices)
elif visualisation == '2':
visualise_chessboard(device_manager, device_list,
intrinsics_devices, charuco_board,
transformation_devices)
elif visualisation == '3':
visualise_point_cloud(key_list, resolution_height,
resolution_width, device_manager,
coordinate_dimentions,
transformation_devices)
elif visualisation == '4':
robot_calibration = CameraRobotCalibration(transformation_devices,
device_manager,
charuco_board_robot_2,
charuco_board_robot)
input("Set target in position for calibration\n")
test = robot_calibration.test_functions()
print(test)
else:
print("Input not recognised")
return device_extrinsics
def disable_streams(self):
self._config.disable_all_streams()
"""
_____ _ _
|_ _|___ ___ | |_ (_) _ __ __ _
| | / _ \/ __|| __|| || '_ \ / _` |
| || __/\__ \| |_ | || | | || (_| |
|_| \___||___/ \__||_||_| |_| \__, |
|___/
"""
if __name__ == "__main__":
try:
c = rs.config()
c.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
c.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
c.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)
c.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
device_manager = DeviceManager(rs.context(), c)
device_manager.enable_all_devices()
for k in range(150):
frames = device_manager.poll_frames()
device_manager.enable_emitter(True)
device_extrinsics = device_manager.get_depth_to_color_extrinsics(frames)
finally:
device_manager.disable_streams()
#!/usr/bin/env python3
"""! This script is a test of the object segmentation code for open CV.
It is primiarily intended as a demo and should not be used without modification
in the final robot setup.
"""
import cv2 as cv
import sys
from os import listdir
import numpy as np
import time
import pyrealsense2 as rs
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_record_to_file("bagfiles/camera_video")
# Start streaming
pipeline.start(config)
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
# depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not color_frame:
continue
# -*- coding: utf-8 -*-
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#####################################################
## librealsense T265 example ##
#####################################################
# First import the library
import pyrealsense2 as rs
# Declare RealSense pipeline, encapsulating the actual device and sensors
pipe = rs.pipeline()
# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
# Start streaming with requested config
pipe.start(cfg)
try:
for _ in range(50):
# Wait for the next set of frames from the camera
frames = pipe.wait_for_frames()
# Fetch pose frame
pose = frames.get_pose_frame()
if pose:
# Print some of the pose data to the terminal
data = pose.get_pose_data()
def getParameter(self):
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
if self.EnableColor is True:
config.enable_stream(rs.stream.color, int(self.width), int(self.height), rs.format.bgr8, 30)
if self.EnableInfrared is True:
config.enable_stream(rs.stream.infrared, 1, int(self.width), int(self.height), rs.format.y8, 30)
config.enable_stream(rs.stream.infrared, 2, int(self.width), int(self.height), rs.format.y8, 30)
if self.EnableDepth is True:
config.enable_stream(rs.stream.depth, int(self.width), int(self.height), rs.format.z16, 30)
# Start streaming
cfg = pipeline.start(config)
# Get Video Stream Intrinsics
if self.printPara is True:
print("=======================================================")
print("Get Video Stream Intrinsics")
print("=======================================================")
if self.EnableColor is True:
colorStream = cfg.get_stream(rs.stream.color)
colorProfile = colorStream.as_video_stream_profile()
self.colorIntrin = colorProfile.get_intrinsics()
if self.printPara is True:
print("Color Intrinsics:")
print(self.colorIntrin)
print("")
if self.EnableInfrared is True:
infraredStream = cfg.get_stream(rs.stream.infrared, 1)
infraredProfile = infraredStream.as_video_stream_profile()
self.infraredIntrin = infraredProfile.get_intrinsics()
if self.printPara is True:
print("Infrared Intrinsics:")
print(self.infraredIntrin)
print("")
if self.EnableDepth is True:
depthStream = cfg.get_stream(rs.stream.depth)
depthProfile = depthStream.as_video_stream_profile()
self.depthIntrin = depthProfile.get_intrinsics()
if self.printPara is True:
print("Depth Intrinsics:")
print(self.depthIntrin)
print("")
# Get and Apply Depth-to-Stream Extrinsics
print("\n=======================================================")
print("Get and Apply Depth-to-Stream Extrinsics")
print("=======================================================")
print("Color Extrinsics:")
colorExtrin = depthStream.get_extrinsics_to(colorStream)
if self.printPara is True:
print(colorExtrin)
print("")
print("Infrared Extrinsics:")
infraredExtrin = depthStream.get_extrinsics_to(infraredStream)
if self.printPara is True:
print(infraredExtrin)
if self.EnableInfrared is True:
# Get Disparity Baseline
if self.printPara is True:
print("\n=======================================================")
print("Get Disparity Baseline")
print("=======================================================")
infraredStream2 = cfg.get_stream(rs.stream.infrared, 2)
self.baseline = infraredStream2.get_extrinsics_to(infraredStream)
if self.printPara is True:
print(self.baseline)
pipeline.stop()
def main():
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server.bind(('127.0.0.1', 8000))
server.listen(5)
print("waiting msg ...")
conn, clint_add = server.accept()
args = parse_args()
torch.backends.cudnn.enabled = True
torch.backends.cudnn.benchmark = True
load_config(cfg, args.config)
logger = Logger(-1, use_tensorboard=False)
predictor = Predictor(cfg, args.model, logger, device='cuda:0')
logger.log('Press "Esc", "q" or "Q" to exit.')
if args.demo == 'image':
if os.path.isdir(args.path):
files = get_image_list(args.path)
else:
files = [args.path]
files.sort()
for image_name in files:
meta, res = predictor.inference(image_name)
predictor.visualize(res, meta, cfg.class_names, 0.35)
ch = cv2.waitKey(0)
if ch == 27 or ch == ord('q') or ch == ord('Q'):
break
elif args.demo == 'video' or args.demo == 'webcam':
pipeline = rs.pipeline()
# 创建 config 对象：
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
#cap = cv2.VideoCapture(args.path if args.demo == 'video' else args.camid)
while True:
next_frames = pipeline.wait_for_frames()
get_next_color_frame = next_frames.get_color_frame()
frame = np.asanyarray(get_next_color_frame.get_data())
meta, res = predictor.inference(frame)
predictor.visualize(res, meta, cfg.class_names, 0.70)
all_box = []
for label in res:
for bbox in res[label]:
score = bbox[-1]
if score > 0.70:
x0, y0, x1, y1 = [int(i) for i in bbox[:4]]
all_box.append([label, x0, y0, x1, y1, score])
all_box.sort(key=lambda v: v[5])
send_data_byte = bytes(0)
time.sleep(0.005)
if len(all_box) == 0:
leftup_rightdown_corner = [-1, 0, 0, 0, 0, time.time(), 'b']
for i in range(len(leftup_rightdown_corner)):
#print(pickup_leftup_rightdown_corner[i])
pickup_senddata = str(leftup_rightdown_corner[i]) + ','
# print(pickup_senddata.encode())
send_data_byte += pickup_senddata.encode()
# print(send_data_byte)
conn.send(send_data_byte)
else:
zzw = all_box[-1]
label, x0, y0, x1, y1, score = zzw
leftup_rightdown_corner = [1, x0, y0, x1, y1, time.time(), 'a']
for i in range(len(leftup_rightdown_corner)):
target_senddata = str(leftup_rightdown_corner[i]) + ','
send_data_byte += target_senddata.encode()
conn.send(send_data_byte)
ch = cv2.waitKey(1)
if ch == 27 or ch == ord('q') or ch == ord('Q'):
break
def main():
#out = cv2.VideoWriter('./data/output.avi', -1, 20.0,(1280, 720))
pipeline = rs.pipeline()
config = rs.config()
vis = o3d.visualization.Visualizer()
vis.create_window('PCD', width=1280, height=720)
pointcloud = o3d.geometry.PointCloud()
vis.add_geometry(pointcloud)
geom_added = False
# note: using 640 x 480 depth resolution produces smooth depth boundaries
# using rs.format.bgr8 for color image format for OpenCV based image visualization
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)
config.enable_device_from_file("./data/realsense.bag",
repeat_playback=True)
# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_scale = depth_sensor.get_depth_scale()
# We will not display the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 3 # 3 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# 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)
# Streaming loop
frame_count = 0
intrinsics = profile.get_stream(
rs.stream.depth).as_video_stream_profile().get_intrinsics()
pcd_old = o3d.geometry.PointCloud()
while True:
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
img_depth = o3d.geometry.Image(depth_image)
img_color = o3d.geometry.Image(color_image)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
img_color,
img_depth,
depth_trunc=clipping_distance_in_meters,
convert_rgb_to_intensity=False)
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy,
intrinsics.ppx, intrinsics.ppy)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd, pinhole_camera_intrinsic)
# o3d.visualization.draw_geometries([pcd])
#print("Update")
start = time.time()
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=3,
num_iterations=60)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
outlier_cloud = pcd.select_by_index(inliers, invert=True)
end = time.time()
print("Plane segmentation took: " + str((end - start) * 1000) + "ms")
# plane_model, inliers = outlier_cloud.segment_plane(distance_threshold=0.01,
# ransac_n=3,
# num_iterations=1000)
# [a, b, c, d] = plane_model
# print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
#
# inlier_cloud = outlier_cloud.select_by_index(inliers)
# inlier_cloud.paint_uniform_color([1.0, 0, 0])
# outlier_cloud = outlier_cloud.select_by_index(inliers, invert=True)
pcd = outlier_cloud.voxel_down_sample(voxel_size=0.007)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
[0, 0, 0, 1]])
print(pcd)
start = time.time()
labels = np.array(
pcd.cluster_dbscan(eps=0.015, min_points=20, print_progress=False))
# with o3d.utility.VerbosityContextManager(
# o3d.utility.VerbosityLevel.Debug) as cm:
# labels = np.array(
# pcd.cluster_dbscan(eps=0.02, min_points=15, print_progress=False))
end = time.time()
print("Clustering took: " + str((end - start) * 1000) + "ms")
max_label = labels.max()
objects = []
#todo paralelize
for i in range(0, max_label + 1):
objects.append(TrackingObject(i))
point_array = np.asarray(pcd.points)
for i, point in enumerate(point_array):
cluster_id = labels[i]
#print(cluster_id)
if cluster_id != -1:
objects[cluster_id].points.append(point)
for obj in objects:
obj.calc_bounding_box(intrinsics)
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels /
(max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
#o3d.visualization.draw_geometries([pcd])
for obj in objects:
cv2.rectangle(color_image, obj.bounding_box[0],
obj.bounding_box[1], (0, 255, 0), 2)
cv2.imshow('bgr', color_image)
key = cv2.waitKey(1)
if key == ord('q'):
break
pointcloud.points = pcd.points
pointcloud.colors = pcd.colors
if geom_added == False:
vis.add_geometry(pointcloud)
geom_added = True
#vis.capture_screen_image("./data/test.png")
#screenshot = np.asanyarray(buf)
#cv.ims
#out.write(screenshot)
vis.update_geometry(pointcloud)
vis.poll_events()
vis.update_renderer()
frame_count += 1
vis.destroy_window()
del vis
#out.release()
pipeline.stop()
def gatherCameraImage(gesture, iterations):
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# Start streaming
profile = pipeline.start(config)
# Store images to process later so capturing is fast
storedImages = {}
print("collecting images")
for iteration in range(iterations):
print("collected image : " + str(iteration))
if (iteration != 0 and iteration % 10 == 0):
pipeline.stop()
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# We will be removing the background of objects more than
# clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 #1 meter
clipping_distance = clipping_distance_in_meters / depth_scale
# 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)
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# frames.get_depth_frame() is a 640x360 depth image
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame(
) # aligned_depth_frame is a 640x480 depth image
# color_frame = aligned_frames.get_color_frame()
depth_image = np.asanyarray(aligned_depth_frame.get_data())
# color_image = np.asanyarray(color_frame.get_data())
#Storing images so we can process later
key = str(len(storedImages))
storedImages[key] = depth_image
# Remove background - Set pixels further than clipping_distance to grey
grey_color = 153
depth_image_3d = np.dstack(
(depth_image, depth_image,
depth_image)) #depth image is 1 channel, color is 3 channels
# Render images
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
images = np.hstack((depth_colormap, depth_colormap))
cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Align Example', images)
key = cv2.waitKey(1)
pipeline.stop()
processedImages = []
print("processing images")
#Processing each stored image
startIdx = startIndex(gesture)
for i in range(0, iterations):
print("processing image : " + str(i))
thread = dcThread(storedImages[str(i)], startIdx + i, gesture)
thread.start()
def main():
# start node
rospy.init_node("kinect_find_xyz")
rospy.loginfo("kinect_find_xyz node initialized")
# create Point32 publisher
pointPub = rospy.Publisher(pointPubTopic, PointStamped, queue_size=1)
# create moving average filters
WINDOW_SIZE = 4
moving_avg_x = MovingWindowAverage(WINDOW_SIZE)
moving_avg_y = MovingWindowAverage(WINDOW_SIZE)
moving_avg_z = MovingWindowAverage(WINDOW_SIZE)
# Create a pipeline
pipeline = rs.pipeline()
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
# enable depth and color streams
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# 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)
while (not rospy.is_shutdown()):
# grab frames from intel RealSense
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame(
) # aligned_depth_frame is a 640x480 depth image
color_frame = aligned_frames.get_color_frame()
# get depth frame intrinsics
depth_intrin = aligned_depth_frame.profile.as_video_stream_profile(
).intrinsics
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
# convert to numpy arrays
numpyRGB = np.asanyarray(color_frame.get_data())
numpyDepth = np.asanyarray(aligned_depth_frame.get_data())
# find min enclosing circle
status, circle = findMinEnclosingCircle(numpyRGB)
if (status):
# draw circles if they are found
(x, y), radius = circle
cv2.circle(numpyRGB, (int(x), int(y)), int(radius), (0, 255, 0), 2)
# find depth of detected circle
reading = circle_to_realworld(aligned_depth_frame, circle,
depth_intrin)
average_x = moving_avg_x.addAndProc(reading[0])
average_y = moving_avg_y.addAndProc(reading[1])
average_z = moving_avg_z.addAndProc(reading[2])
# invert sign of y-point to align with standard cartesian
# views
average_y = -1 * average_y
# create PointStamped message and publish
pointMessage = PointStamped()
pointMessage.header.stamp = rospy.Time.now()
pointMessage.point.x = average_x
pointMessage.point.y = average_y
pointMessage.point.z = average_z
pointPub.publish(pointMessage)
# publish color image
imgMsg = imgBridge.cv2_to_imgmsg(numpyRGB, "bgr8")
imgPub.publish(imgMsg)
# display images
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(numpyDepth, alpha=0.03), cv2.COLORMAP_JET)
cv2.imshow('Depth', depth_colormap)
cv2.imshow('Video', numpyRGB)
if (cv2.waitKey(1) & 0xFF == ord('q')):
break
def run_loop(bag_path, seg_model, seg_opts, save_images=False, output_mode=0):
if save_images:
create_folders()
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
config = rs.config()
# Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
rs.config.enable_device_from_file(config, args.input)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale) # can be commented out
if output_mode == 0 or output_mode == 1:
# Create opencv window to render image in
cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
# pre_seg_mask_sum = None # previous frame path segmentation area - isn't being used right now
# Streaming loop
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get intrinsic in Open3d format for mode 2 and 3 for point cloud output
if output_mode == 1 or output_mode == 2:
intrinsic = o3d.camera.PinholeCameraIntrinsic(
get_intrinsic_matrix(color_frame))
# Get depth frame
depth_frame = frames.get_depth_frame()
# Print intrinsics and extrinsics - not necessary : can be commented out
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
color_image = np.asanyarray(color_frame.get_data())
### Add SEGMENTATION part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track - chosen classes for the model selected (we'd like an oversegmentation of the path)
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
if idx == idx_limit: # 1st frame detection needs to be robust
pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
new_seg_sum = np.sum(seg_mask)
diff = abs(new_seg_sum - pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
pred_color = colorEncode(
pred,
loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=5)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
## removed this part
planes_mask = np.ones_like(depth_array).astype(np.uint8)
planes_mask = np.dstack(
(planes_mask, planes_mask, planes_mask))
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
edges = planes_mask_binary - nd.morphology.binary_dilation(
planes_mask_binary
) # edges calculation - between travesable and non-traversable path
#############################################
if output_mode == 1 or output_mode == 2:
odepth_image = o3d.geometry.Image(depth_array * edges)
ocolor_image = o3d.geometry.Image(color_image * np.dstack(
(edges, edges, edges)))
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
ocolor_image,
odepth_image,
depth_scale=1.0 / depth_scale,
depth_trunc=10, # set to 10 metres
convert_rgb_to_intensity=False)
temp = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, intrinsic)
temp.transform(flip_transform)
# temp = temp.voxel_down_sample(0.03)
# Point cloud output of frame is appended to the list
if output_mode == 1 or output_mode == 2:
output_list.append(temp)
# image format conversion for cv2 visualization/output
if output_mode == 0 or output_mode == 1 or save_images == True:
pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
## for displaying seg_mask
seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
seg_mask = cv2.cvtColor(
seg_mask, cv2.COLOR_GRAY2BGR) # segmentation binary mask
final_output_mask = cv2.cvtColor(
planes_mask_binary,
cv2.COLOR_GRAY2BGR) # final traversable path mask
if output_mode == 0 or output_mode == 1:
# # Blending rgb and depth images for display - can check alignment with this as well
alpha = 0.2
beta = (1.0 - alpha)
dst = cv2.addWeighted(
color_image, alpha, pred_color, beta, 0.0
) # color and segmentation output from ADE20K model blended
dst2 = cv2.addWeighted(depth_color_image, alpha, color_image,
beta,
0.0) # color and depth blended together
##################################
### delete later if needed - color image masked by final traversable path
final_output = color_image * planes_mask_binary_3d
mask = (final_output[:, :, 0] == 0) & (
final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
final_output[:, :, :3][mask] = [255, 255, 255]
######
### Select outputs for visualization - we've chosen some as default
image_set1 = np.vstack((dst, dst2))
# image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
# image_set2 = np.vstack((dst, final_output))
image_set2 = np.vstack((edges, final_output))
### Choose which images you want to display from above
combined_images = np.concatenate((image_set1, image_set2),
axis=1)
if save_images == True:
# Outputs saved - you can modify this
cv2.imwrite("output/visualization/frame%d.png" % idx,
combined_images)
cv2.imwrite("data/color/frame%d.png" % idx,
color_image) # save frame as JPEG file
cv2.imwrite("data/depth/frame%d.png" % idx,
depth_array) # save frame as JPEG file
cv2.imwrite("output/edges/frame%d.png" % idx,
edges) # save frame as JPEG file
cv2.imwrite("output/segmentation_mask/frame%d.png" % idx,
seg_mask) # save frame as JPEG file
cv2.imwrite("output/output_mask/frame%d.png" % idx,
final_output_mask) # save frame as JPEG file
if output_mode == 0 or output_mode == 1:
try:
cv2.imshow('Full Stream', combined_images)
except TypeError as e:
print(idx, e)
key = cv2.waitKey(1)
# if pressed escape exit program
if key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
if output_mode == 0 or output_mode == 1:
cv2.destroyAllWindows()
return
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
###############################################
## Open CV and Numpy integration ##
###############################################
import pyrealsense2 as rs
import numpy as np
import cv2
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
def create_pipeline(config: dict):
"""Sets up the pipeline to extract depth and rgb frames
Arguments:
config {dict} -- A dictionary mapping for configuration. see default.yaml
Returns:
tuple -- pipeline, process modules, filters, t265 device (optional)
"""
# Create pipeline and config for D4XX,L5XX
pipeline = rs.pipeline()
rs_config = rs.config()
# IF t265 is enabled, need to handle seperately
t265_dev = None
t265_sensor = None
t265_pipeline = rs.pipeline()
t265_config = rs.config()
if config['playback']['enabled']:
# Load recorded bag file
rs.config.enable_device_from_file(
rs_config, config['playback']['file'], config['playback'].get('repeat', False))
# This code is only activated if the user points to a T265 Recorded Bag File
if config['tracking']['enabled']:
rs.config.enable_device_from_file(
t265_config, config['tracking']['playback']['file'], config['playback'].get('repeat', False))
t265_config.enable_stream(rs.stream.pose)
t265_pipeline.start(t265_config)
profile_temp = t265_pipeline.get_active_profile()
t265_dev = profile_temp.get_device()
t265_playback = t265_dev.as_playback()
t265_playback.set_real_time(False)
else:
# Ensure device is connected
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:
logging.error("No connected Intel Realsense Device!")
sys.exit(1)
if config['advanced']:
logging.info("Attempting to enter advanced mode and upload JSON settings file")
load_setting_file(ctx, devices, config['advanced'])
if config['tracking']['enabled']:
# Cycle through connected devices and print them
for dev in devices:
dev_name = dev.get_info(rs.camera_info.name)
print("Found {}".format(dev_name))
if "Intel RealSense D4" in dev_name:
pass
elif "Intel RealSense T265" in dev_name:
t265_dev = dev
elif "Intel RealSense L515" in dev_name:
pass
if config['tracking']['enabled']:
if len(devices) != 2:
logging.error("Need 2 connected Intel Realsense Devices!")
sys.exit(1)
if t265_dev is None:
logging.error("Need Intel Realsense T265 Device!")
sys.exit(1)
if t265_dev:
# Unable to open as a pipeline, must use sensors
t265_sensor = t265_dev.query_sensors()[0]
profiles = t265_sensor.get_stream_profiles()
pose_profile = [profile for profile in profiles if profile.stream_name() == 'Pose'][0]
t265_sensor.open(pose_profile)
t265_sensor.start(callback_pose)
logging.info("Started streaming Pose")
rs_config.enable_stream(
rs.stream.depth, config['depth']['width'],
config['depth']['height'],
rs.format.z16, config['depth']['framerate'])
# other_stream, other_format = rs.stream.infrared, rs.format.y8
rs_config.enable_stream(
rs.stream.color, config['color']['width'],
config['color']['height'],
rs.format.rgb8, config['color']['framerate'])
# Start streaming
pipeline.start(rs_config)
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
color_sensor = profile.get_device().first_color_sensor()
depth_scale = depth_sensor.get_depth_scale()
# depth_sensor.set_option(rs.option.global_time_enabled, 1.0)
# color_sensor.set_option(rs.option.global_time_enabled, 1.0)
if config['playback']['enabled']:
dev = profile.get_device()
playback = dev.as_playback()
playback.set_real_time(False)
# Processing blocks
filters = []
decimate = None
align = rs.align(rs.stream.color)
depth_to_disparity = rs.disparity_transform(True)
disparity_to_depth = rs.disparity_transform(False)
# Decimation
if config.get("filters").get("decimation"):
filt = config.get("filters").get("decimation")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
decimate = rs.decimation_filter(**filt)
# Spatial
if config.get("filters").get("spatial"):
filt = config.get("filters").get("spatial")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.spatial_filter(**filt)
filters.append(my_filter)
# Temporal
if config.get("filters").get("temporal"):
filt = config.get("filters").get("temporal")
if filt.get('active', True):
filt.pop('active', None) # Remove active key before passing params
my_filter = rs.temporal_filter(**filt)
filters.append(my_filter)
process_modules = (align, depth_to_disparity, disparity_to_depth, decimate)
intrinsics = get_intrinsics(pipeline, rs.stream.color)
proj_mat = create_projection_matrix(intrinsics)
sensor_meta = dict(depth_scale=depth_scale)
config['sensor_meta'] = sensor_meta
# Note that sensor must be saved so that it is not garbage collected
t265_device = dict(pipeline=t265_pipeline, sensor=t265_sensor)
return pipeline, process_modules, filters, proj_mat, t265_device