GAAS

#ifndef SCENE_RETRIEVE_H
#define SCENE_RETRIEVE_H

#include <iostream>
#include <vector>

#include <glog/logging.h>
#include "DBoW3.h" // defines OrbVocabulary and OrbDatabase

#include <Eigen/Core>

// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "opencv2/calib3d.hpp"

#include "LoopClosingManager.h"
#include "scene_frame_properties.h"
#include "serialization.h"

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include "cv_helper.h"



using namespace std;

                    //pts2d_in    pts3d_in    desp,   R,    t
typedef  std::tuple<std::vector<cv::KeyPoint>, std::vector<cv::Point3d>, cv::Mat, cv::Mat, cv::Mat> SceneFrame;

class Scene  // a scene is divided into multi images with depth.the point3d represents its position in the axis of the whole scene ,not the 3d position in its image.
{
    
public:
    Scene();

    void fromCVMat(std::vector<std::vector<cv::KeyPoint>> points_2d,const std::vector<std::vector<cv::Point3d>>& points_3d,const std::vector<cv::Mat>& point_desps);
    
    void RotateAndTranslate(const cv::Mat &RT)
    {
      ;
    }
    
    void setVisiblePointCloud(const std::string &pointcloud_filename);
    
    //inline 
    int getImageCount();
    
    inline cv::Mat& getDespByIndex(int i)
    {
      return this->point_desps[i];
    }
    
    inline std::vector<std::vector<cv::KeyPoint>>& getP2D()
    {
      return this->vec_p2d;
    }
    
    inline std::vector<std::vector<cv::Point3d>>& getP3D()
    {
      return this->vec_p3d;
    }
    
    inline void addFrame(const std::vector<cv::KeyPoint>& points2d_in, const std::vector<cv::Point3d>points3d_in, const cv::Mat& point_desp_in, const cv::Mat R, const cv::Mat t)
    {
        
        cout<<"Adding frame: "<<R<<t<<endl;

        this->vec_p2d.push_back(points2d_in);
        this->vec_p3d.push_back(points3d_in);
        this->point_desps.push_back(point_desp_in);
        
        this->mVecR.push_back(R);
        this->mVecT.push_back(t);
        
        this->mIndex++;
    }
    inline int getCurrentIndex()
    {
        return this->mIndex;
    }
    
    
    inline void addFrame(const SceneFrame& frame)
    {
        this->addFrame(std::get<0>(frame), std::get<1>(frame), std::get<2>(frame), std::get<3>(frame), std::get<4>(frame));
    }
    
    
    inline void setHasScale(bool hasScale_in)
    {
        this->hasScale = hasScale_in;
    }



    
    void saveVoc();

    void saveFile(const std::string &filename);
    
    void loadFile(const std::string &filename);
    
    void saveDeserializedPoseToCSV();
    
    void test(bool savePosition=false);

    std::vector <cv::Point3d> fetchFrameMapPoints(size_t frame_index);

    void removeEmptyElement();



    /////////////////////////////////// serialization////////////////////////////////////
    BOOST_SERIALIZATION_SPLIT_MEMBER()

    template <class Archive>
    void save (Archive & ar, const unsigned int version) const
    {
        cout << "scene saving started!" << endl;

        ar & mIndex;
        ar & hasScale;
        ar & vec_p2d;
        ar & vec_p3d;
        ar & point_desps;
        
        ar & mVecR;
        ar & mVecT;
        
        //ar & m_RT_Scene_Fix;
        //ar & point_cloud_of_scene;
        cout << "scene saving finished!" << endl;
    }

    template <class Archive>
    void load (Archive & ar, const unsigned int version)
    {
        cout << "scene loading started!" << endl;
        
        ar & mIndex;
        ar & hasScale;
        ar & vec_p2d;
        ar & vec_p3d;
        ar & point_desps;
        
        ar & mVecR;
        ar & mVecT;

        //ar & m_RT_Scene_Fix;
        //ar & point_cloud_of_scene;

        cout << "scene loading finished!" << endl;
    }
    /////////////////////////////////// serialization////////////////////////////////////

    inline cv::Mat getR(size_t index)
    {
        return this->mVecR[index];
    }

    inline cv::Mat getT(size_t index)
    {
        return this->mVecT[index];
    }

    bool hasScale = false;

public:

    size_t mIndex = 0;
    std::vector<std::vector<cv::KeyPoint> > vec_p2d;
    std::vector<std::vector<cv::Point3d> > vec_p3d;
    std::vector <cv::Mat> point_desps;
    
    vector<cv::Mat> mVecR;
    vector<cv::Mat> mVecT;
    
    std::vector<SceneFrame_Properties> vec_properties;
    
    cv::Mat m_RT_Scene_Fix = cv::Mat::eye(4,4,CV_32F);//fix 3d pose of scene.

};




SceneFrame generateSceneFrameFromStereoImage(const cv::Mat &imgl, cv::Mat &imgr, const cv::Mat& RotationMat, const cv::Mat& TranslationMat, const cv::Mat& Q_mat, LoopClosingManager& lcm,bool& success)
{
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<1>"<<endl;
    std::vector<cv::KeyPoint> key_points2d_candidate;
    std::vector<cv::KeyPoint> key_points2d_final;
    std::vector<cv::Point3d> points3d;
    cv::Mat feature;
    //LOG(INFO)<<"generating temp LoopClosingManager in generateSceneFrameFromStereoImage()"<<endl;
    //LoopClosingManager lcm("./config/orbvoc.dbow3");

    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<2>"<<endl;
    //LOG(INFO)<<"temp LoopClosingManager extracting feature."<<endl;
    ptr_frameinfo pleft_image_info = lcm.extractFeature(imgl);
    ptr_frameinfo pright_image_info = lcm.extractFeature(imgr);
    
    cv::Mat feature_l,feature_r;
    feature_l = pleft_image_info->descriptors;
    feature_r = pright_image_info->descriptors;

    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<3>"<<endl;
    std::vector<int> good_2dmatches_index;

    LOG(INFO)<<"FlannBasedMatcher matching."<<endl;
    cv::FlannBasedMatcher matcher = cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12,20,2));
    //cv::FlannBasedMatcher matcher = cv::FlannBasedMatcher(cv::makePtr<flann::IndexParams>(12,20,2));
    //cv::FlannBasedMatcher matcher = FlannBasedMatcher();

    std::vector< cv::DMatch > matches;
    matcher.match(feature_l, feature_r, matches);

    double max_dist = 0; 
    double min_dist = 100;
    for( int i = 0; i < matches.size(); i++ )
    {
        double dist = matches[i].distance;
        if( dist < min_dist ) min_dist = dist;
        if( dist > max_dist ) max_dist = dist;
    }

    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<4>"<<endl;
    std::vector< cv::DMatch > good_matches;
    for( int i = 0; i < matches.size(); i++ )
    {
        if( matches[i].distance <= 4*min_dist && matches[i].distance< 10) // 3.0 too large;2.0 too large.
        {
            good_matches.push_back( matches[i]); 
        }
    }
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<5>"<<endl;
    
    std::vector<cv::Point2f> lk_input_keypoints, lk_output_keypoints;
    for(size_t i = 0; i < good_matches.size(); i++)
    {
        lk_input_keypoints.push_back(pleft_image_info->keypoints[good_matches[i].queryIdx].pt);//will check if LKFlow exist.
        good_2dmatches_index.push_back(good_matches[i].queryIdx);
    }
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<6>"<<endl;

    if (lk_input_keypoints.size()<=5)
    {
        SceneFrame failed;
        success = false;
        return failed;
    }


    std::vector<unsigned char> PyrLKResults;
    std::vector<float> optflow_err;
    LOG(INFO)<<"Calcing opt flow pyrlk."<<endl;
    cv::calcOpticalFlowPyrLK(imgl,
                            imgr,
                            lk_input_keypoints,
                            lk_output_keypoints,
                            PyrLKResults,
                            optflow_err);
    
    cout<<"lk_output_keypoints.size() == 0: "<<(lk_output_keypoints.size())<<endl;
    cout<<"PyrLKResults.size() == 0: "<<(PyrLKResults.size())<<endl;

    LOG(INFO)<<"lk_input_keypoints.size():"<<lk_input_keypoints.size()<<endl;
    LOG(INFO)<<"lk_output_keypoints.size():"<<lk_output_keypoints.size()<<endl;
    LOG(INFO)<<"PyrLKResults.size():"<<PyrLKResults.size()<<endl;
    LOG(INFO)<<"good_2d_index_matches.size()"<<good_2dmatches_index.size()<<endl; 
   
    if (lk_input_keypoints.size() < 5 || lk_output_keypoints.size() < 5 || PyrLKResults.size() < 5)
    {
        
         success = false;
         SceneFrame temp;
         return temp;
    }
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<7>"<<endl;
    
    std::vector<cv::Point2f> matched_points;
    std::vector<float> disparity_of_points;
    cv::Mat descriptors_reserved;

    LOG(INFO)<<"Descriptor size of left img: col,row:"<<pleft_image_info->descriptors.cols<<","<<pleft_image_info->descriptors.rows<<endl;
    for(int index = 0; index<lk_input_keypoints.size(); index++)
    {
        if(!PyrLKResults[index])
        {
            LOG(INFO)<<"pyrlk result index:"<<index<<"invalid.continue."<<endl;
            continue;
        }
        //if(PyrLKResults[index] == 1)
        else
        {
            LOG(INFO)<<"        adding pyrlk result index:"<<index<<"!"<<endl;
            LOG(INFO)<<"        result index in good_match:"<<good_2dmatches_index[index]<<"!"<<endl;
            matched_points.push_back(lk_input_keypoints[index]);
            disparity_of_points.push_back(lk_output_keypoints[index].x-lk_input_keypoints[index].x);
            
            LOG(INFO)<<"        in generateSceneFrameFromStereoImage() stage<1> split row"<<endl;
            cv::Mat desp_reserved = pleft_image_info->descriptors.rowRange(good_2dmatches_index[index], good_2dmatches_index[index]+1).clone().reshape(0,1);
            //cv::Mat desp_reserved = pleft_image_info->descriptors.row(good_2dmatches_index[index]).clone().reshape(0,1);
            LOG(INFO)<<"        in generateSceneFrameFromStereoImage() stage<2> add to mat."<<endl;
            descriptors_reserved.push_back(desp_reserved);//get this row out of mat.
            
            
            //push_back(pleft_image_info->descriptors[good_2dmatches_index[index]]);
        }
    }
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<8>"<<endl;

    std::vector<cv::Point3f> points3f;

    cv::reprojectImageTo3D(disparity_of_points, points3f, Q_mat);
    points3d.resize(points3f.size());
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<9>"<<endl;
    
    //do rotation and translation to points3d.
    for(int i = 0;i<points3f.size();i++)
    {
        cv::Mat point3d_temp(points3f[i]);
        
        Eigen::Matrix3f rotation;
        Eigen::Vector3f translation;
        Eigen::Vector3f point;
        Eigen::Vector3f result;
        
        cv::cv2eigen(RotationMat, rotation);
        cv::cv2eigen(TranslationMat, translation);
        cv::cv2eigen(point3d_temp, point);
        
        result = rotation * point + translation;
        //cv::Mat transformed = RotationMat*point3d_temp + TranslationMat;
        
        cv::Mat transformed;
        cv::eigen2cv(result, transformed);

        cv::Point3d output;
        output.x = transformed.at<float>(0);
        output.y = transformed.at<float>(1);
        output.z = transformed.at<float>(2);
        points3d[i] = output;//do transform in mat form.
    }
    LOG(INFO)<<"in generateSceneFrameFromStereoImage():step<10>"<<endl;
    cv::KeyPoint::convert(matched_points,key_points2d_final);
    
    LOG(INFO)<<"key_points2d_final size: "<<key_points2d_final.size()<<endl;
    LOG(INFO)<<"points3d size: "<<points3d.size()<<endl;
    success = true;  
    return std::make_tuple(key_points2d_final, points3d, descriptors_reserved, RotationMat, TranslationMat);
}



class SceneRetriever
{
public:
    
    SceneRetriever();
    
    SceneRetriever(const string& voc);
    
    //SceneRetriever(Scene& original_scene_input);
    SceneRetriever(const string& voc,const std::string& scene_file);

    SceneRetriever(const string& voc,const string& scene_file, const string config_file);

    float retrieveSceneWithScaleFromMonoImage(cv::Mat image_in_rect, cv::Mat& cameraMatrix, cv::Mat& RT_mat_of_mono_cam_output, bool& match_success,int* pMatchedIndexID_output = nullptr);

    float retrieveSceneFromStereoImage(cv::Mat& image_left_rect, cv::Mat& image_right_rect,
                                       cv::Mat& mavros_pose, cv::Mat& RT_mat_of_stereo_cam_output, bool& match_success,int* pMatchedIndexID_output = nullptr);

    // for SHAKESHAKE
    // Q is not used to recover 3D points now, but you can use this function now, and if you need my to use Q to recover 3D points I will modify related function.
    // input: image_left_rect, image_right_rect, Q_mat
    // output: outpuit: mavros_pose at the time of retrieving pose, recovered mat, bool and int.
    float retrieveSceneFromStereoImage(cv::Mat& image_left_rect, cv::Mat& image_right_rect,
                                       cv::Mat& mavros_pose, cv::Mat& RT_mat_of_stereo_cam_output, cv::Mat& Q_mat, bool& match_success,int* pMatchedIndexID_output = nullptr);

    inline int addFrameToScene(const std::vector<cv::KeyPoint>& points2d_in, const std::vector<cv::Point3d>points3d_in,const cv::Mat& point_desp_in, const cv::Mat R, const cv::Mat t)
    {
        LOG(INFO)<<"    content of desp in addFrameToScene():"<<point_desp_in<<endl;
        LOG(INFO)<<"in addFrameToScene():"<<endl;
        if(!this->ploop_closing_manager_of_scene )
        {
            LOG(ERROR)<<"Calling addFrameToScene() without initiated loop closing manager!"<<endl;
            return -1;
        }
        LOG(INFO)<<"in addFrameToScene() call addFrame():"<<endl;
        LOG(INFO)<<"size of point2d ,point3d:"<<points2d_in.size()<<","<<points3d_in.size()<<endl;
        this->original_scene.addFrame(points2d_in,points3d_in,point_desp_in,R,t);
        struct FrameInfo* pfr = new struct FrameInfo;
        LOG(INFO)<<"in addFrameToScene() query current index:"<<endl;
        LOG(INFO)<<"        index:"<<this->original_scene.getCurrentIndex()<<"obj size of p2d and desp:"<<this->original_scene.getP2D().size()<<","<<this->original_scene.point_desps.size()<<endl;
        pfr->keypoints = this->original_scene.getP2D().back();//this->original_scene.getP2D()[this->original_scene.getCurrentIndex()-1];
        pfr->descriptors = this->original_scene.getDespByIndex(original_scene.getCurrentIndex()-1);
        cout<<"pfr->keypoints size: "<<pfr->keypoints.size()<<endl;
        cout<<"pfr->descriptors size: "<<pfr->descriptors.size()<<endl;
        LOG(INFO)<<"in addFrameToScene() forming frame_info:"<<endl;
        ptr_frameinfo frame_info(pfr);
        LOG(INFO)<<"in addFrameToScene() adding KFrame:"<<endl;
        this->ploop_closing_manager_of_scene->addKeyFrame(frame_info);
        LOG(INFO)<<"addFrameToScene() successfully!";
        return 0;
    }


    float retrieveSceneWithMultiStereoCam(const std::vector<cv::Mat> leftCams,const std::vector<cv::Mat> rightCams,
                                        std::vector<cv::Mat> RT_pose_of_stereo_cams,
                                        cv::Mat &RT_mat_of_multi_stereo_cam_output,
                                        bool &match_success,
                                        int* pMatchedIndexID_output = nullptr
                                        );
    
    float retrieveSceneWithMultiMonoCam(const std::vector<cv::Mat> images,std::vector<cv::Mat> RT_pose_of_mono_cams,cv::Mat &RT_mat_of_multi_mono_cam_output,bool& match_success,int* pMatchedIndexID_output = nullptr);

    void debugVisualize();//visualize pointcloud and cam pose.

    void readImage(vector<string>& image_paths);

    void setImageVecPath(vector<string>& imageVec, int left);

    cv::Mat fetchImage(int index, int left);

    void displayFeatureMatches(cv::Mat curImage, vector<cv::KeyPoint> curKps,
                               cv::Mat oldImage, vector<cv::KeyPoint> oldKps,
                               std::vector<cv::DMatch> matches, size_t loop_index);

    void displayFeatureMatches(size_t loop_index, ptr_frameinfo& current_frame, std::vector<cv::DMatch> matches);

    inline vector<cv::Point3f> pts3dto3f(vector<cv::Point3d>& input_pts)
    {
        vector<cv::Point3f> result;
        cv::Point3f pts3f;
        for(auto& pts3d : input_pts)
        {
            pts3f.x = pts3d.x;
            pts3f.y = pts3d.y;
            pts3f.z = pts3d.z;
            result.emplace_back(pts3d.x, pts3d.y, pts3d.z);
        }
        return result;
    }

    void publishPoseHistory();

    size_t LoopClosureDebugIndex = 0;


    inline Scene& getScene()
    {
        return this->original_scene;
    }

    void MavrosPoseCallback(const geometry_msgs::PoseStamped& pose);

    inline cv::Mat PoseStampedToMat(const geometry_msgs::PoseStamped& pose)
    {
        Eigen::Quaterniond pose_q(pose.pose.orientation.w,
                                  pose.pose.orientation.x,
                                  pose.pose.orientation.y,
                                  pose.pose.orientation.z);

        Eigen::Matrix3d pose_R = pose_q.toRotationMatrix();

        cv::Mat mat_R;
        cv::eigen2cv(pose_R, mat_R);

        cv::Mat T = cv::Mat::zeros(cv::Size(4,4), CV_64FC1);

        T.at<double>(0, 0) = mat_R.at<double>(0, 0);
        T.at<double>(0, 1) = mat_R.at<double>(0, 1);
        T.at<double>(0, 2) = mat_R.at<double>(0, 2);
        T.at<double>(1, 0) = mat_R.at<double>(1, 0);
        T.at<double>(1, 1) = mat_R.at<double>(1, 1);
        T.at<double>(1, 2) = mat_R.at<double>(1, 2);
        T.at<double>(2, 0) = mat_R.at<double>(2, 0);
        T.at<double>(2, 1) = mat_R.at<double>(2, 1);
        T.at<double>(2, 2) = mat_R.at<double>(2, 2);

        T.at<double>(0, 3) = pose.pose.position.x;
        T.at<double>(1, 3) = pose.pose.position.y;
        T.at<double>(2, 3) = pose.pose.position.z;
        T.at<double>(3, 3) = 1;

        return T;
    }
private:

    void _init_retriever();

    Scene original_scene;

    //LoopClosingManager* ploop_closing_manager_of_scene;

    shared_ptr<LoopClosingManager> ploop_closing_manager_of_scene = nullptr;

    //cv_helper* mpCv_helper = nullptr;

    shared_ptr<cv_helper> mpCv_helper = nullptr;

    cv::Mat mCurrentImage;

    vector<string> mVecLeftImagePath;
    vector<string> mVecRightImagePath;

    //ROS related
    ros::NodeHandle mNH;
    ros::Subscriber mMavrosSub;

    cv::Mat mCurMavrosPose;

private:

    float mThresFitnessScore = -1;

};
#endif