#ifndef SIMWORLD_H
#define SIMWORLD_H

# include  <mylib/commen_include.h>
# include  <mylib/QuatPlane.h>
struct sFrame
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    sFrame(Eigen::Matrix4d Tcw): Tcw_(Tcw){}

    Eigen::Matrix4d Tcw_;

    std::unordered_map<int, Eigen::Vector3d> per_plane_feature_;


};

double fx = 1;//750;
double fy = 1;//500;
double cx = 0;//250;
double cy = 0;//250;

const int nCameras = 2;
const int nMapplanes = 2;  // now plane just 3


    class SimWord{
    public:// 修饰的成员变量和函数只可以在类的内部访问
        std::vector<sFrame>& _vFrames;
        std::vector<Eigen::Vector3d>& _vpGeometryFeatures;
    public: //public 修饰的成员变量和函数可以在类的内部和外部访问
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        SimWord(){}
        SimWord(std::vector<sFrame>& vFrames, std::vector<Eigen::Vector3d>& vpGeometryFeatures){

            _vFrames = vFrames_in;
            _vpGeometryFeatures = vpGeometryFeatures_in;
        }
        std::vector<std::vector<Eigen::Vector3d>> readPlaneFile();
        Eigen::Vector4d CreatePlanebyPoints(Eigen::Vector4d pt1_,Eigen::Vector4d pt2_,Eigen::Vector4d pt3_,Eigen::Vector4d pt4_);
        void CreateCameras(std::vector<sFrame>& vFrames, std::vector<Sophus::SE3d>& gtpose, int nCams);
        void CreateSceneFeatures(const std::vector<sFrame>& vFrames, std::vector<Eigen::Vector3d>& vpGeometryFeatures);
        void FeatureDetection(const Eigen::Matrix3d& K, const std::vector<Eigen::Vector3d> &scenefeatures, std::vector<sFrame> &vframes, bool is_add_noise);
        void AddNoise(std::vector<sFrame>& vFrames, std::vector<Eigen::Vector3d>& vpGeometryFeatures, double gs_noise, double cam_noise);

        Eigen::Quaterniond ExpUpDateQuat(const Eigen::Vector3d &omg,const Eigen::Quaterniond q);
        Eigen::Vector3d logQuat(Eigen::Quaterniond q);



    };

#endif //SIMWORLD_H
