VOwithoutBA

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})

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 
+
+
+
+

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
+};

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();
+};

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();
+};

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)));
+};