c++ 利用pcl库在点云中画圆和画方格

背景：
在路端点云中，把坐标原点从主激光雷达光心移到路端中央，需要画圆环和棋盘格才能确定路端中央的大概位置,所使用的数据为pcd格式。

参考：
在PCL中画圆环，pcl可视化pcl::visualization::PCLVisualizer

图片：

代码：

multi_pcd_view_single_lidar.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <dlfcn.h>
#include <dirent.h>
#include <stdio.h>

int fileNum = 5000;

std::string pcd_dir = "/xxx/";
void get_filelist_from_dir(std::string _path, std::vector<std::string>& _files);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Lidar Viewer"));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZI>);

int main(int argc, const char **argv)
{


    cv::namedWindow("Sync camera", CV_WINDOW_NORMAL);
    cv::resizeWindow("Sync camera", 960, 1080);
    cv::moveWindow("Sync camera", 960, 0);

    viewer->setBackgroundColor(0, 0, 0);
    viewer->initCameraParameters();
    viewer->addCoordinateSystem(5.0);
    viewer->setSize(960, 1080);
    viewer->setCameraPosition(0, -120, 180, 0, 0, 0);
    pcl::visualization::Camera camera;
    viewer->getCameraParameters(camera);
    camera.focal[0] = 0;
    camera.focal[1]	= 45;
    camera.focal[2] = 1;
    viewer->setCameraParameters(camera);

    std::vector<std::string> binNames;
    binNames.reserve(fileNum);
    get_filelist_from_dir(pcd_dir, binNames);
    std::cout << binNames.size() << std::endl;



    for(int i = 0; i < binNames.size(); i++)
    {
        std::string pcdfile = pcd_dir + binNames[i].c_str();
        std::cout << "pcdfile : " << pcdfile << std::endl;
        int pointCount;
       

        std::cout << "i : " << i << std::endl;
        if (pcl::io::loadPCDFile(pcdfile, *cloud1) == -1) //* 读入PCD格式的文件，如果文件不存在，返回-1
        {
            PCL_ERROR ("Couldn't read pcd file  \n"); //文件不存在时，返回错误，终止程序。
            return (-1);
        }
        pointCount = cloud1->width * cloud1->height;
        for (int i = 0; i < pointCount; i++) {
            pcl::PointXYZRGB pointXYZRGB;
            pointXYZRGB.x = cloud1->points[i].x;
            pointXYZRGB.y = cloud1->points[i].y;
            pointXYZRGB.z = cloud1->points[i].z;
            int intensity = cloud1->points[i].intensity;

            if (intensity <= 63) {
                    pointXYZRGB.r = 0;
                    pointXYZRGB.g = 254 - 4 * intensity;
                    pointXYZRGB.b = 255;
            } else if (intensity > 63 && intensity <= 127) {
                    pointXYZRGB.r = 0;
                    pointXYZRGB.g = 4 * intensity - 254;
                    pointXYZRGB.b = 510 - 4 * intensity;
            } else if (intensity > 127 && intensity <= 191) {
                    pointXYZRGB.r = 4 * intensity - 510;
                    pointXYZRGB.g = 255;
                    pointXYZRGB.b = 0;
            } else if (intensity > 191 && intensity <= 255) {
                    pointXYZRGB.r = 255;
                    pointXYZRGB.g = 1022 - 4 * intensity;
                    pointXYZRGB.b = 0;
                }
                cloud->push_back(pointXYZRGB);
            }


		std::cout << pointCount << std::endl;

        
        Eigen::Affine3f transform = Eigen::Affine3f::Identity();
        transform.rotate (Eigen::AngleAxisf (0, Eigen::Vector3f::UnitZ()));

        pcl::transformPointCloud(*cloud, *transformed_cloud, transform);


        //画网格
        std::string  id = "id";
        for (int i = -50; i <= 50; i += 10) {
            viewer->addLine(pcl::PointXYZ(i, -50, 0), pcl::PointXYZ(i, 50, 0), std::to_string(i), 0);
            viewer->addLine(pcl::PointXYZ(-50, i, 0), pcl::PointXYZ(50, i, 0), std::to_string(i+200)+id, 0);
        }

        std::string  id1 = "_";
        std::vector<pcl::PointXYZ> points;
        for (int j = 1; j <= 5; j ++) {
            int o_x = 0, o_y = 0, r = 10 * j;    //圆心及半径
            for (int i = 0; i < 100; i++)   //计算圆环上点的坐标
            {
                pcl::PointXYZ point;
                double alpha = 2 * M_PI / (100 - 1);
                point.x = o_x + r * cos(i * alpha);
                point.y = o_y + r * sin(i * alpha);
                point.z = 0;
                points.push_back(point);
            }
            //将圆环上的点用线段连起来
            for (int i = 0; i < points.size() - 1; i++)
            {
                viewer->addLine(points.at(i), points.at(i + 1), std::to_string(j)+id1+std::to_string(i), 0);
            }
            viewer->addLine(points.at(points.size() - 1), points.at(0), std::to_string(points.size()*500+i)+id1, 0);
        }

        viewer->addPointCloud(transformed_cloud);
        viewer->spinOnce(10);
        viewer->removeAllPointClouds();
        viewer->removeAllShapes();
        cloud->clear();
        transformed_cloud->clear();

    }
    return 0;
}

void get_filelist_from_dir(std::string _path, std::vector<std::string>& _files)
{
    DIR* dir = opendir(_path.c_str());
    struct dirent* ptr;
    std::vector<std::string> file;
    while((ptr = readdir(dir)) != NULL)
    {
        if(ptr->d_name[0] == '.')	continue;
        file.push_back(ptr->d_name);
    }
    closedir(dir);
    sort(file.begin(), file.end());
    _files = file;
}

CMakeLists.txt 中的内容为：

cmake_minimum_required(VERSION 3.10)
project(pcl_test)

set(CMAKE_BUILD_TYPE "Release")

set(PCL_DIR /home/xxx/3rdparty/pcl181/share/pcl-1.8)
find_package(PCL 1.8 REQUIRED)
find_package(PCL  REQUIRED)

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

add_executable(multi_pcd_view_single_lidar multi_pcd_view_single_lidar.cpp)
target_link_libraries (multi_pcd_view_single_lidar ${PCL_LIBRARIES})