#include <iostream>
#include <fstream>
#include <math.h>
#include <ctime>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include<g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;

// camera intrinsic parameters
#define _FX (7.215377000000e+02)
#define _FY (7.215377000000e+02)
#define _CX (6.095593000000e+02)
#define _CY (1.728540000000e+02)
#define _BASELINE (5.40e+02) //54cm
#define _SCALE (10)

//params
#define _LAST_FRAME_NUM (4)
#define _GOOD_MATCH_PARAM (50)
#define _MIN_GOOD_MATCHES (30)
#define _GOOD_MATCHES_NUM (500)
#define _GRID_SIZE (0.2)
#define _MIN_INLIERS (5)

//data
#define _IMAGE_NUM (801)
#define _LEFT (0)
#define _RIGHT (1)

//switch
#define _WRITE_KEYPOINTS_IMAGE (false)
#define _WRITE_MATCH_IMAGE (false)
#define _WRITE_DISP (false)
#define _WRITE_DEPTH (false)
#define _WRITE_INLIER (false)
#define _WRITE_RESULT (true)
#define _SHOW_PATH (true)
#define _SHOW_CLOUD (false)

// typedef pcl::PointXYZRGBA PointT;
// typedef pcl::PointCloud<PointT> PointCloud;

struct PnP_result
{
    Mat R, t;
    int inliers;
};
struct frame
{
    int id;
    bool valid = true;
    Mat src[2];
    Mat depth;
    Mat desp[2];
    vector<KeyPoint> kp[2];
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    Mat pose = Mat::eye(4,4,CV_64F);
    Mat R = Mat::eye(3,3,CV_64F);
    Mat t;
};
struct FeaturePoint{
  cv::Point2f  point;
  int id;
  int age;
};
struct FeatureSet {
    std::vector<cv::Point2f>  points;
    std::vector<int>  ages;
    int size(){
        return points.size();
    }
    void clear(){
        points.clear();
        ages.clear();
    }
 };
class Bucket
{

public:
    int id;
    int max_size;

    FeatureSet features;

    Bucket(int);

    void add_feature(cv::Point2f, int);
    void get_features(FeatureSet&);

    int size();

};

Bucket::Bucket(int size){
    max_size = size;
}

int Bucket::size(){
    return features.points.size();
}

void Bucket::add_feature(cv::Point2f point, int age){
    // won't add feature with age > 10;
    int age_threshold = 10;
    if (age < age_threshold)
    {
        // insert any feature before bucket is full
        if (size()<max_size)
        {
            features.points.push_back(point);
            features.ages.push_back(age);

        }
        else
        // insert feature with old age and remove youngest one
        {
            int age_min = features.ages[0];
            int age_min_idx = 0;

            for (int i = 0; i < size(); i++)
            {
                if (age < age_min)
                {
                    age_min = age;
                    age_min_idx = i;
                }
            }
            features.points[age_min_idx] = point;
            features.ages[age_min_idx] = age;
        }
    }

}

void Bucket::get_features(FeatureSet& current_features){

    current_features.points.insert(current_features.points.end(), features.points.begin(), features.points.end());
    current_features.ages.insert(current_features.ages.end(), features.ages.begin(), features.ages.end());
}

class My_VO
{
public:
    vector<vector<string>> addData();
    PnP_result featureDetectAndSolvePnP(int idx, struct frame& l_frame, struct frame& r_frame);
    void stereoSGBM(Mat lpng, Mat rpng, Mat&disp);
    void disp2Depth(Mat disp, Mat& depth);
    Point3f from2dTo3d(const Point3f& point);
    void PnPRes2Eigen(frame& curr_frame, PnP_result& res);
    void run(int start_idx, int end_idx );
    void run_multiframe(int start_idx, int end_idx);
    frame getFrame(const int& idx, bool detect = true);
    struct PnP_result estimateMotion(int idx, frame& last_frame, frame& curr_frame);
    void integrateOdom(frame& curr_frame);
    void integrateOdom(frame& last_frame, frame& curr_frame);
    void display(frame& frame);
    void getMeanPose(vector<frame> frame_list, frame& frame);
    void outputData(frame& frame);
    void run_soft(int start_idx, int end_idx);
    void matchingFeatures(cv::Mat& imageLeft_t0, cv::Mat& imageRight_t0,
                      cv::Mat& imageLeft_t1, cv::Mat& imageRight_t1,
                      FeatureSet& currentVOFeatures,
                      std::vector<cv::Point2f>&  pointsLeft_t0,
                      std::vector<cv::Point2f>&  pointsRight_t0,
                      std::vector<cv::Point2f>&  pointsLeft_t1,
                      std::vector<cv::Point2f>&  pointsRight_t1);
    void appendNewFeatures(cv::Mat& image, FeatureSet& current_features);
    void featureDetectionFast(cv::Mat image, std::vector<cv::Point2f>& points);
    void bucketingFeatures(cv::Mat& image, FeatureSet& current_features, int bucket_size, int features_per_bucket);
    void circularMatching(cv::Mat img_l_0, cv::Mat img_r_0, cv::Mat img_l_1, cv::Mat img_r_1,
                      std::vector<cv::Point2f>& points_l_0, std::vector<cv::Point2f>& points_r_0,
                      std::vector<cv::Point2f>& points_l_1, std::vector<cv::Point2f>& points_r_1,
                      std::vector<cv::Point2f>& points_l_0_return,
                      FeatureSet& current_features);
    void deleteUnmatchFeaturesCircle(std::vector<cv::Point2f>& points0, std::vector<cv::Point2f>& points1,
                          std::vector<cv::Point2f>& points2, std::vector<cv::Point2f>& points3,
                          std::vector<cv::Point2f>& points0_return,
                          std::vector<uchar>& status0, std::vector<uchar>& status1,
                          std::vector<uchar>& status2, std::vector<uchar>& status3,
                          std::vector<int>& ages);
    void checkValidMatch(std::vector<cv::Point2f>& points, std::vector<cv::Point2f>& points_return, std::vector<bool>& status, int threshold);
    void removeInvalidPoints(std::vector<cv::Point2f>& points, const std::vector<bool>& status);
    void trackingFrame2Frame(cv::Mat& projMatrl, cv::Mat& projMatrr,
                         std::vector<cv::Point2f>&  pointsLeft_t0,
                         std::vector<cv::Point2f>&  pointsLeft_t1,
                         cv::Mat& points3D_t0,
                         cv::Mat& rotation,
                         cv::Mat& translation,
                         bool mono_rotation);
    void displayTracking(cv::Mat& imageLeft_t1,
                     std::vector<cv::Point2f>&  pointsLeft_t0,
                     std::vector<cv::Point2f>&  pointsLeft_t1);
    cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R);
    void integrateOdometryStereo(int frame_i, cv::Mat& rigid_body_transformation, cv::Mat& frame_pose, const cv::Mat& rotation, const cv::Mat& translation_stereo);
    void display(int frame_id, cv::Mat& trajectory, cv::Mat& pose);

    struct CAMERA_INTRINSIC_PARAMETERS
    {
        double fx = _FX;
        double fy = _FY;
        double cx = _CX;
        double cy = _CY;
        double baseline = _BASELINE;
        double scale = _SCALE;
    };

private:
    Mat path= cv::Mat::zeros(600, 1200, CV_8UC3);
    Mat pose = Mat::eye(4,4,CV_64F);
    vector<vector<string>> image_list;
    struct CAMERA_INTRINSIC_PARAMETERS camera_parameters;
    Mat l_matrix = (cv::Mat_<float>(3, 4) << camera_parameters.fx, 0., camera_parameters.cx, 0.,
                                                                                0., camera_parameters.fy, camera_parameters.cy, 0.,
                                                                                0,  0., 1., 0.);
    Mat r_matrix = (cv::Mat_<float>(3, 4) << camera_parameters.fx, 0., camera_parameters.cx, -386.1448,
                                                                                0., camera_parameters.fy, camera_parameters.cy, 0.,
                                                                                0,  0., 1., 0.);
    ofstream outputfile;
};

int main(int argc, char** argv)
{
    int start = 0;
    int end = _IMAGE_NUM;
    My_VO my_vo = My_VO();
    if(argc == 4)
    {
        start = atoi(argv[2]);
        end = atoi(argv[3]);
    }
    if(argc == 1||(argc>1&&(string(argv[1])=="single")))
    {
        my_vo.run(start,end);
    }
    else if(argc > 1&&(string(argv[1]) == "multi"))
    {
        my_vo.run_multiframe(start, end);
    }
    else if(argc > 1&&(string(argv[1]) == "soft"))
    {
        my_vo.run_soft(start, end);
    }
    waitKey(0);
    return 0;
}

void My_VO::run_soft(int start_idx, int end_idx)
{
    addData();
    if(_WRITE_RESULT)
    {
        outputfile.open("../data/result/pose_soft.txt");
        if(!outputfile.is_open())
        {
            cout<<"txt open error..."<<endl;
        }
    }

    int curr_idx = start_idx;
    frame last_frame = getFrame(curr_idx);
    outputData(last_frame);

    vector<FeaturePoint> oldFeaturePointsLeft;
    vector<FeaturePoint> currFeaturePointsLeft;
    FeatureSet currentVOFeatures;
    cv::Mat frame_pose = cv::Mat::eye(4, 4, CV_64F);
    cv::Mat trajectory = cv::Mat::zeros(600, 1200, CV_8UC3);

    // 每个优化变量维度为3，误差值维度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    Block* solver_ptr  = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(false);

    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId(curr_idx);
    v->setEstimate(Eigen::Isometry3d::Identity());
    v->setFixed(true);
    optimizer.addVertex(v);

    int last_idx = curr_idx;

    for(curr_idx = start_idx+1; curr_idx<end_idx;curr_idx++)
    {
        cout<<endl<<"-------"<<curr_idx<<"-------"<<endl;
        frame curr_frame = getFrame(curr_idx);

        vector<Point2f> oldPointsLeft_t0 = currentVOFeatures.points;
        std::vector<cv::Point2f> pointsLeft_t0, pointsRight_t0, pointsLeft_t1, pointsRight_t1;
        matchingFeatures( last_frame.src[_LEFT], last_frame.src[_RIGHT],
                          curr_frame.src[_LEFT], curr_frame.src[_RIGHT],
                          currentVOFeatures,
                          pointsLeft_t0,
                          pointsRight_t0,
                          pointsLeft_t1,
                          pointsRight_t1);
        std::vector<cv::Point2f>& currentPointsLeft_t0 = pointsLeft_t0;
        std::vector<cv::Point2f>& currentPointsLeft_t1 = pointsLeft_t1;
        std::vector<cv::Point2f> newPoints;
        std::vector<bool> valid; // valid new points are ture

        cv::Mat points3D_t0, points4D_t0;
        cv::triangulatePoints( l_matrix,  r_matrix,  pointsLeft_t0,  pointsRight_t0,  points4D_t0);
        cv::convertPointsFromHomogeneous(points4D_t0.t(), points3D_t0);
        cv::Mat rotation = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat translation = cv::Mat::zeros(3, 1, CV_64F);
        trackingFrame2Frame(l_matrix, r_matrix, pointsLeft_t0, pointsLeft_t1, points3D_t0, rotation, translation, false);
        displayTracking(curr_frame.src[_LEFT], pointsLeft_t0, pointsLeft_t1);
        cv::Vec3f rotation_euler = rotationMatrixToEulerAngles(rotation);
        cv::Mat rigid_body_transformation;

        if(abs(rotation_euler[1])<0.1 && abs(rotation_euler[0])<0.1 && abs(rotation_euler[2])<0.1)
        {
            integrateOdometryStereo(curr_idx, rigid_body_transformation, frame_pose, rotation, translation);
        } else {
            std::cout << "Too large rotation"  << std::endl;
        }
        cv::Mat xyz = frame_pose.col(3).clone();
        curr_frame.pose = frame_pose;
        display(curr_idx, trajectory, xyz);
        outputData(curr_frame);

        g2o::VertexSE3* v = new g2o::VertexSE3();
        //顶点
        v->setId(curr_idx);
        v->setEstimate(Eigen::Isometry3d::Identity());
        optimizer.addVertex(v);
        //边
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        edge->vertices()[0] = optimizer.vertex(curr_idx-1);
        edge->vertices()[1] = optimizer.vertex(curr_idx);

        Eigen::Matrix<double,6,6> information = Eigen::Matrix<double,6,6>::Identity();
        information(0,0) = information(1,1)=information(2,2) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100;

        for(int i=0;i<3;i++)
        {
            for(int j=0; j<3;j++)
            {
                curr_frame.T(i,j) = rotation.at<double>(i,j);
            }
            curr_frame.T(i,3)=translation.at<double>(i,0);
        }

        edge->setInformation(information);
        edge->setMeasurement(curr_frame.T);
        optimizer.addEdge(edge);

        last_frame = curr_frame;
    }

    cout<<"optimizing pose graph, vertices: "<<optimizer.vertices().size()<<endl;
    optimizer.save("../data/result/result_before.g2o");
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    optimizer.save("../data/result/result_after.g2o");
    cout<<"optimization done"<<endl;

    outputfile.close();

    imwrite("../data/result.jpg", trajectory);
}

void My_VO::run_multiframe(int start_idx, int end_idx)
{
    //get data and open file
    addData();
    if(_WRITE_RESULT)
    {
        outputfile.open("../data/result/pose_multi.txt");
        if(!outputfile.is_open())
        {
            cout<<"txt open error..."<<endl;
        }
    }

    //get first image, add it into last_frame_list
    int curr_idx = start_idx;
    vector<frame> last_frame_list;
    frame start_frame = getFrame(start_idx);
    outputData(start_frame);
    for(int i = 0; i < _LAST_FRAME_NUM; i++)
    {
        last_frame_list.push_back(start_frame);
    }

    for(curr_idx = start_idx + 1; curr_idx < end_idx; curr_idx ++)
    {
        cout<<endl<<"-------curr_idx = "<<curr_idx<<"-------"<<endl;
        frame curr_frame = getFrame(curr_idx);
        vector<frame> curr_frame_list;
        PnP_result res;

        //get current and past frames' relationship
        for(int i = 0; i<_LAST_FRAME_NUM; i++)
        {
            cout<<endl<<"----last_idx = "<<curr_idx - _LAST_FRAME_NUM + i<<"----"<<endl;
            curr_frame_list.push_back(curr_frame);
            res = estimateMotion(curr_idx, last_frame_list[i], curr_frame);//get R, t
            PnPRes2Eigen(curr_frame_list[i], res);//procee result
            integrateOdom(last_frame_list[i],curr_frame_list[i]);//refresh global pose
        }

        getMeanPose(curr_frame_list, curr_frame);

        //refresh data
        pose = curr_frame.pose;

        display(curr_frame);

        if(curr_frame.pose.at<double>(0,0)==1)
            curr_frame.pose=last_frame_list.at(_LAST_FRAME_NUM-1).pose;
        outputData(curr_frame);

        vector<frame>::iterator p = last_frame_list.begin();
        last_frame_list.erase(p);

        last_frame_list.push_back(curr_frame);

        waitKey(1);
    }
    imwrite("../data/result.jpg", path);
    outputfile.close();
}

void My_VO::run(int start_idx , int end_idx)
{
    addData();
    if(_WRITE_RESULT)
    {
        outputfile.open("../data/result/pose_single.txt");
        if(!outputfile.is_open())
        {
            cout<<"txt open error..."<<endl;
        }
    }

    int curr_idx = start_idx;
    frame last_frame = getFrame(curr_idx);
    outputData(last_frame);

    // 每个优化变量维度为3，误差值维度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    Block* solver_ptr  = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(false);

    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId(curr_idx);
    v->setEstimate(Eigen::Isometry3d::Identity());
    v->setFixed(true);
    optimizer.addVertex(v);

    int last_idx = curr_idx;

    for(curr_idx = start_idx +1; curr_idx < end_idx; curr_idx++)
    {
        clock_t loop_time = clock();
        cout<<endl<<"-------"<<curr_idx<<"-------"<<endl;

        frame curr_frame = getFrame(curr_idx);

        clock_t estimate_time = clock();
        PnP_result res = estimateMotion(curr_idx, last_frame, curr_frame);
        cout<<"estimate cost: "<<1000*(clock()-estimate_time)/CLOCKS_PER_SEC<<endl;
        PnPRes2Eigen(curr_frame, res);
        integrateOdom(curr_frame);

        display(curr_frame);
        if(curr_frame.pose.at<double>(0,0)==1)
            curr_frame.pose=last_frame.pose;
        outputData(curr_frame);

        waitKey(1);
        cout<<"loop cost: "<<1000*(clock()- loop_time)/CLOCKS_PER_SEC<<endl;

        g2o::VertexSE3* v = new g2o::VertexSE3();
        //顶点
        v->setId(curr_idx);
        v->setEstimate(Eigen::Isometry3d::Identity());
        optimizer.addVertex(v);
        //边
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        edge->vertices()[0] = optimizer.vertex(last_idx);
        edge->vertices()[1] = optimizer.vertex(curr_idx);

        Eigen::Matrix<double,6,6> information = Eigen::Matrix<double,6,6>::Identity();
        information(0,0) = information(1,1)=information(2,2) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100;

        edge->setInformation(information);
        edge->setMeasurement(curr_frame.T);
        optimizer.addEdge(edge);

        last_idx = curr_idx;
        last_frame = curr_frame;
    }
    imwrite("../data/result.jpg", path);

    cout<<"optimizing pose graph, vertices: "<<optimizer.vertices().size()<<endl;
    optimizer.save("../data/result/result_before.g2o");
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    optimizer.save("../data/result/result_after.g2o");
    cout<<"optimization done"<<endl;

    //optimizer.clear();

    outputfile.close();
}

void My_VO::display(int frame_id, cv::Mat& trajectory, cv::Mat& pose)
{
    int x = int(pose.at<double>(0)) + 300;
    int y = int(pose.at<double>(2)) + 100;
    circle(trajectory, cv::Point(x, y) ,1, CV_RGB(255,0,0), 2);
    cv::imshow( "Trajectory", trajectory );
    cv::waitKey(1);
}

void My_VO::integrateOdometryStereo(int frame_i, cv::Mat& rigid_body_transformation, cv::Mat& frame_pose, const cv::Mat& rotation, const cv::Mat& translation_stereo)
{
    cv::Mat addup = (cv::Mat_<double>(1, 4) << 0, 0, 0, 1);

    cv::hconcat(rotation, translation_stereo, rigid_body_transformation);
    cv::vconcat(rigid_body_transformation, addup, rigid_body_transformation);

    // std::cout << "rigid_body_transformation" << rigid_body_transformation << std::endl;

    double scale = sqrt((translation_stereo.at<double>(0))*(translation_stereo.at<double>(0))
                        + (translation_stereo.at<double>(1))*(translation_stereo.at<double>(1))
                        + (translation_stereo.at<double>(2))*(translation_stereo.at<double>(2))) ;

    // frame_pose = frame_pose * rigid_body_transformation;
    std::cout << "scale: " << scale << std::endl;

    rigid_body_transformation = rigid_body_transformation.inv();
    // if ((scale>0.1)&&(translation_stereo.at<double>(2) > translation_stereo.at<double>(0)) && (translation_stereo.at<double>(2) > translation_stereo.at<double>(1)))
    if (scale > 0.05 && scale < 10)
    {
      frame_pose = frame_pose * rigid_body_transformation;
    }
    else
    {
     std::cout << "[WARNING] scale below 0.1, or incorrect translation" << std::endl;
    }
}

bool isRotationMatrix(cv::Mat &R)
{
    cv::Mat Rt;
    transpose(R, Rt);
    cv::Mat shouldBeIdentity = Rt * R;
    cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());

    return  norm(I, shouldBeIdentity) < 1e-6;

}

cv::Vec3f My_VO::rotationMatrixToEulerAngles(cv::Mat &R)
{
    assert(isRotationMatrix(R));

    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );

    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return cv::Vec3f(x, y, z);
}

void My_VO::displayTracking(cv::Mat& imageLeft_t1,
                     std::vector<cv::Point2f>&  pointsLeft_t0,
                     std::vector<cv::Point2f>&  pointsLeft_t1)
{
      int radius = 2;
      cv::Mat vis;

      cv::cvtColor(imageLeft_t1, vis, cv::COLOR_GRAY2BGR, 3);

      for (int i = 0; i < pointsLeft_t0.size(); i++)
      {
          cv::circle(vis, cv::Point(pointsLeft_t0[i].x, pointsLeft_t0[i].y), radius, CV_RGB(0,255,0));
      }

      for (int i = 0; i < pointsLeft_t1.size(); i++)
      {
          cv::circle(vis, cv::Point(pointsLeft_t1[i].x, pointsLeft_t1[i].y), radius, CV_RGB(255,0,0));
      }

      for (int i = 0; i < pointsLeft_t1.size(); i++)
      {
          cv::line(vis, pointsLeft_t0[i], pointsLeft_t1[i], CV_RGB(0,255,0));
      }

      cv::imshow("vis ", vis );
}

void My_VO::trackingFrame2Frame(cv::Mat& projMatrl, cv::Mat& projMatrr,
                         std::vector<cv::Point2f>&  pointsLeft_t0,
                         std::vector<cv::Point2f>&  pointsLeft_t1,
                         cv::Mat& points3D_t0,
                         cv::Mat& rotation,
                         cv::Mat& translation,
                         bool mono_rotation)
{
      cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
      cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
      cv::Mat intrinsic_matrix = (cv::Mat_<float>(3, 3) << projMatrl.at<float>(0, 0), projMatrl.at<float>(0, 1), projMatrl.at<float>(0, 2),
                                                   projMatrl.at<float>(1, 0), projMatrl.at<float>(1, 1), projMatrl.at<float>(1, 2),
                                                   projMatrl.at<float>(2, 0), projMatrl.at<float>(2, 1), projMatrl.at<float>(2, 2));

      int iterationsCount = 500;        // number of Ransac iterations.
      float reprojectionError = .5;    // maximum allowed distance to consider it an inlier.
      float confidence = 0.999;          // RANSAC successful confidence.
      bool useExtrinsicGuess = true;
      int flags =cv::SOLVEPNP_ITERATIVE;

      cv::Mat inliers;
      cv::solvePnPRansac( points3D_t0, pointsLeft_t1, intrinsic_matrix, distCoeffs, rvec, translation,
                          useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
                          inliers, flags );
    if (!mono_rotation)
      {
        cv::Rodrigues(rvec, rotation);
      }

      std::cout<<"inlier num: "<<inliers.rows<<std::endl;

      std::cout << "[trackingFrame2Frame] inliers size: " << inliers.size() << std::endl;
}

void My_VO::matchingFeatures(cv::Mat& imageLeft_t0, cv::Mat& imageRight_t0,
                      cv::Mat& imageLeft_t1, cv::Mat& imageRight_t1,
                      FeatureSet& currentVOFeatures,
                      std::vector<cv::Point2f>&  pointsLeft_t0,
                      std::vector<cv::Point2f>&  pointsRight_t0,
                      std::vector<cv::Point2f>&  pointsLeft_t1,
                      std::vector<cv::Point2f>&  pointsRight_t1)
{
    std::vector<cv::Point2f>  pointsLeftReturn_t0;

    if (currentVOFeatures.size() < 2000)
    {
        appendNewFeatures(imageLeft_t0, currentVOFeatures);
    }

    int bucket_size = imageLeft_t0.rows/10;
    int features_per_bucket = 1;
    bucketingFeatures(imageLeft_t0, currentVOFeatures, bucket_size, features_per_bucket);

    pointsLeft_t0 = currentVOFeatures.points;

    circularMatching(imageLeft_t0, imageRight_t0, imageLeft_t1, imageRight_t1,
                     pointsLeft_t0, pointsRight_t0, pointsLeft_t1, pointsRight_t1, pointsLeftReturn_t0, currentVOFeatures);

    std::vector<bool> status;
    checkValidMatch(pointsLeft_t0, pointsLeftReturn_t0, status, 0);

    removeInvalidPoints(pointsLeft_t0, status);
    removeInvalidPoints(pointsLeft_t1, status);
    removeInvalidPoints(pointsRight_t0, status);
    removeInvalidPoints(pointsRight_t1, status);

    currentVOFeatures.points = pointsLeft_t1;
}

void My_VO::removeInvalidPoints(std::vector<cv::Point2f>& points, const std::vector<bool>& status)
{
    int index = 0;
    for (int i = 0; i < status.size(); i++)
    {
        if (status[i] == false)
        {
            points.erase(points.begin() + index);
        }
        else
        {
            index ++;
        }
    }
}

void My_VO::checkValidMatch(std::vector<cv::Point2f>& points, std::vector<cv::Point2f>& points_return, std::vector<bool>& status, int threshold)
{
    int offset;
    for (int i = 0; i < points.size(); i++)
    {
        offset = std::max(std::abs(points[i].x - points_return[i].x), std::abs(points[i].y - points_return[i].y));
        // std::cout << offset << ", ";

        if(offset > threshold)
        {
            status.push_back(false);
        }
        else
        {
            status.push_back(true);
        }
    }
}

void My_VO::circularMatching(cv::Mat img_l_0, cv::Mat img_r_0, cv::Mat img_l_1, cv::Mat img_r_1,
                      std::vector<cv::Point2f>& points_l_0, std::vector<cv::Point2f>& points_r_0,
                      std::vector<cv::Point2f>& points_l_1, std::vector<cv::Point2f>& points_r_1,
                      std::vector<cv::Point2f>& points_l_0_return,
                      FeatureSet& current_features)
{
    std::vector<float> err;
  cv::Size winSize=cv::Size(21,21);
  cv::TermCriteria termcrit=cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01);

  std::vector<uchar> status0;
  std::vector<uchar> status1;
  std::vector<uchar> status2;
  std::vector<uchar> status3;

  clock_t tic = clock();
  calcOpticalFlowPyrLK(img_l_0, img_r_0, points_l_0, points_r_0, status0, err, winSize, 3, termcrit, 0, 0.001);
  calcOpticalFlowPyrLK(img_r_0, img_r_1, points_r_0, points_r_1, status1, err, winSize, 3, termcrit, 0, 0.001);
  calcOpticalFlowPyrLK(img_r_1, img_l_1, points_r_1, points_l_1, status2, err, winSize, 3, termcrit, 0, 0.001);
  calcOpticalFlowPyrLK(img_l_1, img_l_0, points_l_1, points_l_0_return, status3, err, winSize, 3, termcrit, 0, 0.001);
  clock_t toc = clock();
  std::cerr << "calcOpticalFlowPyrLK time: " << float(toc - tic)/CLOCKS_PER_SEC*1000 << "ms" << std::endl;

  deleteUnmatchFeaturesCircle(points_l_0, points_r_0, points_r_1, points_l_1, points_l_0_return,
                        status0, status1, status2, status3, current_features.ages);
}

void My_VO::deleteUnmatchFeaturesCircle(std::vector<cv::Point2f>& points0, std::vector<cv::Point2f>& points1,
                          std::vector<cv::Point2f>& points2, std::vector<cv::Point2f>& points3,
                          std::vector<cv::Point2f>& points0_return,
                          std::vector<uchar>& status0, std::vector<uchar>& status1,
                          std::vector<uchar>& status2, std::vector<uchar>& status3,
                          std::vector<int>& ages)
{
    for (int i = 0; i < ages.size(); ++i)
    {
        ages[i] += 1;
    }

    int indexCorrection = 0;
    for( int i=0; i<status3.size(); i++)
        {  cv::Point2f pt0 = points0.at(i- indexCorrection);
            cv::Point2f pt1 = points1.at(i- indexCorrection);
            cv::Point2f pt2 = points2.at(i- indexCorrection);
            cv::Point2f pt3 = points3.at(i- indexCorrection);
            cv::Point2f pt0_r = points0_return.at(i- indexCorrection);

            if ((status3.at(i) == 0)||(pt3.x<0)||(pt3.y<0)||
                (status2.at(i) == 0)||(pt2.x<0)||(pt2.y<0)||
                (status1.at(i) == 0)||(pt1.x<0)||(pt1.y<0)||
                (status0.at(i) == 0)||(pt0.x<0)||(pt0.y<0))
            {
            if((pt0.x<0)||(pt0.y<0)||(pt1.x<0)||(pt1.y<0)||(pt2.x<0)||(pt2.y<0)||(pt3.x<0)||(pt3.y<0))
            {
                status3.at(i) = 0;
            }
            points0.erase (points0.begin() + (i - indexCorrection));
            points1.erase (points1.begin() + (i - indexCorrection));
            points2.erase (points2.begin() + (i - indexCorrection));
            points3.erase (points3.begin() + (i - indexCorrection));
            points0_return.erase (points0_return.begin() + (i - indexCorrection));

            ages.erase (ages.begin() + (i - indexCorrection));
            indexCorrection++;
            }

        }
}

void My_VO::bucketingFeatures(cv::Mat& image, FeatureSet& current_features, int bucket_size, int features_per_bucket)
{
    int image_height = image.rows;
    int image_width = image.cols;
    int buckets_nums_height = image_height/bucket_size;
    int buckets_nums_width = image_width/bucket_size;
    int buckets_number = buckets_nums_height * buckets_nums_width;

    std::vector<Bucket> Buckets;

    // initialize all the buckets
    for (int buckets_idx_height = 0; buckets_idx_height <= buckets_nums_height; buckets_idx_height++)
    {
      for (int buckets_idx_width = 0; buckets_idx_width <= buckets_nums_width; buckets_idx_width++)
      {
        Buckets.push_back(Bucket(features_per_bucket));
      }
    }

    // bucket all current features into buckets by their location
    int buckets_nums_height_idx, buckets_nums_width_idx, buckets_idx;
    for (int i = 0; i < current_features.points.size(); ++i)
    {
      buckets_nums_height_idx = current_features.points[i].y/bucket_size;
      buckets_nums_width_idx = current_features.points[i].x/bucket_size;
      buckets_idx = buckets_nums_height_idx*buckets_nums_width + buckets_nums_width_idx;
      Buckets[buckets_idx].add_feature(current_features.points[i], current_features.ages[i]);

    }

    // get features back from buckets
    current_features.clear();
    for (int buckets_idx_height = 0; buckets_idx_height <= buckets_nums_height; buckets_idx_height++)
    {
      for (int buckets_idx_width = 0; buckets_idx_width <= buckets_nums_width; buckets_idx_width++)
      {
         buckets_idx = buckets_idx_height*buckets_nums_width + buckets_idx_width;
         Buckets[buckets_idx].get_features(current_features);
      }
    }

    std::cout << "current features number after bucketing: " << current_features.size() << std::endl;

}

void My_VO::appendNewFeatures(cv::Mat& image, FeatureSet& current_features)
{
    std::vector<cv::Point2f>  points_new;
    featureDetectionFast(image, points_new);
    current_features.points.insert(current_features.points.end(), points_new.begin(), points_new.end());
    std::vector<int>  ages_new(points_new.size(), 0);
    current_features.ages.insert(current_features.ages.end(), ages_new.begin(), ages_new.end());
}

void My_VO::featureDetectionFast(cv::Mat image, std::vector<cv::Point2f>& points)
{
  std::vector<cv::KeyPoint> keypoints;
  int fast_threshold = 20;
  bool nonmaxSuppression = true;
  cv::FAST(image, keypoints, fast_threshold, nonmaxSuppression);
  cv::KeyPoint::convert(keypoints, points, std::vector<int>());
}

void My_VO::outputData(frame& frame)
{
    if(!outputfile.is_open())
        return;

    for(int i=0; i < 3;i++)
    {
        for(int j=0; j<4;j++)
        {
            outputfile<<frame.pose.at<double>(i,j);
            if(!((i==2)&&(j==3)))
                outputfile<<" ";
        }
    }
    outputfile<<endl;
}

void My_VO::getMeanPose(vector<frame> frame_list, frame& frame)
{
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            double element = 0;
            int cnt = 0;
            for(int idx = 0; idx < frame_list.size(); idx++)
            {
                if(frame_list[idx].valid)
                {
                    element += frame_list[idx].pose.at<double>(i,j);
                    cnt+=1;
                }
            }
            if(cnt == 0)
            {
                frame.valid = false;
                return;
            }
            element /= cnt;
            frame.pose.at<double>(i, j) = element;
        }
    }
}

void My_VO::display(frame& frame)
{
    if(!frame.valid)
        return;

    int x = int(frame.pose.at<double>(0,3)) + 300;
    int y = int(frame.pose.at<double>(2,3)) + 200;
    cout<<"x: "<<x<<endl;
    circle(path, cv::Point(x, y) ,1, CV_RGB(255-255*int(frame.id/_IMAGE_NUM),255-255*int(frame.id/_IMAGE_NUM),255*int(frame.id/_IMAGE_NUM)), 2);

    if(_SHOW_PATH)
        imshow("path", path);
}

void My_VO::integrateOdom(frame& last_frame, frame& curr_frame)
{
    if(!(last_frame.valid&&curr_frame.valid))
    {
        return;
    }
    Mat addup =   (Mat_<double>(1, 4) << 0, 0, 0, 1);
    Mat rigid_body_transformation;

    hconcat(curr_frame.R, curr_frame.t, rigid_body_transformation);
    vconcat(rigid_body_transformation, addup, rigid_body_transformation);

    double scale = sqrt((curr_frame.t.at<double>(0))*(curr_frame.t.at<double>(0))
                        + (curr_frame.t.at<double>(1))*(curr_frame.t.at<double>(1))
                        + (curr_frame.t.at<double>(2))*(curr_frame.t.at<double>(2))) ;
    std::cout << "scale: " << scale << std::endl;

    if (scale < 10)
    {
        rigid_body_transformation = rigid_body_transformation.inv();
        curr_frame.pose = last_frame.pose * rigid_body_transformation;
    }
    else
    {
        std::cout << "[WARNING] scale below 0.1, or incorrect translation" << std::endl;
    }
}

void My_VO::integrateOdom(frame& curr_frame)
{
    if(!(curr_frame.valid))
        return;

    Mat addup =   (Mat_<double>(1, 4) << 0, 0, 0, 1);
    Mat rigid_body_transformation;

    hconcat(curr_frame.R, curr_frame.t, rigid_body_transformation);
    vconcat(rigid_body_transformation, addup, rigid_body_transformation);

    double scale = sqrt((curr_frame.t.at<double>(0))*(curr_frame.t.at<double>(0))
                        + (curr_frame.t.at<double>(1))*(curr_frame.t.at<double>(1))
                        + (curr_frame.t.at<double>(2))*(curr_frame.t.at<double>(2))) ;
    std::cout << "scale: " << scale << std::endl;

    if (scale < 10)
    {
        rigid_body_transformation = rigid_body_transformation.inv();
        pose = pose * rigid_body_transformation;
        curr_frame.pose = pose;
    }
    else
    {
        std::cout << "[WARNING] scale below 0.1, or incorrect translation" << std::endl;
    }
}

struct PnP_result My_VO::estimateMotion(int idx, frame& last_frame, frame& curr_frame)
{
    vector<DMatch> matches[2], lrmatches;
    BFMatcher matcher;
    matcher.match(last_frame.desp[_LEFT], curr_frame.desp[_LEFT], matches[_LEFT]);
    matcher.match(last_frame.desp[_RIGHT], curr_frame.desp[_RIGHT], matches[_RIGHT]);
    matcher.match(last_frame.desp[_LEFT], last_frame.desp[_RIGHT], lrmatches);

    // Mat lrmatch_img;
    // drawMatches(last_frame.src[_LEFT], last_frame.kp[_LEFT], last_frame.src[_RIGHT], last_frame.kp[_RIGHT], lrmatches, lrmatch_img);
    // imshow("lrmatches", lrmatch_img);

    if(_WRITE_MATCH_IMAGE)
    {
        Mat match_img;
        drawMatches(last_frame.src[_LEFT], last_frame.kp[_LEFT], curr_frame.src[_LEFT], curr_frame.kp[_LEFT], matches[_LEFT], match_img);
        imshow("matches", match_img);

        stringstream path;
        path<<"../data/match_images/"<<idx<<".jpg";
        imwrite(path.str(), match_img);
        std::cout<<"match image write"<<endl;
    }

    vector<DMatch> good_matches[2], lrgood_matches;

	double minDist = 10000, maxDist = 0;
	for(int i = 0; i < (int)matches[_LEFT].size(); i++)
	{
		double dist = matches[_LEFT][i].distance;

		minDist = minDist > dist ? dist : minDist;
		maxDist = maxDist < dist ? dist : maxDist;
	}

	double matchDist = max(100.0, minDist * 2);
	for(int i = 0; i < (int)matches[_LEFT].size(); i++)
		if(matches[_LEFT][i].distance <= matchDist)
        {
			good_matches[_LEFT].push_back(matches[_LEFT][i]);
            good_matches[_RIGHT].push_back(matches[_RIGHT][i]);
        }

    minDist = 10000;
    maxDist = 0;
    for(int i = 0; i < (int)lrmatches.size(); i++)
	{
		double dist = lrmatches[i].distance;

		minDist = minDist > dist ? dist : minDist;
		maxDist = maxDist < dist ? dist : maxDist;
	}
    matchDist = max(100.0, minDist * 2);
	for(int i = 0; i < (int)lrmatches.size(); i++)
		if(lrmatches[i].distance <= matchDist)
        {
			lrgood_matches.push_back(lrmatches[i]);
        }

    // drawMatches(last_frame.src[_LEFT], last_frame.kp[_LEFT], last_frame.src[_RIGHT], last_frame.kp[_RIGHT], lrgood_matches, lrmatch_img);
    // imshow("lrgood matches", lrmatch_img);

    if(_WRITE_MATCH_IMAGE)
    {
        Mat match_img;
        drawMatches(last_frame.src[_LEFT], last_frame.kp[_LEFT], curr_frame.src[_LEFT], curr_frame.kp[_LEFT], good_matches[_LEFT], match_img);
        imshow("good matches", match_img);

        stringstream path;
        path<<"../data/good_match_images/"<<idx<<".jpg";
        imwrite(path.str(), match_img);
        std::cout<<"good match image write"<<endl;
    }

    Mat pts_obj, pts_obj_4d;
    vector<Point2f> pts_img, points_left_t0, points_left_t1, points_right_t0, points_right_t1;
    for(int i = 0; i<matches[_LEFT].size(); i++)
    {
        points_left_t0.push_back(Point2f(last_frame.kp[_LEFT][lrmatches[i].queryIdx].pt));
        points_left_t1.push_back(Point2f(curr_frame.kp[_LEFT][matches[_LEFT][i].trainIdx].pt));
        points_right_t0.push_back(Point2f(last_frame.kp[_RIGHT][lrmatches[i].trainIdx].pt));
    }
    triangulatePoints(l_matrix, r_matrix, points_left_t0, points_right_t0, pts_obj_4d);
    convertPointsFromHomogeneous(pts_obj_4d.t(), pts_obj);
    pts_img = points_left_t1;

    double camera_intrinsic_matrix[3][3]=
    {
        {camera_parameters.fx, 0, camera_parameters.cx},
        {0, camera_parameters.fy, camera_parameters.cy},
        {0, 0, 1}
    };

    Mat camera_matrix(3,3, CV_64F, camera_intrinsic_matrix);
    Mat rvec, tvec, inliers;
    Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);

    solvePnPRansac(pts_obj, pts_img, camera_matrix, distCoeffs, rvec, tvec, false, 500, 0.5f, 0.99,inliers);

    cout<<"inliers: "<<inliers.rows<<endl;

    vector< cv::DMatch > inlier_match;
    for(int i=0; i<inliers.rows;i++)
    {
        inlier_match.push_back(matches[_LEFT][inliers.ptr<int>(i)[0]]);
    }
    if(_WRITE_INLIER)
    {
        Mat match_img;
        drawMatches(last_frame.src[_LEFT], last_frame.kp[_LEFT], curr_frame.src[_LEFT], curr_frame.kp[_LEFT], inlier_match, match_img);
        imshow("inliers match", match_img);

        stringstream path;
        path<<"../data/inlier_images/"<<idx<<".jpg";
        imwrite(path.str(), match_img);
        std::cout<<"inlier math image write"<<endl;
    }

    PnP_result res;
    res.R = rvec;
    res.t = tvec;
    res.inliers = inliers.rows;

    return res;
}

frame My_VO::getFrame(const int& idx, bool detect)
{
    frame res;
    res.id  = idx;
    Mat l = imread(image_list[_LEFT][idx],0);
    Mat r = imread(image_list[_RIGHT][idx],0);
    cout << "image get"<<endl;
    res.src[_LEFT] = l;
    res.src[_RIGHT] = r;
    if(!detect)
        return res;
    Ptr<ORB> _detector;
    int nfeatures = 1000;
    _detector = ORB::create(nfeatures);

    _detector->detect(res.src[_LEFT], res.kp[_LEFT]);
    _detector->detect(res.src[_RIGHT], res.kp[_RIGHT]);
    cout<<"Key points of images: "<<res.kp[_LEFT].size()<<endl;

    Mat desp;
    _detector->compute(res.src[_LEFT], res.kp[_LEFT], res.desp[_LEFT]);
    _detector->compute(res.src[_RIGHT], res.kp[_RIGHT], res.desp[_RIGHT]);

    // Mat disp, depth;

    // stereoSGBM(l, r, disp);
    // if(_WRITE_DISP)
    // {
    //     stringstream path;
    //     path<<"../data/disp_images/"<<idx<<".jpg";
    //     imwrite(path.str(),disp);
    //     cout<<"disp image write"<<endl;
    // }

    // disp2Depth(disp, depth);
    // if(_WRITE_DEPTH)
    // {
    //     stringstream path;
    //     path<<"../data/depth_images/"<<idx<<".jpg";
    //     imwrite(path.str(),depth);
    //     cout<<"depth image write"<<endl;
    // }
    // imshow("depth", depth);
    // //waitKey(0);

    // res.depth = depth;
    return res;
}

void My_VO::PnPRes2Eigen(frame& curr_frame, PnP_result& res)
{
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    Eigen::Matrix3d R;
    Mat r;

    Rodrigues(res.R, r);
    for ( int i=0; i<3; i++ )
        for ( int j=0; j<3; j++ )
            R(i,j) = r.at<double>(i,j);

    Eigen::AngleAxisd angle(R);
    T = angle;

    double alpha = atan2(r.at<double>(2,1), r.at<double>(2,2));
    double beta = atan2(-r.at<double>(2,0), sqrt(pow(r.at<double>(2,1),2) + pow(r.at<double>(2,2),2)));
    double gamma = atan2(r.at<double>(1,0), r.at<double>(0,0));
    if(alpha>0.1||beta>0.1||gamma>0.1)
    {
        curr_frame.valid = false;
        return;
    }

    Eigen::Translation<double,3> trans(res.t.at<double>(0,0), res.t.at<double>(1,0), res.t.at<double>(2,0));

    for(int i = 0; i<3; i++)
    {
        T(i,3)=res.t.at<double>(i,0);
        if(T(i,3)>=1.0)
        {
            curr_frame.valid = false;
            return;
        }
    }
    curr_frame.R = r;
    curr_frame.t = res.t;
    curr_frame.T = T;
    cout<<curr_frame.T.matrix()<<endl;
}

vector<vector<string>> My_VO::addData()
{
    vector<string> l, r;

    for(int i = 0; i < _IMAGE_NUM; i++)
    {
        stringstream l_stream, r_stream;

        string zero_num;
        if(i < 10)
            zero_num = "00000";
        else if(i <100)
            zero_num = "0000";
        else
            zero_num = "000";

        l_stream << "../data/image_0/" << zero_num << i <<".png";
        l.push_back(l_stream.str());

        r_stream << "../data/image_1/" << zero_num << i <<".png";
        r.push_back(r_stream.str());
    }
    image_list.push_back(l);
    image_list.push_back(r);

    return image_list;
}

Point3f My_VO::from2dTo3d(const Point3f& point)
{
    Point3f p;
    p.z = double(point.z) / camera_parameters.scale;
    p.x = (point.x - camera_parameters.cx) * p.z / camera_parameters.fx;
    p.y = (point.y - camera_parameters.cy) * p.z / camera_parameters.fy;
    return p;
}

void My_VO::stereoSGBM(Mat lpng, Mat rpng, Mat&disp)
{
    disp.create(lpng.rows, lpng.cols, CV_16S);
    cv::Mat disp1 = cv::Mat(lpng.rows, lpng.cols, CV_8UC1);
    cv::Size img_size = lpng.size();
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create();
    int nmDisparities = 256;((img_size.width / 8) + 15) & -16;
    int pngChannels = lpng.channels();
    int winSize = 6;
    sgbm->setPreFilterCap(13);
    sgbm->setBlockSize(winSize);
    sgbm->setP1(8 * pngChannels * winSize * winSize);
    sgbm->setP2(32 * pngChannels * winSize * winSize);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(nmDisparities);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
    sgbm->compute(lpng, rpng, disp);
    normalize(disp, disp, 0, 255, NORM_MINMAX);
    normalize(disp1, disp1, 0, 255, NORM_MINMAX);
    imshow("disp1", disp1);
    imshow("disp2", disp);
    //disp.convertTo(disp1, CV_32F, 1.0/16.0f);
    //disp = disp1;
}

void My_VO::disp2Depth(Mat disp, Mat& depth)
{
    depth.create(disp.rows, disp.cols, CV_8UC1);
    cv::Mat depth1 = cv::Mat(disp.rows, disp.cols, CV_16S);
    for (int i = 0; i < disp.rows; i++)
    {
        for (int j = 0; j < disp.cols; j++)
        {
            if (!disp.ptr<uint16_t>(i)[j])//��ֹ��0�ж�
                continue;
            depth1.ptr<uint16_t>(i)[j] = camera_parameters.scale * camera_parameters.fx * camera_parameters.baseline / disp.ptr<ushort>(i)[j];
        }
    }
    normalize(depth, depth, 0, 255, NORM_MINMAX);
    depth1.convertTo(depth, CV_8U, 1. / 256);
}


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#include <iostream>
#include <sstream>
#include <sys/time.h>
//#include <random> // can only use with C++11
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>

#include <boost/program_options.hpp>
#include <boost/random.hpp>

namespace po = boost::program_options;
using std::cout;
using std::endl;

#include <jsCore/timer.hpp>

float ToDeg(float rad) {
  return rad*180./M_PI;
}
float ToRad(float deg) {
  return deg/180.*M_PI;
}
double ToDeg(double rad) {
  return rad*180./M_PI;
}
double ToRad(double deg) {
  return deg/180.*M_PI;
}

int main (int argc, char** argv)
{
  // Declare the supported options.
  po::options_description desc("Apply random transformation to input point cloud.\n The affine transformation is sampled as: (1) sample random rotation axis (uniformly on the sphere) and use specified rotation magnitude to obtain rotation and (2) sample random rotation uniformly on the sphere with radius spcified in the translation argument.\nAllowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("input,i", po::value<string>(),"path to input point cloud")
    ("output,o", po::value<string>(),"path to output transformed point cloud")
    ("angle,a", po::value<double>(),"magnitude of rotation (deg)")
    ("translation,t", po::value<double>(),"magnitude of translation (m)")
    ;
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  double angle = 10.; // In degree
  double translation = 1.0;
  string inputPath = "./file.ply";
  string outputPath = "./out.ply";
  if(vm.count("input")) inputPath = vm["input"].as<string>();
  if(vm.count("output")) outputPath = vm["output"].as<string>();
  if(vm.count("angle")) angle = vm["angle"].as<double>();
  if(vm.count("translation")) translation = vm["translation"].as<double>();

  std::stringstream ssOutPath;
  std::stringstream ssTransformationFile;
  ssOutPath << outputPath << "_angle_" << angle << "_translation_" <<
    translation << ".ply";
  ssTransformationFile << outputPath << "_angle_" << angle <<
    "_translation_" << translation << "_TrueTransformation" << ".csv";

  outputPath = ssOutPath.str();
  std::string transformationOutputPath = ssTransformationFile.str();

  // Load point cloud.
  pcl::PointCloud<pcl::PointXYZRGBNormal> pcIn, pcOut;
  pcl::PLYReader reader;
  if (reader.read(inputPath, pcIn))
    std::cout << "error reading " << inputPath << std::endl;
  else
    std::cout << "loaded pc from " << inputPath << ": " << pcIn.width << "x"
      << pcIn.height << std::endl;

  std::cout<< " input pointcloud from "<<inputPath<<std::endl;
  std::cout<< "  angular magnitude "<< angle <<std::endl;
  std::cout<< "  translational magnitude "<< translation <<std::endl;
  std::cout<< " output to "<<outputPath<<std::endl;
  std::cout<< " sampled transformation to " << transformationOutputPath << std::endl;

  // Using boost here because C11 and CUDA seem to have troubles.
  timeval tNow;
  gettimeofday(&tNow, NULL);
  boost::mt19937 gen(tNow.tv_usec);
  boost::normal_distribution<> N(0,1);
  // Sample axis of rotation:
  Eigen::Vector3f axis(N(gen), N(gen), N(gen));
  axis /= axis.norm();
  // Construct rotation:
  Eigen::AngleAxisf aa(ToRad(angle), axis);
  Eigen::Quaternionf q(aa);
  // Sample translation on sphere with radius translation:
  Eigen::Vector3f t(N(gen), N(gen), N(gen));
  t *= translation / t.norm();

  Eigen::Affine3f T = Eigen::Affine3f::Identity();
  T.translation() = t;
  T.rotate(q);

  std::cout << "sampled random transformation:\n"
    << T.matrix() << std::endl;

  // Transform both points by T as well as surface normals by R
  // manually since the standard transformPointCloud does not seem to
  // touch the Surface normals.
  pcOut = pcIn;
  for (uint32_t i=0; i<pcOut.size(); ++i) {
    Eigen::Map<Eigen::Vector3f> p(&(pcOut.at(i).x));
    p = T.rotation() * p + T.translation();
    Eigen::Map<Eigen::Vector3f> n(pcOut.at(i).normal);
    n = T.rotation()*n;
  }

  pcl::PLYWriter writer;
  writer.write(outputPath, pcOut, false, false);

  std::ofstream out(transformationOutputPath.c_str());
  out << "q_w q_x q_y q_z t_x t_y t_z" << std::endl;
  out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
    << t(0) << " " << t(1) << " " << t(2);
  out.close();
}


#include <boost/tuple/tuple.hpp>
#include <map>

#include <boost/graph/adjacency_list.hpp>
#include "tdlib/TD_combinations.hpp"
#include "tdlib/TD_misc.hpp"

#ifndef TD_STRUCT_VERTEX
#define TD_STRUCT_VERTEX

struct Vertex{
    unsigned int id;
};

#endif

typedef boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, Vertex> TD_graph_t;

struct bag{
    std::set<unsigned int> bag;
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, bag> TD_tree_dec_t;

void make_tdlib_graph(TD_graph_t &G, std::vector<unsigned int> &V, std::vector<unsigned int> &E){
    unsigned int max = 0;
    for(unsigned int i = 0; i < V.size(); i++)
        max = (V[i]>max)? V[i] : max;

    std::vector<TD_graph_t::vertex_descriptor> idxMap(max+1);

    for(unsigned int i = 0; i < V.size(); i++){
        idxMap[V[i]] = boost::add_vertex(G);
        G[idxMap[V[i]]].id = V[i];
    }

    if(E.size() != 0){
        for(unsigned int j = 0; j < E.size()-1; j++){
            boost::add_edge(idxMap[E[j]], idxMap[E[j+1]], G);
            j++;
        }
    }
}

void make_sage_decomp(TD_tree_dec_t &T, std::vector<std::vector<int> > &V_T, std::vector<unsigned int> &E_T){
    std::map<boost::graph_traits<TD_tree_dec_t>::vertex_descriptor, unsigned int> vertex_map;
    boost::graph_traits<TD_tree_dec_t>::vertex_iterator tIt, tEnd;
    unsigned int id = 0;

    for(boost::tie(tIt, tEnd) = boost::vertices(T); tIt != tEnd; tIt++){
        vertex_map.insert(std::pair<boost::graph_traits<TD_tree_dec_t>::vertex_descriptor, unsigned int>(*tIt, id++));
        std::vector<int> bag;
        for(std::set<unsigned int>::iterator sIt = T[*tIt].bag.begin(); sIt != T[*tIt].bag.end(); sIt++)
            bag.push_back((int)*sIt);
        V_T.push_back(bag);
    }

    boost::graph_traits<TD_tree_dec_t>::edge_iterator eIt, eEnd;
    for(boost::tie(eIt, eEnd) = boost::edges(T); eIt != eEnd; eIt++){
        std::map<boost::graph_traits<TD_tree_dec_t>::vertex_descriptor, unsigned int>::iterator v, w;
        v = vertex_map.find(boost::source(*eIt, T));
        w = vertex_map.find(boost::target(*eIt, T));
        E_T.push_back(v->second);
        E_T.push_back(w->second);
    }
}

/* EXACT TREE DECOMPOSITIONS */

int sage_exact_decomposition(std::vector<unsigned int> &V_G, std::vector<unsigned int> &E_G, std::vector<std::vector<int> > &V_T, std::vector<unsigned int> &E_T, int lb){
    TD_graph_t G;
    make_tdlib_graph(G, V_G, E_G);

    TD_tree_dec_t T;

    treedec::exact_decomposition_cutset(G, T, lb);

    treedec::make_small(T);

    make_sage_decomp(T, V_T, E_T);

    return treedec::get_width(T);
}


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>
#include <ctime>
#include "Tools.hpp"
#include "Magnet.hpp"
using namespace std;

/*****************************************************************************

This program contains the main function for the project, as well as graphing
capability. Several constants are prompted from the user, and several are just
assumed for the sake of consistency. The program first seeds the random
number generator, vital for the Monte-Carlo method to function. It then
creates an allocated magnet object, initialized with random spins and the
temperature that the user input. It simulates for the specified number of
iterations and then plots the final matrix of spins.

It then jumps into a loop to generate the Energy and Magnetization vs. Time
plots. For each temperature, mag is reset to all zeroes (to make graphs easier
to understand) and simulated. The final energy and magnetization are recorded
and added to their respective vectors along with time. After the loop exits,
a running exponential average is performed on both data vectors before they
are plotted. The Magnet object is then deleted and the program exits.

*****************************************************************************/

const int lattice = inputInt(50,"lattice size");				// Constants used for simulation / initialization
const double defaultJ = 1;										// of the Magnet object. J = kb = 1 from the assignment
const double firstTemp = inputDouble(1,"ambient temperature");;
const double defaultKb = 1;
const double tStep = 0.01;
const int MAX_ITERS = inputInt(100,"maximum iterations");

void plotSpinMatrix(Magnet* m){			// Plot the matrix of spins!
	Gnuplot gp;
	gp << setprecision(3);
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set title \"Spins of a Randomized Ising Model After " << MAX_ITERS;
	gp << " iterations of the Metropolis Algorithm\\nwith Ambient Temperature " << firstTemp;
	gp << ", k_b = " << defaultKb << ", J = " << defaultJ << "\"\n";
	gp << "set output \"mag.png\"\n";
	gp << "set palette grey\n";
	gp << "set pm3d map\n";
	gp << "unset key\n";
	gp << "unset colorbox\n";
	gp << "splot '-' matrix with image\n";
	gp.send1d(m->getAllSpins());		// For some reason, gnuplot requires matrix input from a 2-D array to be sent as 1-D data
}

void plotEnergy(vector<double>& eng, vector<double>& t){	// Plotting energy per spin
	Gnuplot gp;
	gp << setprecision(3);
	gp << "set xrange [1:4]\n";
	gp << "set yrange [-2:0]\n";
	gp << "set title \"Energy per Spin vs. Temperature in the Ising Model\\n";
	gp << "with k_b = " << defaultKb << ", J = " << defaultJ << "\"\n";
	gp << "set xlabel \"Temperature (inv. Boltzmann Constants)\"\n";
	gp << "set ylabel \"Energy per Spin (E_{/Symbol a} / N)\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set output \"energy.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,eng));
}

void plotMagnetization(vector<double>& v, vector<double>& t){	// Magnetization per spin
	Gnuplot gp;
	gp << setprecision(3);
	gp << "set xrange [1:4]\n";
	gp << "set yrange [-1:0.1]\n";
	gp << "set title \"Average Magnetization Per Spin vs. Temperature in the Ising Model\\n";
	gp << "with k_b = " << defaultKb << ", J = " << defaultJ << "\"\n";
	gp << "set xlabel \"Temperature (inv. Boltzmann Constants)\"\n";
	gp << "set ylabel \"Magnetization Per Spin\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set output \"magnetization.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,v));
}

int main(){						// Main function
	srand(time(NULL));			// Seed the random number generator (this carries to the other programs)
	Magnet* mag = new Magnet(lattice, firstTemp, defaultKb, defaultJ);	// Initialize the Magnet object
	vector<double> temp;		// Create vectors to store data for later plots
	vector<double> energy;
	vector<double> mzation;

	mag->simulate(MAX_ITERS);	// Simulate the magnet with user-given parameters
	plotSpinMatrix(mag);		// Plot the final spins
	mag->setNeg();				// Reset to all spin down for nest segment

	for(double t = 0.8; t < 4.2; t+= tStep){	// Bounds chosen to allow rolling average to stabilize at ends (only plotting 1-4)
		mag->setTemp(t);						// Set temperature
		mag->simulate(MAX_ITERS);				// and simulate!
		energy.push_back(mag->getEnergy());		// Then store important quantities
		mzation.push_back(mag->getMag());
		temp.push_back(t);						// As well as the temperature
		mag->setNeg();							// Reset for next simulation
		cout << "Finished T = " << t << endl;	// Just to track progress, print that it finished an iteration
	}

	vector<double> rollingE = expAvg(energy,0.02);	// Get the exponential rolling average of the quantities
	vector<double> rollingM = expAvg(mzation,0.02);

	plotEnergy(rollingE,temp);			// Plot energy and magnetization!
	plotMagnetization(rollingM,temp);

	delete mag;	// Garbage collection (since object was created with "new")
	return 0;	// Return without error
}


/***************************************************************************
 *  @file       square_matrix_multiply.hpp
 *  @author     Yue Wang
 *  @date       29  Aug 2014
 *  @version    2
 *  @remark     CLRS Algorithms implementation in C++ templates.
 ***************************************************************************/

#ifndef SQUARE_MATRIX_MULTIPLY_H
#define SQUARE_MATRIX_MULTIPLY_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace clrs {namespace ch4 {

/**
 *  @brief  type aliasing
 */
template<typename T>
using Matrix = boost::numeric::ublas::matrix<T>;

/**
 * @brief square_matrix_multiply
 * @param lhs
 * @param rhs
 * @return product
 *
 * @pseudocode SQUARE-MATRIX-MULTIPLY,   Page 75
 * @complx  O(n^3)
 */
template<typename T>
Matrix<T> square_matrix_multiply(const Matrix<T>& lhs, const Matrix<T>& rhs)
{
    using namespace boost::numeric::ublas;
    using SizeType  = typename matrix<T>::size_type;

    SizeType size = lhs.size1();
    matrix<T> ret(size, size);
    for(SizeType i = 0; i != size; ++i)
        for(SizeType j = 0; j != size; ++j)
        {
            ret(i,j) = 0;
            for(SizeType k = 0; k != size; ++k)
                ret(i,j) += lhs(i,k) * rhs(k,j);
        }
    return  ret;
}

/**
 * @brief square_matrix_multiply
 * @param lhs
 * @param rhs
 * @return product
 *
 * @pseudocode SQUARE-MATRIX-MULTIPLY-RECURSIVE
 * @complx  O(n^3)
 */
template<typename T>
Matrix<T>
square_matrix_multiply_recursive(const Matrix<T>& lhs, const Matrix<T>& rhs)
{
    //! types def
    using ValueType = T;
    using Matrix    = boost::numeric::ublas::matrix<ValueType>;
    using SizeType  = typename Matrix::size_type;
    using Range     = boost::numeric::ublas::range;
    using namespace boost::numeric::ublas;

    SizeType size = lhs.size1();
    Matrix ret(size,size);

    //! the recurssion bottom
    if(size == 1)
        ret(0,0) = lhs(0,0) * rhs(0,0);
    else
    {
        //! ranges used for matrix partition
        Range r0(0,size/2), r1(size/2, size);

        //! lhs's submatrices
        Matrix lhs00(project(lhs,r0,r0));
        Matrix lhs01(project(lhs,r0,r1));
        Matrix lhs10(project(lhs,r1,r0));
        Matrix lhs11(project(lhs,r1,r1));

        //! rhs's submatrices
        Matrix rhs00(project(rhs,r0,r0));
        Matrix rhs01(project(rhs,r0,r1));
        Matrix rhs10(project(rhs,r1,r0));
        Matrix rhs11(project(rhs,r1,r1));

        //! recurssion
        //! @note must use project() on ret to "reference" it.Otherwise not working.
        project(ret,r0,r0)  =   square_matrix_multiply_recursive(lhs00,rhs00)
                              + square_matrix_multiply_recursive(lhs01,rhs10);

        project(ret,r0,r1)  =   square_matrix_multiply_recursive(lhs00,rhs01)
                              + square_matrix_multiply_recursive(lhs01,rhs11);

        project(ret,r1,r0)  =   square_matrix_multiply_recursive(lhs10,rhs00)
                              + square_matrix_multiply_recursive(lhs11,rhs10);

        project(ret,r1,r1)  =   square_matrix_multiply_recursive(lhs10,rhs01)
                              + square_matrix_multiply_recursive(lhs11,rhs11);
    }
    return ret;
}

/**
 * @brief square_matrix_multiply_strassen
 * @param lhs
 * @param rhs
 * @return product
 *
 * @complx  O(n^2.81)
 */
template<typename T>
Matrix<T>
square_matrix_multiply_strassen(const Matrix<T>& lhs, const Matrix<T>& rhs)
{
    //! types def
    using ValueType = T;
    using Matrix    = boost::numeric::ublas::matrix<ValueType>;
    using SizeType  = typename Matrix::size_type;
    using Range     = boost::numeric::ublas::range;
    using namespace boost::numeric::ublas;

    SizeType size = lhs.size1();
    Matrix  ret(size,size);

    if(size == 1)
        ret(0,0) = lhs(0,0) * rhs(0,0);
    else
    {
        //! ranges used for matrix partition
        Range r0(0,size/2), r1(size/2, size);

        //! step 1 : submatrices
        Matrix lhs00(project(lhs,r0,r0));
        Matrix lhs01(project(lhs,r0,r1));
        Matrix lhs10(project(lhs,r1,r0));
        Matrix lhs11(project(lhs,r1,r1));
        Matrix rhs00(project(rhs,r0,r0));
        Matrix rhs01(project(rhs,r0,r1));
        Matrix rhs10(project(rhs,r1,r0));
        Matrix rhs11(project(rhs,r1,r1));

        //! step 2
        Matrix s0 = rhs01 - rhs11;
        Matrix s1 = lhs00 + lhs01;
        Matrix s2 = lhs10 + lhs11;
        Matrix s3 = rhs10 - rhs00;
        Matrix s4 = lhs00 + lhs11;
        Matrix s5 = rhs00 + rhs11;
        Matrix s6 = lhs01 - lhs11;
        Matrix s7 = rhs10 + rhs11;
        Matrix s8 = lhs00 - lhs10;
        Matrix s9 = rhs00 + rhs01;

        //! step 3
        Matrix p0 = square_matrix_multiply_strassen(lhs00,  s0);
        Matrix p1 = square_matrix_multiply_strassen(s1, rhs11);
        Matrix p2 = square_matrix_multiply_strassen(s2, rhs00);
        Matrix p3 = square_matrix_multiply_strassen(lhs11,  s3);
        Matrix p4 = square_matrix_multiply_strassen(s4, s5);
        Matrix p5 = square_matrix_multiply_strassen(s6, s7);
        Matrix p6 = square_matrix_multiply_strassen(s8, s9);

        //! step 4 recurssion
        project(ret,r0,r0) = p4 + p3 - p1 + p5;
        project(ret,r0,r1) = p0 + p1;
        project(ret,r1,r0) = p2 + p3;
        project(ret,r1,r1) = p4 + p0 - p2 - p6;
    }
    return ret;
}

}}//namespace
#endif // SQUARE_MATRIX_MULTIPLY_H

//! @test  all three functions above
//!
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "square_matrix_multiply.hpp"

//int main ()
//{
//    using namespace boost::numeric::ublas;
//    matrix<int> lhs(2,2), rhs(2,2);

//    lhs(0,0) = 1;
//    lhs(0,1) = 3;
//    lhs(1,0) = 7;
//    lhs(1,1) = 5;

//    rhs(0,0) = 6;
//    rhs(0,1) = 8;
//    rhs(1,0) = 4;
//    rhs(1,1) = 2;

//    std::cout << clrs::ch4::square_matrix_multiply_recursive(lhs,rhs)  << std::endl;
//    std::cout << clrs::ch4::square_matrix_multiply(lhs,rhs)            << std::endl;
//    std::cout << clrs::ch4::square_matrix_multiply_strassen(lhs,rhs)   << std::endl;
//}
//! @output
//!
//[2,2]((18,14),(62,66))
//[2,2]((18,14),(62,66))
//[2,2]((18,14),(62,66))


#ifndef TIKPP_DETAIL_CONVERT_HPP
#define TIKPP_DETAIL_CONVERT_HPP

#include "fmt/format.h"
#include <boost/lexical_cast/try_lexical_convert.hpp>

#include <string>
#include <type_traits>

namespace tikpp::detail {

template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
auto integeral_pow(T base, T exp) -> T {
    T result {1};

    while (true) {
        if (exp & 1) {
            result *= base;
        }

        if (!(exp >>= 1)) {
            break;
        }

        base *= base;
    }

    return result;
}

template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
static inline auto rparse_uint(const std::string &str, std::size_t pos) noexcept
    -> T {
    T    ret {};
    char c;

    for (std::size_t i {pos}; i > 0; --i) {
        c = str[i - 1];

        if (!std::isdigit(c)) {
            break;
        }

        ret += integeral_pow<T>(10, pos - i) * (c - '0');
    }

    if constexpr (std::is_signed_v<T>) {
        if (c == '-') {
            ret = ~ret + 1;
        }
    }

    return ret;
}

template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
static inline auto parse_uint(const std::string &str,
                              std::size_t        pos = 0) noexcept -> T {
    T    ret {};
    char c;

    std::size_t idx {pos};

    while ((c = str[idx++])) {
        if (!std::isdigit(c)) {
            break;
        }

        ret = ret * 10 + (c - '0');
    }

    if constexpr (std::is_signed_v<T>) {
        if (str[pos] == '-') {
            ret = ~ret + 1;
        }
    }

    return ret;
}

template <typename T>
inline auto convert(const std::string &str) -> std::decay_t<T> {
    using type = std::decay_t<T>;

    if constexpr (std::is_constructible_v<type, decltype(str)>) {
        return T {str};
    }

    if constexpr (std::is_integral_v<type> && std::is_unsigned_v<type>) {
        return parse_uint<type>(str);
    }

    type ret {};

    if (!str.empty()) {
        boost::conversion::try_lexical_convert(str, ret);
    }

    return ret;
}

template <>
inline auto convert<std::string>(const std::string &str) -> std::string {
    return str;
}

template <>
inline auto convert<bool>(const std::string &str) -> bool {
    return str == "true" || str == "yes";
}

template <typename T>
inline auto convert_back(const T &value) -> std::string {
    using type = std::decay_t<T>;

    if constexpr (std::is_same_v<type, std::string>) {
        return value;
    } else if constexpr (std::is_constructible_v<std::string, type>) {
        return std::string {value};
    }

    return fmt::to_string(value);
}

} // namespace tikpp::detail

#endif


//standard include
#include <math.h>
#include <iostream>
#include <fstream>

//#include <Eigen/Geometry>
#include <cmath>

//opencv include
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"

#ifndef UTILITIES
#define UTILITIES

#include "objtype.hpp"
namespace zv_utils {

std::pair<float, float> minOfDepthMat(const cv::Mat& img, const cv::Mat& mask, const cv::Rect& bound_rect, int range);
float avgOfDepthMat(const cv::Mat& img, const cv::Mat& mask, const cv::Rect& bound_rect);
void shrinkRect(cv::Rect &rect_in, float shrink_factor);

//void printIsometry(const Eigen::Transform<double, 3, Eigen::Isometry> m);
double slope_list(const std::vector<double>& x, const std::vector<double>& y);
std::pair<double,double> slopeOfMasked(ObjectType ot, const cv::Mat &depth, const cv::Mat &mask, cv::Point2f fov);
double normalCFD(const std::pair<double, double> &meanAndStdev, double value);

}

#endif


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

void main() {
	MatrixXd m(2, 2);
	m(0, 0) = 3;
	m(1, 0) = 2.5; // row, col
	m(0, 1) = -1;
	m(1, 1) = m(1, 0) + m(0, 1);
	std::cout << "Example 01: " << std::endl;
	std::cout << m << std::endl;

	{
		std::cout << "Example 02: Size set at run time" << std::endl;
		MatrixXd m = MatrixXd::Random(3, 3);
		m = (m + MatrixXd::Constant(3, 3, 1.2)) * 50;
		cout << "m = " << endl << m << endl;
		VectorXd v(3);
		v << 1, 2, 3;
		cout << "m * v = " << endl << m * v << endl;
	}
	{
		std::cout << "Example 02: Size set at compile time" << std::endl;
		Matrix3d m = Matrix3d::Random();
		m = (m + Matrix3d::Constant(1.2)) * 50;
		cout << "m = " << endl << m << endl;
		Vector3d v(1, 2, 3);
		cout << "m * v = " << endl << m * v << endl;
	}
	system("pause");
}


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_vector.hpp>

#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

int main() {
    using namespace boost::numeric::ublas;
    matrix<int> m1 (5,5);
    matrix<int> m2 (5,5);
    matrix<int> m3 (5,5);
    vector<int> v (5);
    vector<int> v1 (5);
    int i;

    for(i=0;i<m1.size1();i++){
        m1 (i,0) = 0;
        m1 (i,1) = 0;
        m1 (i,2) = 3;
        m1 (i,3) = 6;
        m1 (i,4) = 5;
    }
    std::cout<<m1<<std::endl;

    identity_matrix<int> m_temp (5);
    std::cout<<m_temp<<std::endl;

    m2=m_temp+m1;
    std::cout<<m2<<std::endl;

    v (0) = 1;
    v (1) = 1;
    v (2) = 9;
    v (3) = 5;
    v (4) = 8;

    axpy_prod(m2,v,v1,true);
    std::cout << v1 <<std::endl;
    std::cout << inner_prod(v, trans(v)) << std::endl;
    std::cout << m1+m2 << std::endl;

/* KOD ZA INVERZ MATRICE NIJE MOJ ORIGINALAN RAD, IDEJA I DIJELOVI KODA PREUZETI SU SA STRANICA
    uBLAS REPOZITORIJA KOJI SLUŽBENO NIJE ODRŽAVAN OD STRANE ADMINISTRATORA
    ALGORITAM I RJEŠENJE U NJEMU REFERENCIRANI SU NA:
    Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery. */

    matrix<double> m2_copy (5, 5);
    m2_copy=m2;

    permutation_matrix<double> pm(m2_copy.size1());

    matrix <double> inverse(5,5);
    for (int i = 0; i < inverse.size1 (); ++ i)
        for (int j = 0; j < inverse.size2 (); ++ j)
            if(i==j) inverse(i,j) = 1;

    int res = lu_factorize(m2_copy, pm);
    lu_substitute(m2_copy, pm, inverse);

    std::cout << inverse << std::endl;

    return 0;

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
int spherical_side_formula(T const& lambda1, T const& delta1,
                           T const& lambda2, T const& delta2,
                           T const& lambda, T const& delta)
{
    // Create temporary points (vectors) on unit a sphere
    T const cos_delta1 = cos(delta1);
    T const c1x = cos_delta1 * cos(lambda1);
    T const c1y = cos_delta1 * sin(lambda1);
    T const c1z = sin(delta1);

    T const cos_delta2 = cos(delta2);
    T const c2x = cos_delta2 * cos(lambda2);
    T const c2y = cos_delta2 * sin(lambda2);
    T const c2z = sin(delta2);

    // (Third point is converted directly)
    T const cos_delta = cos(delta);

    // Apply the "Spherical Side Formula" as presented on my blog
    T const dist
        = (c1y * c2z - c1z * c2y) * cos_delta * cos(lambda)
        + (c1z * c2x - c1x * c2z) * cos_delta * sin(lambda)
        + (c1x * c2y - c1y * c2x) * sin(delta);

    T zero = T();
    return dist > zero ? 1
        : dist < zero ? -1
        : 0;
}

}
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class spherical_side_formula
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calculation_type;

        calculation_type const lambda1 = get_as_radian<0>(p1);
        calculation_type const delta1 = get_as_radian<1>(p1);
        calculation_type const lambda2 = get_as_radian<0>(p2);
        calculation_type const delta2 = get_as_radian<1>(p2);
        calculation_type const lambda = get_as_radian<0>(p);
        calculation_type const delta = get_as_radian<1>(p);

        return detail::spherical_side_formula(lambda1, delta1,
                                              lambda2, delta2,
                                              lambda, delta);
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

/*template <typename CalculationType>
struct default_strategy<spherical_polar_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};*/

template <typename CalculationType>
struct default_strategy<spherical_equatorial_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

template <typename CalculationType>
struct default_strategy<geographic_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP