PCL 强度点云转热力图

cv::Mat Cloud2Image_OnlyImage(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &source, float scale)
{

    //求点云的最值点
    pcl::PointXYZRGB minpt, maxpt;//原始的点云的box的最小点 //原始的点云的box的最大点
    pcl::getMinMax3D(*source, minpt, maxpt);

    int offset = 0;
    float x_length = (maxpt.x - minpt.x) * 1000;
    float y_length = (maxpt.y - minpt.y) * 1000;
    int iwidth = ceil(x_length / scale) + offset * 2;
    int iheight = ceil(y_length / scale) + offset * 2;

    cv::Mat dst = cv::Mat(iheight, iwidth, CV_8UC3, cv::Scalar(255, 255, 255));    //创建值为0的，与源图像一样尺寸的图;

    for (auto point : source->points)
    {
        int ix = floor((point.x - minpt.x) * 1000 / scale) + offset;
        int iy = floor((maxpt.y - point.y) * 1000 / scale) + offset;
        if (ix == iwidth)
            ix = iwidth - 1;
        if (iy == iheight)
            iy = iheight - 1;
        dst.at<cv::Vec3b>(iy, ix)[0] = point.r;//原为point.r，先给一个任意值
        dst.at<cv::Vec3b>(iy, ix)[1] = point.g;
        dst.at<cv::Vec3b>(iy, ix)[2] = point.b;
    }

    return dst;
}

void getPointColor_OnlyImage(float dist, float diff_range, uint8_t& r, uint8_t& g, uint8_t& b, int method)
{
    // 蓝色 -> 青色 -> 绿色 -> 黄色 -> 红色
    r = 0;    g = 0;    b = 0;
    float per_range = diff_range / 2;

    switch (method)
    {
    case 1: //渐变色
    {
        if (dist >= 0)
        {
            if (dist <= per_range)
            {
                r = dist / per_range * 255;
                g = 255;
            }
            else if (dist > per_range && dist <= diff_range)
            {
                r = 255;
                g = (diff_range - dist) / per_range * 255;
            }
            else
            {
                r = 153;
            }
        }
        else
        {
            float fab_dist = -dist;
            if (fab_dist <= per_range)
            {
                g = 255;
                b = fab_dist / per_range * 255;
            }
            else if (fab_dist > per_range && fab_dist <= diff_range)
            {
                g = (diff_range - fab_dist) / per_range * 255;
                b = 255;
            }
            else
            {
                b = 153;
            }
        }

        break;
    }
    case 2: //渐变色2
    {
        if (dist >= 0)
        {
            if (dist <= diff_range)
            {
                r = dist / diff_range * 255;
                g = 255;
            }
            else
            {
                r = 153;
            }
        }
        else
        {
            float fab_dist = -dist;
            if (fab_dist <= diff_range)
            {
                g = 255;
                b = fab_dist / per_range * 255;
            }
            else
            {
                b = 153;
            }
        }

        break;
    }
    case 3:
    {
        if (fabs(dist) <= per_range)
        {
            g = 255;
        }
        else if (dist > 0)
        {
            if (dist <= diff_range)
                r = g = 255;
            else
                r = 255;
        }
        else
        {
            if (dist >= -diff_range)
                g = b = 255;
            else
                b = 255;
        }

        break;
    }
    case 4:
    {
        if (fabs(dist) <= diff_range)
        {
            g = 255;
        }
        else if (dist > diff_range)
        {
            r = 255;
        }
        else
        {
            b = 255;
        }

        break;
    }
    case 5:
    {
        if (dist > 0)
        {
            if (dist < diff_range / 4)
            {
                r = 226;
                g = 137;
                b = 123;
            }
            else if (dist >= diff_range / 4 && dist < diff_range / 2)
            {
                r = 207;
                g = 72;
                b = 68;
            }
            else if (dist >= diff_range / 2 && dist < diff_range * 3 / 4)
            {
                r = 182;
                g = 7;
                b = 22;
            }
            else if (dist >= diff_range * 3 / 4 && dist < 50)
            {
                r = 140;
                g = 0;
                b = 0;
            }
            else if (dist >= 50)
            {
                r = 0;
                g = 0;
                b = 0;
            }
        }
        else if (dist < 0)
        {
            if (dist > -diff_range / 4)
            {
                r = 174;
                g = 185;
                b = 239;
            }
            else if (dist <= -diff_range / 4 && dist > -diff_range / 2)
            {
                r = 107;
                g = 113;
                b = 197;
            }
            else if (dist <= -diff_range / 2 && dist > -diff_range * 3 / 4)
            {
                r = 62;
                g = 56;
                b = 178;
            }
            else if (dist <= -diff_range * 3 / 4 && dist > -50)
            {
                r = 0;
                g = 0;
                b = 138;
            }
            else if (dist <= 50)
            {
                r = 64;
                g = 64;
                b = 64;
            }
        }
        break;
    }
    case 6:
    {
        per_range = diff_range / 4;
        if (dist <= diff_range / 4 && dist >= diff_range / 2)
        {
            r = (diff_range / 4 - dist) / per_range * 255;
            g = 255;
        }
        else if (dist > diff_range / 4)
        {
            r = 255;
            g = abs(dist) / per_range * 255;
        }
        else if (dist < diff_range / 2 && dist >= diff_range / 4 * 3)
        {
            g = 255;
            b = (diff_range / 2 - dist) / per_range * 255;
        }
        else if (dist < diff_range / 4 * 3)
        {
            g = (dist - diff_range) / per_range * 255;
            b = 255;
        }
    }
    return;
    }
}

cv::Mat getOnlyHeatMap(pcl::PointCloud<pcl::PointXYZI>::Ptr &input_pts, double p_up, double p_down)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_pts_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::copyPointCloud(*input_pts, *input_pts_rgb);

    pcl::PointXYZI min_point, max_point;
    pcl::getMinMax3D(*input_pts, min_point, max_point);
    //  范围太大颜色渐变会失败
    float dist_range = 10;
    for (int i = 0; i < input_pts_rgb->points.size(); i++)
    {
        uint8_t r = 0, g = 0, b = 0;
        float dist = input_pts->points[i].intensity;
        getPointColor_OnlyImage(dist - dist_range, dist_range, r, g, b, 1);
        input_pts_rgb->points[i].r = r;
        input_pts_rgb->points[i].g = g;
        input_pts_rgb->points[i].b = b;
        if (input_pts->points[i].intensity < p_up && input_pts->points[i].intensity > p_down)
        {
            input_pts_rgb->points[i].r = 192;
            input_pts_rgb->points[i].g = 192;
            input_pts_rgb->points[i].b = 192;
        }
    }
    return Cloud2Image_OnlyImage(input_pts_rgb, 5);
}

调用时cv::Mat heatImage = getOnlyHeatMap(result, 0, 20);