/**
 * 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 TRACKING_H
#define TRACKING_H

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "Viewer.h"
#include "FrameDrawer.h"
#include "Map.h"
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "Frame.h"
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
#include "ORBextractor.h"
#include "Initializer.h"
#include "MapDrawer.h"
#include "System.h"

#include <mutex>

namespace ORB_SLAM2
{

    class Viewer;
    class FrameDrawer;
    class Map;
    class LocalMapping;
    class LoopClosing;
    class System;

    class Tracking
    {

    public:
        Tracking(System *pSys, ORBVocabulary *pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap,
                 KeyFrameDatabase *pKFDB, const string &strSettingPath, const int sensor);

        // Preprocess the input and call Track(). Extract features and performs stereo matching.
        cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp);
        cv::Mat GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double &timestamp, const vector<vector<double>> &vLabel);
        cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);

        void SetLocalMapper(LocalMapping *pLocalMapper);
        void SetLoopClosing(LoopClosing *pLoopClosing);
        void SetViewer(Viewer *pViewer);

        // Load new settings
        // The focal lenght should be similar or scale prediction will fail when projecting points
        // TODO: Modify MapPoint::PredictScale to take into account focal lenght
        void ChangeCalibration(const string &strSettingPath);

        // Use this function if you have deactivated local mapping and you only want to localize the camera.
        void InformOnlyTracking(const bool &flag);

        // 获得相对位姿 --by钟
        cv::Mat GetVelocity() { return this->mVelocity.clone(); }

        cv::Mat Get_T_Inverse(cv::Mat &Tcw);
        // 是否需要添加进物体模型库
        bool NeedAddObject(MapObject *pObj, int cls, int &min_dis_idx);

    public:
        // Tracking states
        enum eTrackingState
        {
            SYSTEM_NOT_READY = -1,
            NO_IMAGES_YET = 0,
            NOT_INITIALIZED = 1,
            OK = 2,
            LOST = 3
        };

        eTrackingState mState;
        eTrackingState mLastProcessedState;

        // Input sensor
        int mSensor;

        // Current Frame
        Frame mCurrentFrame;
        cv::Mat mImGray;
        cv::Mat mImRGB;

        // Initialization Variables (Monocular)
        std::vector<int> mvIniLastMatches;
        std::vector<int> mvIniMatches;
        std::vector<cv::Point2f> mvbPrevMatched;
        std::vector<cv::Point3f> mvIniP3D;
        Frame mInitialFrame;

        // Lists used to recover the full camera trajectory at the end of the execution.
        // Basically we store the reference keyframe for each frame and its relative transformation
        list<cv::Mat> mlRelativeFramePoses;
        list<KeyFrame *> mlpReferences;
        list<double> mlFrameTimes;
        list<bool> mlbLost;

        // True if local mapping is deactivated and we are performing only localization
        bool mbOnlyTracking;

        void Reset();

        // 目标检测数据
        vector<vector<double>> mvvLabel; // cls,x,y,w,h

        // 物体模型库
        vector<vector<MapObject *>> mvvpObjModel;

        // 要添加进BA质心对约束
        vector<vector<Eigen::Vector3d>> mvvCenters;

        // 物体宽度参数
        vector<float> mvObjWidth;

        // 目标框生成的物体点云
        vector<std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr>> mvCurrentObjCloud;
        vector<std::map<int, Eigen::Vector3d>> mvCurrentCenters; // map<id,centers>

        // 记录目标框和模型库的匹配关系
        vector<std::map<int, int>> mvObjMatches; // <目标框id，模型库id>

    protected:
        // Main tracking function. It is independent of the input sensor.
        void Track();

        // Map initialization for stereo and RGB-D
        void StereoInitialization();

        // Map initialization for monocular
        void MonocularInitialization();
        void CreateInitialMapMonocular();

        void CheckReplacedInLastFrame();
        void CheckReplacedInLastFrameWithObject();
        bool TrackReferenceKeyFrame();

        void UpdateLastFrame();
        void UpdateLastFrameWithObj();

        bool TrackWithMotionModel();

        bool Relocalization();

        void UpdateLocalMap();
        void UpdateLocalPoints();
        void UpdateLocalKeyFrames();
        void UpdateLocalObjects(); // 更新局部物体

        bool TrackLocalMap();
        void SearchLocalPoints();
        void SearchLocalObjects(); // 对 Local Objects 进行跟踪

        bool NeedNewKeyFrame();
        void CreateNewKeyFrame();

        void CreateObjMatches();
        void CreateObjMatchesLast2Current(Frame &CurrentFrame, Frame &LastFrame);
        void CreateObjMatchesReKF2Current(Frame &CurrentFrame, 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);
        // In case of performing only localization, this flag is true when there are no matches to
        // points in the map. Still tracking will continue if there are enough matches with temporal points.
        // In that case we are doing visual odometry. The system will try to do relocalization to recover
        // "zero-drift" localization to the map.
        bool mbVO;

        // Other Thread Pointers
        LocalMapping *mpLocalMapper;
        LoopClosing *mpLoopClosing;

        // ORB
        ORBextractor *mpORBextractorLeft, *mpORBextractorRight;
        ORBextractor *mpIniORBextractor;

        // BoW
        ORBVocabulary *mpORBVocabulary;
        KeyFrameDatabase *mpKeyFrameDB;

        // Initalization (only for monocular)
        Initializer *mpInitializer;

        // Local Map
        KeyFrame *mpReferenceKF;
        std::vector<KeyFrame *> mvpLocalKeyFrames;
        std::vector<MapPoint *> mvpLocalMapPoints;
        std::vector<MapObject *> mvpLocalMapObjects;

        // System
        System *mpSystem;

        // Drawers
        Viewer *mpViewer;
        FrameDrawer *mpFrameDrawer;
        MapDrawer *mpMapDrawer;

        // Map
        Map *mpMap;

        // Calibration matrix
        cv::Mat mK;
        cv::Mat mDistCoef;
        float mbf;

        // New KeyFrame rules (according to fps)
        int mMinFrames;
        int mMaxFrames;

        // Threshold close/far points
        // Points seen as close by the stereo/RGBD sensor are considered reliable
        // and inserted from just one frame. Far points requiere a match in two keyframes.
        float mThDepth;

        // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
        float mDepthMapFactor;

        // Current matches in frame
        int mnMatchesInliers;
        int mnObjMatchesInliers;

        // Last Frame, KeyFrame and Relocalisation Info
        KeyFrame *mpLastKeyFrame;
        Frame mLastFrame;
        unsigned int mnLastKeyFrameId;
        unsigned int mnLastRelocFrameId;

        // Motion Model
        cv::Mat mVelocity;

        // Color order (true RGB, false BGR, ignored if grayscale)
        bool mbRGB;

        list<MapPoint *> mlpTemporalPoints; // 临时的地图点,用于提高双目和RGBD摄像头的帧间效果,用完之后就扔了
        list<MapObject *> mlpTemporalObjects;
    };

} // namespace ORB_SLAM

#endif // TRACKING_H