Guide-Vest/include/DepthProcessor.hpp

#pragma once

#include "opencv2/core/core.hpp"
#include <opencv2/core/utility.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 imgDepth;   // or pointCloud detect by lidar
    
public:
    //控制显示和打印
    bool bPrintObjs = false;
    bool bViewRGB = false;
    bool bViewCloud = false;

    //点云显示对象
    visualization::PCLVisualizer::Ptr pcdViewer;

    //最近物体信息
    double dist, max_x, max_y, min_x, min_y;
    std::string label;
    // 创建点云对象
    PointCloudXYZ::Ptr cloud;
    //相机参数
    double fx, fy, cx, cy;

    //相机初始化高度
    float camHigh;
    
    DepthProcessor();
    bool obstacleDetect(const cv::Mat depthMat, const Eigen::Affine3f& transform);
    PointCloudXYZ::Ptr depth2Cloud(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);

    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);
};