Skip to content

Instantly share code, notes, and snippets.

@jtbandes
Last active January 4, 2022 19:33
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 1 You must be signed in to fork a gist
  • Save jtbandes/938a6d39308f145ebc59b4b1c5c8a11d to your computer and use it in GitHub Desktop.
Save jtbandes/938a6d39308f145ebc59b4b1c5c8a11d to your computer and use it in GitHub Desktop.
# Step 1: a basic ROS node that receives camera images.
import rospy
from sensor_msgs.msg import Image
def image_callback(msg: Image):
rospy.loginfo("Got an image!")
def main():
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, image_callback, queue_size=1)
rospy.spin()
if __name__ == "__main__":
main()
# Step 2: run the dlib face detector and log the number of faces found.
import cv2
import rospy
import dlib
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
def image_callback(msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
def main():
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, image_callback, queue_size=1)
rospy.spin()
if __name__ == "__main__":
main()
# Step 3: publish an ImageMarkerArray topic to draw a box around each face.
import cv2
import rospy
import dlib
from cv_bridge import CvBridge
from std_msgs.msg import ColorRGBA
from sensor_msgs.msg import Image
from foxglove_msgs.msg import ImageMarkerArray
from visualization_msgs.msg import ImageMarker
from geometry_msgs.msg import Point
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
class Node:
pub_markers: rospy.Publisher
def __init__(self):
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback, queue_size=1)
self.pub_markers = rospy.Publisher(
"/usb_cam/face_markers", ImageMarkerArray, queue_size=1
)
rospy.spin()
def image_callback(self, msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
# Display each face as a cyan rectangle
markers = ImageMarkerArray()
for rect in rects:
markers.markers.append(
ImageMarker(
header=msg.header,
type=ImageMarker.LINE_STRIP,
outline_color=ColorRGBA(0, 255, 255, 1),
points=[
Point(rect.left(), rect.top(), 0),
Point(rect.right(), rect.top(), 0),
Point(rect.right(), rect.bottom(), 0),
Point(rect.left(), rect.bottom(), 0),
Point(rect.left(), rect.top(), 0),
],
)
)
self.pub_markers.publish(markers)
def main():
Node()
if __name__ == "__main__":
main()
# Step 4: run the facial landmark predictor and draw a polygon for each facial feature.
import cv2
import rospy
import dlib
from imutils.face_utils import FACIAL_LANDMARKS_68_IDXS
from cv_bridge import CvBridge
from std_msgs.msg import ColorRGBA
from sensor_msgs.msg import Image
from foxglove_msgs.msg import ImageMarkerArray
from visualization_msgs.msg import ImageMarker
from geometry_msgs.msg import Point
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor("shape_predictor_68_face_landmarks.dat")
COLORS = [
ColorRGBA(0, 0, 0, 1),
ColorRGBA(0, 0, 255, 1),
ColorRGBA(0, 255, 0, 1),
ColorRGBA(0, 255, 255, 1),
ColorRGBA(255, 0, 0, 1),
ColorRGBA(255, 0, 255, 1),
ColorRGBA(255, 255, 0, 1),
ColorRGBA(255, 255, 255, 1),
]
class Node:
pub_markers: rospy.Publisher
def __init__(self):
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback, queue_size=1)
self.pub_markers = rospy.Publisher(
"/usb_cam/face_markers", ImageMarkerArray, queue_size=1
)
rospy.spin()
def image_callback(self, msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
markers = ImageMarkerArray()
for rect in rects:
# Run the predictor, which returns a list of 68 facial landmarks as (x,y) points
points = predictor(grayscale_img, rect).parts()
# Draw a line around each face region
for region_idx, (name, (start_idx, end_idx)) in enumerate(
FACIAL_LANDMARKS_68_IDXS.items()
):
region_points = points[start_idx:end_idx]
# Connect the points for each region in a loop, except for the jaw
if name != "jaw":
region_points.append(region_points[0])
markers.markers.append(
ImageMarker(
header=msg.header,
type=ImageMarker.LINE_STRIP,
points=[Point(p.x, p.y, 0) for p in region_points],
outline_color=COLORS[region_idx % len(COLORS)],
)
)
self.pub_markers.publish(markers)
def main():
Node()
if __name__ == "__main__":
main()
@foxymiles
Copy link

Updated at https://gist.github.com/foxymiles/09efce7cb134b3c39fada02fff983621 to account for changes in Foxglove Studio since the post was originally written.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment