#include "PointCloudAlg.h"
#include <QAction>
#include <QCloseEvent>
#include <QColor>
#include <QComboBox>
#include <QDateTime>
#include <QDebug>
#include <QDesktopWidget>
#include <QFileDialog>
#include <QFontComboBox>
#include <QImage>
#include <QLabel>
#include <QMainWindow>
#include <QMenu>
#include <QMenuBar>
#include <QMessageBox>
#include <QProgressDialog>
#include <QRect>
#include <QSettings>
#include <QSpinBox>
#include <QString>
#include <QStringList>
#include <QTextCharFormat>
#include <QTime>
#include <QToolBar>
#include <QToolButton>
#include <QListWidgetItem>

#include <QApplication>
#include <QDateTime> //添加QDateTime头文件
#include <QMovie>
#include <QPixmap>
#include <QSplashScreen>
#include <QTextCodec>

// Point Cloud Library
#include <QColorDialog>
#include <iostream>
#include <pcl/common/common.h>
#include <pcl/console/time.h> // TicToc
#include <pcl/console/time.h>
#include <pcl/filters/convolution_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <boost/random.hpp> //随机数
#include <boost/thread/thread.hpp>
#include <iostream>
#include <pcl/console/time.h> // TicToc
#include <pcl/features/normal_3d.h>
#include <pcl/filters/bilateral.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/fast_bilateral.h>
#include <pcl/filters/fast_bilateral_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/ia_fpcs.h>  // 4PCS算法
#include <pcl/registration/ia_kfpcs.h> //K4PCS算法头文件
#include <pcl/registration/icp.h>
#include <pcl/registration/registration.h>
#include <pcl/search/flann_search.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/gp3.h> //贪婪投影三角化算法类定义的头文件
#include <pcl/surface/marching_cubes_hoppe.h> //移动立方体
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/mls.h>     //MLS
#include <pcl/surface/poisson.h> //泊松重建
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
#include <vector>
#include <vtkOutputWindow.h>

#include <pcl/console/time.h>                   // TicToc
#include <pcl/filters/approximate_voxel_grid.h> // 体素滤波
#include <pcl/filters/voxel_grid.h>             // 体素滤波
#include <vtkOutputWindow.h>

#include <boost/thread/thread.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h> //随机参数估计方法
#include <pcl/sample_consensus/model_types.h>  //模型定义
#include <pcl/segmentation/sac_segmentation.h> //RANSAC分割
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include <stdlib.h> //rand()头文件
#include <vtkMassProperties.h>
#include <vtkPLYReader.h>
#include <vtkSmartPointer.h>
#include <vtkTriangleFilter.h>

#include <Eigen/Core>
#include <iostream>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
#include <string>
#include <vector>
#include <vtkAutoInit.h>
//可视化相关头文件
#include <vtkActor.h>
#include <vtkAutoInit.h>
#include <vtkInteractorStyleTrackballCamera.h>
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
// Boost
#include <boost/math/special_functions/round.hpp>

// Visualization Toolkit (VTK)
#include <vtkRenderWindow.h>
#include "BasePCL.h"
//
//
//// 构建
void NormalEstimation(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud, pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals)
{
    //----------------法线估计-------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::copyPointCloud(*cloud, *cloud_in);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_in);
    n.setInputCloud(cloud_in);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    n.compute(*normals);
    pcl::concatenateFields(*cloud_in, *normals, *cloud_with_normals);
}
//
///*
//                                下采样
//*/
void downSampling(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered,
    double leafSize )
{
    pcl::VoxelGrid<pcl::PointXYZRGBA> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(leafSize, leafSize, leafSize);
    vg.filter(*cloud_filtered);
    int downsampled_points_total_size = cloud_filtered->points.size();
    // show_point_cloud(cloud_filtered, "down sampled point cloud");
    qDebug() << "PointCloud after filtering has: "
        << downsampled_points_total_size << " data points." << "\n";
};

/*
                                添加高斯噪声
*/
void GenerateGaussNoise(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& cloud,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& noise_cloud,
    double miu ,
    double sigma  )
{
    noise_cloud->points.resize(
        cloud->points.size()); //将点云cloud的size赋值给噪声
    noise_cloud->header = cloud->header;
    noise_cloud->width = cloud->width;
    noise_cloud->height = cloud->height;
    boost::mt19937 my_seed; // 随机数生成
    my_seed.seed(static_cast<unsigned int>(time(0)));
    boost::normal_distribution<> nd(
        miu, sigma); // 创建一个有特定期望值和标准差的正态分布：
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<>> ok(
        my_seed, nd);
    for (size_t i = 0; i < cloud->size(); ++i) {
        noise_cloud->points[i].x = cloud->points[i].x + static_cast<float>(ok());
        noise_cloud->points[i].y = cloud->points[i].y + static_cast<float>(ok());
        noise_cloud->points[i].z = cloud->points[i].z + static_cast<float>(ok());
    }
};

/*
                                高斯滤波
*/

void gaussian_filter(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered,
    double radius)
{
    /*double radius = 0.02;*/
    // Set up the Gaussian Kernel
    pcl::filters::GaussianKernel<pcl::PointXYZRGBA, pcl::PointXYZRGBA>::Ptr kernel(
        new pcl::filters::GaussianKernel<pcl::PointXYZRGBA, pcl::PointXYZRGBA>);
    (*kernel).setSigma(4);
    (*kernel).setThresholdRelativeToSigma(4);
    qDebug() << "Kernel made" << "\n";

    // Set up the KDTree
    pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr kdtree(
        new pcl::search::KdTree<pcl::PointXYZRGBA>);
    (*kdtree).setInputCloud(cloud);
    qDebug() << "KdTree made" << "\n";

    // Set up the Convolution Filter
    pcl::filters::Convolution3D<
        pcl::PointXYZRGBA,
        pcl::PointXYZRGBA,
        pcl::filters::GaussianKernel<pcl::PointXYZRGBA, pcl::PointXYZRGBA>>
        convolution;
    convolution.setKernel(*kernel);
    convolution.setInputCloud(cloud);
    convolution.setSearchMethod(kdtree);
    convolution.setRadiusSearch(radius);
    convolution.setNumberOfThreads(
        10); // important! Set Thread number for openMP
    qDebug() << "Convolution Start" << "\n";
    convolution.convolve(*cloud_filtered);
    qDebug() << "Convoluted" << "\n";
};

void bilateralFilter(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output,
    float sigma_s,
    float sigma_r)
{

    pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr tree(
        new pcl::KdTreeFLANN<pcl::PointXYZRGBA>);

    // Apply the filter
    pcl::FastBilateralFilter<pcl::PointXYZRGBA> fbf;
    fbf.setInputCloud(input);
    fbf.setSigmaS(sigma_s);
    fbf.setSigmaR(sigma_r);
    fbf.filter(*output);
};
/*
                                半径滤波器:
                                基于密度的杂散点移除
*/
void radius_filter(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered_out,
    double RadiusSearch  ,
    int minNearSearch  )
{
    pcl::RadiusOutlierRemoval<pcl::PointXYZRGBA> sor;
    sor.setInputCloud(cloud);
    sor.setRadiusSearch(RadiusSearch); //设置半径为0.04m的范围内找临近点
    sor.setMinNeighborsInRadius(minNearSearch); //设置查询点的邻域点集数小于2的删除
    sor.filter(*cloud_filtered); //
    sor.setNegative(true);     //以获取离群点数据(也就是原本滤除掉的点)

    sor.filter(*cloud_filtered_out);
};
/*
                                统计滤波器:
                                基于统计的杂散点移除
*/
void statistical_filter(
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered,
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered_out,
    int meanK ,
    double StddevThresh)
{
    // 创建滤波器对象
    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor;
    sor.setInputCloud(cloud); //设置待滤波的点云
    sor.setMeanK(meanK); //设置在进行统计时考虑查询点邻居点数，K个最近邻点
    sor.setStddevMulThresh(StddevThresh); //设置判断是否为离群点的阈值
    sor.filter(*cloud_filtered); //将滤波结果保存在cloud_filtered中
    sor.setNegative(true); //以获取离群点数据(也就是原本滤除掉的点)
    sor.filter(*cloud_filtered_out);
    // writer.write<PointTRGBA>("table_scene_lms400_outliers.pcd",
    // *cloud_filtered, false);
};


//
///************************************************************************/
///* 表面重建 */
///************************************************************************/
//
//void MLS(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in,
//    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals)
//{
//    qDebug() << "MLS alg module:" << cloud_in->points.size();
//
//    pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA>); // 创建一个KD树
//    // 输出文件中有PointNormal类型，用来存储移动最小二乘法算出的法线
//    // pcl::PointCloud<pcl::PointNormal> mls_points;
//    // 定义对象 (第二种定义类型是为了存储法线, 即使用不到也需要定义出来)
//    pcl::MovingLeastSquares<pcl::PointXYZRGBA, pcl::PointNormal> mls;
//    mls.setComputeNormals(true);
//    //设置参数
//    mls.setInputCloud(cloud_in);
//    mls.setPolynomialOrder(5);  // 在未设置拟合阶数时，默认阶数是2
//    mls.setSearchMethod(tree);
//    mls.setSearchRadius(0.005);
//    // 曲面重建
//    mls.process(*cloud_with_normals);
//    qDebug() << "MLS alg module over:" << cloud_with_normals->points.size();
//};
//void Pos(pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals,
//    pcl::PolygonMesh& mesh,
//    int Degree,
//    int Depth,
//    int IsoDivide ,
//    float SamplesPerNode,
//    int SolverDivide)
//{
//    //------------泊松重建-----------------------------
//    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2( new pcl::search::KdTree<pcl::PointNormal>);
//    tree2->setInputCloud(cloud_with_normals);
//    pcl::Poisson<pcl::PointNormal> pn;
//    pn.setConfidence(false); //是否使用法向量的大小作为置信信息。如果false，所有法向量均归一化。
//    pn.setDegree(Degree); //设置参数degree[1,5],值越大越精细，耗时越久。
//    pn.setDepth(Depth);    //树的最大深度，求解2^d x 2^d x 2^d立方体元。
//                        // 由于八叉树自适应采样密度，指定值仅为最大深度。
//
//    pn.setIsoDivide(IsoDivide); //用于提取ISO等值面的算法的深度
//    pn.setManifold(false); //是否添加多边形的重心，当多边形三角化时。
//                             // 设置流行标志，如果设置为true，则对多边形进行细分三角话时添加重心，设置false则不添加
//    pn.setOutputPolygons(false); //是否输出多边形网格（而不是三角化移动立方体的结果）
//    pn.setSamplesPerNode(SamplesPerNode); //设置落入一个八叉树结点中的样本点的最小数量。无噪声，[1.0-5.0],有噪声[15.-20.]平滑
//    pn.setScale(1); //设置用于重构的立方体直径和样本边界立方体直径的比率。
//    pn.setSolverDivide(SolverDivide); //设置求解线性方程组的Gauss-Seidel迭代方法的深度
//    // pn.setIndices();
//
//    //设置搜索方法和输入点云
//    pn.setSearchMethod(tree2);
//    pn.setInputCloud(cloud_with_normals);
//    pn.performReconstruction(mesh);
//};
//// 贪婪三角网
void GP(pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals,
    pcl::PolygonMesh& mesh,
    double SearchRadius ,
    double Mu,
    int MaximumNearestNeighbors,
    double MaximumSurfaceAngle,
    double MinimumAngle,
    double MaximumAngle)
{
    qDebug() << "[GP ]The number of points :" << cloud_with_normals->points.size();
    //------------------定义搜索树对象------------------------
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(
        new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud(cloud_with_normals);
    //------------------贪婪投影三角化------------------
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3; //定义三角化对象

    gp3.setSearchRadius(SearchRadius); //设置连接点之间的最大距离（即三角形的最大边长）
    gp3.setMu(Mu); //设置被样本点搜索其临近点的最远距离，为了适应点云密度的变化
    gp3.setMaximumNearestNeighbors(MaximumNearestNeighbors); //设置样本点可搜索的邻域个数
    gp3.setMaximumSurfaceAngle(MaximumSurfaceAngle); // 设置某点法线方向偏离样本点法线方向的最大角度
    gp3.setMinimumAngle(MinimumAngle); // 设置三角化后得到三角形内角的最小角度
    gp3.setMaximumAngle(MaximumAngle); // 设置三角化后得到三角形内角的最大角度
    gp3.setNormalConsistency(false); //设置该参数保证法线朝向一致
    //qDebug() << "[GP] start reconstruct :" << cloud_in->points.size();
    // Get result
    gp3.setInputCloud(cloud_with_normals); //设置输入点云为有向点云
    gp3.setSearchMethod(tree2);            //设置搜索方式
    gp3.reconstruct(mesh);                 //重建提取三角化
    // cout << triangles;
   // qDebug() << "[GP ]  reconstruct process over :" << cloud_in->points.size();
};
//
////移动立方体
//void MC(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud, pcl::PolygonMesh& mesh)
//{
//    ////-------------------法线估计----------------------
//    pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> n;
//    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
//    pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(
//        new pcl::search::KdTree<pcl::PointXYZRGBA>);
//    tree->setInputCloud(cloud);
//    n.setInputCloud(cloud);
//    n.setSearchMethod(tree);
//    n.setKSearch(5);
//    n.compute(*normals);
//    //----------将点云和法线放到一起------------------
//    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(
//        new pcl::PointCloud<pcl::PointNormal>);
//    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
//    //---------初始化MarchingCubes对象，并设置参数-------
//    pcl::MarchingCubes<pcl::PointNormal>* mc;
//    mc = new pcl::MarchingCubesHoppe<pcl::PointNormal>();
//    // mc = new pcl::MarchingCubesRBF<pcl::PointNormal>();
//    //创建搜索树
//    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(
//        new pcl::search::KdTree<pcl::PointNormal>);
//    tree2->setInputCloud(cloud_with_normals);
//    mc->setInputCloud(cloud_with_normals);
//    //设置MarchingCubes对象的参数
//    mc->setIsoLevel(0.0f); //该方法设置要提取表面的iso级别
//    int resolution = 50;
//    mc->setGridResolution(
//        resolution, resolution, resolution); //用于设置行进立方体网格分辨率
//    mc->setPercentageExtendGrid( 0.02); //该参数定义在点云的边框和网格限制之间的网格内应该保留多少自由空间
//
//    mc->reconstruct(mesh); //执行重构，结果保存在mesh中
//};
//
///************************************************************************/
///* 点云配准算法 */
///************************************************************************/
//
////// 接下来使用的点云指针
////PointCloudT::Ptr cloud_in; // Original point cloud
////PointCloudT::Ptr cloud_tr; // Transformed point cloud
////PointCloudT::Ptr cloud_RE; // ICP output point cloud
////
//////使用配准基类Registration来实现多态
////pcl::Registration<PointT, PointT>::Ptr ALIGN;
////pcl::IterativeClosestPoint<PointT, PointT>::Ptr icp;
////
//////迭代次数
////int iterations = 1; // Default number of ICP iterations
////// bool next_iteration = false;
//
///*
//                打印4*4的矩阵
//*/
void print4x4Matrix(const Eigen::Matrix4d& matrix)
{
    printf("\nRotate Matrix :\n");
    printf(
        "    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
    printf(
        "R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
    printf(
        "    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
    printf("\nVector :\n");
    printf("t = < %6.3f, %6.3f, %6.3f >\n\n",
        matrix(0, 3),
        matrix(1, 3),
        matrix(2, 3));
};
//
//// TODO
///*
//              4*4的矩阵
//*/
QString get4x4MatrixStr(const Eigen::Matrix4d& matrix)
{
    QString res = QString::number(matrix(0, 0), 'g', 4) + "\t" +
        QString::number(matrix(0, 1), 'g', 4) + "\t" +
        QString::number(matrix(0, 2), 'g', 4) + "\t" +
        QString::number(matrix(0, 3), 'g', 4) + "\n";
    res += QString::number(matrix(1, 0), 'g', 4) + "\t" +
        QString::number(matrix(1, 1), 'g', 4) + "\t" +
        QString::number(matrix(1, 2), 'g', 4) + "\t" +
        QString::number(matrix(1, 3), 'g', 4) + "\n";
    res += QString::number(matrix(2, 0), 'g', 4) + "\t" +
        QString::number(matrix(2, 1), 'g', 4) + "\t" +
        QString::number(matrix(2, 2), 'g', 4) + "\t" +
        QString::number(matrix(2, 3), 'g', 4) + "\n";
    res += QString::number(0, 'g', 4) + "\t" + QString::number(0, 'g', 4) +
        "\t" + QString::number(0, 'g', 4) + "\t" +
        QString::number(1, 'g', 4) + "\n";
    return res;
};
