test.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/learning.h>
00002 #include <agile_grasp/localization.h>
00003 
00004 int main(int argc, char** argv)
00005 {
00006   if (argc > 2)
00007   {
00008     // read PCD filename from command line
00009     std::string pcd_file_name = argv[1];
00010     std::string file_name_left;
00011     std::string file_name_right;
00012         
00013     if (pcd_file_name.find(".pcd") == std::string::npos)
00014     {
00015                         file_name_left = pcd_file_name + "l_reg.pcd";
00016                         file_name_right = pcd_file_name + "r_reg.pcd";
00017                 }
00018                 else
00019                 {
00020                         file_name_left = pcd_file_name;
00021                         file_name_right = "";
00022                 }
00023 
00024     // read SVM filename from command line
00025     std::string svm_file_name = argv[2];
00026 
00027                 /* read number of samples, number of threads and min handle inliers
00028                  * from command line */
00029     int num_samples = 400;
00030     if (argc > 3)
00031       num_samples = atoi(argv[3]);
00032 
00033     int num_threads = 1;
00034     if (argc > 4)
00035       num_threads = atoi(argv[4]);
00036     
00037     int min_inliers = 3;
00038     if (argc > 5)
00039       min_inliers = atoi(argv[5]);
00040 
00041     double taubin_radius = 0.03;
00042     double hand_radius = 0.08;
00043         
00044                 // camera poses for 2-camera Baxter setup
00045     Eigen::Matrix4d base_tf, sqrt_tf;
00046 
00047     base_tf << 0, 0.445417, 0.895323, 0.215, 
00048                1, 0, 0, -0.015, 
00049                0, 0.895323, -0.445417, 0.23, 
00050                0, 0, 0, 1;
00051 
00052     sqrt_tf <<   0.9366,  -0.0162,  0.3500, -0.2863, 
00053                  0.0151,   0.9999,   0.0058,   0.0058, 
00054                 -0.3501, -0.0002, 0.9367, 0.0554, 
00055                  0,        0,      0,      1;
00056 
00057     // workspace dimensions
00058     Eigen::VectorXd workspace(6);
00059     //workspace << 0.4, 0.7, -0.02, 0.06, -0.2, 10;
00060     //workspace << 0.4, 1.0, -0.3, 0.3, -0.2, 2;
00061     //workspace << 0.55, 0.9, -0.35, 0.2, -0.2, 2;
00062     //workspace << 0.6, 0.8, -0.25, 0.1, -0.3, 2;
00063     workspace << 0.55, 0.95, -0.25, 0.07, -0.3, 1;
00064     // workspace << -10, 10, -10, 10, -10, 10;
00065 
00066         // set-up parameters for the hand search
00067     Localization loc(num_threads, true, true);
00068     loc.setCameraTransforms(base_tf * sqrt_tf.inverse(), base_tf * sqrt_tf);
00069     loc.setWorkspace(workspace);
00070     loc.setNumSamples(num_samples);
00071     loc.setNeighborhoodRadiusTaubin(taubin_radius);
00072     loc.setNeighborhoodRadiusHands(hand_radius);
00073     loc.setFingerWidth(0.01);
00074     loc.setHandOuterDiameter(0.09);
00075     loc.setHandDepth(0.06);
00076     loc.setInitBite(0.01);
00077     loc.setHandHeight(0.02);
00078     std::cout << "Localizing hands ...\n";
00079 
00080     // test with fixed set of indices
00081 //    std::vector<int> indices(5);
00082 //    indices[0] = 731;
00083 //    indices[1] = 4507;
00084 //    indices[2] = 4445;
00085 //    indices[3] = 2254;
00086 //    indices[4] = 3716;
00087 //    std::vector<RotatingHand> hand_list = loc.localizeHands(file_name_left, file_name_right, indices, svm_file_name);
00088 
00089 //    // test with randomly sampled indices
00090     std::vector<GraspHypothesis> hands = loc.localizeHands(file_name_left, file_name_right);
00091     std::vector<GraspHypothesis> antipodal_hands = loc.predictAntipodalHands(hands, svm_file_name);
00092     std::vector<Handle> handles = loc.findHandles(antipodal_hands, min_inliers, 0.005, true);
00093 
00094     return 0;
00095   }
00096 
00097   std::cout << "No PCD filename given!\n";
00098   std::cout << "Usage: test_svm pcd_filename svm_filename [num_samples] [num_threads] [min_handle_inliers]\n";
00099   std::cout << "Localize grasps in a *.pcd file using a trained SVM.\n";
00100   
00101   return (-1);
00102 }
00103 


agile_grasp
Author(s):
autogenerated on Thu Aug 27 2015 12:01:28