processor.cpp
Go to the documentation of this file.
00001 
00012 #include <cv_bridge/cv_bridge.h>
00013 #include <iostream>
00014 #include <opencv2/highgui/highgui.hpp>
00015 #include <opencv2/imgproc/imgproc.hpp>
00016 #include <opencv2/nonfree/features2d.hpp>
00017 #include <pcl/ros/conversions.h>
00018 #include <rail_cv_project/processor.h>
00019 #include <ros/package.h>
00020 #include <sensor_msgs/image_encodings.h>
00021 #include <sstream>
00022 
00023 using namespace std;
00024 using namespace cv;
00025 using namespace pcl;
00026 
00027 processor::processor() :
00028     random(12345)
00029 {
00030   feed = node.subscribe<sensor_msgs::Image>("/kinect_head/rgb/image_rect_color", 1, &processor::feed_cback, this);
00031   cloud = node.subscribe<sensor_msgs::PointCloud2>("/kinect_head/depth_registered/points", 1, &processor::cloud_cback,
00032                                                    this);
00033 
00034   erode_element = getStructuringElement(MORPH_ELLIPSE, Size(2 * ERODE_ELEMENT_SIZE + 1, 2 * ERODE_ELEMENT_SIZE + 1),
00035                                         Point(ERODE_ELEMENT_SIZE, ERODE_ELEMENT_SIZE));
00036   dilate_element = getStructuringElement(MORPH_ELLIPSE, Size(2 * DILATE_ELEMENT_SIZE + 1, 2 * DILATE_ELEMENT_SIZE + 1),
00037                                          Point(DILATE_ELEMENT_SIZE, DILATE_ELEMENT_SIZE));
00038 
00039   // create reference rectangle for comparing contours in the feature vector
00040   RotatedRect ref_rectangle = RotatedRect(Point2f(1, 1), Size2f(REF_WIDTH, REF_HEIGHT), 90);
00041   Point2f r_vert[4];
00042   ref_rectangle.points(r_vert);
00043   for (int i = 0; i < 4; i++)
00044     ref_contour.push_back(r_vert[i]);
00045 
00046   // check if we need to capture images first
00047   string capture_input;
00048   cout << "Would you like to capture training images first? (Y/n): ";
00049   cin >> capture_input;
00050   if (capture_input.compare("Y") == 0 || capture_input.compare("y") == 0)
00051   {
00052     capture_count = 0;
00053     capture = true;
00054   }
00055   else
00056   {
00057     string capture_input_2;
00058     cout << "Would you like to run the classifier accuracy test? (Y/n): ";
00059     cin >> capture_input_2;
00060 
00061     // create the classifier
00062     bayes = new CvNormalBayesClassifier();
00063     // train the classifier
00064     train();
00065 
00066     // check if we are running or
00067     if (capture_input_2.compare("Y") == 0 || capture_input_2.compare("y") == 0)
00068     {
00069       // run the test
00070       accuracy();
00071     }
00072     else
00073     {
00074       capture = false;
00075       last_x = 0;
00076       last_y = 0;
00077     }
00078   }
00079 }
00080 
00081 processor::~processor()
00082 {
00083   delete bayes;
00084 }
00085 
00086 void processor::train()
00087 {
00088   cout << "-- Begin Model Training --" << endl;
00089   string path = ros::package::getPath("rail_cv_project");
00090 
00091   Mat training(NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS + NUM_COVERS + NUM_OTHER, D, CV_32FC1);
00092   Mat resp(NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS + NUM_COVERS + NUM_OTHER, 1, CV_32FC1);
00093   // load all training images
00094   for (int i = 0; i < NUM_COMPUTER; i++)
00095   {
00096     stringstream s;
00097     s << path << "/img/computer" << i << ".jpg";
00098     cout << "\tTraining '" << s.str() << "'..." << endl;
00099     vector<float> v = get_feature_vector(imread(s.str()));
00100     for (uint j = 0; j < v.size(); j++)
00101       training.at<float>(i, j) = v.at(j);
00102     resp.at<float>(i, 0) = COMPUTER;
00103   }
00104   for (int i = 0; i < NUM_TURTLE; i++)
00105   {
00106     stringstream s;
00107     s << path << "/img/turtle" << i << ".jpg";
00108     cout << "\tTraining '" << s.str() << "'..." << endl;
00109     vector<float> v = get_feature_vector(imread(s.str()));
00110     for (uint j = 0; j < v.size(); j++)
00111       training.at<float>(i + NUM_COMPUTER, j) = v.at(j);
00112     resp.at<float>(i + NUM_COMPUTER, 0) = TURTLE;
00113   }
00114   for (int i = 0; i < NUM_ROBOT; i++)
00115   {
00116     stringstream s;
00117     s << path << "/img/robot" << i << ".jpg";
00118     cout << "\tTraining '" << s.str() << "'..." << endl;
00119     vector<float> v = get_feature_vector(imread(s.str()));
00120     for (uint j = 0; j < v.size(); j++)
00121       training.at<float>(i + NUM_COMPUTER + NUM_TURTLE, j) = v.at(j);
00122     resp.at<float>(i + NUM_COMPUTER + NUM_TURTLE, 0) = ROBOT;
00123   }
00124   for (int i = 0; i < NUM_BSG; i++)
00125   {
00126     stringstream s;
00127     s << path << "/img/bsg" << i << ".jpg";
00128     cout << "\tTraining '" << s.str() << "'..." << endl;
00129     vector<float> v = get_feature_vector(imread(s.str()));
00130     for (uint j = 0; j < v.size(); j++)
00131       training.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT, j) = v.at(j);
00132     resp.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT, 0) = BSG;
00133   }
00134   for (int i = 0; i < NUM_PENS; i++)
00135   {
00136     stringstream s;
00137     s << path << "/img/pens" << i << ".jpg";
00138     cout << "\tTraining '" << s.str() << "'..." << endl;
00139     vector<float> v = get_feature_vector(imread(s.str()));
00140     for (uint j = 0; j < v.size(); j++)
00141       training.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG, j) = v.at(j);
00142     resp.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG, 0) = PENS;
00143   }
00144   for (int i = 0; i < NUM_COVERS; i++)
00145   {
00146     stringstream s;
00147     s << path << "/img/cover" << i << ".jpg";
00148     cout << "\tTraining '" << s.str() << "'..." << endl;
00149     vector<float> v = get_feature_vector(imread(s.str()));
00150     for (uint j = 0; j < v.size(); j++)
00151       training.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS, j) = v.at(j);
00152     resp.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS, 0) = COVERS;
00153   }
00154   for (int i = 0; i < NUM_OTHER; i++)
00155   {
00156     stringstream s;
00157     s << path << "/img/other" << i << ".jpg";
00158     cout << "\tTraining '" << s.str() << "'..." << endl;
00159     vector<float> v = get_feature_vector(imread(s.str()));
00160     for (uint j = 0; j < v.size(); j++)
00161       training.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS + NUM_COVERS, j) = v.at(j);
00162     resp.at<float>(i + NUM_COMPUTER + NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS + NUM_COVERS, 0) = OTHER;
00163   }
00164 
00165   // train the model
00166   bayes->train(training, resp);
00167 
00168   cout << "-- Model Trained --" << endl;
00169 }
00170 
00171 void processor::accuracy()
00172 {
00173   cout << "-- Begin Model Test --" << endl;
00174   string path = ros::package::getPath("rail_cv_project");
00175 
00176   // load all training images
00177   int num_correct = 0;
00178   int false_positive = 0;
00179   for (int i = 0; i < NUM_COMPUTER; i++)
00180   {
00181     stringstream s;
00182     s << path << "/img/computer" << i << ".jpg";
00183     vector<float> v = get_feature_vector(imread(s.str()));
00184     Mat pred(1, v.size(), CV_32FC1);
00185     for (uint j = 0; j < v.size(); j++)
00186       pred.at<float>(0, j) = v.at(j);
00187 
00188     // check if we found a computer
00189     if ((int)bayes->predict(pred) == COMPUTER)
00190       num_correct++;
00191   }
00192   for (int i = 0; i < NUM_TURTLE; i++)
00193   {
00194     stringstream s;
00195     s << path << "/img/turtle" << i << ".jpg";
00196     cout << "\tTraining '" << s.str() << "'..." << endl;
00197     vector<float> v = get_feature_vector(imread(s.str()));
00198     Mat pred(1, v.size(), CV_32FC1);
00199     for (uint j = 0; j < v.size(); j++)
00200       pred.at<float>(0, j) = v.at(j);
00201 
00202     // check if we found a computer
00203     if ((int)bayes->predict(pred) == COMPUTER)
00204       false_positive++;
00205   }
00206   for (int i = 0; i < NUM_ROBOT; i++)
00207   {
00208     stringstream s;
00209     s << path << "/img/robot" << i << ".jpg";
00210     cout << "\tTraining '" << s.str() << "'..." << endl;
00211     vector<float> v = get_feature_vector(imread(s.str()));
00212     Mat pred(1, v.size(), CV_32FC1);
00213     for (uint j = 0; j < v.size(); j++)
00214       pred.at<float>(0, j) = v.at(j);
00215 
00216     // check if we found a computer
00217     if ((int)bayes->predict(pred) == COMPUTER)
00218       false_positive++;
00219   }
00220   for (int i = 0; i < NUM_BSG; i++)
00221   {
00222     stringstream s;
00223     s << path << "/img/bsg" << i << ".jpg";
00224     cout << "\tTraining '" << s.str() << "'..." << endl;
00225     vector<float> v = get_feature_vector(imread(s.str()));
00226     Mat pred(1, v.size(), CV_32FC1);
00227     for (uint j = 0; j < v.size(); j++)
00228       pred.at<float>(0, j) = v.at(j);
00229 
00230     // check if we found a computer
00231     if ((int)bayes->predict(pred) == COMPUTER)
00232       false_positive++;
00233   }
00234   for (int i = 0; i < NUM_PENS; i++)
00235   {
00236     stringstream s;
00237     s << path << "/img/pens" << i << ".jpg";
00238     cout << "\tTraining '" << s.str() << "'..." << endl;
00239     vector<float> v = get_feature_vector(imread(s.str()));
00240     Mat pred(1, v.size(), CV_32FC1);
00241     for (uint j = 0; j < v.size(); j++)
00242       pred.at<float>(0, j) = v.at(j);
00243 
00244     // check if we found a computer
00245     if ((int)bayes->predict(pred) == COMPUTER)
00246       false_positive++;
00247   }
00248   for (int i = 0; i < NUM_COVERS; i++)
00249   {
00250     stringstream s;
00251     s << path << "/img/cover" << i << ".jpg";
00252     cout << "\tTraining '" << s.str() << "'..." << endl;
00253     vector<float> v = get_feature_vector(imread(s.str()));
00254     Mat pred(1, v.size(), CV_32FC1);
00255     for (uint j = 0; j < v.size(); j++)
00256       pred.at<float>(0, j) = v.at(j);
00257 
00258     // check if we found a computer
00259     if ((int)bayes->predict(pred) == COMPUTER)
00260       false_positive++;
00261   }
00262   for (int i = 0; i < NUM_OTHER; i++)
00263   {
00264     stringstream s;
00265     s << path << "/img/other" << i << ".jpg";
00266     cout << "\tTraining '" << s.str() << "'..." << endl;
00267     vector<float> v = get_feature_vector(imread(s.str()));
00268     Mat pred(1, v.size(), CV_32FC1);
00269     for (uint j = 0; j < v.size(); j++)
00270       pred.at<float>(0, j) = v.at(j);
00271 
00272     // check if we found a computer
00273     if ((int)bayes->predict(pred) == COMPUTER)
00274       false_positive++;
00275   }
00276 
00277   cout << "\tCorrect: " << ((double)num_correct) / NUM_COMPUTER << endl;
00278   cout << "\tFalse Positive: "
00279       << ((double)false_positive) / (NUM_TURTLE + NUM_ROBOT + NUM_BSG + NUM_PENS + NUM_COVERS + NUM_OTHER) << endl;
00280   cout << "-- Model Test Finished --" << endl;
00281   exit(EXIT_SUCCESS);
00282 }
00283 
00284 void processor::feed_cback(const sensor_msgs::Image::ConstPtr& img)
00285 {
00286   // convert from a ROS image to an OpenCV image
00287   cv_bridge::CvImagePtr cv_img;
00288   cv_img = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8);
00289 
00290   // segment the image into bounding boxes on possible objects
00291   vector<RotatedRect> rectangles = segment(cv_img->image);
00292 
00293   // create a separate image for each segmented section
00294   vector<Mat> images = split_image(cv_img->image, rectangles);
00295 
00296   // check if we are training, or if we are classifying
00297   if (capture)
00298   {
00299     // save each and wait
00300     for (uint i = 0; i < images.size(); i++)
00301     {
00302       // save the image
00303       stringstream s;
00304       s << ros::package::getPath("rail_cv_project") << "/img/IMAGE-" << capture_count++ << ".jpg";
00305       imwrite(s.str(), images.at(i));
00306     }
00307 
00308     string capture_input;
00309     cout << "Continue to capture? (Y/n): ";
00310     cin >> capture_input;
00311     if (capture_input.compare("Y") != 0 && capture_input.compare("y") != 0)
00312       exit(0);
00313   }
00314   else
00315   {
00316     //initially add all the smoothed points as unvisited
00317     for (uint i = 0; i < smoothing_points.size(); i++)
00318       non_matches.push_back(i);
00319 
00320     // classify chunks and find the computer
00321     Mat possible_matches = cv_img->image.clone();
00322     Mat classes = cv_img->image.clone();
00323     for (uint i = 0; i < images.size(); i++)
00324     {
00325       // copy the vector into a CV Mat
00326       vector<float> v = get_feature_vector(images.at(i));
00327       Mat pred(1, v.size(), CV_32FC1);
00328       for (uint j = 0; j < v.size(); j++)
00329         pred.at<float>(0, j) = v.at(j);
00330 
00331       // check if we found a computer
00332       int classification = (int)bayes->predict(pred);
00333       if (classification == COMPUTER)
00334       {
00335         // add it to the list of possible computer locations
00336         smooth(rectangles.at(i));
00337 
00338         // draw this on the image as possible locations
00339         Point2f rect_points[4];
00340         RotatedRect possible = rectangles.at(i);
00341         possible.points(rect_points);
00342         for (int j = 0; j < 4; j++)
00343           line(possible_matches, rect_points[j], rect_points[(j + 1) % 4], Scalar(255, 0, 0), 3);
00344       }
00345       // label all classes
00346       string text = "";
00347       switch (classification)
00348       {
00349         case COMPUTER:
00350           text = "Toy";
00351           break;
00352         case TURTLE:
00353           text = "Turtle";
00354           break;
00355         case ROBOT:
00356           text = "Robot";
00357           break;
00358         case BSG:
00359           text = "Cylon";
00360           break;
00361         case PENS:
00362           text = "Pens";
00363           break;
00364         case COVERS:
00365           text = "Cover";
00366           break;
00367         default:
00368           text = "Turtle";
00369           break;
00370       }
00371       // bold text
00372       for (int x = -1; x <= 1; x++)
00373         for (int y = -1; y <= 1; y++)
00374         {
00375           int center_x = max(0, min((int)rectangles.at(i).center.x + x, classes.cols));
00376           int center_y = max(0, min((int)rectangles.at(i).center.y + y, classes.rows));
00377           putText(classes, text, Point(center_x, center_y), FONT_HERSHEY_PLAIN, 2, Scalar(0, 64, 255));
00378         }
00379 
00380     }
00381 
00382     // clear any zeros in the smoothing vector
00383     for (int i = non_matches.size() - 1; i >= 0; i--)
00384     {
00385       smoothing_points.at(non_matches.at(i)).c -= 2;
00386       if (smoothing_points.at(non_matches.at(i)).c <= 0)
00387         smoothing_points.erase(smoothing_points.begin() + non_matches.at(i));
00388     }
00389     non_matches.clear();
00390 
00391     // find the location with the most counts
00392     int max_c = 0;
00393     RotatedRect best;
00394     for (uint i = 0; i < smoothing_points.size(); i++)
00395     {
00396       if (smoothing_points.at(i).c > max_c)
00397       {
00398         best = smoothing_points.at(i).rect;
00399         max_c = smoothing_points.at(i).c;
00400       }
00401     }
00402 
00403     // outline it in the images
00404     if (max_c > 0 && max_c > MAX_SMOOTHING_COUNT / 4)
00405     {
00406       Point2f rect_points[4];
00407       best.points(rect_points);
00408       for (int j = 0; j < 4; j++)
00409       {
00410         line(possible_matches, rect_points[j], rect_points[(j + 1) % 4], Scalar(0, 255, 0), 4);
00411         line(cv_img->image, rect_points[j], rect_points[(j + 1) % 4], Scalar(0, 0, 255), 8);
00412       }
00413 
00414       // check if we should update the TF
00415       if (sqrt(
00416           pow((double)(best.center.x - last_x), 2.0) + pow((double)(best.center.y - last_y), 2.0)) > SMOOTH_DIST_THRESH)
00417       {
00418         // get the depth point
00419         if (latest_cloud.isOrganized() && latest_cloud.height > 1)
00420         {
00421           PointXYZ p = latest_cloud.at(best.center.x, best.center.y);
00422           // create a transform for the point
00423           tf::Transform tf;
00424           tf.setOrigin(tf::Vector3(p.x, p.y, p.z));
00425           tf.setRotation(tf::Quaternion(0, 0, 0, 1));
00426           tfb.sendTransform(tf::StampedTransform(tf, ros::Time::now(), CLOUD_FRAME, OBJECT_FRAME));
00427 
00428           // add a padding point to set as the goal
00429           tf::Transform tf2;
00430           tf2.setOrigin(tf::Vector3(p.x + PADDING_X, p.y + PADDING_Y, p.z + PADDING_Z));
00431           tf2.setRotation(tf::Quaternion(0, 0, 0, 1));
00432           tfb.sendTransform(tf::StampedTransform(tf2, ros::Time::now(), CLOUD_FRAME, GOAL_FRAME));
00433 
00434           // update our latest points
00435           last_x = best.center.x;
00436           last_y = best.center.y;
00437         }
00438       }
00439 
00440     }
00441     imshow("Possible Matches", possible_matches);
00442     imshow("Classes", classes);
00443     imshow("Final", cv_img->image);
00444   }
00445 
00446   // leave the windows open
00447   waitKey(3);
00448 }
00449 
00450 void processor::smooth(RotatedRect r)
00451 {
00452   // go through the list and check for a match
00453   bool match = false;
00454   for (uint i = 0; i < smoothing_points.size(); i++)
00455   {
00456     RotatedRect cur = smoothing_points.at(i).rect;
00457     double dist = sqrt(pow((double)(r.center.x - cur.center.x), 2.0) + pow((double)(r.center.y - cur.center.y), 2.0));
00458     if (dist < SMOOTH_DIST_THRESH)
00459     {
00460       match = true;
00461       // update the box
00462       smoothing_points.at(i).rect = r;
00463       // increment the counter
00464       smoothing_points.at(i).c = min(MAX_SMOOTHING_COUNT, smoothing_points.at(i).c + 1);
00465 
00466       // check if this is inside the non-matches from a previous check
00467       for (uint j = 0; j < non_matches.size(); j++)
00468       {
00469         if (non_matches.at(j) == (int)i)
00470         {
00471           non_matches.erase(non_matches.begin() + j);
00472           continue;
00473         }
00474       }
00475     }
00476   }
00477 
00478   // if no match was found, we add it
00479   if (!match)
00480   {
00481     smoothing_point p;
00482     p.c = 1;
00483     p.rect = r;
00484     smoothing_points.push_back(p);
00485   }
00486 }
00487 
00488 vector<RotatedRect> processor::segment(Mat const src)
00489 {
00490   // blur the image and convert to grayscale
00491   Mat img;
00492   cv::cvtColor(src, img, CV_BGR2GRAY);
00493   blur(img, img, Size(BLUR_KERNEL, BLUR_KERNEL));
00494   // erode to get rid of details
00495   erode(img, img, erode_element);
00496 
00497   // run the Canny edge operator, dilate, then run it again to help find better contours
00498   Canny(img, img, CANNY_MIN_THRESH, CANNY_MAX_THRESH, CANNY_KERNEL);
00499   imshow("First Canny", img);
00500   dilate(img, img, dilate_element);
00501   Canny(img, img, CANNY_MIN_THRESH / 1, CANNY_MAX_THRESH / 1, CANNY_KERNEL);
00502   imshow("Second Canny", img);
00503 
00504   // find the contours
00505   vector<vector<Point> > contours;
00506   vector<Vec4i> hierarchy;
00507   findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
00508 
00509   // search for valid bounding rectangles
00510   vector<RotatedRect> rectangles;
00511   Mat final = Mat::zeros(img.size(), CV_8UC3);
00512   for (uint i = 0; i < contours.size(); i++)
00513   {
00514     // find the bounding box
00515     RotatedRect r = minAreaRect(Mat(contours[i]));
00516 
00517     // filter the bounding box (flip it to be taller rather than wider)
00518     float width = min(r.size.width, r.size.height);
00519     float length = max(r.size.width, r.size.height);
00520     if (width / length >= RATIO_THRESHOLD && width * length >= MIN_AREA && width * length <= MAX_AREA)
00521     {
00522       // pick a random color to draw the contour
00523       Scalar color = Scalar(random.uniform(0, 255), random.uniform(0, 255), random.uniform(0, 255));
00524       drawContours(final, contours, i, color, CV_FILLED, 8, hierarchy, 0, Point());
00525 
00526       // add it to the list
00527       rectangles.push_back(r);
00528     }
00529   }
00530 
00531   // only continue if we found some
00532   if (rectangles.size() > 0)
00533   {
00534     // filter any bounding boxes around the same point
00535     for (uint i = 0; i < rectangles.size() - 1; i++)
00536     {
00537       RotatedRect r = rectangles[i];
00538       vector<int> removeIndeces;
00539       float maxSize = r.size.width + r.size.height;
00540       int maxIndex = i;
00541       for (uint j = i + 1; j < rectangles.size(); j++)
00542       {
00543         if (sqrt(pow(r.center.x - rectangles[j].center.x, 2) + pow(r.center.y - rectangles[j].center.y, 2))
00544             < sqrt(MIN_AREA))
00545         {
00546           // take the larger of the two
00547           int rSize = rectangles[j].size.width + rectangles[j].size.height;
00548           if (rSize > maxSize)
00549           {
00550             removeIndeces.push_back(maxIndex);
00551             maxSize = rSize;
00552             maxIndex = j;
00553           }
00554           else
00555             removeIndeces.push_back(j);
00556         }
00557       }
00558       // remove the ones we don't want
00559       sort(removeIndeces.begin(), removeIndeces.end());
00560       for (int j = removeIndeces.size() - 1; j >= 0; j--)
00561         rectangles.erase(rectangles.begin() + removeIndeces[j]);
00562     }
00563 
00564     // draw them
00565     for (uint i = 0; i < rectangles.size(); i++)
00566     {
00567       Scalar color = Scalar(random.uniform(0, 255), random.uniform(0, 255), random.uniform(0, 255));
00568       // draw the bounding box
00569       Point2f rect_points[4];
00570       rectangles[i].points(rect_points);
00571       for (int j = 0; j < 4; j++)
00572         line(final, rect_points[j], rect_points[(j + 1) % 4], color, 1);
00573     }
00574   }
00575 
00576   imshow("Contours", final);
00577 
00578   return rectangles;
00579 }
00580 
00581 vector<Mat> processor::split_image(Mat const src, vector<RotatedRect> rectangles)
00582 {
00583   vector<Mat> images;
00584   // check all the areas we are interested in
00585   for (uint i = 0; i < rectangles.size(); i++)
00586   {
00587     Point2f pts[4];
00588     rectangles[i].points(pts);
00589 
00590     // grab the points
00591     cv::Point2f srcVertices[3];
00592     srcVertices[0] = pts[0];
00593     srcVertices[1] = pts[1];
00594     srcVertices[2] = pts[3];
00595     Point2f dstVertices[3];
00596     dstVertices[0] = Point(0, 0);
00597     dstVertices[1] = Point(rectangles[i].size.width - 1, 0);
00598     dstVertices[2] = Point(0, rectangles[i].size.height - 1);
00599 
00600     // pick out the area we want from the source
00601     Mat warpAffineMatrix = getAffineTransform(srcVertices, dstVertices);
00602     Size size(rectangles[i].size.width, rectangles[i].size.height);
00603     Mat img;
00604     warpAffine(src, img, warpAffineMatrix, size, INTER_LINEAR, BORDER_CONSTANT);
00605 
00606     images.push_back(img);
00607   }
00608 
00609   return images;
00610 }
00611 
00612 vector<float> processor::get_feature_vector(Mat const src)
00613 {
00614   // average RGB
00615   float avg_red = 0.0;
00616   float avg_green = 0.0;
00617   float avg_blue = 0.0;
00618   for (int i = 0; i < src.rows; i++)
00619     for (int j = 0; j < src.cols; j++)
00620     {
00621       Vec3b intensity = src.at<Vec3b>(i, j);
00622       avg_red += intensity.val[2];
00623       avg_green += intensity.val[1];
00624       avg_blue += intensity.val[0];
00625     }
00626   float area = src.cols * src.rows;
00627   avg_red /= (area * 255.0);
00628   avg_blue /= (area * 255.0);
00629   avg_green /= (area * 255.0);
00630 
00631   // edge and line features
00632   Mat img1, img2;
00633   cvtColor(src, img1, CV_BGR2GRAY);
00634   blur(img1, img1, Size(BLUR_KERNEL, BLUR_KERNEL));
00635   Canny(img1, img1, CANNY_MIN_THRESH, CANNY_MAX_THRESH, CANNY_KERNEL);
00636 
00637   // find the number of lines in the image
00638   vector<cv::Vec4i> lines;
00639   HoughLinesP(img1, lines, HOUGH_RHO, HOUGH_THETA, HOUGH_THRESH, HOUGH_MIN_LINE_LENGTH, HOUGH_MAX_LINE_GAP);
00640   float avg_lines = min((double)lines.size(), MAX_LINES) / MAX_LINES;
00641 
00642   // get the average line length
00643   float avg_line_length = 0.0;
00644   for (uint i = 0; i < lines.size(); i++)
00645     avg_line_length += sqrt(pow(lines[i][0] - lines[i][2], 2.0) + pow(lines[i][1] - lines[i][3], 2.0));
00646   avg_line_length /= lines.size();
00647   avg_line_length = max(0.0, min((double)avg_line_length, MAX_AVG_LINE_LENGTH)) / MAX_AVG_LINE_LENGTH;
00648 
00649   // rerun Canny to get our "bubbled" contours
00650   cvtColor(src, img2, CV_BGR2GRAY);
00651   blur(img2, img2, Size(BLUR_KERNEL, BLUR_KERNEL));
00652   erode(img2, img2, erode_element);
00653   Canny(img2, img2, CANNY_MIN_THRESH, CANNY_MAX_THRESH, CANNY_KERNEL);
00654   dilate(img2, img2, dilate_element);
00655   Canny(img2, img2, CANNY_MIN_THRESH / 2, CANNY_MAX_THRESH / 2, CANNY_KERNEL);
00656   // fine the contours
00657   vector<vector<Point> > contours;
00658   vector<Vec4i> hierarchy;
00659   findContours(img2, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
00660   // compare contours with the rectangle
00661   float rect_match_feature = 0;
00662   for (uint i = 0; i < contours.size(); i++)
00663     if (matchShapes(contours[i], ref_contour, CV_CONTOURS_MATCH_I1, 0) == 0)
00664       rect_match_feature++;
00665   if (contours.size() > 0)
00666     rect_match_feature /= contours.size();
00667   else
00668     rect_match_feature = 0;
00669 
00670   // calculate the number of SIFT features
00671   SiftFeatureDetector detector;
00672   vector<KeyPoint> keypoints;
00673   detector.detect(src, keypoints);
00674   float avg_sift = min((double)keypoints.size(), MAX_SIFT) / MAX_SIFT;
00675 
00676   // create the feature vector
00677   vector<float> features;
00678   features.push_back(avg_red);
00679   features.push_back(avg_green);
00680   features.push_back(avg_blue);
00681   features.push_back(avg_lines);
00682   features.push_back(avg_line_length);
00683   features.push_back(avg_sift);
00684   features.push_back(rect_match_feature);
00685 
00686   return features;
00687 }
00688 
00689 void processor::cloud_cback(const sensor_msgs::PointCloud2::ConstPtr& pc)
00690 {
00691   // copy over the point cloud to save it
00692   fromROSMsg(*pc, latest_cloud);
00693 }
00694 
00695 int main(int argc, char **argv)
00696 {
00697   // initialize ROS and the node
00698   ros::init(argc, argv, "processor");
00699   processor p;
00700 
00701   // continue until a ctrl-c has occurred
00702   ros::spin();
00703 
00704   return EXIT_SUCCESS;
00705 }


rail_cv_project
Author(s): Russell Toris, David Kent, Adrian‎ Boteanu
autogenerated on Sat Dec 28 2013 17:31:25