diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d068b3..bd82858 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,11 +1,11 @@
 cmake_minimum_required(VERSION 2.8)
 project(VisualSLAM)
-set(CMAKE_CXX_FLAGS "-std=c++11 -g -O3 -march=native ")
+set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -march=native ") #-O3 -march=native
+list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
 # opencv
 find_package(OpenCV REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
-
 # pangolin
 find_package(Pangolin REQUIRED)
 include_directories(${Pangolin_INCLUDE_DIRS})
@@ -21,7 +21,8 @@ include_directories(${Sophus_INCLUDE_DIRS})
 find_package(Ceres REQUIRED)
 include_directories(${CERES_INCLUDE_DIRS})
 
+## project h file
 include_directories("./include")
-
-add_executable(slam testVisualSLAM.cpp src/VisualSLAM.cpp src/Map.cpp src/VisualOdometry.cpp src/BundleAdjuster.cpp)
-target_link_libraries(slam ${OpenCV_LIBS} ${CERES_LIBRARIES} ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES})
+##
+add_executable(testVSLAM testVSLAM.cpp src/VisualSLAM.cpp src/VisualOdometry.cpp src/Map.cpp src/BundleAdjuster.cpp)
+target_link_libraries(testVSLAM ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES} ${CERES_INCLUDE_DIRS})
diff --git a/README.md b/README.md
index 763bd2f..f7cc92a 100644
--- a/README.md
+++ b/README.md
@@ -1,27 +1,12 @@
-# VisualSLAM
-Visual-based Navigation project
-
-## Notes:
-1. Please create a feature branch for your work and use master branch only for working solutions. 
-2. **No build flies, images or executables are allowed** to be commited to the repository.
-3. CMake is used to build the project
-
-## How to build and run the code:
-1. Download and install VTK for opencv Viz module: https://www.vtk.org/download/
-2. If you have already installed OpenCV, you need to reinstall it such that OpenCV can find freshly-installed VTK library (credits to https://stackoverflow.com/questions/23559925/how-do-i-use-install-viz-in-opencv?utm_medium=organic&utm_source=google_rich_qa&utm_campaign=google_rich_qa)
-3.
-```bash
-mkdir build
-cmake ..
-make
-./slam ../data/left ../data/right 10
-
-## only 22 for visualization otherwise we can only see pose data
-./slam ../data/left/ ../data/right/ ../data/calib.txt 22
-```
-
-## something yinglong has changed:
-1. add bundle ajusterment of ceres version
-2. performFrontEndStep(), estimatePose3D2D(), so that we optimize the P3d[inlier_index] and P2d[inlier_index] instead of p3d[all] and p2d[all]
-3. please take care, I am using old version of Sophus library, I try to change all SE3-->SE3d and #inlclude<sophus/se3.h> -->#include <sophus/se3.hpp>
-   if you complie with the error of sophus, pleas change it as I said or contact me in messenger
+# VSLAM_project
+
+./testVSLAM ../data/left_image/ ../data/right_image/ 10 ../data/calib.txt 
+
+
+
+####
+./testVSLAM /media/fyl/冯颖龙/vision_based_navigation_linux_disk/kitti_data_set/dataset/sequences/00/image_0/ /media/fyl/冯颖龙/vision_based_navigation_linux_disk/kitti_data_set/dataset/sequences/00/image_1/  100 ../data/calib.txt ../data/00.txt 
+
+
+
+
diff --git a/ResultsPictures/withoutBA_9.png b/ResultsPictures/withoutBA_9.png
new file mode 100644
index 0000000..c157ac1
Binary files /dev/null and b/ResultsPictures/withoutBA_9.png differ
diff --git a/ResultsPictures/withoutBA_norm_3.png b/ResultsPictures/withoutBA_norm_3.png
new file mode 100644
index 0000000..7031d20
Binary files /dev/null and b/ResultsPictures/withoutBA_norm_3.png differ
diff --git a/include/BundleAdjuster.h b/include/BundleAdjuster.h
index 221dc1f..3d59f1c 100644
--- a/include/BundleAdjuster.h
+++ b/include/BundleAdjuster.h
@@ -1,82 +1,13 @@
-// * Class BundleAdjuster is responsible for 3D structure and camera pose optimizations
 
 
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <sophus/se3.hpp>
-using namespace Eigen;
 
-#include <opencv2/opencv.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/xfeatures2d.hpp>
-#include <opencv2/stereo.hpp>
-#include <opencv2/core/eigen.hpp>
 
-#include <string>
-#include <vector>
-using namespace std;
 
-#include <ceres/ceres.h>
-#include <ceres/rotation.h>
-#include <chrono>
-
-using ceres::AutoDiffCostFunction;
-using ceres::CostFunction;
-using ceres::Problem;
-using ceres::Solver;
-using ceres::Solve;
-
-//#include "VisualSLAM.h"
-
-
-
-struct ReprojectionError3D
-{
-        ReprojectionError3D(double observed_u, double observed_v)
-                :observed_u(observed_u), observed_v(observed_v)
-                {}
-
-        template <typename T>
-        bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
-        {
-                T p[3];
-                /***** code from hk technology */
-                ceres::QuaternionRotatePoint(camera_R, point, p);
-                p[0] += camera_T[0];
-                p[1] += camera_T[1];
-                p[2] += camera_T[2];
-
-                double fx=718.856;
-                double fy=718.856;
-                double cx=607.193;
-                double cy=185.216;
-                T xp = p[0]*fx*1./ p[2] +cx;
-                T yp = p[1]*fy*1./ p[2] +cy;
-                /////////////////////////////////////////////////////////////////////
-                residuals[0] = xp - T(observed_u);
-                residuals[1] = yp - T(observed_v);
-                return true;
-        }
-
-        static ceres::CostFunction* Create(const double observed_x,
-                                           const double observed_y)
-        {
-          return (new ceres::AutoDiffCostFunction<
-                  ReprojectionError3D, 2, 4, 3, 3>(
-                        new ReprojectionError3D(observed_x,observed_y)));
-        }
-
-        double observed_u;
-        double observed_v;
-};
-
-class BundleAdjuster {
+class BundleAdjuster{
+private:
 
 public:
-        BundleAdjuster();
-//        int get_camera_parameter();
-        Sophus::SE3d optimizeLocalPoseBA_ceres(std::vector<cv::Point3d> p3d,std::vector<cv::Point2d> p2d,Eigen::Matrix3d K,Sophus::SE3d pose,int iteration_times);
-        Sophus::SE3d optimizeLocalPoseBA_g2o(std::vector<cv::Point3d> p3d,std::vector<cv::Point2d> p2d,Eigen::Matrix3d K,Sophus::SE3d pose,int iteration_times);
+    BundleAdjuster() {};
 
-};
+    //TO DO Bundle Adjustment
+};
\ No newline at end of file
diff --git a/include/Map.h b/include/Map.h
index 1d87e03..2d58cce 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -1,14 +1,20 @@
-#include <Eigen/Core>
+#include <sophus/se3.hpp>
 #include <vector>
 
-/**
- * Class Map is responsible for storing the 3D structure
- */
-class Map {
+
+
+class Map{
 private:
-	std::vector<Eigen::Vector3d> structure3D;
+    int test_a;
+    std::vector<Sophus::SE3d> cumPose;
+    int poseIndex;
 
 public:
-	Map();
-	void updateStructure3d(); // update 3D point locations
-};
+    Map();
+    int getValue();
+
+    void updateCumPose(Sophus::SE3d newPose);
+    void updatePoseIndex();
+
+    std::vector<Sophus::SE3d> getCumPose();
+};
\ No newline at end of file
diff --git a/include/VisualOdometry.h b/include/VisualOdometry.h
index 1679cc1..5d72807 100644
--- a/include/VisualOdometry.h
+++ b/include/VisualOdometry.h
@@ -1,54 +1,44 @@
-#include <Eigen/Core>
-#include <Eigen/Dense>
-#include <sophus/se3.hpp>     //2
-#include <opencv2/ximgproc/disparity_filter.hpp>
-#include <pangolin/pangolin.h>
-
-#include <opencv2/opencv.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/xfeatures2d.hpp>
+#include <iostream>
+#include <vector>
+// opencv
+#include <opencv2/core/core.hpp>
+#include <opencv/cv.hpp>
 #include <opencv2/stereo.hpp>
+#include <opencv2/ximgproc/disparity_filter.hpp>
+//#include <opencv/cxeigen.hpp>
+#include <sophus/so3.h>
 #include <opencv2/core/eigen.hpp>
-
-#include <string>
-#include <vector>
-#include <unistd.h>
-
-struct KeyFrame {
-	cv::Mat image;
-    cv::Mat disparity_map;
-    std::vector<cv::KeyPoint> keypoints;
-	cv::Mat descriptor;
-};
-
-class VisualOdometry {
-	private:
-        KeyFrame refFrame;
-        Sophus::SE3d pose;
-
-	public:
-		VisualOdometry();
-        void setReferenceFrame(const cv::Mat image, const cv::Mat disparity, const std::vector<cv::KeyPoint> keypoints, const cv::Mat descriptor);
-        KeyFrame getReferenceFrame() const;
-
-        void setPose(const Sophus::SE3d pose);
-        Sophus::SE3d getPose() const;
-
-        cv::Mat getDisparityMap(const cv::Mat image_left, const cv::Mat image_right);
-        cv::Rect computeROIDisparityMap(cv::Size2i src_sz, cv::Ptr<cv::stereo::StereoBinarySGBM> matcher_instance);
-
-        void extractORBFeatures(cv::Mat frame_new, std::vector<cv::KeyPoint>& keypoints_new, cv::Mat& descriptors_new);
-        std::vector<cv::DMatch> findGoodORBFeatureMatches(std::vector<cv::KeyPoint> keypoints_new, cv::Mat descriptors_new);
-
-        void get3D2DCorrespondences(std::vector<cv::KeyPoint> keypoints_new, std::vector<cv::DMatch> matches, std::vector<cv::Point3d>& p3d, std::vector<cv::Point2d>& p2d,
-                                    cv::Mat disparity_map, Eigen::Matrix3d K);
-        void get2D2DCorrespondences(std::vector<cv::KeyPoint> keypoints_new, std::vector<cv::DMatch> matches, std::vector<cv::Point2d>& p2d_1, std::vector<cv::Point2d>& p2d_2);
-
-        std::vector<int> estimatePose3D2D(std::vector<cv::Point3d> p3d, std::vector<cv::Point2d> p2d, Eigen::Matrix3d K);
-        void estimatePose2D2D(std::vector<cv::Point2d> p2d_1, std::vector<cv::Point2d> p2d_2, Eigen::Matrix3d K);
-
-        void trackFeatures();
-        void computeAndShowPointCloud(const cv::Mat image_left, const cv::Mat disparity, const float baseline, Eigen::Matrix3d K);
-
-};
+// eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+//sophus
+#include <sophus/se3.hpp>
+
+class VisualOdometry{
+private:
+//    std::vector<unsigned char> status;
+    float baseline = 0.53716;
+    cv::Mat disparityMap;
+//    std::vector<Sophus::SE3d> historyPose;
+    bool reInitial;
+    int thresholdFeactures=100;
+public:
+    VisualOdometry() {};
+
+    //TO DO harrisDection
+    //TO DO featureTracking
+    std::vector<uchar> corr2DPointsFromPreFrame2DPoints(cv::Mat previousImage, cv::Mat currImage,
+                                                                std::vector<cv::Point2f>& previousFrame2DPoints,
+                                                                std::vector<cv::Point2f>& currFrame2DPoints);
+    //TO DO getDisparityMap
+    cv::Rect computeROIDisparityMap(cv::Size2i src_sz, cv::Ptr<cv::stereo::StereoBinarySGBM> matcher_instance);
+    void generateDisparityMap(const cv::Mat image_left, const cv::Mat image_right);
+    //TO DO getDepth  currFrame2DPoints
+    std::vector<cv::Point3f> getDepth3DPointsFromCurrImage(std::vector<cv::Point2f>& currFrame2DPoints,Eigen::Matrix3d K);
+
+    //TO DO poseEstimate2D3DPnp
+    Sophus::SE3d poseEstimate2D3DPNP(std::vector<cv::Point3f>& p3d, std::vector<cv::Point2f>& p2d,Eigen::Matrix3d K,Sophus::SE3d prePose);
+
+    //TO DO getReIntial
+    bool getReInital();
+};
\ No newline at end of file
diff --git a/include/VisualSLAM.h b/include/VisualSLAM.h
index ef26d38..063be2a 100644
--- a/include/VisualSLAM.h
+++ b/include/VisualSLAM.h
@@ -1,28 +1,47 @@
-#include "Map.h"
+#include <iostream>
+#include <fstream>
+#include <cmath>
+// project file
 #include "BundleAdjuster.h"
+#include "Map.h"
 #include "VisualOdometry.h"
+// eigen file
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+// opencv file
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include "opencv2/imgproc/imgproc.hpp"
+// Sophus
+#include <sophus/se3.hpp>
 
-#include <iostream>     // std::cout
-#include <fstream>      // std::ifstream
 
-class VisualSLAM {
+class VisualSLAM{
 private:
-//	Map map;
     BundleAdjuster BA;
+    Map map;
     VisualOdometry VO;
     Eigen::Matrix3d K;
+    std::vector<Sophus::SE3d> groundTruthData;
+
 
-    std::vector<Sophus::SE3d> historyPoses;
 public:
-	VisualSLAM();
-    Sophus::SE3d getPose(int index);
-    int getNumberPoses() const;
-    Eigen::Matrix3d getCameraMatrix() const;
-    double getFocalLength() const;
-
-	void readCameraIntrisics(std::string camera_intrinsics_file);
-	void performFrontEndStep(cv::Mat left_image, cv::Mat right_image); // feature detection / tracking and matching 
-	void runBackEndRoutine(); // optimization over parameters.numImagesBundleAdjustment images
-	void update(); // update map and poses
-    void visualizeAllPoses();
-};
+    VisualSLAM();
+    int getTestValueFromMap();
+    //TO DO get camera intrinsics
+    Eigen::Matrix3d getCameraIntrinsics(std::string camera_intrinsics_path);
+    Sophus::SE3d getPose(int k );
+
+
+    //TO DO estimate3D2DFrontEndWithOpicalFlow()
+    Sophus::SE3d estimate3D2DFrontEndWithOpicalFlow(cv::Mat leftImage_, cv::Mat rightImage, std::vector<cv::Point2f>
+            &previousFrame2DPoints, std::vector<cv::Point2f>&currFrame2DPoints,cv::Mat& previousImage,Sophus::SE3d prePose );
+
+    void readGroundTruthData(std::string fileName, int numberFrames, std::vector<Sophus::SE3d>& groundTruthData);
+
+    void plotTrajectoryNextStep(cv::Mat& window, int index, Eigen::Vector3d& translGTAccumulated, Eigen::Vector3d& translEstimAccumulated,
+                                Sophus::SE3d groundTruthPose, Sophus::SE3d groundTruthPrevPose, Eigen::Matrix3d& cumR, Sophus::SE3d pose,
+                                Sophus::SE3d prevPose = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0)));
+};
\ No newline at end of file
diff --git a/src/BundleAdjuster.cpp b/src/BundleAdjuster.cpp
index c0a0e89..60319c1 100644
--- a/src/BundleAdjuster.cpp
+++ b/src/BundleAdjuster.cpp
@@ -1,177 +1,2 @@
 #include "BundleAdjuster.h"
 
-typedef Eigen::Matrix<double, 6, 1> Vector6d;
-typedef Eigen::Matrix<double, 6, 6> Matrix66d;
-
-BundleAdjuster::BundleAdjuster() {}
-
-Sophus::SE3d BundleAdjuster::optimizeLocalPoseBA_ceres(std::vector<cv::Point3d> p3d,std::vector<cv::Point2d> p2d, Eigen::Matrix3d K,Sophus::SE3d pose,int numberIterations)
-{
-        assert(p3d.size() == p2d.size());
-
-        Sophus::SE3d T_esti(pose); // estimated pose
-        Eigen::Matrix3d R = T_esti.so3().matrix();
-        Eigen::Vector3d t = T_esti.translation();
-
-        Eigen::Quaterniond q_rotation(R) ;
-//        cout<<"Quaterniond: "<<endl<<q_rotation.coeffs()<<endl;
-
-        //local BA
-        ceres::Problem problem;
-        ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
-        //cout << " begin local BA " << endl;
-        // for (int i = 0; i < frame_num; i++)
-        // {
-        int i=0;
-            //double array for ceres
-            int position_num=p3d.size();
-            double rotation[1][4];
-            double translation[1][3];
-            double position[position_num][3];
-
-            translation[i][0] = t[0];
-            translation[i][1] = t[1];
-            translation[i][2] = t[2];
-            rotation[i][0] = q_rotation.w();
-            rotation[i][1] = q_rotation.x();
-            rotation[i][2] = q_rotation.y();
-            rotation[i][3] = q_rotation.z();
-            problem.AddParameterBlock(rotation[i], 4, local_parameterization);
-            problem.AddParameterBlock(translation[i], 3);
-            // if (i == l)
-            // {
-                // problem.SetParameterBlockConstant(rotation[i]);
-            // }
-            // if (i == l || i == frame_num - 1)
-            // {
-                // problem.SetParameterBlockConstant(translation[i]);
-            // }
-        // }
-            // add 3 D points
-            for (int i = 0; i < position_num; ++i)
-            {
-                position[i][0]=p3d[i].x;
-                position[i][1]=p3d[i].y;
-                position[i][2]=p3d[i].z;
-            }
-
-            for (int j = 0; j < p2d.size(); j++)
-            {
-                int l = 0;
-                ceres::CostFunction* cost_function = ReprojectionError3D::Create(
-                                                    p2d[j].x,
-                                                    p2d[j].y);
-
-                problem.AddResidualBlock(cost_function, NULL, rotation[l], translation[l],
-                                        position[j]);
-            }
-
-        // }
-        // Solver::Options options;
-        Eigen::Quaterniond quaterniond_0(rotation[0][0],rotation[0][1],rotation[0][2],rotation[0][3]);
-        Eigen::Vector3d translation_0(translation[0][0],translation[0][1],translation[0][2]);
-        Sophus::SE3d SE3_quaternion0(quaterniond_0,translation_0);
-//        cout<<SE3_quaternion0.matrix()<<endl;
-
-        ceres::Solver::Options options;
-        options.preconditioner_type = ceres::SCHUR_JACOBI;
-        options.linear_solver_type = ceres::DENSE_SCHUR;
-        // options.linear_solver_type = ceres::DENSE_SCHUR;
-        options.minimizer_progress_to_stdout = false;
-        // options.max_solver_time_in_seconds = 0.2;
-        ceres::Solver::Summary summary;
-        ceres::Solve(options, &problem, &summary);
-//        std::cout << summary.BriefReport() << "\n";
-      // std::cout << "Final rotation: " << rotation << " Final translation: " << translation << "\n";
-      // std::cout << "Final   m: " << m << " c: " << c << "\n";
-        Eigen::Quaterniond quaterniond_(rotation[0][0],rotation[0][1],rotation[0][2],rotation[0][3]);
-        Eigen::Vector3d translation_(translation[0][0],translation[0][1],translation[0][2]);
-        Sophus::SE3d SE3_quaternion(quaterniond_,translation_);
-//        cout<<SE3_quaternion.matrix()<<endl;
-        return SE3_quaternion;
-}
-
-//Sophus::SE3 BundleAdjuster::optimizeLocalPoseBA(std::vector<cv::Point3d> p3d,std::vector<cv::Point2d> p2d, Eigen::Matrix3d K,Sophus::SE3 pose,int numberIterations){
-//    assert(p3d.size() == p2d.size());
-//    double cost = 0, lastCost = 0;
-//    int nPoints = p3d.size();
-
-//    double fx = K(0,0);
-//    double fy = K(1,1);
-
-//    Sophus::SE3 T_esti(pose); // estimated pose
-
-//    for (int iter = 0; iter < numberIterations; iter++) {
-//        Matrix66d H = Matrix66d::Zero();
-//        Vector6d b = Vector6d::Zero();
-//        cost = 0;
-//        // compute cost
-//        for (int i = 0; i < nPoints; i++) {
-//            // compute cost for p3d[I] and p2d[I]
-//            Eigen::Vector3d point3DEigen(p3d[i].x, p3d[i].y, p3d[i].z);
-
-//            Eigen::Vector2d point2DEigen(p2d[i].x, p2d[i].y);
-
-//            Eigen::Vector3d pointTransformed = T_esti*point3DEigen;
-
-//            Eigen::Vector3d pointReprojected = K * pointTransformed;
-//            pointReprojected /= pointTransformed[2];
-
-//            Eigen::Vector2d e(0,0);    // error
-//            e[0]=point2DEigen[0] - pointReprojected[0];
-//            e[1]=point2DEigen[1] - pointReprojected[1];
-
-//            // compute jacobian
-//            Eigen::Matrix<double, 2, 6> J;
-
-//            double z = pointTransformed[2];
-//            double y = pointTransformed[1];
-//            double x = pointTransformed[0];
-
-//            J(0,0) = fx/z;
-//            J(0,1) = 0.0;
-//            J(0,2) = -(fx*x)/(z*z);
-//            J(0,3) = -(fx*x*y)/(z*z);
-//            J(0,4) = fx + ((fx*x*x)/(z*z));
-//            J(0,5) = -(fx*y)/z;
-//            J(1,0) = 0.0;
-//            J(1,1) = fy/z;
-//            J(1,2) = -(fy*y)/(z*z);
-//            J(1,3) = -fy - ((fy*y*y)/(z*z));
-//            J(1,4) = (fy*x*y)/(z*z);
-//            J(1,5) = (fy*x)/z;
-
-//            J=-J;
-
-//            H += J.transpose() * J;
-//            b += -J.transpose() * e;
-
-//            cost += 0.5 *(e[0]*e[0] + e[1]*e[1]);
-//            // cout<<"cost on line is : "<<cost<<endl;
-//        }
-
-//        // solve dx
-//        Vector6d dx = H.inverse()*b;    //the caculation of H*dx=b is right
-
-//        if (std::isnan(dx[0])) {
-//            std::cout << "result is nan!" << std::endl;
-//            break;
-//        }
-
-//        if (iter > 0 && cost >= lastCost) {
-//            // cost increase, update is not good
-//            std::cout << "cost: " << cost << ", last cost: " << lastCost << std::endl;
-//            break;
-//        }
-
-//        // update your estimation
-//        T_esti = Sophus::SE3::exp(dx)*T_esti;
-
-//        lastCost = cost;
-
-//        std::cout << "iteration " << iter << " cost=" << std::cout.precision(12) << cost << std::endl;
-//        //std::cout << "estimated pose: \n" << T_esti.matrix() << std::endl;
-
-//    }
-//    return T_esti;
-//}
diff --git a/src/Map.cpp b/src/Map.cpp
index bfa7253..9703074 100644
--- a/src/Map.cpp
+++ b/src/Map.cpp
@@ -1,7 +1,31 @@
 #include "Map.h"
 
-Map::Map() {}
 
-void Map::updateStructure3d() {
-	// TODO
+
+Map::Map() {
+    test_a =666;
+    poseIndex = 0 ;
+
+}
+
+int Map::getValue() {
+    return test_a;
+}
+
+void Map::updateCumPose(Sophus::SE3d newPose) {
+
+    if (cumPose.empty()){
+        cumPose.push_back(newPose);
+    }
+    assert(poseIndex == cumPose.size());
+    cumPose.push_back(newPose.inverse() * cumPose[poseIndex - 1]);
+}
+
+void Map::updatePoseIndex() {
+
+    poseIndex = poseIndex + 1 ;
+}
+
+std::vector<Sophus::SE3d> Map::getCumPose() {
+    return cumPose;
 }
\ No newline at end of file
diff --git a/src/VisualOdometry.cpp b/src/VisualOdometry.cpp
index 2f5a213..29ed3a7 100644
--- a/src/VisualOdometry.cpp
+++ b/src/VisualOdometry.cpp
@@ -1,33 +1,43 @@
 #include "VisualOdometry.h"
-#include "BundleAdjuster.h"
 
-//using namespace cv::xfeatures2d;
-//using namespace cv;
-
-VisualOdometry::VisualOdometry(){}
-
-void VisualOdometry::setReferenceFrame(const cv::Mat image, const cv::Mat disparity, const std::vector<cv::KeyPoint> keypoints, const cv::Mat descriptor){
-    image.copyTo(refFrame.image);
-    disparity.copyTo(refFrame.disparity_map);
-    refFrame.keypoints = keypoints;
-    descriptor.copyTo(refFrame.descriptor);
-}
+//TO DO harrisDection
+//TO DO featureTracking
+std::vector<uchar> VisualOdometry::corr2DPointsFromPreFrame2DPoints(cv::Mat previousImage, cv::Mat currImage,
+                                                                            std::vector<cv::Point2f> &previousFrame2DPoints_,
+                                                                            std::vector<cv::Point2f> &currFrame2DPoints) {
+    // Parameters for lucas kanade optical flow
+//    std::vector<cv::Point2f> currFrame2DPoints= previousFrame2DPoints_;
+    std::vector<uchar> status;
+    std::vector<float> err;
+    cv::Size winSize = cv::Size(31, 31);
+    int maxLevel = 3;
+    cv::TermCriteria termcrit(cv::TermCriteria::COUNT|cv::TermCriteria::EPS,20,0.03);
+
+    cv::calcOpticalFlowPyrLK(previousImage, currImage, previousFrame2DPoints_,currFrame2DPoints,status,err,winSize,maxLevel,termcrit,0.001);
+    std::cout<< "currFrame2DPoints size : "<< currFrame2DPoints.size()<<std::endl;
+    // trackedCurrFrame2DPoints
+
+//    std::cout<<" trackedCurrFrame2DPoints size "<<trackedCurrFrame2DPoints.size() << std::endl;
+    int numTrackedFeactures(0);
+    for (int i = 0; i < status.size() ; ++i) {
+        if (status[i] == 1){
+            numTrackedFeactures ++;
+        }
+    }
+    if (numTrackedFeactures < thresholdFeactures){
+        reInitial = true ;
+    }
 
-KeyFrame VisualOdometry::getReferenceFrame() const {
-    return refFrame;
-}
 
-void VisualOdometry::setPose(const Sophus::SE3d pose){
-    this->pose = pose;
+    return status;
 }
 
-Sophus::SE3d VisualOdometry::getPose() const{
-    return pose;
+bool VisualOdometry::getReInital() {
+    return reInitial ;
 }
 
-
-cv::Rect VisualOdometry::computeROIDisparityMap(cv::Size2i src_sz, cv::Ptr<cv::stereo::StereoBinarySGBM> matcher_instance)
-{
+cv::Rect VisualOdometry::computeROIDisparityMap(cv::Size2i src_sz,
+                                                cv::Ptr<cv::stereo::StereoBinarySGBM> matcher_instance) {
     int min_disparity = matcher_instance->getMinDisparity();
     int num_disparities = matcher_instance->getNumDisparities();
     int block_size = matcher_instance->getBlockSize();
@@ -42,19 +52,22 @@ cv::Rect VisualOdometry::computeROIDisparityMap(cv::Size2i src_sz, cv::Ptr<cv::s
 
     cv::Rect r(xmin, ymin, xmax - xmin, ymax - ymin);
     return r;
+
 }
 
-cv::Mat VisualOdometry::getDisparityMap(const cv::Mat image_left, const cv::Mat image_right){
-    cv::Mat disparity, true_dmap, disparity_norm;
-    cv::Rect ROI;
+void VisualOdometry::generateDisparityMap(const cv::Mat image_left, const cv::Mat image_right) {
+    cv::Mat disparity , true_dmap, disparity_norm ;
+    cv::Rect ROI ;
     int min_disparity = 0;
-    int number_of_disparities = 16*6 - min_disparity;
-    int kernel_size = 7;
+    int number_of_disparities = 16*6 -min_disparity;
+    int kernel_size =7 ;
 
     cv::Ptr<cv::stereo::StereoBinarySGBM> sgbm = cv::stereo::StereoBinarySGBM::create(min_disparity, number_of_disparities, kernel_size);
     // setting the penalties for sgbm
-    sgbm->setP1(8*std::pow(kernel_size, 2));
-    sgbm->setP2(32*std::pow(kernel_size, 2));
+
+    sgbm->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
+    sgbm->setP1(8*std::pow(kernel_size,2));
+    sgbm->setP2(32*std::pow(kernel_size,2));
     sgbm->setMinDisparity(min_disparity);
     sgbm->setUniquenessRatio(3);
     sgbm->setSpeckleWindowSize(200);
@@ -62,12 +75,11 @@ cv::Mat VisualOdometry::getDisparityMap(const cv::Mat image_left, const cv::Mat
     sgbm->setDisp12MaxDiff(1);
     sgbm->setSpekleRemovalTechnique(cv::stereo::CV_SPECKLE_REMOVAL_AVG_ALGORITHM);
     sgbm->setSubPixelInterpolationMethod(cv::stereo::CV_SIMETRICV_INTERPOLATION);
-
     // setting the penalties for sgbm
     ROI = computeROIDisparityMap(image_left.size(),sgbm);
     cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter;
     wls_filter = cv::ximgproc::createDisparityWLSFilterGeneric(false);
-    wls_filter->setDepthDiscontinuityRadius(1);
+    wls_filter->setDepthDiscontinuityRadius(2);
 
     sgbm->compute(image_left, image_right, disparity);
     wls_filter->setLambda(8000.0);
@@ -81,193 +93,114 @@ cv::Mat VisualOdometry::getDisparityMap(const cv::Mat image_left, const cv::Mat
     cv::imshow("filtered disparity", filtered_disp_vis);
     cv::waitKey();
     */
-    filtered_disp_vis.convertTo(true_dmap, CV_32F, 1.0/16.0, 0.0);
-    return true_dmap;
+    filtered_disp_vis.convertTo(true_dmap, CV_32F, 1.0, 0.0);
+    disparityMap = true_dmap ;
 }
-
-void VisualOdometry::extractORBFeatures(cv::Mat frame_new, std::vector<cv::KeyPoint>& keypoints_new, cv::Mat& descriptors_new){
-    int max_features = 500;
-	// Detect ORB features and compute descriptors.
-    cv::Ptr<cv::Feature2D> orb = cv::ORB::create(max_features);
-    orb->detectAndCompute(frame_new, cv::Mat(), keypoints_new, descriptors_new);
-}
-
-std::vector<cv::DMatch> VisualOdometry::findGoodORBFeatureMatches(std::vector<cv::KeyPoint> keypoints_new, cv::Mat descriptors_new){
-    const float good_match_ratio = 0.8;
-    std::vector<cv::DMatch> matches;
-
-    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create("BruteForce-Hamming(2)");
-    matcher->match(refFrame.descriptor, descriptors_new, matches);
-
-    // Sort matches by score
-    std::sort(matches.begin(), matches.end());
-    // Remove not so good matches
-    const int numGoodMatches = matches.size() * good_match_ratio;
-    matches.erase(matches.begin()+numGoodMatches, matches.end());
-
-    return matches;
-}
-
-void VisualOdometry::get3D2DCorrespondences(std::vector<cv::KeyPoint> keypoints_new, std::vector<cv::DMatch> matches,
-                                            std::vector<cv::Point3d>& p3d, std::vector<cv::Point2d>& p2d,
-                                            cv::Mat disparity_map, Eigen::Matrix3d K){
-    if (matches.empty()){
-        throw std::runtime_error("get3d2dCorrespondences() : Input vector with keypoint matching is empty");
-    }
-
-    double b = 0.53716;
-    double fx = K(0,0);
-    double fy = K(1,1);
-    double cx = K(0,2);
-    double cy = K(1,2);
-
-    double f = (fx + fy) / 2;
-    // prepare data
-    for (auto &m: matches) {
-        cv::Point2d p1 = refFrame.keypoints[m.queryIdx].pt;
-        cv::Point2d p2 = keypoints_new[m.trainIdx].pt;
-        float disparity = disparity_map.at<float>(p2.y, p2.x);
-        if (disparity){
-           double z = f*b/disparity;
-           double x = z*(p2.x - cx) / fx;
-           double y = z*(p2.y - cy) / fy;
-           //std::cout << x << " " << y << " " << z << std::endl;
-           cv::Point3d p2_3d(x, y, z);
-           p3d.push_back(p2_3d);
-           p2d.push_back(p1);
+//TO DO get3DPoints
+std::vector<cv::Point3f> VisualOdometry::getDepth3DPointsFromCurrImage(std::vector<cv::Point2f> &currFrame2DPoints,
+                                                                        Eigen::Matrix3d K) {
+    float fx = K(0,0);
+    float fy = K(1,1);
+    float cx = K(0,2);
+    float cy = K(1,2);
+//    std::cout<<"fx is : "<<fx<<" fy is : "<<fy<<" cx is : "<<cx<<" cy is : "<<cy<<std::endl;
+    cv::Mat mask =disparityMap >0 ;
+    double minValue;
+    cv::minMaxLoc(disparityMap,&minValue,NULL,NULL,NULL,mask);
+    std::vector<cv::Point3f> Points;
+    for (int i = 0; i < currFrame2DPoints.size() ; ++i) {
+
+        cv::Point2f point2D = currFrame2DPoints[i];
+        float disparity = disparityMap.at<float>(point2D.y, point2D.x);
+        if (disparity < 0.1)
+        {
+            float neighborsDisparity[4];
+            if (point2D.x - 1 > 0){
+                neighborsDisparity[0] = disparityMap.at<float>(point2D.y , point2D.x - 1);
+            }
+            if (point2D.x + 1 < disparityMap.cols ) {
+                neighborsDisparity[1] = disparityMap.at<float>(point2D.y , point2D.x + 1);
+            }
+            if (point2D.y -1 > 0){
+                neighborsDisparity[2] = disparityMap.at<float>(point2D.y-1 , point2D.x );
+
+            }
+            if (point2D.y + 1 < disparityMap.rows) {
+                neighborsDisparity[3] = disparityMap.at<float>(point2D.y+1 , point2D.x );
+            }
+            disparity = (neighborsDisparity[0] + neighborsDisparity[1] + neighborsDisparity[2] + neighborsDisparity[3] +neighborsDisparity[4])/4;
         }
-    }
-
-    //computeAndShowPointCloud(refFrame.image, refFrame.disparity_map, b, K);
-
-}
+        if (disparity < 0.1 ){
+            disparity = minValue;
+        }
+        float x,y,z;
+        z = fx * baseline / disparity;
+        x = ( point2D.x -cx ) * z /fx;
+        y = ( point2D.y -cy ) * z /fy;
 
-void VisualOdometry::get2D2DCorrespondences(std::vector<cv::KeyPoint> keypoints_new, std::vector<cv::DMatch> matches, std::vector<cv::Point2d>& p2d_1, std::vector<cv::Point2d>& p2d_2){
-    if (matches.empty()){
-        throw std::runtime_error("get2d2dCorrespondences() : Input vector with keypoint matching is empty");
+        cv::Point3f Point3D(x , y , z);
+        Points.push_back(Point3D);
     }
 
-    for (auto &m: matches) {
-        cv::Point2f p1 = refFrame.keypoints[m.queryIdx].pt;
-        cv::Point2f p2 = keypoints_new[m.trainIdx].pt;
 
-        p2d_1.push_back(p1);
-        p2d_2.push_back(p2);
-    }
+    return Points;
 }
 
-std::vector<int> VisualOdometry::estimatePose3D2D(std::vector<cv::Point3d> p3d, std::vector<cv::Point2d> p2d, Eigen::Matrix3d K){
-    cv::Mat cameraMatrix;
-    cv::Mat distCoeffs = cv::Mat::zeros(4,1,CV_64F);
-    cv::Mat rvec,tvec,rot_matrix,inliers;
-    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
-    Eigen::Vector3d t(0,0,0);
-
-//    std::vector<int> inliers_index;  // changed by feng to  Mat inliers
-
-    cv::eigen2cv(K, cameraMatrix);
-    std::cout<<"p3d.size()"<<p3d.size()<<std::endl;
-    bool result=cv::solvePnPRansac(p3d,p2d,cameraMatrix, distCoeffs,rvec,tvec, false, 100, 5.0, 0.99, inliers);
 
+//TO DO poseEstimate2D3DPnp
+Sophus::SE3d VisualOdometry::poseEstimate2D3DPNP(std::vector<cv::Point3f> &p3d, std::vector<cv::Point2f> &p2d,Eigen::Matrix3d K,Sophus::SE3d prePose ) {
+
+    Eigen::Matrix3d R21;
+    Eigen::Vector3d t21;
+    cv::Mat K_cv;
+    cv::eigen2cv(K,K_cv);
+//    cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); // Assuming no lens distortion
+    cv::Mat dist_coeffs = cv::Mat::zeros(4,1,CV_64F);
+    cv::Mat rotationVector;
+    cv::Mat translationVector;
+    cv::Mat R;
+    std::vector<int> inliers;
+    bool result = cv::solvePnPRansac(p3d,p2d,K_cv,dist_coeffs,rotationVector,translationVector, false,100,4.0,0.99,inliers);
     if (result){
-        cv::Rodrigues(rvec, rot_matrix);
-        cv::cv2eigen(rot_matrix, R);
-        cv::cv2eigen(tvec,t);
+        cv::Rodrigues(rotationVector,R);
+        cv::cv2eigen(R,R21);
+        cv::cv2eigen(translationVector,t21);
     }
 
-    pose = Sophus::SE3d(R, t);
-    std::cout << "3D-2D Pnp solved Pose: "<<std::endl<< pose.matrix() << std::endl;
-
-    /* optimization p3d[inliers_index] and p2d[inliers_index] and pose */
-    std::vector<int> inliers_index;
-    for ( int i=0; i<inliers.rows; i++ )
-       {
-          inliers_index.push_back(inliers.at<int>(i,0));
-        }
-    return inliers_index;
-
-}
-
-void VisualOdometry::estimatePose2D2D(std::vector<cv::Point2d> p2d_1, std::vector<cv::Point2d> p2d_2, Eigen::Matrix3d K){
-    cv::Mat tvec,rot_matrix;
-    cv::Mat cameraMatrix;
-    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
-    Eigen::Vector3d t;
-
-    cv::eigen2cv(K, cameraMatrix);
-
-    double focal = (K(0,0) + K(1,1)) / 2;
-    cv::Point2d princip_point = cv::Point2d(K(0,2), K(1,2));
-
-    cv::Mat E = cv::findEssentialMat(p2d_1, p2d_2, focal, princip_point);
-    cv::recoverPose(E, p2d_1, p2d_2, cameraMatrix, rot_matrix, tvec);
+    Sophus::SE3d posePnp(R21,t21);
+//    std::cout<<"pose inverse: "<<std::endl<<posePnp.inverse().matrix()<<std::endl;
+//    historyPose.push_back(posePnp.inverse());
 
-    cv::cv2eigen(rot_matrix, R);
-    cv::cv2eigen(tvec,t);
-
-    pose = Sophus::SE3d(R, t);
+    std::cout<<"pose norm: "<<std::endl<<posePnp.log().norm() <<std::endl;
+    int MAXPoseNorm = 1;
+    if (posePnp.log().norm() > MAXPoseNorm){
+        posePnp = prePose;
+    }
 
-    std::cout << pose.matrix() << std::endl;
-}
+    return posePnp;
 
-void VisualOdometry::trackFeatures(){
-	// TODO
 }
 
-void VisualOdometry::computeAndShowPointCloud(const cv::Mat image_left, const cv::Mat disparity, const float baseline, Eigen::Matrix3d K) {
-    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> pointcloud;
-    double fx = K(0,0);
-    double fy = K(1,1);
-    double cx = K(0,2);
-    double cy = K(1,2);
 
-    // TODO Compute point cloud using disparity
-    // NOTE if your computer is slow, change v++ and u++ to v++2 and u+=2 to generate a sparser point cloud
-    for (int v = 0; v < image_left.rows; v++)
-        for (int u = 0; u < image_left.cols; u++) {
-            /// start your code here (~6 lines)
-
-            double z = fx*baseline/(disparity.at<float>(v,u));
-            double x = (u - cx)*z / fx;
-            double y = (v - cy)*z / fy;
-
-            Eigen::Vector4d point(x, y, z,
-                           image_left.at<uchar>(v, u) / 255.0); // first three components are XYZ and the last is color
-            pointcloud.push_back(point);
-            /// end your code here
-        }
-
-    // draw the point cloud
-
-    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
-    glEnable(GL_DEPTH_TEST);
-    glEnable(GL_BLEND);
-    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-    pangolin::OpenGlRenderState s_cam(
-            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
-            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
-    );
-
-    pangolin::View &d_cam = pangolin::CreateDisplay()
-            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
-            .SetHandler(new pangolin::Handler3D(s_cam));
-
-    while (pangolin::ShouldQuit() == false) {
-        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-        d_cam.Activate(s_cam);
-        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
-
-        glPointSize(2);
-        glBegin(GL_POINTS);
-        for (auto &p: pointcloud) {
-            glColor3f(p[3], p[3], p[3]);
-            glVertex3d(p[0], p[1], p[2]);
-        }
-        glEnd();
-        pangolin::FinishFrame();
-        usleep(5000);   // sleep 5 ms
-    }
-    return;
-}
+//// visualization
+//void VisualOdometry::plotTrajectoryNextStep(cv::Mat& window, int index, Eigen::Vector3d& translGTAccumulated, Eigen::Vector3d& translEstimAccumulated,
+//                                            Sophus::SE3d groundTruthPose, Sophus::SE3d groundTruthPrevPose, Eigen::Matrix3d& cumR, Sophus::SE3d estimPose,  Sophus::SE3d estimPrevPose){
+//    int offsetX = 300;
+//    int offsetY = 300;
+//
+//    Sophus::SE3d pose = estimPose.inverse();
+//    Sophus::SE3d prevPose = estimPrevPose.inverse();
+//
+//    if (index == 0){
+//        translGTAccumulated = groundTruthPose.translation();
+//        translEstimAccumulated = pose.translation();
+//    } else {
+//        translGTAccumulated = translGTAccumulated + (groundTruthPose.so3().inverse()*groundTruthPrevPose.so3())*(groundTruthPose.translation() - groundTruthPrevPose.translation());
+//        translEstimAccumulated = translGTAccumulated + (pose.so3().inverse()*groundTruthPrevPose.so3())*(pose.translation() - prevPose.translation());
+//    }
+//    cv::circle(window, cv::Point2d(offsetX + translGTAccumulated[0], offsetY + translGTAccumulated[2]), 3, cv::Scalar(0,0,255), -1);
+//    cv::circle(window, cv::Point2f(offsetX + translEstimAccumulated[0], offsetY + translEstimAccumulated[2]), 3, cv::Scalar(0,255,0), -1);
+//    cv::imshow("Trajectory", window);
+//    cv::waitKey(3);
+//    cumR = cumR*pose.so3().matrix();
+//}
\ No newline at end of file
diff --git a/src/VisualSLAM.cpp b/src/VisualSLAM.cpp
index 97b7407..72fd9bf 100644
--- a/src/VisualSLAM.cpp
+++ b/src/VisualSLAM.cpp
@@ -1,231 +1,235 @@
 #include "VisualSLAM.h"
 
-VisualSLAM::VisualSLAM() {
-    K = Eigen::Matrix3d::Identity();
-}
-
-Sophus::SE3d VisualSLAM::getPose(int index) {
-    if (index < 0 || index >= historyPoses.size()){
-        throw std::runtime_error("VisualSLAM::getPose() : Index out of bounds");
-    }
-
-    return historyPoses.at(index);
-}
+VisualSLAM::VisualSLAM() {}
 
-int VisualSLAM::getNumberPoses() const{
-    return historyPoses.size();
+int VisualSLAM::getTestValueFromMap() {
+    return map.getValue();
 }
 
-Eigen::Matrix3d VisualSLAM::getCameraMatrix() const {
+//TO DO get camera intrinsics
+Eigen::Matrix3d VisualSLAM::getCameraIntrinsics(std::string camera_intrinsics_path) {
+//    std::cout<<"camera_intrinsics_path is: "<<camera_intrinsics_path<<std::endl;
+    std::ifstream file(camera_intrinsics_path);
+    std::string number;
+    double fx,K01,cx,K10,K11,fy,cy,K21,K22;
+
+    file >>number>>fx>>K01>>cx>>K10>>K11>>fy>>cy>>K21>>K22;
+    K << fx , 0 , cx ,
+         0  , fy ,cy ,
+         0 ,  0 , 1 ;
+//    std::cout<<"camera intrinsics: "<< K << std::endl;
     return K;
+    
 }
 
-double VisualSLAM::getFocalLength() const {
-    return (K(0,0) + K(1,1)) / 2.0;
-}
-
-void VisualSLAM::readCameraIntrisics(std::string camera_file_path){
-	std::ifstream file;
-	file.open(camera_file_path, std::ifstream::in);
-
-	if (!file){
-		throw std::runtime_error("Cannot read the file with camera intrinsics");
-	}
+void VisualSLAM::readGroundTruthData(std::string fileName, int numberFrames, std::vector<Sophus::SE3d>& groundTruthData){
+    std::ifstream inFile;
+    inFile.open(fileName, std::ifstream::in);
 
-	std::string prefix;
-	double data[12];
-
-	file >> prefix;
-	for (int i = 0; i < 12; i++){
-		file >> data[i];
-	}
-
-	// fx, fy, cx, cy
-    K(0,0) = data[0];
-    K(1,1) = data[5];
-    K(0,2) = data[2];
-    K(1,2) = data[6];
+    if (!inFile){
+        throw std::runtime_error("readGroundTruthData() : Cannot read the file with ground truth data");
+    }
+    if (numberFrames <= 0){
+        throw std::runtime_error("readGroundTruthData() : Number of frames is non-positive!");
+    }
 
+    groundTruthData.clear();
+
+    int i = 0;
+    while(i < numberFrames && !inFile.eof()){
+        double rotationElements[9], translationElements[3];
+        int k = 0;
+        for (int j = 1; j <= 12; j++){
+            if (j % 4 == 0){
+                inFile >> translationElements[j / 4 - 1];
+            } else {
+                inFile >> rotationElements[k++];
+            }
+        }
+        cv::Mat R_CV = cv::Mat(3,3, CV_64F, rotationElements);
+        Eigen::Matrix3d R_Eigen;
+        cv::cv2eigen(R_CV, R_Eigen);
+        Sophus::SE3d newPose = Sophus::SE3d(Eigen::Quaterniond(R_Eigen), Eigen::Vector3d(translationElements));
+        groundTruthData.push_back(newPose);
+        i++;
+    }
 }
 
-void VisualSLAM::performFrontEndStep(cv::Mat image_left, cv::Mat image_right){
-    std::vector<cv::KeyPoint> keypoints_new;
-	cv::Mat descriptors_new;
 
-    VO.extractORBFeatures(image_left, keypoints_new, descriptors_new);
-    KeyFrame refFrame = VO.getReferenceFrame();
-    cv::Mat disparity_map = VO.getDisparityMap(image_left, image_right);
+//TO DO poseEstimate3D2DFrontEndWithOpicalFlow()  return pose
+    //TO DO harrisDection
+    //TO DO featureTracking
+    //TO DO poseEstimate2D3DPnp
+    //TO DO reInitialize
+Sophus::SE3d VisualSLAM::estimate3D2DFrontEndWithOpicalFlow(cv::Mat leftImage_, cv::Mat rightImage,
+                                                           std::vector<cv::Point2f> &previousFrame2DPoints,
+                                                           std::vector<cv::Point2f> &currFrame2DPoints,
+                                                           cv::Mat& previousImage, Sophus::SE3d prePose ) {
+    cv::Mat leftImage = leftImage_;
+    Sophus::SE3d pose;
+    int maxCorners=500;
+    cv::Size subPixel(10,10);
+    cv::TermCriteria termcrit(cv::TermCriteria::COUNT|cv::TermCriteria::EPS,20,0.03);
+
+    if( previousFrame2DPoints.empty()){
+        std::cout<<"The first image "<<std::endl;
+        //TO DO Harris Detection
+
+        cv::goodFeaturesToTrack(leftImage,currFrame2DPoints,maxCorners,0.01,10,cv::Mat(),3,3,false,0.04);
+        cv::cornerSubPix(leftImage,currFrame2DPoints,subPixel,cv::Size(-1,-1),termcrit);
+        std::cout << "step1 check feature size " << currFrame2DPoints.size() << std::endl;
+
+
+        /*
+        // test corners detection results
+        std::cout<<"** Number of corners detected: "<<currFrame2DPoints.size()<<std::endl;
+        for (size_t i=0; i<currFrame2DPoints.size(); i++)
+        cv::circle(leftImage, currFrame2DPoints[i], 4, cv::Scalar(200,0,0));
+        cv::namedWindow("leftImage",CV_WINDOW_AUTOSIZE);
+        cv::imshow("leftImage",leftImage);
+        cv::waitKey();
+         */
+        //TO DO getDisparityMapFromCurrImage
+        // Define the first image as the world coordinate
+        std::vector<cv::Point3f> p3d;
+        VO.generateDisparityMap(leftImage,rightImage);
+        p3d = VO.getDepth3DPointsFromCurrImage(currFrame2DPoints,K);
+        int maxDistance = 150 ;
+        for (int i = 0; i <p3d.size() ; ++i) {
+            if (p3d[i].z > maxDistance ){
+//                std::cout<<"depth "<<p3d[i]<<std::endl;
+                p3d.erase(p3d.begin()+i);
+                currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+            }
+            else if (std::isnan(p3d[i].z)){
+                p3d.erase(p3d.begin()+i);
+                currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+            }
+        }
+        std::cout<<"p3d size "<<p3d.size()<<"  currFrame2DPoints size"<<currFrame2DPoints.size()<<std::endl;
+        std::cout << "step 2 check feature size after erase " << currFrame2DPoints.size() << std::endl;
 
-    if (refFrame.keypoints.empty()){
-        VO.setReferenceFrame(image_left, disparity_map, keypoints_new, descriptors_new);
-		return;
-	}
+        map.updatePoseIndex();
+        map.updateCumPose(pose);
 
-	std::vector<cv::DMatch> matches = VO.findGoodORBFeatureMatches(keypoints_new, descriptors_new);
+        leftImage.copyTo(previousImage);
+        previousFrame2DPoints.clear();
+        previousFrame2DPoints=currFrame2DPoints;
+//        currFrame2DPoints.clear();
+        return pose;
+    }
 
-	// Draw top matches
+    //TO DO featureTracking
+    std::vector<uchar> status;
+    status = VO.corr2DPointsFromPreFrame2DPoints(previousImage,leftImage,previousFrame2DPoints,currFrame2DPoints);
+    std::vector<cv::Point2f> trackedCurrFrame2DPoints , trackedPreviousFrame2DPoints;
+    for (int i = 0; i <status.size() ; ++i) {
+        if ( status[i] == 1) {
+            //TO DO delete 3D points in the previousFrame
+            trackedPreviousFrame2DPoints.push_back(previousFrame2DPoints[i]);
+            trackedCurrFrame2DPoints.push_back(currFrame2DPoints[i]);
 
+        }
+    }
 
-    cv::Mat imMatches;
-    cv::drawMatches(refFrame.image, refFrame.keypoints, image_left, keypoints_new, matches, imMatches);
-    cv::imshow("Matches", imMatches);
-    cv::waitKey(0);
+    //TO DO getDisparityMapFromPreviousImage
+    //TO DO getDepth3DPointsFromPreviousImage
+    std::vector<cv::Point3f> p3DCurrFrame;
+    VO.generateDisparityMap(leftImage,rightImage);
+    p3DCurrFrame = VO.getDepth3DPointsFromCurrImage(trackedCurrFrame2DPoints,K);
+//    int maxDistance = 150 ;
+//    for (int i = 0; i <p3DCurrFrame.size() ; ++i) {
+//        if (p3DCurrFrame[i].z > maxDistance ){
+////                std::cout<<"depth "<<p3d[i]<<std::endl;
+//            p3DCurrFrame.erase(p3DCurrFrame.begin()+i);
+//            currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+//            trackedPreviousFrame2DPoints.erase(trackedPreviousFrame2DPoints.begin()+i);
+//        }
+//        else if (std::isnan(p3DCurrFrame[i].z)){
+//            p3DCurrFrame.erase(p3DCurrFrame.begin()+i);
+//            currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+//            trackedPreviousFrame2DPoints.erase(trackedPreviousFrame2DPoints.begin()+i);
+//        }
+//    }
 
+    std::cout<<"previousFrame2DPoints size "<<previousFrame2DPoints.size() << std::endl;
+    std::cout<<"currFrame2DPoints size "<<currFrame2DPoints.size() << std::endl;
+    std::cout<<"trackedCurrFrame2DPoints size "<<trackedCurrFrame2DPoints.size() << std::endl;
+    std::cout<<"p2d size "<<trackedPreviousFrame2DPoints.size() << std::endl;
+    std::cout<<"p3d size  "<<p3DCurrFrame.size()<<std::endl;
+
+    //TO DO poseEstimate3D2DPnp
+    pose=VO.poseEstimate2D3DPNP(p3DCurrFrame,trackedPreviousFrame2DPoints,K,prePose);
+
+    map.updatePoseIndex();
+    map.updateCumPose(pose);
+
+//     //TO DO reInitial
+//     if (VO.getReInital()){
+//         //TO DO Harris Detection
+//         std::cout << "re-initial " << std::endl;
+//         cv::goodFeaturesToTrack(leftImage,currFrame2DPoints,maxCorners,0.01,10,cv::Mat(),3,3,false,0.04);
+//         cv::cornerSubPix(leftImage,currFrame2DPoints,subPixel,cv::Size(-1,-1),termcrit);
+//         std::cout << "step1 check feature size " << currFrame2DPoints.size() << std::endl;
+
+//         //TO DO getDisparityMapFromCurrImage
+//         std::vector<cv::Point3f> p3d;
+//         VO.generateDisparityMap(leftImage,rightImage);
+//         p3d = VO.getDepth3DPointsFromCurrImage(currFrame2DPoints,K);
+//         int maxDistance = 150 ;
+//         for (int i = 0; i <p3d.size() ; ++i) {
+//             if (p3d[i].z > maxDistance ){
+// //                std::cout<<"depth "<<p3d[i]<<std::endl;
+//                 p3d.erase(p3d.begin()+i);
+//                 currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+//             }
+//             else if (std::isnan(p3d[i].z)){
+//                 p3d.erase(p3d.begin()+i);
+//                 currFrame2DPoints.erase(currFrame2DPoints.begin()+i);
+//             }
+//         }
+//         std::cout<<" re-initial p3d size "<<p3d.size()<<"  re-initial currFrame2DPoints size"<<currFrame2DPoints.size()<<std::endl;
+// //        std::cout << "step 2 check feature size after erase " << currFrame2DPoints.size() << std::endl;
+
+//         return pose;
+//     }
+
+
+    previousFrame2DPoints.clear();
+    previousFrame2DPoints=trackedPreviousFrame2DPoints;
+//    currFrame2DPoints.clear();
+    leftImage.copyTo(previousImage);
+
+    return pose;
+}
 
-    std::vector<cv::Point3d> p3d_prevFrame;
-    std::vector<cv::Point2d> p2d_currFrame;
-    std::vector<int> inlier_index;
-    VO.get3D2DCorrespondences(keypoints_new, matches, p3d_prevFrame, p2d_currFrame, disparity_map, K);
 
-    inlier_index=VO.estimatePose3D2D(p3d_prevFrame, p2d_currFrame, K);
-    VO.setReferenceFrame(image_left, disparity_map, keypoints_new, descriptors_new);
+// visualization
+void VisualSLAM::plotTrajectoryNextStep(cv::Mat& window, int index, Eigen::Vector3d& translGTAccumulated, Eigen::Vector3d& translEstimAccumulated,
+                                            Sophus::SE3d groundTruthPose, Sophus::SE3d groundTruthPrevPose, Eigen::Matrix3d& cumR, Sophus::SE3d estimPose,  Sophus::SE3d estimPrevPose){
+    int offsetX = 300;
+    int offsetY = 300;
 
+    Sophus::SE3d pose = estimPose.inverse();
+    Sophus::SE3d prevPose = estimPrevPose.inverse();
 
-    // each motion bundle adjustment (reference image's 3D point and matched image's 2D point)
-    std::vector<cv::Point3d> p3d_prevFrame_inlier;
-    std::vector<cv::Point2d> p2d_currFrame_inlier;
-    for(int i=0;i<inlier_index.size();i++)
-    {
-        int index=inlier_index[i];
-        p3d_prevFrame_inlier.push_back(p3d_prevFrame[index]);
-        p2d_currFrame_inlier.push_back(p2d_currFrame[index]);
+    if (index == 0){
+        translGTAccumulated = groundTruthPose.translation();
+        translEstimAccumulated = pose.translation();
+    } else {
+        translGTAccumulated = translGTAccumulated + (groundTruthPose.so3().inverse()*groundTruthPrevPose.so3())*(groundTruthPose.translation() - groundTruthPrevPose.translation());
+        translEstimAccumulated = translGTAccumulated + (pose.so3().inverse()*groundTruthPrevPose.so3())*(pose.translation() - prevPose.translation());
     }
-    Sophus::SE3d new_pose = BA.optimizeLocalPoseBA_ceres(p3d_prevFrame_inlier,p2d_currFrame_inlier, K, VO.getPose(), 50);
-    historyPoses.push_back(new_pose);
-    std::cout << "After optimizations:\n" << new_pose.matrix() << std::endl;
-}
-
-void VisualSLAM::runBackEndRoutine(){
-	// TODO
+    cv::circle(window, cv::Point2d(offsetX + translGTAccumulated[0], offsetY + translGTAccumulated[2]), 3, cv::Scalar(0,0,255), -1);
+    cv::circle(window, cv::Point2f(offsetX + translEstimAccumulated[0], offsetY + translEstimAccumulated[2]), 3, cv::Scalar(0,255,0), -1);
+    cv::imshow("Trajectory", window);
+    cv::waitKey(3);
+    cumR = cumR*pose.so3().matrix();
 }
 
-void VisualSLAM::update(){
-	//TODO
-}
-
-//void VisualSLAM::visualizeAllPoses(){
-//    // create pangolin window and plot the trajectory
-//    pangolin::CreateWindowAndBind("VisualSLAM Viewer", 1024, 768);
-//    glEnable(GL_DEPTH_TEST);
-//    glEnable(GL_BLEND);
-//    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-//    pangolin::OpenGlRenderState s_cam(
-//                pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
-//                pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
-//                );
-
-//    pangolin::View &d_cam = pangolin::CreateDisplay()
-//            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
-//            .SetHandler(new pangolin::Handler3D(s_cam));
-
-//    double fx = K(0,0);
-//    double fy = K(1,1);
-//    double cx = K(0,2);
-//    double cy = K(1,2);
-
-//    while (pangolin::ShouldQuit() == false)
-//    {
-//        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-//        d_cam.Activate(s_cam);
-//        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
-
-//        // draw poses
-//        float sz = 0.1;
-//        int width = 640, height = 480;
-//        for (auto &Tcw: historyPoses)
-//        {
-//            glPushMatrix();
-//            Sophus::Matrix4f m = Tcw.inverse().matrix().cast<float>();
-//            glMultMatrixf((GLfloat *) m.data());
-//            glColor3f(1, 0, 0);
-//            glLineWidth(2);
-//            glBegin(GL_LINES);
-//            glVertex3f(0, 0, 0);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glVertex3f(0, 0, 0);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(0, 0, 0);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(0, 0, 0);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-//            glEnd();
-//            glPopMatrix();
-//        }
-
-//        pangolin::FinishFrame();
-//        usleep(5000);   // sleep 5 ms
-//    }
-//}
-void VisualSLAM::visualizeAllPoses(){
-    // create pangolin window and plot the trajectory
-    pangolin::CreateWindowAndBind("VisualSLAM Viewer", 1024, 768);
-    glEnable(GL_DEPTH_TEST);
-    glEnable(GL_BLEND);
-    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-    pangolin::OpenGlRenderState s_cam(
-                pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
-                pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
-                );
-
-    pangolin::View &d_cam = pangolin::CreateDisplay()
-            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
-            .SetHandler(new pangolin::Handler3D(s_cam));
-
-    double fx = K(0,0);
-    double fy = K(1,1);
-    double cx = K(0,2);
-    double cy = K(1,2);
-
-    while (pangolin::ShouldQuit() == false)
-    {
-        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-        d_cam.Activate(s_cam);
-        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
-
-        // draw poses
-        float sz = 0.1;
-        int width = 640, height = 480;
-        for (auto &Tcw: historyPoses)
-        {
-            glPushMatrix();
-            Sophus::Matrix4f m = Tcw.inverse().matrix().cast<float>();
-            glMultMatrixf((GLfloat *) m.data());
-            glColor3f(1, 0, 0);
-            glLineWidth(2);
-            glBegin(GL_LINES);
-            glVertex3f(0, 0, 0);
-            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glVertex3f(0, 0, 0);
-            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(0, 0, 0);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(0, 0, 0);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
-            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
-            glEnd();
-            glPopMatrix();
-        }
-
-        pangolin::FinishFrame();
-        usleep(5000);   // sleep 5 ms
+Sophus::SE3d VisualSLAM::getPose(int k) {
+    if (k<0 || k > map.getCumPose().size()){
+        throw std::runtime_error("VisualSLAM::getPose(int k): Index out of the bounds");
     }
+    return map.getCumPose().at(k);
 }
diff --git a/testVSLAM.cpp b/testVSLAM.cpp
new file mode 100644
index 0000000..eb29f34
--- /dev/null
+++ b/testVSLAM.cpp
@@ -0,0 +1,100 @@
+#include <iostream>
+#include <VisualSLAM.h>
+
+
+int main(int argc, char const *argv[]){
+
+//    std::cout<<"hello my slam "<<std::endl;
+    VisualSLAM slam;
+
+//    std::cout<<"get test value from Map.h:  "<<slam.getTestValueFromMap()<<std::endl;
+
+    if(argc < 5) {
+        std::cout << "/slam  left_image_directory right_image_directory camera_intrinsics_file num_images" << std::endl;
+        exit(1);
+    }
+
+
+    //TO DO read input left_image and right_image
+    std::string left_image_path = argv[1];
+    std::string right_image_path = argv[2];
+    int num_images = std::stoi(argv[3]);
+
+    if(num_images <= 0)
+    {
+        throw std::runtime_error("The number of images is smaller than 0 ");
+    }
+
+    std::vector<Sophus::SE3d> groundTruthPoses;
+
+    if (argc >= 6){
+        std::string ground_truth_path = argv[5];
+        slam.readGroundTruthData(ground_truth_path, num_images, groundTruthPoses);
+    }
+    Eigen::Vector3d translGTAccumulated, translEstimAccumulated(1,1,1);
+    cv::Mat window = cv::Mat::zeros(1000, 1000, CV_8UC3);
+    cv::Mat prevImageLeft;
+    std::vector<cv::Point2f> pointsCurrentFrame, pointsPrevFrame;
+    std::vector<cv::KeyPoint> keypoints;
+    cv::Mat descriptors;
+    Eigen::Matrix3d cumR = Eigen::Matrix3d::Identity();
+
+    std::string image_name_template = "00000";
+    //TO DO read camera_instrinsics_path
+    std::string camera_instrinsics_path = argv[4];
+    Eigen::Matrix3d K = slam.getCameraIntrinsics(camera_instrinsics_path);
+    double fx = K(0,0);
+
+    int k(1);
+    std::vector<cv::Point2f> previousFrame2DPoints, currFrame2DPoints;
+    cv::Mat previousImage;
+    for(int i=0;i<num_images;i++) {
+        if(i == std::pow(10,k)){
+            image_name_template = image_name_template.substr(0,image_name_template.length()-1);
+            k++;
+        }
+
+        std::string left_image_name  = left_image_path+image_name_template + std::to_string(i)+".png";
+        std::string right_image_name = right_image_path+image_name_template+ std::to_string(i)+".png";
+        cv::Mat leftImage = cv::imread(left_image_name,0) ;
+//        cv::Mat leftImage_BGR;
+//        cv::cvtColor(leftImage,leftImage_BGR,CV_GRAY2BGR);
+        cv::Mat rightImage = cv::imread(right_image_name,0);
+////        // show image
+//       cv::namedWindow("leftImage",CV_WINDOW_AUTOSIZE);
+//       cv::imshow("leftImage",leftImage);
+//       cv::waitKey();
+        if(leftImage.cols <= 0 || leftImage.rows <= 0){
+            throw std::runtime_error("can not read leftImage and its path is : "+ left_image_name);
+        }
+        if(rightImage.cols <= 0 || rightImage.rows <= 0){
+            throw std::runtime_error("can not read rightImage and its path is : "+ right_image_name);
+        }
+        Sophus::SE3d estimatedPose;
+        Sophus::SE3d prePose;
+        estimatedPose = slam.estimate3D2DFrontEndWithOpicalFlow(leftImage, rightImage, previousFrame2DPoints, currFrame2DPoints, previousImage,prePose);
+
+        prePose = estimatedPose;
+
+
+        //visualization
+        Sophus::SE3d cumPose;
+        if (i != 0){
+            cumPose = slam.getPose(i);
+        }
+        std::cout<<"pose "<< i << std::endl<<cumPose.matrix()<<std::endl;
+        if (!groundTruthPoses.empty() && i < groundTruthPoses.size()){
+            if (i == 0){
+                Sophus::SE3d groundTruthPrevPose = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0));
+                slam.plotTrajectoryNextStep(window, i, translGTAccumulated, translEstimAccumulated, groundTruthPoses[i], groundTruthPrevPose, cumR, estimatedPose);
+            }
+            else {
+                std::cout << "Frame " << i << " / " << groundTruthPoses.size() << std::endl;
+                Sophus::SE3d prevCumPose = slam.getPose(i-1);
+                slam.plotTrajectoryNextStep(window, i, translGTAccumulated, translEstimAccumulated, groundTruthPoses[i], groundTruthPoses[i-1], cumR, cumPose, prevCumPose);
+            }
+        }
+    }
+
+
+}
\ No newline at end of file
diff --git a/testVisualSLAM.cpp b/testVisualSLAM.cpp
deleted file mode 100644
index ebd3bde..0000000
--- a/testVisualSLAM.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include "VisualSLAM.h"
-
-int main(int argc, char** argv){
-
-	if (argc < 5){
-		std::cout << "Usage: ./slam <input_left_image_directory> <input_right_image_directory> <camera_intrinsics_file_path> <num_images>" << std::endl;
-		exit(1);
-	}
-
-	std::string input_left_images_path = argv[1];
-	std::string input_right_images_path = argv[2];
-	std::string camera_intrinsics_path = argv[3];
-	int num_images = std::stoi(argv[4]);
-	std::string image_name_template = "00000";
-
-    if (num_images <= 0)
-    {
-		throw std::runtime_error("The number of image pairs is invalid");
-	}
-
-	VisualSLAM slam;
-    int k = 1;
-	slam.readCameraIntrisics(camera_intrinsics_path);
-    for (int i = 0; i < num_images; i++){
-        if (i == std::pow(10, k)){
-            image_name_template = image_name_template.substr(0, image_name_template.length() - 1);
-            k++;
-        }
-
-        std::string image_left_name = input_left_images_path + image_name_template + std::to_string(i) + ".png";
-		std::string image_right_name = input_right_images_path + image_name_template + std::to_string(i) + ".png";
-        cv::Mat image_left = cv::imread(image_left_name, 0);
-        cv::Mat image_right = cv::imread(image_right_name, 0);
-
-		if (image_left.cols == 0 || image_left.rows == 0){
-			throw std::runtime_error("Cannot read the image with the path: " + image_left_name);
-		}
-
-		if (image_right.cols == 0 || image_right.rows == 0){
-			throw std::runtime_error("Cannot read the image with the path: " + image_right_name);
-		}
-        //cv::imshow("Image_left", image_left);
-        //cv::imshow("Image_right", image_right);
-        //cv::waitKey(0);
-        slam.performFrontEndStep(image_left, image_right);
-    }
-
-    slam.visualizeAllPoses();
-    return 0;
-}