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

话不多说直接上代码

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

        cout << "****************************************************" << endl;
        cout << "Saving projected map to pcd files completed
"
             << 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 + "
" 
                                   + "resolution: " + std::to_string(resolution) + "
" 
                                   + "origin: [" + std::to_string(origin.x()) + ", " + std::to_string(origin.y())
                                   + ", 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
";

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

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

        return !out_.bad();

可能你的坐标系和我不同，适当调整一下