AreaParameterChooserWorker.cpp
Go to the documentation of this file.
00001 #include "face_contour_detector/AreaParameterChooserWorker.h"
00002 
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 #include <face_contour_detector/filters/List.h>
00006 #include <face_contour_detector/filters/GaussianBlur.h>
00007 #include <face_contour_detector/filters/Canny.h>
00008 #include <ros/assert.h>
00009 
00010 namespace face_contour_detector {
00011 
00012 //AreaParameterChooserWorker
00013 
00014 AreaParameterChooserWorker::AreaParameterChooserWorker(
00015                 ros::NodeHandle& node,
00016                 cv::Mat& image,
00017                 cv::Mat& mask,
00018                 const std::map<std::string, std::map<std::string, double> >& targetValue) :
00019                 node(node), image(image), mask(mask), targetValue(targetValue), kdTree(
00020                                 3) {
00021         cv::cvtColor(image, bwimage, CV_BGR2GRAY);
00022 }
00023 
00024 void AreaParameterChooserWorker::GetAreas() {
00025         ros::ServiceClient client = node.serviceClient<
00026                         face_contour_detector::FindFaceAreas>("find_face_areas");
00027         if (!client.exists()) {
00028                 ROS_ERROR("the service find_face_areas does not exist");
00029                 return;
00030         }
00031         face_contour_detector::FindFaceAreas srv;
00032         //Putting the result image into the response
00033         cv::Mat tmp = mask.clone();
00034         try {
00035                 sensor_msgs::CvBridge cvBridge;
00036                 IplImage ipl = tmp;
00037                 srv.request.mask = *(cvBridge.cvToImgMsg(&ipl));
00038         } catch (sensor_msgs::CvBridgeException& error) {
00039                 ROS_ERROR(
00040                                 "could not convert the mask image with the cvBridge to a sensor_msgs/Image");
00041                 return;
00042         }
00043         if (client.call(srv)) {
00044                 if (targetValue.find(srv.response.complete_face.name)
00045                                 != targetValue.end()) {
00046                         areas.push_back(srv.response.complete_face);
00047                 }
00048                 if (targetValue.find(srv.response.nose.name) != targetValue.end()) {
00049                         areas.push_back(srv.response.nose);
00050                 }
00051                 if (targetValue.find(srv.response.left_eye.name) != targetValue.end()) {
00052                         areas.push_back(srv.response.left_eye);
00053                 }
00054                 if (targetValue.find(srv.response.right_eye.name)
00055                                 != targetValue.end()) {
00056                         areas.push_back(srv.response.right_eye);
00057                 }
00058                 if (targetValue.find(srv.response.mouth.name) != targetValue.end()) {
00059                         areas.push_back(srv.response.mouth);
00060                 }
00061 
00062                 //TAG Debug
00063                 //ROS_INFO("Printing areas");
00064                 std::vector<face_contour_detector::image_area>::iterator it;
00065                 for (it = areas.begin(); it != areas.end(); it++) {
00066                         ROS_ASSERT(image.rows >= it->y + it->height);
00067                         ROS_ASSERT(image.cols >= it->x + it->width);
00068                         /*std::string name = std::string(it->name);
00069                          ROS_INFO("area %s x= %i , y= %i , width= %i , height= %i ",name.c_str(), it->x, it->y, it->width, it->height);*/
00070                 }
00071 
00072         } else {
00073                 ROS_ERROR("Failed to call find_face_areas service");
00074         }
00075 }
00076 void AreaParameterChooserWorker::ParamsFunction(
00077                 const std::vector<double>& params,
00078                 std::map<std::string, double>& result) {
00079 
00080         //Load from cache if possible
00081         const std::map<std::string, double>* cacheResult = kdTree.Find(params);
00082         if (cacheResult != 0) {
00083                 //ROS_INFO("found cache entry");
00084                 result = *cacheResult;
00085                 return;
00086         }
00087 
00088         //callculate
00089         std::vector<face_contour_detector::filter_area_result> re;
00090 
00091         ApplyFilters(params, re);
00092 
00093         std::vector<face_contour_detector::filter_area_result>::iterator it;
00094         for (it = re.begin(); it != re.end(); it++) {
00095                 if (targetValue.find(it->area.name) == targetValue.end()) {
00096                         ROS_ERROR(
00097                                         "could not find target values for area %s", it->area.name.c_str());
00098                         return;
00099                 }
00100                 if (currArea->name == it->area.name) {
00101 
00102                         std::map<std::string, double>::iterator it2;
00103                         for (it2 = targetValue[currArea->name].begin();
00104                                         it2 != targetValue[currArea->name].end(); it2++) {
00105 
00106                                 std::vector<face_contour_detector::name_value_pair>::iterator it3;
00107                                 for (it3 = it->result_properties.begin();
00108                                                 it3 != it->result_properties.end(); it3++) {
00109                                         if (it3->name == it2->first) {
00110                                                 result[it3->name] = atof(it3->value.c_str());
00111                                         }
00112                                 }
00113                                 if (result.find(it2->first) == result.end()) {
00114                                         ROS_ERROR(
00115                                                         "Could not find needed target value %s in response for area %s", it2->first.c_str(), it->area.name.c_str());
00116                                         SendStatusMessage(
00117                                                         "Could not find needed target value in response for the area, please send a better image");
00118                                 }
00119                         }
00120 
00121                         break;
00122                 }
00123         }
00124 
00125         //save to cache
00126         kdTree.Insert(params, result);
00127 }
00128 double AreaParameterChooserWorker::CostFunction(
00129                 const std::map<std::string, double>& result
00130                 , const std::map<std::string, double>& targetValue) {
00131         double re = 0.0;
00132         std::map<std::string, double>::const_iterator it;
00133         for (it = result.begin(); it != result.end(); it++) {
00134                 std::string tmp = it->first;
00135                 double tar = targetValue.find(tmp)->second;
00136                 double seen = it->second;
00137                 re += std::abs<double>(tar - seen);
00138         }
00139         return re;
00140 }
00141 
00142 void AreaParameterChooserWorker::PushFilterSetup(
00143                 std::map<std::string, std::vector<std::map<std::string, double> > >& params
00144                 , int numResults
00145                 , std::vector<face_contour_detector::autoselect_result>& results) {
00146 
00147         for (int i = 0; i < numResults; i++) {
00148 
00149                 face_contour_detector::autoselect_result re;
00150 
00151                 //load image
00152                 cv::Mat img = image.clone();
00153 
00154                 try {
00155                         sensor_msgs::CvBridge cvBridge;
00156                         IplImage ipl = img;
00157                         re.image = *(cvBridge.cvToImgMsg(&ipl));
00158                 } catch (sensor_msgs::CvBridgeException& error) {
00159                         ROS_ERROR(
00160                                         "could not convert the image with the cvBridge to a sensor_msgs/Image");
00161                         return;
00162                 }
00163 
00164                 std::vector<face_contour_detector::image_area>::iterator it;
00165                 for (it = areas.begin(); it != areas.end(); it++) {
00166 
00167                         face_contour_detector::filter_area_setup areaSetup;
00168 
00169                         areaSetup.area = *it;
00170 
00171                         //blur
00172                         {
00173                                 face_contour_detector::filter_setup filterSetup;
00174                                 filterSetup.name = "GaussianBlur";
00175                                 {
00176                                         ROS_ASSERT(
00177                                                         params.find(areaSetup.area.name) != params.end());
00178                                         ROS_ASSERT(params[areaSetup.area.name].size() > static_cast<unsigned int>(i));
00179                                         ROS_ASSERT(
00180                                                         params[areaSetup.area.name][i].find("blur") != params[areaSetup.area.name][i].end());
00181                                         std::stringstream s;
00182                                         s << (int(params[areaSetup.area.name][i]["blur"]));
00183                                         face_contour_detector::name_value_pair param;
00184                                         param.name = "blurwidth";
00185                                         param.type = "int";
00186                                         param.value = s.str().c_str();
00187                                         //ROS_INFO("blurwidth = %s", s.str().c_str());
00188                                         filterSetup.parameters.push_back(param);
00189                                 }
00190                                 {
00191                                         std::stringstream s;
00192                                         s << (int(params[areaSetup.area.name][i]["blur"]));
00193                                         face_contour_detector::name_value_pair param;
00194                                         param.name = "blurheight";
00195                                         param.type = "int";
00196                                         param.value = s.str().c_str();
00197                                         //ROS_INFO("blurheight = %s", s.str().c_str());
00198                                         filterSetup.parameters.push_back(param);
00199                                 }
00200                                 areaSetup.filters.push_back(filterSetup);
00201                         }
00202                         //canny
00203                         {
00204                                 face_contour_detector::filter_setup filterSetup;
00205                                 filterSetup.name = "Canny";
00206                                 {
00207                                         std::stringstream s;
00208                                         s << (int(params[areaSetup.area.name][i]["threshold1"]));
00209                                         face_contour_detector::name_value_pair param;
00210                                         param.name = "threshold1";
00211                                         param.type = "float";
00212                                         param.value = s.str().c_str();
00213                                         filterSetup.parameters.push_back(param);
00214                                 }
00215                                 {
00216                                         std::stringstream s;
00217                                         s << (int(params[areaSetup.area.name][i]["threshold2"]));
00218                                         face_contour_detector::name_value_pair param;
00219                                         param.name = "threshold2";
00220                                         param.type = "float";
00221                                         param.value = s.str().c_str();
00222                                         filterSetup.parameters.push_back(param);
00223                                 }
00224                                 {
00225                                         face_contour_detector::name_value_pair param;
00226                                         param.name = "l2gradientent";
00227                                         param.type = "bool";
00228                                         param.value = "true";
00229                                         filterSetup.parameters.push_back(param);
00230                                 }
00231                                 areaSetup.filters.push_back(filterSetup);
00232                         }
00233 
00234                         //edge connector
00235                         {
00236                                 face_contour_detector::filter_setup filterSetup;
00237                                 filterSetup.name = "EdgeConnector";
00238                                 {
00239                                         face_contour_detector::name_value_pair param;
00240                                         param.name = "searchRadius";
00241                                         param.type = "int";
00242                                         param.value = "5";
00243                                         filterSetup.parameters.push_back(param);
00244                                 }
00245                                 areaSetup.filters.push_back(filterSetup);
00246                         }
00247                         //delete short lines
00248                         {
00249                                 face_contour_detector::filter_setup filterSetup;
00250                                 filterSetup.name = "DeleteShortLines";
00251                                 {
00252                                         face_contour_detector::name_value_pair param;
00253                                         param.name = "minNumPixels";
00254                                         param.type = "int";
00255                                         param.value = "10";
00256                                         filterSetup.parameters.push_back(param);
00257                                 }
00258                                 areaSetup.filters.push_back(filterSetup);
00259                         }
00260 
00261                         re.areas.push_back(areaSetup);
00262                 }
00263 
00264                 results.push_back(re);
00265         }
00266 }
00267 
00268 void AreaParameterChooserWorker::ApplyFilters(const std::vector<double>& params,
00269                 std::vector<face_contour_detector::filter_area_result>& results) {
00270 
00271         cv::Mat areaImage = cv::Mat(currArea->height, currArea->width, CV_8UC1);
00272 
00273         for (int y = 0; y < currArea->height; y++) {
00274                 for (int x = 0; x < currArea->width; x++) {
00275                         ROS_ASSERT(bwimage.cols > x);
00276                         ROS_ASSERT(bwimage.rows > y);
00277                         areaImage.at<unsigned char>(y, x) = bwimage.at<unsigned char>(
00278                                         currArea->y + y, currArea->x + x);
00279                 }
00280         }
00281 
00282         cv::Mat currAreaImage = cv::Mat(currArea->height, currArea->width, CV_8UC1);
00283         face_contour_detector::filters::List list;
00284         {
00285                 face_contour_detector::filters::Filter* currFilter =
00286                                 new face_contour_detector::filters::GaussianBlur();
00287                 std::vector<face_contour_detector::filters::Parameter> filterParams =
00288                                 currFilter->GetParameters();
00289                 std::vector<face_contour_detector::filters::Parameter>::iterator it;
00290                 for (it = filterParams.begin(); it != filterParams.end(); it++) {
00291                         *(it->GetIntValuePtr()) = int(params[2]);
00292                 }
00293                 list.Add(currFilter);
00294         }
00295         {
00296                 face_contour_detector::filters::Filter* currFilter =
00297                                 new face_contour_detector::filters::Canny();
00298                 std::vector<face_contour_detector::filters::Parameter> filterParams =
00299                                 currFilter->GetParameters();
00300                 std::vector<face_contour_detector::filters::Parameter>::iterator it;
00301                 for (it = filterParams.begin(); it != filterParams.end(); it++) {
00302                         if (it->GetName() == std::string("threshold1")) {
00303                                 *(it->GetDoubleValuePtr()) = params[0];
00304                         } else if (it->GetName() == std::string("threshold2")) {
00305                                 *(it->GetDoubleValuePtr()) = params[1];
00306                         } else if (it->GetName() == std::string("l2gradientent")) {
00307                                 *(it->GetBoolValuePtr()) = true;
00308                         }
00309                 }
00310                 list.Add(currFilter);
00311         }
00312         cv::Mat result = cv::Mat(currArea->height, currArea->width, CV_8UC1);
00313         list.Apply(areaImage, result);
00314 
00315         //callculating black to white pixel ratio
00316         double numWhite = 0.0;
00317         double numBlack = 0.0;
00318         double whiteFactor = double(image.rows) / 100.0
00319                         + double(image.cols) / 100.0;
00320         for (int y = 0; y < result.rows; y++) {
00321                 for (int x = 0; x < result.cols; x++) {
00322                         if (result.at<unsigned char>(y, x) > 150) {
00323                                 //numWhite += 1.0;
00324                                 numWhite += whiteFactor;
00325                         } else {
00326                                 numBlack += 1.0;
00327                         }
00328                 }
00329         }
00330         //excluding pixels that will be overwritten by later areas
00331         {
00332                 std::vector<face_contour_detector::image_area>::iterator areaIt2;
00333                 bool following = false;
00334                 for (areaIt2 = areas.begin(); areaIt2 != areas.end(); areaIt2++) {
00335                         if (following) {
00336                                 for (int y = 0; y < areaIt2->height; y++) {
00337                                         for (int x = 0; x < areaIt2->width; x++) {
00338                                                 int realX = areaIt2->x + x - currArea->x;
00339                                                 int realY = areaIt2->y + y - currArea->y;
00340                                                 if (realX >= 0 && realX < currArea->width && realY >= 0
00341                                                                 && realY < currArea->height) {
00342                                                         ROS_ASSERT(
00343                                                                         result.rows > realY && result.cols > realX);
00344                                                         if (result.at<unsigned char>(realY, realX) > 150) {
00345                                                                 numWhite -= 1.0;
00346                                                         } else {
00347                                                                 numBlack -= 1.0;
00348                                                         }
00349                                                 }
00350                                         }
00351                                 }
00352                         } else if (areaIt2 == currArea) {
00353                                 following = true;
00354                         }
00355                 }
00356         }
00357         //writing out result
00358         {
00359                 face_contour_detector::filter_area_result areaResult;
00360                 areaResult.area = *currArea;
00361                 face_contour_detector::name_value_pair nv;
00362                 nv.name = "graphmass";
00363                 std::stringstream sstr;
00364                 sstr << (numWhite / numBlack);
00365                 nv.value = sstr.str().c_str();
00366                 nv.type = "float";
00367                 areaResult.result_properties.push_back(nv);
00368                 results.push_back(areaResult);
00369         }
00370         /*face_contour_detector::ApplyFilters req;
00371          std::vector<face_contour_detector::image_area>::iterator it;
00372          for (it = areas.begin(); it != areas.end(); it++) {
00373 
00374          face_contour_detector::filter_area_setup areaSetup;
00375 
00376          areaSetup.area = *it;
00377 
00378          //blur
00379          {
00380          face_contour_detector::filter_setup filterSetup;
00381          filterSetup.name = "GaussianBlur";
00382          {
00383          std::stringstream s;
00384          s<<(int(params[2]));
00385          face_contour_detector::name_value_pair param;
00386          param.name = "blurwidth";
00387          param.type = "int";
00388          param.value = s.str().c_str();
00389          filterSetup.parameters.push_back(param);
00390          }
00391          {
00392          std::stringstream s;
00393          s<<(int(params[2]));
00394          face_contour_detector::name_value_pair param;
00395          param.name = "blurheight";
00396          param.type = "int";
00397          param.value = s.str().c_str();
00398          filterSetup.parameters.push_back(param);
00399          }
00400          areaSetup.filters.push_back(filterSetup);
00401          }
00402          //canny
00403          {
00404          face_contour_detector::filter_setup filterSetup;
00405          filterSetup.name = "Canny";
00406          {
00407          std::stringstream s;
00408          s<<(int(params[0]));
00409          face_contour_detector::name_value_pair param;
00410          param.name = "threshold1";
00411          param.type = "float";
00412          param.value = s.str().c_str();
00413          filterSetup.parameters.push_back(param);
00414          }
00415          {
00416          std::stringstream s;
00417          s<<(int(params[1]));
00418          face_contour_detector::name_value_pair param;
00419          param.name = "threshold2";
00420          param.type = "float";
00421          param.value = s.str().c_str();
00422          filterSetup.parameters.push_back(param);
00423          }
00424          {
00425          face_contour_detector::name_value_pair param;
00426          param.name = "l2gradientent";
00427          param.type = "bool";
00428          param.value = "true";
00429          filterSetup.parameters.push_back(param);
00430          }
00431          areaSetup.filters.push_back(filterSetup);
00432          }
00433 
00434          //edge connector
00435          {
00436          face_contour_detector::filter_setup filterSetup;
00437          filterSetup.name = "EdgeConnector";
00438          {
00439          face_contour_detector::name_value_pair param;
00440          param.name = "searchRadius";
00441          param.type = "int";
00442          param.value = "3";
00443          filterSetup.parameters.push_back(param);
00444          }
00445          areaSetup.filters.push_back(filterSetup);
00446          }
00447          //delete short lines
00448          {
00449          face_contour_detector::filter_setup filterSetup;
00450          filterSetup.name = "DeleteShortLines";
00451          {
00452          face_contour_detector::name_value_pair param;
00453          param.name = "minNumPixels";
00454          param.type = "int";
00455          param.value = "5";
00456          filterSetup.parameters.push_back(param);
00457          }
00458          areaSetup.filters.push_back(filterSetup);
00459          }
00460 
00461          req.request.areas.push_back(areaSetup);
00462          }
00463 
00464          //Putting the mask image into the request
00465          cv::Mat tmp = image.clone();
00466          try {
00467          sensor_msgs::CvBridge cvBridge;
00468          IplImage ipl = tmp;
00469          req.request.input_image = *(cvBridge.cvToImgMsg(&ipl));
00470 
00471          ros::ServiceClient applyClient = node.serviceClient<face_contour_detector::ApplyFilters>("apply_filters");
00472 
00473          if (applyClient.call(req)) {
00474          results = req.response.area_results;
00475          } else {
00476          ROS_ERROR("Failed to call service apply_filters");
00477          return;
00478          }
00479          } catch (sensor_msgs::CvBridgeException error) {
00480          ROS_ERROR("could not convert the mask image with the cvBridge to a sensor_msgs/Image");
00481          return;
00482          }*/
00483 }
00484 
00485 void AreaParameterChooserWorker::SendStatusMessage(const std::string& message) {
00486         ros::ServiceClient client = node.serviceClient<
00487                         portrait_robot_msgs::alubsc_status_msg>("status_msg");
00488         portrait_robot_msgs::alubsc_status_msg srv;
00489         srv.request.ID = 2;
00490         srv.request.message = message;
00491         if (!client.call(srv)) {
00492                 ROS_WARN(
00493                                 "could not reach status message service \"%s\"", client.getService().c_str());
00494                 ROS_INFO("%s", message.c_str());
00495         }
00496 }
00497 
00498 void AreaParameterChooserWorker::CallculateParameters(int numResults, int steps,
00499                 std::vector<face_contour_detector::autoselect_result>& result) {
00500         //clear the kd tree
00501 
00502         //get the areas
00503         GetAreas();
00504         if (areas.size() <= 0) {
00505                 ROS_ERROR("No areas found");
00506                 SendStatusMessage(
00507                                 "Could not find any areas, please send a better mask file");
00508                 return;
00509         }
00510 
00511         //start the explorer for the different areas
00512         boost::function<
00513                         void(const std::vector<double>&, std::map<std::string, double>&)> paramsFunction =
00514                         boost::bind(&AreaParameterChooserWorker::ParamsFunction, this, _1,
00515                                         _2);
00516 
00517         boost::function<
00518                         double(const std::map<std::string, double>&
00519                                         , const std::map<std::string, double>&)> costsFunction =
00520                         boost::bind(&AreaParameterChooserWorker::CostFunction, this, _1,
00521                                         _2);
00522 
00523         std::map<std::string, std::vector<std::map<std::string, double> > > areaResults;
00524 
00525         for (currArea = areas.begin(); currArea != areas.end(); currArea++) {
00526                 std::stringstream sstr;
00527                 sstr << "exploring area \"" << currArea->name << "\" ...";
00528                 SendStatusMessage(sstr.str());
00529 
00530                 //new kd tree cache
00531                 kdTree = face_contour_detector::KdTree<double,
00532                                 std::map<std::string, double> >(3);
00533 
00534                 std::map<std::string, double> targetValue =
00535                                 this->targetValue[currArea->name];
00536                 std::vector<std::map<std::string, double> > results;
00537                 AutoParameterExplorer explorer = AutoParameterExplorer(paramsFunction,
00538                                 costsFunction);
00539                 explorer.AddParameter("threshold1", 0, 30000);
00540                 explorer.AddParameter("threshold2", 0, 30000);
00541                 explorer.AddParameter("blur", 15, 50);
00542                 ROS_INFO("Callculating area \"%s\"", currArea->name.c_str());
00543                 explorer.FindBest(targetValue, steps, numResults, results);
00544                 if (results.size() < static_cast<unsigned int>(numResults)) {
00545                         ROS_ERROR(
00546                                         "Could not find enough results for this image in the area %s", currArea->name.c_str());
00547                         std::stringstream sstr;
00548                         sstr << "Could not find enough results for this image in the area"
00549                                         << currArea->name << std::endl;
00550                         SendStatusMessage(sstr.str());
00551                         return;
00552                 }
00553                 areaResults[currArea->name] = results;
00554         }
00555 
00556         //convert to autparameter results
00557         PushFilterSetup(areaResults, numResults, result);
00558 }
00559 
00560 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


face_contour_detector
Author(s): Fabian Wenzelmann and Julian Schmid
autogenerated on Wed Dec 26 2012 16:18:17