Exemplo n.º 1
0
void InitialGuess::displayCorrespondances(pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints1, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints2, pcl::CorrespondencesPtr corres,pcl::CorrespondencesPtr inlier)
{		
	
	for(int i=1;i<corres->size();i++)
	{
		pcl::PointXYZI srcpt=keypoints1->points.at(corres->at(i).index_query);              //corres[i].index_query);
		 pcl::PointXYZI tgtpt=keypoints2->points.at(corres->at(i).index_match);
		 std::stringstream ss2 ("line_vp_2_");
		 ss2<<i;
		 p->addLine<pcl::PointXYZI>(srcpt,tgtpt,ss2.str(),vp_2);
	}

	for(int i=1;i<inlier->size();i++)
	{
		 pcl::PointXYZI srcpt=keypoints1->points.at(inlier->at(i).index_query);
		 pcl::PointXYZI tgtpt=keypoints2->points.at(inlier->at(i).index_match);
		 std::stringstream ss3 ("line_vp_3_");
		 ss3<<i;
		 p->addLine<pcl::PointXYZI>(srcpt,tgtpt,ss3.str(),vp_3);
	}
	p->spin();
}
Exemplo n.º 2
0
void cluster(const pcl::CorrespondencesPtr &object_world_corrs)
{
	ros::NodeHandle nh_param("~");
	nh_param.param<double>("cg_size" , cg_size_ , 0.01 );
	nh_param.param<double>("cg_thresh", cg_thresh_, 5.0);

	//
	// Debug output
	//
	PointCloud correspondence_object;
	PointCloud correspondence_world;
	for (int j = 0; j < object_world_corrs->size (); ++j)
  {
    PointType& object_point = object_keypoints->at(object_world_corrs->at(j).index_query);
    PointType& world_point = world_keypoints->at(object_world_corrs->at(j).index_match);

		correspondence_object.push_back(object_point);
		correspondence_world.push_back(world_point);

		
		// cout << object_point.x << " " << object_point.y << " " <<  object_point.z << endl;
		// cout << world_point.x << " " << world_point.y << " " <<  world_point.z << endl;
  }

	PointCloudROS pub_me_object2;
	PointCloudROS pub_me_world2;
	toROSMsg(correspondence_object, pub_me_object2);
	toROSMsg(correspondence_world, pub_me_world2);
	pub_me_object2.header.frame_id = "/object";
	pub_me_world2.header.frame_id = "/world";
	pub_object2.publish(pub_me_object2);
	pub_world2.publish(pub_me_world2);

  //
  //  Actual Clustering
  //
	cout << "... clustering ..." << endl;    
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
  std::vector<pcl::Correspondences> clustered_corrs;
    
	pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
	gc_clusterer.setGCSize (cg_size_);
	gc_clusterer.setGCThreshold (cg_thresh_);

	gc_clusterer.setInputCloud (object_keypoints);
	gc_clusterer.setSceneCloud (world_keypoints);
	gc_clusterer.setModelSceneCorrespondences (object_world_corrs);

	gc_clusterer.recognize (rototranslations, clustered_corrs);

    
  //
  //  Output results
  //
  std::cout << "Object instances found: " << rototranslations.size () << std::endl;
	int maximum = 0;
	int best;
	for (int i = 0; i < rototranslations.size (); ++i)
	{
		cout << "Instance "<< i << " has " << clustered_corrs[i].size () << " correspondences" << endl;
		if (maximum < clustered_corrs[i].size ())
		{
			maximum = clustered_corrs[i].size ();
			best = i;
		}
	}

  if (rototranslations.size () > 0)
  {
		cout << "selecting instance " << best << " and calculating TF" << endl;

    // Print the rotation matrix and translation vector
    Eigen::Matrix3f rotation = rototranslations[best].block<3,3>(0, 0);
    Eigen::Vector3f translation = rototranslations[best].block<3,1>(0, 3);
    
    printf ("\n");
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
    printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
    printf ("\n");
    printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));

		// convert Eigen matricies into ROS TF message
		tf::Vector3 object_offset;
		tf::Quaternion object_rotation;
		object_offset[0] = translation (0);
		object_offset[1] = translation (1);
		object_offset[2] = translation (2);
		// convert rotation matrix to quaternion
		Eigen::Quaternionf quaternion (rotation);
		object_rotation[0] = quaternion.x();
		object_rotation[1] = quaternion.y();
		object_rotation[2] = quaternion.z();
		object_rotation[3] = quaternion.w();

		static tf::TransformBroadcaster br;
		tf::Transform transform;
		transform.setOrigin (object_offset);
		transform.setRotation (object_rotation);
		br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "object"));

		//
		// Debug output
		//
		PointCloud correspondence_object_cluster;
		PointCloud correspondence_world_cluster;
		
		for (int j = 0; j < clustered_corrs[best].size (); ++j)
    {
      PointType& model_point = object_keypoints->at(clustered_corrs[best][j].index_query);
      PointType& scene_point = world_keypoints->at(clustered_corrs[best][j].index_match);
			correspondence_object_cluster.push_back(model_point);
			correspondence_world_cluster.push_back(scene_point);
			//cout << model_point.x << " " << model_point.y << " " <<  model_point.z << endl;
			//cout << scene_point.x << " " <<  scene_point.y << " " <<  scene_point.z << endl;
    }

		PointCloudROS pub_me_object;
		PointCloudROS pub_me_world;
		toROSMsg(correspondence_object_cluster, pub_me_object);
		toROSMsg(correspondence_world_cluster, pub_me_world);
		pub_me_object.header.frame_id = "/object";
		pub_me_world.header.frame_id = "/world";
		pub_object.publish(pub_me_object);
		pub_world.publish(pub_me_world);
  }
}