Not able to visualize the Point Cloud from Point cloud registration using Iterative closest point

asked 2020-06-24 23:01:43 -0500

Astronaut gravatar image

updated 2020-06-24 23:29:16 -0500

Hi

I try to perform ICP with PCL to estimate the 6D Pose. So I have my 3D CAD model and try to find the point corresponds with the input point clouds(from my depth camera). It compiles the code and got results.

Here is my code

ros::Publisher pub, markers_pub_;

void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr &msg){


   // Convert to pcl point cloud
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZRGB>);
   pcl::fromROSMsg(*msg,*cloudIn);

   //Deklaration cloud pointers
   //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZRGB>);
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGB>);
   pcl::PointCloud<pcl::PointXYZRGB> finalCloud ;
   pcl::PointCloud<pcl::PointXYZRGB> finalCloud1 ;
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOut_new (new pcl::PointCloud<pcl::PointXYZRGB>) ;

   //Load CAD file
   if(pcl::io::loadPLYFile ("/home/admini/darknet_ros/src/darknet_ros/gb_visual_detection_3d/darknet_ros_3d/darknet_ros_3d/src/box-ikea.ply", *cloudOut)==-1)
   {
     PCL_ERROR ("Couldn't read second input file! \n");
    // return (-1);
   }

  //std::cout << "\nLoaded file "  << " (" << cloudOut->size () << " points) in " << time.toc () << " ms\n" << std::endl;

  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  //pcl::IterativeClosestPoint<PointXYZ, PointXYZ> icp;
  icp.setInputCloud(cloudOut);
  icp.setInputTarget(cloudIn);
  icp.setMaximumIterations (200);
  icp.setTransformationEpsilon (1e-9);
  icp.setMaxCorrespondenceDistance (100);
  icp.setEuclideanFitnessEpsilon (20);
  icp.setRANSACOutlierRejectionThreshold (1.5);
  icp.setRANSACIterations(10);

  icp.align(finalCloud);

  if (icp.hasConverged())
  {
      std::cout << "ICP converged." << std::endl
                << "The score is " << icp.getFitnessScore() << std::endl;
      std::cout << "Transformation matrix:" << std::endl;
      std::cout << icp.getFinalTransformation() << std::endl;
  }
  else std::cout << "ICP did not converge." << std::endl;

  Eigen::Matrix4f transformationMatrix = icp.getFinalTransformation ();
  std::cout<<"trans %n"<<transformationMatrix<<std::endl;

  pcl::transformPointCloud( *cloudOut, *cloudOut_new, transformationMatrix);

  pcl::io::savePCDFileASCII ("/home/admini/darknet_ros/src/darknet_ros/gb_visual_detection_3d/darknet_ros_3d/darknet_ros_3d/src/IcpResult4.pcd", finalCloud);

  finalCloud1=*cloudIn;
  finalCloud1 +=*cloudOut_new;

   //Publish the cloud
   sensor_msgs::PointCloud2 cloud_icp;
   pcl::toROSMsg (*cloudOut_new , cloud_icp);
   //cloudOut_new ->header.frame_id = "/zed_left_camera_frame";
   pub.publish (cloud_icp);

}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/zed/zed_node/point_cloud/cloud_registered", 200, cloud_cb);
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("cloud_icp", 100);
  markers_pub_ = nh.advertise<visualization_msgs::MarkerArray> ("msg_marker", 200);
  ros::spin();
}

Here the results:

 [pcl::PLYReader] /home/admini/darknet_ros/src/darknet_ros/gb_visual_detection_3d/darknet_ros_3d/darknet_ros_3d/src/box-ikea.ply:14: property 'list uint8 uint32 vertex_indices' of element 'face' is not handled
Failed to find match for field 'rgb'.
ICP converged.
The score is 0.000810607
Transformation matrix:
   0.988949    0.148239   0.0019899    0.477872
  -0.146782    0.980937   -0.127346   0.0537747
 -0.0208297    0.125647    0.991856 -0.00312536

It compiles but when visualize in RVIZ I got the error: For frame []: Frame [] does not exist Any help? When I do this way for the final cloud to be published like this

   sensor_msgs::PointCloud2 cloud_icp;
   pcl::toROSMsg (*cloudOut_new , cloud_icp);
   cloudOut_new ->header.frame_id = "/zed_left_camera_frame";
   pub.publish (cloud_icp);

I got the same error in RVIZ

 Transform [sender=unknown_publisher] For frame []: Frame [] does not exist

Please, any help?

edit retag flag offensive close merge delete