PCL点云拼接（通过Lidar外参拼接点云）

六个固态激光雷达扫出来的点云，需要拼接成360°的闭合点云，刚开始我单纯的觉得直接把所有pcd合成一个（就是把所有点累加起来）那么看看效果：

我擦这是啥玩意，官网提供的拼接也是累加点呀，实际项目中没那么多理想情况，好吧，每个pcd出来的坐标信息都是基于当前雷达作为基点的，那么凭什么能完整合起来的呢？哈哈，固态激光雷达外参不就是完美的拼接点云的数据嘛。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(concatenate_clouds)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (concatenate_clouds concatenate_clouds.cpp)
target_link_libraries (concatenate_clouds ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES})

完整逻辑代码 concatenate_clouds.cpp

#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>
#include <pcl/common/transforms.h>
using namespace std;
int main()
{
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_3(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_4(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_5(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_6(new pcl::PointCloud<pcl::PointXYZ>);

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

    pcl::PCDReader reader1;
    reader1.read("1-0.pcd",*cloud_1);
    pcl::PCDReader reader2;
    reader2.read("1-1.pcd",*cloud_2);
    pcl::PCDReader reader3;
    reader3.read("1-2.pcd",*cloud_3);
    pcl::PCDReader reader4;
    reader4.read("1-3.pcd",*cloud_4);
    pcl::PCDReader reader5;
    reader5.read("1-4.pcd",*cloud_5);
    pcl::PCDReader reader6;
    reader6.read("1-5.pcd",*cloud_6);
    
    std::cerr<<"Done  "<<tt.toc()<<"  ms\n"<<std::endl;
    //转换
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();//单位矩阵
        transform_1 (0,0) = 0.999972;
        transform_1 (0,1) = 0;
        transform_1 (0,2) = 0.00750485;
        transform_1 (0,3) = 0;
        transform_1 (1,0) = 0;
        transform_1 (1,1) =1;
        transform_1 (1,2) = 0;
        transform_1 (1,3) = 0;
        transform_1 (2,0) = -0.00750485;
        transform_1 (2,1) =-0;
        transform_1 (2,2) = 0.999972;
        transform_1 (2,3) = 1.909;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_1(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud_1, *cloudOut_1, transform_1);
    
    Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity();//单位矩阵
        transform_2 (0,0) = 0.338535;
        transform_2 (0,1) = -0.939803;
        transform_2 (0,2) = 0.0465286;
        transform_2 (0,3) = -0.001;
        transform_2 (1,0) = 0.929107;
        transform_2 (1,1) =0.341685;
        transform_2 (1,2) = 0.141463;
        transform_2 (1,3) = 0.338;
        transform_2 (2,0) = -0.148845;
        transform_2 (2,1) =-0.00465988;
        transform_2 (2,2) = 0.98885;
        transform_2 (2,3) = 1.909;
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_2(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud_2, *cloudOut_2, transform_2);
    
    Eigen::Matrix4f transform_3 = Eigen::Matrix4f::Identity();//单位矩阵
        transform_3 (0,0) = -0.742701;
        transform_3 (0,1) = -0.656677;
        transform_3 (0,2) = -0.131041;
        transform_3 (0,3) = -1.109;
        transform_3 (1,0) = 0.64927;
        transform_3 (1,1) =-0.754084;
        transform_3 (1,2) = 0.0990242;
        transform_3 (1,3) = 0.15;
        transform_3 (2,0) = -0.163842;
        transform_3 (2,1) =-0.0115354;
        transform_3 (2,2) = 0.986419;
        transform_3 (2,3) = 1.909;
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_3(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud_3, *cloudOut_3, transform_3);
    
    Eigen::Matrix4f transform_4 = Eigen::Matrix4f::Identity();//单位矩阵
           transform_4 (0,0) = -0.742701;
           transform_4 (0,1) = 0.62401;
           transform_4 (0,2) = -0.128361;
           transform_4 (0,3) = -1.2;
           transform_4 (1,0) = -0.617308;
           transform_4 (1,1) =-0.617308;
           transform_4 (1,2) = -0.781369;
           transform_4 (1,3) = -0.149;
           transform_4 (2,0) = -0.157469;
           transform_4 (2,1) = 0.00861766;
           transform_4 (2,2) = 0.987486;
           transform_4 (2,3) = 1.909;
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_4(new pcl::PointCloud<pcl::PointXYZ>);
       pcl::transformPointCloud(*cloud_4, *cloudOut_4, transform_4);
    
    Eigen::Matrix4f transform_5 = Eigen::Matrix4f::Identity();//单位矩阵
           transform_5 (0,0) = 0.33825;
           transform_5 (0,1) = 0.939574;
           transform_5 (0,2) = 0.0528062;
           transform_5 (0,3) = -0.009;
           transform_5 (1,0) = -0.929838;
           transform_5 (1,1) =-0.342329;
           transform_5 (1,2) = -0.134952;
           transform_5 (1,3) = -0.301;
           transform_5 (2,0) = -0.144874;
           transform_5 (2,1) =-0.00345383;
           transform_5 (2,2) = 0.989444;
           transform_5 (2,3) = 1.909;
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_5(new pcl::PointCloud<pcl::PointXYZ>);
       pcl::transformPointCloud(*cloud_5, *cloudOut_5, transform_5);
    
    Eigen::Matrix4f transform_6 = Eigen::Matrix4f::Identity();//单位矩阵
           transform_6 (0,0) = 0.999931;
           transform_6 (0,1) = -0.000861352;
           transform_6 (0,2) = 0.0117086;
           transform_6 (0,3) = -0.037;
           transform_6 (1,0) = 0.00104713;
           transform_6 (1,1) =0.999874;
           transform_6 (1,2) = -0.0158696;
           transform_6 (1,3) = 0.019;
           transform_6 (2,0) = -0.0116934;
           transform_6 (2,1) = 0.0158807;
           transform_6 (2,2) = 0.999806;
           transform_6 (2,3) = 1.909;
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut_6(new pcl::PointCloud<pcl::PointXYZ>);
       pcl::transformPointCloud(*cloud_6, *cloudOut_6, transform_6);
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_x(new pcl::PointCloud<pcl::PointXYZ>);
    *cloud_x=*cloudOut_1+*cloudOut_2;
    *cloud_x+=*cloudOut_3;
    *cloud_x+=*cloudOut_4;
    *cloud_x+=*cloudOut_5;
    *cloud_x+=*cloudOut_6;


//    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("hz.pcd",*cloud_x);

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

    pcl::PCDReader reader_7;
    reader_7.read("hz.pcd",*cloud_7);
    std::cerr<<cloud_7,tt.tic();
    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_7);
    sor.setLeafSize(0.01f,0.01f,0.01f);
    sor.filter(*cloud_filtered);
    std::cerr<<"Done  "<<tt.toc()<<" ms.\n"<<std::endl;

    std::cerr<<"视图...\n",tt.tic();
//    pcl::visualization::CloudViewer viewer("3D Viewer");
//     std::cerr<<"1111  "<<std::endl;
//    viewer.showCloud(cloud_filtered);
//     std::cerr<<"2222  "<<std::endl;
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
     viewer->setBackgroundColor (0, 0, 0);
     viewer->addPointCloud<pcl::PointXYZ> (cloud_filtered, "sample cloud");
     viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
     viewer->addCoordinateSystem (1.0);
     viewer->initCameraParameters ();
    while(!viewer->wasStopped())
    {
    viewer->spinOnce(100);
    }
    std::cerr<<"Done  "<<tt.toc()<<"  ms.\n"<<std::endl;

    return 0;
}

那就跑起来看看吧：

1.cd 这两文件的文件夹目录