def on_sensor_frame(self, data):
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256, 256, 3):
print('image shape not 256, 256, 3')
return None
if rgb_image.shape[0] != self.image_hw:
rgb_image = misc.imresize(rgb_image, (self.image_hw, self.image_hw, 3))
rgb_image = data_iterator.preprocess_input(rgb_image)
pred = np.squeeze(model.predict(np.expand_dims(rgb_image, 0)))
if self.pred_viz_enabled:
self.queue.put([rgb_image, pred])
target_mask = pred[:, :, 2] > 0.5
# reduce the number of false positives by requiring more pixels to be identified as containing the target
if target_mask.sum() > 10:
centroid = scoring_utils.get_centroid_largest_blob(target_mask)
# scale the centroid from the nn image size to the original image size
centroid = centroid.astype(np.int).tolist()
# Obtain 3D world point from centroid pixel
depth_img = get_depth_image(data['depth_image'])
# Get XYZ coordinates for specific pixel
pixel_depth = depth_img[centroid[0]][centroid[1]][0]*50/255.0
point_3d = get_xyz_from_image(centroid[0], centroid[1], pixel_depth, self.image_hw)
point_3d.append(1)
# Get cam_pose from sensor_frame (ROS convention)
cam_pose = get_ros_pose(data['gimbal_pose'])
# Calculate xyz-world coordinates of the point corresponding to the pixel
# Transformation Matrix
R = euler2mat(math.radians(cam_pose[3]), math.radians(cam_pose[4]), math.radians(cam_pose[5]))
T = np.c_[R, cam_pose[:3]]
T = np.vstack([T, [0,0,0,1]]) # transformation matrix from world to quad
# 3D point in ROS coordinates
ros_point = np.dot(T, point_3d)
sio.emit('object_detected', {'coords': [ros_point[0], ros_point[1], ros_point[2]]})
if not self.target_found:
print('Target found!')
self.target_found = True
self.num_no_see = 0
# Publish Hero Marker
marker_pos = [ros_point[0],ros_point[1], ros_point[2]] + [0, 0, 0]
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
sio.emit('create_box_marker', marker_msg)
elif self.target_found:
self.num_no_see += 1
# print(self.num_no_see)
if self.target_found and self.num_no_see > 6:
print('Target lost!')
sio.emit('object_lost', {'data': ''})
self.target_found = False
self.num_no_see = 0
def on_sensor_data(data):
#print('Sensor frame received')
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256,256,3):
print('image shape not 256, 256, 3')
return None
# to save the received rgb images
# np.save(str(np.random.randint(9999)), rgb_image)
# only draw boxes every few seconds
if np.random.randint(45) > 41:
# cast a ray directly at the center of the image
# once this works we would need to vary this so that the centroid is not in the center of the image
# some errors will be suppressed by the symetry of this pixel choice, which is what I want at this moment.
ray = ray_casting.cast_ray(data, [128, 128])
# gimbal pose and quad pose: I am pretty sure they are in the world reference frame
# TODO remove this when sign flip on y is fixed in unity sim
gimbal_pose = tmpfix_position(list(map(float, data['gimbal_pose'].split(','))))
# TODO remove this when sign flip on y is fixed in unity sim
quad_pose = tmpfix_position(list(map(float, data['pose'].split(','))))
# the sim world has a static reference frame, the angle below is the angle between the world frame and the sensor frame
rot = transformations.euler_matrix(to_radians(gimbal_pose[3]),
to_radians(gimbal_pose[4]),
-to_radians(gimbal_pose[5]))[:3,:3]
# this is the identity rotation. The conventions of transformations, is w,x,y,z, and for unity x,y,z,w
gimbal_cam_rot = transformations.quaternion_matrix((1,0,0,0))[:3,:3]
# rotate the ray coordinates, so the ray is in the world frame. I think the order may matter here.
print('ray unrotated', ray)
ray = np.dot(gimbal_cam_rot, ray)
ray = np.dot(rot, np.array(ray))
print('ray rotated', ray)
# print some more debug data
euler = gimbal_pose[3:]
quaternion = transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
print('gimbal_rotation_quat', quaternion)
print('gimbal_pose_received', data['gimbal_pose'])
print('gimbal_pose_fixed', gimbal_pose)
euler = quad_pose[3:]
quaternion = transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
print('quad_rotation_quat', quaternion)
print('quad_position_received', data['pose'])
print('quad_pose_fixed', quad_pose)
# Flip the sign again so that that the pose received set by the sim is correct
# TODO remove the call to tmpfix when sign flip on y is fixed in unity sim
marker_pos = tmpfix_position((gimbal_pose[:3] + ray).tolist() + [0,0,0]).tolist()
marker_rot = [0,0,0]
quaternion = transformations.quaternion_from_euler(*marker_rot)
print('marker_rotation_quat', quaternion, '\n\n')
# prepare the marker message and send
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
socketIO.emit('create_box_marker', marker_msg)
def on_sensor_frame(self, data):
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256, 256, 3):
print('image shape not 256, 256, 3')
return None
if rgb_image.shape[0] != self.image_hw:
rgb_image = misc.imresize(rgb_image, (self.image_hw, self.image_hw, 3))
rgb_image = data_iterator.preprocess_input(rgb_image)
pred = np.squeeze(model.predict(np.expand_dims(rgb_image, 0)))
if self.pred_viz_enabled:
self.queue.put([rgb_image, pred])
target_mask = pred[:, :, 2] > 0.5
# reduce the number of false positives by requiring more pixels to be identified as containing the target
if target_mask.sum() > 10:
centroid = scoring_utils.get_centroid_largest_blob(target_mask)
# scale the centroid from the nn image size to the original image size
centroid = centroid.astype(np.int).tolist()
# Obtain 3D world point from centroid pixel
depth_img = get_depth_image(data['depth_image'])
# Get XYZ coordinates for specific pixel
pixel_depth = depth_img[centroid[0]][centroid[1]][0]*50/255.0
point_3d = get_xyz_from_image(centroid[0], centroid[1], pixel_depth, self.image_hw)
point_3d.append(1)
# Get cam_pose from sensor_frame (ROS convention)
cam_pose = get_ros_pose(data['gimbal_pose'])
# Calculate xyz-world coordinates of the point corresponding to the pixel
# Transformation Matrix
R = euler2mat(math.radians(cam_pose[3]), math.radians(cam_pose[4]), math.radians(cam_pose[5]))
T = np.c_[R, cam_pose[:3]]
T = np.vstack([T, [0,0,0,1]]) # transformation matrix from world to quad
# 3D point in ROS coordinates
ros_point = np.dot(T, point_3d)
sio.emit('object_detected', {'coords': [ros_point[0], ros_point[1], ros_point[2]]})
if not self.target_found:
print('Target found!')
self.target_found = True
self.num_no_see = 0
# Publish Hero Marker
marker_pos = [ros_point[0],ros_point[1], ros_point[2]] + [0, 0, 0]
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
sio.emit('create_box_marker', marker_msg)
elif self.target_found:
self.num_no_see += 1
# print(self.num_no_see)
if self.target_found and self.num_no_see > 6:
print('Target lost!')
sio.emit('object_lost', {'data': ''})
self.target_found = False
self.num_no_see = 0
def on_sensor_data(self, data):
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256, 256, 3):
print('image shape not 256, 256, 3')
return None
rgb_image = data_iterator.preprocess_input(rgb_image)
pred = np.squeeze(model.predict(np.expand_dims(rgb_image, 0)))
target_mask = pred[:, :, 1] > 0.5
if self.pred_viz_enabled:
overlay_viz(rgb_image, pred)
# reduce the number of false positives by requiring more pixels to be identified as containing the target
if target_mask.sum() > 10:
# Temporary move so we only get the overlays with positive identification
if self.image_save_dir is not None:
if time.time() - self.last_time_saved > 1:
result = visualization.overlay_predictions(rgb_image, pred, None, 0.5, 1)
out_file = os.path.join(self.image_save_dir, 'overlay_' + str(time.time()) + '.png')
misc.imsave(out_file, result)
self.last_time_saved = time.time()
centroid = scoring_utils.get_centroid_largest_blob(target_mask)
# scale the centroid from the nn image size to the original image size
centroid = centroid.astype(np.int).tolist()
# Obtain 3D world point from centroid pixel
depth_img = get_depth_image(data['depth_image'])
# Get XYZ coordinates for specific pixel
pixel_depth = depth_img[centroid[0]][centroid[1]][0]*100/255.0
point_3d = get_xyz_from_image(centroid[0], centroid[1], pixel_depth)
point_3d.append(1)
#print("Pixel Depth: ", pixel_depth)
#print ("Hit point: ", point_3d)
# Get cam_pose from sensor_frame (ROS convention)
cam_pose = get_ros_pose(data['gimbal_pose'])
#print ("Quad Pose: ", cam_pose)
# Calculate xyz-world coordinates of the point corresponding to the pixel
# Transformation Matrix
R = euler2mat(math.radians(cam_pose[3]), math.radians(cam_pose[4]), math.radians(cam_pose[5]))
T = np.c_[R, cam_pose[:3]]
T = np.vstack([T, [0,0,0,1]]) # transformation matrix from world to quad
# 3D point in ROS coordinates
ros_point = np.dot(T, point_3d)
socketIO.emit('object_detected', {'coords': [ros_point[0], ros_point[1], ros_point[2]]})
self.target_found = True
self.num_no_see = 0
# Publish Hero Marker
marker_pos = [ros_point[0],ros_point[1], ros_point[2]] + [0, 0, 0]
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
socketIO.emit('create_box_marker', marker_msg)
# 3D point in Unity coordinates
#unity_point = get_unity_pose_from_ros(ros_point)
#print ros_point.shape
# with the depth image, and centroid from prediction we can compute
# the x,y,z coordinates where the ray intersects an object
# ray = ray_casting.cast_ray(data, [centroid[1], centroid[0]])
# pose = np.array(list(map(float, data['gimbal_pose'].split(','))))
# TODO add rotation of the camera with respect to the gimbal
# create the rotation matrix to rotate the sensors frame of reference to the world frame
# rot = transformations.euler_matrix(to_radians(pose[3]),
# to_radians(pose[4]),
# to_radians(pose[5]))[:3, :3]
# rotate array
# ray = np.dot(rot, np.array(ray))
elif self.target_found:
self.num_no_see += 1
if self.target_found and self.num_no_see > 8:
socketIO.emit('object_lost', {'data': ''})
self.target_found = False
self.num_no_see = 0
def emit_marker(self, pose):
msg = sio_msgs.create_box_marker_msg(self.next_marker_id, pose)
self.next_marker_id += 1
print(msg)
socketIO.emit('create_box_marker', msg)
