├── .clang-format ├── doc ├── data_structure.pptx ├── test_without_IMU │ ├── withou_IMU2.png │ └── without_IMU.png ├── test │ └── readme.md └── test_with_IMU │ ├── Screenshot from 2022-04-02 20-28-28.png │ ├── Screenshot from 2022-04-02 20-38-24.png │ └── Screenshot from 2022-04-02 20-38-36.png ├── config ├── run_kitti.yaml ├── frontend_config.yaml └── run_m300.yaml ├── src ├── CMakeLists.txt ├── app │ ├── CMakeLists.txt │ └── visual_odometry.cc ├── common │ ├── CMakeLists.txt │ ├── feature.cc │ ├── frame.cc │ ├── map_point.cc │ ├── camera.cc │ └── map.cc ├── module │ ├── CMakeLists.txt │ ├── local_BA.cc │ └── frontend.cc ├── tool │ ├── CMakeLists.txt │ ├── dataset_base.cc │ ├── image_preprocess.cc │ ├── algorithm.cc │ ├── kitti_dataset.cc │ ├── viewer.cc │ └── m300_dataset.cc └── test │ ├── CMakeLists.txt │ ├── run_kitti.cc │ ├── run_m300.cc │ └── test_m300_dataset.cc ├── .gitignore ├── cmake_modules ├── FindCSparse.cmake ├── FindG2O.cmake └── FindGlog.cmake ├── include └── stereo_camera_vo │ ├── tool │ ├── image_preprocess.h │ ├── kitti_dataset.h │ ├── algorithm.h │ ├── system_lib.h │ ├── dataset_base.h │ ├── m300_dataset.h │ ├── viewer.h │ └── print_ctrl_macro.h │ ├── common │ ├── feature.h │ ├── map_point.h │ ├── camera.h │ ├── map.h │ └── frame.h │ ├── app │ └── visual_odometry.h │ └── module │ ├── local_BA.h │ ├── frontend.h │ └── g2o_types.h ├── CMakeLists.txt └── README.md /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | SortIncludes: false 3 | -------------------------------------------------------------------------------- /doc/data_structure.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/data_structure.pptx -------------------------------------------------------------------------------- /doc/test_without_IMU/withou_IMU2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/test_without_IMU/withou_IMU2.png -------------------------------------------------------------------------------- /doc/test_without_IMU/without_IMU.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/test_without_IMU/without_IMU.png -------------------------------------------------------------------------------- /config/run_kitti.yaml: -------------------------------------------------------------------------------- 1 | msleep_time: 0 2 | config_file: ./config/frontend_config.yaml 3 | dataset_path: /media/ls/WORK/slam_kitti/dataset/sequences/15 4 | -------------------------------------------------------------------------------- /doc/test/readme.md: -------------------------------------------------------------------------------- 1 | ## without IMU 2 | 3 | - the rotation error is much more bigger! 4 | 5 | 6 | ## with IMU 7 | 8 | - rotation error is smaller. 9 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(common) 2 | add_subdirectory(tool) 3 | add_subdirectory(module) 4 | add_subdirectory(app) 5 | 6 | add_subdirectory(test) 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .clangd/ 2 | .ccls-cache/ 3 | .cache/ 4 | .vscode/ 5 | .idea/ 6 | .kdev4/ 7 | build/ 8 | lib/ 9 | bin/ 10 | compile_commands.json 11 | *.pth 12 | *.kdev4 13 | -------------------------------------------------------------------------------- /doc/test_with_IMU/Screenshot from 2022-04-02 20-28-28.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/test_with_IMU/Screenshot from 2022-04-02 20-28-28.png -------------------------------------------------------------------------------- /doc/test_with_IMU/Screenshot from 2022-04-02 20-38-24.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/test_with_IMU/Screenshot from 2022-04-02 20-38-24.png -------------------------------------------------------------------------------- /doc/test_with_IMU/Screenshot from 2022-04-02 20-38-36.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lee-shun/stereo_camera_vo/HEAD/doc/test_with_IMU/Screenshot from 2022-04-02 20-38-36.png -------------------------------------------------------------------------------- /config/frontend_config.yaml: -------------------------------------------------------------------------------- 1 | num_features: 100 2 | num_features_init: 30 3 | num_features_tracking: 20 4 | num_features_tracking_bad: 10 5 | num_features_needed_for_keyframe: 30 6 | -------------------------------------------------------------------------------- /config/run_m300.yaml: -------------------------------------------------------------------------------- 1 | msleep_time: 0 2 | # dataset_path: /media/ls/WORK/slam_m300/m300_data_1 3 | start_from_index: 12000 4 | dataset_path: /home/ls/m300_data 5 | config_file: ./config/frontend_config.yaml 6 | -------------------------------------------------------------------------------- /src/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | ${PROJECT_NAME}_app 3 | SHARED 4 | visual_odometry.cc 5 | ) 6 | 7 | target_link_libraries( 8 | ${PROJECT_NAME}_app 9 | ${PROJECT_NAME}_module 10 | ${THIRD_PARTY_LIBS} 11 | ) 12 | -------------------------------------------------------------------------------- /src/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | ${PROJECT_NAME}_common 3 | SHARED 4 | frame.cc 5 | feature.cc 6 | map_point.cc 7 | map.cc 8 | camera.cc 9 | ) 10 | 11 | target_link_libraries( 12 | ${PROJECT_NAME}_common 13 | ${THIRD_PARTY_LIBS} 14 | ) 15 | -------------------------------------------------------------------------------- /src/module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | ${PROJECT_NAME}_module 3 | SHARED 4 | frontend.cc 5 | local_BA.cc 6 | ) 7 | 8 | target_link_libraries( 9 | ${PROJECT_NAME}_module 10 | ${PROJECT_NAME}_common 11 | ${PROJECT_NAME}_tool 12 | ${THIRD_PARTY_LIBS} 13 | ) 14 | -------------------------------------------------------------------------------- /src/tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( 2 | ${PROJECT_NAME}_tool 3 | SHARED 4 | viewer.cc 5 | dataset_base.cc 6 | kitti_dataset.cc 7 | m300_dataset.cc 8 | algorithm.cc 9 | image_preprocess.cc 10 | ) 11 | target_link_libraries( 12 | ${PROJECT_NAME}_tool 13 | ${PROJECT_NAME}_common 14 | ${THIRD_PARTY_LIBS} 15 | ) 16 | -------------------------------------------------------------------------------- /src/common/feature.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: feature.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | 17 | #include "stereo_camera_vo/common/feature.h" 18 | -------------------------------------------------------------------------------- /src/tool/dataset_base.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: dataset_base.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-20 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | 17 | #include "stereo_camera_vo/tool/dataset_base.h" 18 | -------------------------------------------------------------------------------- /cmake_modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | find_path(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | PATH_SUFFIXES 14 | suitesparse 15 | ) 16 | 17 | find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse 18 | PATHS 19 | /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 27 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 28 | -------------------------------------------------------------------------------- /src/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(run_kitti run_kitti.cc) 2 | 3 | target_link_libraries( 4 | run_kitti 5 | ${PROJECT_NAME}_app 6 | ${PROJECT_NAME}_common 7 | ${PROJECT_NAME}_module 8 | ${PROJECT_NAME}_tool 9 | ${THIRD_PARTY_LIBS} 10 | ) 11 | add_executable(run_m300 run_m300.cc) 12 | 13 | target_link_libraries( 14 | run_m300 15 | ${PROJECT_NAME}_app 16 | ${PROJECT_NAME}_common 17 | ${PROJECT_NAME}_module 18 | ${PROJECT_NAME}_tool 19 | ${THIRD_PARTY_LIBS} 20 | ) 21 | 22 | add_executable(test_m300_dataset test_m300_dataset.cc) 23 | 24 | target_link_libraries( 25 | test_m300_dataset 26 | ${PROJECT_NAME}_app 27 | ${PROJECT_NAME}_common 28 | ${PROJECT_NAME}_module 29 | ${PROJECT_NAME}_tool 30 | ${THIRD_PARTY_LIBS} 31 | ) 32 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/image_preprocess.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: image_preprocess.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-04-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_IMAGE_PREPROCESS_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_IMAGE_PREPROCESS_H_ 18 | 19 | #include 20 | 21 | namespace stereo_camera_vo { 22 | namespace tool { 23 | 24 | cv::Mat AddContrastness(cv::Mat gray); 25 | 26 | void GammaTransform(cv::Mat &gary); 27 | 28 | } // namespace tool 29 | } // namespace stereo_camera_vo 30 | 31 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_IMAGE_PREPROCESS_H_ 32 | -------------------------------------------------------------------------------- /src/tool/image_preprocess.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: image_preprocess.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-04-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/tool/image_preprocess.h" 17 | 18 | namespace stereo_camera_vo { 19 | namespace tool { 20 | 21 | cv::Mat AddContrastness(cv::Mat gray) { 22 | cv::Mat equa; 23 | cv::equalizeHist(gray, equa); 24 | return equa; 25 | } 26 | 27 | void GammaTransform(cv::Mat &gray) { 28 | const float gamma = log(128.0 / 255.0) / log(cv::mean(gray)[0] / 255.0); 29 | cv::Mat look_up_tab = cv::Mat::zeros(cv::Size(1, 256), CV_8UC1); 30 | for (int i = 0; i < 256; i++) 31 | look_up_tab.at(0, i) = pow(i / 255.0, gamma) * 255.0; 32 | 33 | cv::LUT(gray, look_up_tab, gray); 34 | } 35 | } // namespace tool 36 | 37 | } // namespace stereo_camera_vo 38 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/kitti_dataset.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: kitti_dataset.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-11 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_KITTI_DATASET_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_KITTI_DATASET_H_ 18 | 19 | #include "stereo_camera_vo/tool/dataset_base.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace stereo_camera_vo { 26 | namespace tool { 27 | class KittiDataset : public DatasetBase { 28 | public: 29 | typedef std::shared_ptr Ptr; 30 | explicit KittiDataset(const std::string& dataset_path); 31 | 32 | bool Init() override; 33 | 34 | bool NextFrame(common::Frame::Ptr new_frame) override; 35 | }; 36 | } // namespace tool 37 | } // namespace stereo_camera_vo 38 | 39 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_KITTI_DATASET_H_ 40 | -------------------------------------------------------------------------------- /src/common/frame.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: frame.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/common/frame.h" 17 | 18 | namespace stereo_camera_vo { 19 | namespace common { 20 | 21 | Frame::Frame(uint64_t id, double time_stamp, const Sophus::SE3d &pose, 22 | const cv::Mat &left, const cv::Mat &right) 23 | : id_(id), 24 | time_stamp_(time_stamp), 25 | pose_(pose), 26 | left_img_(left), 27 | right_img_(right) {} 28 | 29 | Frame::Ptr Frame::CreateFrame() { 30 | static uint64_t factory_id; 31 | Frame::Ptr new_frame(new Frame); 32 | new_frame->id_ = factory_id++; 33 | return new_frame; 34 | } 35 | 36 | void Frame::SetKeyFrame() { 37 | static uint64_t keyframe_factory_id; 38 | std::unique_lock lck(data_mutex_); 39 | is_keyframe_ = true; 40 | keyframe_id_ = keyframe_factory_id++; 41 | } 42 | 43 | } // namespace common 44 | } // namespace stereo_camera_vo 45 | -------------------------------------------------------------------------------- /src/test/run_kitti.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: run_kitti.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-14 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/app/visual_odometry.h" 17 | #include "stereo_camera_vo/tool/system_lib.h" 18 | #include "stereo_camera_vo/tool/kitti_dataset.h" 19 | 20 | int main(int argc, char **argv) { 21 | YAML::Node node = YAML::LoadFile("./config/run_kitti.yaml"); 22 | const uint64_t msleep_time = 23 | stereo_camera_vo::tool::GetParam(node, "msleep_time", 1000); 24 | 25 | const std::string config_file = 26 | stereo_camera_vo::tool::GetParam(node, "config_file", ""); 27 | 28 | const std::string dataset_path = 29 | stereo_camera_vo::tool::GetParam(node, "dataset_path", ""); 30 | stereo_camera_vo::tool::DatasetBase::Ptr dataset = 31 | std::make_shared(dataset_path); 32 | 33 | stereo_camera_vo::app::VisualOdometry::Ptr vo( 34 | new stereo_camera_vo::app::VisualOdometry(config_file, dataset)); 35 | 36 | vo->Run(msleep_time); 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/common/feature.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: feature.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_COMMON_FEATURE_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_COMMON_FEATURE_H_ 18 | 19 | // h 20 | #include 21 | 22 | // std cpp 23 | #include 24 | 25 | // hpp 26 | #include 27 | 28 | namespace stereo_camera_vo { 29 | namespace common { 30 | 31 | struct Frame; 32 | struct MapPoint; 33 | 34 | struct Feature { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | typedef std::shared_ptr Ptr; 38 | 39 | Feature() {} 40 | 41 | Feature(std::shared_ptr frame, const cv::KeyPoint &kp) 42 | : frame_(frame), position_(kp) {} 43 | 44 | public: 45 | std::weak_ptr frame_; 46 | cv::KeyPoint position_; 47 | std::weak_ptr map_point_; 48 | 49 | bool is_outlier_{false}; 50 | bool is_on_left_image_{true}; 51 | }; 52 | } // namespace common 53 | } // namespace stereo_camera_vo 54 | 55 | #endif // INCLUDE_STEREO_CAMERA_VO_COMMON_FEATURE_H_ 56 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/algorithm.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: algorithm.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_ALGORITHM_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_ALGORITHM_H_ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace stereo_camera_vo { 27 | namespace tool { 28 | 29 | /** 30 | * linear triangulation with SVD 31 | * @param poses poses, 32 | * @param points points in normalized plane 33 | * @param pt_world triangulated point in the world 34 | * @return true if success 35 | */ 36 | bool Triangulation(const std::vector &poses, 37 | const std::vector points, 38 | Eigen::Vector3d &pt_world); 39 | 40 | // converters 41 | inline Eigen::Vector2d ToVec2(const cv::Point2f p) { 42 | return Eigen::Vector2d(p.x, p.y); 43 | } 44 | 45 | } // namespace tool 46 | } // namespace stereo_camera_vo 47 | 48 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_ALGORITHM_H_ 49 | -------------------------------------------------------------------------------- /src/common/map_point.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: map_point.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/common/map_point.h" 17 | #include "stereo_camera_vo/common/feature.h" 18 | 19 | namespace stereo_camera_vo { 20 | namespace common { 21 | 22 | MapPoint::MapPoint(uint64_t id, Eigen::Vector3d position) 23 | : id_(id), pos_(position) {} 24 | 25 | MapPoint::Ptr MapPoint::CreateNewMappoint() { 26 | static uint64_t factory_id; 27 | MapPoint::Ptr new_mappoint(new MapPoint); 28 | new_mappoint->id_ = factory_id++; 29 | return new_mappoint; 30 | } 31 | 32 | /** 33 | * current map point observed_times will decrease 34 | * */ 35 | void MapPoint::RemoveObservation(std::shared_ptr feat) { 36 | std::unique_lock lck(data_mutex_); 37 | for (auto iter = observations_.begin(); iter != observations_.end(); iter++) { 38 | if (iter->lock() == feat) { 39 | observations_.erase(iter); 40 | feat->map_point_.reset(); 41 | observed_times_--; 42 | break; 43 | } 44 | } 45 | } 46 | } // namespace common 47 | 48 | } // namespace stereo_camera_vo 49 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(stereo_camera_vo) 3 | 4 | set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -O0 -Wall -ggdb -fopenmp") 5 | set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -O3 -fopenmp") 6 | 7 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 8 | 9 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 10 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 11 | 12 | include_directories(${PROJECT_SOURCE_DIR}/include) 13 | 14 | # Eigen 15 | include_directories("/usr/include/eigen3") 16 | 17 | # OpenCV 18 | find_package(OpenCV 4 REQUIRED) 19 | include_directories(${OpenCV_INCLUDE_DIRS}) 20 | 21 | # pangolin 22 | find_package(Pangolin REQUIRED) 23 | include_directories(${Pangolin_INCLUDE_DIRS}) 24 | 25 | # Sophus 26 | find_package(Sophus REQUIRED) 27 | include_directories(${Sophus_INCLUDE_DIRS}) 28 | 29 | # fmt 30 | find_package(fmt REQUIRED) 31 | include_directories(${fmt_INCLUDE_DIRS}) 32 | 33 | # G2O 34 | find_package(G2O REQUIRED) 35 | include_directories(${G2O_INCLUDE_DIRS}) 36 | 37 | # csparse 38 | find_package(CSparse REQUIRED) 39 | include_directories(${CSPARSE_INCLUDE_DIR}) 40 | 41 | # yaml 42 | find_package(yaml-cpp REQUIRED) 43 | 44 | set( 45 | THIRD_PARTY_LIBS 46 | ${OpenCV_LIBS} 47 | ${Sophus_LIBRARIES} 48 | ${Pangolin_LIBRARIES} GL GLU GLEW glut 49 | g2o_core g2o_stuff g2o_types_sba g2o_solver_csparse g2o_csparse_extension 50 | pthread 51 | ${CSPARSE_LIBRARY} 52 | fmt::fmt 53 | yaml-cpp 54 | ) 55 | 56 | 57 | add_subdirectory(src) 58 | -------------------------------------------------------------------------------- /src/test/run_m300.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: run_m300.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-21 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/app/visual_odometry.h" 17 | #include "stereo_camera_vo/tool/m300_dataset.h" 18 | #include "stereo_camera_vo/tool/system_lib.h" 19 | 20 | int main(int argc, char **argv) { 21 | YAML::Node node = YAML::LoadFile("./config/run_m300.yaml"); 22 | const uint64_t msleep_time = 23 | stereo_camera_vo::tool::GetParam(node, "msleep_time", 1000); 24 | 25 | const std::string config_file = 26 | stereo_camera_vo::tool::GetParam(node, "config_file", ""); 27 | 28 | const std::string dataset_path = 29 | stereo_camera_vo::tool::GetParam(node, "dataset_path", ""); 30 | 31 | const int start_from_index = 32 | stereo_camera_vo::tool::GetParam(node, "start_from_index", 1); 33 | 34 | stereo_camera_vo::tool::DatasetBase::Ptr dataset = 35 | std::make_shared(dataset_path, 36 | start_from_index); 37 | 38 | stereo_camera_vo::app::VisualOdometry::Ptr vo( 39 | new stereo_camera_vo::app::VisualOdometry(config_file, dataset)); 40 | 41 | vo->Run(msleep_time); 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/system_lib.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: system_lib.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-22 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_SYSTEM_LIB_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_SYSTEM_LIB_H_ 18 | 19 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace stereo_camera_vo { 26 | namespace tool { 27 | 28 | template 29 | T GetParam(const YAML::Node& node, const std::string& var_key, 30 | const T& default_value) { 31 | T v; 32 | try { 33 | v = node[var_key].as(); 34 | } catch (std::exception e) { 35 | v = default_value; 36 | PRINT_WARN("cannot find key: %s, set as default_value", var_key.c_str()); 37 | } 38 | return v; 39 | } 40 | 41 | /** 42 | * locate the file to the line number 43 | * */ 44 | inline std::ifstream& SeekToLine(std::ifstream& in, const uint16_t line_nbr) { 45 | int i; 46 | char buf[1024]; 47 | // locate to begin of the file 48 | in.seekg(0, std::ios::beg); 49 | for (i = 0; i < line_nbr; i++) { 50 | in.getline(buf, sizeof(buf)); 51 | } 52 | return in; 53 | } 54 | 55 | } // namespace tool 56 | } // namespace stereo_camera_vo 57 | 58 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_SYSTEM_LIB_H_ 59 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/app/visual_odometry.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: visual_odometry.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-14 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_APP_VISUAL_ODOMETRY_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_APP_VISUAL_ODOMETRY_H_ 18 | 19 | #include "stereo_camera_vo/module/frontend.h" 20 | #include "stereo_camera_vo/tool/dataset_base.h" 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace stereo_camera_vo { 27 | namespace app { 28 | class VisualOdometry { 29 | public: 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | typedef std::shared_ptr Ptr; 32 | 33 | /// constructor with config file 34 | explicit VisualOdometry(std::string frontend_config_path, 35 | tool::DatasetBase::Ptr dataset); 36 | /** 37 | * start vo in the dataset 38 | */ 39 | void Run(const uint64_t msleep = 0); 40 | 41 | /** 42 | * Make a step forward in dataset 43 | */ 44 | bool Step(); 45 | 46 | module::FrontendStatus GetFrontendStatus() const { 47 | return frontend_->GetStatus(); 48 | } 49 | 50 | private: 51 | bool inited_{false}; 52 | 53 | module::Frontend::Ptr frontend_{nullptr}; 54 | 55 | // dataset 56 | tool::DatasetBase::Ptr dataset_{nullptr}; 57 | }; 58 | } // namespace app 59 | } // namespace stereo_camera_vo 60 | 61 | #endif // INCLUDE_STEREO_CAMERA_VO_APP_VISUAL_ODOMETRY_H_ 62 | -------------------------------------------------------------------------------- /src/tool/algorithm.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: algorithm.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | 17 | #include "stereo_camera_vo/tool/algorithm.h" 18 | 19 | namespace stereo_camera_vo { 20 | namespace tool { 21 | 22 | /** 23 | * linear triangulation with SVD 24 | * @param poses poses, 25 | * @param points points in normalized plane 26 | * @param pt_world triangulated point in the world 27 | * @return true if success 28 | */ 29 | bool Triangulation(const std::vector &poses, 30 | const std::vector points, 31 | Eigen::Vector3d &pt_world) { 32 | Eigen::Matrix A(2 * poses.size(), 4); 33 | Eigen::Matrix b(2 * poses.size()); 34 | b.setZero(); 35 | for (size_t i = 0; i < poses.size(); ++i) { 36 | Eigen::Matrix m = poses[i].matrix3x4(); 37 | A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0); 38 | A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1); 39 | } 40 | auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); 41 | pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>(); 42 | 43 | if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) { 44 | return true; 45 | } 46 | 47 | // bad solution 48 | return false; 49 | } 50 | 51 | } // namespace tool 52 | } // namespace stereo_camera_vo 53 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/dataset_base.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: dataset_base.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-20 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_DATASET_BASE_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_DATASET_BASE_H_ 18 | 19 | #include "stereo_camera_vo/common/frame.h" 20 | #include "stereo_camera_vo/common/camera.h" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace stereo_camera_vo { 29 | namespace tool { 30 | class DatasetBase { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 33 | typedef std::shared_ptr Ptr; 34 | 35 | explicit DatasetBase(const std::string dataset_path, 36 | const uint16_t start_from_index) 37 | : dataset_path_(dataset_path), current_image_index_(start_from_index) {} 38 | 39 | virtual ~DatasetBase() {} 40 | 41 | virtual bool Init() = 0; 42 | 43 | virtual bool NextFrame(common::Frame::Ptr new_frame) = 0; 44 | 45 | common::Camera::Ptr GetCamera(const int camera_id) const { 46 | return cameras_.at(camera_id); 47 | } 48 | 49 | uint16_t GetIndex() const { return current_image_index_; } 50 | 51 | protected: 52 | std::string dataset_path_; 53 | uint16_t current_image_index_{0}; 54 | 55 | std::vector cameras_; 56 | }; 57 | } // namespace tool 58 | } // namespace stereo_camera_vo 59 | 60 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_DATASET_BASE_H_ 61 | -------------------------------------------------------------------------------- /src/common/camera.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: camera.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-11 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/common/camera.h" 17 | 18 | namespace stereo_camera_vo { 19 | namespace common { 20 | 21 | Eigen::Vector3d Camera::world2camera(const Eigen::Vector3d &p_w, 22 | const Sophus::SE3d &T_c_w) { 23 | return pose_ * T_c_w * p_w; 24 | } 25 | 26 | Eigen::Vector3d Camera::camera2world(const Eigen::Vector3d &p_c, 27 | const Sophus::SE3d &T_c_w) { 28 | return T_c_w.inverse() * pose_inv_ * p_c; 29 | } 30 | 31 | Eigen::Vector2d Camera::camera2pixel(const Eigen::Vector3d &p_c) { 32 | return Eigen::Vector2d(fx_ * p_c(0, 0) / p_c(2, 0) + cx_, 33 | fy_ * p_c(1, 0) / p_c(2, 0) + cy_); 34 | } 35 | 36 | Eigen::Vector3d Camera::pixel2camera(const Eigen::Vector2d &p_p, double depth) { 37 | return Eigen::Vector3d((p_p(0, 0) - cx_) * depth / fx_, 38 | (p_p(1, 0) - cy_) * depth / fy_, depth); 39 | } 40 | 41 | Eigen::Vector3d Camera::pixel2world(const Eigen::Vector2d &p_p, 42 | const Sophus::SE3d &T_c_w, double depth) { 43 | return camera2world(pixel2camera(p_p, depth), T_c_w); 44 | } 45 | 46 | Eigen::Vector2d Camera::world2pixel(const Eigen::Vector3d &p_w, 47 | const Sophus::SE3d &T_c_w) { 48 | return camera2pixel(world2camera(p_w, T_c_w)); 49 | } 50 | } // namespace common 51 | } // namespace stereo_camera_vo 52 | -------------------------------------------------------------------------------- /src/test/test_m300_dataset.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: test_m300_dataset.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-21 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/tool/m300_dataset.h" 17 | #include "stereo_camera_vo/tool/system_lib.h" 18 | 19 | #include 20 | #include 21 | 22 | int main(int argc, char** argv) { 23 | YAML::Node node = YAML::LoadFile("./config/run_m300.yaml"); 24 | const std::string dataset_path = 25 | stereo_camera_vo::tool::GetParam(node, "dataset_path", ""); 26 | 27 | stereo_camera_vo::tool::DatasetBase::Ptr dataset = 28 | std::make_shared(dataset_path); 29 | 30 | dataset->Init(); 31 | 32 | std::chrono::steady_clock::time_point start = 33 | std::chrono::steady_clock::now(); 34 | 35 | int num = 0; 36 | stereo_camera_vo::common::Frame::Ptr new_frame = 37 | stereo_camera_vo::common::Frame::CreateFrame(); 38 | while (dataset->NextFrame(new_frame)) { 39 | PRINT_INFO("----------------"); 40 | std::cout << "pose: \n" << new_frame->Pose().matrix() << std::endl; 41 | stereo_camera_vo::common::Frame::Ptr new_frame = 42 | stereo_camera_vo::common::Frame::CreateFrame(); 43 | ++num; 44 | } 45 | std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); 46 | double each_t = 47 | std::chrono::duration_cast(end - start) 48 | .count() / 49 | static_cast(num); 50 | PRINT_INFO("average: %lf ms", each_t); 51 | 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/m300_dataset.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: m300_dataset.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-20 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_M300_DATASET_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_M300_DATASET_H_ 18 | 19 | #include "stereo_camera_vo/tool/dataset_base.h" 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace stereo_camera_vo { 27 | namespace tool { 28 | class M300Dataset : public DatasetBase { 29 | public: 30 | explicit M300Dataset(const std::string dataset_path, 31 | const uint16_t start_from = 1) 32 | : DatasetBase(dataset_path, start_from) {} 33 | 34 | bool Init() override; 35 | 36 | bool NextFrame(common::Frame::Ptr new_frame) override; 37 | 38 | static bool GetAttByIndex(const std::string pose_path, 39 | const uint16_t pose_index, Eigen::Quaterniond* att); 40 | 41 | private: 42 | bool first_frame{true}; 43 | // read the camera data 44 | cv::FileStorage camera_config_file_; 45 | 46 | // the first frame pose 47 | Sophus::SE3d first_frame_pose_Tcw_; 48 | 49 | template 50 | T getParameter(const std::string key) { 51 | T t; 52 | camera_config_file_[key] >> t; 53 | return t; 54 | } 55 | 56 | void convert2Eigen(const cv::Mat proj, Eigen::Matrix3d* K, 57 | Eigen::Vector3d* t); 58 | 59 | Sophus::SE3d Twb2Twc(const Sophus::SE3d& Twb) const; 60 | }; 61 | } // namespace tool 62 | } // namespace stereo_camera_vo 63 | 64 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_M300_DATASET_H_ 65 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/viewer.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: viewer.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-11 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_VIEWER_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_VIEWER_H_ 18 | 19 | #include "stereo_camera_vo/common/map.h" 20 | #include "stereo_camera_vo/common/frame.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | namespace stereo_camera_vo { 31 | namespace tool { 32 | class Viewer { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 35 | typedef std::shared_ptr Ptr; 36 | 37 | Viewer(); 38 | 39 | void SetMap(common::Map::Ptr map) { map_ = map; } 40 | 41 | void Stop(); 42 | 43 | void AddCurrentFrame(common::Frame::Ptr current_frame); 44 | 45 | void UpdateMap(); 46 | 47 | private: 48 | void ThreadLoop(); 49 | 50 | void DrawFrame(common::Frame::Ptr frame, const float* color); 51 | 52 | void DrawMap(); 53 | 54 | void FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera); 55 | 56 | /// plot the features in current frame into an image 57 | cv::Mat PlotFrameImage(); 58 | 59 | common::Frame::Ptr current_frame_{nullptr}; 60 | common::Map::Ptr map_{nullptr}; 61 | 62 | std::thread viewer_thread_; 63 | std::atomic viewer_running_; 64 | 65 | std::unordered_map keyframes_; 66 | std::unordered_map landmarks_; 67 | 68 | std::unordered_map active_keyframes_; 69 | std::unordered_map active_landmarks_; 70 | 71 | std::mutex viewer_data_mutex_; 72 | }; 73 | } // namespace tool 74 | } // namespace stereo_camera_vo 75 | 76 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_VIEWER_H_ 77 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/common/map_point.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: map_point.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_POINT_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_POINT_H_ 18 | 19 | // h 20 | #include 21 | 22 | // std cpp 23 | #include 24 | #include 25 | #include 26 | 27 | // hpp 28 | #include 29 | 30 | namespace stereo_camera_vo { 31 | namespace common { 32 | 33 | struct Frame; 34 | struct Feature; 35 | 36 | struct MapPoint { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 39 | typedef std::shared_ptr Ptr; 40 | 41 | MapPoint() {} 42 | 43 | MapPoint(uint64_t id, Eigen::Vector3d position); 44 | 45 | Eigen::Vector3d Pos() { 46 | std::unique_lock lck(data_mutex_); 47 | return pos_; 48 | } 49 | 50 | void SetPos(const Eigen::Vector3d &pos) { 51 | std::unique_lock lck(data_mutex_); 52 | pos_ = pos; 53 | } 54 | 55 | void AddObservation(std::shared_ptr feature) { 56 | std::unique_lock lck(data_mutex_); 57 | observations_.push_back(feature); 58 | observed_times_++; 59 | } 60 | 61 | void RemoveObservation(std::shared_ptr feat); 62 | 63 | std::list> GetObs() { 64 | std::unique_lock lck(data_mutex_); 65 | return observations_; 66 | } 67 | 68 | static MapPoint::Ptr CreateNewMappoint(); 69 | 70 | public: 71 | uint64_t id_{0}; 72 | bool is_outlier_{false}; 73 | Eigen::Vector3d pos_{Eigen::Vector3d::Zero()}; 74 | std::mutex data_mutex_; 75 | int observed_times_{0}; 76 | std::list> observations_; 77 | }; 78 | } // namespace common 79 | } // namespace stereo_camera_vo 80 | 81 | #endif // INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_POINT_H_ 82 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # stereo camera visual odometry 2 | 3 | > A very lightweight visual odometry with local bundle adjustment (BA). 4 | 5 | - Test running on [kitti dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php). Processing each 6 | frames with **20 ms**. 7 | 8 | ##

![run_kitti](https://github.com/lee-shun/big_files/blob/master/images/stereo_camera_vo_images/kitti_data_test.gif)

9 | 10 | - Test running on DJI M300 front stereo camera, the video is recorded around [Concordia University, Montreal, CA](https://www.google.com/maps/place/Pavillon+Ev+Building/@45.495413,-73.5798817,17z/data=!4m5!3m4!1s0x4cc91a6a337243b7:0x45e30c18a8a7df4a!8m2!3d45.495413!4d-73.577693). 11 | 12 | ##

![run_m300](https://github.com/lee-shun/big_files/blob/master/images/stereo_camera_vo_images/cut.gif)

13 | 14 | - Mapping with [octomap](https://octomap.github.io). 15 | 16 | ##

![mapping](https://github.com/lee-shun/big_files/blob/master/images/stereo_camera_vo_images/mapping.gif)

17 | 18 | **NOTE:** 19 | 20 | - Green frame indicates current camera pose, while the red and black point clouds are the active and inactive landmarks 21 | respectively. 22 | 23 | ## Features 24 | 25 | - Stereo Camera based visual odometry. 26 | - LK optical flow is used to track the keypoints. 27 | - Lost tracking then relocate. 28 | - Insert keyframes according to the keypoints number. 29 | - Local bundle adjustment. 30 | 31 | ## Dependencies 32 | 33 | - [g2o](https://github.com/RainerKuemmerle/g2o) 34 | 35 | > optimization in local bundle adjustment and frontend camera pose estimation. 36 | 37 | - [pangolin](https://github.com/stevenlovegrove/Pangolin) 38 | 39 | > visualization 40 | 41 | - [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) & [Sophus](https://github.com/strasdat/Sophus) 42 | 43 | > basic matrix operations and Lie Algebra operations. 44 | 45 | - [OpenCV 4](https://opencv.org/) 46 | > general image processing methods and basic data structures. 47 | 48 | ## Future Work 49 | 50 | Integrate with [forest fire detection system](https://github.com/lee-shun/forest_fire_detection_system) to locate in GPS 51 | denied environments. 52 | 53 | ## Acknowledgement 54 | 55 | [_"14 Lectures on Visual SLAM: From Theory to Practice"_ Xiang Gao](https://github.com/gaoxiang12/slambook-en) 56 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/module/local_BA.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: local_BA.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_MODULE_LOCAL_BA_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_MODULE_LOCAL_BA_H_ 18 | 19 | #include "stereo_camera_vo/common/camera.h" 20 | #include "stereo_camera_vo/common/map.h" 21 | #include "stereo_camera_vo/module/g2o_types.h" 22 | #include "stereo_camera_vo/common/feature.h" 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace stereo_camera_vo { 33 | namespace module { 34 | class LocalBA { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | typedef std::shared_ptr Ptr; 38 | 39 | /** 40 | * creat local BA thread and hangon 41 | * */ 42 | LocalBA(); 43 | 44 | ~LocalBA() { Detach(); } 45 | 46 | void SetCameras(common::Camera::Ptr left, common::Camera::Ptr right) { 47 | cam_left_ = left; 48 | cam_right_ = right; 49 | } 50 | 51 | void SetMap(std::shared_ptr map) { map_ = map; } 52 | 53 | /** 54 | * toggle one times of local BA optimization 55 | * */ 56 | void UpdateMap(); 57 | 58 | void Restart(); 59 | void Detach(); 60 | 61 | void Stop(); 62 | 63 | private: 64 | void ThreadLoop(); 65 | 66 | void UpdateChiTh( 67 | const std::map& edges_and_features, 68 | double* chi2_th); 69 | 70 | void Optimize(common::Map::KeyframesType& keyframes, 71 | common::Map::LandmarksType& landmarks); 72 | 73 | std::shared_ptr map_; 74 | std::thread local_BA_thread_; 75 | std::mutex data_mutex_; 76 | 77 | std::condition_variable map_update_; 78 | std::atomic local_BA_running_; 79 | 80 | common::Camera::Ptr cam_left_{nullptr}, cam_right_{nullptr}; 81 | }; 82 | } // namespace module 83 | } // namespace stereo_camera_vo 84 | 85 | #endif // INCLUDE_STEREO_CAMERA_VO_MODULE_LOCAL_BA_H_ 86 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/common/camera.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: camera.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-11 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_COMMON_CAMERA_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_COMMON_CAMERA_H_ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | namespace stereo_camera_vo { 26 | namespace common { 27 | struct Camera { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 30 | typedef std::shared_ptr Ptr; 31 | 32 | Camera(); 33 | 34 | Camera(double fx, double fy, double cx, double cy, double baseline, 35 | const Sophus::SE3d &pose) 36 | : fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose) { 37 | pose_inv_ = pose_.inverse(); 38 | } 39 | 40 | Sophus::SE3d pose() const { return pose_; } 41 | 42 | // return intrinsic matrix 43 | Eigen::Matrix3d K() const { 44 | Eigen::Matrix3d k; 45 | k << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1; 46 | return k; 47 | } 48 | 49 | // coordinate transform: world, camera, pixel 50 | Eigen::Vector3d world2camera(const Eigen::Vector3d &p_w, 51 | const Sophus::SE3d &T_c_w); 52 | 53 | Eigen::Vector3d camera2world(const Eigen::Vector3d &p_c, 54 | const Sophus::SE3d &T_c_w); 55 | 56 | Eigen::Vector2d camera2pixel(const Eigen::Vector3d &p_c); 57 | 58 | Eigen::Vector3d pixel2camera(const Eigen::Vector2d &p_p, double depth = 1); 59 | 60 | Eigen::Vector3d pixel2world(const Eigen::Vector2d &p_p, 61 | const Sophus::SE3d &T_c_w, double depth = 1); 62 | 63 | Eigen::Vector2d world2pixel(const Eigen::Vector3d &p_w, 64 | const Sophus::SE3d &T_c_w); 65 | 66 | public: 67 | double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0, 68 | baseline_ = 0; // Camera intrinsics 69 | Sophus::SE3d pose_; // extrinsic, from stereo camera to single camera 70 | Sophus::SE3d pose_inv_; // inverse of extrinsics 71 | }; 72 | } // namespace common 73 | } // namespace stereo_camera_vo 74 | 75 | #endif // INCLUDE_STEREO_CAMERA_VO_COMMON_CAMERA_H_ 76 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/common/map.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: map.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-10 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_H_ 18 | 19 | #include "stereo_camera_vo/common/frame.h" 20 | #include "stereo_camera_vo/common/map_point.h" 21 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace stereo_camera_vo { 30 | namespace common { 31 | class Map { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 34 | typedef std::shared_ptr Ptr; 35 | typedef std::unordered_map LandmarksType; 36 | typedef std::unordered_map KeyframesType; 37 | 38 | Map() {} 39 | 40 | ~Map() { PRINT_INFO("old map destroyed!"); } 41 | 42 | void InsertKeyFrame(Frame::Ptr frame); 43 | 44 | void InsertMapPoint(MapPoint::Ptr map_point); 45 | 46 | LandmarksType GetAllMapPoints() { 47 | std::unique_lock lck(data_mutex_); 48 | return landmarks_; 49 | } 50 | 51 | KeyframesType GetAllKeyFrames() { 52 | std::unique_lock lck(data_mutex_); 53 | return keyframes_; 54 | } 55 | 56 | LandmarksType GetActiveMapPoints() { 57 | std::unique_lock lck(data_mutex_); 58 | return active_landmarks_; 59 | } 60 | 61 | KeyframesType GetActiveKeyFrames() { 62 | std::unique_lock lck(data_mutex_); 63 | return active_keyframes_; 64 | } 65 | 66 | private: 67 | /** 68 | * Deactive the old KeyFrames 69 | * */ 70 | void RemoveOldKeyframe(KeyframesType& keyframes); 71 | 72 | /** 73 | * clean the map point that observations is 0 74 | * */ 75 | void CleanLandmarks(LandmarksType& landmarks); 76 | 77 | std::mutex data_mutex_; 78 | 79 | LandmarksType landmarks_; 80 | LandmarksType active_landmarks_; 81 | KeyframesType keyframes_; 82 | KeyframesType active_keyframes_; 83 | 84 | Frame::Ptr current_frame_{nullptr}; 85 | 86 | const u_int16_t num_active_keyframes_{7}; 87 | }; 88 | } // namespace common 89 | } // namespace stereo_camera_vo 90 | 91 | #endif // INCLUDE_STEREO_CAMERA_VO_COMMON_MAP_H_ 92 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/common/frame.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: frame.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-09 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_COMMON_FRAME_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_COMMON_FRAME_H_ 18 | 19 | // h 20 | #include 21 | 22 | // std cpp 23 | #include 24 | #include 25 | #include 26 | 27 | // hpp 28 | #include 29 | #include 30 | 31 | namespace stereo_camera_vo { 32 | namespace common { 33 | 34 | struct MapPoint; 35 | struct Feature; 36 | 37 | struct Frame { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 | 41 | typedef std::shared_ptr Ptr; 42 | 43 | Frame() {} 44 | 45 | Frame(uint64_t id, double time_stamp, const Sophus::SE3d &pose, 46 | const cv::Mat &left, const cv::Mat &right); 47 | 48 | // Note the pose is Tcw; 49 | Sophus::SE3d Pose() { 50 | std::unique_lock lck(data_mutex_); 51 | return pose_; 52 | } 53 | 54 | // Note the pose is Tcw; 55 | void SetPose(const Sophus::SE3d &pose) { 56 | std::unique_lock lck(data_mutex_); 57 | pose_ = pose; 58 | } 59 | 60 | void SetKeyFrame(); 61 | 62 | static std::shared_ptr CreateFrame(); 63 | 64 | public: 65 | uint64_t id_{0}; 66 | uint64_t keyframe_id_{0}; 67 | bool is_keyframe_{false}; 68 | bool use_init_pose_{false}; 69 | double time_stamp_; 70 | 71 | // Note the pose is Tcw; 72 | Sophus::SE3d pose_; 73 | std::mutex data_mutex_; 74 | 75 | cv::Mat left_img_, right_img_; 76 | 77 | public: 78 | std::vector>& GetFeaturesLeft() { 79 | std::unique_lock lck(data_mutex_); 80 | return features_left_; 81 | } 82 | std::vector>& GetFeaturesRight() { 83 | std::unique_lock lck(data_mutex_); 84 | return features_right_; 85 | } 86 | 87 | private: 88 | // extracted features in left image 89 | std::vector> features_left_; 90 | // corresponding features in right image, set to nullptr if no corresponding 91 | std::vector> features_right_; 92 | }; 93 | } // namespace common 94 | } // namespace stereo_camera_vo 95 | 96 | #endif // INCLUDE_STEREO_CAMERA_VO_COMMON_FRAME_H_ 97 | -------------------------------------------------------------------------------- /src/app/visual_odometry.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: visual_odometry.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-14 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/app/visual_odometry.h" 17 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 18 | #include "stereo_camera_vo/tool/system_lib.h" 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace stereo_camera_vo { 27 | namespace app { 28 | 29 | VisualOdometry::VisualOdometry(std::string frontend_config_path, 30 | tool::DatasetBase::Ptr dataset) 31 | : dataset_(dataset) { 32 | if (!dataset_->Init()) { 33 | return; 34 | } 35 | 36 | // read vo parameters from config file 37 | module::Frontend::Param frontend_param; 38 | 39 | YAML::Node node = YAML::LoadFile(frontend_config_path); 40 | frontend_param.num_features_ = tool::GetParam(node, "num_features", 200); 41 | frontend_param.num_features_init_ = 42 | tool::GetParam(node, "num_features_init", 100); 43 | frontend_param.num_features_tracking_ = 44 | tool::GetParam(node, "num_features_tracking", 50); 45 | frontend_param.num_features_tracking_bad_ = 46 | tool::GetParam(node, "num_features_tracking_bad", 40); 47 | frontend_param.num_features_needed_for_keyframe_ = 48 | tool::GetParam(node, "num_features_needed_for_keyframe", 80); 49 | 50 | // create frontend 51 | frontend_ = module::Frontend::Ptr(new module::Frontend( 52 | dataset_->GetCamera(0), dataset_->GetCamera(1), frontend_param, true)); 53 | } 54 | 55 | void VisualOdometry::Run(const uint64_t msleep) { 56 | while (1) { 57 | PRINT_INFO("VO is running!"); 58 | Step(); 59 | std::this_thread::sleep_for(std::chrono::milliseconds(msleep)); 60 | } 61 | frontend_->Stop(); 62 | PRINT_INFO("VO exit!"); 63 | } 64 | 65 | bool VisualOdometry::Step() { 66 | common::Frame::Ptr new_frame = common::Frame::CreateFrame(); 67 | if (!dataset_->NextFrame(new_frame)) { 68 | PRINT_INFO("end of vo!"); 69 | return false; 70 | } 71 | 72 | auto t1 = std::chrono::steady_clock::now(); 73 | std::cout << "pose before: \n" 74 | << new_frame->Pose().inverse().matrix() << std::endl; 75 | bool success = frontend_->AddFrame(new_frame); 76 | std::cout << "pose after: \n" 77 | << new_frame->Pose().inverse().matrix() << std::endl; 78 | auto t2 = std::chrono::steady_clock::now(); 79 | auto time_used = 80 | std::chrono::duration_cast>(t2 - t1); 81 | PRINT_INFO("VO cost time: %lf", time_used.count()); 82 | return success; 83 | } 84 | } /* namespace app */ 85 | } // namespace stereo_camera_vo 86 | -------------------------------------------------------------------------------- /src/tool/kitti_dataset.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: kitti_dataset.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-20 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/tool/kitti_dataset.h" 17 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace stereo_camera_vo { 28 | namespace tool { 29 | 30 | KittiDataset::KittiDataset(const std::string& dataset_path) 31 | : DatasetBase(dataset_path, 0) {} 32 | 33 | bool KittiDataset::Init() { 34 | std::ifstream fin(dataset_path_ + "/calib.txt"); 35 | if (!fin) { 36 | PRINT_ERROR("can not open %s in given path, no such file or directory!", 37 | (dataset_path_ + "/calib.txt").c_str()); 38 | return false; 39 | } 40 | 41 | for (int i = 0; i < 4; ++i) { 42 | char camera_name[3]; 43 | for (int k = 0; k < 3; ++k) { 44 | fin >> camera_name[k]; 45 | } 46 | double projection_data[12]; 47 | for (int k = 0; k < 12; ++k) { 48 | fin >> projection_data[k]; 49 | } 50 | Eigen::Matrix3d K; 51 | K << projection_data[0], projection_data[1], projection_data[2], 52 | projection_data[4], projection_data[5], projection_data[6], 53 | projection_data[8], projection_data[9], projection_data[10]; 54 | Eigen::Vector3d t; 55 | t << projection_data[3], projection_data[7], projection_data[11]; 56 | t = K.inverse() * t; 57 | // K = K * 0.5; 58 | 59 | common::Camera::Ptr new_camera( 60 | new common::Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2), t.norm(), 61 | Sophus::SE3d(Sophus::SO3d(), t))); 62 | cameras_.push_back(new_camera); 63 | } 64 | 65 | fin.close(); 66 | current_image_index_ = 0; 67 | return true; 68 | } 69 | 70 | bool KittiDataset::NextFrame(common::Frame::Ptr new_frame) { 71 | boost::format fmt("%s/image_%d/%06d.png"); 72 | cv::Mat image_left, image_right; 73 | // read images 74 | image_left = 75 | cv::imread((fmt % dataset_path_ % 0 % current_image_index_).str(), 76 | cv::IMREAD_GRAYSCALE); 77 | image_right = 78 | cv::imread((fmt % dataset_path_ % 1 % current_image_index_).str(), 79 | cv::IMREAD_GRAYSCALE); 80 | 81 | if (image_left.data == nullptr || image_right.data == nullptr) { 82 | PRINT_ERROR("cannot find images at index %d!", current_image_index_); 83 | return false; 84 | } 85 | 86 | cv::Mat image_left_resized, image_right_resized; 87 | cv::resize(image_left, image_left_resized, cv::Size(), 0.5, 0.5, 88 | cv::INTER_NEAREST); 89 | cv::resize(image_right, image_right_resized, cv::Size(), 0.5, 0.5, 90 | cv::INTER_NEAREST); 91 | 92 | new_frame->left_img_ = image_left; 93 | new_frame->right_img_ = image_right; 94 | current_image_index_++; 95 | 96 | return true; 97 | } 98 | 99 | } // namespace tool 100 | } // namespace stereo_camera_vo 101 | -------------------------------------------------------------------------------- /cmake_modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /src/common/map.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: map.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-10 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/common/map.h" 17 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 18 | #include "stereo_camera_vo/common/feature.h" 19 | 20 | namespace stereo_camera_vo { 21 | namespace common { 22 | void Map::InsertKeyFrame(Frame::Ptr frame) { 23 | std::unique_lock lck(data_mutex_); 24 | current_frame_ = frame; 25 | if (keyframes_.find(frame->keyframe_id_) == keyframes_.end()) { 26 | keyframes_.insert(make_pair(frame->keyframe_id_, frame)); 27 | active_keyframes_.insert(make_pair(frame->keyframe_id_, frame)); 28 | } else { 29 | keyframes_[frame->keyframe_id_] = frame; 30 | active_keyframes_[frame->keyframe_id_] = frame; 31 | } 32 | 33 | if (active_keyframes_.size() > num_active_keyframes_) { 34 | RemoveOldKeyframe(active_keyframes_); 35 | CleanLandmarks(active_landmarks_); 36 | } 37 | } 38 | 39 | void Map::InsertMapPoint(MapPoint::Ptr map_point) { 40 | std::unique_lock lck(data_mutex_); 41 | if (landmarks_.find(map_point->id_) == landmarks_.end()) { 42 | landmarks_.insert(make_pair(map_point->id_, map_point)); 43 | active_landmarks_.insert(make_pair(map_point->id_, map_point)); 44 | } else { 45 | landmarks_[map_point->id_] = map_point; 46 | active_landmarks_[map_point->id_] = map_point; 47 | } 48 | } 49 | 50 | void Map::RemoveOldKeyframe(KeyframesType& keyframes) { 51 | if (current_frame_ == nullptr) return; 52 | 53 | // find out the nearest and farest keyframe, use the distance 54 | double max_dis = 0, min_dis = 9999; 55 | double max_kf_id = 0, min_kf_id = 0; 56 | auto Twc = current_frame_->Pose().inverse(); 57 | 58 | for (auto& kf : keyframes) { 59 | if (kf.second == current_frame_) continue; 60 | auto dis = (kf.second->Pose() * Twc).log().norm(); 61 | if (dis > max_dis) { 62 | max_dis = dis; 63 | max_kf_id = kf.first; 64 | } 65 | if (dis < min_dis) { 66 | min_dis = dis; 67 | min_kf_id = kf.first; 68 | } 69 | } 70 | 71 | const double min_dis_th = 0.2; 72 | Frame::Ptr frame_to_remove = nullptr; 73 | if (min_dis < min_dis_th) { 74 | // if there exists very near frame, remove it first. 75 | frame_to_remove = keyframes.at(min_kf_id); 76 | } else { 77 | // remove the farest frame. 78 | frame_to_remove = keyframes.at(max_kf_id); 79 | } 80 | 81 | PRINT_INFO("remove keyframe %ld", frame_to_remove->keyframe_id_); 82 | keyframes.erase(frame_to_remove->keyframe_id_); 83 | 84 | // landmark observation (features) 85 | for (auto feat : frame_to_remove->GetFeaturesLeft()) { 86 | auto map_point = feat->map_point_.lock(); 87 | if (map_point) { 88 | map_point->RemoveObservation(feat); 89 | } 90 | } 91 | for (auto feat : frame_to_remove->GetFeaturesRight()) { 92 | if (feat == nullptr) continue; 93 | auto map_point = feat->map_point_.lock(); 94 | if (map_point) { 95 | map_point->RemoveObservation(feat); 96 | } 97 | } 98 | } 99 | 100 | void Map::CleanLandmarks(LandmarksType& landmarks) { 101 | int cnt_landmark_removed = 0; 102 | for (auto iter = landmarks.begin(); iter != landmarks.end();) { 103 | if (iter->second->observed_times_ == 0) { 104 | iter = landmarks.erase(iter); 105 | cnt_landmark_removed++; 106 | } else { 107 | ++iter; 108 | } 109 | } 110 | PRINT_INFO("removed %d no-observed landmarks", cnt_landmark_removed); 111 | } 112 | } // namespace common 113 | } // namespace stereo_camera_vo 114 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/module/frontend.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: frontend.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_MODULE_FRONTEND_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_MODULE_FRONTEND_H_ 18 | 19 | #include "stereo_camera_vo/common/frame.h" 20 | #include "stereo_camera_vo/common/map.h" 21 | #include "stereo_camera_vo/common/camera.h" 22 | #include "stereo_camera_vo/tool/viewer.h" 23 | #include "stereo_camera_vo/module/local_BA.h" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace stereo_camera_vo { 34 | namespace module { 35 | 36 | enum FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST }; 37 | 38 | class Frontend { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | typedef std::shared_ptr Ptr; 42 | 43 | struct Param { 44 | int num_features_{200}; 45 | int num_features_init_{100}; 46 | int num_features_tracking_{50}; 47 | int num_features_tracking_bad_{40}; 48 | int num_features_needed_for_keyframe_{80}; 49 | }; 50 | 51 | Frontend(common::Camera::Ptr left, common::Camera::Ptr right, 52 | const Param& param, bool use_viewer = true); 53 | 54 | bool AddFrame(common::Frame::Ptr frame); 55 | 56 | FrontendStatus GetStatus() const { return status_; } 57 | 58 | void Stop() { 59 | if (nullptr != local_BA_) local_BA_->Stop(); 60 | if (nullptr != viewer_) viewer_->Stop(); 61 | } 62 | 63 | private: 64 | /** 65 | * Track in normal mode 66 | * @return true if success 67 | */ 68 | bool Track(); 69 | 70 | /** 71 | * Reset when lost 72 | * @return true if success 73 | */ 74 | bool Reset(); 75 | 76 | /** 77 | * Track with last frame, get the keypoints in current frame. 78 | * use LK flow to estimate points in the current image 79 | * @return num of tracked points 80 | */ 81 | int TrackLastFrame(); 82 | 83 | /** 84 | * estimate current frame's pose 85 | * @return num of inliers 86 | */ 87 | int EstimateCurrentPose(); 88 | 89 | /** 90 | * set current frame as a keyframe and insert it into local_BA 91 | * @return true if success 92 | */ 93 | bool UpdateMapWithFrame(); 94 | 95 | /** 96 | * Try init the frontend with stereo images saved in current_frame_ 97 | * @return true if success 98 | */ 99 | bool StereoInit(); 100 | 101 | /** 102 | * Detect new features in left image in current_frame_ 103 | * keypoints will be saved in current_frame_ 104 | * @return the number of new features 105 | */ 106 | int DetectNewFeatures(); 107 | 108 | /** 109 | * Find the corresponding features in right image of current_frame_ 110 | * @return num of features found 111 | */ 112 | int FindFeaturesInRight(); 113 | 114 | /** 115 | * Build the initial map with single image 116 | * @return true if succeed 117 | */ 118 | bool BuildInitMap(); 119 | 120 | /** 121 | * Triangulate the 2D points in current frame 122 | * @return num of triangulated points 123 | */ 124 | int TriangulateNewPoints(); 125 | 126 | /** 127 | * Set the features in keyframe as new observation of the map points 128 | */ 129 | void SetObservationsForKeyFrame(); 130 | 131 | FrontendStatus status_{FrontendStatus::INITING}; 132 | 133 | common::Frame::Ptr current_frame_{nullptr}; 134 | common::Frame::Ptr last_frame_{nullptr}; 135 | 136 | common::Camera::Ptr camera_left_{nullptr}; 137 | common::Camera::Ptr camera_right_{nullptr}; 138 | 139 | common::Map::Ptr map_{nullptr}; 140 | std::shared_ptr local_BA_{nullptr}; 141 | std::shared_ptr viewer_{nullptr}; 142 | 143 | Sophus::SE3d relative_motion_; 144 | Sophus::SE3d last_success_pose; 145 | 146 | uint16_t num_tracking_inliers_{0}; // inliers, used for testing new keyframes 147 | 148 | // params 149 | Param param_; 150 | 151 | // utilities 152 | cv::Ptr cv_detector_; // feature detector in opencv 153 | }; 154 | } // namespace module 155 | } // namespace stereo_camera_vo 156 | 157 | #endif // INCLUDE_STEREO_CAMERA_VO_MODULE_FRONTEND_H_ 158 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/tool/print_ctrl_macro.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2021 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: print_ctrl_macro.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2021-11-10 11 | * 12 | * @Description: macros that print logs to console and file 13 | * 14 | *******************************************************************************/ 15 | 16 | /* NOTE: namespace can not control macros in Cpp! */ 17 | 18 | #ifndef INCLUDE_STEREO_CAMERA_VO_TOOL_PRINT_CTRL_MACRO_H_ 19 | #define INCLUDE_STEREO_CAMERA_VO_TOOL_PRINT_CTRL_MACRO_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #define PCM_NONE 0 27 | #define PCM_ERROR 1 28 | #define PCM_WARN 2 29 | #define PCM_INFO 3 30 | #define PCM_ENTRY 4 31 | #define PCM_DEBUG 5 32 | 33 | #define PCM_PRINT_LEVEL PCM_DEBUG 34 | 35 | #define PCM_FILENAME(x) strrchr(x, '/') ? strrchr(x, '/') + 1 : x 36 | 37 | /** 38 | * console output 39 | * */ 40 | 41 | /* color control */ 42 | #define PCM_COLOR(color, msg) "\033[0;1;" #color "m" msg "\033[0m" 43 | #define PCM_RED 31 44 | #define PCM_GREEN 32 45 | #define PCM_YELLOW 33 46 | #define PCM_PURPLE 34 47 | #define PCM_PINK 35 48 | #define PCM_BLUE 36 49 | 50 | #define PRINT_PURE(level, ...) \ 51 | do { \ 52 | if (level <= PCM_PRINT_LEVEL) { \ 53 | printf("[" #level "]>>" __VA_ARGS__); \ 54 | printf("\n"); \ 55 | } \ 56 | } while (0); 57 | 58 | #define PRINT(color, level, ...) \ 59 | do { \ 60 | if (level <= PCM_PRINT_LEVEL) { \ 61 | printf(PCM_COLOR(color, "[" #level "]")); \ 62 | printf(PCM_COLOR(36, " %s:%d (in %s) "), PCM_FILENAME(__FILE__), \ 63 | __LINE__, __FUNCTION__); \ 64 | printf(__VA_ARGS__); \ 65 | printf("\n"); \ 66 | } \ 67 | } while (0); 68 | 69 | #define PRINT_ERROR(...) \ 70 | do { \ 71 | PRINT(31, PCM_ERROR, ##__VA_ARGS__); \ 72 | } while (0); 73 | 74 | #define PRINT_WARN(...) \ 75 | do { \ 76 | PRINT(33, PCM_WARN, ##__VA_ARGS__); \ 77 | } while (0); 78 | 79 | #define PRINT_INFO(...) \ 80 | do { \ 81 | PRINT(32, PCM_INFO, ##__VA_ARGS__); \ 82 | } while (0); 83 | 84 | #define PRINT_ENTRY(...) \ 85 | do { \ 86 | PRINT(34, PCM_ENTRY, ##__VA_ARGS__); \ 87 | } while (0); 88 | 89 | #define PRINT_DEBUG(...) \ 90 | do { \ 91 | PRINT(35, PCM_DEBUG, ##__VA_ARGS__); \ 92 | } while (0); 93 | 94 | /** 95 | * file output 96 | * */ 97 | #define FPRINT(level, file_name, ...) \ 98 | do { \ 99 | FILE *file_fp = NULL; \ 100 | file_fp = fopen(file_name, "a"); \ 101 | \ 102 | if (level <= PCM_PRINT_LEVEL) { \ 103 | if (file_fp != NULL) { \ 104 | fprintf(file_fp, "[" #level "] [%s:%d|in %s], ", \ 105 | PCM_FILENAME(__FILE__), __LINE__, __FUNCTION__); \ 106 | fprintf(file_fp, __VA_ARGS__); \ 107 | fprintf(file_fp, "\n"); \ 108 | } else { \ 109 | PRINT_ERROR("Can not open file!"); \ 110 | } \ 111 | fclose(file_fp); \ 112 | } else { \ 113 | fclose(file_fp); \ 114 | } \ 115 | } while (0); 116 | 117 | #define FPRINT_ERROR(file_name, ...) \ 118 | do { \ 119 | FPRINT(PCM_ERROR, file_name, ##__VA_ARGS__); \ 120 | } while (0); 121 | 122 | #define FPRINT_WARN(file_name, ...) \ 123 | do { \ 124 | FPRINT(PCM_WARN, file_name, ##__VA_ARGS__); \ 125 | } while (0); 126 | 127 | #define FPRINT_INFO(file_name, ...) \ 128 | do { \ 129 | FPRINT(PCM_INFO, file_name, ##__VA_ARGS__); \ 130 | } while (0); 131 | 132 | #define FPRINT_ENTRY(file_name, ...) \ 133 | do { \ 134 | FPRINT(PCM_ENTRY, file_name, ##__VA_ARGS__); \ 135 | } while (0); 136 | 137 | #define FPRINT_DEBUG(file_name, ...) \ 138 | do { \ 139 | FPRINT(PCM_DEBUG, file_name, ##__VA_ARGS__); \ 140 | } while (0); 141 | 142 | #endif // INCLUDE_STEREO_CAMERA_VO_TOOL_PRINT_CTRL_MACRO_H_ 143 | -------------------------------------------------------------------------------- /include/stereo_camera_vo/module/g2o_types.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: g2o_types.h 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-13 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #ifndef INCLUDE_STEREO_CAMERA_VO_MODULE_G2O_TYPES_H_ 17 | #define INCLUDE_STEREO_CAMERA_VO_MODULE_G2O_TYPES_H_ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace stereo_camera_vo { 27 | namespace module { 28 | 29 | /** 30 | * pose vertex 31 | * */ 32 | class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 35 | 36 | void setToOriginImpl() override { _estimate = Sophus::SE3d(); } 37 | 38 | // left multiplication on Sophus::SE3d 39 | void oplusImpl(const double *update) override { 40 | Eigen::Matrix update_eigen; 41 | update_eigen << update[0], update[1], update[2], update[3], update[4], 42 | update[5]; 43 | _estimate = Sophus::SE3d::exp(update_eigen) * _estimate; 44 | } 45 | 46 | bool read(std::istream &in) override { return true; } 47 | 48 | bool write(std::ostream &out) const override { return true; } 49 | }; 50 | 51 | /** 52 | * landmarks vertex 53 | * */ 54 | class VertexXYZ : public g2o::BaseVertex<3, Eigen::Vector3d> { 55 | public: 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 57 | void setToOriginImpl() override { _estimate = Eigen::Vector3d::Zero(); } 58 | 59 | void oplusImpl(const double *update) override { 60 | _estimate[0] += update[0]; 61 | _estimate[1] += update[1]; 62 | _estimate[2] += update[2]; 63 | } 64 | 65 | bool read(std::istream &in) override { return true; } 66 | 67 | bool write(std::ostream &out) const override { return true; } 68 | }; 69 | 70 | /** 71 | * unary edge for pose 72 | * */ 73 | class EdgeProjectionPoseOnly 74 | : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> { 75 | public: 76 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 77 | 78 | EdgeProjectionPoseOnly(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) 79 | : _pos3d(pos), _K(K) {} 80 | 81 | void computeError() override { 82 | const VertexPose *v = static_cast(_vertices[0]); 83 | Sophus::SE3d T = v->estimate(); 84 | Eigen::Vector3d pos_pixel = _K * (T * _pos3d); 85 | pos_pixel /= pos_pixel[2]; 86 | _error = _measurement - pos_pixel.head<2>(); 87 | } 88 | 89 | void linearizeOplus() override { 90 | const VertexPose *v = static_cast(_vertices[0]); 91 | Sophus::SE3d T = v->estimate(); 92 | Eigen::Vector3d pos_cam = T * _pos3d; 93 | double fx = _K(0, 0); 94 | double fy = _K(1, 1); 95 | double X = pos_cam[0]; 96 | double Y = pos_cam[1]; 97 | double Z = pos_cam[2]; 98 | double Zinv = 1.0 / (Z + 1e-18); 99 | double Zinv2 = Zinv * Zinv; 100 | _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2, 101 | -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv, fy * Y * Zinv2, 102 | fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2, -fy * X * Zinv; 103 | } 104 | 105 | bool read(std::istream &in) override { return true; } 106 | 107 | bool write(std::ostream &out) const override { return true; } 108 | 109 | private: 110 | Eigen::Vector3d _pos3d; 111 | Eigen::Matrix3d _K; 112 | }; 113 | 114 | /** 115 | * unary edge for pose and landmarks 116 | * */ 117 | class EdgeProjection 118 | : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPose, VertexXYZ> { 119 | public: 120 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 121 | 122 | // intrinsic and extrinsic param 123 | EdgeProjection(const Eigen::Matrix3d &K, const Sophus::SE3d &cam_ext) 124 | : _K(K) { 125 | _cam_ext = cam_ext; 126 | } 127 | 128 | void computeError() override { 129 | const VertexPose *v0 = static_cast(_vertices[0]); 130 | const VertexXYZ *v1 = static_cast(_vertices[1]); 131 | Sophus::SE3d T = v0->estimate(); 132 | Eigen::Vector3d pos_pixel = _K * (_cam_ext * (T * v1->estimate())); 133 | pos_pixel /= pos_pixel[2]; 134 | _error = _measurement - pos_pixel.head<2>(); 135 | } 136 | 137 | void linearizeOplus() override { 138 | const VertexPose *v0 = static_cast(_vertices[0]); 139 | const VertexXYZ *v1 = static_cast(_vertices[1]); 140 | Sophus::SE3d T = v0->estimate(); 141 | Eigen::Vector3d pw = v1->estimate(); 142 | Eigen::Vector3d pos_cam = _cam_ext * T * pw; 143 | double fx = _K(0, 0); 144 | double fy = _K(1, 1); 145 | double X = pos_cam[0]; 146 | double Y = pos_cam[1]; 147 | double Z = pos_cam[2]; 148 | double Zinv = 1.0 / (Z + 1e-18); 149 | double Zinv2 = Zinv * Zinv; 150 | _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2, 151 | -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv, fy * Y * Zinv2, 152 | fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2, -fy * X * Zinv; 153 | 154 | _jacobianOplusXj = _jacobianOplusXi.block<2, 3>(0, 0) * 155 | _cam_ext.rotationMatrix() * T.rotationMatrix(); 156 | } 157 | 158 | bool read(std::istream &in) override { return true; } 159 | 160 | bool write(std::ostream &out) const override { return true; } 161 | 162 | private: 163 | Eigen::Matrix3d _K; 164 | Sophus::SE3d _cam_ext; 165 | }; 166 | } // namespace module 167 | } // namespace stereo_camera_vo 168 | 169 | #endif // INCLUDE_STEREO_CAMERA_VO_MODULE_G2O_TYPES_H_ 170 | -------------------------------------------------------------------------------- /src/tool/viewer.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: viewer.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-11 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/common/feature.h" 17 | #include "stereo_camera_vo/tool/viewer.h" 18 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace stereo_camera_vo { 28 | namespace tool { 29 | 30 | Viewer::Viewer() { 31 | viewer_running_.store(true); 32 | viewer_thread_ = std::thread(std::bind(&Viewer::ThreadLoop, this)); 33 | } 34 | 35 | void Viewer::Stop() { 36 | std::unique_lock lck(viewer_data_mutex_); 37 | viewer_running_.store(false); 38 | PRINT_INFO("stop current viewer! wait for join!"); 39 | viewer_thread_.join(); 40 | } 41 | 42 | void Viewer::AddCurrentFrame(common::Frame::Ptr current_frame) { 43 | std::unique_lock lck(viewer_data_mutex_); 44 | current_frame_ = current_frame; 45 | } 46 | 47 | void Viewer::UpdateMap() { 48 | std::unique_lock lck(viewer_data_mutex_); 49 | if (nullptr != map_) { 50 | keyframes_ = map_->GetAllKeyFrames(); 51 | landmarks_ = map_->GetAllMapPoints(); 52 | 53 | active_keyframes_ = map_->GetActiveKeyFrames(); 54 | active_landmarks_ = map_->GetActiveMapPoints(); 55 | } 56 | } 57 | 58 | void Viewer::ThreadLoop() { 59 | pangolin::CreateWindowAndBind("stereo_camera_vo_viewer", 1024, 768); 60 | glEnable(GL_DEPTH_TEST); 61 | glEnable(GL_BLEND); 62 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 63 | 64 | pangolin::OpenGlRenderState vis_camera( 65 | pangolin::ProjectionMatrix(1024, 768, 400, 400, 512, 384, 0.1, 1000), 66 | pangolin::ModelViewLookAt(0, -5, -10, 0, 0, 0, 0.0, -1.0, 0.0)); 67 | 68 | // Add named OpenGL viewport to window and provide 3D Handler 69 | pangolin::View& vis_display = 70 | pangolin::CreateDisplay() 71 | .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f) 72 | .SetHandler(new pangolin::Handler3D(vis_camera)); 73 | 74 | const float green[3] = {0, 1, 0}; 75 | while (!pangolin::ShouldQuit() && viewer_running_.load()) { 76 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 77 | glClearColor(1.0f, 1.0f, 1.0f, 1.0f); 78 | vis_display.Activate(vis_camera); 79 | 80 | std::unique_lock lock(viewer_data_mutex_); 81 | 82 | if (nullptr != current_frame_) { 83 | DrawFrame(current_frame_, green); 84 | FollowCurrentFrame(vis_camera); 85 | 86 | cv::Mat img = PlotFrameImage(); 87 | cv::imshow("image", img); 88 | cv::waitKey(1); 89 | } 90 | 91 | if (nullptr != map_) { 92 | DrawMap(); 93 | } 94 | 95 | pangolin::FinishFrame(); 96 | usleep(5000); 97 | } 98 | PRINT_INFO("stop viewer"); 99 | } 100 | 101 | cv::Mat Viewer::PlotFrameImage() { 102 | cv::Mat img_out; 103 | cv::cvtColor(current_frame_->left_img_, img_out, cv::COLOR_GRAY2BGR); 104 | 105 | for (size_t i = 0; i < current_frame_->GetFeaturesLeft().size(); ++i) { 106 | if (current_frame_->GetFeaturesLeft()[i]->map_point_.lock()) { 107 | auto feat = current_frame_->GetFeaturesLeft()[i]; 108 | cv::circle(img_out, feat->position_.pt, 2, cv::Scalar(0, 250, 0), 2); 109 | } 110 | } 111 | return img_out; 112 | } 113 | 114 | void Viewer::FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera) { 115 | Sophus::SE3d Twc = current_frame_->Pose().inverse(); 116 | pangolin::OpenGlMatrix m(Twc.matrix()); 117 | vis_camera.Follow(m, true); 118 | } 119 | 120 | void Viewer::DrawFrame(common::Frame::Ptr frame, const float* color) { 121 | Sophus::SE3d Twc = frame->Pose().inverse(); 122 | const float sz = 1.0; 123 | const int line_width = 2.0; 124 | const float fx = 400; 125 | const float fy = 400; 126 | const float cx = 512; 127 | const float cy = 384; 128 | const float width = 1080; 129 | const float height = 768; 130 | 131 | glPushMatrix(); 132 | 133 | Sophus::Matrix4f m = Twc.matrix().template cast(); 134 | glMultMatrixf(static_cast(m.data())); 135 | 136 | if (nullptr == color) { 137 | glColor3f(1, 0, 0); 138 | } else { 139 | glColor3f(color[0], color[1], color[2]); 140 | } 141 | 142 | glLineWidth(line_width); 143 | glBegin(GL_LINES); 144 | glVertex3f(0, 0, 0); 145 | glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); 146 | glVertex3f(0, 0, 0); 147 | glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 148 | glVertex3f(0, 0, 0); 149 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 150 | glVertex3f(0, 0, 0); 151 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); 152 | 153 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); 154 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 155 | 156 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 157 | glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 158 | 159 | glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); 160 | glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); 161 | 162 | glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); 163 | glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); 164 | 165 | glEnd(); 166 | glPopMatrix(); 167 | } 168 | 169 | void Viewer::DrawMap() { 170 | // draw all keyframes 171 | const float black[3] = {0, 0, 0}; 172 | const float red[3] = {1.0, 0, 0}; 173 | for (auto& kf : keyframes_) { 174 | if (active_keyframes_.find(kf.first) == active_keyframes_.end()) { 175 | // not in active_keyframes_ 176 | DrawFrame(kf.second, black); 177 | } else { 178 | DrawFrame(kf.second, red); 179 | } 180 | } 181 | glPointSize(2); 182 | glBegin(GL_POINTS); 183 | 184 | // draw landmarks_ 185 | for (auto& landmark : landmarks_) { 186 | auto pos = landmark.second->Pos(); 187 | if (active_landmarks_.find(landmark.first) == active_landmarks_.end()) { 188 | // not in active_landmarks_ 189 | glColor3f(black[0], black[1], black[2]); 190 | glVertex3d(pos[0], pos[1], pos[2]); 191 | } else { 192 | glColor3f(red[0], red[1], red[2]); 193 | glVertex3d(pos[0], pos[1], pos[2]); 194 | } 195 | } 196 | glEnd(); 197 | } 198 | } // namespace tool 199 | } // namespace stereo_camera_vo 200 | -------------------------------------------------------------------------------- /src/tool/m300_dataset.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: m300_dataset.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-21 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/tool/m300_dataset.h" 17 | #include "stereo_camera_vo/tool/image_preprocess.h" 18 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 19 | #include "stereo_camera_vo/tool/system_lib.h" 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace stereo_camera_vo { 28 | namespace tool { 29 | bool M300Dataset::Init() { 30 | /** 31 | * Step: get cameras 32 | * */ 33 | const std::string camera_param = 34 | dataset_path_ + "/m300_front_stereo_param.yaml"; 35 | camera_config_file_.open(camera_param, cv::FileStorage::READ); 36 | if (!camera_config_file_.isOpened()) { 37 | PRINT_ERROR("file %s is not opened", camera_param.c_str()); 38 | camera_config_file_.release(); 39 | return false; 40 | } 41 | cv::Mat param_proj_left = getParameter("leftProjectionMatrix"); 42 | cv::Mat param_proj_right = getParameter("rightProjectionMatrix"); 43 | 44 | Eigen::Matrix3d left_K, right_K; 45 | Eigen::Vector3d left_t, right_t; 46 | convert2Eigen(param_proj_left, &left_K, &left_t); 47 | convert2Eigen(param_proj_right, &right_K, &right_t); 48 | 49 | // left 50 | left_t = Eigen::Vector3d::Zero(); 51 | 52 | // Create new cameras 53 | common::Camera::Ptr left_cam = std::make_shared( 54 | left_K(0, 0), left_K(1, 1), left_K(0, 2), left_K(1, 2), left_t.norm(), 55 | Sophus::SE3d(Sophus::SO3d(), left_t)); 56 | cameras_.push_back(left_cam); 57 | 58 | common::Camera::Ptr right_cam = std::make_shared( 59 | right_K(0, 0), right_K(1, 1), right_K(0, 2), right_K(1, 2), 60 | right_t.norm(), Sophus::SE3d(Sophus::SO3d(), right_t)); 61 | cameras_.push_back(right_cam); 62 | 63 | PRINT_DEBUG("left camera fx: %.8f", left_K(0, 0)); 64 | PRINT_DEBUG("left camera fy: %.8f", left_K(1, 1)); 65 | PRINT_DEBUG("left camera cx: %.8f", left_K(0, 2)); 66 | PRINT_DEBUG("left camera cy: %.8f", left_K(1, 2)); 67 | PRINT_DEBUG("left camera baseline: %.8f", left_t.norm()); 68 | PRINT_DEBUG("left camera baseline*fx: %.8f", left_t.norm() * left_K(0, 0)); 69 | 70 | PRINT_DEBUG("right camera fx: %.8f", right_K(0, 0)); 71 | PRINT_DEBUG("right camera fy: %.8f", right_K(1, 1)); 72 | PRINT_DEBUG("right camera cx: %.8f", right_K(0, 2)); 73 | PRINT_DEBUG("right camera cy: %.8f", right_K(1, 2)); 74 | PRINT_DEBUG("right camera baseline: %.8f", right_t.norm()); 75 | PRINT_DEBUG("right camera baseline*fx: %.8f", right_t.norm() * right_K(0, 0)); 76 | 77 | return true; 78 | } 79 | 80 | bool M300Dataset::NextFrame(common::Frame::Ptr new_frame) { 81 | boost::format fmt("%s/image_%d/%d.png"); 82 | cv::Mat image_left, image_right; 83 | 84 | PRINT_DEBUG(" current_image_index_: %d", current_image_index_); 85 | 86 | // read images 87 | image_left = 88 | cv::imread((fmt % dataset_path_ % 0 % current_image_index_).str(), 89 | cv::IMREAD_GRAYSCALE); 90 | image_right = 91 | cv::imread((fmt % dataset_path_ % 1 % current_image_index_).str(), 92 | cv::IMREAD_GRAYSCALE); 93 | 94 | if (image_left.data == nullptr || image_right.data == nullptr) { 95 | PRINT_WARN("can not find images at index %d", current_image_index_); 96 | return false; 97 | } 98 | 99 | tool::GammaTransform(image_left); 100 | tool::GammaTransform(image_right); 101 | 102 | new_frame->left_img_ = image_left; 103 | new_frame->right_img_ = image_right; 104 | 105 | /* 106 | * Step: read pose 107 | * */ 108 | Eigen::Quaterniond att_body; 109 | if (!GetAttByIndex(dataset_path_ + "/pose.txt", current_image_index_ - 1, 110 | &att_body)) { 111 | return false; 112 | } 113 | 114 | Sophus::SE3d pose_body_Twb(att_body, Eigen::Vector3d::Zero()); 115 | Sophus::SE3d pose_cam_Tcw = Twb2Twc(pose_body_Twb).inverse(); 116 | 117 | if (first_frame) { 118 | first_frame_pose_Tcw_ = pose_cam_Tcw; 119 | std::cout << "First frame absolute pose Tcw:\n" 120 | << first_frame_pose_Tcw_.matrix() << std::endl; 121 | first_frame = false; 122 | } 123 | 124 | // get current frame pose (it is actually relative motion according to the 125 | // first frame ...) 126 | Sophus::SE3d realtive_pose_Tcw = 127 | pose_cam_Tcw * first_frame_pose_Tcw_.inverse(); 128 | 129 | new_frame->use_init_pose_ = true; 130 | new_frame->SetPose(realtive_pose_Tcw); 131 | 132 | ++current_image_index_; 133 | 134 | return true; 135 | } 136 | 137 | void M300Dataset::convert2Eigen(const cv::Mat proj, Eigen::Matrix3d* K, 138 | Eigen::Vector3d* t) { 139 | (*K) << proj.at(0, 0), proj.at(0, 1), proj.at(0, 2), 140 | proj.at(1, 0), proj.at(1, 1), proj.at(1, 2), 141 | proj.at(2, 0), proj.at(2, 1), proj.at(2, 2); 142 | 143 | (*t) << proj.at(0, 3), proj.at(1, 3), proj.at(2, 3); 144 | 145 | (*t) = (*K).inverse() * (*t); 146 | } 147 | 148 | Sophus::SE3d M300Dataset::Twb2Twc(const Sophus::SE3d& Twb) const { 149 | Eigen::Quaterniond rotate_quat_bc; 150 | 151 | rotate_quat_bc.w() = 0.5f; 152 | rotate_quat_bc.x() = -0.5f; 153 | rotate_quat_bc.y() = 0.5f; 154 | rotate_quat_bc.z() = -0.5f; 155 | 156 | // camera and body coordinate only have a rotation between them... 157 | Sophus::SE3d Tbc(rotate_quat_bc, Eigen::Vector3d::Zero()); 158 | 159 | return Twb * Tbc; 160 | } 161 | 162 | bool M300Dataset::GetAttByIndex(const std::string pose_path, 163 | const uint16_t pose_index, 164 | Eigen::Quaterniond* att) { 165 | std::ifstream pose_fin_; 166 | pose_fin_.open(pose_path); 167 | if (!pose_fin_) { 168 | PRINT_ERROR("can not open %s in given path, no such file or directory!", 169 | pose_path.c_str()); 170 | return false; 171 | } 172 | 173 | std::string pose_tmp; 174 | std::vector q_elements; 175 | stereo_camera_vo::tool::SeekToLine(pose_fin_, pose_index); 176 | // read each w, x, y, z, everytime 177 | for (int i = 0; i < 4; ++i) { 178 | if (!getline(pose_fin_, pose_tmp, ',')) { 179 | PRINT_ERROR("pose reading error! at index %d", pose_index); 180 | return false; 181 | } 182 | // PRINT_DEBUG("read pose-wxyz:%.8f", std::stod(pose_tmp)); 183 | q_elements.push_back(std::stod(pose_tmp)); 184 | } 185 | 186 | *att = Eigen::Quaterniond(q_elements[0], q_elements[1], q_elements[2], 187 | q_elements[3]); 188 | 189 | return true; 190 | } 191 | 192 | } // namespace tool 193 | } // namespace stereo_camera_vo 194 | -------------------------------------------------------------------------------- /src/module/local_BA.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: local_BA.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-13 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/module/local_BA.h" 17 | #include "stereo_camera_vo/tool/algorithm.h" 18 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace stereo_camera_vo { 26 | namespace module { 27 | 28 | // TODO(lee-shun): this local BA has memory leak problems??? 29 | 30 | LocalBA::LocalBA() { 31 | local_BA_thread_ = std::thread(std::bind(&LocalBA::ThreadLoop, this)); 32 | local_BA_running_.store(true); 33 | } 34 | 35 | void LocalBA::UpdateMap() { 36 | std::unique_lock lock(data_mutex_); 37 | map_update_.notify_one(); 38 | } 39 | 40 | void LocalBA::Restart() { 41 | Detach(); 42 | local_BA_running_.store(true); 43 | local_BA_thread_ = std::thread(std::bind(&LocalBA::ThreadLoop, this)); 44 | } 45 | 46 | void LocalBA::Stop() { 47 | std::unique_lock lock(data_mutex_); 48 | // release camera and map 49 | cam_left_ = nullptr; 50 | cam_right_ = nullptr; 51 | map_ = nullptr; 52 | 53 | local_BA_running_.store(false); 54 | map_update_.notify_one(); 55 | 56 | PRINT_INFO("stop current local BA! wait for join!"); 57 | local_BA_thread_.join(); 58 | } 59 | 60 | void LocalBA::Detach() { 61 | std::unique_lock lock(data_mutex_); 62 | // release camera and map 63 | cam_left_ = nullptr; 64 | cam_right_ = nullptr; 65 | map_ = nullptr; 66 | 67 | local_BA_running_.store(false); 68 | map_update_.notify_one(); 69 | 70 | PRINT_INFO("detach current local BA!"); 71 | local_BA_thread_.detach(); 72 | } 73 | 74 | void LocalBA::ThreadLoop() { 75 | while (local_BA_running_.load()) { 76 | std::unique_lock lock(data_mutex_); 77 | map_update_.wait(lock); 78 | 79 | // local BA optimize active ONLY 80 | if (nullptr != map_) { 81 | common::Map::KeyframesType active_kfs = map_->GetActiveKeyFrames(); 82 | common::Map::LandmarksType active_landmarks = map_->GetActiveMapPoints(); 83 | Optimize(active_kfs, active_landmarks); 84 | } 85 | } 86 | } 87 | 88 | void LocalBA::Optimize(common::Map::KeyframesType &keyframes, 89 | common::Map::LandmarksType &landmarks) { 90 | // setup g2o 91 | typedef g2o::BlockSolver_6_3 BlockSolverType; 92 | typedef g2o::LinearSolverCSparse 93 | LinearSolverType; 94 | auto solver = new g2o::OptimizationAlgorithmLevenberg( 95 | g2o::make_unique(g2o::make_unique())); 96 | g2o::SparseOptimizer optimizer; 97 | optimizer.setAlgorithm(solver); 98 | 99 | /** 100 | * Step: camera pose vertices bind with keyframe_id 101 | * */ 102 | 103 | std::map veretx_pose_set; 104 | uint64_t max_kf_id = 0; 105 | for (auto &keyframe : keyframes) { 106 | auto kf = keyframe.second; 107 | VertexPose *vertex_pose = new VertexPose(); 108 | 109 | vertex_pose->setId(kf->keyframe_id_); 110 | vertex_pose->setEstimate(kf->Pose()); 111 | optimizer.addVertex(vertex_pose); 112 | 113 | if (kf->keyframe_id_ > max_kf_id) { 114 | max_kf_id = kf->keyframe_id_; 115 | } 116 | 117 | veretx_pose_set.insert({kf->keyframe_id_, vertex_pose}); 118 | } 119 | 120 | /** 121 | * Step: landmark vertex and edges (observation (features) for each landmark) 122 | * */ 123 | 124 | // each edge is each observation == each feature. 125 | std::map vertex_ldmk_set; 126 | std::map edges_and_features; 127 | 128 | Eigen::Matrix3d K = cam_left_->K(); 129 | Sophus::SE3d left_ext = cam_left_->pose(); 130 | Sophus::SE3d right_ext = cam_right_->pose(); 131 | 132 | double chi2_th = 5.991; 133 | int obs_index = 1; 134 | 135 | for (auto &ldmk : landmarks) { 136 | if (ldmk.second->is_outlier_) continue; 137 | 138 | // PRINT_DEBUG("ldmk.first == ldmk.second->id_ ? : %d", 139 | // (ldmk.first == ldmk.second->id_)); 140 | uint64_t landmark_id = ldmk.second->id_; 141 | auto observations = ldmk.second->GetObs(); 142 | 143 | // add position vertex for each landmark 144 | if (vertex_ldmk_set.find(landmark_id) == vertex_ldmk_set.end()) { 145 | VertexXYZ *v = new VertexXYZ; 146 | v->setEstimate(ldmk.second->Pos()); 147 | v->setId(landmark_id + max_kf_id + 1); 148 | v->setMarginalized(true); 149 | vertex_ldmk_set.insert({landmark_id, v}); 150 | optimizer.addVertex(v); 151 | } 152 | 153 | // add observations(edges) for each landmark and frame pose 154 | for (auto &obs : observations) { 155 | if (obs.lock() == nullptr) continue; 156 | auto feat = obs.lock(); 157 | if (feat->is_outlier_ || feat->frame_.lock() == nullptr || 158 | feat->map_point_.lock() == nullptr) 159 | continue; 160 | 161 | auto frame = feat->frame_.lock(); 162 | if (veretx_pose_set.find(frame->keyframe_id_) == veretx_pose_set.end()) 163 | continue; 164 | 165 | EdgeProjection *edge = nullptr; 166 | if (feat->is_on_left_image_) { 167 | edge = new EdgeProjection(K, left_ext); 168 | } else { 169 | edge = new EdgeProjection(K, right_ext); 170 | } 171 | edge->setId(obs_index); 172 | edge->setVertex(0, veretx_pose_set.at(frame->keyframe_id_)); // pose 173 | edge->setVertex(1, vertex_ldmk_set.at(landmark_id)); // landmark 174 | edge->setMeasurement(tool::ToVec2(feat->position_.pt)); 175 | edge->setInformation(Eigen::Matrix2d::Identity()); 176 | auto rk = new g2o::RobustKernelHuber(); 177 | rk->setDelta(chi2_th); 178 | edge->setRobustKernel(rk); 179 | edges_and_features.insert({edge, feat}); 180 | optimizer.addEdge(edge); 181 | obs_index++; 182 | } 183 | } 184 | 185 | // no edges! 186 | if (obs_index == 1) return; 187 | 188 | // do optimization and eliminate the outliers 189 | optimizer.initializeOptimization(); 190 | optimizer.optimize(10); 191 | 192 | UpdateChiTh(edges_and_features, &chi2_th); 193 | 194 | for (auto &ef : edges_and_features) { 195 | if (ef.first->chi2() > chi2_th) { 196 | ef.second->is_outlier_ = true; 197 | // remove the observation 198 | if (nullptr != ef.second->map_point_.lock()) 199 | ef.second->map_point_.lock()->RemoveObservation(ef.second); 200 | } else { 201 | ef.second->is_outlier_ = false; 202 | } 203 | } 204 | 205 | // Set pose and lanrmark position 206 | for (auto &v : veretx_pose_set) { 207 | keyframes.at(v.first)->SetPose(v.second->estimate()); 208 | } 209 | for (auto &v : vertex_ldmk_set) { 210 | landmarks.at(v.first)->SetPos(v.second->estimate()); 211 | } 212 | } 213 | 214 | void LocalBA::UpdateChiTh( 215 | const std::map &edges_and_features, 216 | double *chi2_th) { 217 | int cnt_outlier = 0, cnt_inlier = 0; 218 | int iteration = 0; 219 | 220 | while (iteration < 5) { 221 | cnt_outlier = 0; 222 | cnt_inlier = 0; 223 | // determine if we want to adjust the outlier threshold 224 | for (auto &ef : edges_and_features) { 225 | if (ef.first->chi2() > *chi2_th) { 226 | cnt_outlier++; 227 | } else { 228 | cnt_inlier++; 229 | } 230 | } 231 | double inlier_ratio = 232 | cnt_inlier / static_cast(cnt_inlier + cnt_outlier); 233 | if (inlier_ratio > 0.5) { 234 | break; 235 | } else { 236 | (*chi2_th) *= 2; 237 | iteration++; 238 | } 239 | } 240 | 241 | PRINT_INFO("outlier/inlier in pose estimating: %d/%d", cnt_outlier, 242 | cnt_inlier); 243 | } 244 | 245 | } // namespace module 246 | } // namespace stereo_camera_vo 247 | -------------------------------------------------------------------------------- /cmake_modules/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindGlog.cmake - Find Google glog logging library. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # GLOG_FOUND: TRUE iff glog is found. 37 | # GLOG_INCLUDE_DIRS: Include directories for glog. 38 | # GLOG_LIBRARIES: Libraries required to link glog. 39 | # 40 | # The following variables control the behaviour of this module: 41 | # 42 | # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to 43 | # search for glog includes, e.g: /timbuktu/include. 44 | # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to 45 | # search for glog libraries, e.g: /timbuktu/lib. 46 | # 47 | # The following variables are also defined by this module, but in line with 48 | # CMake recommended FindPackage() module style should NOT be referenced directly 49 | # by callers (use the plural variables detailed above instead). These variables 50 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 51 | # are NOT re-called (i.e. search for library is not repeated) if these variables 52 | # are set with valid values _in the CMake cache_. This means that if these 53 | # variables are set directly in the cache, either by the user in the CMake GUI, 54 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 55 | # explicitly defines a cache variable), then they will be used verbatim, 56 | # bypassing the HINTS variables and other hard-coded search locations. 57 | # 58 | # GLOG_INCLUDE_DIR: Include directory for glog, not including the 59 | # include directory of any dependencies. 60 | # GLOG_LIBRARY: glog library, not including the libraries of any 61 | # dependencies. 62 | 63 | # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when 64 | # FindGlog was invoked. 65 | macro(GLOG_RESET_FIND_LIBRARY_PREFIX) 66 | if (MSVC) 67 | set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") 68 | endif (MSVC) 69 | endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) 70 | 71 | # Called if we failed to find glog or any of it's required dependencies, 72 | # unsets all public (designed to be used externally) variables and reports 73 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 74 | macro(GLOG_REPORT_NOT_FOUND REASON_MSG) 75 | unset(GLOG_FOUND) 76 | unset(GLOG_INCLUDE_DIRS) 77 | unset(GLOG_LIBRARIES) 78 | # Make results of search visible in the CMake GUI if glog has not 79 | # been found so that user does not have to toggle to advanced view. 80 | mark_as_advanced(CLEAR GLOG_INCLUDE_DIR 81 | GLOG_LIBRARY) 82 | 83 | glog_reset_find_library_prefix() 84 | 85 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 86 | # use the camelcase library name, not uppercase. 87 | if (Glog_FIND_QUIETLY) 88 | message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) 89 | elseif (Glog_FIND_REQUIRED) 90 | message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) 91 | else () 92 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 93 | # but continues configuration and allows generation. 94 | message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) 95 | endif () 96 | endmacro(GLOG_REPORT_NOT_FOUND) 97 | 98 | # Handle possible presence of lib prefix for libraries on MSVC, see 99 | # also GLOG_RESET_FIND_LIBRARY_PREFIX(). 100 | if (MSVC) 101 | # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES 102 | # s/t we can set it back before returning. 103 | set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") 104 | # The empty string in this list is important, it represents the case when 105 | # the libraries have no prefix (shared libraries / DLLs). 106 | set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") 107 | endif (MSVC) 108 | 109 | # Search user-installed locations first, so that we prefer user installs 110 | # to system installs where both exist. 111 | list(APPEND GLOG_CHECK_INCLUDE_DIRS 112 | /usr/local/include 113 | /usr/local/homebrew/include # Mac OS X 114 | /opt/local/var/macports/software # Mac OS X. 115 | /opt/local/include 116 | /usr/include) 117 | # Windows (for C:/Program Files prefix). 118 | list(APPEND GLOG_CHECK_PATH_SUFFIXES 119 | glog/include 120 | glog/Include 121 | Glog/include 122 | Glog/Include) 123 | 124 | list(APPEND GLOG_CHECK_LIBRARY_DIRS 125 | /usr/local/lib 126 | /usr/local/homebrew/lib # Mac OS X. 127 | /opt/local/lib 128 | /usr/lib) 129 | # Windows (for C:/Program Files prefix). 130 | list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES 131 | glog/lib 132 | glog/Lib 133 | Glog/lib 134 | Glog/Lib) 135 | 136 | # Search supplied hint directories first if supplied. 137 | find_path(GLOG_INCLUDE_DIR 138 | NAMES glog/logging.h 139 | PATHS ${GLOG_INCLUDE_DIR_HINTS} 140 | ${GLOG_CHECK_INCLUDE_DIRS} 141 | PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) 142 | if (NOT GLOG_INCLUDE_DIR OR 143 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 144 | glog_report_not_found( 145 | "Could not find glog include directory, set GLOG_INCLUDE_DIR " 146 | "to directory containing glog/logging.h") 147 | endif (NOT GLOG_INCLUDE_DIR OR 148 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 149 | 150 | find_library(GLOG_LIBRARY NAMES glog 151 | PATHS ${GLOG_LIBRARY_DIR_HINTS} 152 | ${GLOG_CHECK_LIBRARY_DIRS} 153 | PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) 154 | if (NOT GLOG_LIBRARY OR 155 | NOT EXISTS ${GLOG_LIBRARY}) 156 | glog_report_not_found( 157 | "Could not find glog library, set GLOG_LIBRARY " 158 | "to full path to libglog.") 159 | endif (NOT GLOG_LIBRARY OR 160 | NOT EXISTS ${GLOG_LIBRARY}) 161 | 162 | # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets 163 | # if called. 164 | set(GLOG_FOUND TRUE) 165 | 166 | # Glog does not seem to provide any record of the version in its 167 | # source tree, thus cannot extract version. 168 | 169 | # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and 170 | # thus FIND_[PATH/LIBRARY] are not called, but specified locations are 171 | # invalid, otherwise we would report the library as found. 172 | if (GLOG_INCLUDE_DIR AND 173 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 174 | glog_report_not_found( 175 | "Caller defined GLOG_INCLUDE_DIR:" 176 | " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") 177 | endif (GLOG_INCLUDE_DIR AND 178 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 179 | # TODO: This regex for glog library is pretty primitive, we use lowercase 180 | # for comparison to handle Windows using CamelCase library names, could 181 | # this check be better? 182 | string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) 183 | if (GLOG_LIBRARY AND 184 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 185 | glog_report_not_found( 186 | "Caller defined GLOG_LIBRARY: " 187 | "${GLOG_LIBRARY} does not match glog.") 188 | endif (GLOG_LIBRARY AND 189 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 190 | 191 | # Set standard CMake FindPackage variables if found. 192 | if (GLOG_FOUND) 193 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 194 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 195 | endif (GLOG_FOUND) 196 | 197 | glog_reset_find_library_prefix() 198 | 199 | # Handle REQUIRED / QUIET optional arguments. 200 | include(FindPackageHandleStandardArgs) 201 | find_package_handle_standard_args(Glog DEFAULT_MSG 202 | GLOG_INCLUDE_DIRS GLOG_LIBRARIES) 203 | 204 | # Only mark internal variables as advanced if we found glog, otherwise 205 | # leave them visible in the standard GUI for the user to set manually. 206 | if (GLOG_FOUND) 207 | mark_as_advanced(FORCE GLOG_INCLUDE_DIR 208 | GLOG_LIBRARY) 209 | endif (GLOG_FOUND) 210 | -------------------------------------------------------------------------------- /src/module/frontend.cc: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2022 Concordia NAVlab. All rights reserved. 3 | * 4 | * @Filename: frontend.cc 5 | * 6 | * @Author: Shun Li 7 | * 8 | * @Email: 2015097272@qq.com 9 | * 10 | * @Date: 2022-03-12 11 | * 12 | * @Description: 13 | * 14 | *******************************************************************************/ 15 | 16 | #include "stereo_camera_vo/module/frontend.h" 17 | #include "stereo_camera_vo/module/g2o_types.h" 18 | #include "stereo_camera_vo/common/feature.h" 19 | #include "stereo_camera_vo/tool/algorithm.h" 20 | #include "stereo_camera_vo/tool/print_ctrl_macro.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace stereo_camera_vo { 30 | namespace module { 31 | 32 | Frontend::Frontend(common::Camera::Ptr left, common::Camera::Ptr right, 33 | const Param ¶m, bool use_viewer) 34 | : camera_left_(left), camera_right_(right), param_(param) { 35 | /** 36 | * init submodules, all smart pointers 37 | * */ 38 | cv_detector_ = cv::GFTTDetector::create(param_.num_features_, 0.001, 30); 39 | 40 | map_ = common::Map::Ptr(new common::Map); 41 | 42 | local_BA_ = module::LocalBA::Ptr(new module::LocalBA); 43 | local_BA_->SetMap(map_); 44 | local_BA_->SetCameras(camera_left_, camera_right_); 45 | 46 | if (use_viewer) { 47 | viewer_ = tool::Viewer::Ptr(new tool::Viewer); 48 | viewer_->SetMap(map_); 49 | } 50 | } 51 | 52 | bool Frontend::AddFrame(common::Frame::Ptr frame) { 53 | current_frame_ = frame; 54 | 55 | switch (status_) { 56 | case FrontendStatus::INITING: { 57 | Sophus::SE3d current_pose = 58 | Sophus::SE3d(current_frame_->Pose().rotationMatrix(), 59 | last_success_pose.translation()); 60 | current_frame_->SetPose(current_pose); 61 | StereoInit(); 62 | break; 63 | } 64 | case FrontendStatus::TRACKING_GOOD: 65 | case FrontendStatus::TRACKING_BAD: 66 | Track(); 67 | break; 68 | case FrontendStatus::LOST: 69 | Reset(); 70 | break; 71 | } 72 | 73 | last_frame_ = current_frame_; 74 | 75 | if (status_ == FrontendStatus::LOST) { 76 | return false; 77 | } 78 | return true; 79 | } 80 | 81 | bool Frontend::Track() { 82 | // TODO: use the outside rotatoion 83 | Sophus::SE3d current_pose; 84 | if (nullptr != last_frame_) { 85 | Sophus::SE3d current_estimate = relative_motion_ * last_frame_->Pose(); 86 | 87 | if (current_frame_->use_init_pose_) { 88 | current_pose = Sophus::SE3d(current_frame_->Pose().rotationMatrix(), 89 | current_estimate.translation()); 90 | } else { 91 | current_pose = current_estimate; 92 | } 93 | current_frame_->SetPose(current_pose); 94 | } 95 | 96 | TrackLastFrame(); 97 | num_tracking_inliers_ = EstimateCurrentPose(); 98 | 99 | if (num_tracking_inliers_ > param_.num_features_tracking_) { 100 | status_ = FrontendStatus::TRACKING_GOOD; 101 | } else if (num_tracking_inliers_ > param_.num_features_tracking_bad_) { 102 | status_ = FrontendStatus::TRACKING_BAD; 103 | } else { 104 | PRINT_WARN("track lost with features: %d", num_tracking_inliers_); 105 | status_ = FrontendStatus::LOST; 106 | } 107 | 108 | if (status_ == FrontendStatus::LOST) { 109 | return false; 110 | } 111 | 112 | UpdateMapWithFrame(); 113 | if (nullptr != viewer_) { 114 | viewer_->AddCurrentFrame(current_frame_); 115 | } 116 | 117 | last_success_pose = current_frame_->Pose(); 118 | relative_motion_ = current_frame_->Pose() * last_frame_->Pose().inverse(); 119 | 120 | return true; 121 | } 122 | 123 | int Frontend::TrackLastFrame() { 124 | // calculate the initial guess 125 | std::vector kps_last, kps_current; 126 | for (auto &feat : last_frame_->GetFeaturesLeft()) { 127 | if (feat->map_point_.lock()) { 128 | // use project point 129 | auto mp = feat->map_point_.lock(); 130 | auto px = camera_left_->world2pixel(mp->pos_, current_frame_->Pose()); 131 | kps_last.push_back(feat->position_.pt); 132 | kps_current.push_back(cv::Point2f(px[0], px[1])); 133 | } else { 134 | kps_last.push_back(feat->position_.pt); 135 | kps_current.push_back(feat->position_.pt); 136 | } 137 | } 138 | 139 | std::vector status; 140 | cv::Mat error; 141 | cv::calcOpticalFlowPyrLK( 142 | last_frame_->left_img_, current_frame_->left_img_, kps_last, kps_current, 143 | status, error, cv::Size(21, 21), 3, 144 | cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 145 | 0.01), 146 | cv::OPTFLOW_USE_INITIAL_FLOW); 147 | 148 | int num_good_pts = 0; 149 | 150 | // after tracked with last, set the current features to map points. if not, 151 | // leave it nullptr 152 | for (size_t i = 0; i < status.size(); ++i) { 153 | if (status[i]) { 154 | cv::KeyPoint kp(kps_current[i], 7); 155 | common::Feature::Ptr feature(new common::Feature(current_frame_, kp)); 156 | feature->map_point_ = last_frame_->GetFeaturesLeft()[i]->map_point_; 157 | current_frame_->GetFeaturesLeft().push_back(feature); 158 | num_good_pts++; 159 | } 160 | } 161 | 162 | PRINT_INFO("find %d good tracked points with last image!", num_good_pts); 163 | return num_good_pts; 164 | } 165 | 166 | // it is current pose 167 | int Frontend::EstimateCurrentPose() { 168 | // setup g2o 169 | typedef g2o::BlockSolver_6_3 BlockSolverType; 170 | typedef g2o::LinearSolverDense 171 | LinearSolverType; 172 | auto solver = new g2o::OptimizationAlgorithmLevenberg( 173 | g2o::make_unique(g2o::make_unique())); 174 | g2o::SparseOptimizer optimizer; 175 | optimizer.setAlgorithm(solver); 176 | 177 | // vertex 178 | VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose 179 | vertex_pose->setId(0); 180 | vertex_pose->setEstimate(current_frame_->Pose()); 181 | optimizer.addVertex(vertex_pose); 182 | 183 | // K 184 | Eigen::Matrix3d K = camera_left_->K(); 185 | 186 | // edges 187 | int index = 1; 188 | std::vector edges; 189 | std::vector cur_frame_features; 190 | for (size_t i = 0; i < current_frame_->GetFeaturesLeft().size(); ++i) { 191 | auto mp = current_frame_->GetFeaturesLeft()[i]->map_point_.lock(); 192 | if (nullptr != mp) { 193 | cur_frame_features.push_back(current_frame_->GetFeaturesLeft()[i]); 194 | EdgeProjectionPoseOnly *edge = new EdgeProjectionPoseOnly(mp->pos_, K); 195 | edge->setId(index); 196 | edge->setVertex(0, vertex_pose); 197 | edge->setMeasurement( 198 | tool::ToVec2(current_frame_->GetFeaturesLeft()[i]->position_.pt)); 199 | edge->setInformation(Eigen::Matrix2d::Identity()); 200 | edge->setRobustKernel(new g2o::RobustKernelHuber); 201 | edges.push_back(edge); 202 | optimizer.addEdge(edge); 203 | index++; 204 | } 205 | } 206 | 207 | PRINT_DEBUG("index: %d\n", index); 208 | 209 | // estimate the Pose the determine the outliers 210 | const double chi2_th = 5.95; 211 | int cnt_outlier = 0; 212 | for (int iteration = 0; iteration < 4; ++iteration) { 213 | vertex_pose->setEstimate(current_frame_->Pose()); 214 | // setLevel(int) is useful when you call 215 | // optimizer.initializeOptimization(int). If you assign 216 | // initializeOptimization(0), the optimizer will include all edges up to 217 | // level 0 in the optimization, and edges set to level >=1 will not be 218 | // included. 219 | optimizer.initializeOptimization(); 220 | optimizer.optimize(10); 221 | cnt_outlier = 0; 222 | 223 | // count the outliers 224 | for (size_t i = 0; i < edges.size(); ++i) { 225 | auto e = edges[i]; 226 | if (cur_frame_features[i]->is_outlier_) { 227 | e->computeError(); 228 | } 229 | if (e->chi2() > chi2_th) { 230 | cur_frame_features[i]->is_outlier_ = true; 231 | e->setLevel(1); 232 | cnt_outlier++; 233 | } else { 234 | cur_frame_features[i]->is_outlier_ = false; 235 | e->setLevel(0); 236 | } 237 | 238 | if (iteration == 2) { 239 | e->setRobustKernel(nullptr); 240 | } 241 | } 242 | } 243 | 244 | // if feature is outlier, remove its observations to map_point 245 | current_frame_->SetPose(vertex_pose->estimate()); 246 | for (auto &feat : cur_frame_features) { 247 | if (feat) { 248 | if (feat->is_outlier_) { 249 | feat->map_point_.reset(); 250 | feat->is_outlier_ = false; 251 | } 252 | } 253 | } 254 | 255 | PRINT_INFO("outlier/inlier in pose estimating: %d/%lu", cnt_outlier, 256 | cur_frame_features.size() - cnt_outlier); 257 | return cur_frame_features.size() - cnt_outlier; 258 | } 259 | 260 | // important 261 | bool Frontend::UpdateMapWithFrame() { 262 | if (num_tracking_inliers_ >= param_.num_features_needed_for_keyframe_) { 263 | // still have enough features, don't have potential to be a keyframe. 264 | PRINT_INFO("not keyframe now!"); 265 | return false; 266 | } 267 | // current frame is a new keyframe 268 | current_frame_->SetKeyFrame(); 269 | PRINT_INFO("set frame id: %lu to keyframe keyframe_id: %lu", 270 | current_frame_->id_, current_frame_->keyframe_id_); 271 | 272 | SetObservationsForKeyFrame(); 273 | 274 | // re-detect and add more map points for a keyframe if possible 275 | DetectNewFeatures(); 276 | FindFeaturesInRight(); 277 | TriangulateNewPoints(); 278 | 279 | map_->InsertKeyFrame(current_frame_); 280 | local_BA_->UpdateMap(); 281 | 282 | if (nullptr != viewer_) { 283 | viewer_->UpdateMap(); 284 | } 285 | return true; 286 | } 287 | 288 | void Frontend::SetObservationsForKeyFrame() { 289 | for (auto &feat : current_frame_->GetFeaturesLeft()) { 290 | auto mp = feat->map_point_.lock(); 291 | if (mp) mp->AddObservation(feat); 292 | } 293 | } 294 | 295 | int Frontend::TriangulateNewPoints() { 296 | std::vector poses{camera_left_->pose(), camera_right_->pose()}; 297 | Sophus::SE3d current_pose_Twc = current_frame_->Pose().inverse(); 298 | int cnt_triangulated_pts = 0; 299 | 300 | for (size_t i = 0; i < current_frame_->GetFeaturesLeft().size(); ++i) { 301 | // if features in left don't bind to a mappoint, at the meanwhile, right 302 | // image have corresponding features, Try to triangulate these new points. 303 | if (current_frame_->GetFeaturesLeft()[i]->map_point_.expired() && 304 | current_frame_->GetFeaturesRight()[i] != nullptr) { 305 | std::vector points{ 306 | camera_left_->pixel2camera(Eigen::Vector2d( 307 | current_frame_->GetFeaturesLeft()[i]->position_.pt.x, 308 | current_frame_->GetFeaturesLeft()[i]->position_.pt.y)), 309 | camera_right_->pixel2camera(Eigen::Vector2d( 310 | current_frame_->GetFeaturesRight()[i]->position_.pt.x, 311 | current_frame_->GetFeaturesRight()[i]->position_.pt.y))}; 312 | 313 | Eigen::Vector3d pworld = Eigen::Vector3d::Zero(); 314 | if (tool::Triangulation(poses, points, pworld) && pworld[2] > 0) { 315 | auto new_map_point = common::MapPoint::CreateNewMappoint(); 316 | pworld = current_pose_Twc * pworld; 317 | new_map_point->SetPos(pworld); 318 | new_map_point->AddObservation(current_frame_->GetFeaturesLeft()[i]); 319 | new_map_point->AddObservation(current_frame_->GetFeaturesRight()[i]); 320 | 321 | current_frame_->GetFeaturesLeft()[i]->map_point_ = new_map_point; 322 | current_frame_->GetFeaturesRight()[i]->map_point_ = new_map_point; 323 | map_->InsertMapPoint(new_map_point); 324 | cnt_triangulated_pts++; 325 | } 326 | } 327 | } 328 | PRINT_INFO("triangulate new landmarks: %d", cnt_triangulated_pts); 329 | return cnt_triangulated_pts; 330 | } 331 | 332 | // lock ok 333 | bool Frontend::StereoInit() { 334 | int num_features_left = DetectNewFeatures(); 335 | int num_features_right = FindFeaturesInRight(); 336 | if (num_features_right < param_.num_features_init_) { 337 | PRINT_WARN( 338 | "Due to inadequate features in right image, failed to init stereo, " 339 | "left features num: %d, right: %d (need %d)", 340 | num_features_left, num_features_right, param_.num_features_init_); 341 | 342 | return false; 343 | } 344 | 345 | if (BuildInitMap()) { 346 | status_ = FrontendStatus::TRACKING_GOOD; 347 | if (nullptr != viewer_) { 348 | viewer_->AddCurrentFrame(current_frame_); 349 | viewer_->UpdateMap(); 350 | } 351 | return true; 352 | } 353 | return false; 354 | } 355 | 356 | // this is new frame, this function only modify current frame. 357 | int Frontend::DetectNewFeatures() { 358 | // mask the old feature point position. 359 | cv::Mat mask(current_frame_->left_img_.size(), CV_8UC1, 255); 360 | for (auto &feat : current_frame_->GetFeaturesLeft()) { 361 | cv::rectangle(mask, feat->position_.pt - cv::Point2f(10, 10), 362 | feat->position_.pt + cv::Point2f(10, 10), 0, 363 | cv::LineTypes::FILLED); 364 | } 365 | 366 | std::vector keypoints; 367 | cv_detector_->detect(current_frame_->left_img_, keypoints, mask); 368 | 369 | int cnt_detected = 0; 370 | for (auto &kp : keypoints) { 371 | current_frame_->GetFeaturesLeft().push_back( 372 | common::Feature::Ptr(new common::Feature(current_frame_, kp))); 373 | cnt_detected++; 374 | } 375 | 376 | PRINT_INFO("detected %d new features in current image...", cnt_detected); 377 | return cnt_detected; 378 | } 379 | 380 | // this is new frame, this function only modify current frame. 381 | int Frontend::FindFeaturesInRight() { 382 | // use LK flow to estimate points in the right image 383 | std::vector kps_left, kps_right; 384 | 385 | // init the kps_left, and kps_right. 386 | for (auto &kp : current_frame_->GetFeaturesLeft()) { 387 | kps_left.push_back(kp->position_.pt); 388 | auto mp = kp->map_point_.lock(); 389 | if (mp) { 390 | // use projected points as initial guess 391 | auto px = camera_right_->world2pixel(mp->pos_, current_frame_->Pose()); 392 | kps_right.push_back(cv::Point2f(px[0], px[1])); 393 | } else { 394 | // use same pixel in left iamge 395 | kps_right.push_back(kp->position_.pt); 396 | } 397 | } 398 | 399 | std::vector status; 400 | cv::Mat error; 401 | cv::calcOpticalFlowPyrLK( 402 | current_frame_->left_img_, current_frame_->right_img_, kps_left, 403 | kps_right, status, error, cv::Size(21, 21), 3, 404 | cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 405 | 0.01), 406 | cv::OPTFLOW_USE_INITIAL_FLOW); 407 | 408 | int num_good_pts = 0; 409 | for (size_t i = 0; i < status.size(); ++i) { 410 | if (1 == status[i]) { 411 | cv::KeyPoint kp(kps_right[i], 7); 412 | common::Feature::Ptr feat(new common::Feature(current_frame_, kp)); 413 | feat->is_on_left_image_ = false; 414 | current_frame_->GetFeaturesRight().push_back(feat); 415 | num_good_pts++; 416 | } else { 417 | current_frame_->GetFeaturesRight().push_back(nullptr); 418 | } 419 | } 420 | PRINT_INFO("find %d good feature points via LK flow in right image.", 421 | num_good_pts); 422 | return num_good_pts; 423 | } 424 | 425 | // lock ok 426 | bool Frontend::BuildInitMap() { 427 | int cnt_init_landmarks = TriangulateNewPoints(); 428 | 429 | current_frame_->SetKeyFrame(); 430 | map_->InsertKeyFrame(current_frame_); 431 | local_BA_->UpdateMap(); 432 | 433 | PRINT_INFO("initial map created with %d map points!", cnt_init_landmarks); 434 | 435 | return true; 436 | } 437 | 438 | bool Frontend::Reset() { 439 | PRINT_WARN("reset VO!"); 440 | 441 | current_frame_ = nullptr; 442 | last_frame_ = nullptr; 443 | relative_motion_ = Sophus::SE3d(); 444 | map_ = common::Map::Ptr(new common::Map); 445 | 446 | local_BA_->Restart(); 447 | local_BA_->SetMap(map_); 448 | local_BA_->SetCameras(camera_left_, camera_right_); 449 | 450 | if (nullptr != viewer_) { 451 | viewer_->SetMap(map_); 452 | viewer_->AddCurrentFrame(current_frame_); 453 | viewer_->UpdateMap(); 454 | } 455 | 456 | status_ = FrontendStatus::INITING; 457 | return true; 458 | } 459 | 460 | } // namespace module 461 | } // namespace stereo_camera_vo 462 | --------------------------------------------------------------------------------