根据形变程度给点云上色

最近的激光雷达项目老师要求我做一个根据叶片的形变程度给点云上颜色。

我的想法是通过计算出点云每个点的法向量和曲率

然后根据曲率的大小遍历点云添加颜色

很简单，也很容易实现

因为pcl有直接的代码可以算出点云的曲率，有这一步就成功了一半了。

计算出曲率的最大和最小值，

曲率最小的时候点云为红色

曲率最大的时候点云为蓝色

红色到蓝色的渐变过程需要设置三原色中的green 随曲率值的变化呈正态分布函数，

代码那条公式很简单的，有兴趣可以自己在matlab调出来。

效果不好的话重点调这两个参数就好了，根据点云的特点而变，我的点云坐标比较大，所以参数很大。

只能选择其中一个

 ne.setKSearch(300);
 //ne.setRadiusSearch(8);

上代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>

#include <fstream>  
#include <string>  
#include <vector> 
#include <cmath>

using namespace std;
int main()
{

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile("./cloud_point/xyz_file/LeafPcd/second_test_only2.pcd", *cloud1);

    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr pcNormal(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree->setInputCloud(cloud1);
    ne.setInputCloud(cloud1);
    ne.setSearchMethod(tree);
    //ne.setKSearch(300);
    ne.setRadiusSearch(8);
    ne.compute(*pcNormal);    // 法向量0-2位 曲率3位

    float Qmin = 9999, Qmax = 0;
    // 查找最大最小曲率
    for (int i = 0; i < pcNormal->points.size(); i++)
    {
        if (pcNormal->points[i].curvature > Qmax) Qmax = pcNormal->points[i].curvature;
        if (pcNormal->points[i].curvature < Qmin) Qmin = pcNormal->points[i].curvature;
    }
    //Qmax = 0.0898027; Qmin = 0.0243475;     // 以形变最大的叶片为基准
    cout << "The max curvature is: " << Qmax << "    The min curature is : " << Qmin << endl;
    float Qlen = Qmax - Qmin;
    float tem, tem_y;
    // 上色
    for (int j = 0; j < cloud1->points.size(); ++j)
    {
        if (pcNormal->points[j].curvature < Qmin)
        {
            cloud1->points[j].r = 255;
            cloud1->points[j].g = 0;
            cloud1->points[j].b = 0;
        }
        else if (pcNormal->points[j].curvature > Qmax)
        {
            cloud1->points[j].r = 0;
            cloud1->points[j].g = 0;
            cloud1->points[j].b = 255;
        }
        else 
        {
            tem = (float(pcNormal->points[j].curvature - Qmin) / Qlen) * 255;
            cloud1->points[j].b = tem;
            cloud1->points[j].r = 255 - tem;
            // 正态函数 u = 255/2,theta^2 = 40
            tem_y = (1 / (sqrt(2 * 3.1415) * 40)) * exp(-(tem - 127.5)*(tem - 127.5) / (2 * 40 * 40));   
            cloud1->points[j].g = (tem_y / 0.012) * 255;
        }
        
    }

    pcl::visualization::PCLVisualizer viewer1("1");
    viewer1.setBackgroundColor(0, 0, 0);//背景颜色
    //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZRGB> color1(cloud1, "x");
    viewer1.addPointCloud<pcl::PointXYZRGB>(cloud1);

    while (!viewer1.wasStopped())
    {
        viewer1.spinOnce(1);
    }
    system("pause");
}

效果图：

效果还是挺明显的~

另外，我的第一篇博客哈哈