learning.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/learning.h>
00002 
00003 void Learning::trainBalanced(const std::vector<GraspHypothesis>& hands_list, const std::vector<int> & sizes,
00004         const std::string& file_name, const Eigen::Matrix3Xd& cam_pos, int max_positive, bool is_plotting)
00005 {
00006         std::vector<int> positives;
00007         std::vector<int> negatives;
00008   std::vector<int> indices_selected;
00009   std::vector<int> positives_sub;
00010         int k = 0;
00011   
00012   for (int i = 0; i < hands_list.size(); i++)
00013         {
00014     if (hands_list[i].isFullAntipodal())
00015       positives_sub.push_back(i);
00016     else if (!hands_list[i].isHalfAntipodal())
00017       negatives.push_back(i);
00018     
00019     if (i == sizes[k])
00020     {
00021       if (positives_sub.size() <= max_positive)
00022                         {
00023                                 positives.insert(positives.end(), positives_sub.begin(), positives_sub.end());
00024                         }
00025                         else // select <max_positive> positive examples randomly
00026                         {
00027                                 std::set<int> indices;
00028                                 while (indices.size() < max_positive)
00029                                         indices.insert(indices.end(), std::rand() % positives_sub.size());
00030         
00031         std::cout << positives.size() << " positive examples found\n";
00032         std::cout << " randomly selected indices:";
00033 
00034                                 for (std::set<int>::iterator it = indices.begin(); it != indices.end(); it++)
00035                                 {
00036           std::cout << " " << *it;
00037                                         positives.push_back(positives_sub[*it]);
00038                                 }
00039         
00040         std::cout << std::endl;
00041                         }
00042       
00043       positives_sub.resize(0);
00044       k++;
00045     }
00046   }
00047   
00048   indices_selected.insert(indices_selected.end(), positives.begin(), positives.end());
00049   std::set<int> indices;
00050   while (indices.size() < positives.size())
00051     indices.insert(indices.end(), std::rand() % negatives.size());
00052 
00053   for (std::set<int>::iterator it = indices.begin(); it != indices.end(); it++)
00054   {
00055     indices_selected.push_back(negatives[*it]);
00056   }
00057   
00058   std::cout << "size(positives): " << positives.size() << std::endl;
00059   std::cout << "indices_selected.size: " << indices_selected.size() << std::endl;
00060 
00061   std::vector<Instance> instances;
00062         for (int i = 0; i < indices_selected.size(); i++)
00063         {
00064     int idx = indices_selected[i];
00065     instances.push_back(createInstance(hands_list[idx], cam_pos));
00066       
00067     // add instances for simulated left and right camera
00068     instances.push_back(createInstance(hands_list[idx], cam_pos, 0));
00069     instances.push_back(createInstance(hands_list[idx], cam_pos, 1));
00070   }
00071 
00072         std::cout << "Converting " << instances.size() << " training examples (grasps) to images\n";
00073         convertData(instances, file_name, is_plotting);
00074 }
00075 
00076 void Learning::train(const std::vector<GraspHypothesis>& hands_list, const std::vector<int> & sizes,
00077         const std::string& file_name, const Eigen::Matrix3Xd& cam_pos, int max_positive, bool is_plotting)
00078 {  
00079   std::vector<int> positives;
00080   std::vector<Instance> instances;
00081         int k = 0;
00082 
00083         for (int i = 0; i < hands_list.size(); i++)
00084         {
00085                 if (hands_list[i].isFullAntipodal())
00086                 {
00087                         positives.push_back(i);
00088                 }
00089                 else if (!hands_list[i].isHalfAntipodal())
00090                 {
00091                         instances.push_back(createInstance(hands_list[i], cam_pos));
00092       
00093       // add instances for simulated left and right camera
00094       instances.push_back(createInstance(hands_list[i], cam_pos, 0));
00095       instances.push_back(createInstance(hands_list[i], cam_pos, 1));
00096                 }
00097 
00098                 if (i == sizes[k])
00099                 {
00100                         // std::cout << " i: " << i << " " << sizes[k] << std::endl;
00101                         if (positives.size() <= max_positive)
00102                         {
00103                                 for (int j = 0; j < positives.size(); j++)
00104         {
00105           instances.push_back(createInstance(hands_list[positives[j]], cam_pos));
00106       
00107           // add instances for simulated left and right camera
00108           instances.push_back(createInstance(hands_list[positives[j]], cam_pos, 0));
00109           instances.push_back(createInstance(hands_list[positives[j]], cam_pos, 1));
00110         }
00111                         }
00112                         else // select <max_positive> positive examples randomly
00113                         {
00114                                 std::set<int> indices;
00115                                 while (indices.size() < max_positive)
00116                                         indices.insert(indices.end(), std::rand() % positives.size());
00117         
00118         std::cout << positives.size() << " positive examples found\n";
00119         std::cout << " randomly selected indices:";
00120 
00121                                 for (std::set<int>::iterator it = indices.begin(); it != indices.end(); it++)
00122                                 {
00123                                         std::cout << " " << *it;
00124                                         instances.push_back(createInstance(hands_list[positives[*it]], cam_pos));
00125       
00126           // add instances for simulated left and right camera
00127           instances.push_back(createInstance(hands_list[positives[*it]], cam_pos, 0));
00128           instances.push_back(createInstance(hands_list[positives[*it]], cam_pos, 1));
00129                                 }
00130         
00131         std::cout << std::endl;
00132                         }
00133                         
00134       positives.resize(0);
00135                         k++;
00136                 }
00137         }
00138 
00139         std::cout << "Converting " << instances.size() << " training examples (grasps) to images\n";
00140         convertData(instances, file_name, is_plotting);
00141 }
00142 
00143 void Learning::train(const std::vector<GraspHypothesis>& hands_list, const std::string& file_name,
00144         const Eigen::Matrix3Xd& cam_pos, bool is_plotting)
00145 {
00146         std::vector<Instance> instances;
00147 
00148         for (int i = 0; i < hands_list.size(); i++)
00149         {
00150                 // do not use grasp if it's half-antipodal
00151                 if (!hands_list[i].isHalfAntipodal() || hands_list[i].isFullAntipodal())
00152                 {
00153       instances.push_back(createInstance(hands_list[i], cam_pos));
00154       
00155       // add instances for simulated left and right camera
00156       instances.push_back(createInstance(hands_list[i], cam_pos, 0));
00157       instances.push_back(createInstance(hands_list[i], cam_pos, 1));
00158                 }
00159         }
00160 
00161         std::cout << "Converting " << instances.size() << " training examples (grasps) to images\n";
00162         convertData(instances, file_name, is_plotting);
00163 }
00164 
00165 std::vector<GraspHypothesis> Learning::classify(const std::vector<GraspHypothesis>& hands_list,
00166         const std::string& svm_filename, const Eigen::Matrix3Xd& cam_pos, bool is_plotting)
00167 {
00168         std::cout << "Predicting ...\n";
00169   std::vector<GraspHypothesis> antipodal_hands(0);
00170         
00171         // check if SVM file exists
00172         ifstream f(svm_filename.c_str());
00173         if (!f.good()) 
00174         {
00175                 f.close();
00176                 std::cout << " Error: File " << svm_filename << " does not exist!\n";
00177                 return antipodal_hands;
00178         }
00179                 
00180         // load the SVM model from the file
00181         CvSVM svm;
00182         double t0 = omp_get_wtime();
00183         try
00184         {
00185                 svm.load(svm_filename.c_str());
00186         }
00187         catch (cv::Exception& e)
00188         {
00189                 std::cout << " Exception: " << e.msg << "\n";
00190                 return antipodal_hands;
00191         }
00192         std::cout << " time for loading SVM: " << omp_get_wtime() - t0 << "\n";
00193   std::cout << " # of support vectors: " << svm.get_support_vector_count() << "\n";
00194         cv::HOGDescriptor hog;
00195         hog.winSize = cv::Size(64, 64);
00196         std::vector<bool> is_antipodal(hands_list.size());
00197 
00198 #ifdef _OPENMP // parallelization using OpenMP
00199 #pragma omp parallel for num_threads(num_threads_)
00200 #endif
00201         for (int i = 0; i < hands_list.size(); i++)
00202         {
00203                 const Eigen::Vector3d& source = cam_pos.col(hands_list[i].getCamSource());
00204                 Eigen::Vector3d source_to_center = hands_list[i].getGraspSurface() - source;
00205 
00206                 // convert grasp to image
00207                 cv::Mat image = convertToImage(createInstance(hands_list[i], cam_pos));
00208 
00209                 if (is_plotting)
00210                 {
00211                         cv::namedWindow("Grasp Image", cv::WINDOW_NORMAL); // Create a window for display.
00212                         cv::imshow("Grasp Image", image); // Show our image inside it.
00213                         cv::waitKey(0); // Wait for a keystroke in the window
00214                 }
00215 
00216                 // extract HOG features from image
00217                 //    hog.cellSize = cv::Size(8,8);
00218                 std::vector<float> descriptors;
00219                 std::vector<cv::Point> locations;
00220                 hog.compute(image, descriptors, cv::Size(32, 32), cv::Size(0, 0), locations);
00221 
00222                 cv::Mat features(1, hog.getDescriptorSize() * 2, CV_32FC1);
00223                 for (int k = 0; k < descriptors.size(); k++)
00224                         features.at<float>(k) = descriptors[k];
00225                 float prediction = svm.predict(features);
00226                 if (prediction == 1)
00227                 {
00228                         GraspHypothesis grasp = hands_list[i];
00229                         grasp.setFullAntipodal(true);
00230       is_antipodal[i] = true;
00231                 }
00232     else
00233       is_antipodal[i] = false;
00234         }
00235 
00236   for (int i = 0; i < is_antipodal.size(); i++)
00237   {
00238     if (is_antipodal[i])
00239     {
00240       antipodal_hands.push_back(hands_list[i]);
00241       antipodal_hands[antipodal_hands.size()-1].setFullAntipodal(true);
00242     }
00243   }
00244 
00245   std::cout << " " << antipodal_hands.size() << " antipodal grasps found.\n";
00246         return antipodal_hands;
00247 }
00248 
00249 void Learning::convertData(const std::vector<Instance>& instances, 
00250   const std::string& file_name, bool is_plotting, 
00251   bool uses_linear_kernel)
00252 {
00253         std::vector<cv::Mat> image_list;
00254         cv::HOGDescriptor hog;
00255         hog.winSize = cv::Size(64, 64);
00256         cv::Mat features(instances.size(), hog.getDescriptorSize() * 2, CV_32FC1);
00257         cv::Mat labels(instances.size(), 1, CV_32FC1);
00258         int num_positives = 0;
00259 
00260         for (int i = 0; i < instances.size(); i++)
00261         {
00262                 // convert grasp to image
00263 //              std::cout << "i: " << i << "\n";
00264                 cv::Mat image = convertToImage(instances[i]);
00265 //              std::cout << " Converted grasp data to image\n";
00266                 image_list.push_back(image);
00267 
00268                 // visualize grasp image
00269                 if (is_plotting)
00270                 {
00271                         cv::namedWindow("Grasp Image", cv::WINDOW_NORMAL); // Create a window for display.
00272                         cv::imshow("Grasp Image", image); // Show our image inside it.
00273                         cv::waitKey(0); // Wait for a keystroke in the window
00274                 }
00275 
00276                 // extract HOG features from image
00277 //    hog.cellSize = cv::Size(8,8);
00278                 std::vector<float> descriptors;
00279                 std::vector<cv::Point> locations;
00280 //              std::cout << "HOG descriptor size is " << hog.getDescriptorSize() << std::endl;
00281                 hog.compute(image, descriptors, cv::Size(32, 32), cv::Size(0, 0), locations);
00282 //              std::cout << "HOG descriptor size is " << hog.getDescriptorSize() << std::endl;
00283 //              std::cout << "# descriptors = " << descriptors.size() << std::endl;
00284                 for (int j = 0; j < features.cols; ++j)
00285                 {
00286                         features.at<float>(i, j) = descriptors[j];
00287                 }
00288                 if (instances[i].label == 1)
00289                 {
00290                         labels.at<float>(i, 0) = 1.0;
00291                         num_positives++;
00292                 }
00293                 else
00294                         labels.at<float>(i, 0) = -1.0;
00295         }
00296 
00297   // train the SVM
00298         CvSVMParams params;
00299   // cv::Mat weights(1, 2, CV_32FC1);
00300   // weights.at<float>(0,0) = 0.9;
00301   // weights.at<float>(0,1) = 0.1;
00302   // CvMat cv1_weights = weights;  
00303   // params.class_weights = &cv1_weights;
00304         params.svm_type = CvSVM::C_SVC;
00305   if (uses_linear_kernel)
00306     params.kernel_type = CvSVM::LINEAR;
00307   else
00308   {
00309     params.kernel_type = CvSVM::POLY;
00310     params.degree = 2;
00311         }
00312   CvSVM svm;
00313         svm.train(features, labels, cv::Mat(), cv::Mat(), params);
00314         svm.save(file_name.c_str());
00315         std::cout << "# training examples: " << features.rows << " (# positives: " << num_positives
00316                         << ", # negatives: " << features.rows - num_positives << ")\n";
00317         std::cout << "Saved trained SVM as " << file_name << "\n";
00318 }
00319 
00320 cv::Mat Learning::convertToImage(const Instance& ins)
00321 {
00322         const double HORIZONTAL_LIMITS[2] = { -0.05, 0.05 };
00323         const double VERTICAL_LIMITS[2] = { 0.0, 0.08 };
00324         double cell_size = (HORIZONTAL_LIMITS[1] - HORIZONTAL_LIMITS[0]) / (double) num_horizontal_cells_;
00325 
00326         Eigen::VectorXi horizontal_cells(ins.pts.cols());
00327         Eigen::VectorXi vertical_cells(ins.pts.cols());
00328 
00329   // reverse x-direction to keep orientation consistent
00330         if (ins.binormal.dot(ins.source_to_center) > 0)
00331                 horizontal_cells = floorVector((ins.pts.row(0).array() - HORIZONTAL_LIMITS[0]) / cell_size);
00332         else
00333                 horizontal_cells = floorVector((-ins.pts.row(0).array() - HORIZONTAL_LIMITS[0]) / cell_size);
00334 
00335         vertical_cells = floorVector((ins.pts.row(1).array() - VERTICAL_LIMITS[0]) / cell_size);
00336 
00337         std::set<Eigen::Vector2i, UniqueVectorComparator> cells;
00338         for (int i = 0; i < ins.pts.cols(); i++)
00339         {
00340                 Eigen::Vector2i c;
00341                 c << horizontal_cells(i), vertical_cells(i);
00342                 cells.insert(c);
00343         }
00344 
00345         Eigen::Matrix2Xi cells_mat(2, cells.size());
00346         int i = 0;
00347         cv::Mat image(num_vertical_cells_, num_horizontal_cells_, CV_8UC1);
00348         image.setTo(0);
00349 
00350         for (std::set<Eigen::Vector2i, UniqueVectorComparator>::iterator it = cells.begin(); it != cells.end();
00351                         it++)
00352         {
00353                 Eigen::Vector2i c = *it;
00354                 cells_mat(0, i) = std::max(0, c(0));
00355                 cells_mat(1, i) = std::max(0, c(1));
00356 
00357                 c = cells_mat.col(i);
00358                 cells_mat(0, i) = std::min(num_horizontal_cells_ - 1, c(0));
00359                 cells_mat(1, i) = std::min(num_vertical_cells_ - 1, c(1));
00360                 image.at<uchar>(image.rows - 1 - cells_mat(1, i), cells_mat(0, i)) = 255;
00361                 i++;
00362         }
00363 
00364         return image;
00365 }
00366 
00367 Eigen::VectorXi Learning::floorVector(const Eigen::VectorXd& a)
00368 {
00369         Eigen::VectorXi b(a.size());
00370         for (int i = 0; i < b.size(); i++)
00371                 b(i) = floor(a(i));
00372         return b;
00373 }
00374 
00375 Learning::Instance Learning::createInstance(const GraspHypothesis& h, const Eigen::Matrix3Xd& cam_pos, int cam)
00376 {
00377   Instance ins;
00378   ins.binormal = h.getBinormal();
00379   ins.label = h.isFullAntipodal();
00380   
00381   // calculate camera position to center vector
00382   const Eigen::Vector3d& source = cam_pos.col(h.getCamSource());
00383   ins.source_to_center = h.getGraspSurface() - source;
00384   
00385   if (cam == -1)
00386   {
00387     ins.pts = h.getPointsForLearning();
00388   }
00389   else
00390   {
00391     const std::vector<int>& indices_cam = (cam == 0) ? h.getIndicesPointsForLearningCam1() : h.getIndicesPointsForLearningCam2();
00392     ins.pts.resize(3, indices_cam.size());
00393     for (int i = 0; i < indices_cam.size(); i++)
00394     {
00395       ins.pts.col(i) = h.getPointsForLearning().col(indices_cam[i]);
00396     } 
00397   }
00398 
00399   return ins;
00400 }


agile_grasp
Author(s): Andreas ten Pas
autogenerated on Sat Jun 8 2019 20:08:27