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

#include <vector>
#include <utility>
#include "MapPoint.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "ORBVocabulary.h"
#include "KeyFrame.h"
#include "ORBextractor.h"
#include "Car.h"

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
namespace ORB_SLAM2
{
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

    class MapPoint;
    class KeyFrame;

    class Frame
    {
    public:
        Frame();

        // Copy constructor.
        Frame(const Frame &frame);

        // Constructor for stereo cameras.
        Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft,
              ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf,
              const float &thDepth, vector<vector<double>> &vvLabel, cv::Mat &Tcw_predict, bool &state, vector<Car *> &vpCar);

        // Constructor for RGB-D cameras.
        Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const vector<vector<double>> &vvLabel, cv::Mat &Velocity, bool &state, cv::Mat &Lastframe, float &DepthMapFactor, const cv::Mat &imRGB);

        // Constructor for Monocular cameras.
        Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
        // Extract ORB on the image. 0 for left image and 1 for right image.
        void ExtractORB(int flag, const cv::Mat &im);

        // Compute Bag of Words representation.
        void ComputeBoW();

        // Set the camera pose.
        void SetPose(cv::Mat Tcw);

        // Computes rotation, translation and camera center matrices from the camera pose.
        void UpdatePoseMatrices();

        // Returns the camera center.
        inline cv::Mat GetCameraCenter()
        {
            return mOw.clone();
        }

        // Returns inverse of rotation
        inline cv::Mat GetRotationInverse()
        {
            return mRwc.clone();
        }

        // Check if a MapPoint is in the frustum of the camera
        // and fill variables of the MapPoint to be used by the tracking
        bool isInFrustum(MapPoint *pMP, float viewingCosLimit);

        // Compute the cell of a keypoint (return false if outside the grid)
        bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);

        vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel = -1, const int maxLevel = -1) const;

        // Search a match for each keypoint in the left image to a keypoint in the right image.
        // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
        void ComputeStereoMatches();
        // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
        cv::Mat UnprojectStereo(const int &i);

        void Target_tracking(); // 目标跟踪

        void Motion_consistency_estimation(cv::Mat &Tcl, bool &state);
        // 运动一致性检测
        // by洪
        // 绘制目标框
        void drawRotatedBox(cv::Mat &img, double &x, double &y, double &w, double &l, double &yaw, cv::Scalar &color);
        void drawDectectBox(cv::Mat Tcw, vector<cv::Mat> point, cv::Mat src, cv::Mat output);

        // 计算IOU
        void ComputeCarMatches();
        float get_IOU(cv::Point *PointArray1, cv::Point *PointArray2);

        float get_IOU(const vector<double> &label1, const vector<double> &label2, const cv::Mat &img, cv::Mat &T12);
        void get_bbx_with_T(const vector<double> &label1, std ::vector<cv::Point> &pts, cv::Mat &T12);
        // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
        void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts);

        void T_Inverse(cv::Mat &Tcw);
        void T_Inverse(cv::Mat &T12, cv::Mat &T21);
        void bev_current_last(cv::Point &p_c, cv::Point &p_l, cv::Mat &T12); // bev视图，当前到过去的像素点坐标转换
        void drawRotatedBox(cv::Mat &img, double &x, double &y, double &w, double &l, double &yaw, cv::Scalar &color, cv::Mat &T12);
        void ComputeStereoFromRGBD(const cv::Mat &imDepth);
        void TrackObj();
        void MotionConsistencyEstimation(cv::Mat img, bool &state);
        void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts, double scale);

        /***************************************************************
         *  @brief     目标框的世界坐标转bev的像素坐标
         *  @param     世界坐标vector<cv::Mat> ,  相素坐标vector<cv::Point>  , 相机的位姿cv::Mat
         *  @note
         *  @Sample usage:     函数的使用方法
         **************************************************************/
        void world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw); // 世界坐标转bev像素

    public:
        // Vocabulary used for relocalization.
        ORBVocabulary *mpORBvocabulary;

        // Feature extractor. The right is used only in the stereo case.
        ORBextractor *mpORBextractorLeft, *mpORBextractorRight;

        // Frame timestamp.
        double mTimeStamp;

        // Calibration matrix and OpenCV distortion parameters.
        cv::Mat mK;
        static float fx;
        static float fy;
        static float cx;
        static float cy;
        static float invfx;
        static float invfy;

        static float bev_fx;
        static float bev_fy;
        static float bev_cx;
        static float bev_cy;
        static float bev_invfx;
        static float bev_invfy;

        cv::Mat m_bev_K;
        cv::Mat mDistCoef;
        cv::Mat mRbc; // 相机转bev的矩阵

        // Stereo baseline multiplied by fx.
        float mbf;

        // Stereo baseline in meters.
        float mb;

        // Threshold close/far points. Close points are inserted from 1 view.
        // Far points are inserted as in the monocular case from 2 views.
        float mThDepth;

        // Number of KeyPoints.
        int N;

        vector<cv::KeyPoint> mvKeys_new;
        vector<cv::KeyPoint> mvKeysUn_new;
        vector<float> mvuRight_new;
        vector<float> mvDepth_new;
        cv::Mat mDescriptors_new;
        // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
        // In the stereo case, mvKeysUn is redundant as images must be rectified.
        // In the RGB-D case, RGB images can be distorted.
        std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
        std::vector<cv::KeyPoint> mvKeysUn;

        // Corresponding stereo coordinate and depth for each keypoint.
        // "Monocular" keypoints have a negative value.
        std::vector<float> mvuRight;
        std::vector<float> mvDepth;

        // 建立静态匹配关系
        std::vector<int> mvCarMatches;

        // Bag of Words Vector structures.
        DBoW2::BowVector mBowVec;
        DBoW2::FeatureVector mFeatVec;

        // ORB descriptor, each row associated to a keypoint.
        cv::Mat mDescriptors, mDescriptorsRight;
        // MapPoints associated to keypoints, NULL pointer if no association.
        std::vector<MapPoint *> mvpMapPoints;

        // Flag to identify outlier associations.
        std::vector<bool> mvbOutlier;

        // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
        static float mfGridElementWidthInv;
        static float mfGridElementHeightInv;
        std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

        // Camera pose.
        cv::Mat mTcw;

        // Current and Next Frame id.
        static long unsigned int nNextId;
        long unsigned int mnId;

        // Reference Keyframe.
        KeyFrame *mpReferenceKF;

        // Scale pyramid info.
        int mnScaleLevels;
        float mfScaleFactor;
        float mfLogScaleFactor;
        vector<float> mvScaleFactors;
        vector<float> mvInvScaleFactors;
        vector<float> mvLevelSigma2;
        vector<float> mvInvLevelSigma2;

        // Undistorted Image Bounds (computed once).
        static float mnMinX;
        static float mnMaxX;
        static float mnMinY;
        static float mnMaxY;

        static bool mbInitialComputations;

        // 添加动态物体label的vector,及每帧的图像 --by钟

        vector<vector<double>> mvvLabel;
        vector<vector<double>> mvvDynamic_label;
        cv::Mat mframe;
        cv::Mat mRGB;
        cv::Mat mDepth;
        float mDepthMapFactor;

        // 模板匹配计算相似度 --by钟
        static vector<cv::Mat> mImg1;
        static vector<cv::Mat> mImD;
        static cv::Mat mImg2;
        static vector<cv::Mat> mvImg1_Tcw;
        static cv::Mat mImg2_Tcw;

        // Rotation, translation and camera center
        cv::Mat mRcw;
        cv::Mat mtcw;
        cv::Mat mRwc;
        cv::Mat mOw; //==mtwc
        std::vector<Car *> mvpCar;
        cv::Mat mTcw_predict;

    private:
        // Undistort keypoints given OpenCV distortion parameters.
        // Only for the RGB-D case. Stereo must be already rectified!
        // (called in the constructor).
        void UndistortKeyPoints();

        // Computes image bounds for the undistorted image (called in the constructor).
        void ComputeImageBounds(const cv::Mat &imLeft);

        // Assign keypoints to the grid for speed up feature matching (called in the constructor).
        void AssignFeaturesToGrid();
    };

} // namespace ORB_SLAM

#endif // FRAME_H