ImageRecognizer.cpp
Go to the documentation of this file.
00001 //
00002 // Created by bhetherman on 3/30/15.
00003 //
00004 
00005 #include <rail_recognition/ImageRecognizer.h>
00006 
00007 using namespace cv;
00008 using namespace std;
00009 
00013 bool pairCompare(const std::pair<float, string>& firstElem, const std::pair<float, string>& secondElem) {
00014   return firstElem.first > secondElem.first;
00015 }
00016 
00025 ImageRecognizer::ImageRecognizer() : pnh("~")
00026 {
00027   //ros::NodeHandle private_nh("~");
00028   numResponses = 1;
00029   dictionarySize=10;//the number of bags
00030   histSize = 10;
00031 
00032   //get parameters and directories of all stored data
00033   stringstream ssSavedDataDirPath;
00034   ssSavedDataDirPath << ros::package::getPath("rail_recognition") << "/data/saveddata";
00035   stringstream ssImagesDirPath;
00036   ssImagesDirPath << ros::package::getPath("rail_recognition") << "/data/images";
00037   stringstream ssTestImagesDirPath;
00038   ssTestImagesDirPath << ros::package::getPath("rail_recognition") << "/data/testimages";
00039   stringstream ssNewImagesDirPath;
00040   ssNewImagesDirPath << ros::package::getPath("rail_recognition") << "/data/newimages/";
00041   pnh.param("saved_data_dir", savedDataDirPath, ssSavedDataDirPath.str());
00042   pnh.param("images_dir", imagesDirPath, ssImagesDirPath.str());
00043   pnh.param("test_images_dir", testImagesDirPath, ssTestImagesDirPath.str());
00044   pnh.param("new_images_dir", newImagesDirPath, ssNewImagesDirPath.str());
00045   pnh.param("save_new_images", saveNewImages, false);
00046 
00047   bfMatcher = new BFMatcher(NORM_L2, false );//(new FlannBasedMatcher());
00048   flannMatcher = new FlannBasedMatcher();//new BFMatcher(NORM_HAMMING));
00049 
00050   classifier = new CvANN_MLP();
00051 
00052   if (saveNewImages)
00053   {
00054     string segmentedObjectsDefault = "/rail_segmentation/segmented_objects";
00055     string segmentedObjectsTopic;
00056     pnh.param("segmented_objects_topic", segmentedObjectsTopic, segmentedObjectsDefault);
00057     sub = node.subscribe<rail_manipulation_msgs::SegmentedObjectList>(segmentedObjectsTopic, 1,
00058                                                                       &ImageRecognizer::objectListCallBack, this);
00059   }
00060 }
00061 
00065 void ImageRecognizer::loadImageRecognizer()
00066 {
00067   Mat input, descriptor, featuresUnclustered;
00068   vector<string> FileNames, classes_names;
00069 
00070   FileNode vocabFileNode;
00071   FileStorage fs;
00072 
00073   classLegend.clear();
00074   fs.open(savedDataDirPath + "/recognitionNeuralNet.yml", FileStorage::READ);
00075   FileNode classLegendFileNode = fs["classLegend"];
00076   read(classLegendFileNode, classLegend);
00077   fs.release();
00078   int numClasses = classLegend.size();
00079   string fp = savedDataDirPath + "/recognitionNeuralNet.yml";
00080   classifier->load(fp.c_str(), "ANN");
00081 
00082 }
00083 
00084 void ImageRecognizer::calculateAndSaveFeatures()
00085 {
00086   Mat featuresUnclustered, response_hist;
00087   vector<string> FileNames, classes_names;
00088 
00089   getFilesAndClasses(classes_names, FileNames);
00090 
00091   int numClasses = 0;
00092   map<string,Mat> classes_training_data; //training data for classifiers
00093   classes_training_data.clear();
00094 
00095   cout << "Order Training Data and Calculate Features" << endl;
00096   int total_samples = 0;
00097   for(vector<string>::iterator f = FileNames.begin(), c = classes_names.begin(); f != FileNames.end(); ++f, ++c)
00098   {
00099 
00100     Mat input = imread(*f, CV_LOAD_IMAGE_COLOR);
00101     detectExtractComputeFeatures(input, response_hist);
00102 
00103     if(classes_training_data.count(*c) == 0) //not yet created...
00104     {
00105       classes_training_data[*c].create(0,response_hist.cols,response_hist.type());
00106       numClasses++;
00107     }
00108     classes_training_data[*c].push_back(response_hist);
00109     total_samples++;
00110   }
00111 
00112   printf("total_samples = %d\n", total_samples);
00113 
00114   //remove duplicates from classes
00115   sort(classes_names.begin(), classes_names.end());
00116   classes_names.erase(std::unique(classes_names.begin(), classes_names.end()), classes_names.end());
00117   ofstream outputFile;
00118   outputFile.open("feature_vectors.txt", ios::out | ios::app);
00119 
00120   for (unsigned int i = 0; i < classes_names.size(); i ++)
00121   {
00122     Mat feature_vectors = classes_training_data[classes_names[i]];
00123     for (unsigned int j = 0; j < feature_vectors.rows; j ++)
00124     {
00125       outputFile << classes_names[i] << ",";
00126 
00127       for (unsigned int k = 0; k < feature_vectors.cols; k ++)
00128       {
00129         outputFile << feature_vectors.at<float>(j, k);
00130         if (k < feature_vectors.cols - 1)
00131           outputFile << ",";
00132       }
00133       outputFile << "\n";
00134     }
00135   }
00136   outputFile.close();
00137   printf("Feature vectors saved.");
00138 }
00139 
00143 void ImageRecognizer::trainImageRecognizer()
00144 {
00145   Mat featuresUnclustered, response_hist;
00146   vector<string> FileNames, classes_names;
00147 
00148   getFilesAndClasses(classes_names, FileNames);
00149 
00150   int numClasses = 0;
00151   map<string,Mat> classes_training_data; //training data for classifiers
00152   classes_training_data.clear();
00153 
00154   cout << "Order Training Data and Calculate Features" << endl;
00155   int total_samples = 0;
00156   for(vector<string>::iterator f = FileNames.begin(), c = classes_names.begin(); f != FileNames.end(); ++f, ++c)
00157   {
00158     Mat input = imread(*f, CV_LOAD_IMAGE_COLOR);
00159     detectExtractComputeFeatures(input, response_hist);
00160 
00161     if(classes_training_data.count(*c) == 0) //not yet created...
00162     {
00163       classes_training_data[*c].create(0,response_hist.cols,response_hist.type());
00164       numClasses++;
00165     }
00166     classes_training_data[*c].push_back(response_hist);
00167     total_samples++;
00168   }
00169 
00170   printf("total_samples = %d\n", total_samples);
00171 
00172   printf("Training Neural Net\n");
00173   classLegend.clear();
00174   for (map<string,Mat>::iterator it = classes_training_data.begin(); it != classes_training_data.end(); ++it) {
00175     classLegend.push_back((*it).first);
00176   }
00177 
00178   classifier = new CvANN_MLP();
00179   trainNN(response_hist, classes_training_data,numClasses);
00180   FileStorage fs(savedDataDirPath + "/recognitionNeuralNet.yml", FileStorage::APPEND);//store the vocabulary
00181   fs << "classLegend" << classLegend;
00182   fs.release();
00183   printf("Training Done\n");
00184 }
00185 
00189 void ImageRecognizer::testImageRecognizer()
00190 {
00191   printf("Testing Start\n");
00192 //evaluate
00193   Mat test_response_hist, res;
00194 
00195   cout << "classLegend : [";
00196   for(vector<string>::iterator it = classLegend.begin(); it != classLegend.end(); ++it)
00197     cout << *it << ", ";
00198   cout << "]" << endl;
00199   DIR *testDp = opendir(testImagesDirPath.c_str());
00200 
00201   struct dirent *testEntry = readdir(testDp);
00202 
00203   int right, totalRight=0, count, totalCount=0;
00204 
00205 
00206   map<string,int> confusionMap; //training data for classifiers
00207   confusionMap.clear();
00208   for (int i = 0; i < classLegend.size(); i++)
00209   {
00210     confusionMap[classLegend.at(i)] = i;
00211   }
00212   int actual = 0, predicted = 0;
00213   Mat confusionMatrix = Mat::zeros(classLegend.size(), classLegend.size(), CV_32F);
00214 
00215   while (testEntry != NULL)
00216   {
00217     if (testEntry->d_type == DT_DIR && strcmp(testEntry->d_name, ".") != 0 && strcmp(testEntry->d_name, "..") != 0)
00218     {
00219       actual = confusionMap[testEntry->d_name];
00220       string classDirPath = testImagesDirPath + "/" + testEntry->d_name;
00221       printf("classDirPath: %s\n", classDirPath.c_str());
00222 
00223       DIR *classTestDir = opendir(classDirPath.c_str());
00224       struct dirent *classEntry = readdir(classTestDir);
00225       right = 0;
00226       count = 0;
00227       while (classEntry != NULL)
00228       {
00229         if (classEntry->d_type == DT_DIR || strcmp(classEntry->d_name, ".") == 0 || strcmp(classEntry->d_name, "..") == 0)//|| strstr(classEntry->d_name, ".png") == NULL)
00230         {
00231           classEntry = readdir(classTestDir);
00232           continue;
00233         }
00234         string testfilepath = classDirPath + "/" + classEntry->d_name;
00235 
00236         Mat testInput = imread(testfilepath, CV_LOAD_IMAGE_COLOR);
00237         detectExtractComputeFeatures(testInput, test_response_hist);
00238         classifier->predict(test_response_hist, res);
00239 
00240         vector<pair<float, string> > vRes;
00241         for (int i = 0; i < res.cols; i++)
00242         {
00243           vRes.push_back(std::make_pair(res.at<float>(i),classLegend.at(i)));
00244         }
00245         sort(vRes.begin(), vRes.end(), pairCompare);
00246 
00247         int bestMatchIndex = -1;
00248 
00249         count++;
00250         for (int i = 0; i < numResponses; i++)
00251         {
00252           if(strcmp(vRes.at(i).second.c_str(), testEntry->d_name) == 0)
00253           {
00254             bestMatchIndex = i;
00255           }
00256         }
00257 
00258         if(bestMatchIndex == -1)
00259         {
00260           predicted = confusionMap[vRes.at(0).second];
00261           confusionMatrix.at<float>(actual,predicted) += 1;
00262 
00263           cout << classEntry->d_name << " : WRONG : " << endl;
00264           cout << "classesMap : [";
00265           for(vector<pair<float, string> >::iterator it = vRes.begin(); it != vRes.end(); ++it)
00266             cout << (*it).second << ", ";
00267           cout << "]" << endl;
00268           cout << "vRes : [";
00269           for(vector<pair<float, string> >::iterator it = vRes.begin(); it != vRes.end(); ++it)
00270             cout << (*it).first << ", ";
00271           cout << "]" << endl;
00272         }
00273         else
00274         {
00275           confusionMatrix.at<float>(actual,actual) += 1;
00276           cout << classEntry->d_name << " : CORRECT : " << vRes.at(0).first << endl;//<< res << endl;
00277           right++;
00278         }
00279         classEntry = readdir(classTestDir);
00280       }
00281 
00282 
00283       cout << testEntry->d_name << " : CORRECT = " << right << " : WRONG = " << count-right << " : TOTAL CLASS = " << count << endl << endl;
00284       totalRight += right;
00285       totalCount += count;
00286 
00287       closedir(classTestDir);
00288     }
00289     testEntry = readdir(testDp);
00290   }
00291   cout << endl;
00292 
00293   cout << "confusionMatrix = "<< endl << " "  << confusionMatrix << endl << endl;
00294   cout << "TOTAL CORRECT = " << totalRight << " : TOTAL WRONG = " << totalCount-totalRight;
00295   cout << " : TOTAL ALL = " << totalCount << " --- " << (float)totalRight/totalCount*100 << " %" << endl << endl;
00296   closedir(testDp);
00297 }
00298 
00308 void ImageRecognizer::detectExtractFeatures(Mat &input, vector<KeyPoint> &keypoints, Mat &descriptor, Ptr<DescriptorExtractor> extractor, Ptr<FeatureDetector> detector)
00309 {
00310   detector->detect(input, keypoints);//detect feature points
00311   extractor->compute(input, keypoints, descriptor);//compute the descriptors for each keypoint
00312 }
00313 
00324 void ImageRecognizer::detectExtractComputeFeatures(Mat colorInput, Mat &response_hist)
00325 {
00326   vector<KeyPoint> keypoints;
00327   Mat colorhsv_hist, inputGS, descriptor;
00328 
00329   cvtColor(colorInput,inputGS,CV_RGB2GRAY);
00330 
00331   colorAndHSVDescriptors(colorInput, colorhsv_hist);
00332 
00333   response_hist = colorhsv_hist.clone();
00334 }
00335 
00348 void ImageRecognizer::getFeatures(Mat &featuresUnclustered, Ptr<DescriptorExtractor> extractor,
00349                                   Ptr<FeatureDetector> detector, vector<string> &FileNames)
00350 {
00351   Mat input;//to store the current input image
00352   vector<KeyPoint> keypoints;//To store the keypoints that will be extracted by SIFT
00353   Mat descriptor;//To store the SIFT descriptor of current image
00354 
00355   featuresUnclustered = Mat();
00356   for(vector<string>::iterator f = FileNames.begin(); f != FileNames.end(); ++f)
00357   {
00358     Mat input = imread(*f, CV_LOAD_IMAGE_GRAYSCALE); //Load as grayscale
00359     detectExtractFeatures(input, keypoints, descriptor, extractor, detector);
00360     featuresUnclustered.push_back(descriptor);
00361   }
00362 }
00363 
00374 void ImageRecognizer::getFilesAndClasses(vector<string> &classes_names, vector<string> &FileNames)
00375 {
00376   FileNames.clear();
00377   classes_names.clear();
00378   DIR *dir = opendir(imagesDirPath.c_str());
00379 
00380   struct dirent *entry = readdir(dir);
00381 
00382   while (entry != NULL)
00383   {
00384     if (entry->d_type == DT_DIR && strcmp(entry->d_name, ".") != 0 && strcmp(entry->d_name, "..") != 0)
00385     {
00386       string classDirPath = imagesDirPath + "/" + entry->d_name;
00387       DIR *classDir = opendir(classDirPath.c_str());
00388       struct dirent *classEntry = readdir(classDir);
00389 
00390       while (classEntry != NULL)
00391       {
00392         if (classEntry->d_type == DT_DIR || strcmp(classEntry->d_name, ".") == 0 || strcmp(classEntry->d_name, "..") == 0)
00393         {
00394           classEntry = readdir(classDir);
00395           continue;
00396         }
00397 
00398         string filepath = classDirPath + "/" + classEntry->d_name;
00399         FileNames.push_back(filepath);
00400         classes_names.push_back(entry->d_name);
00401         classEntry = readdir(classDir);
00402       }
00403       closedir(classDir);
00404     }
00405     entry = readdir(dir);
00406   }
00407   closedir(dir);
00408 }
00409 
00420 void ImageRecognizer::trainVocabulary(string featureName, Mat featuresUnclustered, Mat &vocabulary)
00421 {
00422 
00423   printf("BOWKMeansTrainer\n");
00424   TermCriteria tc(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,1000000,0.000001);//define Term Criteria
00425   int retries=1;//retries number
00426   int flags=KMEANS_PP_CENTERS;//necessary flags
00427 
00428   Mat featuresUnclustered_32f;
00429   featuresUnclustered.convertTo(featuresUnclustered_32f, CV_32F);
00430   BOWKMeansTrainer bowTrainer(dictionarySize,tc,retries,flags);//Create the BoW (or BoF) trainer
00431   bowTrainer.add(featuresUnclustered_32f);
00432   vocabulary=bowTrainer.cluster();//cluster the feature vectors
00433 
00434   FileStorage fs(savedDataDirPath + "/" + featureName + "_dictionary.yml", FileStorage::WRITE);//store the vocabulary
00435   fs << "vocabulary" << vocabulary;
00436   fs.release();
00437 }
00438 
00449 void ImageRecognizer::trainNN(Mat &inputData, map<string,Mat> &classesTrainingData, int numClasses)
00450 {
00451   Mat layers = Mat(3, 1, CV_32S);
00452   layers.row(0)  = inputData.cols;
00453   layers.row(1)  = inputData.cols*2;
00454   layers.row(2)  = numClasses;
00455   cout << "inputData.cols : " << inputData.cols << endl;
00456 
00457   CvANN_MLP_TrainParams params;
00458   TermCriteria criteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,500000,0.0000001);
00459 
00460   params.train_method = CvANN_MLP_TrainParams::BACKPROP;
00461   params.bp_dw_scale  = 0.1;
00462   params.bp_moment_scale = 0.05;
00463   params.term_crit  = criteria;
00464 
00465   classifier->create(layers, CvANN_MLP::SIGMOID_SYM,1,1);
00466 
00467   int index = 0;
00468   Mat samples(0,inputData.cols,inputData.type());
00469   Mat labels(0,numClasses,CV_32F);
00470 
00471   for (map<string,Mat>::iterator it = classesTrainingData.begin(); it != classesTrainingData.end(); ++it) {
00472     string class_ = (*it).first;
00473     samples.push_back(classesTrainingData[class_]);
00474     Mat class_label = Mat::ones(classesTrainingData[class_].rows, numClasses, CV_32F);
00475     class_label *= -1;
00476     class_label.col(index) += 2.0;
00477     labels.push_back(class_label);
00478 
00479     index++;
00480   }
00481 
00482   Mat samples_32f;
00483   samples.convertTo(samples_32f, CV_32F);
00484 
00485   classifier->train(samples_32f,labels, cv::Mat(), cv::Mat(), params);
00486   string sp = savedDataDirPath + "/recognitionNeuralNet.yml";
00487   classifier->save(sp.c_str(), "ANN");
00488 }
00489 
00500 void ImageRecognizer::colorAndHSVDescriptors(Mat colorInput, Mat &result_hist)
00501 {
00502   float hsvRange[] = { 0, 180 } ; //the upper boundary is exclusive
00503   float range[] = { 0, 256 } ; //the upper boundary is exclusive
00504   const float* hsvHistRange = { hsvRange };
00505   const float* histRange = { range };
00506   bool uniform = true; bool accumulate = false;
00507   Mat b_hist, g_hist, r_hist, hue_hist, sat_hist, val_hist;
00508   Mat temp_results, hsv;
00509   vector<Mat> bgr_planes, hsv_planes;
00510 
00511   if(!colorInput.data )
00512   {
00513     cout << "No image data" << endl;
00514     return;
00515   }
00516 
00517   split( colorInput, bgr_planes );
00518 
00519   cvtColor( colorInput, hsv, CV_RGB2HSV );
00520   split( hsv, hsv_planes );
00521 
00523   calcHist( &hsv_planes[0], 1, 0, Mat(), hue_hist, 1, &histSize, &hsvHistRange, uniform, accumulate );
00524   calcHist( &hsv_planes[1], 1, 0, Mat(), sat_hist, 1, &histSize, &hsvHistRange, uniform, accumulate );
00525   calcHist( &hsv_planes[2], 1, 0, Mat(), val_hist, 1, &histSize, &hsvHistRange, uniform, accumulate );
00526 
00527   calcHist( &bgr_planes[0], 1, 0, Mat(), b_hist, 1, &histSize, &histRange, uniform, accumulate );
00528   calcHist( &bgr_planes[1], 1, 0, Mat(), g_hist, 1, &histSize, &histRange, uniform, accumulate );
00529   calcHist( &bgr_planes[2], 1, 0, Mat(), r_hist, 1, &histSize, &histRange, uniform, accumulate );
00530 
00532   int numPixels = colorInput.size().height*colorInput.size().width;
00533   normalizeFeatureVector(hue_hist, numPixels);
00534   normalizeFeatureVector(sat_hist, numPixels);
00535   normalizeFeatureVector(val_hist, numPixels);
00536   normalizeFeatureVector(b_hist, numPixels);
00537   normalizeFeatureVector(g_hist, numPixels);
00538   normalizeFeatureVector(r_hist, numPixels);
00539 
00540   temp_results = hue_hist.t();
00541   hconcat(temp_results, sat_hist.t(),temp_results);
00542   hconcat(temp_results, val_hist.t(),temp_results);
00543 
00544   hconcat(temp_results,b_hist.t(),temp_results);
00545   hconcat(temp_results,g_hist.t(),temp_results);
00546   hconcat(temp_results,r_hist.t(),temp_results);
00547 
00548   result_hist = temp_results.clone();
00549 }
00550 
00551 void ImageRecognizer::normalizeFeatureVector(Mat &featureVector, int numPixels)
00552 {
00553   for (unsigned int i = 0; i < featureVector.rows; i ++) {
00554     for (unsigned int j = 0; j < featureVector.cols; j++) {
00555       featureVector.at<float>(i, j) /= ((float)numPixels);
00556     }
00557   }
00558 }
00559 
00569 void ImageRecognizer::objectListCallBack(const rail_manipulation_msgs::SegmentedObjectList msg)
00570 {
00571   int n = 0;
00572   time_t seconds;
00573   seconds = time (NULL);
00574 
00575   rail_manipulation_msgs::SegmentedObjectList newObjectList;
00576   newObjectList.header = msg.header;
00577   newObjectList.objects.resize(msg.objects.size());
00578   //cout << "newObjectList.objects.size() = " << newObjectList.objects.size() << endl;
00579   for (unsigned int i = 0; i < newObjectList.objects.size(); i++)
00580   {
00581     newObjectList.objects[i] = msg.objects[i];
00582 
00583     cv_bridge::CvImagePtr cv_ptr;
00584     try
00585     {
00586       //cv_ptr = cv_bridge::toCvCopy(newObjectList.objects[i].image, sensor_msgs::image_encodings::RGB8);
00587       cv_ptr = cv_bridge::toCvCopy(newObjectList.objects[i].image, sensor_msgs::image_encodings::BGR8);
00588     }
00589     catch (cv_bridge::Exception &e)
00590     {
00591       ROS_ERROR("cv_bridge exception: %s", e.what());
00592       continue;
00593     }
00594 
00595     //save image
00596     n++;
00597     stringstream savePath;
00598     savePath << newImagesDirPath << seconds << "_" << n << ".png";
00599     imwrite(savePath.str(), cv_ptr->image);
00600   }
00601 }
00602 
00603 void ImageRecognizer::recognizeObject(const rail_manipulation_msgs::SegmentedObject object, vector< pair<float, string> > &recognitionResults)
00604 {
00605   cv_bridge::CvImagePtr cv_ptr;
00606   try
00607   {
00608     //cv_ptr = cv_bridge::toCvCopy(object.image, sensor_msgs::image_encodings::RGB8);
00609     cv_ptr = cv_bridge::toCvCopy(object.image, sensor_msgs::image_encodings::BGR8);
00610   }
00611   catch (cv_bridge::Exception &e)
00612   {
00613     ROS_ERROR("cv_bridge exception: %s", e.what());
00614     return;
00615   }
00616 
00617   testImage(cv_ptr->image,recognitionResults);
00618 }
00619 
00629 void ImageRecognizer::testImage(Mat colorInput, vector<pair<float, string> > &testResults)
00630 {
00631   Mat testResponseHist, res;
00632   detectExtractComputeFeatures(colorInput, testResponseHist);
00633   classifier->predict(testResponseHist, res);
00634 
00635   testResults.clear();
00636   for (int i = 0; i < res.cols; i++)
00637   {
00638     testResults.push_back(std::make_pair(res.at<float>(i),classLegend.at(i)));
00639   }
00640   sort(testResults.begin(), testResults.end(), pairCompare);
00641 }


rail_recognition
Author(s): David Kent , Russell Toris , bhetherman
autogenerated on Thu Jun 6 2019 19:44:08