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


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