antipodal_test.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/localization.h>
00002 
00003 #include <Eigen/Dense>
00004 
00005 #include <stdlib.h>
00006 
00007 
00008 int main(int argc, char** argv)
00009 {
00010   if (argc > 1)
00011   {
00012     std::string file_name = argv[1];
00013     std::string file_name_left = file_name + "l_reg.pcd";
00014     std::string file_name_right = file_name + "r_reg.pcd";
00015     
00016     int num_samples = 1000;
00017     if (argc > 2)
00018       num_samples = atoi(argv[2]);
00019     
00020     int num_threads = 1;
00021     if (argc > 3)
00022         num_threads = atoi(argv[3]);
00023 
00024     double taubin_radius = 0.03;
00025     double hand_radius = 0.08;
00026 
00027     Eigen::Matrix4d base_tf, sqrt_tf;
00028 
00029     base_tf << 0,  0.445417,  0.895323,   0.21,
00030                1,         0,         0,  -0.02,
00031                0,  0.895323, -0.445417,   0.24,
00032                0,         0,         0,   1;
00033 
00034     sqrt_tf << 0.9366,  -0.0162,  0.3500,  -0.2863,
00035                0.0151,   0.9999,  0.0058,   0.0058,
00036               -0.3501,  -0.0002,  0.9367,   0.0554,
00037                     0,        0,       0,        1;
00038 
00039     Eigen::VectorXd workspace(6);
00040     workspace << 0.4, 0.7, -0.02, 0.06, -0.2, 10;
00041 
00042     Localization loc(num_threads, false, true);
00043     loc.setCameraTransforms(base_tf * sqrt_tf.inverse(), base_tf * sqrt_tf);
00044     loc.setWorkspace(workspace);
00045     loc.setNumSamples(num_samples);
00046     loc.setNeighborhoodRadiusTaubin(taubin_radius);
00047     loc.setNeighborhoodRadiusHands(hand_radius);
00048     loc.setFingerWidth(0.01);
00049     loc.setHandOuterDiameter(0.09);
00050     loc.setHandDepth(0.06);
00051     loc.setInitBite(0.01);
00052     loc.setHandHeight(0.02);
00053     std::cout << "Localizing hands ...\n";
00054 
00055     std::vector<int> indices(5);
00056     indices[0] = 731;
00057     indices[1] = 4507;
00058     indices[2] = 4445;
00059     indices[3] = 2254;
00060     indices[4] = 3716;
00061     loc.localizeHands(file_name_left, file_name_right, indices, true);
00062 
00063                 // std::vector<int> indices(0);
00064                 // loc.localizeHands(file_name_left, file_name_right, indices, "". true);
00065 
00066                 // loc.voxelizeCloud(file_name_left, file_name_right);
00067 
00068     return 0;
00069   }
00070   
00071   std::cout << "No PCD filename given!\n";
00072   std::cout << "Usage: test_local_axes filename [num_samples]\n";
00073   return (-1);
00074 }


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