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

Posted 2021-09-16 COCO_PEAK_NOODLE

话不多说直接上代码

        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();

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