1-2將點雲轉成深度圖檔

1-2將點雲轉成深度圖檔

程式說明:

範例程式:

#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <pcl/visualization/common/float_image_utils.h>

#ifndef INFINITY
#define INFINITY HUGE_VAL
#endif

#ifndef NAN
#define NAN 0xffc00000
#endif


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

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile("five_people.pcd", *pointCloud);


    // 創建深度圖時，需要決定角度的解析度
    float angularResolution = (float)(0.1f * (M_PI / 180.0f)); // 1.0 degree in radians

    //模擬深度相機最大寬的取像角度
    float maxAngleWidth = (float)(72.0f * (M_PI / 180.0f)); // 72.0 degree in radians

    //模擬深度相機最大高的取像角度
    float maxAngleHeight = (float)(54.0f * (M_PI / 180.0f)); // 54.0 degree in radians

    Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
    float noiseLevel = 0.00;
    float minRange = 0.0f;
    int borderSize = 1;

    pcl::RangeImage rangeImage;
    rangeImage.createFromPointCloud(*pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
       sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);

    float min;
    float max;
    rangeImage.getMinMaxRanges(min, max);
    float min_max_range = max - min;

    std::cout << "min: " << min << std::endl;
    std::cout << "max: " << max << std::endl;

    int width = rangeImage.width;
    int height = rangeImage.height;

    std::cout << "width: " << width << std::endl;
    std::cout << "height: " << height << std::endl;

    cv::Mat rangeImgCV = cv::Mat(cv::Size(width,height), CV_32FC1, cv::Scalar(0));
    cv::Mat rangeImgCV8U = cv::Mat(cv::Size(width, height), CV_8UC3, cv::Scalar(0));


    unsigned char r, g, b;
    for (int y = 0; y < height; y++)
    {
       for (int x = 0; x < width; x++)
       {
          pcl::PointWithRange rangePt = rangeImage.getPoint(x, y);
          if (rangePt.range == INFINITY || rangePt.range == -INFINITY || rangePt.range == NAN)
          {
             pcl::visualization::FloatImageUtils::getColorForFloat(rangePt.range, r, g, b);
          }
          else
          {
             float value = (rangePt.range - min) / min_max_range;
             pcl::visualization::FloatImageUtils::getColorForFloat(value, r, g, b);
          }

          rangeImgCV8U.at(y, x)[0] = b;
          rangeImgCV8U.at(y, x)[1] = g;
          rangeImgCV8U.at(y, x)[2] = r;
       }
    }

    cv::imwrite("range.png", rangeImgCV8U);
    std::cout << rangeImage << "\n";
    return 0;
}