├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── comparative_test.yaml ├── extention_test.yaml ├── orbvoc │ └── ORBvoc.bin └── rviz │ └── default.rviz ├── core ├── include │ └── dre_slam │ │ ├── ORBextractor.h │ │ ├── camera.h │ │ ├── common.h │ │ ├── config.h │ │ ├── dre_slam.h │ │ ├── dynamic_pixel_culling.h │ │ ├── dynamic_pixel_detector.h │ │ ├── encoder.h │ │ ├── encoder_integration.h │ │ ├── feature_detector.h │ │ ├── frame.h │ │ ├── keyframe.h │ │ ├── local_mapping.h │ │ ├── loop_closing.h │ │ ├── map.h │ │ ├── map_point.h │ │ ├── octomap_fusion.h │ │ ├── optimizer.h │ │ ├── ros_puber.h │ │ ├── run_timer.h │ │ ├── sub_octomap.h │ │ ├── sub_octomap_construction.h │ │ ├── tracking.h │ │ └── vocabulary.h └── src │ ├── ORBextractor.cpp │ ├── camera.cpp │ ├── common.cpp │ ├── config.cpp │ ├── dre_slam.cpp │ ├── dynamic_pixel_culling.cpp │ ├── dynamic_pixel_detector.cpp │ ├── encoder_integration.cpp │ ├── feature_detector.cpp │ ├── frame.cpp │ ├── keyframe.cpp │ ├── local_mapping.cpp │ ├── loop_closing.cpp │ ├── map.cpp │ ├── map_point.cpp │ ├── octomap_fusion.cpp │ ├── optimizer.cpp │ ├── ros_puber.cpp │ ├── sub_octomap.cpp │ ├── sub_octomap_construction.cpp │ └── tracking.cpp ├── corridor.gif ├── launch ├── comparative_test.launch └── extention_test.launch ├── node └── dre_slam_node.cpp ├── object_detector ├── CMakeLists.txt ├── object_detector.cpp └── object_detector.h ├── package.xml └── third_party ├── DBoW2 ├── CMakeLists.txt ├── DBoW2 │ ├── BowVector.cpp │ ├── BowVector.h │ ├── FClass.h │ ├── FORB.cpp │ ├── FORB.h │ ├── FeatureVector.cpp │ ├── FeatureVector.h │ ├── ScoringObject.cpp │ ├── ScoringObject.h │ └── TemplatedVocabulary.h ├── DUtils │ ├── Random.cpp │ ├── Random.h │ ├── Timestamp.cpp │ └── Timestamp.h ├── LICENSE.txt └── README.txt └── Sophus ├── CMakeLists.txt ├── README ├── SophusConfig.cmake.in ├── cmake_modules └── FindEigen3.cmake └── sophus ├── scso3.cpp ├── scso3.h ├── se2.cpp ├── se2.h ├── se3.cpp ├── se3.h ├── sim3.cpp ├── sim3.h ├── so2.cpp ├── so2.h ├── so3.cpp ├── so3.h ├── test_scso3.cpp ├── test_se2.cpp ├── test_se3.cpp ├── test_sim3.cpp ├── test_so2.cpp └── test_so3.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dre_slam) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | ## Find catkin macros and libraries 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | cv_bridge 12 | image_transport 13 | geometry_msgs 14 | message_filters 15 | pcl_ros 16 | ) 17 | 18 | catkin_package( 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | find_package(PCL REQUIRED) 23 | find_package(OpenCV 3 REQUIRED) 24 | find_package(Eigen3 REQUIRED) 25 | find_package(Ceres REQUIRED ) 26 | find_package(Sophus REQUIRED) 27 | find_package(octomap REQUIRED) 28 | 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ${OpenCV_INCLUDE_DIRS} 32 | ${EIGEN3_INCLUDE_DIR} 33 | ${PCL_INCLUDE_DIRS} 34 | ${CERES_INCLUDE_DIRS} 35 | ${Sophus_INCLUDE_DIRS} 36 | ${OCTOMAP_INCLUDE_DIRS} 37 | ${PROJECT_SOURCE_DIR} 38 | ${PROJECT_SOURCE_DIR}/core/include/ 39 | ${PROJECT_SOURCE_DIR}/third_party/ 40 | ) 41 | 42 | ## Declare a C++ library 43 | add_library(${PROJECT_NAME}_lib 44 | core/src/dre_slam.cpp 45 | core/src/config.cpp 46 | core/src/ORBextractor.cpp 47 | core/src/camera.cpp 48 | core/src/tracking.cpp 49 | core/src/local_mapping.cpp 50 | core/src/loop_closing.cpp 51 | core/src/map.cpp 52 | core/src/map_point.cpp 53 | core/src/feature_detector.cpp 54 | core/src/frame.cpp 55 | core/src/keyframe.cpp 56 | core/src/encoder_integration.cpp 57 | core/src/common.cpp 58 | core/src/optimizer.cpp 59 | core/src/dynamic_pixel_detector.cpp 60 | core/src/dynamic_pixel_culling.cpp 61 | core/src/ros_puber.cpp 62 | core/src/octomap_fusion.cpp 63 | core/src/sub_octomap.cpp 64 | core/src/sub_octomap_construction.cpp 65 | ) 66 | 67 | target_link_libraries(${PROJECT_NAME}_lib 68 | ${catkin_LIBRARIES} 69 | ${OpenCV_LIBS} 70 | ${EIGEN3_LIBS} 71 | ${PCL_LIBRARIES} 72 | ${CERES_LIBRARIES} 73 | ${Sophus_LIBRARIES} 74 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/lib/libDBoW2.so 75 | ${PROJECT_SOURCE_DIR}/object_detector/lib/libobject_detector.so 76 | ${OCTOMAP_LIBRARIES} 77 | ) 78 | 79 | 80 | # dre_slam_node 81 | add_executable(dre_slam_node 82 | node/dre_slam_node.cpp) 83 | target_link_libraries(dre_slam_node 84 | ${PROJECT_NAME}_lib 85 | ${catkin_LIBRARIES} 86 | ) 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DRE-SLAM 2 | ## Dynamic RGB-D Encoder SLAM for a Differential-Drive Robot 3 | **Authors**: [Dongsheng Yang](https://github.com/ydsf16), [Shusheng Bi](http://ir.lib.buaa.edu.cn/Scholar/ScholarCard/5784), [Wei Wang](http://ir.lib.buaa.edu.cn/Scholar/ScholarCard/5800), Chang Yuan, Wei Wang, Xianyu Qi, and [Yueri Cai](http://ir.lib.buaa.edu.cn/Scholar/ScholarCard/5785) 4 | 5 | ![image](https://github.com/ydsf16/dre_slam/blob/master/corridor.gif) 6 | 7 | **DRE-SLAM** is developed for a differential-drive robot that runs in dynamic indoor scenarios. It takes the information of an RGB-D camera and two wheel-encoders as inputs. The outputs are the 2D pose of the robot and a static background OctoMap. 8 | 9 | **Video**: [Youtube](https://youtu.be/3A5wpWgrHTI) or [Dropbox](https://www.dropbox.com/s/uvqyb3mo6tj4pf2/DRE-SLAM-20190111-v3.mp4?dl=0) or [Pan.Baidu](https://pan.baidu.com/s/1vVakfXZJziU12-vqw7Go1Q) 10 | 11 | DRE-SLAM 13 | 14 | **Paper**: ***DRE-SLAM: Dynamic RGB-D Encoder SLAM for a Differential-Drive Robot***, Dongsheng Yang, Shusheng Bi, Wei Wang, Chang Yuan, Wei Wang, Xianyu Qi, and Yueri Cai. (Remote Sensing, 2019) [PDF](https://www.mdpi.com/2072-4292/11/4/380/pdf), [WEB](https://www.mdpi.com/2072-4292/11/4/380) 15 | 16 | # Prerequisites 17 | ### 1. **Ubuntu 16.04** 18 | 19 | ### 2. **[ROS Kinetic](http://wiki.ros.org)** 20 | Follow the instructions in: 21 | ### 3. **ROS pacakges** 22 | ``` 23 | sudo apt-get install ros-kinetic-cv-bridge ros-kinetic-tf ros-kinetic-message-filters ros-kinetic-image-transport ros-kinetic-octomap ros-kinetic-octomap-msgs ros-kinetic-octomap-ros ros-kinetic-octomap-rviz-plugins ros-kinetic-octomap-server ros-kinetic-pcl-ros ros-kinetic-pcl-msgs ros-kinetic-pcl-conversions ros-kinetic-geometry-msgs 24 | ``` 25 | 26 | ### 4. [OpenCV 4.0](https://opencv.org/opencv-4-0-0.html) 27 | We use the YOLOv3 implemented in OpenCV 4.0. 28 | 29 | Follow the instructions in: 30 | 31 | ### 5. Ceres 32 | Follow the instructions in: 33 | 34 | # Build DRE-SLAM 35 | ### 1. Clone the repository 36 | ``` 37 | cd ~/catkin_ws/src 38 | git clone https://github.com/ydsf16/dre_slam.git 39 | ``` 40 | 41 | ### 2. Build DBow2 42 | ``` 43 | cd dre_slam/third_party/DBoW2 44 | mkdir build 45 | cd build 46 | cmake .. 47 | make -j4 48 | ``` 49 | 50 | ### 3. Build Sophus 51 | ``` 52 | cd ../../Sophus 53 | mkdir build 54 | cd build 55 | cmake .. 56 | make -j4 57 | ``` 58 | 59 | ### 4. Build object detector 60 | ``` 61 | cd ../../../object_detector 62 | mkdir build 63 | cd build 64 | cmake .. 65 | make -j4 66 | ``` 67 | 68 | ### 5. Download the YOLOv3 model 69 | ``` 70 | cd ../../config 71 | mkdir yolov3 72 | cd yolov3 73 | wget https://pjreddie.com/media/files/yolov3.weights 74 | wget https://github.com/pjreddie/darknet/blob/master/cfg/yolov3.cfg?raw=true -O ./yolov3.cfg 75 | wget https://github.com/pjreddie/darknet/blob/master/data/coco.names?raw=true -O ./coco.names 76 | ``` 77 | 78 | ### 6. Catkin_make 79 | ``` 80 | cd ~/catkin_ws 81 | catkin_make 82 | source ~/catkin_ws/devel/setup.bash 83 | ``` 84 | 85 | # Example 86 | 87 | ## Dataset 88 | We collected several data sequences in our lab using our Redbot robot. The dataset is available at [Pan.Baidu](https://pan.baidu.com/s/1freJVLeIE525xHUZY01HmQ) or [Dropbox](https://www.dropbox.com/sh/f7fsx8s9k9oya3r/AACBoOUlPNo7inOVceHD5gy_a?dl=0). 89 | 90 | ## Run 91 | ### 1. Open a terminal and launch dre_slam 92 | ``` 93 | roslaunch dre_slam comparative_test.launch 94 | ``` 95 | 96 | ### 2. Open a terminal and play one rosbag 97 | 98 | ``` 99 | rosbag play .bag 100 | ``` 101 | 102 | # Run on your own robot 103 | **You need to do three things:** 104 | 105 | 1. Calibrate the intrinsic parameter of the camera, the robot odometry parameter, and the rigid transformation from the camera to the robot. 106 | 107 | 2. Prepare a parameter configuration file, refer to the ***config*** folder. 108 | 109 | 3. Prepare a launch file, refer to the ***launch*** folder. 110 | 111 | # Contact us 112 | For any issues, please feel free to contact **[Dongsheng Yang](https://github.com/ydsf16)**: 113 | -------------------------------------------------------------------------------- /config/comparative_test.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.2 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera params. Pinhole 5 | #-------------------------------------------------------------------------------------------- 6 | cam_rgb_topic_: /kinect2/qhd/image_color 7 | cam_depth_topic_: /kinect2/qhd/image_depth_rect 8 | 9 | cam_fx_: 525.2866213437447 10 | cam_fy_: 525.2178123117577 11 | cam_cx_: 472.85738972861157 12 | cam_cy_: 264.77181506420266 13 | cam_k1_: 0.04160142651680036 14 | cam_k2_: -0.04771035303381654 15 | cam_p1_: -0.0032638387781624705 16 | cam_p2_: -0.003985120051161831 17 | cam_k3_: 0.01110263483766991 18 | 19 | cam_height_: 540 20 | cam_width_: 960 21 | 22 | cam_depth_factor_: 0.001 # Depth scale factor. 23 | 24 | cam_dmax_: 8.0 # Max depth value to be used. (m) 25 | cam_dmin_: 0.1 # Min depth value to be used. (m) 26 | 27 | cam_fps_: 20 # Camera FPs. 28 | 29 | #-------------------------------------------------------------------------------------------- 30 | # Robot intrinsic and extrinsic 31 | #-------------------------------------------------------------------------------------------- 32 | #### Intrinsic 33 | encoder_topic_: /rbot/encoder 34 | 35 | odom_kl_: 4.0652e-5 # left wheel factor 36 | odom_kr_: 4.0668e-5 # right wheel factor 37 | odom_b_: 0.3166 # wheel space 38 | odom_K_: 0.008 # Noise factor. 39 | 40 | #### Extrinsic Trc 41 | Trc_: !!opencv-matrix 42 | rows: 3 43 | cols: 4 44 | dt: d 45 | data: [ 0.0, -0.034899497, 0.999390827, 0.124, -1.0, 0.0, 0.0, -0.1, 0.0, -0.999390827, -0.034899497, 0.0 ] 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # RGB-D Encoder Tracking 49 | #-------------------------------------------------------------------------------------------- 50 | #### ORB feature 51 | ret_ft_n_features_: 1600 # Number of ORB features per frame. 52 | 53 | #### Tracking 54 | ret_tk_dist_th_: 4.0 # Local map search radius. (m) 55 | ret_tk_angle_th_: 1.0 # Local map search angle. (rad) 56 | ret_tk_db_: 0.20 # Base threshold for erroneous match discard. (m) 57 | ret_tk_kd_: 0.025 # Scale factor of the threshold for erroneous match discard. 58 | 59 | #### Keyframe decision 60 | # Condation 1. 61 | ret_kd_fps_factor_: 0.9 62 | # Condation 2: 63 | ret_kd_dist_th_: 0.3 # Max distance (m) 64 | ret_kd_angle_th_: 0.5 # Max angle (rad) 65 | 66 | #-------------------------------------------------------------------------------------------- 67 | # Dynamic Pixels Culling 68 | #-------------------------------------------------------------------------------------------- 69 | dpc_n_near_kfs_: 5 # Number of near keyframes. 70 | dpc_npts_per_cluster_: 6000 # Number of points per cluster. 71 | dpc_n_sel_pts_per_cluster_: 100 # Number of points per cluster to be selected for dynamic cluster decision. 72 | dpc_search_square_size_: 9 # 9 pixels 73 | 74 | #-------------------------------------------------------------------------------------------- 75 | # Sparse Mapping 76 | #-------------------------------------------------------------------------------------------- 77 | #### Local Mapping 78 | sm_lm_window_size_: 8 # local BA window size sp_lm_window_size_ KFs. 79 | 80 | 81 | #-------------------------------------------------------------------------------------------- 82 | # OctoMap Construction 83 | #-------------------------------------------------------------------------------------------- 84 | oc_voxel_size_: 0.1 # Voxel size of the OctoMap (m). 85 | oc_submap_size_: 5 # Sub-OctoMap size (KFs) 86 | 87 | 88 | -------------------------------------------------------------------------------- /config/extention_test.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.2 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera params. Pinhole 5 | #-------------------------------------------------------------------------------------------- 6 | cam_rgb_topic_: /kinect2/qhd/image_color 7 | cam_depth_topic_: /kinect2/qhd/image_depth_rect 8 | 9 | cam_fx_: 525.2866213437447 10 | cam_fy_: 525.2178123117577 11 | cam_cx_: 472.85738972861157 12 | cam_cy_: 264.77181506420266 13 | cam_k1_: 0.04160142651680036 14 | cam_k2_: -0.04771035303381654 15 | cam_p1_: -0.0032638387781624705 16 | cam_p2_: -0.003985120051161831 17 | cam_k3_: 0.01110263483766991 18 | 19 | cam_height_: 540 20 | cam_width_: 960 21 | 22 | cam_depth_factor_: 0.001 # Depth scale factor. 23 | 24 | cam_dmax_: 8.0 # Max depth value to be used. (m) 25 | cam_dmin_: 0.1 # Min depth value to be used. (m) 26 | 27 | cam_fps_: 10 # Camera FPs. 28 | 29 | #-------------------------------------------------------------------------------------------- 30 | # Robot intrinsic and extrinsic 31 | #-------------------------------------------------------------------------------------------- 32 | #### Intrinsic 33 | encoder_topic_: /rbot/encoder 34 | 35 | odom_kl_: 4.0652e-5 # left wheel factor 36 | odom_kr_: 4.0668e-5 # right wheel factor 37 | odom_b_: 0.3166 # wheel space 38 | odom_K_: 0.008 # Noise factor. 39 | 40 | #### Extrinsic Trc 41 | Trc_: !!opencv-matrix 42 | rows: 3 43 | cols: 4 44 | dt: d 45 | data: [ 0.0, 0.275637, 0.9612617, 0.124, -1.0, 0.0, 0.0, -0.1, 0.0, -0.9612617, 0.275637, 0.0 ] 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # RGB-D Encoder Tracking 49 | #-------------------------------------------------------------------------------------------- 50 | #### ORB feature 51 | ret_ft_n_features_: 1600 # Number of ORB features per frame. 52 | 53 | #### Tracking 54 | ret_tk_dist_th_: 4.0 # Local map search radius. (m) 55 | ret_tk_angle_th_: 1.0 # Local map search angle. (rad) 56 | ret_tk_db_: 0.20 # Base threshold for erroneous match discard. (m) 57 | ret_tk_kd_: 0.025 # Scale factor of the threshold for erroneous match discard. 58 | 59 | #### Keyframe decision 60 | # Condation 1. 61 | ret_kd_fps_factor_: 0.9 62 | # Condation 2: 63 | ret_kd_dist_th_: 0.3 # Max distance (m) 64 | ret_kd_angle_th_: 0.5 # Max angle (rad) 65 | 66 | #-------------------------------------------------------------------------------------------- 67 | # Dynamic Pixels Culling 68 | #-------------------------------------------------------------------------------------------- 69 | dpc_n_near_kfs_: 5 # Number of near keyframes. 70 | dpc_npts_per_cluster_: 6000 # Number of points per cluster. 71 | dpc_n_sel_pts_per_cluster_: 100 # Number of points per cluster to be selected for dynamic cluster decision. 72 | dpc_search_square_size_: 9 # 9 pixels 73 | 74 | #-------------------------------------------------------------------------------------------- 75 | # Sparse Mapping 76 | #-------------------------------------------------------------------------------------------- 77 | #### Local Mapping 78 | sm_lm_window_size_: 8 # local BA window size sp_lm_window_size_ KFs. 79 | 80 | 81 | #-------------------------------------------------------------------------------------------- 82 | # OctoMap Construction 83 | #-------------------------------------------------------------------------------------------- 84 | oc_voxel_size_: 0.1 # Voxel size of the OctoMap (m). 85 | oc_submap_size_: 5 # Sub-OctoMap size (KFs) 86 | -------------------------------------------------------------------------------- /config/orbvoc/ORBvoc.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ydsf16/dre_slam/346e740578d2e58e1b2daa4fd60c811af7134375/config/orbvoc/ORBvoc.bin -------------------------------------------------------------------------------- /core/include/dre_slam/ORBextractor.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | /** 18 | * This file is part of ORB-SLAM2. 19 | * 20 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 21 | * For more information see 22 | * 23 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 24 | * it under the terms of the GNU General Public License as published by 25 | * the Free Software Foundation, either version 3 of the License, or 26 | * (at your option) any later version. 27 | * 28 | * ORB-SLAM2 is distributed in the hope that it will be useful, 29 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 30 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | * GNU General Public License for more details. 32 | * 33 | * You should have received a copy of the GNU General Public License 34 | * along with ORB-SLAM2. If not, see . 35 | */ 36 | 37 | #ifndef ORBEXTRACTOR_H 38 | #define ORBEXTRACTOR_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | 45 | namespace dre_slam 46 | { 47 | 48 | class ExtractorNode 49 | { 50 | public: 51 | ExtractorNode():bNoMore(false){} 52 | 53 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 54 | 55 | std::vector vKeys; 56 | cv::Point2i UL, UR, BL, BR; 57 | std::list::iterator lit; 58 | bool bNoMore; 59 | }; 60 | 61 | class ORBextractor 62 | { 63 | public: 64 | 65 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 66 | 67 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 68 | int iniThFAST, int minThFAST); 69 | 70 | ~ORBextractor(){} 71 | 72 | // Compute the ORB features and descriptors on an image. 73 | // ORB are dispersed on the image using an octree. 74 | // Mask is ignored in the current implementation. 75 | void operator()( cv::InputArray image, cv::InputArray mask, 76 | std::vector& keypoints, 77 | cv::OutputArray descriptors); 78 | 79 | int inline GetLevels(){ 80 | return nlevels;} 81 | 82 | float inline GetScaleFactor(){ 83 | return scaleFactor;} 84 | 85 | std::vector inline GetScaleFactors(){ 86 | return mvScaleFactor; 87 | } 88 | 89 | std::vector inline GetInverseScaleFactors(){ 90 | return mvInvScaleFactor; 91 | } 92 | 93 | std::vector inline GetScaleSigmaSquares(){ 94 | return mvLevelSigma2; 95 | } 96 | 97 | std::vector inline GetInverseScaleSigmaSquares(){ 98 | return mvInvLevelSigma2; 99 | } 100 | 101 | std::vector mvImagePyramid; 102 | 103 | protected: 104 | 105 | void ComputePyramid(cv::Mat image); 106 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 107 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 108 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 109 | 110 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 111 | std::vector pattern; 112 | 113 | int nfeatures; 114 | double scaleFactor; 115 | int nlevels; 116 | int iniThFAST; 117 | int minThFAST; 118 | 119 | std::vector mnFeaturesPerLevel; 120 | 121 | std::vector umax; 122 | 123 | std::vector mvScaleFactor; 124 | std::vector mvInvScaleFactor; 125 | std::vector mvLevelSigma2; 126 | std::vector mvInvLevelSigma2; 127 | }; 128 | 129 | } //namespace dre_slam 130 | 131 | #endif 132 | 133 | -------------------------------------------------------------------------------- /core/include/dre_slam/camera.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef CAMERA_H 18 | #define CAMERA_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace dre_slam 26 | { 27 | 28 | class Camera 29 | { 30 | public: 31 | Camera ( int width, int height, 32 | double fx, double fy, double cx, double cy, 33 | double k1, double k2 , double p1 , double p2, double k3); 34 | 35 | 36 | Eigen::Vector3d img2Cam ( const Eigen::Vector2d& px, const double& depth ); 37 | Eigen::Vector2d cam2Img ( const Eigen::Vector3d& ptc ); 38 | void undistortKeyPoints ( const std::vector< cv::KeyPoint >& dist_kps, std::vector< cv::KeyPoint >& undist_kps ); 39 | bool projectWithCovariance(const Sophus::SE3& Trc, const Sophus::SE3& T_w_rr, const Sophus::SE2& T_rr_rc, const Eigen::Matrix3d& cov_o, const Eigen::Vector3d& pw, const double& simga_p, Eigen::Vector2d& u, Eigen::Matrix2d& cov_u); 40 | bool projectWorldPoint2Img(const Sophus::SE3& Trc, const Sophus::SE3& Twr, const Eigen::Vector3d& pw, Eigen::Vector2d& u); 41 | 42 | inline bool isInFrame ( const Eigen::Vector2d & obs, int boundary=0 ) const { 43 | if ( obs[0]>=boundary && obs[0]=boundary && obs[1] 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef COMMON_H 18 | #define COMMON_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace dre_slam{ 27 | 28 | double normAngle ( double angle ); 29 | int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 30 | std::vector CvMat2DescriptorVector(const cv::Mat &Descriptors); 31 | Eigen::Matrix4d AngleAxisTrans2EigenT(cv::Vec3d rvec, cv::Vec3d tvec); 32 | Sophus::SE2 EigenT2Pose2d(Eigen::Matrix4d& T); 33 | 34 | } ; // namespace dre_slam 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /core/include/dre_slam/config.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef CONFIG_H 18 | #define CONFIG_H 19 | 20 | #include 21 | #include 22 | 23 | namespace dre_slam{ 24 | 25 | class Config{ 26 | public: 27 | Config(const std::string& cfg_dir); 28 | 29 | public: 30 | /**** Camera ****/ 31 | std::string cam_rgb_topic_; 32 | std::string cam_depth_topic_; 33 | 34 | double cam_fx_; 35 | double cam_fy_; 36 | double cam_cx_; 37 | double cam_cy_; 38 | double cam_k1_; 39 | double cam_k2_; 40 | double cam_p1_; 41 | double cam_p2_; 42 | double cam_k3_; 43 | int cam_height_; 44 | int cam_width_; 45 | double cam_depth_factor_; // Depth scale factor. 46 | double cam_dmax_; // Max depth value to be used. 47 | double cam_dmin_; // Min depth value to be used. 48 | double cam_fps_; // Camera FPs. 49 | 50 | /**** Robot intrinsic and extrinsic ****/ 51 | std::string encoder_topic_; 52 | 53 | double odom_kl_; // left wheel factor 54 | double odom_kr_; // right wheel factor 55 | double odom_b_; // wheel space 56 | double odom_K_; // Noise factor. 57 | Sophus::SE3 Trc_; // Extrinsic parameter. Translation from the camera to the robot. 58 | 59 | 60 | /**** RGB-D Encoder Tracking ****/ 61 | /** ORB feature **/ 62 | int ret_ft_n_features_; // Number of ORB features per frame. 63 | float ret_ft_scale_factor_ = 1.2; 64 | int ret_ft_n_levels_ = 8; 65 | int ret_ft_init_th_ = 20; 66 | int ret_ft_min_th_ = 7; 67 | 68 | /** Tracking **/ 69 | double ret_tk_dist_th_; // Local map search radius. 70 | double ret_tk_angle_th_; // Local map search angle. 71 | double ret_tk_max_local_mpts_ = 4000; // Max mappoints in the local map. 72 | double ret_tk_sigma_p_ = 0.03; // (m) 73 | double ret_tk_db_; // The base threshold for erroneous match discard. 74 | double ret_tk_kd_; // The scale factor of the threshold for erroneous match discard. 75 | 76 | /** Keyframe decision **/ 77 | // Condation 1. 78 | double ret_kd_fps_factor_; 79 | // Condation 2: 80 | double ret_kd_dist_th_; // Max distance threshold. 81 | double ret_kd_angle_th_; // Max angle threshold. 82 | 83 | 84 | 85 | /**** Dynamic Pixels Culling ****/ 86 | double dpc_ob_scale_up_factor_ = 1.2; // Scale-up factor of the bounding boxes 87 | std::vector dpc_predef_dyn_obj_names_ = {"person", "bicycle", "car", "motorbike", "bird", "cat", "dog", "horse", "cow"}; // Names of the predefined dynamic objects. 88 | int dpc_n_near_kfs_; // Only using nearest dpc_max_nkf_passed_ KFs that are in the local map for dynamic cluster decision. 89 | int dpc_npts_per_cluster_; // Number of points per cluster. 90 | int dpc_n_sel_pts_per_cluster_; // Number of points per cluster to be selected for dynamic cluster decision. 91 | int dpc_search_square_size_; // Size of the square search area. 92 | 93 | 94 | /**** Sparse Mapping ****/ 95 | /** Local Mapping **/ 96 | int sm_lm_cobv_th_ = 20; // sp_lm_cobv_th_ a visual edge. 97 | int sm_lm_lba_niter = 50; // sp_lm_lba_niter iter. 98 | int sm_lm_window_size_; // local BA window size: sp_lm_window_size_ KFs. 99 | 100 | /** Loop Closure **/ 101 | double sm_lc_search_dist_ = 8.0; 102 | double sm_lc_search_angle_ = 2.0; 103 | double sm_lc_loop_edge_weight_ = 90; 104 | double sm_lc_cov_edge_weight_ = 30; 105 | double sm_lc_encoder_edge_weight_ = 60; 106 | int sm_lc_pgop_niter_ = 120; // sp_lc_pgop_niter_ iter. 107 | 108 | 109 | /**** OctoMap Construction ****/ 110 | double oc_voxel_size_; // Voxel size of the OctoMap (m). 111 | double oc_submap_size_; // Sub-OctoMap size (KFs) 112 | 113 | /** OctoMap parameters **/ 114 | double oc_occ_th_ = 0.61; 115 | double oc_prob_hit_= 0.6; 116 | double oc_prob_miss_ = 0.45; 117 | 118 | }; // class Config 119 | 120 | } // namespace dre_slam 121 | 122 | 123 | #endif // CONFIG_H -------------------------------------------------------------------------------- /core/include/dre_slam/dre_slam.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef DRE_SLAM_H 18 | #define DRE_SLAM_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace dre_slam{ 32 | 33 | class DRE_SLAM{ 34 | public: 35 | DRE_SLAM( ros::NodeHandle& nh, Config* cfg, const std::string& orbvoc_dir, const std::string& yolov3_classes_dir, const std::string& yolov3_model_dir, const std::string& yolov3_weights_dir ); 36 | 37 | void addRGBDImage(const cv::Mat& rgb, const cv::Mat& depth, const double& timestamp); 38 | void addEncoder(const double& enc_l, const double& enc_r, const double& timestamp); 39 | 40 | // Save Results 41 | void saveMapPoints(const std::string& dir); 42 | void saveKeyFrames(const std::string& dir); 43 | void saveFrames(const std::string& dir); 44 | void saveOctoMap(const std::string& dir); 45 | 46 | private: 47 | Config* cfg_; 48 | Camera* cam_; 49 | 50 | Map* map_; 51 | Tracking* tracking_; 52 | DynamicPixelCulling* dynamic_pixel_culling_; 53 | LocalMapping* local_mapping_; 54 | LoopClosing* loop_closing_; 55 | SubOctoMapConstruction* sub_octomap_construction_; 56 | OctoMapFusion* octomap_fusion_; 57 | Optimizer* optimizer_; 58 | RosPuber* ros_puber_; 59 | 60 | Vocabulary* orb_voc_; 61 | ObjectDetector* objector_detector_; 62 | 63 | }; // class DRE_SLAM 64 | 65 | 66 | } // namespace dre_slam 67 | 68 | 69 | #endif // DRE_SLAM_H -------------------------------------------------------------------------------- /core/include/dre_slam/dynamic_pixel_culling.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef DYNAMIC_PIXEL_CULLING_H 18 | #define DYNAMIC_PIXEL_CULLING_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace dre_slam { 25 | 26 | class DynamicPixelCulling{ 27 | 28 | public: 29 | DynamicPixelCulling( LocalMapping* local_mapping, ObjectDetector* object_detector, RosPuber* ros_puber, Map* map, Camera* cam, Config* cfg ); 30 | void processing(); 31 | 32 | void insertKeyFrame(KeyFrame* kf); 33 | bool checkNewFrame(); 34 | KeyFrame* getNewKeyFrame(); 35 | 36 | private: 37 | LocalMapping* local_mapping_; 38 | RosPuber* ros_puber_; 39 | Map* map_; 40 | Camera* cam_; 41 | Config* cfg_; 42 | 43 | // thread. 44 | std::mutex mutex_kfs_queue_; 45 | std::queue kfs_queue_; 46 | std::thread* th_dynamic_pixel_culling_; 47 | 48 | DynamicPixelDetector* dynamic_pixel_detector_; 49 | 50 | // Passed KFs 51 | std::queue passed_kfs_queue_; 52 | 53 | }; // class DynamicPixelCulling 54 | 55 | } //namespace dre_slam 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /core/include/dre_slam/dynamic_pixel_detector.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef DYNAMIC_PIXEL_DETECTOR_H 18 | #define DYNAMIC_PIXEL_DETECTOR_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace dre_slam { 26 | 27 | enum PointType{ 28 | UNKNOWN, 29 | STATIC, 30 | DYNAMIC 31 | }; 32 | 33 | class DynamicPixelDetector{ 34 | public: 35 | DynamicPixelDetector( ObjectDetector* object_detector, Map* map, Camera* cam, Config* cfg ); 36 | 37 | void removeDynamicPixels( KeyFrame* ckf ); 38 | 39 | private: 40 | // detect Objects by YOLOv3 41 | void detectObjects( const cv::Mat& rgb, std::vector& object ); 42 | void detectDynamicObjects(const cv::Mat& rgb, const std::vector& dynamic_object_names, const float expand_ratio, std::vector& dynamic_objects); 43 | bool isObjectDynamic(const std::string& object_name, const std::vector& dynamic_object_names); 44 | void expandBoundingBox(Object& object, const float expand_ratio); 45 | void drawObjectsOnRgbImage(const std::vector& objects, cv::Mat& rgb); 46 | void dynamicDepthImgCullingByDynamicObjects(cv::Mat& depth, const std::vector& dynamic_objects); 47 | 48 | // Kmeans 49 | void computePointCloud( const cv::Mat& depth, std::vector& pt2d, std::vector& pt3d ); 50 | void segmentPointCloudByKmeans( const vector< Eigen::Vector2d >& pts2d, const vector< Eigen::Vector3d >& pts3d, const int n_clusters, vector< vector< Eigen::Vector2d > >& clusters_2d, vector< vector< Eigen::Vector3d > >& clusters_3d ); 51 | void drawClustersOnImage(cv::Mat& io_img, const int width, const int height, const vector< vector< Eigen::Vector2d > >& clusters_2d, const std::vector& colors); 52 | 53 | // Detect dynamic clusters. 54 | void detectDynamicClusters( Map* map, KeyFrame* ckf, const vector< vector< Eigen::Vector2d > >& clusters_2d, const vector< vector< Eigen::Vector3d > >& clusters_3d, std::vector& dynamic_flags ); 55 | bool isDynamicCluster( const vector< Eigen::Vector2d >& cluster_2d, const vector< Eigen::Vector3d >& cluster_3d, KeyFrame* ckf, std::vector< KeyFrame* > near_kfs ); 56 | void dynamicDepthImgCullingByMultiViewConstraint( cv::Mat& depth, const std::vector>& clusters_2d, const std::vector& dynamic_flags ); 57 | void drawDynamicClusters(cv::Mat& io_img, const std::vector>& clusters_2d, const std::vector& dynamic_flags); 58 | 59 | // Convert depth to mask. 60 | void convertDepth2Mask(const cv::Mat& depth, cv::Mat& mask); 61 | 62 | // Paper writing 63 | void drawResult( const cv::Mat& depth, const std::vector& dynamic_objects, const std::vector>& clusters_2d, const std::vector& dynamic_flags, cv::Mat& color_mask); 64 | 65 | private: 66 | Map* map_; 67 | Camera* cam_; 68 | Config* cfg_; 69 | ObjectDetector* object_detector_; 70 | int width_; 71 | int height_; 72 | std::vector objects_; 73 | cv::Mat img_objects_; 74 | std::vector colors_ = {213,0,0,197,17,98,170,0,255,98,0,234,48,79,254,41,98,255,0,145,234,0,184,212,0,191,165,0,200,83,100,221,23,174,234,0,255,214,0,255,171,0,255,109,0,221,44,0,62,39,35,33,33,33,38,50,56,144,164,174,224,224,224,161,136,127,255,112,67,255,152,0,255,193,7,255,235,59,192,202,51,139,195,74,67,160,71,0,150,136,0,172,193,3,169,244,100,181,246,63,81,181,103,58,183,171,71,188,236,64,122,239,83,80, 213,0,0,197,17,98,170,0,255,98,0,234,48,79,254,41,98,255,0,145,234,0,184,212,0,191,165,0,200,83,100,221,23,174,234,0,255,214,0,255,171,0,255,109,0,221,44,0,62,39,35,33,33,33,38,50,56,144,164,174,224,224,224,161,136,127,255,112,67,255,152,0,255,193,7,255,235,59,192,202,51,139,195,74,67,160,71,0,150,136,0,172,193,3,169,244,100,181,246,63,81,181,103,58,183,171,71,188,236,64,122,239,83,80}; 75 | 76 | }; // class DynamicPixelDetector 77 | 78 | } // namespace dre_slam 79 | 80 | 81 | #endif -------------------------------------------------------------------------------- /core/include/dre_slam/encoder.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef ENCODER_H 18 | #define ENCODER_H 19 | 20 | namespace dre_slam 21 | { 22 | 23 | class Encoder 24 | { 25 | public: 26 | Encoder(): enl_(0.0), enr_(0.0), timestamp_(0.0){} 27 | Encoder(const double& enl, const double& enr, const double& timestamp): 28 | enl_(enl), enr_(enr), timestamp_(timestamp) {} 29 | 30 | double enl_, enr_, timestamp_; 31 | }; // class Encoder 32 | 33 | } // namespace dre_slam 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /core/include/dre_slam/encoder_integration.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef ENCODER_INTEGRATION_H 18 | #define ENCODER_INTEGRATION_H 19 | 20 | #include 21 | #include 22 | 23 | namespace dre_slam 24 | { 25 | class EncoderIntegration 26 | { 27 | public: 28 | EncoderIntegration(){} 29 | EncoderIntegration(const double& kl, const double& kr, const double& b, const double& k); 30 | Sophus::SE2 addEncoder(const Encoder& enc); // 31 | Sophus::SE2 addEncoders(const std::vector< Encoder >& encs); 32 | 33 | Sophus::SE2 getTrr(); 34 | Eigen::Matrix3d getCov(); 35 | 36 | private: 37 | double kl_, kr_, b_, k_; 38 | 39 | Sophus::SE2 Trr_; 40 | Eigen::Matrix3d cov_; 41 | 42 | bool is_init_; 43 | Encoder last_enc_; 44 | double x_, y_, th_; 45 | 46 | }; // class EncoderIntegratation 47 | 48 | } // namespace dre_slam 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /core/include/dre_slam/feature_detector.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef FEATURE_DETECTOR_H 18 | #define FEATURE_DETECTOR_H 19 | 20 | #include 21 | 22 | namespace dre_slam 23 | { 24 | 25 | class FeatureDetector 26 | { 27 | public: 28 | FeatureDetector(int n_features, float scale_factor, int n_levels, int ini_th, int min_th); 29 | void detect(const cv::Mat& img, std::vector& keypoints, cv::Mat& descriptors); 30 | 31 | ~FeatureDetector(){ 32 | delete orb_extractor_; 33 | } 34 | 35 | private: 36 | ORBextractor* orb_extractor_; 37 | }; // class FeatureDetector 38 | 39 | } // namespace dre_slam 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /core/include/dre_slam/frame.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef FRAME_H 18 | #define FRAME_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace dre_slam{ 32 | class MapPoint; 33 | class KeyFrame; 34 | 35 | 36 | class Frame{ 37 | public: 38 | Frame(const cv::Mat& rgb, const cv::Mat& depth, const double& timestamp, FeatureDetector* feature_detector, Camera* cam, Config* cfg); 39 | void extractFeatures(); 40 | void getDepths(); 41 | bool getDepth(const double& x, const double& y, double& dp); 42 | void setPose(const Sophus::SE2& Twr); 43 | Sophus::SE2 getSE2Pose(); 44 | Sophus::SE3 getSE3Pose(); 45 | Sophus::SE2 getSE2PoseRefKF(); 46 | Sophus::SE3 getSE3PoseRefKF(); 47 | void setPoseRefKF(KeyFrame* kf); 48 | void freeMemory(); 49 | 50 | void constructKDTree( const std::vector& kps, pcl::KdTreeFLANN& kd_tree); 51 | int radiusSearch( const pcl::KdTreeFLANN& kd_tree, const Eigen::Vector2d& u, const double& radius,std::vector& ids); 52 | bool radiusSearchBestMatchKps( const Eigen::Vector2d& u, const double& radius, const cv::Mat& mpt_des, cv::KeyPoint& kp, int& des_dist, int& idx ); 53 | bool radiusSearchBestMatchKpWithDistLimit( const Eigen::Vector2d& u, const double& radius, const Eigen::Vector3d& mpt_ptw, const cv::Mat& mpt_des, cv::KeyPoint& kp, int& des_dist, int& idx ); 54 | 55 | void setEncoders(const std::vector& encs); 56 | std::vector& getEncoders(); 57 | 58 | public: 59 | static long int N_FRAMES; 60 | long int id_; 61 | double timestamp_; 62 | 63 | std::vector kps_; // keypoints. 64 | std::vector static_flags_; // Static flag with each keypoint. 65 | cv::Mat des_; // Descriptor with each keypoint. 66 | std::vector dps_; // Depth value with each keypoint. 67 | std::vector mpts_; // Mappoint with each keypoint. 68 | int n_features_, n_matched_; // Number of features and the Number of matches. 69 | 70 | cv::Mat rgb_, gray_, depth_; // Raw images 71 | cv::Mat img_match_; 72 | 73 | Camera* cam_; 74 | Config* cfg_; 75 | 76 | private: 77 | std::vector encoders_; 78 | FeatureDetector* feature_detector_; 79 | 80 | // Frame pose 81 | std::mutex mutex_pose_; 82 | Sophus::SE2 T_rkf_f; 83 | KeyFrame* ref_kf_; 84 | Sophus::SE2 Twr2_; 85 | Sophus::SE3 Twr3_; 86 | 87 | pcl::KdTreeFLANN kd_tree_kps_; 88 | }; //class Frame 89 | 90 | 91 | } // namespace dre_slam 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /core/include/dre_slam/keyframe.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef KEYFRAME_H 18 | #define KEYFRAME_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace dre_slam 27 | { 28 | class MapPoint; 29 | class Frame; 30 | 31 | class KeyFrame 32 | { 33 | public: 34 | KeyFrame( Frame* frame , Vocabulary* voc); 35 | 36 | // pose 37 | void cvtEigen2Double(); 38 | void cvtDouble2Eigen(); 39 | void setPose(const Sophus::SE2& Twr); 40 | Sophus::SE2 getSE2Pose(); 41 | Sophus::SE3 getSE3Pose(); 42 | 43 | // Pose graph 44 | void addLastKeyFrameEdge(KeyFrame* kf); 45 | void addVisualEdge(KeyFrame* kf); 46 | 47 | std::set getObKFs(); 48 | void setObKFs(const std::set& ob_kfs); 49 | 50 | void addLoopEdge(KeyFrame* kf, const Sophus::SE2& T); 51 | KeyFrame* getLastKeyFrameEdge(); 52 | std::set getVisualEdge(); 53 | int getLoopEdge(std::vector& loop_kfs, std::vector& Ts); 54 | 55 | // Optimization 56 | void calculateRelativePosition(); 57 | void reCalculateMpts(); 58 | void reCalculateSingleMpts(); 59 | void freeMemory(); 60 | void freeDepth(); 61 | 62 | // select static features. 63 | void selectStaticFeatures(); 64 | 65 | 66 | public: 67 | // Number of all keyframes. 68 | static long int N_KFS; 69 | 70 | // Read only data. 71 | long int kfid_; 72 | long int fid_; 73 | double timestamp_; 74 | 75 | // The static KeyPoints and descriptors. 76 | std::vector kps_; 77 | std::vector static_flags_; 78 | cv::Mat des_; 79 | 80 | Vocabulary* voc_; 81 | DBoW2::BowVector bow_vec_; 82 | DBoW2::FeatureVector feat_vec_; 83 | 84 | int n_features_; 85 | std::vector dps_; 86 | std::vector mpts_; 87 | 88 | std::vector pts_r_; 89 | 90 | Camera* cam_; 91 | Config* cfg_; 92 | cv::Mat rgb_; 93 | cv::Mat depth_; 94 | 95 | // segmentation used. 96 | cv::Mat image_mask_; 97 | cv::Mat image_objects_; 98 | cv::Mat image_source_clusters_; 99 | cv::Mat image_static_clusters_; 100 | 101 | Sophus::SE2 Trr_; 102 | Eigen::Matrix3d covrr_; 103 | 104 | double Twrd_[3]; 105 | 106 | private: 107 | Sophus::SE2 Twr2_; // robot pose 108 | Sophus::SE3 Twr3_; // robot pose 109 | 110 | // Pose graph connections. 111 | KeyFrame* last_kf_; // The last frame. 112 | std::set ob_kfs_; // Cov keyframes. 113 | std::vector loop_kfs_; // Loop Frames. 114 | std::vector loop_delta_pose_; 115 | 116 | std::mutex mutex_pose_; 117 | std::mutex mutex_graph_; 118 | std::mutex mute_mpts_; 119 | 120 | }; // class KeyFrame 121 | 122 | } // namespace dre_slam 123 | 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /core/include/dre_slam/local_mapping.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef LOCAL_MAPPING_H 18 | #define LOCAL_MAPPING_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace dre_slam 33 | { 34 | class LocalMapping 35 | { 36 | public: 37 | 38 | LocalMapping( LoopClosing* loop_closing, SubOctoMapConstruction* sub_OctoMap_construction, RosPuber* ros_puber, Optimizer* optimizer, Map* map, Camera* cam, Config* cfg ); 39 | 40 | void processing(); 41 | int addNewMpts(KeyFrame* kf); 42 | void addVisualEdges(KeyFrame* kf); 43 | void insertKeyFrame(KeyFrame* kf); 44 | bool checkNewFrame(); 45 | KeyFrame* getNewKeyFrame(); 46 | 47 | 48 | private: 49 | LoopClosing* loop_closing_; 50 | SubOctoMapConstruction* sub_OctoMap_construction_; 51 | RosPuber* ros_puber_; 52 | Optimizer* optimizer_; 53 | Map* map_; 54 | Camera* cam_; 55 | Config* cfg_; 56 | 57 | std::mutex mutex_kfs_queue_; 58 | std::queue kfs_queue_; 59 | std::thread* th_local_mapping_; 60 | 61 | }; //class LocalMapping 62 | } // namespace dre_slam 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /core/include/dre_slam/loop_closing.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef LOOP_CLOSING_H 18 | #define LOOP_CLOSING_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace dre_slam 34 | { 35 | 36 | class LoopClosing 37 | { 38 | public: 39 | LoopClosing( SubOctoMapConstruction* sub_octomap_construction, OctoMapFusion* octomap_fusion, RosPuber* ros_puber, Optimizer* optimizer, Map* map, Camera* cam, Vocabulary* voc, Config* cfg); 40 | void insertKeyFrame(KeyFrame* kf); 41 | 42 | private: 43 | bool checkLoop( KeyFrame* kf, KeyFrame*& lkf ); 44 | bool computeTrans2KF(KeyFrame* rkf, KeyFrame* lkf, Sophus::SE2& delta_pose); 45 | int matchByBow(KeyFrame* pKF1, KeyFrame* pKF2, vector &vpMatches12); 46 | int matchNnearKfs(KeyFrame* kf, std::vector& lkfs, vector &vpMatches12); 47 | void processing(); 48 | bool checkNewFrame(); 49 | KeyFrame* getNewKeyFrame(); 50 | 51 | private: 52 | SubOctoMapConstruction* sub_octomap_construction_; 53 | OctoMapFusion* octomap_fusion_; 54 | RosPuber* ros_puber_; 55 | Optimizer* optimizer_; 56 | Map* map_; 57 | Camera* cam_; 58 | Vocabulary* voc_; 59 | Config* cfg_; 60 | 61 | long int last_loop_kfid_; 62 | std::thread* th_loop_; 63 | std::queue kfs_queue_; 64 | std::mutex mutex_kfs_queue_; 65 | }; //class LoopClosing 66 | 67 | }//namespace dre_slam 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /core/include/dre_slam/map.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef MAP_H 18 | #define MAP_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace dre_slam 26 | { 27 | 28 | class Map 29 | { 30 | public: 31 | Map(Config* cfg); 32 | 33 | void addMapPoint(MapPoint* mpt); 34 | void addKeyFrame(KeyFrame* kf); 35 | void updateKFKDTree(); 36 | 37 | //Get last N keyframes for dynamic pixel culling and local sliding window BA. 38 | int getLastNKeyFrames( int N, bool keep_last, std::vector& last_n_kfs); 39 | 40 | // Get Local MapPoints for RGB-D Encoder Tracking. 41 | // Keyframes are sorted by distance from near to far 42 | int getLocalKeyFrames( Frame* frame, double dist_th, double angle_th, std::vector& local_kfs ); 43 | int getLocalMappoints( Frame* frame, double dist_th, double angle_th, int max_n_mpts, std::set< MapPoint* >& local_mpts); 44 | 45 | // Get near KFs in dist and angle radius for loop closure. the near kfs must be befor N kfs. 46 | int getNearKeyFrames( KeyFrame* kf, double dist_th, double angle_th, int npassed_th, std::vector& near_kfs ); 47 | 48 | // Get n befor and n after kfs around a KF. 49 | int getNeighbourKeyFrames(KeyFrame* lkf, int n, std::vector< KeyFrame* >& kfs ); 50 | 51 | // Get map data. 52 | int getKFsSize(); 53 | void addFrame(Frame* frame); 54 | std::vector getAllFrames(); 55 | std::set getAllMapPoints(); 56 | std::map getAllKeyFrames(); 57 | 58 | // For map update. 59 | std::mutex update_mutex_; 60 | 61 | private: 62 | Config* cfg_; 63 | std::set mpts_; // all map point. 64 | std::map kfs_; // all keyframes, with their kf id. 65 | pcl::KdTreeFLANN KDTree_kfs_; // KD-Tree of the 2D positions of the KFs. 66 | std::vector frames_; // all frames. 67 | std::mutex mutex_map_; // For map change. 68 | }; //class Map 69 | 70 | } //namespace dre_slam 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /core/include/dre_slam/map_point.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef MAP_POINT_H 18 | #define MAP_POINT_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace dre_slam{ 27 | 28 | class KeyFrame; 29 | 30 | class MapPoint{ 31 | public: 32 | MapPoint(const Eigen::Vector3d& ptw, const cv::Mat& des, Config* cfg_); 33 | 34 | void setPosition( const Eigen::Vector3d& ptw); 35 | Eigen::Vector3d getPosition(); 36 | 37 | void addObservation(KeyFrame* kf, int idx); 38 | cv::Mat& getDescriptor(); 39 | 40 | // Graph 41 | std::map ob_kfs_; 42 | KeyFrame* first_ob_kf_; 43 | 44 | // Used for optimization. 45 | void cvtEigen2Double(); 46 | void cvtDouble2Eigen(); 47 | double ptwd_[3]; 48 | 49 | // used for match. 50 | bool is_selected_; 51 | int des_dist_; 52 | private: 53 | Eigen::Vector3d ptw_; 54 | cv::Mat des_; 55 | // Thread safe. 56 | std::mutex mutex_position_; 57 | std::mutex mutex_des_; 58 | 59 | }; // class MapPoint 60 | 61 | } //namespace dre_slam 62 | 63 | #endif -------------------------------------------------------------------------------- /core/include/dre_slam/octomap_fusion.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef OCTOMAP_FUSION 18 | #define OCTOMAP_FUSION 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace dre_slam { 26 | class SubOctoMapConstruction; 27 | 28 | class OctoMapFusion{ 29 | public: 30 | OctoMapFusion( RosPuber* ros_puber, Map* map, Config* cfg ); 31 | void insertSubMap( SubOctomap* submap ); 32 | void insertOneScan2FullMapAndPub( KeyFrame* kf, octomap::Pointcloud& point_cloud_c ); 33 | void fusionAndPub(); 34 | void insertSubMap2NewTree(SubOctomap* submap, octomap::OcTree* new_tree); 35 | void processing(); 36 | void setLoopFlag(); 37 | bool getLoopFlag(); 38 | void saveOctoMap( const std::string& dir ); 39 | 40 | private: 41 | void transformTree(octomap::OcTree* src_tree, Sophus::SE3& Twc, octomap::OcTree* dst_tree); 42 | 43 | RosPuber* ros_puber_; 44 | Map* map_; 45 | Config* cfg_; 46 | octomap::OcTree* full_map_; 47 | std::list submaps_; 48 | 49 | std::thread* th_octomap_fusion_; 50 | std::mutex mutex_submaps_; 51 | std::mutex mutex_full_map_; 52 | 53 | std::mutex mutex_loop_flag_; 54 | bool loop_flag_ = false; 55 | KeyFrame* ckf_; 56 | 57 | double clamping_max_; 58 | double clamping_min_; 59 | 60 | }; // class OctoMapFusion 61 | 62 | } // namespace dre_slam 63 | 64 | #endif -------------------------------------------------------------------------------- /core/include/dre_slam/optimizer.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef OPTIMIZER_H 18 | #define OPTIMIZER_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace dre_slam 30 | { 31 | 32 | class Optimizer 33 | { 34 | public: 35 | Optimizer(Map* map, Config* cfg):map_(map), cfg_(cfg){} 36 | 37 | // Tracking 38 | void motionOnlyBA( Frame* frame, KeyFrame* ref_kf, EncoderIntegration& encoder_kf2f_); 39 | 40 | // Local Mapping. Sliding window local BA. 41 | void localBA(std::vector& opt_kfs ); 42 | 43 | // Loop closure. 44 | void motionOnlyBA(std::vector& kps, std::vector& mpts, Sophus::SE2& pose); 45 | void poseGraphOptimization( KeyFrame* loop_KF ); 46 | 47 | private: 48 | Config* cfg_; 49 | Map* map_; 50 | Eigen::Matrix2d sqrtMatrix2d(const Eigen::Matrix2d& TT); 51 | 52 | template 53 | T sqrtMatrix(const T& TT); 54 | 55 | Eigen::Matrix3d sqrtMatrix3d(const Eigen::Matrix3d& TT); 56 | 57 | };//class Optimizer 58 | 59 | } //namespace dre_slam 60 | 61 | #endif 62 | 63 | -------------------------------------------------------------------------------- /core/include/dre_slam/ros_puber.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef ROS_PUBER_H 18 | #define ROS_PUBER_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace dre_slam { 28 | 29 | class RosPuber{ 30 | public: 31 | RosPuber( ros::NodeHandle& nh ); 32 | 33 | void pubCurrentFrame( Frame* frame ); 34 | void pubDynamicPixelCullingResults( KeyFrame* kf); 35 | void pubSparseMap( Map* map); 36 | void pubOctoMap( octomap::OcTree* octree ); 37 | 38 | private: 39 | ros::NodeHandle nh_; 40 | 41 | // Info of the current frame. 42 | ros::Publisher puber_robot_pose_; 43 | image_transport::Publisher puber_img_match_; 44 | 45 | // Dynamic pixel detection results. 46 | image_transport::Publisher puber_dpc_img_objects_; 47 | image_transport::Publisher puber_dpc_img_clusters_; 48 | image_transport::Publisher puber_dpc_img_mask_; 49 | 50 | 51 | // KFs and pose graph. Sparse Map 52 | ros::Publisher puber_mappoints_; 53 | ros::Publisher puber_kfs_puber_; 54 | ros::Publisher puber_encoder_graph_; 55 | ros::Publisher puber_loop_graph_; 56 | ros::Publisher puber_visual_graph_; 57 | 58 | // OctoMap. 59 | ros::Publisher puber_octomap_; 60 | }; // RosPuber 61 | 62 | } // namespace dre_slam 63 | 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /core/include/dre_slam/run_timer.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef RUN_TIMER_H 18 | #define RUN_TIMER_H 19 | 20 | #include 21 | #include 22 | 23 | namespace dre_slam 24 | { 25 | 26 | class RunTimer 27 | { 28 | public: 29 | inline void start() { 30 | t_s_ = std::chrono::steady_clock::now(); 31 | } 32 | 33 | inline void stop() { 34 | t_e_ = std::chrono::steady_clock::now(); 35 | } 36 | 37 | inline double duration() { 38 | return std::chrono::duration_cast> ( t_e_ - t_s_ ).count() * 1000.0; 39 | } 40 | 41 | private: 42 | std::chrono::steady_clock::time_point t_s_; //start time ponit 43 | std::chrono::steady_clock::time_point t_e_; //stop time point 44 | }; 45 | 46 | 47 | } //namespace dre_slam 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /core/include/dre_slam/sub_octomap.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SUB_OCTOMAP_H 18 | #define SUB_OCTOMAP_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace dre_slam{ 25 | 26 | class SubOctomap{ 27 | public: 28 | SubOctomap( Config* cfg ); 29 | void insertKeyFrame( KeyFrame* kf, octomap::Pointcloud& point_cloud_c ); 30 | bool isContainKeyframe(KeyFrame* kf); 31 | 32 | ~SubOctomap() 33 | { 34 | delete sub_octree_; 35 | } 36 | 37 | // main contant. 38 | Config* cfg_; 39 | octomap::OcTree* sub_octree_; 40 | std::set kfs_; 41 | KeyFrame* kf_base_; 42 | };// class SubOctomap 43 | 44 | } //namespace dre_slam 45 | 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /core/include/dre_slam/sub_octomap_construction.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SUB_OCTOMAPPING_H 18 | #define SUB_OCTOMAPPING_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace dre_slam{ 29 | class OctoMapFusion; 30 | 31 | class SubOctoMapConstruction{ 32 | public: 33 | SubOctoMapConstruction( OctoMapFusion* octomap_fusion, Config* cfg ); 34 | 35 | // insert new frame to 36 | void insertKeyFrame(KeyFrame* kf); 37 | void processing(); 38 | bool checkNewFrame(); 39 | KeyFrame* getNewKeyFrame(); 40 | 41 | // create points. 42 | void createPointCloud( KeyFrame* kf, octomap::Pointcloud& point_cloud ); 43 | void createCleanCloud( KeyFrame* kf, octomap::Pointcloud& point_cloud ); 44 | 45 | void insertAllData2OctoMapFusion(); 46 | private: 47 | OctoMapFusion* octomap_fusion_; 48 | Config* cfg_; 49 | 50 | // Thread. 51 | std::thread* sub_octomap_construction_thread_; 52 | std::queue kfs_queue_; 53 | std::mutex mutex_kfs_queue_; 54 | 55 | // current sub map. 56 | SubOctomap* cur_sub_map_; 57 | int nkf_passed_; 58 | 59 | // requestAllData 60 | std::mutex mutex_all_data_; 61 | 62 | }; // class SubOctoMapConstruction 63 | 64 | } // namespace dre_slam 65 | 66 | 67 | #endif -------------------------------------------------------------------------------- /core/include/dre_slam/tracking.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef TRACKING_H 18 | #define TRACKING_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace dre_slam 37 | { 38 | 39 | class Tracking 40 | { 41 | public: 42 | Tracking(DynamicPixelCulling* dynamic_pixel_culling, RosPuber* ros_puber, Optimizer* optimizer, Map* map, Camera* cam, Vocabulary* voc, Config* cfg); 43 | 44 | void addRGBD(const cv::Mat& rgb, const cv::Mat& depth, const double& timestamp); 45 | void addEncoder(const double& enc_l, const double& enc_r, const double& timestamp); 46 | 47 | private: 48 | void RGBDThread(); 49 | void RGBDProcessing(); 50 | 51 | bool initialization(); 52 | int matchByProjection(const std::set& mpts, Frame* frame); 53 | 54 | bool checkNewFrame(); 55 | Frame* getNewFrame(); 56 | 57 | bool newKeyFrame(); 58 | int discardOutliers(double th); 59 | void drawMatchedFeatures( cv::Mat& img_sum ); 60 | 61 | double getSemiMajorAxisLength( const Eigen::Matrix2d& cov ); 62 | 63 | 64 | bool inited_; 65 | 66 | DynamicPixelCulling* dynamic_pixel_culling_; 67 | RosPuber* ros_puber_; 68 | Optimizer* optimizer_; 69 | Map* map_; 70 | Camera* cam_; 71 | Vocabulary* voc_; 72 | Config* cfg_; 73 | FeatureDetector* feature_detector_; 74 | 75 | Frame* cur_frame_; // current frame 76 | 77 | std::mutex mutex_input_frames_; 78 | std::queue input_frames_; 79 | std::thread* rgbd_thread_; 80 | 81 | long int last_frame_id_; 82 | 83 | // reference keyframe 84 | KeyFrame* ref_kf_; 85 | 86 | // raw encoder data. 87 | std::vector encoders_f2f_; 88 | 89 | // integrated encoder between the reference KF and the current frame. 90 | EncoderIntegration encoder_kf2f_; 91 | 92 | }; //class Tracking 93 | 94 | } //namespace dre_slam 95 | 96 | #endif -------------------------------------------------------------------------------- /core/include/dre_slam/vocabulary.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef VOCABULARY_H 18 | #define VOCABULARY_H 19 | 20 | #include 21 | #include 22 | 23 | namespace dre_slam{ 24 | typedef DBoW2::TemplatedVocabulary Vocabulary; 25 | } // namespace dre_slam 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /core/src/common.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | namespace dre_slam 20 | { 21 | 22 | double normAngle ( double angle ) 23 | { 24 | static double Two_PI = 2.0 * M_PI; 25 | 26 | if ( angle >= M_PI ) { 27 | angle -= Two_PI; 28 | } 29 | if ( angle < -M_PI ) { 30 | angle += Two_PI; 31 | } 32 | return angle; 33 | } 34 | 35 | 36 | int DescriptorDistance ( const cv::Mat &a, const cv::Mat &b ) 37 | { 38 | const int *pa = a.ptr(); 39 | const int *pb = b.ptr(); 40 | 41 | int dist=0; 42 | 43 | for ( int i=0; i<8; i++, pa++, pb++ ) { 44 | unsigned int v = *pa ^ *pb; 45 | v = v - ( ( v >> 1 ) & 0x55555555 ); 46 | v = ( v & 0x33333333 ) + ( ( v >> 2 ) & 0x33333333 ); 47 | dist += ( ( ( v + ( v >> 4 ) ) & 0xF0F0F0F ) * 0x1010101 ) >> 24; 48 | } 49 | return dist; 50 | } 51 | 52 | std::vector CvMat2DescriptorVector ( const cv::Mat &Descriptors ) 53 | { 54 | std::vector vDesc; 55 | vDesc.reserve ( Descriptors.rows ); 56 | for ( int j=0; j ( 0, 0 ), R.at ( 0, 1 ), R.at ( 0, 2 ), tvec[0], 73 | R.at ( 1, 0 ), R.at ( 1, 1 ), R.at ( 1, 2 ), tvec[1], 74 | R.at ( 2, 0 ), R.at ( 2, 1 ), R.at ( 2, 2 ), tvec[2], 75 | 0.,0.,0.,1.; 76 | return T; 77 | } 78 | 79 | Sophus::SE2 EigenT2Pose2d ( Eigen::Matrix4d& T ) 80 | { 81 | double theta = atan2 ( T ( 1,0 ), T ( 0,0 ) ); 82 | double x = T ( 0, 3 ); 83 | double y = T ( 1, 3 ); 84 | return Sophus::SE2 ( theta, Eigen::Vector2d ( x, y ) ); 85 | } 86 | 87 | } // namespace dre_slam 88 | -------------------------------------------------------------------------------- /core/src/config.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | namespace dre_slam{ 20 | 21 | Config::Config ( const std::string& cfg_dir) 22 | { 23 | 24 | cv::FileStorage fs ( cfg_dir, cv::FileStorage::READ ); 25 | 26 | /**** Camera ****/ 27 | fs["cam_rgb_topic_"] >> cam_rgb_topic_; 28 | fs["cam_depth_topic_"] >> cam_depth_topic_; 29 | 30 | fs["cam_fx_"] >> cam_fx_; 31 | fs["cam_fy_"] >> cam_fy_; 32 | fs["cam_cx_"] >> cam_cx_; 33 | fs["cam_cy_"] >> cam_cy_; 34 | fs["cam_k1_"] >> cam_k1_; 35 | fs["cam_k2_"] >> cam_k2_; 36 | fs["cam_p1_"] >> cam_p1_; 37 | fs["cam_p2_"] >> cam_p2_; 38 | fs["cam_k3_"] >> cam_k3_; 39 | fs["cam_height_"] >> cam_height_; 40 | fs["cam_width_"] >> cam_width_; 41 | fs["cam_depth_factor_"] >> cam_depth_factor_; // Depth scale factor. 42 | fs["cam_dmax_"] >> cam_dmax_; // Max depth value to be used. 43 | fs["cam_dmin_"] >> cam_dmin_; // Min depth value to be used. 44 | fs["cam_fps_"] >> cam_fps_; // Camera FPs. 45 | 46 | /**** Robot intrinsic and extrinsic ****/ 47 | fs["encoder_topic_"] >> encoder_topic_; 48 | 49 | fs["odom_kl_"] >> odom_kl_; // left wheel factor 50 | fs["odom_kr_"] >> odom_kr_; // right wheel factor 51 | fs["odom_b_"] >> odom_b_; // wheel space 52 | fs["odom_K_"] >> odom_K_; // Noise factor. 53 | 54 | /** robot extrinsic **/ 55 | cv::Mat cvTrc; 56 | fs["Trc_"] >> cvTrc; 57 | Eigen::Matrix3d R; 58 | R << cvTrc.at ( 0, 0 ), cvTrc.at ( 0, 1 ), cvTrc.at ( 0, 2 ), 59 | cvTrc.at ( 1, 0 ), cvTrc.at ( 1, 1 ), cvTrc.at ( 1, 2 ), 60 | cvTrc.at ( 2, 0 ), cvTrc.at ( 2, 1 ), cvTrc.at ( 2, 2 ); 61 | Eigen::Vector3d t; 62 | t << cvTrc.at ( 0, 3 ), cvTrc.at ( 1, 3 ), cvTrc.at ( 2, 3 ); 63 | Trc_ = Sophus::SE3 ( R, t ); 64 | 65 | 66 | /**** RGB-D Encoder Tracking ****/ 67 | /** ORB feature **/ 68 | fs["ret_ft_n_features_"] >> ret_ft_n_features_; // Number of ORB features per frame. 69 | 70 | /** Tracking **/ 71 | fs["ret_tk_dist_th_"] >> ret_tk_dist_th_; // Local map search radius. 72 | fs["ret_tk_angle_th_"] >> ret_tk_angle_th_; // Local map search angle. 73 | fs["ret_tk_db_"] >> ret_tk_db_; // The base threshold for erroneous match discard. 74 | fs["ret_tk_kd_"] >> ret_tk_kd_; // The scale factor of the threshold for erroneous match discard. 75 | 76 | /** Keyframe decision **/ 77 | // Condation 1. 78 | fs["ret_kd_fps_factor_"] >> ret_kd_fps_factor_; 79 | // Condation 2: 80 | fs["ret_kd_dist_th_"] >> ret_kd_dist_th_; // Max distance 81 | fs["ret_kd_angle_th_"] >> ret_kd_angle_th_; // Max angle 82 | 83 | 84 | 85 | /**** Dynamic Pixels Culling ****/ 86 | fs["dpc_n_near_kfs_"] >> dpc_n_near_kfs_; 87 | fs["dpc_npts_per_cluster_"] >> dpc_npts_per_cluster_; // Number of points per cluster. 88 | fs["dpc_n_sel_pts_per_cluster_"] >> dpc_n_sel_pts_per_cluster_; // Number of points per cluster to be selected for dynamic cluster decision. 89 | fs["dpc_search_square_size_"] >> dpc_search_square_size_; 90 | 91 | 92 | /**** Sparse Mapping ****/ 93 | /** Local Mapping **/ 94 | fs["sm_lm_window_size_"] >> sm_lm_window_size_; // local BA window size: sp_lm_window_size_ KFs. 95 | 96 | /**** OctoMap Construction ****/ 97 | fs["oc_voxel_size_"] >> oc_voxel_size_; // Voxel size of the OctoMap (m). 98 | fs["oc_submap_size_"] >> oc_submap_size_; // Sub-OctoMap size (KFs) 99 | 100 | fs.release(); 101 | } // Config 102 | 103 | } // namespace dre_slam 104 | -------------------------------------------------------------------------------- /core/src/dynamic_pixel_culling.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | namespace dre_slam 21 | { 22 | DynamicPixelCulling::DynamicPixelCulling ( LocalMapping* local_mapping,ObjectDetector* object_detector,RosPuber* ros_puber, Map* map, Camera* cam, Config* cfg ) : local_mapping_ ( local_mapping ), ros_puber_ ( ros_puber ), map_ ( map ), cam_ ( cam ), cfg_ ( cfg ) 23 | { 24 | dynamic_pixel_detector_ = new DynamicPixelDetector ( object_detector, map, cam, cfg ); 25 | th_dynamic_pixel_culling_ = new std::thread ( &DynamicPixelCulling::processing, this ); 26 | } // DynamicPixelCulling 27 | 28 | void DynamicPixelCulling::processing() 29 | { 30 | while ( true ) { 31 | if ( checkNewFrame() == true ) { 32 | KeyFrame* kf = getNewKeyFrame(); 33 | 34 | // remove dynamic pixel. 35 | RunTimer t; 36 | t.start(); 37 | dynamic_pixel_detector_->removeDynamicPixels ( kf ); 38 | t.stop(); 39 | std::cout << "\nKeyFrame " << kf->kfid_ << " Dynamic Pixel Culling time: " << t.duration() << "\n\n"; 40 | 41 | 42 | // Send to local mapping 43 | local_mapping_->insertKeyFrame ( kf ); 44 | 45 | // publish results of the dynamic pixel culling. 46 | ros_puber_->pubDynamicPixelCullingResults ( kf ); 47 | 48 | // free the depth image of the last Nth kf. 49 | passed_kfs_queue_.push ( kf ); 50 | 51 | if ( passed_kfs_queue_.size() > ( cfg_->dpc_n_near_kfs_+1 ) ) { 52 | KeyFrame* pass_kf = passed_kfs_queue_.front(); 53 | passed_kfs_queue_.pop(); 54 | pass_kf->freeDepth(); 55 | } 56 | } // if new KF come in. 57 | 58 | usleep ( 5000 ); // sleep 5 ms. 59 | } 60 | } // processing 61 | 62 | bool DynamicPixelCulling::checkNewFrame() 63 | { 64 | std::unique_lock lock ( mutex_kfs_queue_ ); 65 | return ( !kfs_queue_.empty() ); 66 | } // checkNewFrame 67 | 68 | KeyFrame* DynamicPixelCulling::getNewKeyFrame() 69 | { 70 | std::unique_lock lock ( mutex_kfs_queue_ ); 71 | KeyFrame* kf = kfs_queue_.front(); 72 | kfs_queue_.pop(); 73 | return kf; 74 | } // getNewKeyFrame 75 | 76 | void DynamicPixelCulling::insertKeyFrame ( KeyFrame* kf ) 77 | { 78 | std::unique_lock lock ( mutex_kfs_queue_ ); 79 | kfs_queue_.push ( kf ); 80 | } // insertKeyFrame 81 | 82 | } // namespace dre_slam 83 | -------------------------------------------------------------------------------- /core/src/encoder_integration.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | namespace dre_slam 21 | { 22 | 23 | EncoderIntegration::EncoderIntegration ( const double& kl, const double& kr, const double& b, const double& k ) : 24 | kl_ ( kl ), kr_ ( kr ), b_ ( b ), k_ ( k ), is_init_ ( false ), x_ ( 0.0 ), y_ ( 0.0 ), th_ ( 0.0 ) 25 | { 26 | } 27 | 28 | Sophus::SE2 EncoderIntegration::addEncoder ( const Encoder& enc ) 29 | { 30 | 31 | if ( is_init_ == false ) { 32 | last_enc_ = enc; 33 | Trr_ = Sophus::SE2(); 34 | cov_.setZero(); 35 | is_init_ = true; 36 | } else { 37 | double delta_enl = enc.enl_ - last_enc_.enl_; 38 | double delta_enr = enc.enr_ - last_enc_.enr_; 39 | last_enc_ = enc; 40 | 41 | double delta_sl = kl_ * delta_enl; 42 | double delta_sr = kr_ * delta_enr; 43 | 44 | double delta_theta = ( delta_sr - delta_sl ) / b_; 45 | double delta_s = 0.5 * ( delta_sr + delta_sl ); 46 | 47 | double tmp_th = th_ + 0.5 * delta_theta; 48 | double cos_tmp_th = cos ( tmp_th ); 49 | double sin_tmp_th = sin ( tmp_th ); 50 | 51 | x_ += delta_s * cos_tmp_th; 52 | y_ += delta_s * sin_tmp_th; 53 | th_ += delta_theta; 54 | th_ = normAngle ( th_ ); 55 | 56 | Trr_ = Sophus::SE2 ( th_, Eigen::Vector2d ( x_, y_ ) ); 57 | 58 | Eigen::Matrix3d Gt; 59 | Gt << 1.0, 0.0, -delta_s * sin_tmp_th, 60 | 0.0, 1.0, delta_s * cos_tmp_th, 61 | 0.0, 0.0, 1.0; 62 | 63 | Eigen::Matrix Gu; 64 | Gu << 0.5 * ( cos_tmp_th - delta_s * sin_tmp_th / b_ ), 0.5 * ( cos_tmp_th + delta_s * sin_tmp_th / b_ ), 65 | 0.5 * ( sin_tmp_th + delta_s * cos_tmp_th /b_ ), 0.5 * ( sin_tmp_th - delta_s * cos_tmp_th/b_ ), 66 | 1.0/b_, -1.0/b_; 67 | 68 | Eigen::Matrix2d sigma_u; 69 | sigma_u << k_ * k_ * delta_sr * delta_sr, 0.0, 0.0, k_ * k_* delta_sl * delta_sl; 70 | 71 | cov_ = Gt * cov_ *Gt.transpose() + Gu * sigma_u * Gu.transpose() ; 72 | } // odom 73 | 74 | return Trr_; 75 | } 76 | 77 | Sophus::SE2 EncoderIntegration::addEncoders ( const std::vector& encs ) 78 | { 79 | for ( size_t i = 0; i < encs.size(); i ++ ) { 80 | const Encoder& enc = encs.at ( i ); 81 | addEncoder ( enc ); 82 | } 83 | } 84 | 85 | 86 | Eigen::Matrix3d EncoderIntegration::getCov() 87 | { 88 | return cov_; 89 | } // getCov 90 | 91 | 92 | Sophus::SE2 EncoderIntegration::getTrr() 93 | { 94 | return Trr_; 95 | } // getTrr 96 | 97 | 98 | } // namespace dre_slam 99 | -------------------------------------------------------------------------------- /core/src/feature_detector.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | namespace dre_slam 20 | { 21 | 22 | FeatureDetector::FeatureDetector ( int n_features, float scale_factor, int n_levels, int ini_th, int min_th ) 23 | { 24 | orb_extractor_ = new ORBextractor ( n_features, scale_factor, n_levels, ini_th, min_th ); 25 | } 26 | 27 | void FeatureDetector::detect ( const cv::Mat& img, std::vector& keypoints, cv::Mat& descriptors ) 28 | { 29 | keypoints.clear(); 30 | ( *orb_extractor_ ) ( img, cv::Mat(), keypoints, descriptors ); 31 | } // detect 32 | 33 | } //namespace dre_slam 34 | -------------------------------------------------------------------------------- /core/src/local_mapping.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | 20 | namespace dre_slam 21 | { 22 | 23 | LocalMapping::LocalMapping ( LoopClosing* loop_closing, SubOctoMapConstruction* sub_OctoMap_construction, RosPuber* ros_puber, Optimizer* optimizer, Map* map, Camera* cam, Config* cfg ) : 24 | loop_closing_ ( loop_closing ), sub_OctoMap_construction_( sub_OctoMap_construction ), ros_puber_(ros_puber), optimizer_ ( optimizer ), map_ ( map ), cam_ ( cam ), cfg_ ( cfg ) 25 | { 26 | // start thread 27 | th_local_mapping_ = new std::thread ( &LocalMapping::processing, this ); 28 | 29 | } // local mapping 30 | 31 | void LocalMapping::insertKeyFrame ( KeyFrame* kf ) 32 | { 33 | std::unique_lock lock ( mutex_kfs_queue_ ); 34 | kfs_queue_.push ( kf ); 35 | } 36 | 37 | bool LocalMapping::checkNewFrame() 38 | { 39 | std::unique_lock lock ( mutex_kfs_queue_ ); 40 | return ( !kfs_queue_.empty() ); 41 | } 42 | 43 | KeyFrame* LocalMapping::getNewKeyFrame() 44 | { 45 | std::unique_lock lock ( mutex_kfs_queue_ ); 46 | KeyFrame* kf = kfs_queue_.front(); 47 | kfs_queue_.pop(); 48 | return kf; 49 | } 50 | 51 | void LocalMapping::processing() 52 | { 53 | while ( true ) { 54 | 55 | if ( checkNewFrame() == true ) { 56 | 57 | KeyFrame* kf = getNewKeyFrame(); 58 | 59 | kf->selectStaticFeatures(); 60 | 61 | // add new mpts. 62 | int N = addNewMpts ( kf ); 63 | 64 | // add visual edges. 65 | addVisualEdges ( kf ); 66 | 67 | // Local optimization. 68 | if ( map_->getKFsSize() > 1.5 * cfg_->sm_lm_window_size_ ) { 69 | 70 | std::vector opt_kfs; 71 | map_->getLastNKeyFrames( cfg_->sm_lm_window_size_, true, opt_kfs ); 72 | 73 | RunTimer t; t.start(); 74 | optimizer_->localBA ( opt_kfs ); 75 | t.stop(); 76 | std::cout << "\nKeyFrame " << kf->kfid_ << " Local BA time: " << t.duration() << "\n\n"; 77 | } 78 | 79 | // send keyframe to other thread. 80 | loop_closing_->insertKeyFrame ( kf ); 81 | 82 | // send kf for octomap construction. 83 | sub_OctoMap_construction_->insertKeyFrame ( kf ); 84 | 85 | // Publish KFs, and pose graph. 86 | ros_puber_->pubSparseMap( map_ ); 87 | } 88 | usleep ( 5000 ); // sleep 5 ms 89 | } // while true. 90 | 91 | } // processing 92 | 93 | 94 | int LocalMapping::addNewMpts ( KeyFrame* kf ) 95 | { 96 | int N = 0; 97 | for ( size_t i = 0; i < kf->mpts_.size(); i ++ ) { 98 | 99 | // skip dynamic features. 100 | if ( kf->static_flags_.at ( i ) == false ) { 101 | continue; 102 | } 103 | 104 | MapPoint* mpt = kf->mpts_.at ( i ); 105 | 106 | // If have a map point / add observation. 107 | if ( mpt != NULL ) { 108 | mpt->addObservation ( kf, i ); 109 | continue; 110 | } 111 | 112 | // If no map point. created one. 113 | const cv::KeyPoint& kp = kf->kps_.at ( i ); 114 | 115 | double& dp = kf->dps_.at ( i ); 116 | if ( ( dp < cfg_->cam_dmin_ ) || ( dp > cfg_->cam_dmax_ ) ) { 117 | continue; 118 | } 119 | 120 | Eigen::Vector3d ptc = cam_ ->img2Cam ( Eigen::Vector2d ( kp.pt.x, kp.pt.y ), dp ); 121 | Eigen::Vector3d ptw =kf->getSE3Pose() * cfg_->Trc_ * ptc; 122 | MapPoint* nmpt = new MapPoint ( ptw, kf->des_.row ( i ) , cfg_ ); 123 | kf->mpts_.at ( i ) = nmpt; 124 | nmpt->addObservation ( kf, i ); 125 | map_->addMapPoint ( nmpt ); 126 | 127 | N ++; 128 | }// for all mpts in kf 129 | 130 | return N; 131 | } // addNewMpts 132 | 133 | 134 | void LocalMapping::addVisualEdges ( KeyFrame* kf ) 135 | { 136 | std::vector& mpts = kf->mpts_; 137 | std::map kfs_statistics; 138 | 139 | for ( MapPoint* mpt: mpts ) { 140 | if ( mpt == NULL ) { 141 | continue; 142 | } 143 | 144 | std::map::iterator it; 145 | for ( it = mpt->ob_kfs_.begin(); it != mpt->ob_kfs_.end(); it ++ ) { 146 | KeyFrame* nkf = it->first; 147 | kfs_statistics[nkf] ++; 148 | } // for all ob kfs 149 | }//for all mpts 150 | 151 | std::map::iterator it; 152 | for ( it = kfs_statistics.begin(); it != kfs_statistics.end(); it ++ ) { 153 | if ( it->first != kf && it->first != kf->getLastKeyFrameEdge() ) { 154 | if ( it->second > cfg_->sm_lm_cobv_th_ ) { 155 | kf->addVisualEdge ( it->first ); 156 | }// 157 | } // if not last kf 158 | } // for all kfs_statistics 159 | 160 | } // addVisualEdges 161 | 162 | } // namespace dre_slam 163 | -------------------------------------------------------------------------------- /core/src/map_point.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | 19 | namespace dre_slam 20 | { 21 | MapPoint::MapPoint ( const Eigen::Vector3d& ptw, const cv::Mat& des , Config* cfg_) : 22 | ptw_ ( ptw ), des_ ( des.clone() ) 23 | { 24 | is_selected_ = false; 25 | } // MapPoint 26 | 27 | void MapPoint::setPosition ( const Eigen::Vector3d& ptw ) 28 | { 29 | std::unique_lock lock ( mutex_position_ ); 30 | ptw_ = ptw; 31 | } // setPosition 32 | 33 | Eigen::Vector3d MapPoint::getPosition() 34 | { 35 | std::unique_lock lock ( mutex_position_ ); 36 | return ptw_; 37 | } // getPosition 38 | 39 | 40 | void MapPoint::addObservation ( KeyFrame* kf, int idx ) 41 | { 42 | if ( ob_kfs_.empty() ) 43 | first_ob_kf_ = kf ; 44 | 45 | ob_kfs_[kf] = idx; 46 | 47 | } // addObservation 48 | 49 | 50 | cv::Mat& MapPoint::getDescriptor() 51 | { 52 | return des_; 53 | } // getDescriptor 54 | 55 | 56 | void MapPoint::cvtEigen2Double() 57 | { 58 | std::unique_lock lock ( mutex_position_ ); 59 | ptwd_[0] = ptw_( 0 ); 60 | ptwd_[1] = ptw_( 1 ); 61 | ptwd_[2] = ptw_( 2 ); 62 | } 63 | 64 | void MapPoint::cvtDouble2Eigen() 65 | { 66 | std::unique_lock lock ( mutex_position_ ); 67 | ptw_[0] = ptwd_[0]; 68 | ptw_[1] = ptwd_[1]; 69 | ptw_[2] = ptwd_[2] ; 70 | } 71 | 72 | 73 | } // namespace dre_slam 74 | 75 | -------------------------------------------------------------------------------- /core/src/sub_octomap.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | 18 | #include 19 | 20 | namespace dre_slam 21 | { 22 | 23 | SubOctomap::SubOctomap ( Config* cfg ) :cfg_ ( cfg ) 24 | { 25 | sub_octree_ = new octomap::OcTree ( cfg->oc_voxel_size_ ); 26 | sub_octree_->setOccupancyThres ( cfg->oc_occ_th_ ); 27 | sub_octree_->setProbHit ( cfg->oc_prob_hit_ ); 28 | sub_octree_->setProbMiss ( cfg->oc_prob_miss_ ); 29 | } // constructor 30 | 31 | void SubOctomap::insertKeyFrame ( KeyFrame* kf, octomap::Pointcloud& point_cloud_c ) 32 | { 33 | if ( kfs_.size() == 0 ) { 34 | kf_base_ = kf; 35 | 36 | } // the first is set as base. 37 | 38 | kfs_.insert ( kf ); 39 | 40 | Sophus::SE3 Tcw_base = ( kf_base_->getSE3Pose() * cfg_->Trc_ ).inverse(); 41 | Sophus::SE3 Twc = kf->getSE3Pose() * cfg_->Trc_; 42 | Sophus::SE3 Tbc = Tcw_base * Twc; // base to cur kf. 43 | 44 | // rotate the point cloud to base frame. 45 | octomap::Pointcloud point_cloud_b; 46 | for ( size_t i = 0; i < point_cloud_c.size(); i ++ ) { 47 | octomap::point3d& pt = point_cloud_c[i]; 48 | Eigen::Vector3d ptc ( pt.x(), pt.y(), pt.z() ); 49 | Eigen::Vector3d ptb = Tbc * ptc; 50 | 51 | // Delete error points. 52 | if(ptb[2] > 6.0 || ptb[2] < -2.0) 53 | continue; 54 | 55 | 56 | point_cloud_b.push_back ( octomap::point3d ( ptb[0], ptb[1], ptb[2] ) ); 57 | } 58 | 59 | // update the sub octree. 60 | Eigen::Vector3d& pt_o = Tbc.translation(); 61 | sub_octree_->insertPointCloud ( point_cloud_b, octomap::point3d ( pt_o[0],pt_o[1],pt_o[2] ), -1, true, true ); 62 | sub_octree_->updateInnerOccupancy(); 63 | } // insertKeyFrame 64 | 65 | 66 | bool SubOctomap::isContainKeyframe ( KeyFrame* kf ) 67 | { 68 | std::set::iterator iter = kfs_.find ( kf ); 69 | if ( iter == kfs_.end() ) { 70 | return false; 71 | } 72 | return true; 73 | } // isContainKeyframe 74 | 75 | 76 | } // namespace dre_slam 77 | 78 | -------------------------------------------------------------------------------- /core/src/sub_octomap_construction.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace dre_slam 25 | { 26 | 27 | SubOctoMapConstruction::SubOctoMapConstruction ( OctoMapFusion* octomap_fusion, Config* cfg ) : octomap_fusion_ ( octomap_fusion ), cfg_ ( cfg ) 28 | { 29 | sub_octomap_construction_thread_ = new std::thread ( &SubOctoMapConstruction::processing, this ); 30 | nkf_passed_ = 0; 31 | } // SubOctomapping 32 | 33 | 34 | void SubOctoMapConstruction::processing() 35 | { 36 | while ( true ) { 37 | 38 | if ( checkNewFrame() == true ) { 39 | KeyFrame* kf = getNewKeyFrame(); 40 | 41 | /*std::unique_lock lock ( mutex_all_data_ );*/ 42 | 43 | // construct sub map. 44 | if ( nkf_passed_ == 0 ) { 45 | cur_sub_map_ = new SubOctomap ( cfg_ ); 46 | } 47 | 48 | // Construct 3D point cloud in the camera frame. 49 | octomap::Pointcloud point_cloud_c; 50 | //createCleanCloud ( kf, point_cloud_c ); // voxel_grid filtered 51 | createPointCloud( kf, point_cloud_c ); 52 | 53 | // Insert one scan to full map. 54 | octomap_fusion_->insertOneScan2FullMapAndPub ( kf, point_cloud_c ); 55 | 56 | // Insert one scan to cur sub map. 57 | cur_sub_map_->insertKeyFrame ( kf, point_cloud_c ); 58 | 59 | // Check if need to construct new submap. 60 | nkf_passed_ ++; 61 | if ( nkf_passed_ > cfg_->oc_submap_size_ ) { 62 | nkf_passed_ = 0; 63 | SubOctomap* new_sub_map = cur_sub_map_; 64 | 65 | // insert one submap to fullmap. 66 | octomap_fusion_->insertSubMap ( new_sub_map ); 67 | } // if have to insert submap. 68 | 69 | } // if have new keyframe. 70 | usleep ( 5000 ); 71 | }// while true. 72 | } // processing new keyframe. 73 | 74 | 75 | void SubOctoMapConstruction::insertKeyFrame ( KeyFrame* kf ) 76 | { 77 | std::unique_lock lock ( mutex_kfs_queue_ ); 78 | kfs_queue_.push ( kf ); 79 | } 80 | 81 | 82 | bool SubOctoMapConstruction::checkNewFrame() 83 | { 84 | std::unique_lock lock ( mutex_kfs_queue_ ); 85 | return ( !kfs_queue_.empty() ); 86 | } // checkNewFrame 87 | 88 | KeyFrame* SubOctoMapConstruction::getNewKeyFrame() 89 | { 90 | std::unique_lock lock ( mutex_kfs_queue_ ); 91 | KeyFrame* kf = kfs_queue_.front(); 92 | kfs_queue_.pop(); 93 | return kf; 94 | } // getNewKeyFrame 95 | 96 | 97 | void SubOctoMapConstruction::createPointCloud ( KeyFrame* kf, octomap::Pointcloud& point_cloud ) 98 | { 99 | cv::Mat& depth = kf->depth_; 100 | Camera* camera = kf->cam_; 101 | 102 | for ( int v = 0; v < depth.rows; v ++ ) { 103 | for ( int u = 0; u < depth.cols; u ++ ) { 104 | 105 | float dp = depth.at ( v,u ); 106 | if ( dp > cfg_->cam_dmin_ && dp < cfg_->cam_dmax_ ) { 107 | Eigen::Vector3d ptc = camera->img2Cam ( Eigen::Vector2d ( u, v ), dp ); 108 | point_cloud.push_back ( ptc[0], ptc[1], ptc[2] ); 109 | } // if is good point. 110 | } // for all pixels. 111 | } // for all pixels. 112 | 113 | } // createPointCloud 114 | 115 | void SubOctoMapConstruction::createCleanCloud ( KeyFrame* kf, octomap::Pointcloud& point_cloud ) 116 | { 117 | cv::Mat& depth = kf->depth_; 118 | Camera* camera = kf->cam_; 119 | 120 | // Create point cloud from the depth image. 121 | pcl::PointCloud::Ptr cloud ( new pcl::PointCloud ); 122 | pcl::PointCloud::Ptr cloud_filtered ( new pcl::PointCloud ); 123 | for ( int v = 0; v < depth.rows; v ++ ) { 124 | for ( int u = 0; u < depth.cols; u ++ ) { 125 | 126 | float dp = depth.at ( v,u ); 127 | if ( dp > cfg_->cam_dmin_ && dp < cfg_->cam_dmax_ ) { 128 | Eigen::Vector3d ptc = camera->img2Cam ( Eigen::Vector2d ( u, v ), dp ); 129 | 130 | cloud->points.push_back ( pcl::PointXYZ ( ptc[0], ptc[1], ptc[2] ) ); 131 | } // if is good point. 132 | } // for all pixels. 133 | } // for all pixels. 134 | 135 | 136 | // Create the filtering object 137 | pcl::VoxelGrid sor; 138 | sor.setInputCloud ( cloud ); 139 | sor.setLeafSize ( 0.25*cfg_->oc_voxel_size_, 0.25*cfg_->oc_voxel_size_, 0.25*cfg_->oc_voxel_size_ ); 140 | sor.filter ( *cloud_filtered ); 141 | 142 | // Assigned octomap cloud 143 | for ( size_t i = 0; i < cloud_filtered->size(); i ++ ) { 144 | pcl::PointXYZ pt = cloud_filtered->points.at ( i ); 145 | point_cloud.push_back ( pt.x, pt.y, pt.z ); 146 | } 147 | } // createCleanCloud 148 | 149 | 150 | void SubOctoMapConstruction::insertAllData2OctoMapFusion() 151 | { 152 | // std::unique_lock lock ( mutex_all_data_ ); 153 | SubOctomap* new_sub_map = cur_sub_map_; 154 | if ( new_sub_map->kfs_.size() == 0 ) { 155 | return; 156 | } 157 | octomap_fusion_->insertSubMap ( new_sub_map ); 158 | nkf_passed_ = 0; 159 | } // requestAllData 160 | 161 | 162 | } // namespace dre_slam 163 | -------------------------------------------------------------------------------- /corridor.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ydsf16/dre_slam/346e740578d2e58e1b2daa4fd60c811af7134375/corridor.gif -------------------------------------------------------------------------------- /launch/comparative_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/extention_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /node/dre_slam_node.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace dre_slam; 27 | 28 | class SensorGrabber 29 | { 30 | public: 31 | SensorGrabber ( DRE_SLAM* slam ) :slam_ ( slam ) {} 32 | 33 | void grabRGBD ( const sensor_msgs::ImageConstPtr& msg_rgb,const sensor_msgs::ImageConstPtr& msg_depth ) { 34 | // Get images. 35 | cv_bridge::CvImageConstPtr cv_ptr_rgb = cv_bridge::toCvShare ( msg_rgb ); 36 | cv_bridge::CvImageConstPtr cv_ptr_depth = cv_bridge::toCvShare ( msg_depth ); 37 | 38 | // Add RGB-D images. 39 | slam_->addRGBDImage ( cv_ptr_rgb->image, cv_ptr_depth->image, cv_ptr_rgb->header.stamp.toSec() ); 40 | 41 | }// grabRGBD 42 | 43 | void grabEncoder ( const geometry_msgs::QuaternionStamped::ConstPtr& en_ptr ) { 44 | 45 | // Extract left and right encoder measurements. 46 | double enl1 = en_ptr->quaternion.x; 47 | double enl2 = en_ptr->quaternion.y; 48 | double enr1 = en_ptr->quaternion.z; 49 | double enr2 = en_ptr->quaternion.w; 50 | 51 | // Calculate left and right encoder. 52 | double enl = 0.5* ( enl1 + enl2 ); 53 | double enr = 0.5* ( enr1 + enr2 ); 54 | double ts= en_ptr->header.stamp.toSec(); 55 | 56 | // Check bad data. 57 | { 58 | 59 | if ( last_enl_ == 0 && last_enr_ == 0 ) { 60 | last_enl_ = enl; 61 | last_enr_ = enr; 62 | return; 63 | } 64 | 65 | double delta_enl = fabs ( enl - last_enl_ ); 66 | double delta_enr = fabs ( enr - last_enr_ ); 67 | 68 | const double delta_th = 4000; 69 | 70 | if ( delta_enl > delta_th || delta_enr > delta_th ) { 71 | std::cout << "\nJUMP\n"; 72 | return; 73 | } 74 | 75 | last_enl_ = enl; 76 | last_enr_ = enr; 77 | } 78 | 79 | // Add encoder measurements. 80 | slam_->addEncoder ( enl, enr, ts ); 81 | }// grabEncoder 82 | 83 | private: 84 | DRE_SLAM* slam_; 85 | double last_enl_ = 0; 86 | double last_enr_ = 0; 87 | }; 88 | 89 | int main ( int argc, char** argv ) 90 | { 91 | // Init ROS 92 | ros::init ( argc, argv, "DRE_SLAM" ); 93 | ros::start(); 94 | ros::NodeHandle nh; 95 | 96 | // Load parameters 97 | std::string dre_slam_cfg_dir, orbvoc_dir, yolov3_classes_dir, yolov3_model_dir, yolov3_weights_dir; 98 | std::string results_dir; 99 | if ( ! nh.getParam ( "/dre_slam_node/dre_slam_cfg_dir", dre_slam_cfg_dir ) ) { 100 | std::cout << "Read dre_slam_cfg_dir failure !\n"; 101 | return -1; 102 | } 103 | 104 | if ( ! nh.getParam ( "/dre_slam_node/orbvoc_dir", orbvoc_dir ) ) { 105 | std::cout << "Read orbvoc_dir failure !\n"; 106 | return -1; 107 | } 108 | 109 | if ( ! nh.getParam ( "/dre_slam_node/yolov3_classes_dir", yolov3_classes_dir ) ) { 110 | std::cout << "Read yolov3_classes_dir failure !\n"; 111 | return -1; 112 | } 113 | if ( ! nh.getParam ( "/dre_slam_node/yolov3_model_dir", yolov3_model_dir ) ) { 114 | std::cout << "Read yolov3_model_dir failure !\n"; 115 | return -1; 116 | } 117 | if ( ! nh.getParam ( "/dre_slam_node/yolov3_weights_dir", yolov3_weights_dir ) ) { 118 | std::cout << "Read yolov3_weights_dir failure !\n"; 119 | return -1; 120 | } 121 | 122 | if ( ! nh.getParam ( "/dre_slam_node/results_dir", results_dir ) ) { 123 | std::cout << "Read results_dir failure !\n"; 124 | return -1; 125 | } 126 | 127 | // Load system configure. 128 | Config* cfg = new Config( dre_slam_cfg_dir ); 129 | 130 | // Init SLAM system. 131 | DRE_SLAM slam( nh, cfg, orbvoc_dir, yolov3_classes_dir, yolov3_model_dir, yolov3_weights_dir ); 132 | 133 | // Sub topics. 134 | SensorGrabber sensors ( &slam ); 135 | message_filters::Subscriber rgb_sub ( nh, cfg->cam_rgb_topic_, 1 ); 136 | message_filters::Subscriber depth_sub ( nh, cfg->cam_depth_topic_, 1 ); 137 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 138 | message_filters::Synchronizer sync ( sync_pol ( 5 ), rgb_sub,depth_sub ); 139 | sync.registerCallback ( boost::bind ( &SensorGrabber::grabRGBD,&sensors,_1,_2 ) ); // 140 | ros::Subscriber encoder_sub = nh.subscribe ( cfg->encoder_topic_, 1, &SensorGrabber::grabEncoder,&sensors ); 141 | 142 | std::cout << "\n\nDRE-SLAM Started\n\n"; 143 | 144 | ros::spin(); 145 | 146 | // System Stoped. 147 | std::cout << "\n\nDRE-SLAM Stoped\n\n"; 148 | 149 | // Save results. 150 | mkdir(results_dir.c_str(), S_IRWXU ); // Make a new folder. 151 | slam.saveFrames( results_dir +"/pose_frames.txt" ); 152 | slam.saveKeyFrames( results_dir +"/pose_keyframes.txt" ); 153 | slam.saveOctoMap( results_dir +"/octomap.bt" ); 154 | 155 | std::cout << "Results have been saved to \"" + results_dir + "\".\n\n\n\n"; 156 | return 0; 157 | } 158 | 159 | 160 | -------------------------------------------------------------------------------- /object_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project(object_detection) 3 | 4 | add_compile_options(-std=c++11) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(OpenCV 4 REQUIRED) 8 | 9 | include_directories( 10 | ${OpenCV_INCLUDE_DIRS} 11 | ${PROJECT_SOURCE_DIR} 12 | ) 13 | 14 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 15 | 16 | add_library(object_detector SHARED 17 | object_detector.cpp 18 | ) 19 | 20 | target_link_libraries(object_detector 21 | ${OpenCV_LIBS} 22 | ) 23 | 24 | -------------------------------------------------------------------------------- /object_detector/object_detector.h: -------------------------------------------------------------------------------- 1 | // This file is part of dre_slam - Dynamic RGB-D Encoder SLAM for Differential-Drive Robot. 2 | // 3 | // Copyright (C) 2019 Dongsheng Yang 4 | // (Biologically Inspired Mobile Robot Laboratory, Robotics Institute, Beihang University) 5 | // 6 | // dre_slam is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // dre_slam is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef OBJECT_DETECTOR_H 18 | #define OBJECT_DETECTOR_H 19 | 20 | #include 21 | 22 | class Object{ 23 | public: 24 | Object(){}; 25 | std::string name_; 26 | int id_; 27 | cv::Rect rect_box_; 28 | float score_; 29 | }; //class Objects 30 | 31 | 32 | class ObjectDetector{ 33 | public: 34 | ObjectDetector( const std::string& classes_file, const std::string& model_config, const std::string& model_weights ); 35 | int detect( const cv::Mat& imag_in, cv::Mat& image_out, std::vector< Object >& objects); 36 | 37 | private: 38 | std::vector classes_; 39 | }; //class ObjectDetector 40 | 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dre_slam 4 | 0.0.0 5 | The dre_slam package 6 | 7 | 8 | 9 | 10 | yds 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | roscpp 54 | roscpp 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /third_party/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 3.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FORB.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.cpp 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | * Distance function has been modified 9 | * 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "FORB.h" 19 | 20 | using namespace std; 21 | 22 | namespace DBoW2 { 23 | 24 | // -------------------------------------------------------------------------- 25 | 26 | const int FORB::L=32; 27 | 28 | void FORB::meanValue(const std::vector &descriptors, 29 | FORB::TDescriptor &mean) 30 | { 31 | if(descriptors.empty()) 32 | { 33 | mean.release(); 34 | return; 35 | } 36 | else if(descriptors.size() == 1) 37 | { 38 | mean = descriptors[0]->clone(); 39 | } 40 | else 41 | { 42 | vector sum(FORB::L * 8, 0); 43 | 44 | for(size_t i = 0; i < descriptors.size(); ++i) 45 | { 46 | const cv::Mat &d = *descriptors[i]; 47 | const unsigned char *p = d.ptr(); 48 | 49 | for(int j = 0; j < d.cols; ++j, ++p) 50 | { 51 | if(*p & (1 << 7)) ++sum[ j*8 ]; 52 | if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; 53 | if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; 54 | if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; 55 | if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; 56 | if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; 57 | if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; 58 | if(*p & (1)) ++sum[ j*8 + 7 ]; 59 | } 60 | } 61 | 62 | mean = cv::Mat::zeros(1, FORB::L, CV_8U); 63 | unsigned char *p = mean.ptr(); 64 | 65 | const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; 66 | for(size_t i = 0; i < sum.size(); ++i) 67 | { 68 | if(sum[i] >= N2) 69 | { 70 | // set bit 71 | *p |= 1 << (7 - (i % 8)); 72 | } 73 | 74 | if(i % 8 == 7) ++p; 75 | } 76 | } 77 | } 78 | 79 | // -------------------------------------------------------------------------- 80 | 81 | int FORB::distance(const FORB::TDescriptor &a, 82 | const FORB::TDescriptor &b) 83 | { 84 | // Bit set count operation from 85 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 86 | 87 | const int *pa = a.ptr(); 88 | const int *pb = b.ptr(); 89 | 90 | int dist=0; 91 | 92 | for(int i=0; i<8; i++, pa++, pb++) 93 | { 94 | unsigned int v = *pa ^ *pb; 95 | v = v - ((v >> 1) & 0x55555555); 96 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 97 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 98 | } 99 | 100 | return dist; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | std::string FORB::toString(const FORB::TDescriptor &a) 106 | { 107 | stringstream ss; 108 | const unsigned char *p = a.ptr(); 109 | 110 | for(int i = 0; i < a.cols; ++i, ++p) 111 | { 112 | ss << (int)*p << " "; 113 | } 114 | 115 | return ss.str(); 116 | } 117 | 118 | // -------------------------------------------------------------------------- 119 | 120 | void FORB::fromString(FORB::TDescriptor &a, const std::string &s) 121 | { 122 | a.create(1, FORB::L, CV_8U); 123 | unsigned char *p = a.ptr(); 124 | 125 | stringstream ss(s); 126 | for(int i = 0; i < FORB::L; ++i, ++p) 127 | { 128 | int n; 129 | ss >> n; 130 | 131 | if(!ss.fail()) 132 | *p = (unsigned char)n; 133 | } 134 | 135 | } 136 | 137 | // -------------------------------------------------------------------------- 138 | 139 | void FORB::toMat32F(const std::vector &descriptors, 140 | cv::Mat &mat) 141 | { 142 | if(descriptors.empty()) 143 | { 144 | mat.release(); 145 | return; 146 | } 147 | 148 | const size_t N = descriptors.size(); 149 | 150 | mat.create(N, FORB::L*8, CV_32F); 151 | float *p = mat.ptr(); 152 | 153 | for(size_t i = 0; i < N; ++i) 154 | { 155 | const int C = descriptors[i].cols; 156 | const unsigned char *desc = descriptors[i].ptr(); 157 | 158 | for(int j = 0; j < C; ++j, p += 8) 159 | { 160 | p[0] = (desc[j] & (1 << 7) ? 1 : 0); 161 | p[1] = (desc[j] & (1 << 6) ? 1 : 0); 162 | p[2] = (desc[j] & (1 << 5) ? 1 : 0); 163 | p[3] = (desc[j] & (1 << 4) ? 1 : 0); 164 | p[4] = (desc[j] & (1 << 3) ? 1 : 0); 165 | p[5] = (desc[j] & (1 << 2) ? 1 : 0); 166 | p[6] = (desc[j] & (1 << 1) ? 1 : 0); 167 | p[7] = desc[j] & (1); 168 | } 169 | } 170 | } 171 | 172 | // -------------------------------------------------------------------------- 173 | 174 | void FORB::toMat8U(const std::vector &descriptors, 175 | cv::Mat &mat) 176 | { 177 | mat.create(descriptors.size(), 32, CV_8U); 178 | 179 | unsigned char *p = mat.ptr(); 180 | 181 | for(size_t i = 0; i < descriptors.size(); ++i, p += 32) 182 | { 183 | const unsigned char *d = descriptors[i].ptr(); 184 | std::copy(d, d+32, p); 185 | } 186 | 187 | } 188 | 189 | // -------------------------------------------------------------------------- 190 | 191 | } // namespace DBoW2 192 | 193 | 194 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/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 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /third_party/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/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 | -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /third_party/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /third_party/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /third_party/Sophus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME Sophus) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | 6 | SET (CMAKE_VERBOSE_MAKEFILE ON) 7 | 8 | IF( NOT CMAKE_BUILD_TYPE ) 9 | SET( CMAKE_BUILD_TYPE Release ) 10 | ENDIF() 11 | 12 | IF (CMAKE_COMPILER_IS_GNUCXX ) 13 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 14 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") 15 | 16 | ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable 17 | -Wno-unused-but-set-variable -Wno-unknown-pragmas ") 18 | ENDIF() 19 | 20 | ################################################################################ 21 | # Add local path for finding packages, set the local version first 22 | set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) 23 | list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) 24 | 25 | ################################################################################ 26 | # Create variables used for exporting in SophusConfig.cmake 27 | set( Sophus_LIBRARIES "" ) 28 | set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) 29 | 30 | ################################################################################ 31 | 32 | #SET (INCLUDE_DIRS "../eigen3.1/") 33 | find_package( Eigen3 REQUIRED ) 34 | INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) 35 | SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) 36 | 37 | SET (SOURCE_DIR "sophus") 38 | SET (CLASSES so2 39 | se2 40 | se3 41 | so3 42 | scso3 43 | sim3 44 | ) 45 | 46 | SET (SOURCES) 47 | 48 | FOREACH(class ${CLASSES}) 49 | LIST(APPEND SOURCES ${SOURCE_DIR}/${class}.cpp ${SOURCE_DIR}/${class}.h) 50 | ENDFOREACH(class) 51 | 52 | LINK_LIBRARIES (${PROJECT_NAME} ${LIBS}) 53 | 54 | set( Sophus_LIBRARIES ${Sophus_LIBRARIES} ${LIBS} ) 55 | 56 | INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) 57 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES}) 58 | 59 | ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp) 60 | ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp) 61 | ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp) 62 | ADD_EXECUTABLE(test_scso3 sophus/test_scso3.cpp) 63 | ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp) 64 | ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp) 65 | ENABLE_TESTING() 66 | 67 | ADD_TEST(test_so2 test_so2) 68 | ADD_TEST(test_se2 test_se2) 69 | ADD_TEST(test_so3 test_so3) 70 | ADD_TEST(test_scso3 test_scso3) 71 | ADD_TEST(test_se3 test_se3) 72 | ADD_TEST(test_sim3 test_sim3) 73 | 74 | ############################################################################## 75 | # Get full library name 76 | GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) 77 | set( Sophus_LIBRARIES ${Sophus_LIBRARIES} ${FULL_LIBRARY_NAME} ) 78 | set( Sophus_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) 79 | 80 | ################################################################################ 81 | # Create the SophusConfig.cmake file for other cmake projects. 82 | CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in 83 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) 84 | export( PACKAGE Sophus ) 85 | 86 | INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) 87 | INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) -------------------------------------------------------------------------------- /third_party/Sophus/README: -------------------------------------------------------------------------------- 1 | #Sophus 2 | -------------------------------------------------------------------------------- /third_party/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Sophus source dir 3 | set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ################################################################################ 6 | # Sophus build dir 7 | set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ################################################################################ 10 | set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) 11 | set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) 12 | 13 | set( Sophus_LIBRARIES "@Sophus_LIBRARIES@" ) 14 | set( Sophus_LIBRARY "@Sophus_LIBRARIES@" ) 15 | 16 | set( Sophus_LIBRARY_DIR "@Sophus_LIBRARY_DIR@" ) 17 | set( Sophus_LIBRARY_DIRS "@Sophus_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /third_party/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if( EIGEN3_INCLUDE_DIR ) 12 | # in cache already 13 | set( EIGEN3_FOUND TRUE ) 14 | else (EIGEN3_INCLUDE_DIR) 15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core 16 | PATH_SUFFIXES eigen3/ 17 | HINTS 18 | ${INCLUDE_INSTALL_DIR} 19 | /usr/local/include 20 | ${KDE4_INCLUDE_DIR} 21 | ) 22 | include( FindPackageHandleStandardArgs ) 23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 24 | mark_as_advanced( EIGEN3_INCLUDE_DIR ) 25 | endif(EIGEN3_INCLUDE_DIR) 26 | 27 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/scso3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_ScSO3_H 24 | #define SOPHUS_ScSO3_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "so3.h" 31 | 32 | 33 | namespace Sophus 34 | { 35 | using namespace Eigen; 36 | 37 | class ScSO3 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | ScSO3 (); 43 | 44 | ScSO3 (const ScSO3 & other); 45 | 46 | explicit 47 | ScSO3 (const Matrix3d & scale_times_R); 48 | 49 | ScSO3 (double scale, 50 | const SO3 & so3); 51 | 52 | ScSO3 (double scale, 53 | const Matrix3d & R); 54 | 55 | explicit 56 | ScSO3 (const Quaterniond & quaternion); 57 | 58 | void 59 | operator= (const ScSO3 & ScSO3); 60 | 61 | ScSO3 62 | operator* (const ScSO3 & ScSO3) const; 63 | 64 | void 65 | operator*= (const ScSO3 & ScSO3); 66 | 67 | Vector3d 68 | operator* (const Vector3d & xyz) const; 69 | 70 | ScSO3 71 | inverse () const; 72 | 73 | Matrix3d 74 | matrix () const; 75 | 76 | Matrix3d 77 | rotationMatrix () const; 78 | 79 | double 80 | scale () const; 81 | 82 | void 83 | setRotationMatrix (const Matrix3d & R); 84 | 85 | void 86 | setScale (double scale); 87 | 88 | Matrix4d 89 | Adj () const; 90 | 91 | Matrix3d 92 | generator (int i); 93 | 94 | Vector4d 95 | log () const; 96 | 97 | static ScSO3 98 | exp (const Vector4d & omega); 99 | 100 | static ScSO3 101 | expAndTheta (const Vector4d & omega, 102 | double * theta); 103 | 104 | static Vector4d 105 | log (const ScSO3 & ScSO3); 106 | 107 | static Vector4d 108 | logAndTheta (const ScSO3 & ScSO3, 109 | double * theta); 110 | 111 | static Matrix3d 112 | hat (const Vector4d & omega); 113 | 114 | static Vector4d 115 | vee (const Matrix3d & Omega); 116 | 117 | static Vector4d 118 | lieBracket (const Vector4d & omega1, 119 | const Vector4d & omega2); 120 | 121 | static Matrix4d 122 | d_lieBracketab_by_d_a (const Vector4d & b); 123 | 124 | 125 | const Quaterniond & quaternion() const 126 | { 127 | return quaternion_; 128 | } 129 | Quaterniond& quaternion() 130 | { 131 | return quaternion_; 132 | } 133 | 134 | static const int DoF = 4; 135 | 136 | protected: 137 | Quaterniond quaternion_; 138 | }; 139 | 140 | inline std::ostream& operator <<(std::ostream & out_str, 141 | const ScSO3& scso3) 142 | { 143 | out_str << scso3.scale() << " * " << 144 | scso3.log().head<3>().transpose() << std::endl; 145 | return out_str; 146 | } 147 | 148 | } // end namespace 149 | 150 | 151 | #endif 152 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/se2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SE2_H 24 | #define SOPHUS_SE2_H 25 | 26 | #include "so2.h" 27 | 28 | namespace Sophus 29 | { 30 | using namespace Eigen; 31 | using namespace std; 32 | 33 | class SE2 34 | { 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | SE2 (); 39 | 40 | SE2 (const SO2 & so2, 41 | const Vector2d & translation); 42 | 43 | SE2 (const Matrix2d & rotation_matrix, 44 | const Vector2d & translation); 45 | 46 | SE2 (double theta, 47 | const Vector2d & translation_); 48 | 49 | SE2 (const SE2 & other); 50 | 51 | SE2 & 52 | operator= (const SE2 & other); 53 | 54 | SE2 55 | operator* (const SE2& other) const; 56 | 57 | SE2& 58 | operator*= (const SE2& other); 59 | 60 | SE2 61 | inverse () const; 62 | 63 | Vector3d 64 | log () const; 65 | 66 | Vector2d 67 | operator* (const Vector2d & xy) const; 68 | 69 | static SE2 70 | exp (const Vector3d & update); 71 | 72 | static Vector3d 73 | log (const SE2 & SE2); 74 | 75 | Matrix 76 | matrix () const; 77 | 78 | Matrix 79 | Adj () const; 80 | 81 | static Matrix3d 82 | hat (const Vector3d & omega); 83 | 84 | static Vector3d 85 | vee (const Matrix3d & Omega); 86 | 87 | static Vector3d 88 | lieBracket (const Vector3d & v1, 89 | const Vector3d & v2); 90 | 91 | static Matrix3d 92 | d_lieBracketab_by_d_a (const Vector3d & b); 93 | 94 | const Vector2d & translation() const 95 | { 96 | return translation_; 97 | } 98 | 99 | Vector2d& translation() 100 | { 101 | return translation_; 102 | } 103 | 104 | Matrix2d rotation_matrix() const 105 | { 106 | return so2_.matrix(); 107 | } 108 | 109 | void setRotationMatrix(const Matrix2d & rotation_matrix) 110 | { 111 | so2_ = SO2(rotation_matrix); 112 | } 113 | 114 | const SO2& so2() const 115 | { 116 | return so2_; 117 | } 118 | 119 | SO2& so2() 120 | { 121 | return so2_; 122 | } 123 | 124 | static const int DoF = 3; 125 | 126 | private: 127 | SO2 so2_; 128 | Vector2d translation_; 129 | 130 | }; 131 | 132 | inline std::ostream& operator <<(std::ostream & out_str, 133 | const SE2 & SE2) 134 | { 135 | out_str << SE2.so2() << SE2.translation() << std::endl; 136 | return out_str; 137 | } 138 | 139 | } // end namespace 140 | 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/se3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SE3_H 24 | #define SOPHUS_SE3_H 25 | 26 | #include "so3.h" 27 | 28 | namespace Sophus 29 | { 30 | using namespace Eigen; 31 | using namespace std; 32 | 33 | typedef Matrix< double, 6, 1 > Vector6d; 34 | typedef Matrix< double, 6, 6 > Matrix6d; 35 | 36 | class SE3 37 | { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | SE3 (); 42 | 43 | SE3 (const SO3 & so3, 44 | const Vector3d & translation); 45 | SE3 (const Matrix3d & rotation_matrix, 46 | const Vector3d & translation); 47 | SE3 (const Quaterniond & unit_quaternion, 48 | const Vector3d & translation_); 49 | SE3 (const SE3 & other); 50 | 51 | SE3 & 52 | operator= (const SE3 & other); 53 | 54 | SE3 55 | operator* (const SE3& other) const; 56 | 57 | SE3& 58 | operator*= (const SE3& other); 59 | 60 | SE3 61 | inverse () const; 62 | 63 | Vector6d 64 | log () const; 65 | 66 | Vector3d 67 | operator* (const Vector3d & xyz) const; 68 | 69 | static SE3 70 | exp (const Vector6d & update); 71 | 72 | static Vector6d 73 | log (const SE3 & se3); 74 | 75 | Matrix 76 | matrix () const; 77 | 78 | Matrix 79 | Adj () const; 80 | 81 | static Matrix4d 82 | hat (const Vector6d & omega); 83 | 84 | static Vector6d 85 | vee (const Matrix4d & Omega); 86 | 87 | static Vector6d 88 | lieBracket (const Vector6d & v1, 89 | const Vector6d & v2); 90 | 91 | static Matrix6d 92 | d_lieBracketab_by_d_a (const Vector6d & b); 93 | 94 | void 95 | setQuaternion (const Quaterniond& quaternion); 96 | 97 | //TODO: remove later 98 | static SE3 from_SE3(const SE3 & se3) 99 | { 100 | return se3; 101 | } 102 | 103 | //TODO: remove later 104 | static SE3 to_SE3(const SE3 & se3) 105 | { 106 | return se3.to_SE3(); 107 | } 108 | 109 | //TODO: remove later 110 | SE3 to_SE3() const 111 | { 112 | return *this; 113 | } 114 | 115 | const Vector3d & translation() const 116 | { 117 | return translation_; 118 | } 119 | 120 | Vector3d& translation() 121 | { 122 | return translation_; 123 | } 124 | 125 | const Quaterniond & unit_quaternion() const 126 | { 127 | return so3_.unit_quaternion(); 128 | } 129 | 130 | Matrix3d rotation_matrix() const 131 | { 132 | return so3_.matrix(); 133 | } 134 | 135 | void setRotationMatrix(const Matrix3d & rotation_matrix) 136 | { 137 | so3_.setQuaternion(Quaterniond(rotation_matrix)); 138 | } 139 | 140 | const SO3& so3() const 141 | { 142 | return so3_; 143 | } 144 | 145 | SO3& so3() 146 | { 147 | return so3_; 148 | } 149 | 150 | static const int DoF = 6; 151 | 152 | private: 153 | SO3 so3_; 154 | Vector3d translation_; 155 | 156 | }; 157 | 158 | inline std::ostream& operator <<(std::ostream & out_str, 159 | const SE3 & se3) 160 | { 161 | out_str << se3.so3() << se3.translation().transpose() << std::endl; 162 | return out_str; 163 | } 164 | 165 | } // end namespace 166 | 167 | 168 | #endif 169 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/sim3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SIMILARITY_H 24 | #define SOPHUS_SIMILARITY_H 25 | 26 | #include "scso3.h" 27 | #include "se3.h" 28 | 29 | namespace Sophus 30 | { 31 | typedef Matrix< double, 7, 1 > Vector7d; 32 | typedef Matrix< double, 7, 7 > Matrix7d; 33 | 34 | /** 35 | * This class implements the Lie group and the corresponding Lie 36 | * algebra of 3D similarity transformationns Sim3 as described in: 37 | * 38 | * > H. Strasdat, J.M.M. Montiel, A.J. Davison: 39 | * "Scale Drift-Aware Large Scale Monocular SLAM", 40 | * Proc. of Robotics: Science and Systems (RSS), 41 | * Zaragoza, Spain, 2010. 42 | * http://www.roboticsproceedings.org/rss06/p10.html 43 | */ 44 | class Sim3 45 | { 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | Sim3 (); 50 | 51 | Sim3 (const ScSO3& scso3, 52 | const Vector3d & t); 53 | Sim3 (double s, 54 | const Matrix3d & R, 55 | const Vector3d & t); 56 | Sim3 (const Quaterniond& q, 57 | const Vector3d & t); 58 | Sim3 (const Sim3 & sim3); 59 | 60 | static Sim3 61 | from_SE3 (const SE3 & se3); 62 | 63 | static SE3 64 | to_SE3 (const Sim3 & sim3); 65 | 66 | SE3 67 | to_SE3 () const; 68 | 69 | Matrix7d 70 | Adj () const; 71 | 72 | static Sim3 73 | exp (const Matrix & vect); 74 | 75 | static Vector7d 76 | log (const Sim3& sim3); 77 | 78 | Sim3 79 | inverse () const; 80 | 81 | Sim3 & 82 | operator*= (const Sim3 & sim3); 83 | 84 | Sim3 85 | operator* (const Sim3 & sim3) const; 86 | 87 | Matrix4d 88 | matrix () const; 89 | 90 | Vector3d 91 | operator* (const Vector3d & xyz) const; 92 | 93 | static Matrix4d 94 | hat (const Vector7d & omega); 95 | 96 | static Vector7d 97 | vee (const Matrix4d & Omega); 98 | 99 | static Vector7d 100 | lieBracket (const Vector7d & omega1, 101 | const Vector7d & omega2); 102 | 103 | static Matrix7d 104 | d_lieBracketab_by_d_a (const Vector7d & b); 105 | 106 | SO3 107 | so3 () const; 108 | 109 | const Vector3d& translation() const 110 | { 111 | return translation_; 112 | } 113 | 114 | Vector3d& translation() 115 | { 116 | return translation_; 117 | } 118 | 119 | const Quaterniond & quaternion() const 120 | { 121 | return scso3_.quaternion(); 122 | } 123 | 124 | Quaterniond& quaternion() 125 | { 126 | return scso3_.quaternion(); 127 | } 128 | 129 | const ScSO3 & scso3() const 130 | { 131 | return scso3_; 132 | } 133 | 134 | ScSO3& scso3() 135 | { 136 | return scso3_; 137 | } 138 | 139 | Matrix3d rotation_matrix() const 140 | { 141 | return scso3_.rotationMatrix(); 142 | } 143 | 144 | 145 | double scale() const 146 | { 147 | return scso3_.scale(); 148 | } 149 | 150 | Matrix log() const 151 | { 152 | return Sim3::log(*this); 153 | } 154 | 155 | static const int DoF = 7; 156 | 157 | protected: 158 | static Matrix3d 159 | calcW (double theta, 160 | double sigma, 161 | double scale, 162 | const Matrix3d & Omega); 163 | 164 | ScSO3 scso3_; 165 | Vector3d translation_; 166 | }; 167 | 168 | 169 | inline std::ostream& operator <<(std::ostream& out_str, 170 | const Sim3 & sim3) 171 | { 172 | out_str << sim3.scso3() << 173 | sim3.translation().transpose() << std::endl; 174 | return out_str; 175 | } 176 | 177 | 178 | } 179 | 180 | 181 | #endif 182 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/so2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include "so2.h" 25 | #include "so3.h" 26 | 27 | namespace Sophus 28 | { 29 | 30 | SO2::SO2() 31 | { 32 | unit_complex_.real() = 1.; 33 | unit_complex_.imag() = 0.; 34 | } 35 | 36 | SO2 37 | ::SO2(const SO2 & other) : unit_complex_(other.unit_complex_) {} 38 | 39 | SO2 40 | ::SO2(const Matrix2d & R) : unit_complex_(0.5*(R(0,0)+R(1,1)), 41 | 0.5*(R(1,0)-R(0,1))) {} 42 | 43 | SO2 44 | ::SO2(const Complexd & complex) : unit_complex_(complex) 45 | { 46 | assert(abs(unit_complex_)!=0); 47 | normalize(); 48 | } 49 | 50 | SO2 51 | ::SO2(double theta) 52 | { 53 | unit_complex_ = SO2::exp(theta).unit_complex_; 54 | } 55 | 56 | void SO2 57 | ::operator=(const SO2 & other) 58 | { 59 | this->unit_complex_ = other.unit_complex_; 60 | } 61 | 62 | SO2 SO2 63 | ::operator*(const SO2& other) const 64 | { 65 | SO2 result(*this); 66 | result.unit_complex_ *= other.unit_complex_; 67 | result.normalize(); 68 | return result; 69 | } 70 | 71 | void SO2 72 | ::operator*=(const SO2& other) 73 | { 74 | unit_complex_ *= other.unit_complex_; 75 | normalize(); 76 | } 77 | 78 | Vector2d SO2 79 | ::operator*(const Vector2d & xy) const 80 | { 81 | const double & real = unit_complex_.real(); 82 | const double & imag = unit_complex_.imag(); 83 | return Vector2d(real*xy[0] - imag*xy[1], imag*xy[0] + real*xy[1]); 84 | } 85 | 86 | SO2 SO2 87 | ::inverse() const 88 | { 89 | return SO2(conj(unit_complex_)); 90 | } 91 | 92 | Matrix2d SO2 93 | ::matrix() const 94 | { 95 | const double & real = unit_complex_.real(); 96 | const double & imag = unit_complex_.imag(); 97 | Matrix2d R; 98 | R(0,0) = real; R(0,1) = -imag; 99 | R(1,0) = imag; R(1,1) = real; 100 | return R; 101 | } 102 | 103 | double SO2 104 | ::Adj() const 105 | { 106 | return 1.; 107 | } 108 | 109 | Matrix2d SO2 110 | ::generator(int i) 111 | { 112 | assert(i==0); 113 | return hat(1.); 114 | } 115 | 116 | double SO2 117 | ::log() const 118 | { 119 | return SO2::log(*this); 120 | } 121 | 122 | double SO2 123 | ::log(const SO2 & other) 124 | { 125 | return atan2(other.unit_complex_.imag(), other.unit_complex().real()); 126 | } 127 | 128 | SO2 SO2 129 | ::exp(double theta) 130 | { 131 | return SO2(Complexd(cos(theta), sin(theta))); 132 | } 133 | 134 | Matrix2d SO2 135 | ::hat(double v) 136 | { 137 | Matrix2d Omega; 138 | Omega << 0, -v 139 | , v, 0; 140 | return Omega; 141 | } 142 | 143 | double SO2 144 | ::vee(const Matrix2d & Omega) 145 | { 146 | assert(fabs(Omega(1,0)+Omega(0,1)) 27 | 28 | #include 29 | 30 | 31 | namespace Sophus 32 | { 33 | using namespace Eigen; 34 | 35 | 36 | class SO2 37 | { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | typedef std::complex Complexd; 42 | 43 | SO2 (); 44 | 45 | SO2 (const SO2 & other); 46 | 47 | explicit 48 | SO2 (const Matrix2d & R); 49 | 50 | explicit 51 | SO2 (const Complexd & unit_complex); 52 | 53 | SO2 (double theta); 54 | void 55 | operator= (const SO2 & SO2); 56 | 57 | SO2 58 | operator* (const SO2 & SO2) const; 59 | 60 | void 61 | operator*= (const SO2 & SO2); 62 | 63 | Vector2d 64 | operator* (const Vector2d & xy) const; 65 | 66 | SO2 67 | inverse () const; 68 | 69 | Matrix2d 70 | matrix () const; 71 | 72 | double 73 | Adj () const; 74 | 75 | Matrix2d 76 | generator (int i); 77 | 78 | double 79 | log () const; 80 | 81 | static SO2 82 | exp (double theta); 83 | 84 | static double 85 | log (const SO2 & SO2); 86 | 87 | static Matrix2d 88 | hat (double omega); 89 | 90 | static double 91 | vee (const Matrix2d & Omega); 92 | 93 | static double 94 | lieBracket (double omega1, 95 | double omega2); 96 | void 97 | setComplex (const Complexd& z); 98 | 99 | const Complexd & unit_complex() const 100 | { 101 | return unit_complex_; 102 | } 103 | 104 | static const int DoF = 2; 105 | 106 | protected: 107 | void 108 | normalize (); 109 | 110 | Complexd unit_complex_; 111 | }; 112 | 113 | inline std::ostream& operator <<(std::ostream & out_str, 114 | const SO2 & SO2) 115 | { 116 | 117 | out_str << SO2.log() << std::endl; 118 | return out_str; 119 | } 120 | 121 | } // end namespace 122 | 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/so3.h: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_SO3_H 24 | #define SOPHUS_SO3_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | namespace Sophus 32 | { 33 | using namespace Eigen; 34 | 35 | const double SMALL_EPS = 1e-10; 36 | 37 | class SO3 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | SO3 (); 43 | 44 | SO3 (const SO3 & other); 45 | 46 | explicit 47 | SO3 (const Matrix3d & _R); 48 | 49 | explicit 50 | SO3 (const Quaterniond & unit_quaternion); 51 | 52 | SO3 (double rot_x, 53 | double rot_y, 54 | double rot_z); 55 | void 56 | operator= (const SO3 & so3); 57 | 58 | SO3 59 | operator* (const SO3 & so3) const; 60 | 61 | void 62 | operator*= (const SO3 & so3); 63 | 64 | Vector3d 65 | operator* (const Vector3d & xyz) const; 66 | 67 | SO3 68 | inverse () const; 69 | 70 | Matrix3d 71 | matrix () const; 72 | 73 | Matrix3d 74 | Adj () const; 75 | 76 | Matrix3d 77 | generator (int i); 78 | 79 | Vector3d 80 | log () const; 81 | 82 | static SO3 83 | exp (const Vector3d & omega); 84 | 85 | static SO3 86 | expAndTheta (const Vector3d & omega, 87 | double * theta); 88 | static Vector3d 89 | log (const SO3 & so3); 90 | 91 | static Vector3d 92 | logAndTheta (const SO3 & so3, 93 | double * theta); 94 | 95 | static Matrix3d 96 | hat (const Vector3d & omega); 97 | 98 | static Vector3d 99 | vee (const Matrix3d & Omega); 100 | 101 | static Vector3d 102 | lieBracket (const Vector3d & omega1, 103 | const Vector3d & omega2); 104 | 105 | static Matrix3d 106 | d_lieBracketab_by_d_a (const Vector3d & b); 107 | 108 | void 109 | setQuaternion (const Quaterniond& quaternion); 110 | 111 | const Quaterniond & unit_quaternion() const 112 | { 113 | return unit_quaternion_; 114 | } 115 | 116 | static const int DoF = 3; 117 | 118 | protected: 119 | Quaterniond unit_quaternion_; 120 | }; 121 | 122 | inline std::ostream& operator <<(std::ostream & out_str, 123 | const SO3 & so3) 124 | { 125 | 126 | out_str << so3.log().transpose() << std::endl; 127 | return out_str; 128 | } 129 | 130 | } // end namespace 131 | 132 | 133 | #endif 134 | -------------------------------------------------------------------------------- /third_party/Sophus/sophus/test_scso3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include "scso3.h" 6 | 7 | using namespace Sophus; 8 | using namespace std; 9 | 10 | 11 | bool scso3explog_tests() 12 | { 13 | double pi = 3.14159265; 14 | vector omegas; 15 | omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0, 1.))); 16 | omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, -1.0, 1.1))); 17 | omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0., 1.1))); 18 | omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.))); 19 | omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.00001))); 20 | omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0))); 21 | omegas.push_back(ScSO3::exp(Vector4d(pi, 0, 0, 0.9))); 22 | omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,0)) 23 | *ScSO3::exp(Vector4d(pi, 0, 0,0.0)) 24 | *ScSO3::exp(Vector4d(-0.2, -0.5, -0.0,0))); 25 | omegas.push_back(ScSO3::exp(Vector4d(0.3, 0.5, 0.1,0)) 26 | *ScSO3::exp(Vector4d(pi, 0, 0,0)) 27 | *ScSO3::exp(Vector4d(-0.3, -0.5, -0.1,0))); 28 | 29 | bool failed = false; 30 | 31 | for (size_t i=0; iSMALL_EPS) 40 | { 41 | cerr << "ScSO3 - exp(log(ScSO3))" << endl; 42 | cerr << "Test case: " << i << endl; 43 | cerr << sR1 << endl; 44 | cerr << omegas[i].log() << endl; 45 | cerr << sR2 << endl; 46 | cerr << DiffR <SMALL_EPS) 63 | { 64 | cerr << "Transform vector" << endl; 65 | cerr << "Test case: " << i << endl; 66 | cerr << (res1-res2) <SMALL_EPS) 83 | { 84 | cerr << "Inverse" << endl; 85 | cerr << "Test case: " << i << endl; 86 | cerr << (res-I) < vecs; 102 | Vector4d tmp; 103 | tmp << 0,0,0,0; 104 | vecs.push_back(tmp); 105 | tmp << 1,0,0,0; 106 | vecs.push_back(tmp); 107 | tmp << 1,0,0,0.1; 108 | vecs.push_back(tmp); 109 | tmp << 0,1,0,0.1; 110 | vecs.push_back(tmp); 111 | tmp << 0,0,1,-0.1; 112 | vecs.push_back(tmp); 113 | tmp << -1,1,0,-0.1; 114 | vecs.push_back(tmp); 115 | tmp << 20,-1,0,2; 116 | vecs.push_back(tmp); 117 | for (size_t i=0; iSMALL_EPS) 121 | { 122 | cerr << "Hat-vee Test" << endl; 123 | cerr << "Test case: " << i << endl; 124 | cerr << resDiff.transpose() << endl; 125 | cerr << endl; 126 | } 127 | 128 | for (size_t j=0; jSMALL_EPS) 137 | { 138 | cerr << "ScSO3 Lie Bracket Test" << endl; 139 | cerr << "Test case: " << i << ", " <SMALL_EPS) 157 | { 158 | cerr << "expmap(hat(x)) - exp(x)" << endl; 159 | cerr << "Test case: " << i << endl; 160 | cerr << exp_x < 2 | #include 3 | 4 | #include 5 | #include "se2.h" 6 | #include "so3.h" 7 | 8 | using namespace Sophus; 9 | using namespace std; 10 | 11 | bool se2explog_tests() 12 | { 13 | double pi = 3.14159265; 14 | vector omegas; 15 | omegas.push_back(SE2(SO2(0.0),Vector2d(0,0))); 16 | omegas.push_back(SE2(SO2(0.2),Vector2d(10,0))); 17 | omegas.push_back(SE2(SO2(0.),Vector2d(0,100))); 18 | omegas.push_back(SE2(SO2(-1.),Vector2d(20,-1))); 19 | omegas.push_back(SE2(SO2(0.00001),Vector2d(-0.00000001,0.0000000001))); 20 | omegas.push_back(SE2(SO2(0.2),Vector2d(0,0)) 21 | *SE2(SO2(pi),Vector2d(0,0)) 22 | *SE2(SO2(-0.2),Vector2d(0,0))); 23 | omegas.push_back(SE2(SO2(0.3),Vector2d(2,0)) 24 | *SE2(SO2(pi),Vector2d(0,0)) 25 | *SE2(SO2(-0.3),Vector2d(0,6))); 26 | 27 | bool failed = false; 28 | 29 | for (size_t i=0; iSMALL_EPS) 37 | { 38 | cerr << "SE2 - exp(log(SE2))" << endl; 39 | cerr << "Test case: " << i << endl; 40 | cerr << DiffR <()*p + T.topRightCorner<2,1>(); 51 | 52 | double nrm = (res1-res2).norm(); 53 | 54 | if (isnan(nrm) || nrm>SMALL_EPS) 55 | { 56 | cerr << "Transform vector" << endl; 57 | cerr << "Test case: " << i << endl; 58 | cerr << (res1-res2) <SMALL_EPS) 75 | { 76 | cerr << "Inverse" << endl; 77 | cerr << "Test case: " << i << endl; 78 | cerr << (res-I) < vecs; 92 | Vector3d tmp; 93 | tmp << 0,0,0; 94 | vecs.push_back(tmp); 95 | tmp << 1,0,0; 96 | vecs.push_back(tmp); 97 | tmp << 0,1,1; 98 | vecs.push_back(tmp); 99 | tmp << -1,1,0; 100 | vecs.push_back(tmp); 101 | tmp << 20,-1,-1; 102 | vecs.push_back(tmp); 103 | tmp << 30,5,20; 104 | vecs.push_back(tmp); 105 | for (size_t i=0; iSMALL_EPS) 109 | { 110 | cerr << "Hat-vee Test" << endl; 111 | cerr << "Test case: " << i << endl; 112 | cerr << resDiff.transpose() << endl; 113 | cerr << endl; 114 | } 115 | 116 | for (size_t j=0; jSMALL_EPS) 125 | { 126 | cerr << "SE2 Lie Bracket Test" << endl; 127 | cerr << "Test case: " << i << ", " <SMALL_EPS) 146 | { 147 | cerr << "expmap(hat(x)) - exp(x)" << endl; 148 | cerr << "Test case: " << i << endl; 149 | cerr << exp_x < 2 | #include 3 | 4 | #include 5 | #include "se3.h" 6 | 7 | using namespace Sophus; 8 | using namespace std; 9 | 10 | bool se3explog_tests() 11 | { 12 | double pi = 3.14159265; 13 | vector omegas; 14 | omegas.push_back(SE3(SO3::exp(Vector3d(0.2, 0.5, 0.0)),Vector3d(0,0,0))); 15 | omegas.push_back(SE3(SO3::exp(Vector3d(0.2, 0.5, -1.0)),Vector3d(10,0,0))); 16 | omegas.push_back(SE3(SO3::exp(Vector3d(0., 0., 0.)),Vector3d(0,100,5))); 17 | omegas.push_back(SE3(SO3::exp(Vector3d(0., 0., 0.00001)),Vector3d(0,0,0))); 18 | omegas.push_back(SE3(SO3::exp(Vector3d(0., 0., 0.00001)),Vector3d(0,-0.00000001,0.0000000001))); 19 | omegas.push_back(SE3(SO3::exp(Vector3d(0., 0., 0.00001)),Vector3d(0.01,0,0))); 20 | omegas.push_back(SE3(SO3::exp(Vector3d(pi, 0, 0)),Vector3d(4,-5,0))); 21 | omegas.push_back(SE3(SO3::exp(Vector3d(0.2, 0.5, 0.0)),Vector3d(0,0,0)) 22 | *SE3(SO3::exp(Vector3d(pi, 0, 0)),Vector3d(0,0,0)) 23 | *SE3(SO3::exp(Vector3d(-0.2, -0.5, -0.0)),Vector3d(0,0,0))); 24 | omegas.push_back(SE3(SO3::exp(Vector3d(0.3, 0.5, 0.1)),Vector3d(2,0,-7)) 25 | *SE3(SO3::exp(Vector3d(pi, 0, 0)),Vector3d(0,0,0)) 26 | *SE3(SO3::exp(Vector3d(-0.3, -0.5, -0.1)),Vector3d(0,6,0))); 27 | 28 | bool failed = false; 29 | 30 | for (size_t i=0; iSMALL_EPS) 38 | { 39 | cerr << "SE3 - exp(log(SE3))" << endl; 40 | cerr << "Test case: " << i << endl; 41 | cerr << DiffR <()*p + T.topRightCorner<3,1>(); 52 | 53 | double nrm = (res1-res2).norm(); 54 | 55 | if (isnan(nrm) || nrm>SMALL_EPS) 56 | { 57 | cerr << "Transform vector" << endl; 58 | cerr << "Test case: " << i << endl; 59 | cerr << (res1-res2) <SMALL_EPS) 76 | { 77 | cerr << "Inverse" << endl; 78 | cerr << "Test case: " << i << endl; 79 | cerr << (res-I) < vecs; 92 | Vector6d tmp; 93 | tmp << 0,0,0,0,0,0; 94 | vecs.push_back(tmp); 95 | tmp << 1,0,0,0,0,0; 96 | vecs.push_back(tmp); 97 | tmp << 0,1,0,1,0,0; 98 | vecs.push_back(tmp); 99 | tmp << 0,-5,10,0,0,0; 100 | vecs.push_back(tmp); 101 | tmp << -1,1,0,0,0,1; 102 | vecs.push_back(tmp); 103 | tmp << 20,-1,0,-1,1,0; 104 | vecs.push_back(tmp); 105 | tmp << 30,5,-1,20,-1,0; 106 | vecs.push_back(tmp); 107 | for (size_t i=0; iSMALL_EPS) 111 | { 112 | cerr << "Hat-vee Test" << endl; 113 | cerr << "Test case: " << i << endl; 114 | cerr << resDiff.transpose() << endl; 115 | cerr << endl; 116 | failed = true; 117 | } 118 | 119 | for (size_t j=0; jSMALL_EPS) 128 | { 129 | cerr << "SE3 Lie Bracket Test" << endl; 130 | cerr << "Test case: " << i << ", " <SMALL_EPS) 147 | { 148 | cerr << "expmap(hat(x)) - exp(x)" << endl; 149 | cerr << "Test case: " << i << endl; 150 | cerr << exp_x < 2 | #include 3 | 4 | #include 5 | 6 | #include "sim3.h" 7 | 8 | using namespace Sophus; 9 | using namespace std; 10 | 11 | bool sim3explog_tests() 12 | { 13 | double pi = 3.14159265; 14 | vector omegas; 15 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,1.)),Vector3d(0,0,0))); 16 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0.2, 0.5, -1.0,1.1)),Vector3d(10,0,0))); 17 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0., 0., 0.,1.1)),Vector3d(0,100,5))); 18 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.)),Vector3d(0,0,0))); 19 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.0000001)),Vector3d(1,-1.00000001,2.0000000001))); 20 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0., 0., 0.00001, 0)),Vector3d(0.01,0,0))); 21 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(pi, 0, 0,0.9)),Vector3d(4,-5,0))); 22 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,0)),Vector3d(0,0,0)) 23 | *Sim3(ScSO3::exp(Vector4d(pi, 0, 0,0)),Vector3d(0,0,0)) 24 | *Sim3(ScSO3::exp(Vector4d(-0.2, -0.5, -0.0,0)),Vector3d(0,0,0))); 25 | omegas.push_back(Sim3(ScSO3::exp(Vector4d(0.3, 0.5, 0.1,0)),Vector3d(2,0,-7)) 26 | *Sim3(ScSO3::exp(Vector4d(pi, 0, 0,0)),Vector3d(0,0,0)) 27 | *Sim3(ScSO3::exp(Vector4d(-0.3, -0.5, -0.1,0)),Vector3d(0,6,0))); 28 | 29 | bool failed = false; 30 | 31 | for (size_t i=0; iSMALL_EPS) 40 | { 41 | cerr << "Sim3 - exp(log(Sim3))" << endl; 42 | cerr << "Test case: " << i << endl; 43 | cerr << DiffR <()*p + T.topRightCorner<3,1>(); 54 | 55 | double nrm = (res1-res2).norm(); 56 | 57 | if (isnan(nrm) || nrm>SMALL_EPS) 58 | { 59 | cerr << "Transform vector" << endl; 60 | cerr << "Test case: " << i << endl; 61 | cerr << (res1-res2) <SMALL_EPS) 78 | { 79 | cerr << "Inverse" << endl; 80 | cerr << "Test case: " << i << endl; 81 | cerr << (res-I) < vecs; 94 | Vector7d tmp; 95 | tmp << 0,0,0,0,0,0,0; 96 | vecs.push_back(tmp); 97 | tmp << 1,0,0,0,0,0,0; 98 | vecs.push_back(tmp); 99 | tmp << 0,1,0,1,0,0,0.1; 100 | vecs.push_back(tmp); 101 | tmp << 0,0,1,0,1,0,0.1; 102 | vecs.push_back(tmp); 103 | tmp << -1,1,0,0,0,1,-0.1; 104 | vecs.push_back(tmp); 105 | tmp << 20,-1,0,-1,1,0,-0.1; 106 | vecs.push_back(tmp); 107 | tmp << 30,5,-1,20,-1,0,2; 108 | vecs.push_back(tmp); 109 | for (size_t i=0; iSMALL_EPS) 113 | { 114 | cerr << "Hat-vee Test" << endl; 115 | cerr << "Test case: " << i << endl; 116 | cerr << resDiff.transpose() << endl; 117 | cerr << endl; 118 | failed = true; 119 | } 120 | 121 | for (size_t j=0; jSMALL_EPS) 130 | { 131 | cerr << "Sim3 Lie Bracket Test" << endl; 132 | cerr << "Test case: " << i << ", " <SMALL_EPS) 150 | { 151 | cerr << "expmap(hat(x)) - exp(x)" << endl; 152 | cerr << "Test case: " << i << endl; 153 | cerr << exp_x < 2 | #include 3 | 4 | #include 5 | 6 | #include "so2.h" 7 | #include "so3.h" 8 | 9 | using namespace Sophus; 10 | using namespace std; 11 | 12 | bool so2explog_tests() 13 | { 14 | double pi = 3.14159265; 15 | vector so2; 16 | so2.push_back(SO2::exp(0.0)); 17 | so2.push_back(SO2::exp(0.2)); 18 | so2.push_back(SO2::exp(10.)); 19 | so2.push_back(SO2::exp(0.00001)); 20 | so2.push_back(SO2::exp(pi)); 21 | so2.push_back(SO2::exp(0.2) 22 | *SO2::exp(pi) 23 | *SO2::exp(-0.2)); 24 | so2.push_back(SO2::exp(-0.3) 25 | *SO2::exp(pi) 26 | *SO2::exp(0.3)); 27 | 28 | 29 | bool failed = false; 30 | 31 | for (size_t i=0; iSMALL_EPS) 40 | { 41 | cerr << "SO3 - exp(log(SO3))" << endl; 42 | cerr << "Test case: " << i << endl; 43 | cerr << DiffR <SMALL_EPS) 59 | { 60 | cerr << "Transform vector" << endl; 61 | cerr << "Test case: " << i << endl; 62 | cerr << (res1-res2) <SMALL_EPS) 79 | { 80 | cerr << "Inverse" << endl; 81 | cerr << "Test case: " << i << endl; 82 | cerr << (res-I) <SMALL_EPS) 97 | { 98 | cerr << "expmap(hat(x)) - exp(x)" << endl; 99 | cerr << "Test case: " << i << endl; 100 | // cerr << exp_x < 2 | #include 3 | 4 | 5 | #include 6 | 7 | #include "so3.h" 8 | 9 | using namespace Sophus; 10 | using namespace std; 11 | 12 | 13 | bool so3explog_tests() 14 | { 15 | 16 | vector omegas; 17 | omegas.push_back(SO3(Quaterniond(0.1e-11, 0., 1., 0.))); 18 | omegas.push_back(SO3(Quaterniond(-1,0.00001,0.0,0.0))); 19 | omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0))); 20 | omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, -1.0))); 21 | omegas.push_back(SO3::exp(Vector3d(0., 0., 0.))); 22 | omegas.push_back(SO3::exp(Vector3d(0., 0., 0.00001))); 23 | omegas.push_back(SO3::exp(Vector3d(M_PI, 0, 0))); 24 | omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0)) 25 | *SO3::exp(Vector3d(M_PI, 0, 0)) 26 | *SO3::exp(Vector3d(-0.2, -0.5, -0.0))); 27 | omegas.push_back(SO3::exp(Vector3d(0.3, 0.5, 0.1)) 28 | *SO3::exp(Vector3d(M_PI, 0, 0)) 29 | *SO3::exp(Vector3d(-0.3, -0.5, -0.1))); 30 | 31 | bool failed = false; 32 | 33 | for (size_t i=0; iSMALL_EPS) 43 | { 44 | cerr << "SO3 - exp(log(SO3))" << endl; 45 | cerr << "Test case: " << i << endl; 46 | cerr << DiffR <M_PI || theta<-M_PI) 52 | { 53 | cerr << "log theta not in [-pi,pi]" << endl; 54 | cerr << "Test case: " << i << endl; 55 | cerr << theta <SMALL_EPS) 72 | { 73 | cerr << "Transform vector" << endl; 74 | cerr << "Test case: " << i << endl; 75 | cerr << (res1-res2) <SMALL_EPS) 92 | { 93 | cerr << "Inverse" << endl; 94 | cerr << "Test case: " << i << endl; 95 | cerr << (res-I) < vecs; 108 | vecs.push_back(Vector3d(0,0,0)); 109 | vecs.push_back(Vector3d(1,0,0)); 110 | vecs.push_back(Vector3d(0,1,0)); 111 | vecs.push_back(Vector3d(M_PI_2,M_PI_2,0.0)); 112 | vecs.push_back(Vector3d(-1,1,0)); 113 | vecs.push_back(Vector3d(20,-1,0)); 114 | vecs.push_back(Vector3d(30,5,-1)); 115 | for (uint i=0; iSMALL_EPS) 126 | { 127 | cerr << "SO3 Lie Bracket Test" << endl; 128 | cerr << "Test case: " << i << ", " <SMALL_EPS) 142 | { 143 | cerr << "expmap(hat(x)) - exp(x)" << endl; 144 | cerr << "Test case: " << i << endl; 145 | cerr << exp_x <