diff --git a/CMakeLists.txt b/CMakeLists.txt index b4d309c0..fb39261f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(opencv_apps) -find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp) +find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs) find_package(OpenCV REQUIRED) message(STATUS "OpenCV VERSION: ${OpenCV_VERSION}") @@ -32,6 +32,7 @@ generate_dynamic_reconfigure_options( cfg/EdgeDetection.cfg cfg/HoughLines.cfg cfg/HoughCircles.cfg cfg/FindContours.cfg cfg/ConvexHull.cfg cfg/GeneralContours.cfg cfg/ContourMoments.cfg cfg/FaceDetection.cfg + cfg/FaceRecognition.cfg cfg/GoodfeatureTrack.cfg cfg/CornerHarris.cfg # @@ -63,7 +64,6 @@ add_message_files( FlowStamped.msg FlowArray.msg FlowArrayStamped.msg - RectArrayStamped.msg Size.msg Face.msg FaceArray.msg @@ -86,9 +86,15 @@ add_message_files( ContourArrayStamped.msg ) +add_service_files( + FILES + FaceRecognitionTrain.srv +) + ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES + sensor_msgs std_msgs ) @@ -253,6 +259,7 @@ opencv_apps_add_nodelet(camshift camshift/camshift src/nodelet/camshift_nodelet. # ./em.cpp # ./example_cmake/example.cpp opencv_apps_add_nodelet(face_detection face_detection/face_detection src/nodelet/face_detection_nodelet.cpp) # ./facedetect.cpp +opencv_apps_add_nodelet(face_recognition face_recognition/face_recognition src/nodelet/face_recognition_nodelet.cpp) # ./facial_features.cpp opencv_apps_add_nodelet(fback_flow fback_flow/fback_flow src/nodelet/fback_flow_nodelet.cpp) # ./fback.cpp # ./ffilldemo.cpp @@ -343,7 +350,7 @@ install(TARGETS ${_opencv_apps_nodelet_targets} install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install(DIRECTORY launch test +install(DIRECTORY launch test scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/cfg/FaceRecognition.cfg b/cfg/FaceRecognition.cfg new file mode 100755 index 00000000..bdff7139 --- /dev/null +++ b/cfg/FaceRecognition.cfg @@ -0,0 +1,61 @@ +#! /usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, Yuki Furuta. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Kei Okada nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +PACKAGE='face_recognition' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +methods = gen.enum([gen.const("eigen", str_t, "eigen", "eigen"), + gen.const("fisher", str_t, "fisher", "fisher"), + gen.const("LBPH", str_t, "LBPH", "LBPH")], + "Method to recognize faces") + +# variable type level description default min max +gen.add("model_method", str_t, 0, "Method to recognize faces", "eigen", edit_method=methods) +gen.add("use_saved_data", bool_t, 0, "Use saved data", True) +gen.add("save_train_data", bool_t, 0, "Save train data", True) +gen.add("data_dir", str_t, 0, "Save directory for train data", "~/.ros/opencv_apps/face_data") +gen.add("face_model_width", int_t, 0, "Width of training face image", 190, 30, 500) +gen.add("face_model_height", int_t, 0, "Height of training face image", 90, 30, 500) +gen.add("face_padding", double_t, 0, "Padding ratio of each face", 0.1, 0.0, 2.0) +gen.add("model_num_components", int_t, 0, "Number of components for face recognizer model", 0, 0, 100) +gen.add("model_threshold", double_t, 0, "Threshold for face recognizer model", 8000.0, 0.0, 10000.0) +gen.add("lbph_radius", int_t, 0, "Radius parameter used only for LBPH model", 1, 1, 10) +gen.add("lbph_neighbors", int_t, 0, "Neighbors parameter used only for LBPH model", 8, 1, 30) +gen.add("lbph_grid_x", int_t, 0, "grid_x parameter used only for LBPH model", 8, 1, 30) +gen.add("lbph_grid_y", int_t, 0, "grid_y parameter used only for LBPH model", 8, 1, 30) + +exit(gen.generate(PACKAGE, "face_recognition", "FaceRecognition")) diff --git a/launch/face_recognition.launch b/launch/face_recognition.launch new file mode 100644 index 00000000..b7dbca80 --- /dev/null +++ b/launch/face_recognition.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/Face.msg b/msg/Face.msg index 557556b6..789e0cfe 100644 --- a/msg/Face.msg +++ b/msg/Face.msg @@ -1,2 +1,4 @@ Rect face Rect[] eyes +string label +float64 confidence diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 74a7466b..1ab76dac 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -43,6 +43,10 @@ Nodelet to find faces + + Nodelet to identify faces + + Nodelet for detecting corners using Shi-Tomasi method diff --git a/scripts/face_recognition_trainer.py b/scripts/face_recognition_trainer.py new file mode 100755 index 00000000..b62c5e30 --- /dev/null +++ b/scripts/face_recognition_trainer.py @@ -0,0 +1,97 @@ +#! /usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, Yuki Furuta. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Kei Okada nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +from __future__ import print_function + +try: + input = raw_input +except: + pass + +import rospy +import message_filters +from sensor_msgs.msg import Image +from opencv_apps.msg import FaceArrayStamped +from opencv_apps.srv import FaceRecognitionTrain, FaceRecognitionTrainRequest + +class FaceRecognitionTrainer(object): + def __init__(self): + self.queue_size = rospy.get_param("~queue_size", 100) + + self.img_sub = message_filters.Subscriber("image", Image) + self.face_sub = message_filters.Subscriber("faces", FaceArrayStamped) + + self.req = FaceRecognitionTrainRequest() + self.label = "" + self.ok = False + + self.sync = message_filters.TimeSynchronizer([self.img_sub, self.face_sub], + self.queue_size) + self.sync.registerCallback(self.callback) + def callback(self, img, faces): + if len(faces.faces) <= 0: + return + if self.ok: + faces.faces.sort(key=lambda f: f.face.width * f.face.height) + self.req.images.append(img) + self.req.rects.append(faces.faces[0].face) + self.req.labels.append(self.label) + self.ok = False + def run(self): + rospy.wait_for_service("train") + train = rospy.ServiceProxy("train", FaceRecognitionTrain) + self.label = input("Please input your name and press Enter: ") + while len(self.label) <= 0 or input("Your name is %s. Correct? [y/n]: " % self.label) not in ["", "y", "Y"]: + self.label = input("Please input your name and press Enter: ") + + input("Please stand at the center of the camera and press Enter: ") + while True: + self.ok = True + while self.ok: + print("taking picture...") + rospy.sleep(1) + if input("One more picture? [y/n]: ") not in ["", "y", "Y"]: + break + print("sending to trainer...") + + res = train(self.req) + if res.ok: + print("OK. Trained successfully!") + else: + print("NG. Error: %s" % res.error) + +if __name__ == '__main__': + rospy.init_node("face_recognition_trainer") + t = FaceRecognitionTrainer() + t.run() diff --git a/src/nodelet/face_recognition_nodelet.cpp b/src/nodelet/face_recognition_nodelet.cpp new file mode 100644 index 00000000..24b8051e --- /dev/null +++ b/src/nodelet/face_recognition_nodelet.cpp @@ -0,0 +1,559 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, Yuki Furuta. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Kei Okada nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + +namespace enc = sensor_msgs::image_encodings; +#if BOOST_VERSION < 105000 +namespace fs = boost::filesystem3; // for hydro +#else +namespace fs = boost::filesystem; +#endif + +#if CV_MAJOR_VERSION >= 3 +#include +namespace face = cv::face; +#else +namespace face = cv; +#endif + +// utility for resolving path +namespace boost { +#if BOOST_VERSION < 105000 +namespace filesystem3 { // for hydro +#else +namespace filesystem { +#endif + template< > + path& path::append( + typename path::iterator lhs, typename path::iterator rhs, const codecvt_type& cvt) { + for(;lhs != rhs; ++lhs) *this /= *lhs; + return *this; + } + path user_expanded_path(const path &p) { + path::const_iterator it(p.begin()); + std::string user_dir = (*it).string(); + if (user_dir.length() == 0 || user_dir[0] != '~') + return p; + path ret; + char* homedir; + if (user_dir.length() == 1) { + homedir = getenv("HOME"); + if (homedir == NULL) { + homedir = getpwuid(getuid())->pw_dir; + } + } else { + std::string uname = user_dir.substr(1, user_dir.length()); + passwd* pw = getpwnam(uname.c_str()); + if (pw == NULL) return p; + homedir = pw->pw_dir; + } + ret = path(std::string(homedir)); + return ret.append(++it, p.end(), path::codecvt()); + } +}} // end of utility for resolving paths + +namespace face_recognition { + + class LabelMapper { + std::map m_; + public: + void add(std::vector &l) { + int id = 0; + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { + if (id < it->second) id = it->second + 1; + } + for (size_t i = 0; i < l.size(); ++i) { + if (m_.find(l[i]) == m_.end()) { + m_[l[i]] = id; + id++; + } + } + } + std::vector get(std::vector &v) { + std::vector ret(v.size()); + for (size_t i = 0; i < v.size(); ++i) { + ret[i] = m_[v[i]]; + } + return ret; + } + std::string lookup(int id) { + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { + if (it->second == id) return it->first; + } + return "nan"; + } + + void debugPrint() { + ROS_WARN_STREAM("label mapper: debug print:"); + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { + ROS_WARN_STREAM("\t" << it->first << ": " << it->second); + } + ROS_WARN_STREAM("label mapper: debug print end"); + } + }; + + class Storage { + fs::path base_dir_; + public: + Storage(const fs::path &base_dir) { + base_dir_ = fs::user_expanded_path(base_dir); + if (!fs::exists(base_dir_)) { + init(); + } + if (!fs::is_directory(base_dir_)) { + std::stringstream ss; + ss << base_dir_ << " is not a directory"; + throw std::runtime_error(ss.str()); + } + }; + void init() { + if (!fs::create_directories(base_dir_)) { + std::stringstream ss; + ss << "failed to initialize directory: " << base_dir_; + throw std::runtime_error(ss.str()); + } + } + + void load(std::vector &images, std::vector &labels, bool append = true) { + if (!append) { + images.clear(); + labels.clear(); + } + fs::directory_iterator end; + for (fs::directory_iterator it(base_dir_); it != end; ++it) { + if (fs::is_directory(*it)) { + std::string label = it->path().stem().string(); + for (fs::directory_iterator cit(it->path()); cit != end; ++cit) { + if (fs::is_directory(*cit)) continue; + fs::path file_path = cit->path(); + try { + cv::Mat img = cv::imread(file_path.string(), CV_LOAD_IMAGE_COLOR); + labels.push_back(label); + images.push_back(img); + } catch (cv::Exception &e) { + ROS_ERROR_STREAM("Error: load image from " << file_path << ": " << e.what()); + } + } + } + } + } + + void save(const std::vector &images, const std::vector &labels) { + if (images.size() != labels.size()) { + throw std::length_error("images.size() != labels.size()"); + } + for (size_t i = 0; i < images.size(); ++i) { + save(images[i], labels[i]); + } + } + + void save(const cv::Mat &image, const std::string &label) { + fs::path img_dir = base_dir_ / fs::path(label); + if (!fs::exists(img_dir) && !fs::create_directories(img_dir)) { + std::stringstream ss; + ss << "failed to initialize directory: " << img_dir; + throw std::runtime_error(ss.str()); + } + fs::directory_iterator end; + int file_count = 0; + for (fs::directory_iterator it(img_dir); it != end; ++it) { + if (!fs::is_directory(*it)) file_count++; + } + + std::stringstream ss; + // data_dir/person_label/person_label_123456.jpg + ss << label << "_" << std::setw(6) << std::setfill('0') << file_count << ".jpg"; + fs::path file_path = img_dir / ss.str(); + ROS_INFO_STREAM("saving image to :" << file_path); + try { + cv::imwrite(file_path.string(), image); + } catch(cv::Exception &e) { + ROS_ERROR_STREAM("Error: save image to " << file_path << ": " << e.what()); + } + } + }; + + class FaceRecognitionNodelet: public opencv_apps::Nodelet + { + typedef face_recognition::FaceRecognitionConfig Config; + typedef dynamic_reconfigure::Server Server; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, opencv_apps::FaceArrayStamped> SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, opencv_apps::FaceArrayStamped> ApproximateSyncPolicy; + + Config config_; + boost::shared_ptr cfg_srv_; + boost::shared_ptr it_; + boost::shared_ptr > sync_; + boost::shared_ptr > async_; + image_transport::SubscriberFilter img_sub_; + message_filters::Subscriber face_sub_; + ros::Publisher debug_img_pub_; + ros::Publisher face_pub_; + ros::ServiceServer train_srv_; + + bool save_train_data_; + bool use_async_; + bool use_saved_data_; + double face_padding_; + int queue_size_; + std::string data_dir_; + boost::mutex mutex_; + + boost::shared_ptr label_mapper_; + boost::shared_ptr storage_; + cv::Size face_model_size_; + cv::Ptr model_; + + void drawFace(cv::Mat &img, const opencv_apps::Face &face) { + cv::Rect r(int(face.face.x - face.face.width / 2.0 - face.face.width * face_padding_ / 2.0), + int(face.face.y - face.face.height / 2.0 - face.face.height * face_padding_ / 2.0), + int(face.face.width + face.face.width * face_padding_), + int(face.face.height + face.face.height * face_padding_)); + cv::Scalar color(0.0, 0.0, 255.0); + int boldness = 2; + cv::rectangle(img, r.tl(), r.br(), color, boldness, CV_AA); + + double font_scale = 1.5; + int text_height = 20; + cv::Point text_bl; + if (r.br().y + text_height > img.rows) text_bl = r.tl() + cv::Point(0, -text_height); + else text_bl = r.br() + cv::Point(-r.width, text_height); + std::stringstream ss; + ss << face.label << " (" << std::fixed << std::setprecision(2) << face.confidence << ")"; + cv::putText(img, ss.str(), text_bl, cv::FONT_HERSHEY_PLAIN, font_scale, color, boldness, CV_AA); + } + + void extractImage(const cv::Mat &img, const opencv_apps::Rect &rect, cv::Mat &ret, double padding = 0.0) { + int x = std::max(0, int(rect.x - rect.width / 2.0 - rect.width * padding / 2.0)); + int y = std::max(0, int(rect.y - rect.height / 2.0 - rect.height * padding / 2.0)); + cv::Rect r(x, y, + std::min(img.cols - x, int(rect.width + rect.width * padding)), + std::min(img.rows - y, int(rect.height + rect.height * padding))); + ret = img(r); + } + + void extractImage(const cv::Mat &img, const opencv_apps::Face &face, cv::Mat &ret, double padding = 0.0) { + extractImage(img, face.face, ret, padding); + } + + void retrain() { + NODELET_DEBUG("retrain"); + std::vector images; + std::vector labels; + train(images, labels); + } + + void train(std::vector &images, std::vector &labels) { + size_t new_image_size = images.size(); + + if (use_saved_data_) { + storage_->load(images, labels); + } + + if (images.size() == 0) return; + + std::vector resized_images(images.size()); + for (int i = 0; i < images.size(); ++i) { + cv::resize(images[i], resized_images[i], face_model_size_, + 0, 0, cv::INTER_CUBIC); + cv::cvtColor(resized_images[i], resized_images[i], CV_BGR2GRAY); + } + + label_mapper_->add(labels); + std::vector ids = label_mapper_->get(labels); + NODELET_INFO_STREAM("training with " << images.size() << " images"); + // label_mapper_->debugPrint(); + model_->train(resized_images, ids); + + if (save_train_data_ && new_image_size > 0) { + std::vector new_images(images.begin(), images.begin()+new_image_size); + std::vector new_labels(labels.begin(), labels.begin()+new_image_size); + storage_->save(new_images, new_labels); + } + } + + void predict(const cv::Mat &img, int &label, double &confidence) { + cv::Mat resized_img; + cv::resize(img, resized_img, face_model_size_, 0, 0, cv::INTER_CUBIC); + cv::cvtColor(resized_img, resized_img, CV_BGR2GRAY); + model_->predict(resized_img, label, confidence); + } + + void faceImageCallback(const sensor_msgs::Image::ConstPtr &image, + const opencv_apps::FaceArrayStamped::ConstPtr &faces) { + NODELET_DEBUG("faceImageCallback"); + boost::mutex::scoped_lock lock(mutex_); + + // check if need to draw and publish debug image + bool publish_debug_image = debug_img_pub_.getNumSubscribers() > 0; + + try { + cv::Mat cv_img = cv_bridge::toCvShare(image, enc::BGR8)->image; + opencv_apps::FaceArrayStamped ret_msg = *faces; + for (size_t i = 0; i < faces->faces.size(); ++i) { + cv::Mat face_img, resized_image; + extractImage(cv_img, faces->faces[i], face_img, face_padding_); + + int label_id = -1; + double confidence = 0.0; + predict(face_img, label_id, confidence); + if (label_id == -1) continue; + ret_msg.faces[i].label = label_mapper_->lookup(label_id); + ret_msg.faces[i].confidence = confidence; + + // draw debug image + if (publish_debug_image) drawFace(cv_img, ret_msg.faces[i]); + } + face_pub_.publish(ret_msg); + if (publish_debug_image) { + sensor_msgs::Image::Ptr debug_img + = cv_bridge::CvImage(image->header, enc::BGR8, cv_img).toImageMsg(); + debug_img_pub_.publish(debug_img); + NODELET_DEBUG("Published debug image"); + } + } catch (cv::Exception &e) { + NODELET_ERROR_STREAM("error at image processing: " + << e.err << " " + << e.func << " " + << e.file << " " + << e.line); + } + } + + bool trainCallback(opencv_apps::FaceRecognitionTrain::Request &req, + opencv_apps::FaceRecognitionTrain::Response &res) { + boost::mutex::scoped_lock lock(mutex_); + try { + std::vector images(req.images.size()); + bool use_roi = req.rects.size() == 0 ? false : true; + + if (use_roi && req.images.size() != req.rects.size()) { + throw std::length_error("req.images.size() != req.rects.size()"); + } + + for (size_t i = 0; i < req.images.size(); ++i) { + sensor_msgs::Image img = req.images[i]; + images[i] = cv_bridge::toCvCopy(img, enc::BGR8)->image; + if (use_roi) { + cv::Mat face_img; + extractImage(images[i], req.rects[i], face_img); + images[i] = face_img; + } + } + std::vector labels(req.labels.begin(), req.labels.end()); + train(images, labels); + res.ok = true; + return true; + } catch (cv::Exception &e) { + std::stringstream ss; + ss << "error at training: " + << e.err << " " + << e.func << " " + << e.file << " " + << e.line; + res.ok = false; + res.error = ss.str(); + } + return false; + } + + void configCallback(Config &config, uint32_t level) { + boost::mutex::scoped_lock lock(mutex_); + + bool need_recreate_model = false; + bool need_retrain = false; + + use_saved_data_ = config.use_saved_data; + save_train_data_ = config.save_train_data; + face_padding_ = config.face_padding; + + if (face_model_size_.width != config.face_model_width) { + face_model_size_.width = config.face_model_width; + need_retrain = true; + } + if (face_model_size_.height != config.face_model_height) { + face_model_size_.height = config.face_model_height; + need_retrain = true; + } + + if (data_dir_ != config.data_dir) { + data_dir_ = config.data_dir; + need_retrain = true; + label_mapper_.reset(new LabelMapper()); + storage_.reset(new Storage(fs::path(data_dir_))); + } + + if (config_.model_method != config.model_method) { + need_recreate_model = true; + } + + if (config_.model_num_components != config.model_num_components) { + need_recreate_model = true; + } + + if (config.model_method == "LBPH" && ( + config_.lbph_radius != config.lbph_radius || + config_.lbph_neighbors != config.lbph_neighbors || + config_.lbph_grid_x != config.lbph_grid_x || + config_.lbph_grid_y != config.lbph_grid_y )) { + need_recreate_model = true; + } + + if (need_recreate_model) { + try { + if (config.model_method == "eigen") { + model_ = face::createEigenFaceRecognizer(config.model_num_components, + config.model_threshold); + } else if (config.model_method == "fisher") { + model_ = face::createFisherFaceRecognizer(config.model_num_components, + config.model_threshold); + } else if (config.model_method == "LBPH") { + model_ = face::createLBPHFaceRecognizer(config.lbph_radius, + config.lbph_neighbors, + config.lbph_grid_x, + config.lbph_grid_y); + } + need_retrain = true; + } catch (cv::Exception &e) { + NODELET_ERROR_STREAM("Error: create face recognizer: " << e.what()); + } + } + + if (need_retrain) { + try { + retrain(); + } catch (cv::Exception &e) { + NODELET_ERROR_STREAM("Error: train: " << e.what()); + } + } + + if (config_.model_threshold != config.model_threshold) { + try { +#if CV_MAJOR_VERSION >= 3 + if(face::BasicFaceRecognizer* model = dynamic_cast(model_.get())) { + model->setThreshold(config.model_threshold); + } else if (face::LBPHFaceRecognizer* model = dynamic_cast(model_.get())) { + model->setThreshold(config.model_threshold); + } +#else + model_->set("threshold", config.model_threshold); +#endif + } catch (cv::Exception &e) { + NODELET_ERROR_STREAM("Error: set threshold: " << e.what()); + } + } + config_ = config; + } + + void subscribe() { + NODELET_DEBUG("subscribe"); + img_sub_.subscribe(*it_, "image", 1); + face_sub_.subscribe(*nh_, "faces", 1); + if (use_async_) { + async_ = boost::make_shared >(queue_size_); + async_->connectInput(img_sub_, face_sub_); + async_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); + } else { + sync_ = boost::make_shared >(queue_size_); + sync_->connectInput(img_sub_, face_sub_); + sync_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); + } + } + + void unsubscribe() { + NODELET_DEBUG("unsubscribe"); + img_sub_.unsubscribe(); + face_sub_.unsubscribe(); + } + + public: + virtual void onInit() { + Nodelet::onInit(); + + // variables + face_model_size_ = cv::Size(190, 90); + + // dynamic reconfigures + cfg_srv_ = boost::make_shared(*pnh_); + Server::CallbackType f = boost::bind(&FaceRecognitionNodelet::configCallback, this, _1, _2); + cfg_srv_->setCallback(f); + + // parameters + pnh_->param("approximate_sync", use_async_, false); + pnh_->param("queue_size", queue_size_, 100); + + // advertise + debug_img_pub_ = advertise(*pnh_, "debug_image", 1); + face_pub_ = advertise(*pnh_, "output", 1); + train_srv_ = pnh_->advertiseService("train", + &FaceRecognitionNodelet::trainCallback, this); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + + onInitPostProcess(); + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS(face_recognition::FaceRecognitionNodelet, nodelet::Nodelet); diff --git a/srv/FaceRecognitionTrain.srv b/srv/FaceRecognitionTrain.srv new file mode 100644 index 00000000..6c4c7882 --- /dev/null +++ b/srv/FaceRecognitionTrain.srv @@ -0,0 +1,6 @@ +sensor_msgs/Image[] images +Rect[] rects +string[] labels +--- +bool ok +string error diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 4d39cf59..8d0e7b23 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -14,6 +14,15 @@ add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag add_custom_target(vslam_tutorial_bag DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag) add_dependencies(tests vslam_tutorial_bag) +# test data for face recognition +add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/face_data + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/face_data.tar.gz + COMMAND ${CMAKE_COMMAND} -E tar zxf face_data.tar.gz + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Extracting face_data.tar.gz") +add_custom_target(face_data DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/face_data) +add_dependencies(tests face_data) + #add_rostest(test-apps.test) add_rostest(test-adding_images.test ARGS gui:=false) add_rostest(test-discrete_fourier_transform.test ARGS gui:=false) @@ -33,11 +42,14 @@ add_rostest(test-contour_moments.test ARGS gui:=false) if(cv_bridge_VERSION VERSION_LESS "1.11.9") # hydro skip face_detection.test elseif(OpenCV_VERSION VERSION_LESS "3.0") add_rostest(test-face_detection.test ARGS gui:=false) + add_rostest(test-face_recognition.test ARGS gui:=false) else() if(OpenCV_VERSION VERSION_LESS "3.2") add_rostest(test-face_detection.test ARGS gui:=false use_opencv3:=true use_opencv3_1:=true) + add_rostest(test-face_recognition.test ARGS gui:=false use_opencv3:=true use_opencv3_1:=true) else() add_rostest(test-face_detection.test ARGS gui:=false use_opencv3:=true use_opencv3_2:=true) + add_rostest(test-face_recognition.test ARGS gui:=false use_opencv3:=true use_opencv3_2:=true) endif() endif() add_rostest(test-goodfeature_track.test ARGS gui:=false) diff --git a/test/face_data.tar.gz b/test/face_data.tar.gz new file mode 100644 index 00000000..d1a96f0b Binary files /dev/null and b/test/face_data.tar.gz differ diff --git a/test/test-face_recognition.test b/test/test-face_recognition.test new file mode 100644 index 00000000..fd8a1b21 --- /dev/null +++ b/test/test-face_recognition.test @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +