点云各向异性扩散滤波

参考论文：各向异性扩散滤波的三维散乱点云平滑去噪算法

博主学识尚浅，针对该算法实现了一半，现将已完成的算法（半成品）分享给大家，希望可以得到大家的帮助，其中对于I对x,y,z求偏导不知道如何进行编程。

公式已贴图给出。

 

 

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/eigen.h>//eigen矩阵运算库
#include <math.h>

using namespace std;
using pointT = pcl::PointXYZ;

int main()
{
    //定义超参数
    int m = 16, k = 10, tao = 1;//k近邻个数，迭代次数，时间间隔
    float alpha = 0.005;//扩散因子

    pcl::PointCloud<pointT>::Ptr cloud_in(new pcl::PointCloud<pointT>);
    //加载点云
    pcl::io::loadPCDFile<pointT>("", *cloud_in);

    if (!cloud_in->size())
        return 0;

    //创建kdtree
    pcl::KdTreeFLANN<pointT> kdtree;//定义kdtree
    kdtree.setInputCloud(cloud_in);

    //循环处理每一个点
    for(size_t i=0;i<cloud_in->size();i++)
    {
        vector<int> indices(0,0);//初始化索引
        indices.reserve(m);
        vector<float> dist(0,0.0);//初始化距离
        dist.reserve(m);
        pcl::PointCloud<pointT>::Ptr neigh_cloud(new pcl::PointCloud<pointT>);

        if(kdtree.nearestKSearch(cloud_in->points[i],m,indices,dist)>0)
        {
            //将原点云按照索引值复制到近邻点云
            pcl::copyPointCloud(*cloud_in,indices,*neigh_cloud);
            
            //计算sigma
            float sigma = 0.0;
            vector<float> sm(0,0.0);
            for(int i=0;i<neigh_cloud->size();i++)
            {
                sm.push_back(dist[i]);
                sigma += dist[i];
            }
            sigma =sigma / m;

            //计算张量投票矩阵
            Eigen::Matrix3f T;
            //创建单位矩阵
            Eigen::Matrix3f T3 = Eigen::Matrix3f::Identity();
            for(int j=0;j<neigh_cloud->size();j++)
            {
                float temp = - (pow(sm[i],2) / pow(sigma,2));
                float um = exp(temp); 
                Eigen::Vector3f vm;
                vm(0) = cloud_in->points[i].x - neigh_cloud->points[j].x;
                vm(1) = cloud_in->points[i].y - neigh_cloud->points[j].y;
                vm(2) = cloud_in->points[i].z - neigh_cloud->points[j].z;
                
                T += um * (T3 - (vm * vm.transpose()) / (vm * vm.transpose()).squaredNorm());
            }

            //计算T的特征值和特征向量
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(T);
            Eigen::Vector3f lambda = es.eigenvalues();
            Eigen::Matrix3f E = es.eigenvectors();

            //计算扩散张量
            float u1,u2,u3;
            float k12,k13,k23;

            k12 = pow((lambda(0)-lambda(1)),2);
            k13 = pow((lambda(0)-lambda(2)),2);
            k12 = pow((lambda(1)-lambda(2)),2);

            u1 = alpha;
            if(k12 == 0)
            {
                u2 = alpha;
            }
            else
            {
                u2 = alpha + (1-alpha) * exp(-(1/k12));
            }

            if(k23 == 0)
            {
                u3 = u2;
            }
            else
            {
                u3 = u2 + (1-u2) * exp(-(1/k23));
            }

            Eigen::Vector3f e1 = E.col(0);//第一个特征向量
            Eigen::Vector3f e2 = E.col(1);//第二个特征向量
            Eigen::Vector3f e3 = E.col(2);//第三个特征向量

            //计算扩散张量D的系数
            float d11 = u1 * pow(e1(0),2) + u2 * pow(e1(1),2) + u3 * (e1(2),2);
            float d22 = u1 * pow(e2(0),2) + u2 * pow(e2(1),2) + u3 * (e2(2),2);
            float d33 = u1 * pow(e3(0),2) + u2 * pow(e3(1),2) + u3 * (e3(2),2);
            float d12 = u1 * e1(0) * e2(0) + u2 * e1(1) * e2(1) + u3 * e1(2) * e2(2);
            float d13 = u1 * e1(0) * e3(0) + u2 * e1(1) * e3(1) + u3 * e1(2) * e3(2);
            float d23 = u1 * e2(0) * e3(0) + u2 * e2(1) * e3(1) + u3 * e2(2) * e3(2);

            Eigen::Matrix3f D;
            D << d11,d12,d13,
                 d12,d22,d23,
                 d13,d23,d33;
            
            //计算I的导数
            

        }

    }


    return 0;
}