PCL 点云拼接

实现两个点云的叠加： cloud_1 + cloud_2 = cloud_3
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/time.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace std;
int main()
{
    pcl::PointCloud<pcl::PointXYZ> cloud_1;
    pcl::PointCloud<pcl::PointXYZ> cloud_2;

    pcl::console::TicToc tt;
    std::cerr<<"Reader...\n",tt.tic();

    pcl::PCDReader reader1;
    reader1.read("2box.pcd",cloud_1);
    pcl::PCDReader reader2;
    reader2.read("4box.pcd",cloud_2);
    std::cerr<<"Done  "<<tt.toc()<<"  ms\n"<<std::endl;

    pcl::PointCloud<pcl::PointXYZ> cloud_3;
    cloud_3=cloud_1+cloud_2;

    std::cerr<<"The point cloud_1 has:  "<<cloud_1.points.size()<<"  points data."<<std::endl;
    std::cerr<<"The point cloud_2 has:  "<<cloud_2.points.size()<<"  points data."<<std::endl;
    std::cerr<<"The point cloud_3 has:  "<<cloud_3.points.size()<<"  points data."<<std::endl;

    pcl::PCDWriter writer;
    writer.write("6box.pcd",cloud_3);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_4(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCDReader reader_3;
    reader_3.read("6box.pcd",*cloud_4);

    std::cerr<<"SorFilter...\n",tt.tic();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_4);
    sor.setLeafSize(0.01f,0.01f,0.01f);
    sor.filter(*cloud_filtered);
    std::cerr<<"Done  "<<tt.toc()<<" ms.\n"<<std::endl;

    std::cerr<<"visualization...\n",tt.tic();
    pcl::visualization::CloudViewer viewer1("3D Viewer1");
    viewer1.showCloud(cloud_filtered);
    while(!viewer1.wasStopped())
    {}
    std::cerr<<"Done  "<<tt.toc()<<"  ms.\n"<<std::endl;

    return 0;
}
From： PCL——点云拼接
猜你喜欢
