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 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 run_demo():
# Define some constants
resolution_width = 640 # pixels
resolution_height = 480 # pixels
frame_rate = 15 # fps
dispose_frames_for_stablisation = 30 # frames
chessboard_width = 6 # squares
chessboard_height = 9 # squares
square_size = 0.0253 # meters
align_to = rs.stream.color
align = rs.align(align_to)
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,maps = device_manager.poll_frames(align)
assert( len(device_manager._available_devices) > 0 )
"""
1. Calibrate cameras and return transformation matrix (rotation matrix + translation vectors)
"""
chessboard_params = [chessboard_height, chessboard_width, square_size]
cameras = calibrateCameras(align,device_manager,frames,chessboard_params)
"""
2. Run OpenPose on each view frame
"""
# 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)
depth_list = {}
color_list = {}
frame_id = 0
points = {}
# Continue acquisition until terminated with Ctrl+C by the user
switch = True
while 1:
# Get the frames from all the devices
if switch:
frames_devices, maps = device_manager.poll_frames(align,500)
# print(frames_devices)
# List collector for display
depth_color= []
color = []
devices = [i for i in maps]
devices.sort()
project_depth = []
for i in devices:
# 1. Get depth map and colorize
temp = maps[i]['depth']
depth_list.setdefault(i,[])
depth_list[i].append(np.array(temp))
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(temp, alpha=0.03), cv2.COLORMAP_JET)
depth_color.append(depth_colormap)
project_depth.append((cameras[i]['matrix'],depth_colormap))
# 2. Run OpenPose detector on image
if FLAGS_USE_BBOX:
box = bbox[i]
else:
box = None
joints,img = predict_keypoints(maps[i]['color'],box)
# 3. Save annotated color image for display
color.append(img)
color_list.setdefault(i,[])
color_list[i].append(img)
# 4. Save keypoints for that camera viewpoint
cameras[i][frame_id] = joints
# 5. Save images to folder
if FLAGS_SAVE_IMGS:
cv2.imwrite('./images/depth_{}_{}.png'.format(i,frame_id),temp)
cv2.imwrite('./images/color_{}_{}.png'.format(i,frame_id),img)
#Triangulate 3d keypoints
points[frame_id] = find3dpoints_rt(cameras,0.2,frame_id)
if points[frame_id] != 'Invalid Frame':
depth_projected = []
for img in project_depth:
points2d = project_2d(img[0],points[frame_id])
img_draw = draw_pose(points2d,'openpose',img[1],True,points[frame_id])
depth_projected.append(img_draw)
# print(img_draw.shape)
depth_color = depth_projected
# proj_img = show_img(cameras,devices[0],frame_id,points)
frame_id += 1
images = np.vstack((np.hstack(color),np.hstack(depth_color)))
# images = proj_img
# Show images for debugging
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
key = cv2.waitKey(1)
if key == 32:
switch = not(switch)
else:
continue
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
break
except KeyboardInterrupt:
print("The program was interupted by the user. Closing the program...")
finally:
device_manager.disable_streams()
cv2.destroyAllWindows()
if FLAGS_SAVE_MATRIX:
cam_pkl = open('cameras.pkl','wb')
pkl.dump(cameras,cam_pkl)
points_pkl = open('3dpoints.pkl','wb')
pkl.dump(points,points_pkl)
class RealSense:
def __init__(self):
self.WIN_NAME = 'RealSense'
self.pitch, self.yaw = math.radians(-10), math.radians(-15)
self.translation = np.array([0, 0, -1], dtype=np.float32)
self.distance = 2
self.paused = False
self.decimate = 1
self.scale = True
self.color = True
# Define some constants
self.resolution_width = 1280 # pixels
self.resolution_height = 720 # pixels
self.frame_rate = 15 # fps
self.dispose_frames_for_stablisation = 25 # frames
self.chessboard_width = 6 # squares
self.chessboard_height = 9 # squares
self.square_size = 0.0253 # meters
# Allow some frames for the auto-exposure controller to stablise
self.intrinsics_devices = None
self.chessboard_params = None
self.rs_config = rs.config()
self.rs_config.enable_stream(rs.stream.depth, self.resolution_width,
self.resolution_height, rs.format.z16,
self.frame_rate)
self.rs_config.enable_stream(rs.stream.infrared, 1,
self.resolution_width,
self.resolution_height, rs.format.y8,
self.frame_rate)
self.rs_config.enable_stream(rs.stream.color, self.resolution_width,
self.resolution_height, rs.format.bgr8,
self.frame_rate)
# Use the device manager class to enable the devices and get the frames
self.device_manager = DeviceManager(rs.context(), self.rs_config)
self.device_manager.enable_all_devices()
print('0')
for self.frame in range(self.dispose_frames_for_stablisation):
self.frames = self.device_manager.poll_frames()
#print('framm = ',self.frame)
# assert( len(self.device_manager._available_devices) > 0 )
print('realsense initialized!!')
def calibaration(self):
self.intrinsics_devices = self.device_manager.get_device_intrinsics(
self.frames)
#print(' Set the chessboard parameters for calibration ')
self.chessboard_params = [
self.chessboard_height, self.chessboard_width, self.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(
self.device_manager._available_devices):
self.frames = self.device_manager.poll_frames()
pose_estimator = PoseEstimation(self.frames,
self.intrinsics_devices,
self.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 self.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
print('calibrated_device_count =', calibrated_device_count)
# Save the transformation object for all devices in an array to use for measurements
self.transformation_devices = {}
chessboard_points_cumulative_3d = np.array([-1, -1, -1]).transpose()
for device in self.device_manager._available_devices:
self.transformation_devices[device] = transformation_result_kabsch[
device][1].inverse()
points3D = object_point[device][2][:, object_point[device][3]]
points3D = self.transformation_devices[
device].apply_transformation(points3D)
chessboard_points_cumulative_3d = np.column_stack(
(chessboard_points_cumulative_3d, points3D))
print(
' Extract the bounds between which the objects 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)
self.roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)
print(
"Calibration completed... \nPlace the box in the field of view of the devices..."
)
def setEmitter(self):
# print('Enable the emitter of the devices')
self.device_manager.enable_emitter(True)
# print('-------Enable the emitter of the devices')
# Load the JSON settings file in order to enable High Accuracy preset for the realsense
#self.device_manager.load_settings_json("HighResHighAccuracyPreset.json")
#print(' Get the extrinsics of the device to be used later')
extrinsics_devices = self.device_manager.get_depth_to_color_extrinsics(
self.frames)
print(
' Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image'
)
self.calibration_info_devices = defaultdict(list)
for calibration_info in (self.transformation_devices,
self.intrinsics_devices, extrinsics_devices):
for key, value in calibration_info.items():
self.calibration_info_devices[key].append(value)
print("CalibrasetEmittertion completed... ")
def processing(self):
frames_devices = self.device_manager.poll_frames()
print('get frames')
# Calculate the pointcloud using the depth frames from all the devices
point_cloud = calculate_cumulative_pointcloud(
frames_devices, self.calibration_info_devices, self.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, self.calibration_info_devices)
print('길이=', length, ' 폭=', width, ' 높이', height)
# 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)
def closeSense(self):
self.device_manager.disable_streams()