Example #1
0
    Eigen::Matrix4f CloudMapper::ComputeInitialAlignment(Cloud::Ptr &cloud1, Cloud::Ptr &cloud2, pcl::CorrespondencesPtr &correspondences)
    {
        if (correspondences->empty())
        {
            return Eigen::Matrix4f::Identity();
        }

        pcl::registration::CorrespondenceRejectorSampleConsensus<PointType> sac;
        sac.setInlierThreshold(0.5);
        sac.setInputSource(cloud1);
        sac.setInputTarget(cloud2);
        sac.setInputCorrespondences(correspondences);
        pcl::Correspondences inliers;
        sac.getCorrespondences(inliers);

        Eigen::Matrix4f transformation;
        if (m_estimateScale)
        {
            pcl::registration::TransformationEstimationSVDScale<PointType, PointType> transformationEstimation;
            transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
        }
        else
        {
            pcl::registration::TransformationEstimationSVD<PointType, PointType> transformationEstimation;
            transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
        }


        if (transformation == Eigen::Matrix4f::Identity() || IsNotNAN(transformation) == false)
        {
            std::cout << "Identity or NAN" << std::endl;
        }

        return transformation;
    }
Example #2
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();
}
Example #3
0
void 
	InitialGuess::findCorrespondance(PointCloudOfFpfhPtr descriptor1, PointCloudOfFpfhPtr descriptor2, pcl::CorrespondencesPtr &corres ){

	 pcl::registration::CorrespondenceEstimation< pcl::FPFHSignature33, pcl::FPFHSignature33  > cer;
	 
	 //pcl::registration::CorrespondenceEstimation <pcl::PointXYZRGB,pcl::PointXYZRGB > cer2;
	 //pcl::Correspondences corres,recpCorres;
	 
	 cer.setInputCloud(descriptor1);
	 cer.setInputTarget(descriptor2);
	 cer.determineCorrespondences(*corres);
	 //cer.determineReciprocalCorrespondences(*corres);
	 cerr<<"\nFPFH Corres size "<<corres->size();
	 
}
void
print_correspondences_and_histograms(
    pcl::CorrespondencesPtr correspondences,
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr features1,
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr features2
)
{
    for (size_t i = 0; i < correspondences->size(); i++) {
        int q = (*correspondences)[i].index_query;
        int m = (*correspondences)[i].index_match;

		std::cout << "!for correspondence n." << i 
            << " index_query=" << q
            << " index_match=" << m
            << endl;
        cout << "!!1!!";
        print_feature_descriptor(features1->points[q]);
        cout << "!!2!!";
        print_feature_descriptor(features2->points[q]);
    }
}
Example #5
0
/* perform 2D SURF feature matching */
void match (Mat img_1, Mat img_2, vector<KeyPoint> keypoints_1,
    vector<KeyPoint> keypoints_2, vector<DMatch> &good_matches,
    pcl::CorrespondencesPtr &correspondences)
{
  SurfDescriptorExtractor extractor;
  Mat descriptors_1, descriptors_2;

  extractor.compute (img_1, keypoints_1, descriptors_1);
  extractor.compute (img_2, keypoints_2, descriptors_2);

  //FlannBasedMatcher matcher;
  BFMatcher matcher (NORM_L2);
  std::vector<DMatch> matches;

  matcher.match (descriptors_1, descriptors_2, matches);

  double max_dist = 0;
  double min_dist = 100;

  for (int i = 0; i < descriptors_1.rows; i++)
  {
    double dist = matches[i].distance;

    if (dist < min_dist)
      min_dist = dist;
    if (dist > max_dist)
      max_dist = dist;
  }

  for (int i = 0; i < descriptors_1.rows; i++)
  {
    // need to change the factor "2" to adapt to different cases
    if (matches[i].distance < 3 * min_dist)  //may adapt for changes
    {
      good_matches.push_back (matches[i]);
    }
  }

  correspondences->resize (good_matches.size ());

  for (unsigned cIdx = 0; cIdx < good_matches.size (); cIdx++)
  {
    (*correspondences)[cIdx].index_query = good_matches[cIdx].queryIdx;
    (*correspondences)[cIdx].index_match = good_matches[cIdx].trainIdx;

    if (0)  // for debugging
    {
      cout << good_matches[cIdx].queryIdx << " " << good_matches[cIdx].trainIdx
          << " " << good_matches[cIdx].distance << endl;
      cout << good_matches.size () << endl;
    }
  }

  // change the constant value of SHOW_MATCHING to 1 if you want to visulize the matching result
  if (SHOW_MATCHING)
  {
    Mat img_matches;
    drawMatches (img_1, keypoints_1, img_2, keypoints_2, good_matches,
        img_matches, Scalar::all (-1), Scalar::all (-1), vector<char> (),
        DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

    //-- Show detected matches
    imshow ("Good Matches", img_matches);
    waitKey (0);
  }
}
void
visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1,
			   const PointCloudPtr points2, const PointCloudPtr keypoints2,
			   const pcl::CorrespondencesPtr pCorrespondences
			   /* std::vector<int> &correspondences ,
			   const std::vector<float> &correspondence_scores, int max_to_display */)
{
  // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
  // by shifting one to the left and the other to the right. Then we'll draw lines between the corresponding points
  // Create some new point clouds to hold our transformed data
  PointCloudPtr points_left (new PointCloud);
  PointCloudPtr keypoints_left (new PointCloud);
  PointCloudPtr points_right (new PointCloud);
  PointCloudPtr keypoints_right (new PointCloud);
  // Shift the first clouds' points to the left
  //const Eigen::Vector3f translate (0.0, 0.0, 0.3);
  const Eigen::Vector3f translate (0.4, 0.0, 0.0);
  const Eigen::Quaternionf no_rotation (0, 0, 0, 0);
  pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation);
  pcl::transformPointCloud (*keypoints1, *keypoints_left, -translate, no_rotation);
  // Shift the second clouds' points to the right
  pcl::transformPointCloud (*points2, *points_right, translate, no_rotation);
  pcl::transformPointCloud (*keypoints2, *keypoints_right, translate, no_rotation);
  // Add the clouds to the visualizer
  pcl::visualization::PCLVisualizer vis;
  vis.addPointCloud (points_left, "points_left");
  vis.addPointCloud (points_right, "points_right");
//   // Compute the weakest correspondence score to display
//   std::vector<float> temp (correspondence_scores);
//   std::sort (temp.begin (), temp.end ());
//   if (max_to_display >= temp.size ())
//     max_to_display = temp.size () - 1;
//   float threshold = temp[max_to_display];
//   std::cout << max_to_display << std::endl;
  // Draw lines between the best corresponding points
  for (size_t i = 0; i < pCorrespondences->size (); ++i)
  {
//     if (correspondence_scores[i] > threshold)
//     {
//       continue; // Don't draw weak correspondences
//     }
    // Get the pair of points
    const PointT & p_left = keypoints_left->points[(*pCorrespondences)[i].index_query];
    const PointT & p_right = keypoints_right->points[(*pCorrespondences)[i].index_match];
    // Generate a random (bright) color
    double r = (rand() % 100);
    double g = (rand() % 100);
    double b = (rand() % 100);
    double max_channel = std::max (r, std::max (g, b));
    r /= max_channel;
    g /= max_channel;
    b /= max_channel;
    // Generate a unique string for each line
    std::stringstream ss ("line");
    ss << i;
    // Draw the line
    vis.addLine (p_left, p_right, r, g, b, ss.str ());
  }
  vis.resetCamera ();
  vis.spin ();
}
Example #7
0
Eigen::Matrix4f twostepAlign (const PointCloud &src_points,
    const PointCloud &tgt_points, const PointCloud &src_key,
    const PointCloud &tgt_key, pcl::CorrespondencesPtr correspondences,
    const Eigen::Matrix4f initial_matrix, PointCloud &output, bool initial,
    double threshold)
{
  Eigen::Matrix4f matrix1;  // initial
  Eigen::Matrix4f matrix2;  //fine*initial
  Eigen::Matrix4f matrix_final;

  // smart pointer no need to delete it at the end
  PointCloudPtr src (new PointCloud (src_points));
  PointCloudPtr tgt (new PointCloud (tgt_points));

  bool flag = 1;

  // initial alignment
  if (initial && flag == 1)
  {
	  if (correspondences->size () < 3)
	  {
	    printf (
	        "The correspondence points are less than 3. The RANSAC based initial alignment will be swithched off.  \n");
	    printf ("Please relax the threshold for SURF feature mtaching.  \n");
	    flag = 0;
	  }
	  else
		  matrix1 = computeInitialAlignment (src_key, tgt_key, correspondences);
  }

  // Switch off the initial alignment
  if (!initial || flag == 0)
  {
    matrix1 << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
  }
  PointCloudPtr points_transformed (new PointCloud);
  pcl::transformPointCloud (*src, *points_transformed, matrix1);

  // fine registration
  if (FINE_FLAG)
  {
    matrix2 = fineAlign (points_transformed, tgt, output, threshold,initial) * matrix1;  //pairwise alignment matrix
  }

  else  // Switch off the fine alignment
  {
    matrix2 = matrix1;
    output = *points_transformed;
  }

  // initial_matrix: the initial alignment refered to the first frame
  // output is the transformed point cloud refered to the firs frame
  matrix_final = initial_matrix * matrix2;
  pcl::transformPointCloud (output, output, initial_matrix);

  // print the transform for debugging
  if (PRINT_TRANSFORM_PAIR)
  {
    cout << "Final Tranform: " << endl;
    for (int k = 0; k < 3; k++)
    {
      for (int j = 0; j < 4; j++)
      {
        cout << matrix_final (k, j) << " ";
      }
      cout << endl;
    }
  }

  // return the final ransfrom relative to the first frame
  return matrix_final;
}
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);
  }
}