├── .gitignore ├── README.md ├── camera_model ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ └── camodocal │ │ ├── calib │ │ └── CameraCalibration.h │ │ ├── camera_models │ │ ├── Camera.h │ │ ├── CameraFactory.h │ │ ├── CataCamera.h │ │ ├── CostFunctionFactory.h │ │ ├── EquidistantCamera.h │ │ ├── PinholeCamera.h │ │ └── ScaramuzzaCamera.h │ │ ├── chessboard │ │ ├── Chessboard.h │ │ ├── ChessboardCorner.h │ │ ├── ChessboardQuad.h │ │ └── Spline.h │ │ ├── gpl │ │ ├── EigenQuaternionParameterization.h │ │ ├── EigenUtils.h │ │ └── gpl.h │ │ └── sparse_graph │ │ └── Transform.h ├── instruction ├── package.xml ├── readme.md └── src │ ├── calib │ └── CameraCalibration.cc │ ├── camera_models │ ├── Camera.cc │ ├── CameraFactory.cc │ ├── CataCamera.cc │ ├── CostFunctionFactory.cc │ ├── EquidistantCamera.cc │ ├── PinholeCamera.cc │ └── ScaramuzzaCamera.cc │ ├── chessboard │ └── Chessboard.cc │ ├── gpl │ ├── EigenQuaternionParameterization.cc │ └── gpl.cc │ ├── intrinsic_calib.cc │ └── sparse_graph │ └── Transform.cc ├── config ├── campus.rviz ├── fisheye_mask.jpg ├── fisheye_mask_752x480.jpg ├── indoor.rviz ├── openloris │ ├── openloris_vio.yaml │ └── openloris_vio_atlas.yaml ├── realsense │ ├── vio d455.yaml │ ├── vio.yaml │ ├── vio_atlas copy.yaml │ ├── vio_atlas.yaml │ ├── vio_campus.yaml │ └── vio_indoor.yaml ├── tagaided.rviz ├── tsinghua.rviz ├── tum_rgbd │ ├── tum_fr3.yaml │ └── tum_fr3_atlas.yaml └── video.rviz ├── doc ├── INSTALL.md └── RUNNING_PROCEDURE.md ├── output └── .gitignore ├── pose_graph ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── nodelet_plugin.xml ├── package.xml └── src │ ├── ThirdParty │ ├── DBoW │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── DBoW2.h │ │ ├── FBrief.cpp │ │ ├── FBrief.h │ │ ├── FClass.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── QueryResults.cpp │ │ ├── QueryResults.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ ├── TemplatedDatabase.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── DException.h │ │ ├── DUtils.h │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── DVision │ │ ├── BRIEF.cpp │ │ ├── BRIEF.h │ │ └── DVision.h │ ├── VocabularyBinary.cpp │ └── VocabularyBinary.hpp │ ├── keyframe │ ├── keyframe.cpp │ └── keyframe.h │ ├── pose_graph │ ├── pose_graph.cpp │ └── pose_graph.h │ ├── pose_graph_nodelet.cpp │ └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── parameters.h │ ├── tic_toc.h │ ├── utility.cpp │ └── utility.h ├── support_files ├── brief_k10L6.bin └── brief_pattern.yml └── vins_estimator ├── CMakeLists.txt ├── cmake └── FindEigen.cmake ├── launch ├── atlas200 │ ├── compressed2img.launch │ └── img2compressed.launch ├── openloris │ ├── openloris_vio_atlas.launch │ ├── openloris_vio_pytorch.launch │ ├── openloris_vio_tensorrt.launch │ └── openloris_vo.launch ├── realsense │ ├── l515.launch │ ├── realsense_vio.launch │ ├── realsense_vio_atlas.launch │ └── rs_camera.launch ├── tum_rgbd │ ├── tum_rgbd_atlas.launch │ ├── tum_rgbd_pytorch.launch │ └── tum_rgbd_tensorrt.launch ├── vins_rviz.launch └── yolo │ ├── atlas.launch │ ├── pytorch.launch │ └── tensorrt.launch ├── nodelet_description.xml ├── package.xml └── src ├── estimator ├── estimator.cpp └── estimator.h ├── estimator_nodelet.cpp ├── factor ├── imu_factor.h ├── imu_factor_modified.h ├── integration_base.h ├── marginalization_factor.cpp ├── marginalization_factor.h ├── pose_local_parameterization.cpp ├── pose_local_parameterization.h ├── projection_factor.cpp ├── projection_factor.h ├── projection_td_factor.cpp └── projection_td_factor.h ├── feature_manager ├── feature_manager.cpp └── feature_manager.h ├── feature_tracker ├── ThreadPool.h ├── feature_tracker.cpp └── feature_tracker.h ├── initial ├── initial_aligment.cpp ├── initial_alignment.h ├── initial_ex_rotation.cpp ├── initial_ex_rotation.h ├── initial_sfm.cpp ├── initial_sfm.h ├── solve_5pts.cpp └── solve_5pts.h └── utility ├── CameraPoseVisualization.cpp ├── CameraPoseVisualization.h ├── parameters.cpp ├── parameters.h ├── tic_toc.h ├── utility.cpp ├── utility.h ├── visualization.cpp └── visualization.h /.gitignore: -------------------------------------------------------------------------------- 1 | **/build 2 | **/devel 3 | **/.idea 4 | **/.vscode 5 | **/cmake-build-debug 6 | **/.cache 7 | 8 | !.gitignore 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VINS-RGBD-FAST 2 | 3 | **VINS-RGBD-FAST** is a SLAM system based on VINS-RGBD. We do some refinements to accelerate the system's performance in resource-constrained embedded paltform, like [HUAWEI Atlas200 DK](https://e.huawei.com/en/products/cloud-computing-dc/atlas/atlas-200), [NVIDIA Jetson AGX Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/). 4 | 5 | ## Refinements 6 | * grid-based feature detection 7 | * extract FAST feature instead of Harris feature 8 | * added stationary initialization 9 | * added IMU-aided feature tracking 10 | * added extracted-feature area's quality judgement 11 | * solved feature clusttering problem result frome FAST feature 12 | * use "sensor_msg::CompressedImage" as image topic type 13 | 14 | ## Related Paper: 15 | ``` 16 | @ARTICLE{9830851, 17 | author={Liu, Jianheng and Li, Xuanfu and Liu, Yueqian and Chen, Haoyao}, 18 | journal={IEEE Robotics and Automation Letters}, 19 | title={RGB-D Inertial Odometry for a Resource-Restricted Robot in Dynamic Environments}, 20 | year={2022}, 21 | volume={7}, 22 | number={4}, 23 | pages={9573-9580}, 24 | doi={10.1109/LRA.2022.3191193}} 25 | ``` 26 | 27 | ## RGBD-Inertial Trajectory Estimation and Mapping for Small Ground Rescue Robot 28 | 29 | Based one open source SLAM framework [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). 30 | 31 | The approach contains 32 | + Depth-integrated visual-inertial initialization process. 33 | + Visual-inertial odometry by utilizing depth information while avoiding the limitation is working for 3D pose estimation. 34 | + Noise elimination map which is suitable for path planning and navigation. 35 | 36 | However, the proposed approach can also be applied to other application like handheld and wheeled robot. 37 | 38 | ## 1. Prerequisites 39 | 1.1. **Ubuntu** 16.04 or 18.04. 40 | 41 | 1.2. **ROS** version Kinetic or Melodic fully installation 42 | 43 | 1.3. **Ceres Solver** 44 | Follow [Ceres Installation](http://ceres-solver.org/installation.html) 45 | 46 | 1.4. **Sophus** 47 | ``` 48 | git clone http://github.com/strasdat/Sophus.git 49 | git checkout a621ff 50 | ``` 51 | 52 | 1.3. **Atlas 200 DK环境配置** 53 | 54 | [https://blog.csdn.net/qq_42703283/article/details/110389270](https://blog.csdn.net/qq_42703283/article/details/110389270) 55 | 56 | 1.4. **ROS多机通信** 57 | 58 | [https://blog.csdn.net/qq_42703283/article/details/110408848]( 59 | 60 | ## 2. Datasets 61 | 62 | Recording by RealSense D435i. Contain 9 bags in three different applicaions: 63 | + [Handheld](https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/) 64 | + [Wheeled robot](https://star-center.shanghaitech.edu.cn/seafile/d/78c0375114854774b521/) ([Jackal](https://www.clearpathrobotics.com/jackal-small-unmanned-ground-vehicle/)) 65 | + [Tracked robot](https://star-center.shanghaitech.edu.cn/seafile/d/f611fc44df0c4b3d936d/) 66 | 67 | Note the rosbags are in compressed format. Use "rosbag decompress" to decompress. 68 | 69 | Topics: 70 | + depth topic: /camera/aligned_depth_to_color/image_raw 71 | + color topic: /camera/color/image_raw 72 | + imu topic: /camera/imu 73 | 74 | 我们使用的是压缩图像节点： 75 | 76 | + depth topic: /camera/aligned_depth_to_color/image_raw 77 | + color topic: /camera/color/image_raw/compressed 78 | + imu topic: /camera/imu 79 | 80 | 如何录制一个数据包 81 | 82 | 1. 运行d435i 83 | 2. `rosbag record /camera/imu /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/color/image_raw/compressed` 84 | 85 | 86 | ## 3. Run with Docker 87 | 88 | make Dockerfile like below 89 | ```c 90 | FROM ros:melodic-ros-core-bionic 91 | 92 | # apt-get update 93 | RUN apt-get update 94 | 95 | # install essentials 96 | RUN apt install -y gcc 97 | RUN apt install -y g++ 98 | RUN apt-get install -y cmake 99 | RUN apt-get install -y wget 100 | RUN apt install -y git 101 | 102 | # install ceres 103 | WORKDIR /home 104 | RUN apt-get install -y libgoogle-glog-dev libgflags-dev 105 | RUN apt-get install -y libatlas-base-dev 106 | RUN apt-get install -y libeigen3-dev 107 | RUN apt-get install -y libsuitesparse-dev 108 | RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz 109 | RUN tar zxf ceres-solver-2.1.0.tar.gz 110 | WORKDIR /home/ceres-solver-2.1.0 111 | RUN mkdir build 112 | WORKDIR /home/ceres-solver-2.1.0/build 113 | RUN cmake .. 114 | RUN make 115 | RUN make install 116 | 117 | # install sophus 118 | WORKDIR /home 119 | RUN git clone https://github.com/demul/Sophus.git 120 | WORKDIR /home/Sophus 121 | RUN git checkout fix/unit_complex_eror 122 | RUN mkdir build 123 | WORKDIR /home/Sophus/build 124 | RUN cmake .. 125 | RUN make 126 | RUN make install 127 | 128 | # install ros dependencies 129 | WORKDIR /home 130 | RUN mkdir ros_ws 131 | WORKDIR /home/ros_ws 132 | RUN apt-get -y install ros-melodic-cv-bridge 133 | RUN apt-get -y install ros-melodic-nodelet 134 | RUN apt-get -y install ros-melodic-tf 135 | RUN apt-get -y install ros-melodic-image-transport 136 | RUN apt-get -y install ros-melodic-rviz 137 | 138 | # build vins-rgbd-fast 139 | RUN mkdir src 140 | WORKDIR /home/ros_ws/src 141 | RUN git clone https://github.com/jianhengLiu/VINS-RGBD-FAST 142 | WORKDIR /home/ros_ws 143 | RUN /bin/bash -c ". /opt/ros/melodic/setup.bash; cd /home/ros_ws; catkin_make" 144 | RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc 145 | ``` 146 | docker build . 147 | 148 | ## 4. Licence 149 | The source code is released under [GPLv3](http://www.gnu.org/licenses/) license. 150 | 151 | -------------------------------------------------------------------------------- /camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(camera_model) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | # https://blog.csdn.net/qq_24624539/article/details/111056791 6 | #set(CMAKE_CXX_FLAGS "-std=c++11") 7 | set(CMAKE_CXX_FLAGS "-std=c++14") 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC") 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | ) 14 | 15 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 16 | include_directories(${Boost_INCLUDE_DIRS}) 17 | 18 | find_package(OpenCV REQUIRED) 19 | 20 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 21 | find_package(Ceres REQUIRED) 22 | include_directories(${CERES_INCLUDE_DIRS}) 23 | 24 | 25 | catkin_package( 26 | INCLUDE_DIRS include 27 | LIBRARIES camera_model 28 | CATKIN_DEPENDS roscpp std_msgs 29 | # DEPENDS system_lib 30 | ) 31 | 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | include_directories("include") 37 | 38 | 39 | add_executable(Calibration 40 | src/intrinsic_calib.cc 41 | src/chessboard/Chessboard.cc 42 | src/calib/CameraCalibration.cc 43 | src/camera_models/Camera.cc 44 | src/camera_models/CameraFactory.cc 45 | src/camera_models/CostFunctionFactory.cc 46 | src/camera_models/PinholeCamera.cc 47 | src/camera_models/CataCamera.cc 48 | src/camera_models/EquidistantCamera.cc 49 | src/camera_models/ScaramuzzaCamera.cc 50 | src/sparse_graph/Transform.cc 51 | src/gpl/gpl.cc 52 | src/gpl/EigenQuaternionParameterization.cc) 53 | 54 | add_library(camera_model 55 | src/chessboard/Chessboard.cc 56 | src/calib/CameraCalibration.cc 57 | src/camera_models/Camera.cc 58 | src/camera_models/CameraFactory.cc 59 | src/camera_models/CostFunctionFactory.cc 60 | src/camera_models/PinholeCamera.cc 61 | src/camera_models/CataCamera.cc 62 | src/camera_models/EquidistantCamera.cc 63 | src/camera_models/ScaramuzzaCamera.cc 64 | src/sparse_graph/Transform.cc 65 | src/gpl/gpl.cc 66 | src/gpl/EigenQuaternionParameterization.cc) 67 | 68 | target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 69 | target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 70 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/calib/CameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERACALIBRATION_H 2 | #define CAMERACALIBRATION_H 3 | 4 | #include 5 | 6 | #include "camodocal/camera_models/Camera.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class CameraCalibration 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | CameraCalibration(); 16 | 17 | CameraCalibration(Camera::ModelType modelType, 18 | const std::string& cameraName, 19 | const cv::Size& imageSize, 20 | const cv::Size& boardSize, 21 | float squareSize); 22 | 23 | void clear(void); 24 | 25 | void addChessboardData(const std::vector& corners); 26 | 27 | bool calibrate(void); 28 | 29 | int sampleCount(void) const; 30 | std::vector >& imagePoints(void); 31 | const std::vector >& imagePoints(void) const; 32 | std::vector >& scenePoints(void); 33 | const std::vector >& scenePoints(void) const; 34 | CameraPtr& camera(void); 35 | const CameraConstPtr camera(void) const; 36 | 37 | Eigen::Matrix2d& measurementCovariance(void); 38 | const Eigen::Matrix2d& measurementCovariance(void) const; 39 | 40 | cv::Mat& cameraPoses(void); 41 | const cv::Mat& cameraPoses(void) const; 42 | 43 | void drawResults(std::vector& images) const; 44 | 45 | void writeParams(const std::string& filename) const; 46 | 47 | bool writeChessboardData(const std::string& filename) const; 48 | bool readChessboardData(const std::string& filename); 49 | 50 | void setVerbose(bool verbose); 51 | 52 | private: 53 | bool calibrateHelper(CameraPtr& camera, 54 | std::vector& rvecs, std::vector& tvecs) const; 55 | 56 | void optimize(CameraPtr& camera, 57 | std::vector& rvecs, std::vector& tvecs) const; 58 | 59 | template 60 | void readData(std::ifstream& ifs, T& data) const; 61 | 62 | template 63 | void writeData(std::ofstream& ofs, T data) const; 64 | 65 | cv::Size m_boardSize; 66 | float m_squareSize; 67 | 68 | CameraPtr m_camera; 69 | cv::Mat m_cameraPoses; 70 | 71 | std::vector > m_imagePoints; 72 | std::vector > m_scenePoints; 73 | 74 | Eigen::Matrix2d m_measurementCovariance; 75 | 76 | bool m_verbose; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace camodocal 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camodocal/camera_models/Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camodocal 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | // forward declarations 11 | class ChessboardCorner; 12 | typedef boost::shared_ptr ChessboardCornerPtr; 13 | class ChessboardQuad; 14 | typedef boost::shared_ptr ChessboardQuadPtr; 15 | 16 | class Chessboard 17 | { 18 | public: 19 | Chessboard(cv::Size boardSize, cv::Mat& image); 20 | 21 | void findCorners(bool useOpenCV = false); 22 | const std::vector& getCorners(void) const; 23 | bool cornersFound(void) const; 24 | 25 | const cv::Mat& getImage(void) const; 26 | const cv::Mat& getSketch(void) const; 27 | 28 | private: 29 | bool findChessboardCorners(const cv::Mat& image, 30 | const cv::Size& patternSize, 31 | std::vector& corners, 32 | int flags, bool useOpenCV); 33 | 34 | bool findChessboardCornersImproved(const cv::Mat& image, 35 | const cv::Size& patternSize, 36 | std::vector& corners, 37 | int flags); 38 | 39 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 40 | 41 | void findConnectedQuads(std::vector& quads, 42 | std::vector& group, 43 | int group_idx, int dilation); 44 | 45 | // int checkQuadGroup(std::vector& quadGroup, 46 | // std::vector& outCorners, 47 | // cv::Size patternSize); 48 | 49 | void labelQuadGroup(std::vector& quad_group, 50 | cv::Size patternSize, bool firstRun); 51 | 52 | void findQuadNeighbors(std::vector& quads, int dilation); 53 | 54 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 55 | std::vector& existingQuads, int existingDilation); 56 | 57 | void generateQuads(std::vector& quads, 58 | cv::Mat& image, int flags, 59 | int dilation, bool firstRun); 60 | 61 | bool checkQuadGroup(std::vector& quads, 62 | std::vector& corners, 63 | cv::Size patternSize); 64 | 65 | void getQuadrangleHypotheses(const std::vector< std::vector >& contours, 66 | std::vector< std::pair >& quads, 67 | int classId) const; 68 | 69 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 70 | 71 | bool checkBoardMonotony(std::vector& corners, 72 | cv::Size patternSize); 73 | 74 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, 75 | ChessboardQuadPtr& quad2, int corner2) const; 76 | 77 | cv::Mat mImage; 78 | cv::Mat mSketch; 79 | std::vector mCorners; 80 | cv::Size mBoardSize; 81 | bool mCornersFound; 82 | }; 83 | 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camodocal 8 | { 9 | 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | 13 | class ChessboardCorner 14 | { 15 | public: 16 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 17 | 18 | float meanDist(int &n) const 19 | { 20 | float sum = 0; 21 | n = 0; 22 | for (int i = 0; i < 4; ++i) 23 | { 24 | if (neighbors[i].get()) 25 | { 26 | float dx = neighbors[i]->pt.x - pt.x; 27 | float dy = neighbors[i]->pt.y - pt.y; 28 | sum += sqrt(dx*dx + dy*dy); 29 | n++; 30 | } 31 | } 32 | return sum / std::max(n, 1); 33 | } 34 | 35 | cv::Point2f pt; // X and y coordinates 36 | int row; // Row and column of the corner 37 | int column; // in the found pattern 38 | bool needsNeighbor; // Does the corner require a neighbor? 39 | int count; // number of corner neighbors 40 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camodocal/chessboard/ChessboardCorner.h" 7 | 8 | namespace camodocal 9 | { 10 | 11 | class ChessboardQuad; 12 | typedef boost::shared_ptr ChessboardQuadPtr; 13 | 14 | class ChessboardQuad 15 | { 16 | public: 17 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 18 | 19 | int count; // Number of quad neighbors 20 | int group_idx; // Quad group ID 21 | float edge_len; // Smallest side length^2 22 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 23 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 24 | bool labeled; // Has this corner been labeled? 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camodocal 7 | { 8 | 9 | class EigenQuaternionParameterization : public ceres::LocalParameterization 10 | { 11 | public: 12 | virtual ~EigenQuaternionParameterization() {} 13 | virtual bool Plus(const double* x, 14 | const double* delta, 15 | double* x_plus_delta) const; 16 | virtual bool ComputeJacobian(const double* x, 17 | double* jacobian) const; 18 | virtual int GlobalSize() const { return 4; } 19 | virtual int LocalSize() const { return 3; } 20 | 21 | private: 22 | template 23 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 24 | }; 25 | 26 | 27 | template 28 | void 29 | EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const 30 | { 31 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 32 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 33 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 34 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 35 | } 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /camera_model/include/camodocal/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | class Transform 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | Transform(); 17 | Transform(const Eigen::Matrix4d& H); 18 | 19 | Eigen::Quaterniond& rotation(void); 20 | const Eigen::Quaterniond& rotation(void) const; 21 | double* rotationData(void); 22 | const double* const rotationData(void) const; 23 | 24 | Eigen::Vector3d& translation(void); 25 | const Eigen::Vector3d& translation(void) const; 26 | double* translationData(void); 27 | const double* const translationData(void) const; 28 | 29 | Eigen::Matrix4d toMatrix(void) const; 30 | 31 | private: 32 | Eigen::Quaterniond m_q; 33 | Eigen::Vector3d m_t; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /camera_model/instruction: -------------------------------------------------------------------------------- 1 | rosrun camera_model Calibration -w 8 -h 11 -s 70 -i ~/bag/PX/calib/ 2 | -------------------------------------------------------------------------------- /camera_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_model 4 | 0.0.0 5 | The camera_model package 6 | 7 | 8 | 9 | 10 | dvorak 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /camera_model/readme.md: -------------------------------------------------------------------------------- 1 | part of [camodocal](https://github.com/hengli/camodocal) 2 | 3 | [Google Ceres](http://ceres-solver.org) is needed. 4 | 5 | # Calibration: 6 | 7 | Use [intrinsic_calib.cc](https://github.com/dvorak0/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. 8 | 9 | # Undistortion: 10 | 11 | See [Camera.h](https://github.com/dvorak0/camera_model/blob/master/include/camodocal/camera_models/Camera.h) for general interface: 12 | 13 | - liftProjective: Lift points from the image plane to the projective space. 14 | - spaceToPlane: Projects 3D points to the image plane (Pi function) 15 | 16 | -------------------------------------------------------------------------------- /camera_model/src/camera_models/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/camera_models/CameraFactory.h" 2 | 3 | #include 4 | 5 | 6 | #include "camodocal/camera_models/CataCamera.h" 7 | #include "camodocal/camera_models/EquidistantCamera.h" 8 | #include "camodocal/camera_models/PinholeCamera.h" 9 | #include "camodocal/camera_models/ScaramuzzaCamera.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | namespace camodocal 14 | { 15 | 16 | boost::shared_ptr CameraFactory::m_instance; 17 | 18 | CameraFactory::CameraFactory() 19 | { 20 | 21 | } 22 | 23 | boost::shared_ptr 24 | CameraFactory::instance(void) 25 | { 26 | if (m_instance.get() == 0) 27 | { 28 | m_instance.reset(new CameraFactory); 29 | } 30 | 31 | return m_instance; 32 | } 33 | 34 | CameraPtr 35 | CameraFactory::generateCamera(Camera::ModelType modelType, 36 | const std::string& cameraName, 37 | cv::Size imageSize) const 38 | { 39 | switch (modelType) 40 | { 41 | case Camera::KANNALA_BRANDT: 42 | { 43 | EquidistantCameraPtr camera(new EquidistantCamera); 44 | 45 | EquidistantCamera::Parameters params = camera->getParameters(); 46 | params.cameraName() = cameraName; 47 | params.imageWidth() = imageSize.width; 48 | params.imageHeight() = imageSize.height; 49 | camera->setParameters(params); 50 | return camera; 51 | } 52 | case Camera::PINHOLE: 53 | { 54 | PinholeCameraPtr camera(new PinholeCamera); 55 | 56 | PinholeCamera::Parameters params = camera->getParameters(); 57 | params.cameraName() = cameraName; 58 | params.imageWidth() = imageSize.width; 59 | params.imageHeight() = imageSize.height; 60 | camera->setParameters(params); 61 | return camera; 62 | } 63 | case Camera::SCARAMUZZA: 64 | { 65 | OCAMCameraPtr camera(new OCAMCamera); 66 | 67 | OCAMCamera::Parameters params = camera->getParameters(); 68 | params.cameraName() = cameraName; 69 | params.imageWidth() = imageSize.width; 70 | params.imageHeight() = imageSize.height; 71 | camera->setParameters(params); 72 | return camera; 73 | } 74 | case Camera::MEI: 75 | default: 76 | { 77 | CataCameraPtr camera(new CataCamera); 78 | 79 | CataCamera::Parameters params = camera->getParameters(); 80 | params.cameraName() = cameraName; 81 | params.imageWidth() = imageSize.width; 82 | params.imageHeight() = imageSize.height; 83 | camera->setParameters(params); 84 | return camera; 85 | } 86 | } 87 | } 88 | 89 | CameraPtr 90 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 91 | { 92 | cv::FileStorage fs(filename, cv::FileStorage::READ); 93 | 94 | if (!fs.isOpened()) 95 | { 96 | return CameraPtr(); 97 | } 98 | 99 | Camera::ModelType modelType = Camera::MEI; 100 | if (!fs["model_type"].isNone()) 101 | { 102 | std::string sModelType; 103 | fs["model_type"] >> sModelType; 104 | 105 | if (boost::iequals(sModelType, "kannala_brandt")) 106 | { 107 | modelType = Camera::KANNALA_BRANDT; 108 | } 109 | else if (boost::iequals(sModelType, "mei")) 110 | { 111 | modelType = Camera::MEI; 112 | } 113 | else if (boost::iequals(sModelType, "scaramuzza")) 114 | { 115 | modelType = Camera::SCARAMUZZA; 116 | } 117 | else if (boost::iequals(sModelType, "pinhole")) 118 | { 119 | modelType = Camera::PINHOLE; 120 | } 121 | else 122 | { 123 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 124 | return CameraPtr(); 125 | } 126 | } 127 | 128 | switch (modelType) 129 | { 130 | case Camera::KANNALA_BRANDT: 131 | { 132 | EquidistantCameraPtr camera(new EquidistantCamera); 133 | 134 | EquidistantCamera::Parameters params = camera->getParameters(); 135 | params.readFromYamlFile(filename); 136 | camera->setParameters(params); 137 | return camera; 138 | } 139 | case Camera::PINHOLE: 140 | { 141 | PinholeCameraPtr camera(new PinholeCamera); 142 | 143 | PinholeCamera::Parameters params = camera->getParameters(); 144 | params.readFromYamlFile(filename); 145 | camera->setParameters(params); 146 | return camera; 147 | } 148 | case Camera::SCARAMUZZA: 149 | { 150 | OCAMCameraPtr camera(new OCAMCamera); 151 | 152 | OCAMCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | case Camera::MEI: 158 | default: 159 | { 160 | CataCameraPtr camera(new CataCamera); 161 | 162 | CataCamera::Parameters params = camera->getParameters(); 163 | params.readFromYamlFile(filename); 164 | camera->setParameters(params); 165 | return camera; 166 | } 167 | } 168 | 169 | return CameraPtr(); 170 | } 171 | 172 | } 173 | 174 | -------------------------------------------------------------------------------- /camera_model/src/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camodocal/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camodocal 6 | { 7 | 8 | bool 9 | EigenQuaternionParameterization::Plus(const double* x, 10 | const double* delta, 11 | double* x_plus_delta) const 12 | { 13 | const double norm_delta = 14 | sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 15 | if (norm_delta > 0.0) 16 | { 17 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 18 | double q_delta[4]; 19 | q_delta[0] = sin_delta_by_delta * delta[0]; 20 | q_delta[1] = sin_delta_by_delta * delta[1]; 21 | q_delta[2] = sin_delta_by_delta * delta[2]; 22 | q_delta[3] = cos(norm_delta); 23 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 24 | } 25 | else 26 | { 27 | for (int i = 0; i < 4; ++i) 28 | { 29 | x_plus_delta[i] = x[i]; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | bool 36 | EigenQuaternionParameterization::ComputeJacobian(const double* x, 37 | double* jacobian) const 38 | { 39 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT 40 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT 41 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT 42 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT 43 | return true; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /camera_model/src/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camodocal 4 | { 5 | 6 | Transform::Transform() 7 | { 8 | m_q.setIdentity(); 9 | m_t.setZero(); 10 | } 11 | 12 | Transform::Transform(const Eigen::Matrix4d& H) 13 | { 14 | m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); 15 | m_t = H.block<3,1>(0,3); 16 | } 17 | 18 | Eigen::Quaterniond& 19 | Transform::rotation(void) 20 | { 21 | return m_q; 22 | } 23 | 24 | const Eigen::Quaterniond& 25 | Transform::rotation(void) const 26 | { 27 | return m_q; 28 | } 29 | 30 | double* 31 | Transform::rotationData(void) 32 | { 33 | return m_q.coeffs().data(); 34 | } 35 | 36 | const double* const 37 | Transform::rotationData(void) const 38 | { 39 | return m_q.coeffs().data(); 40 | } 41 | 42 | Eigen::Vector3d& 43 | Transform::translation(void) 44 | { 45 | return m_t; 46 | } 47 | 48 | const Eigen::Vector3d& 49 | Transform::translation(void) const 50 | { 51 | return m_t; 52 | } 53 | 54 | double* 55 | Transform::translationData(void) 56 | { 57 | return m_t.data(); 58 | } 59 | 60 | const double* const 61 | Transform::translationData(void) const 62 | { 63 | return m_t.data(); 64 | } 65 | 66 | Eigen::Matrix4d 67 | Transform::toMatrix(void) const 68 | { 69 | Eigen::Matrix4d H; 70 | H.setIdentity(); 71 | H.block<3,3>(0,0) = m_q.toRotationMatrix(); 72 | H.block<3,1>(0,3) = m_t; 73 | 74 | return H; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /config/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jianhengLiu/VINS-RGBD-FAST/757c256c747a3a24f13e9d71fc7da0fb857dea21/config/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/fisheye_mask_752x480.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jianhengLiu/VINS-RGBD-FAST/757c256c747a3a24f13e9d71fc7da0fb857dea21/config/fisheye_mask_752x480.jpg -------------------------------------------------------------------------------- /config/openloris/openloris_vio.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | num_threads: 0 # 0 Use the max number of threads of your device. 4 | # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) 5 | # x It is proper that 1 < x < MAX_THREAD. 6 | # For now, this parameter is relevant to grid-detector to run in parallel. 7 | 8 | #common parameters 9 | imu: 1 10 | static_init: 0 11 | imu_topic: "/d400/imu0" 12 | image_topic: "/d400/color/image_raw" 13 | depth_topic: "/d400/aligned_depth_to_color/image_raw" 14 | output_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output" 15 | 16 | #RGBD camera Ideal Range 17 | depth_min_dist: 0.3 18 | depth_max_dist: 3 19 | 20 | frontend_freq: 30 # It should be raised in VO mode(without IMU). 21 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the "keyframe_parallax" 22 | num_grid_rows: 7 23 | num_grid_cols: 8 24 | 25 | #unsynchronization parameters 26 | estimate_td: 1 ########## # online estimate time offset between camera and imu 27 | td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 28 | 29 | #camera calibration 30 | model_type: PINHOLE 31 | camera_name: camera 32 | image_width: 848 33 | image_height: 480 34 | #TODO modify distortion 35 | 36 | distortion_parameters: 37 | k1: 0.0 38 | k2: 0.0 39 | p1: 0.0 40 | p2: 0.0 41 | projection_parameters: 42 | fx: 611.4509887695312 43 | fy: 611.4857177734375 44 | cx: 433.2039794921875 45 | cy: 249.4730224609375 46 | 47 | # Extrinsic parameter between IMU and Camera. 48 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 49 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 50 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 51 | #If you choose 0 or 1, you should write down the following matrix. 52 | #Rotation from camera frame to imu frame, imu^R_cam 53 | extrinsicRotation: !!opencv-matrix 54 | rows: 3 55 | cols: 3 56 | dt: d 57 | data: [ 0.999975572679493, 0.003849141066713, 0.005854714944742, 58 | -0.003828680351062, 0.999986658473099, -0.003501944262433, 59 | -0.005868115609379, 0.003479442469180, 0.999976848846595] 60 | #Translation from camera frame to imu frame, imu^T_cam 61 | extrinsicTranslation: !!opencv-matrix 62 | rows: 3 63 | cols: 1 64 | dt: d 65 | data: [0.0203127935529, -0.00510325236246, -0.0112013882026] 66 | 67 | #feature traker paprameters 68 | max_cnt: 130 # max feature number in feature tracking. It is suggested to be raised in VO mode. 69 | min_dist: 30 # min distance between two features 70 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 71 | F_threshold: 1.0 # ransac threshold (pixel) 72 | show_track: 0 # publish tracking image as topic 73 | equalize: 0 # if image is too dark or light, trun on equalize to find enough features 74 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 75 | 76 | #optimization parameters 77 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 78 | max_num_iterations: 8 # max solver itrations, to guarantee real time 79 | 80 | #imu parameters The more accurate parameters you provide, the better performance 81 | acc_n: 0.1 # accelerometer measurement noise standard deviation. 82 | gyr_n: 0.01 # gyroscope measurement noise standard deviation. 83 | acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02 84 | gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 85 | 86 | g_norm: 9.805 # gravity magnitude 87 | 88 | #rolling shutter parameters 89 | rolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera 90 | rolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet) 91 | 92 | #loop closure parameters 93 | loop_closure: 0 # start loop closure 94 | fast_relocalization: 0 # useful in real-time and large project 95 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 96 | pose_graph_save_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output/pose_graph" # save and load path 97 | 98 | #visualization parameters 99 | save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 100 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 101 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 102 | 103 | #暂时仅考虑动物 104 | dynamic_label: ["person", "cat", "dog", "bicycle", "car","bus"] 105 | 106 | -------------------------------------------------------------------------------- /config/openloris/openloris_vio_atlas.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | num_threads: 6 # 0 Use the max number of threads of your device. 4 | # For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.) 5 | # x It is proper that 1 < x < MAX_THREAD. 6 | # For now, this parameter is relevant to grid-detector to run in parallel. 7 | 8 | 9 | #common parameters 10 | imu: 1 11 | static_init: 1 12 | imu_topic: "/d400/imu0" 13 | image_topic: "/decompressed_img" 14 | depth_topic: "/d400/aligned_depth_to_color/image_raw" # check your depth image bandwidth is lower than 30Mbs, otherwise compressedDepth is suggested 15 | output_path: "/home/nrosliu/DynamicVINS-Atlas/src/Dynamic_VINS/output" 16 | 17 | # 0.3 2 | 4 | 5 | PoseGraphNodelet 6 | 7 | 8 | -------------------------------------------------------------------------------- /pose_graph/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_graph 4 | 0.0.0 5 | pose_graph package 6 | 7 | 8 | 9 | 10 | tony-ws 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | roscpp 45 | camera_model 46 | camera_model 47 | 48 | nodelet 49 | nodelet 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/DBoW2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DBoW2.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: Generic include file for the DBoW2 classes and 6 | * the specialized vocabularies and databases 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DBoW2 Library 12 | * 13 | * DBoW2 library for C++: 14 | * Bag-of-word image database for image retrieval. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://doriangalvez.com 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries, 23 | * as well as the boost::dynamic_bitset class. 24 | * 25 | * \section citation Citation 26 | * If you use this software in academic works, please cite: 27 | 
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |
41 | * 42 | * \section license License 43 | * This file is licensed under a Creative Commons 44 | * Attribution-NonCommercial-ShareAlike 3.0 license. 45 | * This file can be freely used and users can use, download and edit this file 46 | * provided that credit is attributed to the original author. No users are 47 | * permitted to use this file for commercial purposes unless explicit permission 48 | * is given by the original author. Derivative works must be licensed using the 49 | * same or similar license. 50 | * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further 51 | * details. 52 | * 53 | */ 54 | 55 | #ifndef __D_T_DBOW2__ 56 | #define __D_T_DBOW2__ 57 | 58 | /// Includes all the data structures to manage vocabularies and image databases 59 | namespace DBoW2 60 | { 61 | } 62 | 63 | #include "TemplatedVocabulary.h" 64 | #include "TemplatedDatabase.h" 65 | #include "BowVector.h" 66 | #include "FeatureVector.h" 67 | #include "QueryResults.h" 68 | #include "FBrief.h" 69 | 70 | /// BRIEF Vocabulary 71 | typedef DBoW2::TemplatedVocabulary 72 | BriefVocabulary; 73 | 74 | /// BRIEF Database 75 | typedef DBoW2::TemplatedDatabase 76 | BriefDatabase; 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FBrief.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "FBrief.h" 15 | 16 | using namespace std; 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | void FBrief::meanValue(const std::vector &descriptors, 23 | FBrief::TDescriptor &mean) 24 | { 25 | mean.reset(); 26 | 27 | if(descriptors.empty()) return; 28 | 29 | const int N2 = descriptors.size() / 2; 30 | const int L = descriptors[0]->size(); 31 | 32 | vector counters(L, 0); 33 | 34 | vector::const_iterator it; 35 | for(it = descriptors.begin(); it != descriptors.end(); ++it) 36 | { 37 | const FBrief::TDescriptor &desc = **it; 38 | for(int i = 0; i < L; ++i) 39 | { 40 | if(desc[i]) counters[i]++; 41 | } 42 | } 43 | 44 | for(int i = 0; i < L; ++i) 45 | { 46 | if(counters[i] > N2) mean.set(i); 47 | } 48 | 49 | } 50 | 51 | // -------------------------------------------------------------------------- 52 | 53 | double FBrief::distance(const FBrief::TDescriptor &a, 54 | const FBrief::TDescriptor &b) 55 | { 56 | return (double)DVision::BRIEF::distance(a, b); 57 | } 58 | 59 | // -------------------------------------------------------------------------- 60 | 61 | std::string FBrief::toString(const FBrief::TDescriptor &a) 62 | { 63 | // from boost::bitset 64 | string s; 65 | to_string(a, s); // reversed 66 | return s; 67 | } 68 | 69 | // -------------------------------------------------------------------------- 70 | 71 | void FBrief::fromString(FBrief::TDescriptor &a, const std::string &s) 72 | { 73 | // from boost::bitset 74 | stringstream ss(s); 75 | ss >> a; 76 | } 77 | 78 | // -------------------------------------------------------------------------- 79 | 80 | void FBrief::toMat32F(const std::vector &descriptors, 81 | cv::Mat &mat) 82 | { 83 | if(descriptors.empty()) 84 | { 85 | mat.release(); 86 | return; 87 | } 88 | 89 | const int N = descriptors.size(); 90 | const int L = descriptors[0].size(); 91 | 92 | mat.create(N, L, CV_32F); 93 | 94 | for(int i = 0; i < N; ++i) 95 | { 96 | const TDescriptor& desc = descriptors[i]; 97 | float *p = mat.ptr(i); 98 | for(int j = 0; j < L; ++j, ++p) 99 | { 100 | *p = (desc[j] ? 1 : 0); 101 | } 102 | } 103 | } 104 | 105 | // -------------------------------------------------------------------------- 106 | 107 | } // namespace DBoW2 108 | 109 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FBrief.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_BRIEF__ 11 | #define __D_T_F_BRIEF__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | #include "../DVision/DVision.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Functions to manipulate BRIEF descriptors 23 | class FBrief: protected FClass 24 | { 25 | public: 26 | 27 | typedef DVision::BRIEF::bitset TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | static void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean); 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | 68 | }; 69 | 70 | } // namespace DBoW2 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/QueryResults.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.cpp 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include "QueryResults.h" 13 | 14 | using namespace std; 15 | 16 | namespace DBoW2 17 | { 18 | 19 | // --------------------------------------------------------------------------- 20 | 21 | ostream & operator<<(ostream& os, const Result& ret ) 22 | { 23 | os << ""; 24 | return os; 25 | } 26 | 27 | // --------------------------------------------------------------------------- 28 | 29 | ostream & operator<<(ostream& os, const QueryResults& ret ) 30 | { 31 | if(ret.size() == 1) 32 | os << "1 result:" << endl; 33 | else 34 | os << ret.size() << " results:" << endl; 35 | 36 | QueryResults::const_iterator rit; 37 | for(rit = ret.begin(); rit != ret.end(); ++rit) 38 | { 39 | os << *rit; 40 | if(rit + 1 != ret.end()) os << endl; 41 | } 42 | return os; 43 | } 44 | 45 | // --------------------------------------------------------------------------- 46 | 47 | void QueryResults::saveM(const std::string &filename) const 48 | { 49 | fstream f(filename.c_str(), ios::out); 50 | 51 | QueryResults::const_iterator qit; 52 | for(qit = begin(); qit != end(); ++qit) 53 | { 54 | f << qit->Id << " " << qit->Score << endl; 55 | } 56 | 57 | f.close(); 58 | } 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | } // namespace DBoW2 63 | 64 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/QueryResults.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.h 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_QUERY_RESULTS__ 11 | #define __D_T_QUERY_RESULTS__ 12 | 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | /// Id of entries of the database 18 | typedef unsigned int EntryId; 19 | 20 | /// Single result of a query 21 | class Result 22 | { 23 | public: 24 | 25 | /// Entry id 26 | EntryId Id; 27 | 28 | /// Score obtained 29 | double Score; 30 | 31 | /// debug 32 | int nWords; // words in common 33 | // !!! this is filled only by Bhatt score! 34 | // (and for BCMatching, BCThresholding then) 35 | 36 | double bhatScore, chiScore; 37 | /// debug 38 | 39 | // only done by ChiSq and BCThresholding 40 | double sumCommonVi; 41 | double sumCommonWi; 42 | double expectedChiScore; 43 | /// debug 44 | 45 | /** 46 | * Empty constructors 47 | */ 48 | inline Result(){} 49 | 50 | /** 51 | * Creates a result with the given data 52 | * @param _id entry id 53 | * @param _score score 54 | */ 55 | inline Result(EntryId _id, double _score): Id(_id), Score(_score){} 56 | 57 | /** 58 | * Compares the scores of two results 59 | * @return true iff this.score < r.score 60 | */ 61 | inline bool operator<(const Result &r) const 62 | { 63 | return this->Score < r.Score; 64 | } 65 | 66 | /** 67 | * Compares the scores of two results 68 | * @return true iff this.score > r.score 69 | */ 70 | inline bool operator>(const Result &r) const 71 | { 72 | return this->Score > r.Score; 73 | } 74 | 75 | /** 76 | * Compares the entry id of the result 77 | * @return true iff this.id == id 78 | */ 79 | inline bool operator==(EntryId id) const 80 | { 81 | return this->Id == id; 82 | } 83 | 84 | /** 85 | * Compares the score of this entry with a given one 86 | * @param s score to compare with 87 | * @return true iff this score < s 88 | */ 89 | inline bool operator<(double s) const 90 | { 91 | return this->Score < s; 92 | } 93 | 94 | /** 95 | * Compares the score of this entry with a given one 96 | * @param s score to compare with 97 | * @return true iff this score > s 98 | */ 99 | inline bool operator>(double s) const 100 | { 101 | return this->Score > s; 102 | } 103 | 104 | /** 105 | * Compares the score of two results 106 | * @param a 107 | * @param b 108 | * @return true iff a.Score > b.Score 109 | */ 110 | static inline bool gt(const Result &a, const Result &b) 111 | { 112 | return a.Score > b.Score; 113 | } 114 | 115 | /** 116 | * Compares the scores of two results 117 | * @return true iff a.Score > b.Score 118 | */ 119 | inline static bool ge(const Result &a, const Result &b) 120 | { 121 | return a.Score > b.Score; 122 | } 123 | 124 | /** 125 | * Returns true iff a.Score >= b.Score 126 | * @param a 127 | * @param b 128 | * @return true iff a.Score >= b.Score 129 | */ 130 | static inline bool geq(const Result &a, const Result &b) 131 | { 132 | return a.Score >= b.Score; 133 | } 134 | 135 | /** 136 | * Returns true iff a.Score >= s 137 | * @param a 138 | * @param s 139 | * @return true iff a.Score >= s 140 | */ 141 | static inline bool geqv(const Result &a, double s) 142 | { 143 | return a.Score >= s; 144 | } 145 | 146 | 147 | /** 148 | * Returns true iff a.Id < b.Id 149 | * @param a 150 | * @param b 151 | * @return true iff a.Id < b.Id 152 | */ 153 | static inline bool ltId(const Result &a, const Result &b) 154 | { 155 | return a.Id < b.Id; 156 | } 157 | 158 | /** 159 | * Prints a string version of the result 160 | * @param os ostream 161 | * @param ret Result to print 162 | */ 163 | friend std::ostream & operator<<(std::ostream& os, const Result& ret ); 164 | }; 165 | 166 | /// Multiple results from a query 167 | class QueryResults: public std::vector 168 | { 169 | public: 170 | 171 | /** 172 | * Multiplies all the scores in the vector by factor 173 | * @param factor 174 | */ 175 | inline void scaleScores(double factor); 176 | 177 | /** 178 | * Prints a string version of the results 179 | * @param os ostream 180 | * @param ret QueryResults to print 181 | */ 182 | friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret ); 183 | 184 | /** 185 | * Saves a matlab file with the results 186 | * @param filename 187 | */ 188 | void saveM(const std::string &filename) const; 189 | 190 | }; 191 | 192 | // -------------------------------------------------------------------------- 193 | 194 | inline void QueryResults::scaleScores(double factor) 195 | { 196 | for(QueryResults::iterator qit = begin(); qit != end(); ++qit) 197 | qit->Score *= factor; 198 | } 199 | 200 | // -------------------------------------------------------------------------- 201 | 202 | } // namespace TemplatedBoW 203 | 204 | #endif 205 | 206 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DBoW/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | }; 45 | 46 | /** 47 | * Macro for defining Scoring classes 48 | * @param NAME name of class 49 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 50 | * @param NORM type of norm to use when MUSTNORMALIZE 51 | */ 52 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 53 | NAME: public GeneralScoring \ 54 | { public: \ 55 | /** \ 56 | * Computes score between two vectors \ 57 | * @param v \ 58 | * @param w \ 59 | * @return score between v and w \ 60 | */ \ 61 | virtual double score(const BowVector &v, const BowVector &w) const; \ 62 | \ 63 | /** \ 64 | * Says if a vector must be normalized according to the scoring function \ 65 | * @param norm (out) if true, norm to use 66 | * @return true iff vectors must be normalized \ 67 | */ \ 68 | virtual inline bool mustNormalize(LNorm &norm) const \ 69 | { norm = NORM; return MUSTNORMALIZE; } \ 70 | } 71 | 72 | /// L1 Scoring object 73 | class __SCORING_CLASS(L1Scoring, true, L1); 74 | 75 | /// L2 Scoring object 76 | class __SCORING_CLASS(L2Scoring, true, L2); 77 | 78 | /// Chi square Scoring object 79 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 80 | 81 | /// KL divergence Scoring object 82 | class __SCORING_CLASS(KLScoring, true, L1); 83 | 84 | /// Bhattacharyya Scoring object 85 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 86 | 87 | /// Dot product Scoring object 88 | class __SCORING_CLASS(DotProductScoring, false, L1); 89 | 90 | #undef __SCORING_CLASS 91 | 92 | } // namespace DBoW2 93 | 94 | #endif 95 | 96 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/DException.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DException.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: general exception of the library 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #ifndef __D_EXCEPTION__ 14 | #define __D_EXCEPTION__ 15 | 16 | #include 17 | #include 18 | using namespace std; 19 | 20 | namespace DUtils { 21 | 22 | /// General exception 23 | class DException : 24 | public exception 25 | { 26 | public: 27 | /** 28 | * Creates an exception with a general error message 29 | */ 30 | DException(void) throw(): m_message("DUtils exception"){} 31 | 32 | /** 33 | * Creates an exception with a custom error message 34 | * @param msg: message 35 | */ 36 | DException(const char *msg) throw(): m_message(msg){} 37 | 38 | /** 39 | * Creates an exception with a custom error message 40 | * @param msg: message 41 | */ 42 | DException(const string &msg) throw(): m_message(msg){} 43 | 44 | /** 45 | * Destructor 46 | */ 47 | virtual ~DException(void) throw(){} 48 | 49 | /** 50 | * Returns the exception message 51 | */ 52 | virtual const char* what() const throw() 53 | { 54 | return m_message.c_str(); 55 | } 56 | 57 | protected: 58 | /// Error message 59 | string m_message; 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | 66 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/DUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DUtils.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: include file for including all the library functionalities 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DUtils Library 12 | * 13 | * DUtils library for C++: 14 | * Collection of classes with general utilities for C++ applications. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section license License 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU Lesser General Public License (LGPL) as 24 | * published by the Free Software Foundation, either version 3 of the License, 25 | * or any later version. 26 | * 27 | */ 28 | 29 | 30 | #pragma once 31 | 32 | #ifndef __D_UTILS__ 33 | #define __D_UTILS__ 34 | 35 | /// Several utilities for C++ programs 36 | namespace DUtils 37 | { 38 | } 39 | 40 | // Exception 41 | #include "DException.h" 42 | 43 | #include "Timestamp.h" 44 | // Random numbers 45 | #include "Random.h" 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DVision/BRIEF.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BRIEF.cpp 3 | * Author: Dorian Galvez 4 | * Date: September 2010 5 | * Description: implementation of BRIEF (Binary Robust Independent 6 | * Elementary Features) descriptor by 7 | * Michael Calonder, Vincent Lepetitand Pascal Fua 8 | * + close binary tests (by Dorian Galvez-Lopez) 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include "BRIEF.h" 14 | #include "../DUtils/DUtils.h" 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace DVision; 21 | 22 | // ---------------------------------------------------------------------------- 23 | 24 | BRIEF::BRIEF(int nbits, int patch_size, Type type): 25 | m_bit_length(nbits), m_patch_size(patch_size), m_type(type) 26 | { 27 | assert(patch_size > 1); 28 | assert(nbits > 0); 29 | generateTestPoints(); 30 | } 31 | 32 | // ---------------------------------------------------------------------------- 33 | 34 | BRIEF::~BRIEF() 35 | { 36 | } 37 | 38 | // --------------------------------------------------------------------------- 39 | 40 | void BRIEF::compute(const cv::Mat &image, 41 | const std::vector &points, 42 | vector &descriptors, 43 | bool treat_image) const 44 | { 45 | const float sigma = 2.f; 46 | const cv::Size ksize(9, 9); 47 | 48 | cv::Mat im; 49 | if(treat_image) 50 | { 51 | cv::Mat aux; 52 | if(image.depth() == 3) 53 | { 54 | cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY); 55 | } 56 | else 57 | { 58 | aux = image; 59 | } 60 | 61 | cv::GaussianBlur(aux, im, ksize, sigma, sigma); 62 | 63 | } 64 | else 65 | { 66 | im = image; 67 | } 68 | 69 | assert(im.type() == CV_8UC1); 70 | assert(im.isContinuous()); 71 | 72 | // use im now 73 | const int W = im.cols; 74 | const int H = im.rows; 75 | 76 | descriptors.resize(points.size()); 77 | std::vector::iterator dit; 78 | 79 | std::vector::const_iterator kit; 80 | 81 | int x1, y1, x2, y2; 82 | 83 | dit = descriptors.begin(); 84 | for(kit = points.begin(); kit != points.end(); ++kit, ++dit) 85 | { 86 | dit->resize(m_bit_length); 87 | dit->reset(); 88 | 89 | for(unsigned int i = 0; i < m_x1.size(); ++i) 90 | { 91 | x1 = (int)(kit->pt.x + m_x1[i]); 92 | y1 = (int)(kit->pt.y + m_y1[i]); 93 | x2 = (int)(kit->pt.x + m_x2[i]); 94 | y2 = (int)(kit->pt.y + m_y2[i]); 95 | 96 | if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H 97 | && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H) 98 | { 99 | if( im.ptr(y1)[x1] < im.ptr(y2)[x2] ) 100 | { 101 | dit->set(i); 102 | } 103 | } // if (x,y)_1 and (x,y)_2 are in the image 104 | 105 | } // for each (x,y) 106 | } // for each keypoint 107 | } 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | void BRIEF::generateTestPoints() 112 | { 113 | m_x1.resize(m_bit_length); 114 | m_y1.resize(m_bit_length); 115 | m_x2.resize(m_bit_length); 116 | m_y2.resize(m_bit_length); 117 | 118 | const float g_mean = 0.f; 119 | const float g_sigma = 0.2f * (float)m_patch_size; 120 | const float c_sigma = 0.08f * (float)m_patch_size; 121 | 122 | float sigma2; 123 | if(m_type == RANDOM) 124 | sigma2 = g_sigma; 125 | else 126 | sigma2 = c_sigma; 127 | 128 | const int max_v = m_patch_size / 2; 129 | 130 | DUtils::Random::SeedRandOnce(); 131 | 132 | for(int i = 0; i < m_bit_length; ++i) 133 | { 134 | int x1, y1, x2, y2; 135 | 136 | do 137 | { 138 | x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 139 | } while( x1 > max_v || x1 < -max_v); 140 | 141 | do 142 | { 143 | y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 144 | } while( y1 > max_v || y1 < -max_v); 145 | 146 | float meanx, meany; 147 | if(m_type == RANDOM) 148 | meanx = meany = g_mean; 149 | else 150 | { 151 | meanx = x1; 152 | meany = y1; 153 | } 154 | 155 | do 156 | { 157 | x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2); 158 | } while( x2 > max_v || x2 < -max_v); 159 | 160 | do 161 | { 162 | y2 = DUtils::Random::RandomGaussianValue(meany, sigma2); 163 | } while( y2 > max_v || y2 < -max_v); 164 | 165 | m_x1[i] = x1; 166 | m_y1[i] = y1; 167 | m_x2[i] = x2; 168 | m_y2[i] = y2; 169 | } 170 | 171 | } 172 | 173 | // ---------------------------------------------------------------------------- 174 | 175 | 176 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/DVision/DVision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DVision.h 3 | * Project: DVision library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 4, 2010 6 | * Description: several functions for computer vision 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DVision Library 12 | * 13 | * DVision library for C++ and OpenCV: 14 | * Collection of classes with computer vision functionality 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. 23 | * 24 | * \section license License 25 | * This program is free software: you can redistribute it and/or modify 26 | * it under the terms of the GNU Lesser General Public License (LGPL) as 27 | * published by the Free Software Foundation, either version 3 of the License, 28 | * or any later version. 29 | * 30 | */ 31 | 32 | #ifndef __D_VISION__ 33 | #define __D_VISION__ 34 | 35 | 36 | /// Computer vision functions 37 | namespace DVision 38 | { 39 | } 40 | 41 | #include "BRIEF.h" 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/VocabularyBinary.cpp: -------------------------------------------------------------------------------- 1 | #include "VocabularyBinary.hpp" 2 | #include 3 | using namespace std; 4 | 5 | VINSLoop::Vocabulary::Vocabulary() 6 | : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) { 7 | } 8 | 9 | VINSLoop::Vocabulary::~Vocabulary() { 10 | if (nodes != nullptr) { 11 | delete [] nodes; 12 | nodes = nullptr; 13 | } 14 | 15 | if (words != nullptr) { 16 | delete [] words; 17 | words = nullptr; 18 | } 19 | } 20 | 21 | void VINSLoop::Vocabulary::serialize(ofstream& stream) { 22 | stream.write((const char *)this, staticDataSize()); 23 | stream.write((const char *)nodes, sizeof(Node) * nNodes); 24 | stream.write((const char *)words, sizeof(Word) * nWords); 25 | } 26 | 27 | void VINSLoop::Vocabulary::deserialize(ifstream& stream) { 28 | stream.read((char *)this, staticDataSize()); 29 | 30 | nodes = new Node[nNodes]; 31 | stream.read((char *)nodes, sizeof(Node) * nNodes); 32 | 33 | words = new Word[nWords]; 34 | stream.read((char *)words, sizeof(Word) * nWords); 35 | } 36 | -------------------------------------------------------------------------------- /pose_graph/src/ThirdParty/VocabularyBinary.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VocabularyBinary_hpp 2 | #define VocabularyBinary_hpp 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace VINSLoop { 9 | 10 | struct Node { 11 | int32_t nodeId; 12 | int32_t parentId; 13 | double weight; 14 | uint64_t descriptor[4]; 15 | }; 16 | 17 | struct Word { 18 | int32_t nodeId; 19 | int32_t wordId; 20 | }; 21 | 22 | struct Vocabulary { 23 | int32_t k; 24 | int32_t L; 25 | int32_t scoringType; 26 | int32_t weightingType; 27 | 28 | int32_t nNodes; 29 | int32_t nWords; 30 | 31 | Node *nodes; 32 | Word *words; 33 | 34 | Vocabulary(); 35 | ~Vocabulary(); 36 | 37 | void serialize(std::ofstream &stream); 38 | void deserialize(std::ifstream &stream); 39 | 40 | inline static size_t staticDataSize() { 41 | return sizeof(Vocabulary) - sizeof(Node *) - sizeof(Word *); 42 | } 43 | }; 44 | 45 | } // namespace VINSLoop 46 | 47 | #endif /* VocabularyBinary_hpp */ 48 | -------------------------------------------------------------------------------- /pose_graph/src/keyframe/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../ThirdParty/DBoW/DBoW2.h" 4 | #include "../ThirdParty/DVision/DVision.h" 5 | #include "../utility/parameters.h" 6 | #include "../utility/tic_toc.h" 7 | #include "../utility/utility.h" 8 | #include "camodocal/camera_models/CameraFactory.h" 9 | #include "camodocal/camera_models/CataCamera.h" 10 | #include "camodocal/camera_models/PinholeCamera.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define MIN_LOOP_NUM 25 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | using namespace DVision; 21 | 22 | class BriefExtractor { 23 | public: 24 | virtual void operator()(const cv::Mat &im, vector &keys, 25 | vector &descriptors) const; 26 | BriefExtractor(const std::string &pattern_file); 27 | 28 | DVision::BRIEF m_brief; 29 | }; 30 | 31 | class KeyFrame { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, 35 | Matrix3d &_vio_R_w_i, cv::Mat &_image, 36 | vector &_point_3d, vector &_point_2d_uv, 37 | vector &_point_2d_normal, vector &_point_id, 38 | int _sequence); 39 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, 40 | Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, 41 | cv::Mat &_image, int _loop_index, 42 | Eigen::Matrix &_loop_info, 43 | vector &_keypoints, 44 | vector &_keypoints_norm, 45 | vector &_brief_descriptors); 46 | bool findConnection(KeyFrame *old_kf); 47 | void computeWindowBRIEFPoint(); 48 | void computeBRIEFPoint(); 49 | // void extractBrief(); 50 | int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b); 51 | bool searchInAera(const BRIEF::bitset window_descriptor, 52 | const std::vector &descriptors_old, 53 | const std::vector &keypoints_old, 54 | const std::vector &keypoints_old_norm, 55 | cv::Point2f &best_match, cv::Point2f &best_match_norm); 56 | void searchByBRIEFDes(std::vector &matched_2d_old, 57 | std::vector &matched_2d_old_norm, 58 | std::vector &status, 59 | const std::vector &descriptors_old, 60 | const std::vector &keypoints_old, 61 | const std::vector &keypoints_old_norm); 62 | void 63 | FundmantalMatrixRANSAC(const std::vector &matched_2d_cur_norm, 64 | const std::vector &matched_2d_old_norm, 65 | vector &status); 66 | void PnPRANSAC(const vector &matched_2d_old_norm, 67 | const std::vector &matched_3d, 68 | std::vector &status, Eigen::Vector3d &PnP_T_old, 69 | Eigen::Matrix3d &PnP_R_old); 70 | void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 71 | void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 72 | void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); 73 | void updateVioPose(const Eigen::Vector3d &_T_w_i, 74 | const Eigen::Matrix3d &_R_w_i); 75 | void updateLoop(Eigen::Matrix &_loop_info); 76 | 77 | Eigen::Vector3d getLoopRelativeT(); 78 | double getLoopRelativeYaw(); 79 | Eigen::Quaterniond getLoopRelativeQ(); 80 | 81 | double time_stamp; 82 | int index; 83 | int local_index; 84 | Eigen::Vector3d vio_T_w_i; 85 | Eigen::Matrix3d vio_R_w_i; 86 | Eigen::Vector3d T_w_i; 87 | Eigen::Matrix3d R_w_i; 88 | Eigen::Vector3d origin_vio_T; 89 | Eigen::Matrix3d origin_vio_R; 90 | cv::Mat image; 91 | cv::Mat thumbnail; 92 | vector point_3d; 93 | vector point_2d_uv; 94 | vector point_2d_norm; 95 | vector point_id; 96 | vector keypoints; 97 | vector keypoints_norm; 98 | vector window_keypoints; 99 | vector brief_descriptors; 100 | vector window_brief_descriptors; 101 | bool has_fast_point; 102 | int sequence; 103 | 104 | bool has_loop; 105 | int loop_index; 106 | Eigen::Matrix loop_info; 107 | }; 108 | -------------------------------------------------------------------------------- /pose_graph/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "parameters.h" 11 | 12 | class CameraPoseVisualization { 13 | public: 14 | std::string m_marker_ns; 15 | 16 | CameraPoseVisualization(float r, float g, float b, float a); 17 | 18 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 19 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 20 | void setScale(double s); 21 | void setLineWidth(double width); 22 | 23 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 24 | void reset(); 25 | 26 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 27 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 28 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 29 | //void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src); 30 | void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header); 31 | private: 32 | std::vector m_markers; 33 | std_msgs::ColorRGBA m_image_boundary_color; 34 | std_msgs::ColorRGBA m_optical_center_connector_color; 35 | double m_scale; 36 | double m_line_width; 37 | 38 | static const Eigen::Vector3d imlt; 39 | static const Eigen::Vector3d imlb; 40 | static const Eigen::Vector3d imrt; 41 | static const Eigen::Vector3d imrb; 42 | static const Eigen::Vector3d oc ; 43 | static const Eigen::Vector3d lt0 ; 44 | static const Eigen::Vector3d lt1 ; 45 | static const Eigen::Vector3d lt2 ; 46 | }; 47 | -------------------------------------------------------------------------------- /pose_graph/src/utility/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "camodocal/camera_models/CameraFactory.h" 4 | #include "camodocal/camera_models/CataCamera.h" 5 | #include "camodocal/camera_models/PinholeCamera.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | extern camodocal::CameraPtr m_camera; 14 | extern Eigen::Vector3d tic; 15 | extern Eigen::Matrix3d qic; 16 | extern Eigen::Matrix ti_d; 17 | extern Eigen::Matrix qi_d; 18 | extern ros::Publisher pub_match_img; 19 | extern ros::Publisher pub_match_points; 20 | extern int VISUALIZATION_SHIFT_X; 21 | extern int VISUALIZATION_SHIFT_Y; 22 | extern std::string BRIEF_PATTERN_FILE; 23 | extern std::string POSE_GRAPH_SAVE_PATH; 24 | extern int ROW; 25 | extern int COL; 26 | extern std::string VINS_RESULT_PATH; 27 | extern int DEBUG_IMAGE; 28 | extern int FAST_RELOCALIZATION; 29 | 30 | 31 | -------------------------------------------------------------------------------- /pose_graph/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /pose_graph/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /pose_graph/src/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Utility 9 | { 10 | public: 11 | template 12 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 13 | { 14 | typedef typename Derived::Scalar Scalar_t; 15 | 16 | Eigen::Quaternion dq; 17 | Eigen::Matrix half_theta = theta; 18 | half_theta /= static_cast(2.0); 19 | dq.w() = static_cast(1.0); 20 | dq.x() = half_theta.x(); 21 | dq.y() = half_theta.y(); 22 | dq.z() = half_theta.z(); 23 | return dq; 24 | } 25 | 26 | template 27 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 28 | { 29 | Eigen::Matrix ans; 30 | ans << typename Derived::Scalar(0), -q(2), q(1), 31 | q(2), typename Derived::Scalar(0), -q(0), 32 | -q(1), q(0), typename Derived::Scalar(0); 33 | return ans; 34 | } 35 | 36 | template 37 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 38 | { 39 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 40 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 41 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 42 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 43 | return q; 44 | } 45 | 46 | template 47 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 48 | { 49 | Eigen::Quaternion qq = positify(q); 50 | Eigen::Matrix ans; 51 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 52 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 53 | return ans; 54 | } 55 | 56 | template 57 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 58 | { 59 | Eigen::Quaternion pp = positify(p); 60 | Eigen::Matrix ans; 61 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 62 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 63 | return ans; 64 | } 65 | 66 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 67 | { 68 | Eigen::Vector3d n = R.col(0); 69 | Eigen::Vector3d o = R.col(1); 70 | Eigen::Vector3d a = R.col(2); 71 | 72 | Eigen::Vector3d ypr(3); 73 | double y = atan2(n(1), n(0)); 74 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 75 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 76 | ypr(0) = y; 77 | ypr(1) = p; 78 | ypr(2) = r; 79 | 80 | return ypr / M_PI * 180.0; 81 | } 82 | 83 | template 84 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 85 | { 86 | typedef typename Derived::Scalar Scalar_t; 87 | 88 | Scalar_t y = ypr(0) / 180.0 * M_PI; 89 | Scalar_t p = ypr(1) / 180.0 * M_PI; 90 | Scalar_t r = ypr(2) / 180.0 * M_PI; 91 | 92 | Eigen::Matrix Rz; 93 | Rz << cos(y), -sin(y), 0, 94 | sin(y), cos(y), 0, 95 | 0, 0, 1; 96 | 97 | Eigen::Matrix Ry; 98 | Ry << cos(p), 0., sin(p), 99 | 0., 1., 0., 100 | -sin(p), 0., cos(p); 101 | 102 | Eigen::Matrix Rx; 103 | Rx << 1., 0., 0., 104 | 0., cos(r), -sin(r), 105 | 0., sin(r), cos(r); 106 | 107 | return Rz * Ry * Rx; 108 | } 109 | 110 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 111 | 112 | template 113 | struct uint_ 114 | { 115 | }; 116 | 117 | template 118 | void unroller(const Lambda &f, const IterT &iter, uint_) 119 | { 120 | unroller(f, iter, uint_()); 121 | f(iter + N); 122 | } 123 | 124 | template 125 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 126 | { 127 | f(iter); 128 | } 129 | 130 | template 131 | static T normalizeAngle(const T& angle_degrees) { 132 | T two_pi(2.0 * 180); 133 | if (angle_degrees > 0) 134 | return angle_degrees - 135 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 136 | else 137 | return angle_degrees + 138 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 139 | }; 140 | }; 141 | -------------------------------------------------------------------------------- /support_files/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jianhengLiu/VINS-RGBD-FAST/757c256c747a3a24f13e9d71fc7da0fb857dea21/support_files/brief_k10L6.bin -------------------------------------------------------------------------------- /vins_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vins_estimator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | # set(CMAKE_BUILD_TYPE "RelWithDebInfo") 6 | # set(CMAKE_BUILD_TYPE "Debug") 7 | set(CMAKE_CXX_FLAGS "-std=c++14") 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | geometry_msgs 14 | nav_msgs 15 | tf 16 | cv_bridge 17 | camera_model 18 | message_filters 19 | image_transport 20 | 21 | nodelet 22 | ) 23 | 24 | find_package(OpenCV REQUIRED) 25 | 26 | # message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}") 27 | 28 | find_package(Ceres REQUIRED) 29 | 30 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 31 | find_package(Eigen3) 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ${CERES_INCLUDE_DIRS} 35 | ${EIGEN3_INCLUDE_DIR} 36 | ) 37 | 38 | catkin_package() 39 | 40 | add_library(vins_lib 41 | src/utility/parameters.cpp 42 | src/estimator/estimator.cpp 43 | src/feature_manager/feature_manager.cpp 44 | src/factor/pose_local_parameterization.cpp 45 | src/factor/projection_factor.cpp 46 | src/factor/projection_td_factor.cpp 47 | src/factor/marginalization_factor.cpp 48 | src/utility/utility.cpp 49 | src/utility/visualization.cpp 50 | src/utility/CameraPoseVisualization.cpp 51 | src/initial/solve_5pts.cpp 52 | src/initial/initial_aligment.cpp 53 | src/initial/initial_sfm.cpp 54 | src/initial/initial_ex_rotation.cpp 55 | src/feature_tracker/feature_tracker.cpp) 56 | target_link_libraries(vins_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES} /usr/local/lib/libSophus.so 57 | ) 58 | 59 | add_library(estimator_nodelet src/estimator_nodelet.cpp) 60 | target_link_libraries(estimator_nodelet vins_lib) 61 | -------------------------------------------------------------------------------- /vins_estimator/launch/atlas200/compressed2img.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | -------------------------------------------------------------------------------- /vins_estimator/launch/atlas200/img2compressed.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /vins_estimator/launch/openloris/openloris_vio_atlas.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 28 | 29 | -------------------------------------------------------------------------------- /vins_estimator/launch/openloris/openloris_vio_pytorch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /vins_estimator/launch/openloris/openloris_vio_tensorrt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 26 | 27 | -------------------------------------------------------------------------------- /vins_estimator/launch/openloris/openloris_vo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 23 | 24 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense/realsense_vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | -------------------------------------------------------------------------------- /vins_estimator/launch/realsense/realsense_vio_atlas.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 26 | 27 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum_rgbd/tum_rgbd_atlas.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 27 | 28 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum_rgbd/tum_rgbd_pytorch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /vins_estimator/launch/tum_rgbd/tum_rgbd_tensorrt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 26 | 27 | -------------------------------------------------------------------------------- /vins_estimator/launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /vins_estimator/launch/yolo/atlas.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /vins_estimator/launch/yolo/pytorch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /vins_estimator/launch/yolo/tensorrt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /vins_estimator/nodelet_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | EstimatorNodelet 4 | 5 | -------------------------------------------------------------------------------- /vins_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vins_estimator 4 | 0.0.0 5 | The vins_estimator package 6 | 7 | 8 | 9 | 10 | qintong 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | roscpp 20 | camera_model 21 | roscpp 22 | camera_model 23 | 24 | nodelet 25 | nodelet 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/parameters.h" 11 | #include "../utility/tic_toc.h" 12 | #include "../utility/utility.h" 13 | 14 | const int NUM_MAR_THREADS = 4; 15 | 16 | struct ResidualBlockInfo 17 | { 18 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, 19 | std::vector _parameter_blocks, std::vector _drop_set) 20 | : cost_function(_cost_function), loss_function(_loss_function), 21 | parameter_blocks(_parameter_blocks), drop_set(_drop_set) 22 | { 23 | } 24 | 25 | void Evaluate(); 26 | 27 | ceres::CostFunction *cost_function; 28 | ceres::LossFunction *loss_function; 29 | std::vector parameter_blocks; 30 | std::vector drop_set; 31 | 32 | double **raw_jacobians; 33 | std::vector> jacobians; 34 | Eigen::VectorXd residuals; 35 | 36 | int localSize(int size) 37 | { 38 | return size == 7 ? 6 : size; 39 | } 40 | }; 41 | 42 | struct ThreadsStruct 43 | { 44 | std::vector sub_factors; 45 | Eigen::MatrixXd A; 46 | Eigen::VectorXd b; 47 | std::unordered_map parameter_block_size; // global size 48 | std::unordered_map parameter_block_idx; // local size 49 | }; 50 | 51 | class MarginalizationInfo 52 | { 53 | public: 54 | ~MarginalizationInfo(); 55 | int localSize(int size) const; 56 | int globalSize(int size) const; 57 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 58 | void preMarginalize(); 59 | void marginalize(); 60 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 61 | 62 | std::vector factors; 63 | int m, n; 64 | std::unordered_map parameter_block_size; // global size 65 | int sum_block_size; 66 | std::unordered_map parameter_block_idx; // local size 67 | std::unordered_map parameter_block_data; 68 | 69 | std::vector keep_block_size; // global size 70 | std::vector keep_block_idx; // local size 71 | std::vector keep_block_data; 72 | 73 | Eigen::MatrixXd linearized_jacobians; 74 | Eigen::VectorXd linearized_residuals; 75 | const double eps = 1e-8; 76 | }; 77 | 78 | class MarginalizationFactor : public ceres::CostFunction 79 | { 80 | public: 81 | MarginalizationFactor(MarginalizationInfo *_marginalization_info); 82 | virtual bool Evaluate(double const *const *parameters, double *residuals, 83 | double **jacobians) const; 84 | 85 | MarginalizationInfo *marginalization_info; 86 | }; 87 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, 4 | double *x_plus_delta) const 5 | { 6 | Eigen::Map _p(x); 7 | Eigen::Map _q(x + 3); 8 | 9 | Eigen::Map dp(delta); 10 | 11 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 12 | 13 | Eigen::Map p(x_plus_delta); 14 | Eigen::Map q(x_plus_delta + 3); 15 | 16 | p = _p + dp; 17 | q = (_q * dq).normalized(); 18 | 19 | return true; 20 | } 21 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 22 | { 23 | Eigen::Map> j(jacobian); 24 | j.topRows<6>().setIdentity(); 25 | j.bottomRows<1>().setZero(); 26 | 27 | return true; 28 | } 29 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const 12 | { 13 | return 7; 14 | }; 15 | virtual int LocalSize() const 16 | { 17 | return 6; 18 | }; 19 | }; 20 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../utility/parameters.h" 9 | 10 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, 15 | double **jacobians) const; 16 | void check(double **parameters); 17 | 18 | Eigen::Vector3d pts_i, pts_j; 19 | Eigen::Matrix tangent_base; 20 | static Eigen::Matrix2d sqrt_info; 21 | static double sum_t; 22 | }; 23 | -------------------------------------------------------------------------------- /vins_estimator/src/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../utility/parameters.h" 9 | 10 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, 16 | const double _row_j); 17 | virtual bool Evaluate(double const *const *parameters, double *residuals, 18 | double **jacobians) const; 19 | void check(double **parameters); 20 | 21 | Eigen::Vector3d pts_i, pts_j; 22 | Eigen::Vector3d velocity_i, velocity_j; 23 | double td_i, td_j; 24 | Eigen::Matrix tangent_base; 25 | double row_i, row_j; 26 | static Eigen::Matrix2d sqrt_info; 27 | static double sum_t; 28 | }; 29 | -------------------------------------------------------------------------------- /vins_estimator/src/feature_manager/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | 11 | #include 12 | 13 | using namespace Eigen; 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include "../utility/parameters.h" 21 | 22 | #include "../utility/tic_toc.h" 23 | 24 | struct DynamicObject 25 | { 26 | int x_center; 27 | int y_center; 28 | int x_vel; 29 | int y_vel; 30 | int x_weight_vel; 31 | int y_weight_vel; 32 | int no_update_times; 33 | bool is_update; 34 | int x1; 35 | int y1; 36 | int x2; 37 | int y2; 38 | }; 39 | 40 | class FeaturePerFrame 41 | { 42 | public: 43 | FeaturePerFrame(Eigen::Matrix _point, double td, double _depth) 44 | { 45 | point.x() = _point(0); 46 | point.y() = _point(1); 47 | point.z() = _point(2); 48 | uv.x() = _point(3); 49 | uv.y() = _point(4); 50 | velocity.x() = _point(5); 51 | velocity.y() = _point(6); 52 | depth = _depth; 53 | cur_td = td; 54 | } 55 | 56 | double cur_td; 57 | Vector3d point; 58 | Vector2d uv; 59 | Vector2d velocity; 60 | double z{}; 61 | bool is_used{}; 62 | MatrixXd A; 63 | VectorXd b; 64 | double depth; 65 | }; 66 | 67 | class FeaturePerId 68 | { 69 | public: 70 | const int feature_id; 71 | int start_frame; 72 | vector feature_per_frame; 73 | 74 | int used_num; 75 | bool is_dynamic; 76 | double estimated_depth; 77 | int estimate_flag; // 0 initial; 1 by depth image; 2 by triangulate 78 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 79 | 80 | FeaturePerId(int _feature_id, int _start_frame) 81 | : feature_id(_feature_id), start_frame(_start_frame), used_num(0), is_dynamic(false), 82 | estimated_depth(-1.0), estimate_flag(0), solve_flag(0) 83 | { 84 | } 85 | 86 | int endFrame(); 87 | }; 88 | 89 | class FeatureManager 90 | { 91 | public: 92 | explicit FeatureManager(Matrix3d Rs[]); 93 | 94 | void setRic(Matrix3d _ric[]); 95 | 96 | void clearState(); 97 | 98 | int getFeatureCount(); 99 | 100 | bool addFeatureCheckParallax(int frame_count, map> &image, 101 | double td); 102 | 103 | void debugShow(); 104 | 105 | vector> getCorresponding(int frame_count_l, int frame_count_r); 106 | 107 | vector> getCorrespondingWithDepth(int frame_count_l, 108 | int frame_count_r); 109 | 110 | // void updateDepth(const VectorXd &x); 111 | void setDepth(const VectorXd &x); 112 | 113 | void removeFailures(); 114 | void removeFailuresOutliers(Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d _ric[]); 115 | 116 | void clearDepth(const VectorXd &x); 117 | 118 | VectorXd getDepthVector(); 119 | 120 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d _ric[]); 121 | 122 | void triangulateWithDepth(Vector3d Ps[], Vector3d _tic[], Matrix3d _ric[]); 123 | 124 | void initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], 125 | Matrix3d ric[]); 126 | 127 | bool solvePoseByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, 128 | vector &pts2D, vector &pts3D); 129 | 130 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, 131 | Eigen::Vector3d new_P); 132 | 133 | void removeBack(); 134 | 135 | void removeFront(int frame_count); 136 | 137 | void removeOutlier(set &outlierIndex); 138 | 139 | void inputDepth(const cv::Mat &_depth_img); 140 | 141 | void seedFilling(int x, int y, unsigned short last_depth, unsigned short _object_id); 142 | 143 | list feature; // cl:Lists将元素按顺序储存在链表中. 与 向量(vectors)相比, 144 | // 它允许快速的插入和删除，但是随机访问却比较慢. 145 | int last_track_num{}; 146 | 147 | cv::Mat depth_img; 148 | 149 | private: 150 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 151 | 152 | const Matrix3d *Rs; 153 | Matrix3d ric[NUM_OF_CAM]; 154 | }; 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /vins_estimator/src/feature_tracker/ThreadPool.h: -------------------------------------------------------------------------------- 1 | #ifndef THREAD_POOL_H 2 | #define THREAD_POOL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class ThreadPool 15 | { 16 | public: 17 | ThreadPool(size_t); 18 | template 19 | auto enqueue(F &&f, Args &&...args) -> std::future::type>; 20 | ~ThreadPool(); 21 | 22 | // void wait_workers(); 23 | 24 | private: 25 | // need to keep track of threads so we can join them 26 | std::vector workers; 27 | // the task queue 28 | std::queue> tasks; 29 | 30 | // synchronization 31 | std::mutex queue_mutex; 32 | std::condition_variable condition; 33 | bool stop; 34 | }; 35 | 36 | // the constructor just launches some amount of workers 37 | inline ThreadPool::ThreadPool(size_t threads) : stop(false) 38 | { 39 | for (size_t i = 0; i < threads; ++i) 40 | workers.emplace_back( 41 | [this] 42 | { 43 | for (;;) 44 | { 45 | std::function task; 46 | { 47 | std::unique_lock lock(this->queue_mutex); 48 | this->condition.wait(lock, 49 | [this] { return this->stop || !this->tasks.empty(); }); 50 | if (this->stop && this->tasks.empty()) 51 | return; 52 | task = std::move(this->tasks.front()); 53 | this->tasks.pop(); 54 | } 55 | 56 | task(); 57 | } 58 | }); 59 | } 60 | 61 | // add new work item to the pool 62 | template 63 | auto ThreadPool::enqueue(F &&f, Args &&...args) 64 | -> std::future::type> 65 | { 66 | using return_type = typename std::result_of::type; 67 | 68 | auto task = std::make_shared>( 69 | std::bind(std::forward(f), std::forward(args)...)); 70 | 71 | std::future res = task->get_future(); 72 | { 73 | std::unique_lock lock(queue_mutex); 74 | 75 | // don't allow enqueueing after stopping the pool 76 | if (stop) 77 | throw std::runtime_error("enqueue on stopped ThreadPool"); 78 | 79 | tasks.emplace([task]() { (*task)(); }); 80 | } 81 | condition.notify_one(); 82 | return res; 83 | } 84 | 85 | // the destructor joins all threads 86 | inline ThreadPool::~ThreadPool() 87 | { 88 | { 89 | std::unique_lock lock(queue_mutex); 90 | stop = true; 91 | } 92 | condition.notify_all(); 93 | for (std::thread &worker : workers) 94 | worker.join(); 95 | } 96 | 97 | // inline void ThreadPool::wait_workers() { 98 | // for (std::thread &worker : workers) 99 | // worker.join(); 100 | // } 101 | 102 | #endif 103 | -------------------------------------------------------------------------------- /vins_estimator/src/feature_tracker/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camodocal/camera_models/CameraFactory.h" 13 | #include "camodocal/camera_models/CataCamera.h" 14 | #include "camodocal/camera_models/PinholeCamera.h" 15 | 16 | #include "../utility/parameters.h" 17 | #include "../utility/tic_toc.h" 18 | 19 | #include "ThreadPool.h" 20 | 21 | using namespace std; 22 | using namespace camodocal; 23 | using namespace Eigen; 24 | 25 | bool inBorder(const cv::Point2f &pt); 26 | 27 | void reduceVector(vector &v, vector status); 28 | 29 | void reduceVector(vector &v, vector status); 30 | 31 | class FeatureTracker 32 | { 33 | public: 34 | FeatureTracker(); 35 | 36 | void readImage(const cv::Mat &_img, double _cur_time, 37 | const Matrix3d &_relative_R = Matrix3d::Identity()); 38 | 39 | void setMask(); 40 | 41 | void addPoints(); 42 | 43 | void addPoints(vector &Keypts); 44 | 45 | void addPoints(int n_max_cnt, vector &Keypts); 46 | 47 | bool updateID(unsigned int i); 48 | 49 | void readIntrinsicParameter(const string &calib_file); 50 | 51 | void showUndistortion(const string &name); 52 | 53 | void rejectWithF(); 54 | 55 | void undistortedPoints(); 56 | 57 | void predictPtsInNextFrame(const Matrix3d &_relative_R); 58 | 59 | Eigen::Vector3d get3dPt(const cv::Mat &_depth, const cv::Point2f &pt); 60 | 61 | void initGridsDetector(); 62 | 63 | static bool inBorder(const cv::Point2f &pt); 64 | 65 | std::vector gridDetect(size_t grid_id); 66 | 67 | cv::Mat mask; 68 | cv::Mat fisheye_mask; 69 | cv::Mat cur_img, forw_img; 70 | vector n_pts; 71 | vector cur_pts, forw_pts, predict_pts, unstable_pts; 72 | vector prev_un_pts, cur_un_pts; 73 | vector pts_velocity; 74 | vector ids; 75 | vector track_cnt; 76 | map cur_un_pts_map; 77 | map prev_un_pts_map; 78 | camodocal::CameraPtr m_camera; 79 | double cur_time{}; 80 | double prev_time{}; 81 | 82 | int n_id; 83 | cv::Ptr p_fast_feature_detector; 84 | 85 | std::shared_ptr pool; 86 | 87 | std::vector grids_rect; 88 | std::vector grids_track_num; 89 | std::vector grids_texture_status; // true: abundant texture grid; false: 90 | // textureless grid 91 | int grid_height{}; 92 | int grid_width{}; 93 | int grid_res_height{}; 94 | int grid_res_width{}; 95 | int grids_threshold{}; 96 | cv::Mat grids_detector_img; 97 | }; 98 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../factor/imu_factor.h" 3 | #include "../feature_manager/feature_manager.h" 4 | #include "../utility/utility.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | class ImageFrame 14 | { 15 | public: 16 | ImageFrame(){}; 17 | ImageFrame(const map> &_points, double _t) 18 | : t{_t}, is_key_frame{false} 19 | { 20 | points = _points; 21 | }; 22 | map> points; 23 | double t; 24 | Matrix3d R; 25 | Vector3d T; 26 | IntegrationBase *pre_integration; 27 | bool is_key_frame; 28 | }; 29 | 30 | bool VisualIMUAlignment(map &all_image_frame, Vector3d *Bgs, Vector3d &g, 31 | VectorXd &x); 32 | bool LinearAlignmentWithDepth(map &all_image_frame, Vector3d &g, VectorXd &x); 33 | bool LinearAlignmentWithDepthGravity(map &all_image_frame, Vector3d &g, 34 | VectorXd &x); 35 | void solveGyroscopeBias(map &all_image_frame, Vector3d *Bgs); 36 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../utility/parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally 14 | * don't konw the extrinsic parameter */ 15 | class InitialEXRotation 16 | { 17 | public: 18 | InitialEXRotation(); 19 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, 20 | Matrix3d &calib_ric_result); 21 | 22 | private: 23 | Matrix3d solveRelativeR(const vector> &corres); 24 | 25 | double testTriangulation(const vector &l, const vector &r, 26 | cv::Mat_ R, cv::Mat_ t); 27 | void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1, 28 | cv::Mat_ &t2); 29 | int frame_count; 30 | 31 | vector Rc; 32 | vector Rimu; 33 | vector Rc_g; 34 | Matrix3d ric; 35 | }; 36 | -------------------------------------------------------------------------------- /vins_estimator/src/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | struct SFMFeature 15 | { 16 | bool state; 17 | int id; 18 | vector> observation; 19 | double position[3]; 20 | double depth; 21 | vector> observation_depth; 22 | }; 23 | 24 | struct ReprojectionError3D 25 | { 26 | ReprojectionError3D(double observed_u, double observed_v) 27 | : observed_u(observed_u), observed_v(observed_v) 28 | { 29 | } 30 | 31 | template 32 | bool operator()(const T *const camera_R, const T *const camera_T, const T *point, 33 | T *residuals) const 34 | { 35 | T p[3]; 36 | ceres::QuaternionRotatePoint(camera_R, point, p); 37 | p[0] += camera_T[0]; 38 | p[1] += camera_T[1]; 39 | p[2] += camera_T[2]; 40 | T xp = p[0] / p[2]; 41 | T yp = p[1] / p[2]; 42 | residuals[0] = xp - T(observed_u); 43 | residuals[1] = yp - T(observed_v); 44 | return true; 45 | } 46 | 47 | static ceres::CostFunction *Create(const double observed_x, const double observed_y) 48 | { 49 | return (new ceres::AutoDiffCostFunction( 50 | new ReprojectionError3D(observed_x, observed_y))); 51 | } 52 | 53 | double observed_u; 54 | double observed_v; 55 | }; 56 | 57 | class GlobalSFM 58 | { 59 | public: 60 | GlobalSFM(); 61 | bool construct(int frame_num, Quaterniond *q, Vector3d *T, int l, const Matrix3d relative_R, 62 | const Vector3d relative_T, vector &sfm_f, 63 | map &sfm_tracked_points); 64 | bool constructWithDepth(int frame_num, Quaterniond *q, Vector3d *T, int l, 65 | const Matrix3d relative_R, const Vector3d relative_T, 66 | vector &sfm_f, map &sfm_tracked_points); 67 | 68 | private: 69 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, 70 | vector &sfm_f); 71 | 72 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 73 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 74 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, int frame1, 75 | Eigen::Matrix &Pose1, vector &sfm_f); 76 | void triangulateTwoFramesWithDepth(int frame0, Eigen::Matrix &Pose0, int frame1, 77 | Eigen::Matrix &Pose1, 78 | vector &sfm_f); 79 | 80 | int feature_num; 81 | }; -------------------------------------------------------------------------------- /vins_estimator/src/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 17 | bool solveRelativeRT_ICP(vector> &corres, Matrix3d &R, Vector3d &T); 18 | bool solveRelativeRT_PNP(const vector> &corres, Matrix3d &Rotation, 19 | Vector3d &Translation); 20 | 21 | private: 22 | double testTriangulation(const vector &l, const vector &r, 23 | cv::Mat_ R, cv::Mat_ t); 24 | void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, cv::Mat_ &t1, 25 | cv::Mat_ &t2); 26 | }; 27 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization 11 | { 12 | public: 13 | std::string m_marker_ns; 14 | 15 | CameraPoseVisualization(float r, float g, float b, float a); 16 | 17 | void setImageBoundaryColor(float r, float g, float b, float a = 1.0); 18 | void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0); 19 | void setScale(double s); 20 | void setLineWidth(double width); 21 | 22 | void add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q); 23 | void reset(); 24 | 25 | void publish_by(ros::Publisher &pub, const std_msgs::Header &header); 26 | void add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); 27 | void add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); 28 | 29 | private: 30 | std::vector m_markers; 31 | std_msgs::ColorRGBA m_image_boundary_color; 32 | std_msgs::ColorRGBA m_optical_center_connector_color; 33 | double m_scale; 34 | double m_line_width; 35 | 36 | static const Eigen::Vector3d imlt; 37 | static const Eigen::Vector3d imlb; 38 | static const Eigen::Vector3d imrt; 39 | static const Eigen::Vector3d imrb; 40 | static const Eigen::Vector3d oc; 41 | static const Eigen::Vector3d lt0; 42 | static const Eigen::Vector3d lt1; 43 | static const Eigen::Vector3d lt2; 44 | }; 45 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "utility.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | const double FOCAL_LENGTH = 460.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | //#define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | extern Eigen::Vector3d G; 27 | 28 | extern double BIAS_ACC_THRESHOLD; 29 | extern double BIAS_GYR_THRESHOLD; 30 | extern double SOLVER_TIME; 31 | extern int NUM_ITERATIONS; 32 | extern std::string EX_CALIB_RESULT_PATH; 33 | extern std::string VINS_RESULT_PATH; 34 | extern std::string IMAGE_TOPIC; 35 | extern std::string DEPTH_TOPIC; 36 | extern std::string IMU_TOPIC; 37 | extern double TD; 38 | extern double TR; 39 | extern int ESTIMATE_TD; 40 | extern int ROLLING_SHUTTER; 41 | extern double ROW, COL; 42 | 43 | extern int IMAGE_SIZE; 44 | 45 | extern double DEPTH_MIN_DIST; 46 | extern double DEPTH_MAX_DIST; 47 | extern unsigned short DEPTH_MIN_DIST_MM; 48 | extern unsigned short DEPTH_MAX_DIST_MM; 49 | extern int MAX_CNT; 50 | extern int MAX_CNT_SET; 51 | extern int MIN_DIST; 52 | extern int FREQ; 53 | extern double F_THRESHOLD; 54 | extern int SHOW_TRACK; 55 | extern int EQUALIZE; 56 | extern int FISHEYE; 57 | extern std::string FISHEYE_MASK; 58 | extern std::string CAM_NAMES; 59 | extern int STEREO_TRACK; 60 | extern bool PUB_THIS_FRAME; 61 | extern Eigen::Matrix3d Ric; 62 | 63 | extern std::vector SEMANTIC_LABEL; 64 | extern std::vector STATIC_LABEL; 65 | extern std::vector DYNAMIC_LABEL; 66 | 67 | extern int NUM_GRID_ROWS; 68 | extern int NUM_GRID_COLS; 69 | 70 | extern int FRONTEND_FREQ; 71 | 72 | extern int USE_IMU; 73 | extern int NUM_THREADS; 74 | 75 | extern int STATIC_INIT; 76 | 77 | extern int FIX_DEPTH; 78 | 79 | void readParameters(ros::NodeHandle &n); 80 | 81 | enum SIZE_PARAMETERIZATION 82 | { 83 | SIZE_POSE = 7, 84 | SIZE_SPEEDBIAS = 9, 85 | // SIZE_SPEED = 3, 86 | // SIZE_BIAS = 6, 87 | SIZE_FEATURE = 1 88 | }; 89 | 90 | enum StateOrder 91 | { 92 | O_P = 0, 93 | O_R = 3, 94 | O_V = 6, 95 | O_BA = 9, 96 | O_BG = 12 97 | }; 98 | 99 | enum NoiseOrder 100 | { 101 | O_AN = 0, 102 | O_GN = 3, 103 | O_AW = 6, 104 | O_GW = 9 105 | }; 106 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | // https://blog.csdn.net/huanghaihui_123/article/details/103075107#commentBox 4 | // R_W_I 5 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 6 | { 7 | Eigen::Matrix3d R0; 8 | Eigen::Vector3d ng1 = g.normalized(); 9 | Eigen::Vector3d ng2{0, 0, 1.0}; 10 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 11 | double yaw = Utility::R2ypr(R0).x(); 12 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 13 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 14 | return R0; 15 | } 16 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Utility 9 | { 10 | public: 11 | template 12 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 13 | { 14 | typedef typename Derived::Scalar Scalar_t; 15 | 16 | Eigen::Quaternion dq; 17 | Eigen::Matrix half_theta = theta; 18 | half_theta /= static_cast(2.0); 19 | dq.w() = static_cast(1.0); 20 | dq.x() = half_theta.x(); 21 | dq.y() = half_theta.y(); 22 | dq.z() = half_theta.z(); 23 | return dq; 24 | } 25 | 26 | template 27 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 28 | { 29 | Eigen::Matrix ans; 30 | ans << typename Derived::Scalar(0), -q(2), q(1), 31 | q(2), typename Derived::Scalar(0), -q(0), 32 | -q(1), q(0), typename Derived::Scalar(0); 33 | return ans; 34 | } 35 | 36 | template 37 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 38 | { 39 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 40 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 41 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 42 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 43 | return q; 44 | } 45 | 46 | template 47 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 48 | { 49 | Eigen::Quaternion qq = positify(q); 50 | Eigen::Matrix ans; 51 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 52 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 53 | return ans; 54 | } 55 | 56 | template 57 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 58 | { 59 | Eigen::Quaternion pp = positify(p); 60 | Eigen::Matrix ans; 61 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 62 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 63 | return ans; 64 | } 65 | 66 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 67 | { 68 | Eigen::Vector3d n = R.col(0); 69 | Eigen::Vector3d o = R.col(1); 70 | Eigen::Vector3d a = R.col(2); 71 | 72 | Eigen::Vector3d ypr(3); 73 | double y = atan2(n(1), n(0)); 74 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 75 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 76 | ypr(0) = y; 77 | ypr(1) = p; 78 | ypr(2) = r; 79 | 80 | return ypr / M_PI * 180.0; 81 | } 82 | 83 | template 84 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 85 | { 86 | typedef typename Derived::Scalar Scalar_t; 87 | 88 | Scalar_t y = ypr(0) / 180.0 * M_PI; 89 | Scalar_t p = ypr(1) / 180.0 * M_PI; 90 | Scalar_t r = ypr(2) / 180.0 * M_PI; 91 | 92 | Eigen::Matrix Rz; 93 | Rz << cos(y), -sin(y), 0, 94 | sin(y), cos(y), 0, 95 | 0, 0, 1; 96 | 97 | Eigen::Matrix Ry; 98 | Ry << cos(p), 0., sin(p), 99 | 0., 1., 0., 100 | -sin(p), 0., cos(p); 101 | 102 | Eigen::Matrix Rx; 103 | Rx << 1., 0., 0., 104 | 0., cos(r), -sin(r), 105 | 0., sin(r), cos(r); 106 | 107 | return Rz * Ry * Rx; 108 | } 109 | 110 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 111 | 112 | template 113 | struct uint_ 114 | { 115 | }; 116 | 117 | template 118 | void unroller(const Lambda &f, const IterT &iter, uint_) 119 | { 120 | unroller(f, iter, uint_()); 121 | f(iter + N); 122 | } 123 | 124 | template 125 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 126 | { 127 | f(iter); 128 | } 129 | 130 | template 131 | static T normalizeAngle(const T& angle_degrees) { 132 | T two_pi(2.0 * 180); 133 | if (angle_degrees > 0) 134 | return angle_degrees - 135 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 136 | else 137 | return angle_degrees + 138 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 139 | }; 140 | }; 141 | -------------------------------------------------------------------------------- /vins_estimator/src/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../estimator/estimator.h" 4 | #include "CameraPoseVisualization.h" 5 | #include "parameters.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | extern ros::Publisher pub_odometry; 27 | extern ros::Publisher pub_path, pub_pose; 28 | extern ros::Publisher pub_cloud, pub_map; 29 | extern ros::Publisher pub_key_poses; 30 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 31 | extern ros::Publisher pub_key; 32 | extern nav_msgs::Path path; 33 | extern ros::Publisher pub_pose_graph; 34 | extern int IMAGE_ROW, IMAGE_COL; 35 | 36 | void registerPub(ros::NodeHandle &n); 37 | 38 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, 39 | const Eigen::Vector3d &V, const double &t); 40 | 41 | // void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, 42 | // const Eigen::Vector3d &V, 43 | // // const Eigen::Vector3d &A, 44 | // // const Eigen::Vector3d &omega, 45 | // const std_msgs::Header &header); 46 | 47 | void printStatistics(const Estimator &estimator, double t); 48 | 49 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 50 | 51 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 52 | 53 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 54 | 55 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 56 | 57 | void pubIMUPose(const Estimator &estimator, const std_msgs::Header &header); 58 | 59 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 60 | 61 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 62 | 63 | void pubKeyframe(const Estimator &estimator); 64 | 65 | void pubRelocalization(const Estimator &estimator); 66 | 67 | void pubTrackImg(const cv_bridge::CvImageConstPtr &img_ptr); 68 | 69 | void pubTrackImg(const sensor_msgs::ImagePtr &img_msg); 70 | 71 | void pubTrackImg(const cv::Mat &img); 72 | --------------------------------------------------------------------------------