PCL库I/O模块之连接两个点云中的字段或数据形成新的点云

 之前已经讲过PCL库的I/O模块，以及怎么用它去操作pcd文件。那么，在本篇中，是通过写代码学习如何连接两个不同点云为一个点云，进行连接操作前要确保两个数据集中字段的类型和维度相等。此外，还要学习如何连接两个不同点云的字段（例如，颜色、发线），这种操作的强制约束条件是两个数据集中点的数目必须一样，如，点云A是N个点的xyz点，点云B是N个点的RGB点，则连接两个字段形成点云C是N个点xyzrgb类型。

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

void print_cloud(pcl::PointCloud<pcl::PointXYZ> &cloud);
void print_cloud(pcl::PointCloud<pcl::Normal> &cloud);
void print_cloud(pcl::PointCloud<pcl::PointNormal> &cloud);
void concatenate_same_cloud();
void concatenate_not_same_cloud();


int main(int argc, char** argv){
    concatenate_same_cloud();
    concatenate_not_same_cloud();
    return 0;
}

void print_cloud(pcl::PointCloud<pcl::PointXYZ> &cloud){
    std::cout << "point data: " << std::endl;
    for(size_t i = 0; i < cloud.points.size(); ++i){
        std::cout << "\t" << "(" << cloud.points[i].x << ", "
                  << cloud.points[i].y << ", " << cloud.points[i].z
                  << ")" << std::endl;
    }
}

void print_cloud(pcl::PointCloud<pcl::Normal> &cloud){
    std::cout << "point data: " << std::endl;
    for(size_t i = 0; i < cloud.points.size(); ++i){
        std::cout << "\t" << "(" << cloud.points[i].normal_x << ", "
                  << cloud.points[i].normal_y << ", " << cloud.points[i].normal_z
                  << ")" << std::endl;
    }
}

void print_cloud(pcl::PointCloud<pcl::PointNormal> &cloud){
    std::cout << "point data: " << std::endl;
    for(size_t i = 0; i < cloud.points.size(); ++i){
        std::cout << "\t" << "(" << cloud.points[i].x << ", "
                  << cloud.points[i].y << ", " << cloud.points[i].z << ", "
                  << cloud.points[i].normal_x << ", " << cloud.points[i].normal_y
                  << ", " << cloud.points[i].normal_z
                  << ")" << std::endl;
    }
}

void concatenate_same_cloud(){
    std::cout << __FUNCTION__ << std::endl;
    pcl::PointCloud<pcl::PointXYZ> cloud_1, cloud_2, cloud_3;
    cloud_1.height = 1;
    cloud_1.width = 5;
    cloud_1.points.resize(cloud_1.width * cloud_1.height);
    srand((uint32_t)time(nullptr));
    for(size_t i = 0; i < cloud_1.points.size(); ++i){
        cloud_1.points[i].x = 1024*rand() / (RAND_MAX + 1.f);
        cloud_1.points[i].y = 1024*rand() / (RAND_MAX + 1.f);
        cloud_1.points[i].z = 1024*rand() / (RAND_MAX + 1.f);
    }

    cloud_2.height = 1;
    cloud_2.width = 5;
    cloud_2.points.resize(cloud_2.width * cloud_2.height);
    for(size_t i = 0; i < cloud_2.points.size(); ++i){
        cloud_2.points[i].x = 1024*rand() / (RAND_MAX + 1.f);
        cloud_2.points[i].y = 1024*rand() / (RAND_MAX + 1.f);
        cloud_2.points[i].z = 1024*rand() / (RAND_MAX + 1.f);
    }
    // 同种点云进行拼接，需要保证数据类型和维度相同。
    cloud_3 = cloud_1 + cloud_2;
    std::cout << "print cloud_1:" << std::endl;
    print_cloud(cloud_1);
    std::cout << "print cloud_2:" << std::endl;
    print_cloud(cloud_2);
    std::cout << "print cloud_3:" << std::endl;
    print_cloud(cloud_3);
}

void concatenate_not_same_cloud(){
    std::cout << __FUNCTION__ << std::endl;
    pcl::PointCloud<pcl::PointXYZ> cloud_1;
    pcl::PointCloud<pcl::Normal> normal_cloud;
    pcl::PointCloud<pcl::PointNormal> xyznormal_cloud;
    cloud_1.height = 1;
    cloud_1.width = 5;
    cloud_1.points.resize(cloud_1.width * cloud_1.height);
    srand((uint32_t)time(nullptr));
    for(size_t i = 0; i < cloud_1.points.size(); ++i){
        cloud_1.points[i].x = 1024*rand() / (RAND_MAX + 1.f);
        cloud_1.points[i].y = 1024*rand() / (RAND_MAX + 1.f);
        cloud_1.points[i].z = 1024*rand() / (RAND_MAX + 1.f);
    }

    normal_cloud.width = 1;
    normal_cloud.height = 5;
    normal_cloud.points.resize(normal_cloud.width * normal_cloud.height);
    for(size_t i = 0; i < normal_cloud.points.size(); ++i){
        normal_cloud.points[i].normal_x = 1024*rand() / (RAND_MAX + 1.f);
        normal_cloud.points[i].normal_y = 1024*rand() / (RAND_MAX + 1.f);
        normal_cloud.points[i].normal_z = 1024*rand() / (RAND_MAX + 1.f);
    }
	// 不同数据结构的点云进行concat，需要注意的是两组点云所包含的点的个数必须一致。
    pcl::concatenateFields(cloud_1, normal_cloud, xyznormal_cloud);

    print_cloud(cloud_1);
    print_cloud(normal_cloud);
    print_cloud(xyznormal_cloud);
}



result：
concatenate_same_cloud
print cloud_1:
point data: 
	(-0.897227, -0.246491, 0.712056)
	(-0.653481, 0.0182548, 0.476729)
	(-0.76618, 0.959315, -0.786077)
	(-0.418579, -0.364152, -0.0971465)
	(-0.415787, 0.401982, 0.0647926)
print cloud_2:
point data: 
	(0.213576, 0.273437, -0.997824)
	(-0.386747, -0.936085, -0.468531)
	(-0.354034, 0.867138, 0.203534)
	(0.791641, 0.739882, -0.789068)
	(0.232594, 0.548069, -0.533694)
print cloud_3:
point data: 
	(-0.897227, -0.246491, 0.712056)
	(-0.653481, 0.0182548, 0.476729)
	(-0.76618, 0.959315, -0.786077)
	(-0.418579, -0.364152, -0.0971465)
	(-0.415787, 0.401982, 0.0647926)
	(0.213576, 0.273437, -0.997824)
	(-0.386747, -0.936085, -0.468531)
	(-0.354034, 0.867138, 0.203534)
	(0.791641, 0.739882, -0.789068)
	(0.232594, 0.548069, -0.533694)
concatenate_not_same_cloud
point data: 
	(-0.897227, -0.246491, 0.712056)
	(-0.653481, 0.0182548, 0.476729)
	(-0.76618, 0.959315, -0.786077)
	(-0.418579, -0.364152, -0.0971465)
	(-0.415787, 0.401982, 0.0647926)
point data: 
	(0.213576, 0.273437, -0.997824)
	(-0.386747, -0.936085, -0.468531)
	(-0.354034, 0.867138, 0.203534)
	(0.791641, 0.739882, -0.789068)
	(0.232594, 0.548069, -0.533694)
point data: 
	(-0.897227, -0.246491, 0.712056, 0.213576, 0.273437, -0.997824)
	(-0.653481, 0.0182548, 0.476729, -0.386747, -0.936085, -0.468531)
	(-0.76618, 0.959315, -0.786077, -0.354034, 0.867138, 0.203534)
	(-0.418579, -0.364152, -0.0971465, 0.791641, 0.739882, -0.789068)
	(-0.415787, 0.401982, 0.0647926, 0.232594, 0.548069, -0.533694)

# CMAKELists.txt文件，采用cmake的方式进行编译
cmake_minimum_required(VERSION 3.14 FATAL_ERROR)
project(pcl_test)
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
include_directories(/usr/include)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
 
add_executable(pcl_test pcl_io_concatenate_test.cpp)
target_link_libraries(pcl_test
        ${PCL_LIBRARIES}
        boost_system
)