Skip to content

Commit

Permalink
Change way to visualize detections.
Browse files Browse the repository at this point in the history 
Issue #15 was caused by errors in using the `_debug` member variable, such as
forgetting to wrap access to it in `#ifdef DEBUG`. A couple of build problems were
discovered, when the `DEBUG` flag was either set or unset, resulting from such
logical errors in the code. This issue has been resolved, and the code made a
little cleaner, by refactoring the way detections are visualized.

Instead of modifying `_debug`  whenever facial features or
head poses are detected, define member functions for drawing the
most recent detections onto a copy of the original image. Detections can be
visualized even when the flag to enable debug visualizations is not set, as in
`show_head_pose.cpp`.

Rename the flag so as not to conflate the meaning of `DEBUG`, which
conventionally refers to *Debug* vs. *Release* compilation mode.
One may want to compile the package in Release mode and still be
able to visualize detections.

NOTE: visualization of numeric IDs of facial features, as well as
reprojected points has been commented out due to personal preference.

Resolves #15.
  • Loading branch information
@severin-lemaignan
James Giller authored and severin-lemaignan committed Dec 5, 2017
1 parent 55f3f52 commit 2d82998
Show file tree
Hide file tree
Showing 10 changed files with 147 additions and 100 deletions.
11 changes: 5 additions & 6 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,9 @@ add_definitions(-std=c++11 -DGAZR_VERSION=${VERSION})
find_package(dlib REQUIRED)
include_directories(${dlib_INCLUDE_DIRS})

option(DEBUG "Enable debug visualizations" ON)
option(WITH_TOOLS "Compile sample tools" ON)
option(WITH_ROS "Build ROS nodes" OFF)
option(DEBUG_OUTPUT "Enable debug visualizations" OFF)
option(WITH_TOOLS "Compile sample tools" OFF)
option(WITH_ROS "Build ROS nodes" ON)

if(WITH_ROS)

Expand All @@ -32,7 +32,7 @@ if(WITH_ROS)

endif()

if(DEBUG)
if(DEBUG_OUTPUT)
find_package(OpenCV COMPONENTS core imgproc calib3d highgui REQUIRED)
else()
find_package(OpenCV COMPONENTS core imgproc calib3d REQUIRED)
Expand All @@ -53,10 +53,9 @@ if(WITH_ROS)
)
endif()

if(DEBUG)
if(DEBUG_OUTPUT)
add_definitions(-DHEAD_POSE_ESTIMATION_DEBUG)
endif()

include_directories(${OpenCV_INCLUDE_DIRS})

add_library(gazr SHARED src/head_pose_estimation.cpp)
Expand Down
2 changes: 1 addition & 1 deletion launch/gazr.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@

<group ns="$(arg ns)">
<node pkg="gazr" type="estimate" name="gazr" output="screen" required="true" >
<param name="face_model" value="$(find gazr)/shape_predictor_68_face_landmarks.dat" />
<param name="face_model" value="$(find gazr)/share/shape_predictor_68_face_landmarks.dat" />
<param name="prefix" value="$(arg face_prefix)" />
<param name="with_depth" value="$(arg with_depth)" />
<remap from="rgb" to="$(arg rgb)"/>
Expand Down
11 changes: 8 additions & 3 deletions src/facialfeaturescloud.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,16 @@ FacialFeaturesPointCloudPublisher::FacialFeaturesPointCloudPublisher(ros::NodeHa
/// Publishing
facial_features_pub = rosNode.advertise<PointCloud>("facial_features", 1);

#ifdef HEAD_POSE_ESTIMATION_DEBUG
pub = rgb_it_->advertise("attention_tracker/faces/image",1);
#endif
}

/**
* Based on https://github.com/ros-perception/image_pipeline/blob/indigo/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp
*/
template<typename T>
Point3f FacialFeaturesPointCloudPublisher::makeFeatureCloud(const vector<Point> points2d,
void FacialFeaturesPointCloudPublisher::makeFeatureCloud(const vector<Point> points2d,
const sensor_msgs::ImageConstPtr& depth_msg,
PointCloud::Ptr& cloud_msg) {

Expand Down Expand Up @@ -191,7 +194,9 @@ void FacialFeaturesPointCloudPublisher::imageCb(const sensor_msgs::ImageConstPtr


auto poses = estimator.poses();
ROS_INFO_STREAM(poses.size() << " faces detected.");
#ifdef HEAD_POSE_ESTIMATION_DEBUG
ROS_INFO_STREAM(poses.size() << " faces detected.");
#endif

std_msgs::Char nb_faces;
nb_faces.data = poses.size();
Expand Down Expand Up @@ -227,7 +232,7 @@ void FacialFeaturesPointCloudPublisher::imageCb(const sensor_msgs::ImageConstPtr
#ifdef HEAD_POSE_ESTIMATION_DEBUG
if(pub.getNumSubscribers() > 0) {
ROS_INFO_ONCE("Starting to publish face tracking output for debug");
auto debugmsg = cv_bridge::CvImage(msg->header, "bgr8", estimator._debug).toImageMsg();
auto debugmsg = cv_bridge::CvImage(rgb_msg->header, "bgr8", estimator.drawDetections(rgb, all_features, poses)).toImageMsg();
pub.publish(debugmsg);
}
#endif
Expand Down
3 changes: 2 additions & 1 deletion src/facialfeaturescloud.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ class FacialFeaturesPointCloudPublisher {
private:

template<typename T>
cv::Point3f makeFeatureCloud(const std::vector<cv::Point> points2d,
void makeFeatureCloud(const std::vector<cv::Point> points2d,
const sensor_msgs::ImageConstPtr& depth_msg,
PointCloud::Ptr& cloud_msg);

Expand All @@ -60,6 +60,7 @@ class FacialFeaturesPointCloudPublisher {

// Publishers
/////////////////////////////////////////////////////////
image_transport::Publisher pub;

// prefix prepended to TF frames generated for each frame
std::string facePrefix;
Expand Down
178 changes: 105 additions & 73 deletions src/head_pose_estimation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@
#include <ros/ros.h>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#ifdef HEAD_POSE_ESTIMATION_DEBUG
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#endif

Expand Down Expand Up @@ -72,54 +72,6 @@ std::vector<std::vector<Point>> HeadPoseEstimation::update(cv::InputArray _image
all_features.push_back(features);
}


#ifdef HEAD_POSE_ESTIMATION_DEBUG
// Draws the contours of the face and face features onto the image

_debug = image.clone();

auto color = Scalar(0,128,128);

for (size_t j = 0; j < shapes.size(); ++j)
{
const full_object_detection& d = shapes[j];

for (size_t i = 1; i <= 16; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);

for (size_t i = 28; i <= 30; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);

for (size_t i = 18; i <= 21; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
for (size_t i = 23; i <= 26; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
for (size_t i = 31; i <= 35; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
line(_debug, toCv(d.part(30)), toCv(d.part(35)), color, 2, CV_AA);

for (size_t i = 37; i <= 41; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
line(_debug, toCv(d.part(36)), toCv(d.part(41)), color, 2, CV_AA);

for (size_t i = 43; i <= 47; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
line(_debug, toCv(d.part(42)), toCv(d.part(47)), color, 2, CV_AA);

for (size_t i = 49; i <= 59; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
line(_debug, toCv(d.part(48)), toCv(d.part(59)), color, 2, CV_AA);

for (size_t i = 61; i <= 67; ++i)
line(_debug, toCv(d.part(i)), toCv(d.part(i-1)), color, 2, CV_AA);
line(_debug, toCv(d.part(60)), toCv(d.part(67)), color, 2, CV_AA);

for (size_t i = 0; i < 68 ; i++) {
putText(_debug, to_string(i), toCv(d.part(i)), FONT_HERSHEY_DUPLEX, 0.6, Scalar(255,255,255));
}
}
#endif

return all_features;
}

Expand Down Expand Up @@ -185,48 +137,128 @@ head_pose HeadPoseEstimation::pose(size_t face_idx) const
0, 0, 0, 1
};

#ifdef HEAD_POSE_ESTIMATION_DEBUG
return pose;
}

std::vector<head_pose> HeadPoseEstimation::poses() const {

std::vector<Point2f> reprojected_points;
std::vector<head_pose> res;

projectPoints(head_points, rvec, tvec, projection, noArray(), reprojected_points);
for (auto i = 0; i < faces.size(); i++){
res.push_back(pose(i));
}

return res;

for (auto point : reprojected_points) {
circle(_debug, point,2, Scalar(0,255,255),2);
}

Mat HeadPoseEstimation::drawDetections(const cv::Mat& original_image, const std::vector<std::vector<Point>>& detected_features, const std::vector<head_pose>& detected_poses) {
auto result = original_image.clone();
if (!detected_features.empty()) {
drawFeatures(detected_features, result);
}
for (size_t i = 0; i < detected_poses.size(); ++i)
{
drawPose(detected_poses[i], i, result);
}
return result;
}

std::vector<Point3f> axes;
axes.push_back(Point3f(0,0,0));
axes.push_back(Point3f(50,0,0));
axes.push_back(Point3f(0,50,0));
axes.push_back(Point3f(0,0,50));
std::vector<Point2f> projected_axes;
void HeadPoseEstimation::drawFeatures(const std::vector<std::vector<Point>>& detected_features, Mat& result) const {

projectPoints(axes, rvec, tvec, projection, noArray(), projected_axes);
static const auto line_color = Scalar(0,128,128);
static const auto text_color = Scalar(255,255,255);

line(_debug, projected_axes[0], projected_axes[3], Scalar(255,0,0),2,CV_AA);
line(_debug, projected_axes[0], projected_axes[2], Scalar(0,255,0),2,CV_AA);
line(_debug, projected_axes[0], projected_axes[1], Scalar(0,0,255),2,CV_AA);
for (size_t j = 0; j < detected_features.size(); ++j)
{
const auto& feature_points = detected_features[j];

putText(_debug, "(" + to_string(int(pose(0,3) * 100)) + "cm, " + to_string(int(pose(1,3) * 100)) + "cm, " + to_string(int(pose(2,3) * 100)) + "cm)", coordsOf(face_idx, SELLION), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,0,255),2);
for (size_t i = 1; i <= 16; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);

for (size_t i = 28; i <= 30; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);

#endif
for (size_t i = 18; i <= 21; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
for (size_t i = 23; i <= 26; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
for (size_t i = 31; i <= 35; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
line(result, feature_points[30], feature_points[35], line_color, 2, CV_AA);

return pose;
}
for (size_t i = 37; i <= 41; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
line(result, feature_points[36], feature_points[41], line_color, 2, CV_AA);

std::vector<head_pose> HeadPoseEstimation::poses() const {
for (size_t i = 43; i <= 47; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
line(result, feature_points[42], feature_points[47], line_color, 2, CV_AA);

std::vector<head_pose> res;
for (size_t i = 49; i <= 59; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
line(result, feature_points[48], feature_points[59], line_color, 2, CV_AA);

for (auto i = 0; i < faces.size(); i++){
res.push_back(pose(i));
for (size_t i = 61; i <= 67; ++i)
line(result, feature_points[i], feature_points[i-1], line_color, 2, CV_AA);
line(result, feature_points[60], feature_points[67], line_color, 2, CV_AA);

// for (size_t i = 0; i < 68 ; i++) {
// putText(result, to_string(i), feature_points[i], FONT_HERSHEY_DUPLEX, 0.6, text_color);
// }
}
}

return res;
void HeadPoseEstimation::drawPose(const head_pose& detected_pose, size_t face_idx, cv::Mat& result) const {
const auto rotation = Mat(detected_pose)(Range(0, 3), Range(0, 3));
auto rvec = Mat_<double>(3, 1);
Rodrigues(rotation, rvec);

auto tvec = Mat(detected_pose).col(3).rowRange(0, 3);


cv::Matx33f projection(focalLength, 0.0, opticalCenterX,
0.0, focalLength, opticalCenterY,
0.0, 0.0, 1.0);


std::vector<Point3f> head_points;

head_points.push_back(P3D_SELLION);
head_points.push_back(P3D_RIGHT_EYE);
head_points.push_back(P3D_LEFT_EYE);
head_points.push_back(P3D_RIGHT_EAR);
head_points.push_back(P3D_LEFT_EAR);
head_points.push_back(P3D_MENTON);
head_points.push_back(P3D_NOSE);
head_points.push_back(P3D_STOMMION);

// std::vector<Point2f> reprojected_points;
// projectPoints(head_points, rvec, tvec, projection, noArray(), reprojected_points);

// static const auto circle_color = Scalar(0, 255, 255);
// for (auto point : reprojected_points) {
// circle(result, point,2, circle_color,2);
// }

std::vector<Point3f> axes;
axes.push_back(Point3f(0,0,0));
axes.push_back(Point3f(50,0,0));
axes.push_back(Point3f(0,50,0));
axes.push_back(Point3f(0,0,50));

std::vector<Point2f> projected_axes;
projectPoints(axes, rvec, tvec, projection, noArray(), projected_axes);

static const auto x_axis_color = Scalar(255, 0, 0);
static const auto y_axis_color = Scalar(0, 255, 0);
static const auto z_axis_color = Scalar(0, 0, 255);
line(result, projected_axes[0], projected_axes[3], x_axis_color,2,CV_AA);
line(result, projected_axes[0], projected_axes[2], y_axis_color,2,CV_AA);
line(result, projected_axes[0], projected_axes[1], z_axis_color,2,CV_AA);

static const auto text_color = Scalar(0,0,255);
putText(result, "(" + to_string(int(detected_pose(0,3) * 100)) + "cm, " + to_string(int(detected_pose(1,3) * 100)) + "cm, " + to_string(int(detected_pose(2,3) * 100)) + "cm)", coordsOf(face_idx, SELLION), FONT_HERSHEY_SIMPLEX, 0.5, text_color,2);
}

Point2f HeadPoseEstimation::coordsOf(size_t face_idx, FACIAL_FEATURE feature) const
Expand Down
14 changes: 10 additions & 4 deletions src/head_pose_estimation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,14 +64,16 @@ class HeadPoseEstimation {

std::vector<head_pose> poses() const;

/** Returns an augmented image with the detected facial features and head pose drawn in.
*
* Leave either detected_features or detected_poses empty to skip drawing the respective detections.
*/
cv::Mat drawDetections(const cv::Mat& original_image, const std::vector<std::vector<cv::Point>>& detected_features, const std::vector<head_pose>& detected_poses);

float focalLength;
float opticalCenterX;
float opticalCenterY;

#ifdef HEAD_POSE_ESTIMATION_DEBUG
mutable cv::Mat _debug;
#endif

private:

dlib::cv_image<dlib::bgr_pixel> current_image;
Expand All @@ -84,6 +86,10 @@ class HeadPoseEstimation {
std::vector<dlib::full_object_detection> shapes;


void drawFeatures(const std::vector<std::vector<cv::Point>>& detected_features, cv::Mat& result) const;

void drawPose(const head_pose& detected_pose, size_t face_idx, cv::Mat& result) const;

/** Return the point corresponding to the dictionary marker.
*/
cv::Point2f coordsOf(size_t face_idx, FACIAL_FEATURE feature) const;
Expand Down
6 changes: 4 additions & 2 deletions src/ros_head_pose_estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,12 @@ void HeadPoseEstimator::detectFaces(const sensor_msgs::ImageConstPtr& rgb_msg,
* Faces detection *
********************************************************************/

estimator.update(rgb);
auto all_features = estimator.update(rgb);

auto poses = estimator.poses();
#ifdef HEAD_POSE_ESTIMATION_DEBUG
ROS_INFO_STREAM(poses.size() << " faces detected.");
#endif

std_msgs::Char nb_faces;
nb_faces.data = poses.size();
Expand Down Expand Up @@ -105,7 +107,7 @@ void HeadPoseEstimator::detectFaces(const sensor_msgs::ImageConstPtr& rgb_msg,
#ifdef HEAD_POSE_ESTIMATION_DEBUG
if(pub.getNumSubscribers() > 0) {
ROS_INFO_ONCE("Starting to publish face tracking output for debug");
auto debugmsg = cv_bridge::CvImage(msg->header, "bgr8", estimator._debug).toImageMsg();
auto debugmsg = cv_bridge::CvImage(rgb_msg->header, "bgr8", estimator.drawDetections(rgb, all_features, poses)).toImageMsg();
pub.publish(debugmsg);
}
#endif
Expand Down
4 changes: 2 additions & 2 deletions tools/estimate_head_direction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ int main(int argc, char **argv)
}


estimator.update(frame);
auto all_features = estimator.update(frame);


auto poses = estimator.poses();
Expand Down Expand Up @@ -150,7 +150,7 @@ int main(int argc, char **argv)
cout << "}\n" << flush;

if (show_frame) {
imshow("headpose", estimator._debug);
imshow("headpose", estimator.drawDetections(frame, all_features, poses));
if(use_camera) {
waitKey(10);
}
Expand Down
Loading

0 comments on commit 2d82998

Please sign in to comment.