使用C++与PCL详细过程版实现的点云转强度图像

#include <numeric> #include"PointCloudToIntensity.h" bool Point2Image::setInputCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& input_cloud) { m_cloud = input_cloud; return true; } void Point2Image::compute(cv::Mat& img_fliped) { //1、进行格网化 float Xmax, Xmin, Ymax, Ymin; pcl::PointXYZI min_pt, max_pt; pcl::getMinMax3D(*m_cloud, min_pt, max_pt); Ymax = max_pt.y; Ymin = min_pt.y; Xmax = max_pt.x; Xmin = min_pt.x; int Row = ceil((Ymax - Ymin) / m_step);//格网行数 int Col = ceil((Xmax - Xmin) / m_step);//格网列数 //2、构建包含点的格网数据结构 std::vector<float>** PointsInGrid = new std::vector<float>*[Row]; std::vector<std::vector<float>> aveIntensity(Row); for (int i = 0; i < Row; ++i) { PointsInGrid[i] = new std::vector<float>[Col]; aveIntensity[i].resize(Col); } //3、计算任一点所在的行列号及强度最小值 float minInsty = FLT_MAX; for (int i = 0; i < (*m_cloud).size(); ++i) { int rowID = std::floor((m_cloud->points[i].y - Ymin) / m_step); int colID = std::floor((m_cloud->points[i].x - Xmin) / m_step); PointsInGrid[rowID][colID].push_back(m_cloud->points[i].intensity); minInsty = std::min(minInsty, m_cloud->points[i].intensity); } //4、计算格网强度均值的最值 float minAveI = FLT_MAX; float maxAveI = -FLT_MAX; for (int i = 0; i < Row; ++i) { for (int j = 0; j < Col; ++j) { // 格网内有点，则进行计算 if (PointsInGrid[i][j].size() > 0) { float meanValue = 0.0; meanValue = std::accumulate(PointsInGrid[i][j].begin(), PointsInGrid[i][j].end(), meanValue); meanValue /= float(PointsInGrid[i][j].size()); aveIntensity[i][j] = meanValue; maxAveI = std::max(maxAveI, meanValue); minAveI = std::min(minAveI, meanValue); } } } delete[] PointsInGrid; //5、根据点云强度给图像赋色 cv::Mat img(Row, Col, CV_8UC1, cv::Scalar(0, 0, 0));//cv::Scalar(0, 0, 0)设置图像背景颜色 //6、强度均值拉伸到0-255之间赋值 for (int i = 0; i < Row; i++) { for (int j = 0; j < Col; j++) { int pixel = 0; // 强度均值大于点云的最小强度，则说明该格网内有点 if (aveIntensity[i][j] > minAveI) { pixel = (int)((aveIntensity[i][j] - maxAveI) / (maxAveI - minAveI) * 255); } img.ptr<uchar>(i)[j] = pixel; } } // 按照y轴堆成（图像和点云坐标的原点不一致导致） cv::flip(img, img_fliped, 0); }