Go to the documentation of this file.00001 #include <ros/ros.h>
00002
00003 #include <face_contour_detector/filters/List.h>
00004 #include <face_contour_detector/filters/GaussianBlur.h>
00005 #include <face_contour_detector/filters/Canny.h>
00006 #include <face_contour_detector/filters/EdgeConnectorGraphBased.h>
00007 #include <face_contour_detector/filters/DeleteShortLines.h>
00008 #include <face_contour_detector/ImageForest.h>
00009
00010 #include <face_contour_detector/ParameterIterator.h>
00011 #include <face_contour_detector/MultiParameterIterator.h>
00012
00013 #include <face_contour_detector/XMLWriter.h>
00014 #include <face_contour_detector/XMLReader.h>
00015 #include <face_contour_detector/ImageInformationBridge.h>
00016
00017 #include <face_contour_detector/xml_image.h>
00018 #include <face_contour_detector/xml_image_entry.h>
00019
00020 #include <boost/shared_ptr.hpp>
00021
00022 #include <opencv/cv.h>
00023 #include <sensor_msgs/Image.h>
00024 #include <cv_bridge/CvBridge.h>
00025
00026 #include <string>
00027 #include <map>
00028 #include <utility>
00029 #include <sstream>
00030
00031 face_contour_detector::filters::Parameter GetParameter(const std::string& name, face_contour_detector::filters::Filter& filter) {
00032 std::vector<face_contour_detector::filters::Parameter> parameters = filter.GetParameters();
00033 std::vector<face_contour_detector::filters::Parameter>::iterator it;
00034 for (it = parameters.begin(); it != parameters.end(); it++) {
00035 if (it->GetName() == name) {
00036 return *it;
00037 }
00038 }
00039 std::cout<<"could not find parameter "<<name<<std::endl;
00040 throw "error";
00041 }
00042
00043 void GenerateFilterParameterMessageEntries(face_contour_detector::filters::List& filterList, face_contour_detector::xml_image::_entries_type& entries) {
00044 face_contour_detector::filters::List::Iterator it;
00045 std::stringstream filterSetupStream;
00046 int i = 0;
00047 for (it = filterList.Begin(); it != filterList.End(); it++) {
00048 filterSetupStream<<(*it)->GetFilterName()<<",";
00049 std::vector<face_contour_detector::filters::Parameter> params = (*it)->GetParameters();
00050 std::vector<face_contour_detector::filters::Parameter>::iterator it2;
00051 for (it2 = params.begin(); it2 != params.end(); it2++) {
00052 face_contour_detector::xml_image_entry paramEntry;
00053 std::stringstream paramName;
00054 paramName<<"filter"<<i<<"_"<<it2->GetName();
00055 paramEntry.entry_name = paramName.str();
00056 std::stringstream paramValue;
00057 if (it2->GetType() == face_contour_detector::filters::TBOOL) {
00058 paramEntry.type = "bool";
00059 paramValue<<(*(it2->GetBoolValuePtr()));
00060 break;
00061 } else if (it2->GetType() == face_contour_detector::filters::TDOUBLE) {
00062 paramEntry.type = "float";
00063 paramValue<<(*(it2->GetDoubleValuePtr()));
00064 } else if (it2->GetType() == face_contour_detector::filters::TINT) {
00065 paramEntry.type = "int";
00066 paramValue<<(*(it2->GetIntValuePtr()));
00067 } else if (it2->GetType() == face_contour_detector::filters::TSTRING) {
00068 paramEntry.type = "string";
00069 paramValue<<(*(it2->GetStringValuePtr()));
00070 } else {
00071 throw "unknown type";
00072 }
00073 paramEntry.content = paramValue.str();
00074 entries.push_back(paramEntry);
00075 }
00076 i++;
00077 }
00078 face_contour_detector::xml_image_entry filterSetupEntry;
00079 filterSetupEntry.entry_name = "filtersetup";
00080 filterSetupEntry.type = "string";
00081 filterSetupEntry.content = filterSetupStream.str();
00082 entries.push_back(filterSetupEntry);
00083
00084 }
00085
00086 void PushParams(std::vector<face_contour_detector::xml_image_entry>& toCopy, std::vector<face_contour_detector::xml_image_entry>& copyTarget) {
00087 std::vector<face_contour_detector::xml_image_entry>::iterator it;
00088 for (it = toCopy.begin(); it != toCopy.end(); it++) {
00089 face_contour_detector::xml_image_entry entry;
00090 entry.entry_name = it->entry_name;
00091 entry.type = it->type;
00092 entry.content = it->content;
00093 copyTarget.push_back(entry);
00094 }
00095 }
00096
00097 void PushEndParams(cv::Mat& result, std::vector<face_contour_detector::xml_image_entry>& target) {
00098
00099 face_contour_detector::ImageForest forest(result, 200);
00100 std::map<int, face_contour_detector::ImageForest::Graph> graphs = forest.GetGraphs();
00101
00102 face_contour_detector::xml_image_entry entry;
00103 std::stringstream valuestream;
00104
00105
00106 {
00107 face_contour_detector::xml_image_entry entry;
00108 std::stringstream valuestream;
00109 entry.type = "int";
00110 entry.entry_name = "result_numgraphs";
00111 valuestream<<graphs.size();
00112 entry.content = valuestream.str().c_str();
00113 target.push_back(entry);
00114 }
00115
00116
00117 {
00118 face_contour_detector::xml_image_entry entry;
00119 std::stringstream valuestream;
00120 int count = 0;
00121 std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
00122 for (it = graphs.begin(); it != graphs.end(); it++) {
00123 count += it->second.connectingPoints.size();
00124 }
00125 entry.type = "int";
00126 entry.entry_name = "result_numconnectingpoints";
00127 valuestream<<count;
00128 entry.content = valuestream.str().c_str();
00129 target.push_back(entry);
00130 }
00131
00132
00133 {
00134 face_contour_detector::xml_image_entry entry;
00135 std::stringstream valuestream;
00136 int count = 0;
00137 std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
00138 for (it = graphs.begin(); it != graphs.end(); it++) {
00139 count += it->second.endPoints.size();
00140 }
00141 entry.type = "int";
00142 entry.entry_name = "result_numendpoints";
00143 valuestream<<count;
00144 entry.content = valuestream.str().c_str();
00145 target.push_back(entry);
00146 }
00147
00148
00149 {
00150 face_contour_detector::xml_image_entry entry;
00151 std::stringstream valuestream;
00152 entry.type = "float";
00153 entry.entry_name = "result_edgyness";
00154 valuestream<<forest.CallculateEdgyness();
00155 entry.content = valuestream.str().c_str();
00156 target.push_back(entry);
00157 }
00158
00159
00160 {
00161 face_contour_detector::xml_image_entry entry;
00162 std::stringstream valuestream;
00163 entry.type = "float";
00164 entry.entry_name = "result_graphmass";
00165 double graphMass = forest.CallculateGraphMass();
00166 valuestream<<graphMass;
00167 entry.content = valuestream.str().c_str();
00168 target.push_back(entry);
00169 }
00170
00171 }
00172
00173 int main(int argc, char* argv[]) {
00174
00175
00176 ros::init(argc, argv, "filter_parameter_sampler");
00177 ros::NodeHandle n;
00178
00179
00180 std::string filename;
00181 if (!n.getParam("/image", filename)) {
00182 ROS_ERROR("image has not been set! Typical command-line usage:\n"
00183 "rosparam set /image '<filename>'");
00184 return 1;
00185 }
00186
00187 std::string xmlFilename;
00188
00189 if (!n.getParam("xmlfile", xmlFilename)) {
00190 ROS_ERROR("xmlfile has not been set! Typical command-line usage:\n"
00191 "rosparam set /xmlfile '<filename>'");
00192 return 1;
00193 }
00194
00195 ros::Publisher chatter_pub = n.advertise<sensor_msgs::Image>("images", 5);
00196 sensor_msgs::CvBridge cvbridge;
00197
00198
00199 cv::Mat image = cv::imread(filename);
00200
00201
00202 std::vector<face_contour_detector::xml_image_entry> imageProperties;
00203 ros::ServiceClient imageInformationClient = n.serviceClient<face_contour_detector::ImageInformationBridge>("image_information");
00204 face_contour_detector::ImageInformationBridge imageInformationSrv;
00205 IplImage ipl = image;
00206 imageInformationSrv.request.input_image = *cvbridge.cvToImgMsg(&ipl);
00207 ROS_INFO("Triying to get properties of the image from the image_information service");
00208 if (imageInformationClient.call(imageInformationSrv)) {
00209
00210 imageProperties = imageInformationSrv.response.image.entries;
00211 ROS_INFO("Successfully loaded image information");
00212 } else {
00213 ROS_ERROR("Failed to call service image_information");
00214 return 1;
00215 }
00216
00217
00218 cv::Mat grayImage;
00219 cv::cvtColor(image, grayImage, CV_BGR2GRAY);
00220
00221
00222 face_contour_detector::filters::List list;
00223 face_contour_detector::filters::GaussianBlur* gaussBlur = new face_contour_detector::filters::GaussianBlur();
00224 list.Add(gaussBlur);
00225 face_contour_detector::filters::Canny* canny = new face_contour_detector::filters::Canny();
00226 list.Add(canny);
00227 list.Add(new face_contour_detector::filters::EdgeConnectorGraphBased());
00228 list.Add(new face_contour_detector::filters::DeleteShortLines());
00229
00230
00231 face_contour_detector::MultiParameterIterator mp;
00232
00233 face_contour_detector::filters::Parameter gaussBlurWidth = GetParameter("blurwidth", *gaussBlur);
00234 face_contour_detector::filters::Parameter gaussBlurHeight = GetParameter("blurheight", *gaussBlur);
00235 face_contour_detector::filters::Parameter cannyTh1 = GetParameter("threshold1", *canny);
00236 face_contour_detector::filters::Parameter cannyTh2 = GetParameter("threshold2", *canny);
00237
00238 face_contour_detector::ParallelParameterIterator<int>* blurIt = new face_contour_detector::ParallelParameterIterator<int>(3, 61, 2);
00239 blurIt->Add(gaussBlurWidth.GetIntValuePtr());
00240 blurIt->Add(gaussBlurHeight.GetIntValuePtr());
00243 face_contour_detector::ParameterIterator<double>* cannyTh1It = new face_contour_detector::ParameterIterator<double>(400.0, 20000.0, 200.0, cannyTh1.GetDoubleValuePtr());
00244 face_contour_detector::ParameterIterator<double>* cannyTh2It = new face_contour_detector::ParameterIterator<double>(400.0, 20000.0, 200.0, cannyTh2.GetDoubleValuePtr());
00245
00246 mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(blurIt));
00247 mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(cannyTh1It));
00248 mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(cannyTh2It));
00249
00250 mp.Reset();
00251
00252
00253
00254
00255 ros::ServiceClient readerClient = n.serviceClient<face_contour_detector::XMLReader>("xml_file_bridge_loader");
00256 std::vector<face_contour_detector::xml_image> imagesdata;
00257 face_contour_detector::XMLReader readerSrv;
00258 readerSrv.request.file_name = xmlFilename;
00259 ROS_INFO("Triying to load xml data");
00260 if (readerClient.call(readerSrv)) {
00261 if (!readerSrv.response.success) {
00262 ROS_ERROR("Service failed to load the data");
00263 } else {
00264 imagesdata = readerSrv.response.images;
00265 ROS_INFO("Successfully loaded old xml file");
00266 }
00267 } else {
00268 ROS_ERROR("Failed to call service XMLReader");
00269 return 1;
00270 }
00271
00272
00273
00274 std::cout<<"Starting sampeling"<<std::endl;
00275
00276 while (mp.HasNext() && ros::ok()) {
00277 std::cout<<"\r"<<"["<<(*gaussBlurWidth.GetIntValuePtr())<<", "<<(*cannyTh1.GetDoubleValuePtr())<<", "<<(*cannyTh2.GetDoubleValuePtr())<<"] "<<std::flush;
00278 mp.Next();
00279 cv::Mat result;
00280
00281 list.Apply(grayImage, result);
00282
00283 face_contour_detector::xml_image dataMsg;
00284
00285
00286 face_contour_detector::xml_image_entry filenameEntry;
00287 filenameEntry.entry_name = "filename";
00288 filenameEntry.type = "string";
00289 filenameEntry.content = filename;
00290 dataMsg.entries.push_back(filenameEntry);
00291
00292
00293 PushParams(imageProperties, dataMsg.entries);
00294
00295
00296 GenerateFilterParameterMessageEntries(list, dataMsg.entries);
00297
00298
00299 PushEndParams(result, dataMsg.entries);
00300
00301
00302 imagesdata.push_back(dataMsg);
00303
00304
00305 IplImage ipl = result;
00306 chatter_pub.publish(cvbridge.cvToImgMsg(&ipl));
00307
00308 }
00309
00310
00311 ros::ServiceClient writerClient = n.serviceClient<face_contour_detector::XMLWriter>("xml_file_bridge");
00312 face_contour_detector::XMLWriter srv;
00313 srv.request.file_name = xmlFilename;
00314 srv.request.images = imagesdata;
00315
00316 if (writerClient.call(srv)) {
00317 if (!srv.response.success) {
00318 ROS_ERROR("Service failed to save the data");
00319 }
00320 } else {
00321 ROS_ERROR("Failed to call service XMLWriter");
00322 }
00323
00324 std::cout<<"\r"<<"done!"<<std::endl;
00325 return 0;
00326 }