ROS下点云转换和存储

ssyy123

一些全局变量：
typedef pcl::PointXYZRGB PointT;typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr cloud(new PointCloudT);
PointCloudT cloud_out;
tf::TransformListener* tf_listener;
std::string world_frame_id;
std::string frame_id;
std::string path;

回调函数：
void
cloud_cb (const PointCloudT::ConstPtr& callback_cloud)
{
  *cloud = *callback_cloud;
  frame_id = cloud->header.frame_id;
  std::stringstream ss;
  std::string s1;
  ss<<ros::Time::now();
  s1=ss.str();
       
  //获取转换信息
  tf::StampedTransform transform;
  try
  {
    tf_listener->lookupTransform(world_frame_id, frame_id, ros::Time(0), transform);
  }
  catch (tf::TransformException ex)
  {
     ROS_ERROR("transform exception: %s", ex.what());
  }
       

  //4*4 transform matrix:

  Eigen::Affine3d pcl_transform;
  tf::transformTFToEigen(transform, pcl_transform);

  //transform:
pcl::transformPointCloud(*cloud,cloud_out,pcl_transform);

  // cloud saving:
  std::string file_name=path+"cloud_"+frame_id.substr(1, frame_id.length()-1)+s1+".pcd";
pcl::io::savePCDFileBinary(file_name,cloud_out);        
}


主函数：
int
main (int argc, char** argv)
{
  ros::init(argc, argv, "cloudtf");
  ros::NodeHandle nh("~");

  std::string pointcloud_topic;
  nh.param("pointcloud_topic", pointcloud_topic, std::string("/camera/depth_registered/points"));
  nh.param("world_frame_id", world_frame_id, std::string("/odom"));
  nh.param("path", path, std::string("/home/ubuntu1/pointtest/"));
  std::cout << "Read some parameters from launch file." << std::endl;
       
  tf_listener = new tf::TransformListener();
       
  // Subscribers:
ros::Subscriber sub = nh.subscribe(pointcloud_topic, 1, cloud_cb);

  ros::spin();

  return 0;
}

zhoujt

:)不错,希望有帮助