hand_search.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/hand_search.h>
00002 
00003 
00004 std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud, 
00005   const Eigen::VectorXi& pts_cam_source, const std::vector<int>& indices, 
00006   const PointCloud::Ptr cloud_plot, bool calculates_antipodal, 
00007   bool uses_clustering)
00008 {
00009   // create KdTree for neighborhood search
00010         pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00011         kdtree.setInputCloud(cloud);
00012 
00013         cloud_normals_.resize(3, cloud->size());
00014         cloud_normals_.setZero(3, cloud->size());
00015 
00016         // calculate normals for all points
00017         if (calculates_antipodal)
00018         {
00019                 std::cout << "Calculating normals for all points\n";
00020                 nn_radius_taubin_ = 0.01;
00021                 std::vector<int> indices_cloud(cloud->size());
00022                 for (int i = 0; i < indices_cloud.size(); i++)
00023                         indices_cloud[i] = i;
00024                 findQuadrics(cloud, pts_cam_source, kdtree, indices_cloud);
00025                 nn_radius_taubin_ = 0.03;
00026         }
00027 
00028         // draw samples from the point cloud uniformly
00029         std::vector<int> indices_rand;
00030         Eigen::VectorXi hands_cam_source;
00031         if (indices.size() == 0)
00032         {
00033                 double t_rand = omp_get_wtime();
00034                 std::cout << "Generating uniform random indices ...\n";
00035                 indices_rand.resize(num_samples_);
00036                 pcl::RandomSample<pcl::PointXYZ> random_sample;
00037                 random_sample.setInputCloud(cloud);
00038                 random_sample.setSample(num_samples_);
00039                 random_sample.filter(indices_rand);
00040                 hands_cam_source.resize(num_samples_);
00041                 for (int i = 0; i < num_samples_; i++)
00042                         hands_cam_source(i) = pts_cam_source(indices_rand[i]);
00043                 std::cout << " Done in " << omp_get_wtime() - t_rand << " sec\n";
00044         }
00045         else
00046                 indices_rand = indices;
00047 
00048         if (plots_samples_)
00049                 plot_.plotSamples(indices_rand, cloud);
00050 
00051         // find quadrics
00052         std::cout << "Estimating local axes ...\n";
00053         std::vector<Quadric> quadric_list = findQuadrics(cloud, pts_cam_source, kdtree, indices_rand);
00054         if (plots_local_axes_)
00055                 plot_.plotLocalAxes(quadric_list, cloud_plot);
00056 
00057         // find hands
00058         std::cout << "Finding hand poses ...\n";
00059         std::vector<GraspHypothesis> hand_list = findHands(cloud, pts_cam_source, quadric_list, hands_cam_source, kdtree);
00060   
00061   return hand_list;
00062 }
00063 
00064 
00065 std::vector<Quadric> HandSearch::findQuadrics(const PointCloud::Ptr cloud,
00066         const Eigen::VectorXi& pts_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree,
00067         const std::vector<int>& indices)
00068 {
00069         double t1 = omp_get_wtime();
00070         std::vector<int> nn_indices;
00071         std::vector<float> nn_dists;
00072         std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
00073         T_cams.push_back(cam_tf_left_);
00074         T_cams.push_back(cam_tf_right_);
00075         std::vector<Quadric> quadric_list(indices.size());
00076 
00077 #ifdef _OPENMP // parallelization using OpenMP
00078 #pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_)
00079 #endif
00080         for (int i = 0; i < indices.size(); i++)
00081         {
00082                 const pcl::PointXYZ& sample = cloud->points[indices[i]];
00083 //    std::cout << "i: " << i << ", index: " << indices[i] << ", sample: " << sample << std::endl;
00084 
00085                 if (kdtree.radiusSearch(sample, nn_radius_taubin_, nn_indices, nn_dists) > 0)
00086                 {
00087                         Eigen::VectorXi nn_cam_source(nn_indices.size());
00088 //      std::cout << " Found " << nn_indices.size() << " neighbors.\n";
00089 
00090                         for (int j = 0; j < nn_cam_source.size(); j++)
00091                         {
00092                                 nn_cam_source(j) = pts_cam_source(nn_indices[j]);
00093                         }
00094 
00095                         Eigen::Vector3d sample_eigen = sample.getVector3fMap().cast<double>();
00096                         Quadric q(T_cams, cloud, sample_eigen, uses_determinstic_normal_estimation_);
00097                         q.fitQuadric(nn_indices);
00098 //      std::cout << " Fitted quadric\n";
00099                         q.findTaubinNormalAxis(nn_indices, nn_cam_source);
00100 //      std::cout << " Found local axes\n";
00101                         quadric_list[i] = q;
00102                         cloud_normals_.col(indices[i]) = q.getNormal();
00103                 }
00104         }
00105 
00106         double t2 = omp_get_wtime();
00107         std::cout << "Fitted " << quadric_list.size() << " quadrics in " << t2 - t1 << " sec.\n";
00108 
00109 //  quadric_list[0].print(); // debugging
00110 //  plot_.plotLocalAxes(quadric_list, cloud);
00111 
00112         return quadric_list;
00113 }
00114 
00115 
00116 std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud,
00117         const Eigen::VectorXi& pts_cam_source, const std::vector<Quadric>& quadric_list,
00118         const Eigen::VectorXi& hands_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree)
00119 {
00120         double t1 = omp_get_wtime();
00121         std::vector<int> nn_indices;
00122         std::vector<float> nn_dists;
00123         Eigen::Matrix3Xd nn_normals(3, nn_indices.size());
00124         Eigen::VectorXi nn_cam_source(nn_indices.size());
00125         Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size());
00126         std::vector<RotatingHand> hand_list(quadric_list.size());
00127 //  std::vector<RotatingHand> hand_list;
00128         double time_eval_hand = 0.0;
00129         double time_iter = 0.0;
00130         double time_nn = 0.0;
00131         double time_tf = 0.0;
00132 
00133         std::vector< std::vector<GraspHypothesis> > grasp_lists(quadric_list.size(), std::vector<GraspHypothesis>(0));
00134 
00135 #ifdef _OPENMP // parallelization using OpenMP
00136 #pragma omp parallel for private(nn_indices, nn_dists, nn_normals, nn_cam_source, centered_neighborhood) num_threads(num_threads_)
00137 #endif
00138         for (std::size_t i = 0; i < quadric_list.size(); i++)
00139         {
00140                 double timei = omp_get_wtime();
00141                 pcl::PointXYZ sample;
00142                 sample.x = quadric_list[i].getSample()(0);
00143                 sample.y = quadric_list[i].getSample()(1);
00144                 sample.z = quadric_list[i].getSample()(2);
00145 //    std::cout << "i: " << i << ", sample: " << sample << std::endl;
00146 
00147                 if (kdtree.radiusSearch(sample, nn_radius_hands_, nn_indices, nn_dists) > 0)
00148                 {
00149                         time_nn += omp_get_wtime() - timei;
00150                         nn_normals.setZero(3, nn_indices.size());
00151                         nn_cam_source.setZero(nn_indices.size());
00152                         centered_neighborhood.setZero(3, nn_indices.size());
00153 
00154                         for (int j = 0; j < nn_indices.size(); j++)
00155                         {
00156                                 nn_cam_source(j) = pts_cam_source(nn_indices[j]);
00157                                 centered_neighborhood.col(j) = (cloud->points[nn_indices[j]].getVector3fMap()
00158                                                 - sample.getVector3fMap()).cast<double>();
00159                                 nn_normals.col(j) = cloud_normals_.col(nn_indices[j]);
00160                         }
00161 
00162                         FingerHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_);
00163 
00164                         Eigen::Vector3d sample_eig = sample.getVector3fMap().cast<double>();
00165                         RotatingHand rotating_hand(cam_tf_left_.block<3, 1>(0, 3) - sample_eig,
00166                                 cam_tf_right_.block<3, 1>(0, 3) - sample_eig, finger_hand, tolerant_antipodal_, hands_cam_source(i));
00167                         const Quadric& q = quadric_list[i];
00168                         double time_tf1 = omp_get_wtime();
00169                         rotating_hand.transformPoints(centered_neighborhood, q.getNormal(), q.getCurvatureAxis(), nn_normals,
00170                                 nn_cam_source, hand_height_);
00171                         time_tf += omp_get_wtime() - time_tf1;
00172                         double time_eval1 = omp_get_wtime();
00173                         std::vector<GraspHypothesis> grasps = rotating_hand.evaluateHand(init_bite_, sample_eig, true);
00174                         time_eval_hand += omp_get_wtime() - time_eval1;
00175 
00176                         if (grasps.size() > 0)
00177                         {
00178                                 // grasp_list.insert(grasp_list.end(), grasps.begin(), grasps.end());
00179         grasp_lists[i] = grasps;
00180                         }
00181                 }
00182 
00183                 time_iter += omp_get_wtime() - timei;
00184         }
00185         time_eval_hand /= quadric_list.size();
00186         time_nn /= quadric_list.size();
00187         time_iter /= quadric_list.size();
00188         time_tf /= quadric_list.size();
00189         std::cout << " avg time for transforming point neighborhood: " << time_tf << " sec.\n";
00190         std::cout << " avg time for NN search: " << time_nn << " sec.\n";
00191         std::cout << " avg time for rotating_hand.evaluate(): " << time_eval_hand << " sec.\n";
00192         std::cout << " avg time per iteration: " << time_iter << " sec.\n";
00193   
00194   std::vector<GraspHypothesis> grasp_list;
00195   for (std::size_t i = 0; i < grasp_lists.size(); i++)
00196   {
00197     // std::cout << i << " " << grasp_lists[i].size() << "\n";
00198     if (grasp_lists[i].size() > 0)
00199       grasp_list.insert(grasp_list.end(), grasp_lists[i].begin(), grasp_lists[i].end());
00200   }
00201 
00202         double t2 = omp_get_wtime();
00203         std::cout << " Found " << grasp_list.size() << " robot hand poses in " << t2 - t1 << " sec.\n";
00204 
00205         return grasp_list;
00206 }


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