PCL点云数据库的使用

主要使用CMake工具组织实现

• PCL 1.9.1
• LAS 1.8.1
• Eigen 3.1.0

使用pcl与las库实现了，

• bin点云读取
• las点云读取
• 点云拼接
• 点云分块保存

其中还有其他的一些小功能，比如：获取指定路径下的指定后缀名称的文件名称，并按照名称排序，自定义STL模板的排序。

include文件夹下

#ifndef BUILDPRIORMAP_H
#define OPTIMIZER_H

#include <string>
#include <iostream>
#include <set>
#include <vector>
#include <fstream>
#include <sstream>
#include <liblas/liblas.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h> //使用OMP需要添加的头文件
#include <pcl/filters/passthrough.h>    //分块使用
#include <pcl/common/transforms.h>

namespace PCL_TOOLS
{

    class BuildPriorMap
    {
    public:
        BuildPriorMap(std::string rootfile, std::string PriorMapDir);
        std::string rootfile;                                   //原始的las地图路径
        std::string PriorMapDir;                                //处理后的地图存储路径
        std::string GroundTruth;                                // ground truth txt文件路径
        std::string MapbinDir;                                  //分块bin地图存储路径
        std::string MappcdDir;                                  //分块的pcd地图保存路径
        pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloudPtr;      //所有的点云地图
        pcl::PointCloud<pcl::Normal>::Ptr pointCloudNormalsPtr; //所有的点云地图的法向量

    public:
        void LoadFiles(std::string DirPath, std::string Suffix, std::vector<std::string> &files);                           //获取指定路径下的指定后缀的所有文件的文件名并排序
        void MapPointXYZFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr filterptr, float x = 0.2, float y = 0.2, float z = 0.2); //降采样
        void readlas2pcl(bool bSaveAllPCD = false, bool pose2Identity = true);                                              // las格式转换为pcl格式
        void bin2pcd(std::string _MapbinDir, std::string _MappcdDir);                                                       //读取bin格式转化为pcd格式
        void readpcd2pcl(std::string GroundTruth, bool bSaveAllPCD = false);                                                // 使用ground truth位姿实现 pcd 点云拼接
        void BuildNormalsMap(pcl::PointCloud<pcl::PointXYZ>::Ptr PointXYZPtr, pcl::PointCloud<pcl::Normal>::Ptr NormalPtr); //根据PointXYZ点云生成pcl::Normal点云地图
        void SavePriorMap(unsigned int mapCubeSize = 50);                                                                   //按照方格坐标切分保存点云地图
    };
}
#endif

src文件夹下

#include <BuildPriorMap.h>

//模板函数：将string类型变量转换为常用的数值类型（此方法具有普遍适用性）
template <class Type>
Type stringToNum(const std::string &str)
{
    std::istringstream iss(str);
    Type num;
    iss >> num;
    return num;
}

bool computePairNum(std::string pair1, std::string pair2)
{
    return pair1 < pair2;
}

namespace PCL_TOOLS
{
    /**
     * @brief Construct a new Build Prior Map:: Build Prior Map object
     *
     * @param rootfile urban格式数据集根目录
     * @param PriorMapDir PCD格式先验地图输出保存路径
     */
    BuildPriorMap::BuildPriorMap(std::string rootfile, std::string PriorMapDir) : rootfile(rootfile), PriorMapDir(PriorMapDir), pointCloudPtr(new pcl::PointCloud<pcl::PointXYZ>), pointCloudNormalsPtr(new pcl::PointCloud<pcl::Normal>)
    {
        std::cout << "rootfile:" << rootfile << std::endl;
        std::cout << "PriorMapDir:" << PriorMapDir << std::endl;
        GroundTruth = rootfile + "07.txt";
        MapbinDir = rootfile + "velodyne/";
        MappcdDir = rootfile + "pcd/";
    }

    /**
     * @brief
     *
     * @param dir 文件夹路径
     * @param Suffix 要获取的文件后缀名
     * @param files 获取文件路径名称存储向量
     */
    void BuildPriorMap::LoadFiles(std::string DirPath, std::string Suffix, std::vector<std::string> &files)
    {
        std::cout << "DirPath : " << DirPath << "  get[*." << Suffix << "]files";
        std::vector<std::string> file_lists;
        // step 1 获取指定后缀的所有文件
        struct dirent *ptr;
        DIR *dir;
        dir = opendir(DirPath.c_str());
        file_lists.clear();
        while ((ptr = readdir(dir)) != NULL)
        {
            std::string tmp_file = ptr->d_name;
            if (tmp_file[0] == '.')
            { //取读取文件的第一个字符，如果为.则说明是隐藏文件不读取
                continue;
            }
            if (Suffix.size() <= 0)
            { //如果无传入的类型，则该路径下所有文件都读取
                file_lists.push_back(ptr->d_name);
            }
            else
            {
                if (tmp_file.size() < Suffix.size())
                {
                    //说明不可能，也就是文件名的长度小于后缀名这3长度
                    continue;
                }
                std::string tmp_cut_type = tmp_file.substr(tmp_file.size() - Suffix.size(), Suffix.size()); //截取后三位
                if (tmp_cut_type == Suffix)
                { //如果截取文件名的后三位和传入的类型一致的话，添加这个文件的全称包括后缀
                    file_lists.push_back(ptr->d_name);
                }
            }
        }
        // step 2 对指定文件进行排序
        if (file_lists.empty())
            return;
        std::sort(file_lists.begin(), file_lists.end(), computePairNum); //按照computePairNum函数的方法进行排序

        // step 3 保存排序结果
        for (int i = 0; i < file_lists.size(); ++i)
        {
            std::string file = file_lists[i];
            files.push_back(file); //将文件名字传入到pcd_filename里
        }
        std::cout << "num:" << files.size() << std::endl;
    }

    /**
     * @brief  PointXYZ地图进行下采样滤波
     *
     * @param filterptr 输入地图
     * @param x 滤波网格大小
     * @param y 滤波网格大小
     * @param z 滤波网格大小
     */
    void BuildPriorMap::MapPointXYZFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr filterptr, float x, float y, float z)
    {

        pcl::VoxelGrid<pcl::PointXYZ> filter; //创建滤波器
        filter.setInputCloud(filterptr);      //输入要滤波的点云
        filter.setLeafSize(x, y, z);          //用0.2 x 0.2 x 0.2的立方体对点云进行稀疏化
        filter.filter(*filterptr);            //获得滤波结果
    }

    /**
     * @brief 读取las数据格式转换为pcd格式
     *
     * @param bSaveAllPCD 是否保存完整的pcd格式地图
     * @param pose2Identity 是否对初始位姿进行坐标变换
     */
    void BuildPriorMap::readlas2pcl(bool bSaveAllPCD, bool pose2Identity)
    {
        std::string lasfile = rootfile + "sick_pointcloud.las";
        std::ifstream inFile;
        inFile.open(lasfile, std::ios::in | std::ios::binary);

        liblas::ReaderFactory readLas;
        liblas::Reader reader = readLas.CreateWithStream(inFile);
        liblas::Header const &header = reader.GetHeader();

        std::cout << "Compressed: " << (header.Compressed() == true ? "True" : "False") << std::endl;
        std::cout << "Signature: " << header.GetFileSignature() << std::endl;

        int count = header.GetPointRecordsCount();

        while (reader.ReadNextPoint())
        {
            liblas::Point const &point = reader.GetPoint();
            double x, y, z;
            x = point.GetX();
            y = point.GetY();
            z = point.GetZ();

            pcl::PointXYZ p(x, y, z);
            pointCloudPtr->push_back(p);
        }
        std::cout << "las2pcd :successed!  Point num:" << count << std::endl;
        // MapPointXYZFilter(pointCloudPtr, 1, 1, 1); //滤波

        //初始位姿转换到单位矩阵
        Eigen::Matrix4f T_init = Eigen::Matrix4f::Identity();
        if (pose2Identity)
        {
            std::ifstream inFilecsv(rootfile + "global_pose.csv", std::ios::in);
            std::string initPosestring;
            //读取csv以,为分割
            std::getline(inFilecsv, initPosestring, '\n'); //取出一行
            std::stringstream ss(initPosestring);
            std::string Pose_i;
            std::vector<float> Pose;
            while (std::getline(ss, Pose_i, ','))
            {
                float temp;
                temp = stringToNum<double>(Pose_i);
                Pose.push_back(temp);
            }
            T_init(0, 0) = Pose[1];
            T_init(0, 1) = Pose[2];
            T_init(0, 2) = Pose[3];
            T_init(0, 3) = Pose[4];

            T_init(1, 0) = Pose[5];
            T_init(1, 1) = Pose[6];
            T_init(1, 2) = Pose[7];
            T_init(1, 3) = Pose[8];

            T_init(2, 0) = Pose[9];
            T_init(2, 1) = Pose[10];
            T_init(2, 2) = Pose[11];
            T_init(2, 3) = Pose[12];

            Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
            // z轴旋转90°
            trans << 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;

            // Urban39
            Eigen::Matrix4f Vehicle2IMU;
            Vehicle2IMU << 1, 0, 0, -0.07,
                0, 1, 0, 0,
                0, 0, 1, 1.7,
                0, 0, 0, 1;

            std::cout << "point[0] pose before:" << (*pointCloudPtr)[0] << std::endl;
            std::cout << "T_init:" << T_init << std::endl;
            pcl::transformPointCloud(*pointCloudPtr, *pointCloudPtr, Vehicle2IMU.inverse() * T_init.inverse()); //将点云进行旋转平移变换到IMU坐标系
            std::cout << "point[0] pose after:" << (*pointCloudPtr)[0] << std::endl;
            std::cout << "pose2Identity succeed" << std::endl;
            inFilecsv.close();
        }

        BuildNormalsMap(pointCloudPtr, pointCloudNormalsPtr); //构建法向量地图

        // 调试 z 轴的方向, 使用pcl_viewer 查看pcd文件
        /*
        {
            for (int index_i = -2; index_i < 2; ++index_i)
            {
                for (int index_j = -2; index_j < 2; ++index_j)
                {
                    for (int j = 0; j < 200; j++)
                    {
                        pcl::PointXYZ point_z;
                        point_z.x = index_i * 50;
                        point_z.y = index_j * 50;
                        point_z.z = 10 * j;
                        pointCloudPtr->push_back(point_z);
                    }
                }
            }
        }
        */

        if (bSaveAllPCD)
        {
            std::string pcdfile = PriorMapDir + "allpointCloud.pcd";
            pcl::io::savePCDFileASCII(pcdfile, *pointCloudPtr);
            std::cout << "allpointCloud.pcd save:" << pcdfile << std::endl;

            pcdfile = PriorMapDir + "allnoramls.pcd";
            pcl::io::savePCDFileASCII(pcdfile, *pointCloudNormalsPtr);
            std::cout << "allnoramls.pcd save:" << pcdfile << std::endl;
        }

        inFile.close();
        std::cout << "las2pcl finished!" << std::endl;
    }

    /**
     * @brief
     *
     * @param _MapbinDir bin格式地图路径
     * @param _MappcdDir 保存pcd格式地图路径
     */
    void BuildPriorMap::bin2pcd(std::string _MapbinDir, std::string _MappcdDir)
    {
        std::cout << "load bin map from: " << _MapbinDir << " and save pcd map" << std::endl;
        //设置路径
        MapbinDir = _MapbinDir;
        MappcdDir = _MappcdDir;
        //获取文件名称
        std::vector<std::string> binfiles;
        std::string suffix = "bin";
        LoadFiles(MapbinDir, suffix, binfiles);
        //读取bin
        int32_t num = 1000000;
        int32_t file_size;
        float *data;
        pcl::PointCloud<pcl::PointXYZI> point_cloud;
        for (int i = 0; i < binfiles.size(); ++i)
        {
            data = (float *)malloc(num * sizeof(float));
            // pointers
            float *px = data + 0;
            float *py = data + 1;
            float *pz = data + 2;
            float *pr = data + 3;

            std::string binfile = MapbinDir + binfiles[i]; //存储路径 + 文件名
            std::cout << "binfile:[" << binfile << "]  ";
            std::fstream a_file(binfile.c_str(), std::ios::binary | std::ios::in | std::ios::ate);
            file_size = a_file.tellg();
            std::cout << "file size: " << file_size << "  ";
            a_file.seekg(0, std::ios::beg);
            if (!a_file.read(reinterpret_cast<char *>(data), file_size))
            {
                std::cout << "Error reading from file" << std::endl;
                return;
            }
            point_cloud.clear();
            point_cloud.width = (file_size / sizeof(float)) / 4;
            point_cloud.height = 1;
            point_cloud.is_dense = false;
            point_cloud.points.resize(point_cloud.width * point_cloud.height);
            std::cout << "resized to " << point_cloud.points.size() << std::endl;
            // fill in the point cloud
            for (int j = 0; j < point_cloud.points.size(); ++j)
            {
                point_cloud.points[j].x = *px;
                point_cloud.points[j].y = *py;
                point_cloud.points[j].z = *pz;
                point_cloud.points[j].intensity = *pr;
                px += 4;
                py += 4;
                pz += 4;
                pr += 4;
            }
            //保存pcd格式
            std::string name = binfiles[i].substr(0, binfiles[i].size() - suffix.size() - 1); //去掉后缀名三位
            std::string pcdfile = MappcdDir + name + ".pcd";
            std::cout << "save:[" << pcdfile << "]" << std::endl;
            pcl::io::savePCDFileASCII(pcdfile, point_cloud);
        }
        std::cout << "bin2pcd finished!" << std::endl;
    }

    /**
     * @brief
     *
     * @param GroundTruth  位姿真值的 txt
     * @param bSaveAllPCD  是否保存拼接后的地图
     */
    void BuildPriorMap::readpcd2pcl(std::string _GroundTruth, bool bSaveAllPCD)
    {
        GroundTruth = _GroundTruth;
        //获取文件名称
        std::vector<std::string> pcdfiles;
        std::string suffix = "pcd";
        LoadFiles(MappcdDir, suffix, pcdfiles);

        //读取ground进行地图拼接
        std::ifstream FileIn(GroundTruth);
        std::cout << "GroundTruth:[" << GroundTruth << "]" << std::endl;

        Eigen::Matrix4f pose_GT;
        Eigen::Matrix4f velo2cam, cam2velo, x_270;

        //给两个变换矩阵赋初值
        cam2velo << 0, 0, 1, 0,
            -1, 0, 0, 0,
            0, -1, 0, 0.08,
            0, 0, 0, 1;

        velo2cam << 0, -1, 0, 0,
            0, 0, -1, 0,
            1, 0, 0, -0.08,
            0, 0, 0, 1;

        // !kitti的z轴是前进方向所以是不是应该按照x,z轴分块
        // ! 要不就整体绕x旋转-90° //实现这一种方式吧
        x_270 << 1, 0, 0, 0,
            0, 0, 1, 0,
            0, -1, 0, 0,
            0, 0, 0, 1;

        int i = 0;
        pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);

        while (FileIn >> pose_GT(0, 0) >> pose_GT(0, 1) >> pose_GT(0, 2) >> pose_GT(0, 3) >> pose_GT(1, 0) >> pose_GT(1, 1) >> pose_GT(1, 2) >> pose_GT(1, 3) >> pose_GT(2, 0) >> pose_GT(2, 1) >> pose_GT(2, 2) >> pose_GT(2, 3))
        {
            pose_GT(3, 0) = 0;
            pose_GT(3, 1) = 0;
            pose_GT(3, 2) = 0;
            pose_GT(3, 3) = 1;
            // pose_GT = cam2velo * pose_data2 * velo2cam; //激光坐标系下的
            pose_GT = pose_GT * velo2cam; //相机坐标系下的

            std::string pcdfile = MappcdDir + pcdfiles[i];
            std::cout << "load:[" << pcdfile << "]";

            pcl::io::loadPCDFile(pcdfile, *source); //读入pcd格式的文件
            std::cout << " !" << std::endl;

            //将点云按照transform[i]的变换矩阵进行旋转平移变换，最终存入target中
            MapPointXYZFilter(source, 0.2, 0.2, 0.2);                    //滤波
            pcl::transformPointCloud(*source, *target, x_270 * pose_GT); //绕x轴再旋转90°
            //拼接

            *pointCloudPtr = *pointCloudPtr + *target;
            i++;
        }

        std::vector<int> indices_src; //保存去除的点的索引
        //移除 NaNs，从传感器获得的点云可能包含几种测量误差和/或不准确。其中之一是在一些点的坐标中存在NaN（不是数）值，
        pcl::removeNaNFromPointCloud(*pointCloudPtr, *pointCloudPtr, indices_src);

        BuildNormalsMap(pointCloudPtr, pointCloudNormalsPtr); //构建法向量地图

        // 调试 z 轴的方向, 使用pcl_viewer 查看pcd文件
        /*
        {
            for (int index_i = -2; index_i < 2; ++index_i)
            {
                for (int index_j = -2; index_j < 2; ++index_j)
                {
                    for (int j = 0; j < 50; j++)
                    {
                        pcl::PointXYZ point_z;
                        point_z.x = index_i * 50;
                        point_z.y = index_j * 50;
                        point_z.z = 10 * j;
                        pointCloudPtr->push_back(point_z);
                    }
                }
            }
        }
        */

        if (bSaveAllPCD)
        {
            std::string pcdfile = PriorMapDir + "allpointCloud.pcd";
            pcl::io::savePCDFileASCII(pcdfile, *pointCloudPtr);
            std::cout << "allpointCloud.pcd save:" << pcdfile << std::endl;

            pcdfile = PriorMapDir + "allnoramls.pcd";
            pcl::io::savePCDFileASCII(pcdfile, *pointCloudNormalsPtr);
            std::cout << "allnoramls.pcd save:" << pcdfile << std::endl;
        }
        FileIn.close();
        std::cout << "pcl2pcl finished!" << std::endl;
    }

    void BuildPriorMap::BuildNormalsMap(pcl::PointCloud<pcl::PointXYZ>::Ptr PointXYZPtr, pcl::PointCloud<pcl::Normal>::Ptr NormalPtr)
    {
        //计算法线
        pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n; // OMP加速
        //建立kdtree来进行近邻点集搜索
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
        n.setNumberOfThreads(10); //设置openMP的线程数
        // n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）
        n.setInputCloud(PointXYZPtr);
        n.setSearchMethod(tree);
        n.setKSearch(20); //点云法向计算时，需要所搜的近邻点大小
        // n.setRadiusSearch(0.03);//半径搜素
        n.compute(*NormalPtr); //开始进行法向计
    }

    /**
     * @brief  保存分块地图
     *
     * @param mapCubeSize 分块地图大小
     */
    void BuildPriorMap::SavePriorMap(unsigned int mapCubeSize)
    {

        // step 1 写构造config 保存分块大小，以及初始化地图是加载哪几块
        std::ofstream outFile;
        outFile.open(PriorMapDir + "config.txt", std::ios::out);
        outFile << mapCubeSize << std::endl;

        // step 2 分块保存地图
        std::set<std::pair<int, int>> keys; //分块地图的索引

        pcl::PointXYZ min, max;
        pcl::getMinMax3D(*pointCloudPtr, min, max);
        std::cout << "x:[" << min.x << "," << max.x << "]" << std::endl;
        std::cout << "y:[" << min.y << "," << max.y << "]" << std::endl;
        std::cout << "z:[" << min.z << "," << max.z << "]" << std::endl;

        //计算keys范围
        std::pair<int, int> key_x, key_y;
        key_x.first = std::floor(min.x / mapCubeSize);
        key_x.second = std::floor(max.x / mapCubeSize);
        key_y.first = std::floor(min.y / mapCubeSize);
        key_y.second = std::floor(max.y / mapCubeSize);
        std::cout << "key_x:[" << key_x.first << "," << key_x.second << "]" << std::endl;
        std::cout << "key_y:[" << key_y.first << "," << key_y.second << "]" << std::endl;

        //保存索引范围与分块地图
        for (int index_i = key_x.first; index_i < key_x.second + 1; ++index_i)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr subPointCloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr subTempPtr(new pcl::PointCloud<pcl::PointXYZ>);

            pcl::PassThrough<pcl::PointXYZ> pass_x, pass_y;                                                                 // 声明直通滤波
            pass_x.setInputCloud(pointCloudPtr);                                                                            // 传入点云数据
            pass_x.setFilterFieldName("x");                                                                                 // 设置操作的坐标轴
            pass_x.setFilterLimits(static_cast<int>(index_i * mapCubeSize), static_cast<int>((index_i + 1) * mapCubeSize)); // 设置坐标范围
            // pass_x.setFilterLimitsNegative(true);//保存范围内or范围外

            pass_x.filter(*subTempPtr); // 进行滤波输出

            for (int index_j = key_y.first; index_j < key_y.second + 1; ++index_j)
            {

                pass_y.setInputCloud(subTempPtr);                                                                               // 传入点云数据
                pass_y.setFilterFieldName("y");                                                                                 // 设置操作的坐标轴
                pass_y.setFilterLimits(static_cast<int>(index_j * mapCubeSize), static_cast<int>((index_j + 1) * mapCubeSize)); // 设置坐标范围
                // pass_y.setFilterLimitsNegative(true);//保存范围内or范围外

                pass_y.filter(*subPointCloudPtr); // 进行滤波输出

                if (!(*subPointCloudPtr).empty())
                {
                    //保存索引
                    std::pair<int, int> key;
                    key.first = index_i;
                    key.second = index_j;
                    outFile << index_i << " " << index_j << std::endl;
                    keys.insert(key);

                    std::string subPointCloudFile = PriorMapDir + std::to_string(mapCubeSize) + "_" + std::to_string(index_i) + "_" + std::to_string(index_j) + "_points.pcd"; //分块的3D点云pcd<cubesize_indexi_indexj_point.pcd>
                    pcl::io::savePCDFileASCII(subPointCloudFile, *subPointCloudPtr);
                    std::cout << "sub PointCloud save:" << subPointCloudFile << std::endl;

                    pcl::PointCloud<pcl::Normal>::Ptr subnormalsPtr(new pcl::PointCloud<pcl::Normal>);
                    BuildNormalsMap(subPointCloudPtr, subnormalsPtr);

                    std::string subNormalsFile = PriorMapDir + std::to_string(mapCubeSize) + "_" + std::to_string(index_i) + "_" + std::to_string(index_j) + "_normals.pcd"; //分块的法向量
                    pcl::io::savePCDFileASCII(subNormalsFile, *subnormalsPtr);
                    std::cout << "sub NormalsFile save:" << subNormalsFile << std::endl;
                }
            }
        }
        outFile.close();
    }

}

Examples文件夹

1. KaistUrban.cc

#include <iostream>
#include <BuildPriorMap.h>
int main(int agrc, char **argv)
{
    // step 1 读取lasmap
    std::string rootfile = "/home/xxx/urban39/";
    std::string PriorMapDir = "/home/xxx/urban39/prior_map/";
    PCL_TOOLS::BuildPriorMap kaisturban(rootfile, PriorMapDir);
    kaisturban.readlas2pcl(false, true);
    // kaisturban.readlas2pcl(true, false);
    //  step 2 分块保存
    kaisturban.SavePriorMap(50);
    std::cout << "save  successed!" << std::endl;

    return 0;
}

2. Kitti.cc

#include <iostream>
#include <BuildPriorMap.h>
int main(int agrc, char **argv)
{
    // step 1 读取bin2pcd
    std::string rootfile = "/home/xxx/datas/KITTI/07/";
    std::string PriorMapDir = "/home/xxx/datas/KITTI/07/prior_map/";
    std::string GroundTruth = "/home/xxx/datas/KITTI/07/07.txt";
    std::string MapbinDir = "/home/xxx/datas/KITTI/07/velodyne/";
    std::string MappcdDir = "/home/xxx/datas/KITTI/07/pcd/";
    PCL_TOOLS::BuildPriorMap kitti(rootfile, PriorMapDir);
    // step 1.1 bin转化pcd
    kitti.bin2pcd(MapbinDir, MappcdDir);
    //    step 2 根据GT拼接pcd
    kitti.readpcd2pcl(GroundTruth, true);
    //   step 3 分块保存
    kitti.SavePriorMap(50);
    std::cout << "save  successed!" << std::endl;

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pcl_tools)

IF(NOT CMAKE_BUILD_TYPE)
    SET(CMAKE_BUILD_TYPE = Release)
ENDIF()
MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})

set(CMAKE_CXX_STANDARD 17)
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(libLAS REQUIRED)

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${LIBLAS_INCLUDE_DIR}
)

set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)

add_library(${PROJECT_NAME} SHARED
src/BuildPriorMap.cc
include/BuildPriorMap.h
)

target_link_libraries(${PROJECT_NAME}
${EIGEN3_LIBS}
${PCL_LIBRARIES}
${libLAS_LIBRARIES}
)

# examples
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)

# kaist数据集的先验地图处理
add_executable(kaisturban
        Examples/KaistUrban.cc)
target_link_libraries(kaisturban ${PROJECT_NAME})

# kitti数据集的先验地图处理
add_executable(kitti
        Examples/Kitti.cc)
target_link_libraries(kitti ${PROJECT_NAME})

#求解坐标变换
add_executable(matrix2 Examples/matrix2.cc)
target_link_libraries(matrix2
${EIGEN3_LIBS})

本文介绍了点云处理库中常见的集中滤波方法，包括直通滤波器，体素滤波器，统计滤波器，半径滤波器。

1、直通滤波器

使用直通滤波器，确定点云在x或y方向上的范围，可较快去除一定范围为之外的点云，达到第一步粗处理的目的。

  pcl::PassThrough<pcl::PointXYZ> passthrough;
  passthrough.setInputCloud(cloud);//输入点云
  passthrough.setFilterFieldName("z");//对z轴进行操作
  passthrough.setFilterLimits(0.0, 1.0);//设置直通滤波器操作范围
  passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
  passthrough.filter(*cloud_after_PassThrough);//执行滤波，

 坐标轴表示为红色(x)、绿色(y) 和蓝色(z)。这五个点用绿色表示过滤后剩余的点，红色表示已被过滤器删除的点。

2、体素滤波器

体素的概念类似于像素，使用AABB包围盒将点云数据体素化，一般体素越密集的地方信息越多，噪音点及离群点可通过体素网格去除。另一方面如果使用高分辨率相机等设备对点云进行采集，往往点云会较为密集。过多的点云数量会对后续分割工作带来困难。体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能。 

 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
 sor.setInputCloud (cloud);
 sor.setLeafSize (0.01f, 0.01f, 0.01f);
 sor.filter (*cloud_filtered);

体素滤波前

体素滤波后 

3、统计滤波器

统计滤波器主要用于去除明显离群点。 离群点特征在空间中分布稀疏。定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。

其大致的原理思路是：

1. 对每个点，我们计算它到它的K邻域内所有点的平均距离d。对输入点云中的每个点都进行这样的计算，即每个点都可以求得其对应的d，因此可以得到一个包含各点d值的数组，记为distance。
2. 对于输入点云中的所有点，假设得到distance数组中的各元素构成一个高斯分布，该数组即为一个样本，样本容量为点云包含的点数目。高斯分布曲线的形状由样本的均值和标准差决定，d值在标准范围（由样本的均值和方差定义）之外的对应点，可被定义为离群点并可从数据集中去除掉。

pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);

4、半径滤波器

点云半径滤波也叫基于连通分析的点云滤波，该方法的基本思想是假定原始点云中每个激光点在指定的半径邻域中至少包含一定数量的近邻点。原始点云中符合假设条件的激光点被视为正常点进行保留，反之，则视为噪声点并进行去除。该方法对原始激光点云中存在的一些悬空的孤立点或无效点具有很好的去除效果。

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::RadiusOutlierRemoval<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud);
        sor.setRadiusSearch(0.02);
        sor.setMinNeighborsInRadius(15);
        sor.setNegative(false);
        sor.filter(*cloud_filter);