Guide-Vest/include/DepthProcessor.hpp

#pragma once

#include "opencv2/core/core.hpp"

#include <iostream>
#include <vector>
#include <string>
// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry> 

// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/PointIndices.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

// boost.format 字符串处理
#include <boost/format.hpp>

using namespace std;
using namespace pcl;

// 定义点云类型
typedef PointCloud<PointXYZ> PointCloudXYZ;

class DepthProcessor{
private:
    cv::Mat imgRGB;
    cv::Mat imgDepth;   // or pointCloud detect by lidar
   
    cv::Mat obstacle;   // [dis, cls, maby size]

    
public:
    //点云显示对象
    visualization::PCLVisualizer::Ptr pcdViewer;
    visualization::PCLVisualizer::Ptr tmpViewer;
    cv::Mat mK; // camera intrinsic or external parameters between camera and lidar
    //最近物体信息
    double dist, max_x, max_y, min_x, min_y;
    std::string label;
    // 创建点云对象
    PointCloudXYZ::Ptr cloud;
    //相机参数
    double fx, fy, cx, cy;
    //深度阈值，单位米
    double minDepth, maxDepth;
    //过滤孤立点的阈值/
    int thre_count;
    double radius;
    //过滤通行区域的阈值
    double thre_low_x, thre_high_x, thre_low_y, thre_high_y;

    //地面高度阈值
    double thre_ground;

    //相机初始化高度
    float camHigh;

    //判断物体的点云个数阈值、半径阈值
    int thre_ponit_count, thre_point_radius;
    float ground_height;
    
    DepthProcessor(const std::string file_path, const cv::Mat tmp_mk);
    bool obstacleDetect(const cv::Mat depthMat, const Eigen::Affine3f& transform);
    PointCloudXYZ::Ptr depth2Cloud(PointCloudXYZ::Ptr oriCloud, const cv::Mat depthMat, int startRow, int endRow);
    bool camHighInit(const cv::Mat depthMat, const Eigen::Affine3f& transform);

    static PointCloudXYZ::Ptr downsampling(const PointCloudXYZ::Ptr& pointCloud, float leaf);
    static PointCloudXYZ::Ptr axisFilter(const PointCloudXYZ::Ptr& pointCloud, const double&thre_low, const double& thre_high, const string& field, const bool& flag_in);

    PointCloudXYZ::Ptr delGround(const PointCloudXYZ::Ptr& pointCloud);
    static void calHistogram(const PointCloudXYZ::Ptr& pointCloud, float& maxY, std::vector<std::vector<int>>& histogram);
    static void radiusSearch(const PointCloudXYZ::Ptr& pointCloud, const PointXYZ& queryPoint, const float& radius, std::vector<int>& indResults);

    PointCloudXYZ::Ptr delGround_else(const PointCloudXYZ::Ptr& pointCloud, std::vector<int> index);

    void RadiusOutlierFilter(const PointCloudXYZ::Ptr &pcd_cloud0);
    
    void load_config(const string filepath);

    void ConditionFilter(const double &ground);
    double IOU(double max_x1,double min_x1,double max_y1,double min_y1,double max_x2,double min_x2,double max_y2,double min_y2);

    void calHistogramZ(const PointCloudXYZ::Ptr &pointCloud, std::vector<std::vector<int>>& histogram);
    
    bool obstacleRecognize(cv::Mat boxes);
    cv::Mat getObstacle();
};

//用作深度转点云的 多线程并行操作
class Depth2cloud : public cv::ParallelLoopBody{
    public:
    cv::Mat depthMat;
    PointCloudXYZ::Ptr _cloud;
    Depth2cloud(cv::Mat mat, PointCloudXYZ::Ptr cloud):depthMat(mat), _cloud(cloud){};

    virtual void operator()(const cv::Range& range) const override{
        PointCloudXYZ::Ptr tmpcloud(new PointCloudXYZ);
        int col = depthMat.cols;
        cv::Mat image = depthMat.clone();
 	image.convertTo(image, CV_32FC1);
	
        for(int i=range.start;i<range.end;i++){
            for(int j=0;j<col;j++){
                // 获取深度值
                float depth = image.at<float>(i, j);
                // 忽略深度值小于320mm的点
                if (depth <= 320)
                    continue;
    
                // 计算三维坐标
                double X = (j - 322.771) * depth / 388.588 / 1000;
                if(X > 0.5 || X < -0.5)
                    continue;
                double Y = (i - 236.947) * depth / 388.588 / 1000;
                double Z = depth / 1000;
    
                // 创建点云点并设置坐标
                PointXYZ point;
                point.x = X;
                point.y = Y;
                point.z = Z;
    
                // 将点添加到点云中
                tmpcloud->push_back(point);
            }
        }
        *_cloud = *tmpcloud; 
    }
};