Pipeline

CMakeLists.txt

@@ -10,13 +10,12 @@ MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
 add_definitions("-DENABLE_SSE")
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3 -march=native")
-
+# add_definitions(-D_GLIBCXX_USE_CXX11_ABI=0)
 
 set(CMAKE_BUILD_TYPE Release)
 
-set(CMAKE_CXX_FLAGS "-std=c++11 -Wall  -O2 ${SSE_FLAGS} -msse4")
-set(CMAKE_CXX_FLAGS_RELEASE  "-std=c++11 -O3 -fopenmp -pthread")
-
+set(CMAKE_CXX_FLAGS "-std=c++14 -Wall  -O2 ${SSE_FLAGS} -msse4")
+set(CMAKE_CXX_FLAGS_RELEASE  "-std=c++14 -O3 -fopenmp -pthread")
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
@@ -32,11 +31,23 @@ find_package(Eigen3 REQUIRED)
 # pangolin
 find_package(Pangolin REQUIRED)
 
+# G2O
+find_package(G2O REQUIRED)
+include_directories(${G2O_INCLUDE_DIRS})
+
+# glog
+find_package(Glog REQUIRED)
+include_directories(${GLOG_INCLUDE_DIRS})
+
 # Sophus
 find_package(Sophus REQUIRED)
 # set(Sophus_LIBRARIES libSophus.so)
 # include_directories(${Sophus_INCLUDE_DIRS})
 
+# csparse
+find_package(CSparse REQUIRED)
+include_directories(${CSPARSE_INCLUDE_DIR})
+
 include_directories(
 ${PROJECT_SOURCE_DIR}
 ${PROJECT_SOURCE_DIR}/include
@@ -57,13 +68,15 @@ src/feature.cpp
 src/map.cpp
 src/mappoint.cpp
 src/camera.cpp
-src/orb.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
 ${OpenCV_LIBS}
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
+g2o_core g2o_stuff g2o_types_sba g2o_solver_csparse g2o_csparse_extension
+${CSPARSE_LIBRARY}
+${GLOG_LIBRARIES}
 pthread
 )
 

README.md

@@ -0,0 +1,8 @@
+# ECT_SLAM
+ECT_SLAM is a real-time SLAM framework for **Monocular Visual Systems**. It features initialization, feature-based visual odometry and local pose graph optimization. This code runs on **Linux**, and is fully integrated with **ROS**. 
+## Reference
+* **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**, Mur-Artal, Raul, Jose Maria Martinez Montiel, Juan D. Tardos, IEEE Transactions on Robotics [PDF](http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf)
+
+* **VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator**, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, IEEE Transactions on Robotics [PDF](https://ieeexplore.ieee.org/document/8421746/?arnumber=8421746&source=authoralert) 
+
+* **视觉 SLAM 十四讲: 从理论到实践**， 高翔， 电子工业出版社， 2017 

build.sh

@@ -3,5 +3,5 @@ echo "Building ECT_SLAM ..."
 rm -rf build
 mkdir build
 cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release 
+cmake .. -DCMAKE_BUILD_TYPE=Debug
 make -j8

build_ros.sh

@@ -4,5 +4,5 @@ cd example/ROS/ECT_SLAM
 rm -rf build
 mkdir build
 cd build
-cmake .. -DROS_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3
+cmake .. -DROS_BUILD_TYPE=Debug -DPYTHON_EXECUTABLE=/usr/bin/python3 
 make -j8

config/default.yaml

@@ -9,9 +9,23 @@ camera.fy: 516.5
 camera.cx: 325.1
 camera.cy: 249.7
 
+# camera.fx: 493.0167
+# camera.fy: 491.55953
+# camera.cx: 317.97856
+# camera.cy: 242.392
+
+is_equalize: 1
+
 num_features: 150
 num_features_init: 50
 num_features_tracking: 50
+ratio_for_keyframe: 0.50
+num_for_keyframe: 80
+
 
 # ros
-# img_topic: "cam0/image_raw"
+image_topic: "/camera/rgb/image_color" #"cam0/image_raw"
+
+# backend
+chi2_th: 5.991
+num_iteration: 20

example/ROS/ECT_SLAM/CMakeLists.txt

@@ -12,9 +12,10 @@ MESSAGE("Build type: " ${ROS_BUILD_TYPE})
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
 
+
 # Check C++11 or C++0x support
-set(CMAKE_CXX_FLAGS "-std=c++11 -Wall")
-set(CMAKE_CXX_FLAGS_RELEASE  "-std=c++11 -O3 -fopenmp -pthread")
+set(CMAKE_CXX_FLAGS "-std=c++14 -Wall")
+set(CMAKE_CXX_FLAGS_RELEASE  "-std=c++14 -O3 -fopenmp -pthread")
 
 
 find_package(OpenCV 3.0 QUIET)
@@ -37,6 +38,7 @@ ${Pangolin_INCLUDE_DIRS}
 
 set(LIBS 
 ${OpenCV_LIBS} 
+opencv_core
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/../../../lib/libECT_SLAM.so
@@ -47,6 +49,15 @@ rosbuild_add_executable(Mono
 src/ros_mono.cpp
 )
 
+# Node for TUM dataset
+rosbuild_add_executable(TUM
+src/tum.cpp
+)
+
 target_link_libraries(Mono
 ${LIBS}
 )
+
+target_link_libraries(TUM
+${LIBS}
+)

example/ROS/ECT_SLAM/src/ros_mono.cpp

@@ -10,6 +10,7 @@
 #include<opencv2/core/core.hpp>
 
 #include"system.hpp"
+#include"config.hpp"
 
 using namespace std;
 
@@ -28,9 +29,9 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "Mono");
     ros::start();
 
-    if(argc != 2)
+    if(argc != 3)
     {
-        cerr << endl << "Usage: rosrun ECT_SLAM Mono path_to_settings" << endl;        
+        cerr << endl << "Usage: rosrun ECT_SLAM Mono path_to_settings image_topic" << endl;        
         ros::shutdown();
         return 1;
     }    
@@ -39,11 +40,10 @@ int main(int argc, char **argv)
     ECT_SLAM::System SLAM(argv[1],ECT_SLAM::System::MONOCULAR);
     SLAM.Init();
 
-
     ImageGrabber igb(&SLAM);
 
     ros::NodeHandle nodeHandler;
-    ros::Subscriber sub = nodeHandler.subscribe("cam0/image_raw", 1, &ImageGrabber::GrabImage,&igb);
+    ros::Subscriber sub = nodeHandler.subscribe(argv[2], 1, &ImageGrabber::GrabImage,&igb);
 
     ros::spin();
 

include/algorithm.hpp

@@ -1,36 +1,40 @@
+#pragma once
 #include "common_include.hpp"
 
-namespace ECT_SLAM {
+namespace ECT_SLAM
+{
 
-/**
+    /**
  * linear triangulation with SVD
  * @param poses     poses,
  * @param points    points in normalized plane
  * @param pt_world  triangulated point in the world
  * @return true if success
  */
-inline bool triangulation(const std::vector<SE3> &poses,
-                   const std::vector<Vec3> points, Vec3 &pt_world) {
-    MatXX A(2 * poses.size(), 4);
-    VecX b(2 * poses.size());
-    b.setZero();
-    for (size_t i = 0; i < poses.size(); ++i) {
-        Mat34 m = poses[i].matrix3x4();
-        A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
-        A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
-    }
-    auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
-    pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();
+    inline bool triangulation(const std::vector<SE3> &poses,
+                              const std::vector<Vec3> points, Vec3 &pt_world)
+    {
+        MatXX A(2 * poses.size(), 4);
+        VecX b(2 * poses.size());
+        b.setZero();
+        for (size_t i = 0; i < poses.size(); ++i)
+        {
+            Mat34 m = poses[i].matrix3x4();
+            A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
+            A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
+        }
+        auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
+        pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();
 
-    if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) {
-        // 解质量不好，放弃
-        return true;
+        if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2)
+        {
+            // 解质量不好，放弃
+            return true;
+        }
+        return false;
     }
-    return false;
-}
-
-// converters
-inline Vec2 toVec2(const cv::Point2f p) { return Vec2(p.x, p.y); }
 
-}  // namespace ECT_SLAM
+    // converters
+    inline Vec2 toVec2(const cv::Point2f p) { return Vec2(p.x, p.y); }
 
+} // namespace ECT_SLAM

include/camera.hpp

@@ -2,41 +2,59 @@
 
 #include "common_include.hpp"
 
-namespace ECT_SLAM {
+namespace ECT_SLAM
+{
 
-// Pinhole stereo camera model
-class Camera {
-   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-    typedef std::shared_ptr<Camera> Ptr;
+    // Pinhole stereo camera model
+    class Camera
+    {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+        typedef std::shared_ptr<Camera> Ptr;
 
-    double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0;  // Camera intrinsics
+        double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0; // Camera intrinsics
+        double k1_ = 0, k2_ = 0, r1_ = 0, r2_ = 0;
 
-    Camera();
+        Camera();
 
-    Camera(double fx, double fy, double cx, double cy)
-        : fx_(fx), fy_(fy), cx_(cx), cy_(cy) {
-    }
+        Camera(double fx, double fy, double cx, double cy)
+            : fx_(fx), fy_(fy), cx_(cx), cy_(cy)
+        {
+        }
 
-    // return intrinsic matrix
-    Mat33 K() const {
-        Mat33 k;
-        k << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
-        return k;
-    }
+        Mat33f K() const
+        {
+            Mat33f k;
+            k << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
+            return k;
+        }
 
-    // coordinate transform: world, camera, pixel
-    Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w);
+        // return intrinsic matrix
+        Mat33 K_d() const
+        {
+            Mat33 k;
+            k << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
+            return k;
+        }
 
-    Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w);
+        // return intrinsic matrix
+        cv::Mat K_cv() const
+        {
+            return (cv::Mat1d(3, 3) << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1);
+        }
 
-    Vec2 camera2pixel(const Vec3 &p_c);
+        // coordinate transform: world, camera, pixel
+        Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w);
 
-    Vec3 pixel2camera(const Vec2 &p_p, double depth = 1);
+        Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w);
 
-    Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth = 1);
+        Vec2 camera2pixel(const Vec3 &p_c);
 
-    Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w);
-};
+        Vec3 pixel2camera(const Vec2 &p_p, double depth = 1);
 
-}  // namespace ECT_SLAM
+        Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth = 1);
+
+        Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w);
+    };
+
+} // namespace ECT_SLAM

include/common_include.hpp

@@ -55,9 +55,11 @@ typedef Eigen::Matrix<double, 14, 14> Mat1414;
 
 // float matricies
 typedef Eigen::Matrix<float, 3, 3> Mat33f;
+typedef Eigen::Matrix<float, 3, 4> Mat34f;
 typedef Eigen::Matrix<float, 10, 3> Mat103f;
 typedef Eigen::Matrix<float, 2, 2> Mat22f;
 typedef Eigen::Matrix<float, 3, 1> Vec3f;
+typedef Eigen::Matrix<float, 4, 1> Vec4f;
 typedef Eigen::Matrix<float, 2, 1> Vec2f;
 typedef Eigen::Matrix<float, 6, 1> Vec6f;
 typedef Eigen::Matrix<float, 1, 8> Mat18f;
@@ -110,10 +112,11 @@ typedef Sophus::SO3d SO3;
 
 // for cv
 #include <opencv2/core/core.hpp>
+#include <opencv2/core/eigen.hpp>
 
 using cv::Mat;
 
-// // glog
-// #include <glog/logging.h>
+// glog
+#include <glog/logging.h>
 
 #endif  // ECT_SLAM_COMMON_INCLUDE_H