ubuntu下将单帧点云保存为pcd文件

方法一：指令

直接终端运行命令：

rosrun pcl_ros bag_to_pcd xxx.bag  /velodyne_points /home/xx/catkin_my/pcd

xxx.bag：是bag包名
/velodyne_points /home/xx/catkin_my/pcd：是保存pcd帧的路径

方法二：代码

#include<ros/ros.h>
#include<iostream>
#include<fstream>
#include<sstream>
#include<string>
#include<vector>
#include <math.h>


#include <stdio.h>

#include<sensor_msgs/PointCloud2.h>  


#include "std_msgs/String.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/filter.h>
#include "tf/transform_datatypes.h"
#include <tf2_msgs/TFMessage.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/passthrough.h>  //直通滤波
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgEkfEuler.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>

#define PI 3.141592653
using namespace std;
using namespace pcl;

void lonLat2WebMercator(double jing,double wei,double *x,double *y)
{
    *x = jing * 20037508.34 / 180;
    double ly = log(tan((90+ wei)*PI/360))/(PI/180);
    *y =  ly *20037508.34/180;

}

void filter_point_cloud(const pcl::PointCloud<pcl::PointXYZ>&original_cloud,pcl::PointCloud<pcl::PointXYZ>&tmp_cloud_filtered)
{
    tmp_cloud_filtered.clear();
    float x_pass;
    float y_pass;
    float z_pass; 
    for (int i = 0; i < original_cloud.size(); i++)
    {
        x_pass=original_cloud.points[i].x;
        y_pass=original_cloud.points[i].y;
        z_pass=original_cloud.points[i].z;
        tmp_cloud_filtered.push_back(original_cloud.points[i]);
    }

}
int i=0;
//pcl::visualization::CloudViewer viewer("pcd viewer");
//const  boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg,
void chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfEuler>& quatmsg)
{

   
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*pointmsg,pcl_pc2);

    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*tmp_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    filter_point_cloud(*tmp_cloud,*tmp_cloud_filtered);
    viewer.showCloud(tmp_cloud_filtered);
    pcl::io::savePCDFileASCII("test_pcd.pcd",*tmp_cloud_filtered);
  //pcl::io::savePCDFileASCII("write_pcd_test.pcd", pcl_pc2);//保存pcd  


/*
    imu_data.orientation.x = quatmsg->quaternion.x;
    imu_data.orientation.y = quatmsg->quaternion.y;
    imu_data.orientation.z = quatmsg->quaternion.z;
    imu_data.orientation.w = quatmsg->quaternion.w;
    imu_data.angular_velocity.x = gpsmsg->position.x;
    imu_data.angular_velocity.y = gpsmsg->position.y;
    imu_data.angular_velocity.z = gpsmsg->position.z;
*/

  
    //cout<< setprecision(10) <<quatmsg->quaternion.w<<endl;
    //cout<< setprecision(10) <<quatmsg->quaternion.x<<endl;
    //cout<< setprecision(10) <<quatmsg->quaternion.y<<endl;
    //cout<< setprecision(10) <<quatmsg->quaternion.z<<endl;
    
    double x,y=0;
    lonLat2WebMercator(gpsmsg->position.y,gpsmsg->position.x,&x,&y);

    cout<< setprecision(10) <<quatmsg->angle.x<<endl;
    cout<< setprecision(10) <<quatmsg->angle.y<<endl;
    cout<< setprecision(10) <<quatmsg->angle.z<<endl;
    cout<< setprecision(10) <<x<<endl;
    cout<< setprecision(10) <<y<<endl;
    cout<< setprecision(10) <<gpsmsg->position.z<<endl;
    FILE *fpWrite=fopen("data.txt","a");//a续写，w清除后写入
   fprintf(fpWrite,"%16.10f %16.10f %16.10f %16.10f %16.10f %16.10f \n",quatmsg->angle.x,quatmsg->angle.y,quatmsg->angle.z,x,y,gpsmsg->position.z);

   fclose(fpWrite);

    i++;

}



int main(int argc,char** argv)
{
  ros::init (argc, argv, "ros_points");
  ros::NodeHandle n;

 
  message_filters::Subscriber<sensor_msgs::PointCloud2> points_sub(n, "/velodyne_points", 2000);
  message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 100);     
  message_filters::Subscriber<sbg_driver::SbgEkfEuler> quat_sub(n, "/sbg/ekf_euler", 100);   
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sbg_driver::SbgEkfNav,sbg_driver::SbgEkfEuler> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100),points_sub,gps_sub, quat_sub);  // 同步
  sync.registerCallback(boost::bind(&chatterCallback, _1,_2 ,_3 ));   // _1, 

 
  ros::spin ();
}

（用的时候可以把不需要的部分删掉）

主要留以一下部分即可
[ 想要保存第几帧就写一个if（i==?）]

void filter_point_cloud(const pcl::PointCloud<pcl::PointXYZ>&original_cloud,pcl::PointCloud<pcl::PointXYZ>&tmp_cloud_filtered)
{
    tmp_cloud_filtered.clear();
    float x_pass;
    float y_pass;
    float z_pass; 
    for (int i = 0; i < original_cloud.size(); i++)
    {
        x_pass=original_cloud.points[i].x;
        y_pass=original_cloud.points[i].y;
        z_pass=original_cloud.points[i].z;
        tmp_cloud_filtered.push_back(original_cloud.points[i]);
    }

}
int i=0;
pcl::visualization::CloudViewer viewer("pcd viewer");
void chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg)
{

   
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*pointmsg,pcl_pc2);

    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*tmp_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    filter_point_cloud(*tmp_cloud,*tmp_cloud_filtered);
    viewer.showCloud(tmp_cloud_filtered);
    pcl::io::savePCDFileASCII("test_pcd.pcd",*tmp_cloud_filtered);
    i++;
}