#ifndef SEMANTIC_MAP_H
#define SEMANTIC_MAP_H

#include <iostream>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

#include "KeyFrame.h"

using namespace std;
using namespace cv;
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

namespace ORB_SLAM2
{
    class KeyFrame;

    class Semantic_Map
    {
    public:
        Semantic_Map(const string &strSettingPath);
        void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel);
        void get_bbx(const vector<double> &label1, std::vector<cv::Point> &pts, double scale);
        void Run();
        void InsertKeyFrame(KeyFrame *pKF);
        bool CheckNewKeyFrames();
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr GenratePointCloudMap(KeyFrame *mpCurrentKF, cv::String cloudAddress, cv::String imgadress);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr GenratePointCloudMap(KeyFrame *mpCurrentKF, cv::String cloudAddress, cv::String imgadress, std::vector<std::vector<double>> vvlabel);
        void SetPose(Eigen::Isometry3d &T, cv::Mat &Twc);
        // void SetCurrentKeyFrame(KeyFrame *pkf);
        void Transform3DPointFromLidarToImage02KITTI(const cv::Mat &x3Dl, cv::Mat &x3Dc, cv::Point2i &point2D, float &d);
        /**请求终止当前线程 */
        void RequestFinish();
        /** 当前线程的run函数是否已经终止 */
        bool isFinished();
        bool CheckFinish();
        /** 设置当前线程已经真正地结束了,由本线程run函数调用 */
        void SetFinish();
        bool Stop();
        bool isStopped();

    public:
        // pcl::PointCloud<pcl::PointXYZI>::Ptr mpPointCloud;
        std::vector<cv::String> mvpointcloud_adress;
        std::vector<cv::String> mvRGBIMG_adress;
        std::vector<std::vector<std::vector<double>>> mvvvLabel;
        vector<vector<double>> mvDynamic_label;
        cv::Mat mRGB;
        cv::Mat mDepth;
        cv::Mat P2;
        cv::Mat R0_rect;
        cv::Mat Tr_velo_to_cam;
        Mat trans;
        // Mat p_result;
        // Mat mimg;

        float fx, fy, cx, cy, mDepthMapFactor;
        // cv::Mat Twc;
        std::vector<KeyFrame *> mlpSemanticKeyFrameQueue;
        // KeyFrame *mpCurrentKF;
        // PointCloud::Ptr mpPointCloud;

        std::mutex mMutexSemanticQueue;

        /// 当前线程是否收到了请求终止的信号
        bool mbFinishRequested;
        /// 当前线程的主函数是否已经终止
        bool mbFinished;
        // 和"线程真正结束"有关的互斥锁
        std::mutex mMutexFinish;
    };
}

#endif