Exemplo n.º 1
0
std::vector<Quadric> HandSearch::findQuadrics(const PointCloud::Ptr cloud,
	const Eigen::VectorXi& pts_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree,
	const std::vector<int>& indices)
{
	double t1 = omp_get_wtime();
	std::vector<int> nn_indices;
	std::vector<float> nn_dists;
	std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
	T_cams.push_back(cam_tf_left_);
	T_cams.push_back(cam_tf_right_);
	std::vector<Quadric> quadric_list(indices.size());

#ifdef _OPENMP // parallelization using OpenMP
#pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_)
#endif
	for (int i = 0; i < indices.size(); i++)
	{
		const pcl::PointXYZ& sample = cloud->points[indices[i]];
//    std::cout << "i: " << i << ", index: " << indices[i] << ", sample: " << sample << std::endl;

		if (kdtree.radiusSearch(sample, nn_radius_taubin_, nn_indices, nn_dists) > 0)
		{
			Eigen::VectorXi nn_cam_source(nn_indices.size());
//      std::cout << " Found " << nn_indices.size() << " neighbors.\n";

			for (int j = 0; j < nn_cam_source.size(); j++)
			{
				nn_cam_source(j) = pts_cam_source(nn_indices[j]);
			}

			Eigen::Vector3d sample_eigen = sample.getVector3fMap().cast<double>();
			Quadric q(T_cams, cloud, sample_eigen, uses_determinstic_normal_estimation_);
			q.fitQuadric(nn_indices);
//      std::cout << " Fitted quadric\n";
			q.findTaubinNormalAxis(nn_indices, nn_cam_source);
//      std::cout << " Found local axes\n";
			quadric_list[i] = q;
			cloud_normals_.col(indices[i]) = q.getNormal();
		}
	}

	double t2 = omp_get_wtime();
	//std::cout << "Fitted " << quadric_list.size() << " quadrics in " << t2 - t1 << " sec.\n";

//  quadric_list[0].print(); // debugging
//  plot_.plotLocalAxes(quadric_list, cloud);

	return quadric_list;
}
Exemplo n.º 2
0
int main(int argc, char** argv)
{
  double taubin_radius = 0.03; // radius of curvature-estimation neighborhood
  double hand_radius = 0.08; // radius of hand configuration neighborhood

//  std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
  std::string file = "/home/andreas/data/mlaffordance/training/rect31l_reg.pcd";
  PointCloud::Ptr cloud(new PointCloud);
  if (pcl::io::loadPCDFile<pcl::PointXYZ>(file, *cloud) == -1) //* load the file
  {
    PCL_ERROR("Couldn't read input PCD file\n");
    return (-1);
  }
  
  pcl::search::OrganizedNeighbor<pcl::PointXYZ>::Ptr organized_neighbor(
    new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>());

//  int sample_index = 0;
  int sample_index = 731;
  if (cloud->isOrganized())
  {
    organized_neighbor->setInputCloud(cloud);
    organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
  }
  else
  {
    tree->setInputCloud(cloud);
    tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
  }
 
  std::cout << "Found point neighborhood for sample " << sample_index << "\n";

  Eigen::Matrix4d T_base, T_sqrt;
  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;

  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;

  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
  T_cams.push_back(T_base * T_sqrt.inverse());
  std::cout << T_cams[0] << std::endl;

  // fit quadric
  Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
  Quadric quadric(T_cams, cloud, sample, true);
  quadric.fitQuadric(nn_indices);

  Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
  quadric.findTaubinNormalAxis(nn_indices, cam_source);

  quadric.print();

  // fit hand
  tree->radiusSearch(cloud->points[sample_index], hand_radius, nn_indices, nn_dists);

  Eigen::VectorXi pts_cam_source = Eigen::VectorXi::Ones(1, cloud->size());

  Eigen::Matrix3Xd nn_normals(3, nn_indices.size());
  Eigen::VectorXi nn_cam_source(nn_indices.size());
  Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size());
  nn_normals.setZero();

  for (int j = 0; j < nn_indices.size(); j++)
  {
    nn_cam_source(j) = pts_cam_source(nn_indices[j]);
    centered_neighborhood.col(j) = cloud->points[nn_indices[j]].getVector3fMap().cast<double>() - sample;
  }

  double finger_width_ = 0.01;
  double hand_outer_diameter_ = 0.09;
  double hand_depth_ = 0.06;

  ParallelHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_);

  double hand_height_ = 0.02;
  double init_bite_ = 0.01;

  RotatingHand rotating_hand(T_cams[0].block(0, 3, 3, 1) - sample, 
    T_cams[0].block(0, 3, 3, 1) - sample, finger_hand, true, pts_cam_source(sample_index));
  rotating_hand.transformPoints(centered_neighborhood, quadric.getNormal(), quadric.getCurvatureAxis(), nn_normals, nn_cam_source,
                                hand_height_);
  std::vector<GraspHypothesis> h = rotating_hand.evaluateHand(init_bite_, sample, 1);
  for (int i = 0; i < h.size(); i++)
  {
  	std::cout << "-- orientation " << i << " --\n";
		h[i].print();
	}

  // std::cout << "\n";
  // rotating_hand.evaluateHand(init_bite_, sample, 1);
  // rotating_hand.print();

  return 0;
}
Exemplo n.º 3
0
std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud,
	const Eigen::VectorXi& pts_cam_source, const std::vector<Quadric>& quadric_list,
	const Eigen::VectorXi& hands_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree)
{
	double t1 = omp_get_wtime();
	std::vector<int> nn_indices;
	std::vector<float> nn_dists;
	Eigen::Matrix3Xd nn_normals(3, nn_indices.size());
	Eigen::VectorXi nn_cam_source(nn_indices.size());
	Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size());
	std::vector<RotatingHand> hand_list(quadric_list.size());
//  std::vector<RotatingHand> hand_list;
	double time_eval_hand = 0.0;
	double time_iter = 0.0;
	double time_nn = 0.0;
	double time_tf = 0.0;

	std::vector< std::vector<GraspHypothesis> > grasp_lists(quadric_list.size(), std::vector<GraspHypothesis>(0));

#ifdef _OPENMP // parallelization using OpenMP
#pragma omp parallel for private(nn_indices, nn_dists, nn_normals, nn_cam_source, centered_neighborhood) num_threads(num_threads_)
#endif
	for (std::size_t i = 0; i < quadric_list.size(); i++)
	{
		double timei = omp_get_wtime();
		pcl::PointXYZ sample;
		sample.x = quadric_list[i].getSample()(0);
		sample.y = quadric_list[i].getSample()(1);
		sample.z = quadric_list[i].getSample()(2);
//    std::cout << "i: " << i << ", sample: " << sample << std::endl;

		if (kdtree.radiusSearch(sample, nn_radius_hands_, nn_indices, nn_dists) > 0)
		{
			time_nn += omp_get_wtime() - timei;
			nn_normals.setZero(3, nn_indices.size());
			nn_cam_source.setZero(nn_indices.size());
			centered_neighborhood.setZero(3, nn_indices.size());

			for (int j = 0; j < nn_indices.size(); j++)
			{
				nn_cam_source(j) = pts_cam_source(nn_indices[j]);
				centered_neighborhood.col(j) = (cloud->points[nn_indices[j]].getVector3fMap()
						- sample.getVector3fMap()).cast<double>();
				nn_normals.col(j) = cloud_normals_.col(nn_indices[j]);
			}

			FingerHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_);

			Eigen::Vector3d sample_eig = sample.getVector3fMap().cast<double>();
			RotatingHand rotating_hand(cam_tf_left_.block<3, 1>(0, 3) - sample_eig,
				cam_tf_right_.block<3, 1>(0, 3) - sample_eig, finger_hand, tolerant_antipodal_, hands_cam_source(i));
			const Quadric& q = quadric_list[i];
			double time_tf1 = omp_get_wtime();
			rotating_hand.transformPoints(centered_neighborhood, q.getNormal(), q.getCurvatureAxis(), nn_normals,
				nn_cam_source, hand_height_);
			time_tf += omp_get_wtime() - time_tf1;
			double time_eval1 = omp_get_wtime();
			std::vector<GraspHypothesis> grasps = rotating_hand.evaluateHand(init_bite_, sample_eig, true);
			time_eval_hand += omp_get_wtime() - time_eval1;

			if (grasps.size() > 0)
			{
				// grasp_list.insert(grasp_list.end(), grasps.begin(), grasps.end());
        grasp_lists[i] = grasps;
			}
		}

		time_iter += omp_get_wtime() - timei;
	}
	time_eval_hand /= quadric_list.size();
	time_nn /= quadric_list.size();
	time_iter /= quadric_list.size();
	time_tf /= quadric_list.size();
	//std::cout << " avg time for transforming point neighborhood: " << time_tf << " sec.\n";
	//std::cout << " avg time for NN search: " << time_nn << " sec.\n";
	//std::cout << " avg time for rotating_hand.evaluate(): " << time_eval_hand << " sec.\n";
	//std::cout << " avg time per iteration: " << time_iter << " sec.\n";
  
  std::vector<GraspHypothesis> grasp_list;
  for (std::size_t i = 0; i < grasp_lists.size(); i++)
  {
    // std::cout << i << " " << grasp_lists[i].size() << "\n";
    if (grasp_lists[i].size() > 0)
      grasp_list.insert(grasp_list.end(), grasp_lists[i].begin(), grasp_lists[i].end());
  }

	double t2 = omp_get_wtime();
	//std::cout << " Found " << grasp_list.size() << " robot hand poses in " << t2 - t1 << " sec.\n";

	return grasp_list;
}