/**
 * This file is part of ORB-SLAM2.
 *
 * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
 * For more information see <https://github.com/raulmur/ORB_SLAM2>
 *
 * ORB-SLAM2 is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * ORB-SLAM2 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef MAPOBJECT_H
#define MAPOBJECT_H

#include "KeyFrame.h"
#include "Frame.h"
#include "Map.h"
#include <map>
#include <random>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <mutex>

namespace ORB_SLAM2
{

    class KeyFrame;
    class Map;
    class Frame;

    class MapObject
    {
    public:
        MapObject(cv::Mat &depth, std::vector<double> &vlabel, float width, cv::Mat &Twc, cv::Mat K, KeyFrame *pRefKF, Map *pMap);
        MapObject(cv::Mat &depth, vector<double> &vlabel, float width, cv::Mat &Twc, cv::Mat K, Frame *pFrame, Map *pMap);

        void AddObservation(KeyFrame *pKF, size_t idx); // 添加关键帧的观测
        MapObject *GetReplaced();
        void IncreaseVisible(int n = 1);
        void IncreaseFound(int n = 1); // 能找到的

        int Observations(); // 获取当前物体的被观测次数

        float GetFoundRatio_object();
        void SetBadFlag();
        bool isBad();                     // 是否为坏点
        bool IsInKeyFrame(KeyFrame *pKF); // 查看某个关键帧是否看到了当前的地图点

        Eigen::Isometry3d Mat2Eigen(cv::Mat &Twc);
        bool dbscan(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, // DBSCAN聚类
                    std::vector<std::vector<int>> &clusters_index,
                    const double &r, const int &size, int &idx);
        void SetColor();

    public:
        pcl::PointCloud<pcl::PointXYZ> mcloud; // 点云
        Eigen::Vector3d mcenter;               // 质心,非齐次坐标
        cv::Mat mbbox;                         // x1,y1,z1,x2,y2,z2,cls 世界坐标系下的坐标
        float mfWidth;                         // 物体宽度
        double mr;                             // 自定义物体颜色
        double mg;
        double mb;

        bool mstate;
        bool mbTrackInView;
        long unsigned int mnLastFrameSeen;
        const long int mnFirstKFid; ///< 创建该MapObject的关键帧ID
        /// Tracking counters
        int mnVisible;
        int mnFound;

        long unsigned int mnTrackReferenceForFrame; // TrackLocalMap - UpdateLocalObjects 中防止将MapObjects重复添加至mvpLocalMapObjects的标记

        int nObs;               // 被观测到的次数
        long unsigned int mnId; ///< Global ID for MapObject
        static long unsigned int nNextId;
        bool mbBad; // 是否是坏点
        Map *mpMap; /// 所属的地图

    protected:
        std::map<KeyFrame *, size_t> mObservations; ///< 观测到该MapObject的KF和该MapObject在KF中的索引
        std::mutex mMutexObject;                    // 对当前物体模型进行操作的互斥量（如添加观测关系的时候）
        MapObject *mpReplaced;                      // 替换当前物体的新物体
    };

} // namespace ORB_SLAM

#endif // MAPOBJECT_H