pcl点云保存成图片pgm和yaml，用于机器人导航规划

最新推荐文章于 2023-08-13 00:42:10 发布

可峰科技

最新推荐文章于 2023-08-13 00:42:10 发布

阅读量1.3k

点赞数 2

分类专栏： 3d_slam 文章标签： c++ 自动驾驶 ios

本文链接：https://blog.csdn.net/windxf/article/details/120006125

版权

3d_slam 专栏收录该内容

20 篇文章 8 订阅

订阅专栏

这段代码主要展示了如何将点云数据保存为PCD文件，然后将点云投影到图像上并保存为PGM文件。同时，还创建了一个YAML文件，包含了图像信息、分辨率和坐标框等数据。整个过程涉及到点云处理、图像处理和文件存储操作。

摘要由CSDN通过智能技术生成

话不多说直接上代码

        pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMapProjected.pcd", *cloud_projected);

        cout << "****************************************************" << endl;
        cout << "Saving projected map to pcd files completed\n"
             << endl;

        //save pcl_points to image
        cout << "req.resolution " << req.resolution;
        std::vector<cv::Point2i> points;
        pcl::PointXYZ p_temp;

        for(int i=0; i<globalMapCloud->points.size(); i++)
        {
            cv::Point2i point;
            point.x = globalMapCloud->points[i].x / req.resolution;
            point.y = globalMapCloud->points[i].y / req.resolution;
            points.push_back(point);
        }

        cv::Rect box = cv::boundingRect(cv::Mat(points));
        cout << "cv::Rect box br x y " << box.br().x << " " << box.br().y << endl;
        cout << "cv::Rect box tl x y " << box.tl().x << " " << box.tl().y << endl;
        cout << "cv::Area box " << box.area() << endl;

        //create Mat image
        int rows = box.height;
        int cols = box.width;
        cv::Mat map_image(cols,rows, CV_8UC1, 255);

        for(int i=0; i<globalMapCloud->points.size(); i++)
        {
            int x = (globalMapCloud->points[i].x / req.resolution - box.tl().x);
            int y = (globalMapCloud->points[i].y / req.resolution - box.tl().y);

            if( x > 0 && x < cols
               && y > 0 && y < rows)
               {
                   int value_change = map_image.at<uchar>(x,y) * 0.9;
                   map_image.at<uchar>(x,y) = value_change;
               }
        }

        cv::imwrite(saveMapDirectory + "/global_map.pgm", map_image);

        //write yaml
        std::string pgm_filename = saveMapDirectory + "/global_map.pgm";
        double resolution = req.resolution;

        std::ofstream out_(saveMapDirectory + "/global_map.yaml", std::ios::out | std::ios::binary);
        const Eigen::Vector2d origin(
            box.tl().y * resolution,
            box.br().x * resolution);

        const std::string output = "image: " + pgm_filename + "\n" 
                                   + "resolution: " + std::to_string(resolution) + "\n" 
                                   + "origin: [" + std::to_string(origin.x()) + ", " + std::to_string(origin.y())
                                   + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";

        if (out_.bad())
        {
            return false;
        }
        else
        {
            out_.write(output.data(), output.length());
            out_.close();

            cout << "****************************************************" << endl;
            cout << "Saving yaml map files completed\n"
                 << endl;
        }

        return !out_.bad();