hands_test.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/quadric.h>
00002 #include <agile_grasp/rotating_hand.h>
00003 #include <agile_grasp/finger_hand.h>
00004 #include <agile_grasp/grasp_hypothesis.h>
00005 
00006 #include <pcl/io/pcd_io.h>
00007 #include <pcl/point_types.h>
00008 #include <pcl/search/organized.h>
00009 #include <pcl/kdtree/kdtree_flann.h>
00010 
00011 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
00012 
00013 int main(int argc, char** argv)
00014 {
00015   double taubin_radius = 0.03; // radius of curvature-estimation neighborhood
00016   double hand_radius = 0.08; // radius of hand configuration neighborhood
00017 
00018 //  std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
00019   std::string file = "/home/andreas/data/mlaffordance/training/rect31l_reg.pcd";
00020   PointCloud::Ptr cloud(new PointCloud);
00021   if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(file, *cloud) == -1) //* load the file
00022   {
00023     PCL_ERROR("Couldn't read input PCD file\n");
00024     return (-1);
00025   }
00026   
00027   pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00028     new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00029   std::vector<int> nn_indices;
00030   std::vector<float> nn_dists;
00031   pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGBA>());
00032 
00033 //  int sample_index = 0;
00034   int sample_index = 731;
00035   if (cloud->isOrganized())
00036   {
00037     organized_neighbor->setInputCloud(cloud);
00038     organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
00039   }
00040   else
00041   {
00042     tree->setInputCloud(cloud);
00043     tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
00044   }
00045  
00046   std::cout << "Found point neighborhood for sample " << sample_index << "\n";
00047 
00048   Eigen::Matrix4d T_base, T_sqrt;
00049   T_base << 0, 0.445417, 0.895323, 0.22, 1, 0, 0, -0.02, 0, 0.895323, -0.445417, 0.24, 0, 0, 0, 1;
00050 
00051   T_sqrt << 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;
00052 
00053   std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
00054   T_cams.push_back(T_base * T_sqrt.inverse());
00055   std::cout << T_cams[0] << std::endl;
00056 
00057   // fit quadric
00058   Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
00059   Quadric quadric(T_cams, cloud, sample, true);
00060   quadric.fitQuadric(nn_indices);
00061 
00062   Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
00063   quadric.findTaubinNormalAxis(nn_indices, cam_source);
00064 
00065   quadric.print();
00066 
00067   // fit hand
00068   tree->radiusSearch(cloud->points[sample_index], hand_radius, nn_indices, nn_dists);
00069 
00070   Eigen::VectorXi pts_cam_source = Eigen::VectorXi::Ones(1, cloud->size());
00071 
00072   Eigen::Matrix3Xd nn_normals(3, nn_indices.size());
00073   Eigen::VectorXi nn_cam_source(nn_indices.size());
00074   Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size());
00075   nn_normals.setZero();
00076 
00077   for (int j = 0; j < nn_indices.size(); j++)
00078   {
00079     nn_cam_source(j) = pts_cam_source(nn_indices[j]);
00080     centered_neighborhood.col(j) = cloud->points[nn_indices[j]].getVector3fMap().cast<double>() - sample;
00081   }
00082 
00083   double finger_width_ = 0.01;
00084   double hand_outer_diameter_ = 0.09;
00085   double hand_depth_ = 0.06;
00086 
00087   FingerHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_);
00088 
00089   double hand_height_ = 0.02;
00090   double init_bite_ = 0.01;
00091 
00092   RotatingHand rotating_hand(T_cams[0].block(0, 3, 3, 1) - sample, 
00093     T_cams[0].block(0, 3, 3, 1) - sample, finger_hand, true, pts_cam_source(sample_index));
00094   rotating_hand.transformPoints(centered_neighborhood, quadric.getNormal(), quadric.getCurvatureAxis(), nn_normals, nn_cam_source,
00095                                 hand_height_);
00096   std::vector<GraspHypothesis> h = rotating_hand.evaluateHand(init_bite_, sample, 1);
00097   for (int i = 0; i < h.size(); i++)
00098   {
00099         std::cout << "-- orientation " << i << " --\n";
00100                 h[i].print();
00101         }
00102 
00103   // std::cout << "\n";
00104   // rotating_hand.evaluateHand(init_bite_, sample, 1);
00105   // rotating_hand.print();
00106 
00107   return 0;
00108 }


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