├── .vscode └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── brief_k10L6.bin ├── brief_pattern.yml ├── params_camera.yaml ├── params_camera1.yaml ├── params_camera2.yaml └── params_lidar.yaml ├── doc ├── demo.gif └── device.png ├── include └── ikd-Tree │ ├── README.md │ ├── ikd_Tree.cpp │ └── ikd_Tree.h ├── launch ├── include │ ├── config │ │ ├── newRviz.rviz │ │ ├── robot.urdf.xacro │ │ └── rviz.rviz │ ├── module_robot_state_publisher.launch │ ├── module_rviz.launch │ ├── module_sam.launch │ ├── module_sam_bak.launch │ ├── module_sam_hesai.launch │ └── rosconsole │ │ ├── rosconsole_error.conf │ │ ├── rosconsole_info.conf │ │ └── rosconsole_warn.conf └── run.launch ├── lvi_params_for_zju_data ├── msg └── cloud_info.msg ├── package.xml └── src ├── lidar_odometry ├── featureExtraction.cpp ├── imageProjection.cpp ├── imuPreintegration.cpp ├── imu_tracker.cc ├── imu_tracker.h ├── mapOptmization.cpp └── utility.h └── visual_odometry ├── visual_estimator ├── estimator.cpp ├── estimator.h ├── estimator_node.cpp ├── factor │ ├── imu_factor.h │ ├── integration_base.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── pose_local_parameterization.cpp │ ├── pose_local_parameterization.h │ ├── projection_factor.cpp │ ├── projection_factor.h │ ├── projection_td_factor.cpp │ └── projection_td_factor.h ├── feature_manager.cpp ├── feature_manager.h ├── initial │ ├── initial_aligment.cpp │ ├── initial_alignment.h │ ├── initial_ex_rotation.cpp │ ├── initial_ex_rotation.h │ ├── initial_sfm.cpp │ ├── initial_sfm.h │ ├── solve_5pts.cpp │ └── solve_5pts.h ├── parameters.cpp ├── parameters.h └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.cpp │ ├── utility.h │ ├── visualization.cpp │ └── visualization.h ├── visual_feature ├── camera_models │ ├── Camera.cc │ ├── Camera.h │ ├── CameraFactory.cc │ ├── CameraFactory.h │ ├── CataCamera.cc │ ├── CataCamera.h │ ├── CostFunctionFactory.cc │ ├── CostFunctionFactory.h │ ├── EquidistantCamera.cc │ ├── EquidistantCamera.h │ ├── PinholeCamera.cc │ ├── PinholeCamera.h │ ├── ScaramuzzaCamera.cc │ ├── ScaramuzzaCamera.h │ ├── gpl.cc │ └── gpl.h ├── feature_tracker.cpp ├── feature_tracker.h ├── feature_tracker_node.cpp ├── parameters.cpp ├── parameters.h └── tic_toc.h └── visual_loop ├── ThirdParty ├── DBoW │ ├── BowVector.cpp │ ├── BowVector.h │ ├── DBoW2.h │ ├── FBrief.cpp │ ├── FBrief.h │ ├── FClass.h │ ├── FeatureVector.cpp │ ├── FeatureVector.h │ ├── QueryResults.cpp │ ├── QueryResults.h │ ├── ScoringObject.cpp │ ├── ScoringObject.h │ ├── TemplatedDatabase.h │ └── TemplatedVocabulary.h ├── DUtils │ ├── DException.h │ ├── DUtils.h │ ├── Random.cpp │ ├── Random.h │ ├── Timestamp.cpp │ └── Timestamp.h ├── DVision │ ├── BRIEF.cpp │ ├── BRIEF.h │ └── DVision.h ├── VocabularyBinary.cpp └── VocabularyBinary.hpp ├── keyframe.cpp ├── keyframe.h ├── loop_detection.cpp ├── loop_detection.h ├── loop_detection_node.cpp └── parameters.h /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "array": "cpp", 4 | "deque": "cpp", 5 | "list": "cpp", 6 | "vector": "cpp", 7 | "string_view": "cpp" 8 | } 9 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(emv_lio) 3 | 4 | ###################### 5 | ### Cmake flags 6 | ###################### 7 | set(CMAKE_BUILD_TYPE "Release") 8 | set(CMAKE_CXX_FLAGS "-std=c++14") 9 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread -w") 10 | 11 | ###################### 12 | ### Packages 13 | ###################### 14 | find_package(catkin REQUIRED COMPONENTS 15 | tf 16 | roscpp 17 | rospy 18 | roslib 19 | # msg 20 | std_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | nav_msgs 24 | # cv 25 | cv_bridge 26 | # pcl 27 | pcl_conversions 28 | # msg generation 29 | message_generation 30 | ) 31 | 32 | find_package(OpenMP REQUIRED) 33 | find_package(PCL REQUIRED) 34 | find_package(OpenCV REQUIRED) 35 | find_package(Ceres REQUIRED) 36 | find_package(GTSAM REQUIRED QUIET) 37 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 38 | 39 | 40 | ###################### 41 | ### Message generation 42 | ###################### 43 | add_message_files( 44 | DIRECTORY msg 45 | FILES 46 | cloud_info.msg 47 | ) 48 | 49 | generate_messages( 50 | DEPENDENCIES 51 | geometry_msgs 52 | std_msgs 53 | nav_msgs 54 | sensor_msgs 55 | ) 56 | 57 | ###################### 58 | ### Catkin 59 | ###################### 60 | catkin_package( 61 | INCLUDE_DIRS include 62 | DEPENDS PCL GTSAM 63 | ) 64 | 65 | include_directories( 66 | include 67 | ${catkin_INCLUDE_DIRS} 68 | ${PCL_INCLUDE_DIRS} 69 | ${OpenCV_INCLUDE_DIRS} 70 | ${CERES_INCLUDE_DIRS} 71 | ${Boost_INCLUDE_DIRS} 72 | ${GTSAM_INCLUDE_DIR} 73 | ) 74 | 75 | link_directories( 76 | include 77 | ${PCL_LIBRARY_DIRS} 78 | ${OpenCV_LIBRARY_DIRS} 79 | ${GTSAM_LIBRARY_DIRS} 80 | ) 81 | 82 | ###################### 83 | ### visual odometry 84 | ###################### 85 | file(GLOB visual_feature_files 86 | "src/visual_odometry/visual_feature/*.cpp" 87 | "src/visual_odometry/visual_feature/camera_models/*.cc" 88 | ) 89 | file(GLOB visual_odometry_files 90 | "src/visual_odometry/visual_estimator/*.cpp" 91 | "src/visual_odometry/visual_estimator/factor/*.cpp" 92 | "src/visual_odometry/visual_estimator/initial/*.cpp" 93 | "src/visual_odometry/visual_estimator/utility/*.cpp" 94 | ) 95 | file(GLOB visual_loop_files 96 | "src/visual_odometry/visual_loop/*.cpp" 97 | "src/visual_odometry/visual_loop/utility/*.cpp" 98 | "src/visual_odometry/visual_loop/ThirdParty/*.cpp" 99 | "src/visual_odometry/visual_loop/ThirdParty/DBoW/*.cpp" 100 | "src/visual_odometry/visual_loop/ThirdParty/DUtils/*.cpp" 101 | "src/visual_odometry/visual_loop/ThirdParty/DVision/*.cpp" 102 | "src/visual_odometry/visual_feature/camera_models/*.cc" 103 | ) 104 | # Visual Feature Tracker 105 | add_executable(${PROJECT_NAME}_visual_feature ${visual_feature_files}) 106 | target_link_libraries(${PROJECT_NAME}_visual_feature ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 107 | # Visual Odometry 108 | add_executable(${PROJECT_NAME}_visual_odometry ${visual_odometry_files}) 109 | target_link_libraries(${PROJECT_NAME}_visual_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 110 | # Visual Lopp 111 | add_executable(${PROJECT_NAME}_visual_loop ${visual_loop_files}) 112 | target_link_libraries(${PROJECT_NAME}_visual_loop ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 113 | 114 | ###################### 115 | ### lidar odometry 116 | ###################### 117 | 118 | # IMU Preintegration 119 | add_executable(${PROJECT_NAME}_imuPreintegration src/lidar_odometry/imuPreintegration.cpp src/lidar_odometry/imu_tracker.cc) 120 | target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) 121 | # Range Image Projection 122 | add_executable(${PROJECT_NAME}_imageProjection src/lidar_odometry/imageProjection.cpp src/lidar_odometry/imu_tracker.cc) 123 | add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 124 | target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 125 | # Feature Association 126 | add_executable(${PROJECT_NAME}_featureExtraction src/lidar_odometry/featureExtraction.cpp) 127 | add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 128 | target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 129 | # Mapping Optimization 130 | add_executable(${PROJECT_NAME}_mapOptmization src/lidar_odometry/mapOptmization.cpp include/ikd-Tree/ikd_Tree.cpp) 131 | add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 132 | target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS}) 133 | target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam) 134 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Bingqi Shen 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 are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EMV-LIO 2 | ## An Efficient Multiple Vision aided LiDAR-Inertial Odometry 3 | 4 | **EMV-LIO** is an Efficient Multiple vision aided LiDAR-inertial odometry system based on **LVI-SAM**, which introduces multiple cameras in the VIO subsystem to expand the range of visual observation to guarantee the whole system can still maintain the relatively high accuracy in case of the failure of the monocular visual observation. Apart from this, an efficiency-enhanced LVIO system is also introduced to increase the system’s efficiency, including removing LiDAR’s noise via range image, setting condition for nearest neighbor search, and replacing kd-Tree with ikd-Tree. 5 | 6 | Our implementation will be available upon acceptance 7 | 8 |

9 | drawing 10 |

11 | 12 | --- 13 | 14 | ## 1. Prerequisites 15 | 16 | ### 1.1 **Ubuntu** and **ROS** 17 | Ubuntu 64-bit 16.04 or 18.04. 18 | ROS Kinetic or Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) and its additional ROS pacakge: 19 | 20 | ### 1.2. **Ceres Solver** 21 | Follow [Ceres Installation](http://ceres-solver.org/installation.html). 22 | 23 | ``` 24 | sudo apt-get install -y libgoogle-glog-dev 25 | sudo apt-get install -y libatlas-base-dev 26 | wget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip 27 | cd ~/Downloads/ && unzip ceres.zip -d ~/Downloads/ 28 | cd ~/Downloads/ceres-solver-1.14.0 29 | mkdir ceres-bin && cd ceres-bin 30 | cmake .. 31 | sudo make install -j4 32 | ``` 33 | 34 | ### 1.3. **GTSAM** 35 | 36 | Install the dependencies 37 | ``` 38 | sudo apt-get install libboost-all-dev 39 | sudo apt-get install cmake 40 | sudo apt-get install libtbb-dev 41 | ``` 42 | Compile the GTSAM's code 43 | ``` 44 | git clone https://bitbucket.org/gtborg/gtsam.git 45 | cd gtsam/ 46 | mkdir build &&cd build 47 | cmake .. 48 | make check 49 | sudo make install 50 | ``` 51 | 52 | --- 53 | 54 | ## 2. Compile 55 | You can use the following commands to download and compile the package. 56 | 57 | ``` 58 | cd ~/emv_ws/src 59 | git clone https://github.com/BingqiShen/EMV-LIO.git 60 | cd .. 61 | catkin_make -j4 62 | ``` 63 | 64 | --- 65 | 66 | ## 3. Run our examples 67 | 68 | 69 | ### 3.1 Download our rosbag files 70 | 71 |

72 | drawing 73 |

74 | 75 | The datasets used in the paper can be downloaded from Baidu Drive. The data-gathering sensor suite includes: HESAI PandarXT-32 LiDAR, DAHENG MER2-202 camera, and Xsens MTi-300 IMU. 76 | 77 | ``` 78 | url：https://pan.baidu.com/s/1QjQzn1ZwN1SHqHPghYN2tw 79 | 80 | code：sreu 81 | ``` 82 | 83 | ### 3.2 Run the package 84 | 85 | 1. Configure parameters: 86 | 87 | ``` 88 | Configure sensor parameters in the .yaml files in the ```config``` folder. 89 | 90 | You can select the number of camera used by change NUM_OF_CAM in params_lidar.yaml 91 | ``` 92 | 93 | 2. Run the launch file: 94 | 95 | ``` 96 | roslaunch emv_lio run.launch 97 | ``` 98 | 99 | 3. Play existing bag files: 100 | ``` 101 | rosbag play iplus.bag 102 | ``` 103 | 104 | --- 105 | 106 | ## 4. Acknowledgement 107 | Our repository mainly develops from [LVI-SAM](https://github.com/TixiaoShan/LVI-SAM), where the visual-inertial odometry module is adapted from [Vins-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) and the lidar-inertial odometry module is adapted from [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM/tree/a246c960e3fca52b989abf888c8cf1fae25b7c25). Besides, our implementation also use the codes of [ikd-Tree](https://github.com/hku-mars/ikd-Tree), [M-LOAM](https://github.com/gogojjh/M-LOAM), and [Cartographer](https://github.com/cartographer-project/cartographer). 108 | -------------------------------------------------------------------------------- /config/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BingqiShen/EMV-LIO/efc31dc812ad2d3cfcf3f6227ca921474e76f54f/config/brief_k10L6.bin -------------------------------------------------------------------------------- /config/params_camera.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Project 4 | project_name: "emv_lio" 5 | 6 | #common parameters 7 | imu_topic: "/imu_data" 8 | image_topic: "/camera2/camera2_resize" 9 | point_cloud_topic: "/hesai/pandar" 10 | 11 | # Lidar Params 12 | use_lidar: 1 # whether use depth info from lidar or not 13 | lidar_skip: 3 # skip this amount of scans 14 | align_camera_lidar_estimation: 1 # align camera and lidar estimation for visualization 15 | 16 | # cam's number 17 | # num_of_cam: 1 18 | 19 | # camera to lidar extrinsic 20 | cam_to_lidar_tx: -0.1338 21 | cam_to_lidar_ty: 0.0959 22 | cam_to_lidar_tz: -0.1029 23 | cam_to_lidar_rx: -1.5837 24 | cam_to_lidar_ry: 0.0104 25 | cam_to_lidar_rz: 1.0541 26 | # camera model 27 | model_type: PINHOLE 28 | camera_name: camera 29 | 30 | # Mono camera config 31 | image_width: 1224 32 | image_height: 1024 33 | mirror_parameters: 34 | xi: 1.9926618269451453 35 | distortion_parameters: 36 | k1: -0.0383 37 | k2: 0.2589 38 | p1: 0.0 39 | p2: 0.0 40 | projection_parameters: 41 | gamma1: 1222.0 42 | gamma2: 1221.6 43 | u0: 628.5 44 | v0: 484.7 45 | fx: 1222.0 46 | fy: 1221.6 47 | cx: 628.5 48 | cy: 484.7 49 | fisheye_mask: "/config/fisheye_mask_720x540.jpg" 50 | 51 | #imu parameters The more accurate parameters you provide, the worse performance 52 | imu_freq: 400 53 | acc_n: 0.03015059 # accelerometer measurement noise standard deviation. 54 | gyr_n: 0.42306179 # gyroscope measurement noise standard deviation. 55 | acc_w: 0.00198931 # accelerometer bias random work noise standard deviation. 56 | gyr_w: 0.00015932 # gyroscope bias random work noise standard deviation. 57 | g_norm: 9.79 # 58 | 59 | # Extrinsic parameter between IMU and Camera. 60 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 61 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 62 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 63 | #Rotation from camera frame to imu frame, imu^R_cam 64 | extrinsicRotation: !!opencv-matrix 65 | rows: 3 66 | cols: 3 67 | dt: d 68 | data: [ -0.2846, -0.0102, 0.9586, 69 | -0.9586, -0.0010, -0.2846, 70 | 0.0038, -0.9999, -0.0095] 71 | 72 | #Translation from camera frame to imu frame, imu^T_cam 73 | extrinsicTranslation: !!opencv-matrix 74 | rows: 3 75 | cols: 1 76 | dt: d 77 | data: [0.1316, -0.0471, -0.0379] 78 | 79 | #feature tracker paprameters 80 | max_cnt: 100 # max feature number in feature tracking 81 | min_dist: 20 # min distance between two features 82 | freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 83 | F_threshold: 1.0 # ransac threshold (pixel) 84 | show_track: 1 # publish tracking image as topic 85 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 86 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 87 | 88 | #optimization parameters 89 | max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time 90 | max_num_iterations: 10 # max solver itrations, to guarantee real time 91 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 92 | 93 | #unsynchronization parameters 94 | estimate_td: 0 # online estimate time offset between camera and imu 95 | td: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 96 | 97 | #rolling shutter parameters 98 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 99 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 100 | 101 | #loop closure parameters 102 | loop_closure: 0 # start loop closure 103 | skip_time: 0.0 104 | skip_dist: 0.0 105 | debug_image: 0 # save raw image in loop detector for visualization prupose; you can close this function by setting 0 106 | match_image_scale: 0.5 107 | vocabulary_file: "/config/brief_k10L6.bin" 108 | brief_pattern_file: "/config/brief_pattern.yml" 109 | -------------------------------------------------------------------------------- /config/params_camera1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Project 4 | project_name: "emv_lio" 5 | 6 | #common parameters 7 | imu_topic: "/imu_data" 8 | image_topic: "/camera3/camera3_resize" 9 | point_cloud_topic: "/hesai/pandar" 10 | 11 | # Lidar Params 12 | use_lidar: 1 # whether use depth info from lidar or not 13 | lidar_skip: 3 # skip this amount of scans 14 | align_camera_lidar_estimation: 1 # align camera and lidar estimation for visualization 15 | 16 | # cam's number 17 | #num_of_cam: 1 18 | 19 | # camera to lidar extrinsic 20 | cam_to_lidar_tx: 0.1285 21 | cam_to_lidar_ty: 0.1073 22 | cam_to_lidar_tz: -0.0957 23 | cam_to_lidar_rx: -1.5883 24 | cam_to_lidar_ry: -0.0093 25 | cam_to_lidar_rz: -1.0618 26 | # camera model 27 | model_type: PINHOLE 28 | camera_name: camera 29 | 30 | # Mono camera config 31 | image_width: 1224 32 | image_height: 1024 33 | mirror_parameters: 34 | xi: 1.9926618269451453 35 | distortion_parameters: 36 | k1: -0.0250 37 | k2: 0.1441 38 | p1: 0.0 39 | p2: 0.0 40 | projection_parameters: 41 | fx: 1202.0 42 | fy: 1204.0 43 | cx: 615.2 44 | cy: 480.4 45 | fisheye_mask: "/config/fisheye_mask_720x540.jpg" 46 | 47 | #imu parameters The more accurate parameters you provide, the worse performance 48 | acc_n: 0.03015059 # accelerometer measurement noise standard deviation. 49 | gyr_n: 0.42306179 # gyroscope measurement noise standard deviation. 50 | acc_w: 0.00198931 # accelerometer bias random work noise standard deviation. 51 | gyr_w: 0.00015932 # gyroscope bias random work noise standard deviation. 52 | g_norm: 9.80 # 53 | 54 | # Extrinsic parameter between IMU and Camera. 55 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 56 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 57 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 58 | #Rotation from camera frame to imu frame, imu^R_cam 59 | extrinsicRotation: !!opencv-matrix 60 | rows: 3 61 | cols: 3 62 | dt: d 63 | data: [ -0.6727, 0.0004, -0.7399, 64 | 0.7399, 0.0070, -0.6727, 65 | 0.0049, -1.0000, -0.0051] 66 | 67 | #Translation from camera frame to imu frame, imu^T_cam 68 | extrinsicTranslation: !!opencv-matrix 69 | rows: 3 70 | cols: 1 71 | dt: d 72 | data: [-0.1201, -0.0628, -0.0320] 73 | 74 | #feature tracker paprameters 75 | max_cnt: 100 # max feature number in feature tracking 76 | min_dist: 20 # min distance between two features 77 | freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 78 | F_threshold: 1.0 # ransac threshold (pixel) 79 | show_track: 1 # publish tracking image as topic 80 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 81 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 82 | 83 | #optimization parameters 84 | max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time 85 | max_num_iterations: 10 # max solver itrations, to guarantee real time 86 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 87 | 88 | #unsynchronization parameters 89 | estimate_td: 0 # online estimate time offset between camera and imu 90 | td: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 91 | 92 | #rolling shutter parameters 93 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 94 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 95 | 96 | #loop closure parameters 97 | loop_closure: 1 # start loop closure 98 | skip_time: 0.0 99 | skip_dist: 0.0 100 | debug_image: 0 # save raw image in loop detector for visualization prupose; you can close this function by setting 0 101 | match_image_scale: 0.5 102 | vocabulary_file: "/config/brief_k10L6.bin" 103 | brief_pattern_file: "/config/brief_pattern.yml" 104 | -------------------------------------------------------------------------------- /config/params_camera2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Project 4 | project_name: "emv_lio" 5 | 6 | #common parameters 7 | imu_topic: "/imu_data" 8 | image_topic: "/camera1/camera1_resize" 9 | point_cloud_topic: "/hesai/pandar" 10 | 11 | # Lidar Params 12 | use_lidar: 1 # whether use depth info from lidar or not 13 | lidar_skip: 3 # skip this amount of scans 14 | align_camera_lidar_estimation: 1 # align camera and lidar estimation for visualization 15 | 16 | 17 | # camera to lidar extrinsic 18 | cam_to_lidar_tx: -0.0102 19 | cam_to_lidar_ty: -0.1885 20 | cam_to_lidar_tz: -0.1003 21 | cam_to_lidar_rx: -1.5399 22 | cam_to_lidar_ry: 0.0052 23 | cam_to_lidar_rz: 3.1229 24 | # camera model 25 | model_type: PINHOLE 26 | camera_name: camera 27 | 28 | # Mono camera config 29 | image_width: 1224 30 | image_height: 1024 31 | mirror_parameters: 32 | xi: 1.9926618269451453 33 | distortion_parameters: 34 | k1: -0.0223 35 | k2: 0.1693 36 | p1: 0.0 37 | p2: 0.0 38 | projection_parameters: 39 | fx: 1207.2 40 | fy: 1206.9 41 | cx: 610.0 42 | cy: 491.0 43 | fisheye_mask: "/config/fisheye_mask_720x540.jpg" 44 | 45 | #imu parameters The more accurate parameters you provide, the worse performance 46 | acc_n: 0.03015059 # accelerometer measurement noise standard deviation. 47 | gyr_n: 0.42306179 # gyroscope measurement noise standard deviation. 48 | acc_w: 0.00198931 # accelerometer bias random work noise standard deviation. 49 | gyr_w: 0.00015932 # gyroscope bias random work noise standard deviation. 50 | g_norm: 9.80 # 51 | 52 | # Extrinsic parameter between IMU and Camera. 53 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 54 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 55 | # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. 56 | #Rotation from camera frame to imu frame, imu^R_cam 57 | extrinsicRotation: !!opencv-matrix 58 | rows: 3 59 | cols: 3 60 | dt: d 61 | data: [ 0.9781, -0.0123, -0.2076, 62 | 0.2077, 0.0144, 0.9781, 63 | -0.0090, -0.9998, 0.0167] 64 | 65 | #Translation from camera frame to imu frame, imu^T_cam 66 | extrinsicTranslation: !!opencv-matrix 67 | rows: 3 68 | cols: 1 69 | dt: d 70 | data: [-0.0332, 0.1919, -0.0388] 71 | 72 | #feature tracker paprameters 73 | max_cnt: 150 # max feature number in feature tracking 74 | min_dist: 20 # min distance between two features 75 | freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 76 | F_threshold: 1.0 # ransac threshold (pixel) 77 | show_track: 1 # publish tracking image as topic 78 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 79 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 80 | 81 | #optimization parameters 82 | max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time 83 | max_num_iterations: 10 # max solver itrations, to guarantee real time 84 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 85 | 86 | #unsynchronization parameters 87 | estimate_td: 0 # online estimate time offset between camera and imu 88 | td: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 89 | 90 | #rolling shutter parameters 91 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 92 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 93 | 94 | #loop closure parameters 95 | loop_closure: 0 # start loop closure 96 | skip_time: 0.0 97 | skip_dist: 0.0 98 | debug_image: 0 # save raw image in loop detector for visualization prupose; you can close this function by setting 0 99 | match_image_scale: 0.5 100 | vocabulary_file: "/config/brief_k10L6.bin" 101 | brief_pattern_file: "/config/brief_pattern.yml" 102 | -------------------------------------------------------------------------------- /config/params_lidar.yaml: -------------------------------------------------------------------------------- 1 | # project name 2 | PROJECT_NAME: "emv_lio" 3 | 4 | emv_lio: 5 | 6 | # Topics 7 | pointCloudTopic: "/hesai/pandar" # Point cloud data 8 | imuTopic: "/imu_data" # IMU data 9 | 10 | # Heading 11 | useImuHeadingInitialization: true # if using GPS data, set to "true" 12 | 13 | # Export settings 14 | savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3 15 | savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation 16 | 17 | # lidar Settings 18 | N_SCAN: 32 # number of lidar channel (i.e., 16, 32, 64, 128) 19 | Horizon_SCAN: 2000 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 20 | ang_res_y: 1.0 # vertical resolution of lidar 21 | lidar_type: 2 # 1 for ouster, 2 for hesai, 3 for velodyne 22 | timeField: "timestamp" # point timestamp field, Velodyne - "time", Ouster - "t", hesai - "timestamp" 23 | downsampleRate: 2 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 24 | lidarMaxRange: 50.0 25 | lidarMinRange: 0.5 26 | NUM_OF_CAM: 2 27 | feature_enable: 0 28 | remove_noise: 0 29 | 30 | # noise removal setting 31 | min_cluster_size: 10 32 | segment_valid_point_num: 5 33 | segment_valid_line_num: 3 34 | 35 | # IMU Settings 36 | imuAccNoise: 0.03015059 37 | imuGyrNoise: 0.42306179 38 | imuAccBiasN: 0.00198931 39 | imuGyrBiasN: 0.00015932 40 | imuGravity: 9.80 41 | 42 | # Extrinsics (lidar -> IMU) 43 | extrinsicTrans: [-0.0204, 0.0175, 0.0642] 44 | extrinsicRot: [-0.9741, 0.2262, 0.0008, -0.2261, -0.9740, 0.0147, 0.0041, 0.0141, 0.9999] 45 | #extrinsicTrans: [-0.0162, 0.0207, -0.0644] 46 | #extrinsicRot: [-0.9741, -0.2262, 0.0041, 0.2261, -0.9740, 0.0141, 0.0008, 0.0147, 0.9999] 47 | # extrinsicRPY: [0, 1, 0, -1, 0, 0, 0, 0, 1] 48 | extrinsicRPY: [0, -1, 0, -1, 0, 0, 0, 0, -1] 49 | # LOAM feature threshold 50 | edgeThreshold: 1.0 51 | surfThreshold: 0.1 52 | edgeFeatureMinValidNum: -1 53 | surfFeatureMinValidNum: 100 54 | 55 | # voxel filter paprams 56 | odometrySurfLeafSize: 0.4 # default: 0.4 57 | mappingCornerLeafSize: 0.2 # default: 0.2 58 | mappingSurfLeafSize: 0.4 # default: 0.4 59 | 60 | # robot motion constraint (in case you are using a 2D robot) 61 | z_tollerance: 1000 # meters 62 | rotation_tollerance: 1000 # radians 63 | 64 | # CPU Params 65 | numberOfCores: 4 # number of cores for mapping optimization 66 | mappingProcessInterval: 0.05 # seconds, regulate mapping frequency 67 | 68 | # Surrounding map 69 | surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold 70 | surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold 71 | surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses 72 | surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled) 73 | 74 | # Loop closure 75 | loopClosureEnableFlag: False 76 | surroundingKeyframeSize: 25 # submap size (when loop closure enabled) 77 | historyKeyframeSearchRadius: 20.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure 78 | historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure 79 | historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure 80 | historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment 81 | 82 | # Visualization 83 | globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius 84 | globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density 85 | globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density 86 | 87 | -------------------------------------------------------------------------------- /doc/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BingqiShen/EMV-LIO/efc31dc812ad2d3cfcf3f6227ca921474e76f54f/doc/demo.gif -------------------------------------------------------------------------------- /doc/device.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BingqiShen/EMV-LIO/efc31dc812ad2d3cfcf3f6227ca921474e76f54f/doc/device.png -------------------------------------------------------------------------------- /include/ikd-Tree/README.md: -------------------------------------------------------------------------------- 1 | # ikd-Tree 2 | ikd-Tree is an incremental k-d tree for robotic applications. 3 | -------------------------------------------------------------------------------- /launch/include/config/robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /launch/include/module_robot_state_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/include/module_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/include/module_sam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/include/module_sam_bak.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/include/module_sam_hesai.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/include/rosconsole/rosconsole_error.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = ERROR -------------------------------------------------------------------------------- /launch/include/rosconsole/rosconsole_info.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = INFO -------------------------------------------------------------------------------- /launch/include/rosconsole/rosconsole_warn.conf: -------------------------------------------------------------------------------- 1 | # Set the default ros output to warning and higher 2 | log4j.logger.ros = WARN -------------------------------------------------------------------------------- /launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /lvi_params_for_zju_data: -------------------------------------------------------------------------------- 1 | cam2lidar: 2 | [0.4940 0.0061 -0.8694 -0.1338 3 | 0.8694 -0.0154 0.4939 0.0959 4 | -0.0104 -0.9999 -0.0129 -0.1029 5 | 0 0 0 1.0000] 6 | 7 | quat(w,x,y,z): [0.6053 -0.6169 -0.3548 0.3566] 8 | eul(yaw,pitch.roll) :[ 1.0542 0.0105 -1.5837] 9 | 10 | Eigen::Quaterniond cam2lidar_q (0.6053,-0.6169,-0.3548,0.3566); 11 | Eigen::Vector3d cam2lidar_t (-0.1338, 0.0959,-0.1029); 12 | 13 | lidar2cam: 14 | [0.4940 0.8694 -0.0104 -0.0183 15 | 0.0061 -0.0154 -0.9999 -0.1006 16 | -0.8694 0.4939 -0.0129 -0.1650 17 | 0 0 0 1.0000] 18 | quat(w,x,y,z): [0.6053 0.6169 0.3548 -0.3566] 19 | eul(yaw,pitch.roll) :[ 0.0123 1.0542 1.5969] 20 | 21 | cam2imu: 22 | [-0.2846, -0.0102, 0.9586, 0.1316; 23 | -0.9586, -0.0010, -0.2846 , -0.0471; 24 | 0.0038, -0.9999, -0.0095,-0.0379 25 | 0 0 0 1] 26 | quat: [0.4198 -0.4260 0.5686 -0.5648] 27 | eul(yaw,pitch.roll) :[-1.8594 -0.0038 -1.5803] 28 | 29 | imu2cam: 30 | [ -0.2846 -0.9587 0.0039 -0.0076 31 | -0.0102 -0.0009 -1.0000 -0.0366 32 | 0.9586 -0.2846 -0.0095 -0.1399 33 | 0 0 0 1.0000] 34 | 35 | quat: [0.4198 0.4260 -0.5686 0.5648] 36 | eul(yaw,pitch.roll) :[-3.1056 -1.2820 -1.6043] 37 | 38 | lidar2imu: 39 | [-0.9741 0.2262 0.0008 -0.0203 40 | -0.2261 -0.9739 0.0147 0.0175 41 | 0.0041 0.0140 0.9998 0.0642 42 | 0 0 0 1.0000] 43 | quat:[0.1138 -0.0012 -0.0072 -0.9935] 44 | eul(yaw,pitch.roll) :[-2.9135 -0.0040 0.0140] 45 | 46 | imu2lidar: 47 | [-0.9741 -0.2261 0.0041 -0.0161 48 | 0.2262 -0.9740 0.0141 0.0207 49 | 0.0008 0.0146 0.9999 -0.0644 50 | 0 0 0 1.0000] 51 | quat: [0.1138 0.0012 0.0072 0.9935] 52 | eul(yaw,pitch.roll) :[2.9135 -0.0007 0.0146] 53 | 54 | -------------------------------------------------------------------------------- /msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | # Cloud Info 2 | Header header 3 | 4 | int32[] startRingIndex 5 | int32[] endRingIndex 6 | 7 | int32[] pointColInd # point column index in range image 8 | float32[] pointRange # point range 9 | 10 | int64 imuAvailable 11 | int64 odomAvailable 12 | 13 | # Attitude for lidar odometry initialization 14 | float32 imuRollInit 15 | float32 imuPitchInit 16 | float32 imuYawInit 17 | 18 | # Odometry 19 | float32 odomX 20 | float32 odomY 21 | float32 odomZ 22 | float32 odomRoll 23 | float32 odomPitch 24 | float32 odomYaw 25 | 26 | # Odometry reset ID 27 | int64 odomResetId 28 | 29 | # Point cloud messages 30 | sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed 31 | sensor_msgs/PointCloud2 cloud_corner # extracted corner feature 32 | sensor_msgs/PointCloud2 cloud_surface # extracted surface feature -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | emv_lio 4 | 0.0.0 5 | A package 6 | lalala 7 | Bingqi Shen 8 | BSD-3 9 | 10 | 11 | catkin 12 | 13 | roscpp 14 | roscpp 15 | rospy 16 | rospy 17 | 18 | tf 19 | tf 20 | 21 | cv_bridge 22 | cv_bridge 23 | 24 | pcl_conversions 25 | pcl_conversions 26 | 27 | std_msgs 28 | std_msgs 29 | sensor_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | geometry_msgs 33 | nav_msgs 34 | nav_msgs 35 | 36 | message_generation 37 | message_generation 38 | message_runtime 39 | message_runtime 40 | 41 | GTSAM 42 | GTSAM 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/lidar_odometry/imu_tracker.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "imu_tracker.h" 18 | #include 19 | #include 20 | #include "glog/logging.h" 21 | ImuTracker::ImuTracker(const double imu_gravity_time_constant, 22 | const double time) 23 | : imu_gravity_time_constant_(imu_gravity_time_constant), time_(time), 24 | last_linear_acceleration_time_(0.), 25 | orientation_(Eigen::Quaterniond::Identity()), 26 | gravity_vector_(Eigen::Vector3d::UnitZ()), 27 | imu_angular_velocity_(Eigen::Vector3d::Zero()) 28 | { 29 | } 30 | 31 | void ImuTracker::Advance(const double time) 32 | { 33 | const double delta_t = (time - time_); 34 | const Eigen::Quaterniond rotation = AngleAxisVectorToRotationQuaternion( 35 | Eigen::Vector3d(imu_angular_velocity_ * delta_t)); 36 | orientation_ = (orientation_ * rotation).normalized(); 37 | gravity_vector_ = rotation.conjugate() * gravity_vector_; 38 | time_ = time; 39 | } 40 | 41 | void ImuTracker::AddImuLinearAccelerationObservation( 42 | const Eigen::Vector3d& imu_linear_acceleration) 43 | { 44 | // Update the 'gravity_vector_' with an exponential moving average using the 45 | // 'imu_gravity_time_constant'. 46 | const double delta_t = last_linear_acceleration_time_ > 0 47 | ? (time_ - last_linear_acceleration_time_) 48 | : std::numeric_limits::infinity(); 49 | last_linear_acceleration_time_ = time_; 50 | const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_); 51 | gravity_vector_ = 52 | (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; 53 | 54 | // Change the 'orientation_' so that it agrees with the current 55 | // 'gravity_vector_'. 56 | const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( 57 | gravity_vector_, orientation_.conjugate() * (Eigen::Vector3d::UnitZ())); 58 | orientation_ = (orientation_ * rotation).normalized(); 59 | } 60 | 61 | void ImuTracker::AddImuAngularVelocityObservation( 62 | const Eigen::Vector3d& imu_angular_velocity) 63 | { 64 | imu_angular_velocity_ = imu_angular_velocity; 65 | } 66 | -------------------------------------------------------------------------------- /src/lidar_odometry/imu_tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ 18 | #define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ 19 | 20 | #include "Eigen/Geometry" 21 | #include "Eigen/Eigen" 22 | // Keeps track of the orientation using angular velocities and linear 23 | // accelerations from an IMU. Because averaged linear acceleration (assuming 24 | // slow movement) is a direct measurement of gravity, roll/pitch does not drift, 25 | // though yaw does. 26 | class ImuTracker 27 | { 28 | public: 29 | ImuTracker(const double imu_gravity_time_constant, const double time); 30 | 31 | // Advances to the given 'time' and updates the orientation to reflect this. 32 | void Advance(const double time); 33 | 34 | // Updates from an IMU reading (in the IMU frame). 35 | void AddImuLinearAccelerationObservation( 36 | const Eigen::Vector3d& imu_linear_acceleration); 37 | void 38 | AddImuAngularVelocityObservation(const Eigen::Vector3d& imu_angular_velocity); 39 | 40 | // Query the current time. 41 | double time() const { return time_; } 42 | 43 | // Query the current orientation estimate. 44 | Eigen::Quaterniond orientation() const { return orientation_; } 45 | 46 | template 47 | Eigen::Quaternion 48 | AngleAxisVectorToRotationQuaternion(const Eigen::Matrix& angle_axis) 49 | { 50 | T scale = T(0.5); 51 | T w = T(1.); 52 | constexpr double kCutoffAngle = 1e-8; // We linearize below this angle. 53 | if (angle_axis.squaredNorm() > kCutoffAngle) 54 | { 55 | const T norm = angle_axis.norm(); 56 | scale = sin(norm / 2.) / norm; 57 | w = cos(norm / 2.); 58 | } 59 | const Eigen::Matrix quaternion_xyz = scale * angle_axis; 60 | return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(), 61 | quaternion_xyz.z()); 62 | } 63 | 64 | private: 65 | const double imu_gravity_time_constant_; 66 | double time_; 67 | double last_linear_acceleration_time_; 68 | Eigen::Quaterniond orientation_; 69 | Eigen::Vector3d gravity_vector_; 70 | Eigen::Vector3d imu_angular_velocity_; 71 | }; 72 | #endif // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ 73 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/estimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "feature_manager.h" 5 | #include "utility/utility.h" 6 | #include "utility/tic_toc.h" 7 | #include "initial/solve_5pts.h" 8 | #include "initial/initial_sfm.h" 9 | #include "initial/initial_alignment.h" 10 | #include "initial/initial_ex_rotation.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include "factor/imu_factor.h" 16 | #include "factor/pose_local_parameterization.h" 17 | #include "factor/projection_factor.h" 18 | #include "factor/projection_td_factor.h" 19 | #include "factor/marginalization_factor.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | class Estimator 27 | { 28 | public: 29 | Estimator(); 30 | 31 | void setParameter(); 32 | 33 | // interface 34 | void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 35 | void processImage(const map>>> &image, 36 | const vector &lidar_initialization_info, 37 | const std_msgs::Header &header, 38 | int cam_id); 39 | void processImage2(const map>>> &image, 40 | const map>>> &image1, 41 | const vector &lidar_initialization_info, 42 | const vector &lidar_initialization_info_1, 43 | const std_msgs::Header &header); 44 | void processImage3(const map>>> &image, 45 | const map>>> &image1, 46 | const map>>> &image2, 47 | const vector &lidar_initialization_info, 48 | const vector &lidar_initialization_info_1, 49 | const vector &lidar_initialization_info_2, 50 | const std_msgs::Header &header); 51 | 52 | // internal 53 | void clearState(); 54 | bool initialStructure(int cam_id); 55 | bool visualInitialAlign(int cam_id); 56 | // void visualInitialAlignWithDepth(); 57 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 58 | void slideWindow(int cam_id); 59 | void solveOdometry(); 60 | void slideWindowNew(int cam_id); 61 | void slideWindowOld(int cam_id); 62 | void optimization(); 63 | void vector2double(); 64 | void double2vector(); 65 | bool failureDetection(); 66 | 67 | 68 | enum SolverFlag 69 | { 70 | INITIAL, 71 | NON_LINEAR 72 | }; 73 | 74 | enum MarginalizationFlag 75 | { 76 | MARGIN_OLD = 0, 77 | MARGIN_SECOND_NEW = 1 78 | }; 79 | 80 | SolverFlag solver_flag; 81 | MarginalizationFlag marginalization_flag; 82 | Vector3d g; 83 | MatrixXd Ap[2], backup_A; 84 | VectorXd bp[2], backup_b; 85 | 86 | // transformation from cam to imu 87 | Matrix3d ric[NUM_OF_CAM_ALL]; 88 | Vector3d tic[NUM_OF_CAM_ALL]; 89 | 90 | Vector3d Ps[(WINDOW_SIZE + 1)]; 91 | Vector3d Vs[(WINDOW_SIZE + 1)]; 92 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 93 | Vector3d Bas[(WINDOW_SIZE + 1)]; 94 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 95 | double td; 96 | 97 | Matrix3d back_R0, last_R, last_R0; 98 | Vector3d back_P0, last_P, last_P0; 99 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 100 | 101 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 102 | Vector3d acc_0, gyr_0; 103 | 104 | vector dt_buf[(WINDOW_SIZE + 1)]; 105 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 106 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 107 | 108 | int frame_count; 109 | 110 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 111 | 112 | FeatureManager f_manager; 113 | MotionEstimator m_estimator; 114 | InitialEXRotation initial_ex_rotation; 115 | 116 | bool first_imu; 117 | bool is_valid, is_key; 118 | bool failure_occur; 119 | 120 | vector point_cloud; 121 | vector margin_cloud; 122 | vector key_poses; 123 | double initial_timestamp; 124 | 125 | 126 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 127 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 128 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 129 | double para_Ex_Pose[NUM_OF_CAM_ALL][SIZE_POSE]; 130 | double para_Retrive_Pose[SIZE_POSE]; 131 | double para_Td[1][1]; 132 | double para_Tr[1][1]; 133 | 134 | int loop_window_index; 135 | 136 | MarginalizationInfo *last_marginalization_info; 137 | vector last_marginalization_parameter_blocks; 138 | 139 | map all_image_frame; 140 | IntegrationBase *tmp_pre_integration; 141 | 142 | int failureCount; 143 | }; 144 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "../utility/utility.h" 11 | #include "../utility/tic_toc.h" 12 | 13 | const int NUM_THREADS = 4; 14 | 15 | struct ResidualBlockInfo 16 | { 17 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 18 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 19 | 20 | void Evaluate(); 21 | 22 | ceres::CostFunction *cost_function; 23 | ceres::LossFunction *loss_function; 24 | std::vector parameter_blocks; 25 | std::vector drop_set; 26 | 27 | double **raw_jacobians; 28 | std::vector> jacobians; 29 | Eigen::VectorXd residuals; 30 | 31 | int localSize(int size) 32 | { 33 | return size == 7 ? 6 : size; 34 | } 35 | }; 36 | 37 | struct ThreadsStruct 38 | { 39 | std::vector sub_factors; 40 | Eigen::MatrixXd A; 41 | Eigen::VectorXd b; 42 | std::unordered_map parameter_block_size; //global size 43 | std::unordered_map parameter_block_idx; //local size 44 | }; 45 | 46 | class MarginalizationInfo 47 | { 48 | public: 49 | ~MarginalizationInfo(); 50 | int localSize(int size) const; 51 | int globalSize(int size) const; 52 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 53 | void preMarginalize(); 54 | void marginalize(); 55 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 56 | 57 | std::vector factors; 58 | int m, n; 59 | std::unordered_map parameter_block_size; //global size 60 | int sum_block_size; 61 | std::unordered_map parameter_block_idx; //local size 62 | std::unordered_map parameter_block_data; 63 | 64 | std::vector keep_block_size; //global size 65 | std::vector keep_block_idx; //local size 66 | std::vector keep_block_data; 67 | 68 | Eigen::MatrixXd linearized_jacobians; 69 | Eigen::VectorXd linearized_residuals; 70 | const double eps = 1e-8; 71 | 72 | }; 73 | 74 | class MarginalizationFactor : public ceres::CostFunction 75 | { 76 | public: 77 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 78 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 79 | 80 | MarginalizationInfo* marginalization_info; 81 | }; 82 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../utility/utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; 14 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/projection_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "projection_factor.h" 2 | 3 | Eigen::Matrix2d ProjectionFactor::sqrt_info; 4 | double ProjectionFactor::sum_t; 5 | 6 | ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, int _cam_id) : pts_i(_pts_i), pts_j(_pts_j), cam_id(_cam_id) 7 | { 8 | #ifdef UNIT_SPHERE_ERROR 9 | Eigen::Vector3d b1, b2; 10 | Eigen::Vector3d a = pts_j.normalized(); 11 | Eigen::Vector3d tmp(0, 0, 1); 12 | if(a == tmp) 13 | tmp << 1, 0, 0; 14 | b1 = (tmp - a * (a.transpose() * tmp)).normalized(); 15 | b2 = a.cross(b1); 16 | tangent_base.block<1, 3>(0, 0) = b1.transpose(); 17 | tangent_base.block<1, 3>(1, 0) = b2.transpose(); 18 | #endif 19 | }; 20 | 21 | bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 22 | { 23 | TicToc tic_toc; 24 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 25 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 26 | 27 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 28 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 29 | 30 | Eigen::Vector3d tic[NUM_OF_CAM_ALL]; 31 | Eigen::Quaterniond qic[NUM_OF_CAM_ALL]; 32 | for(int i = 0; i < NUM_OF_CAM; i++) 33 | { 34 | tic[i] << parameters[2][0+7*i], parameters[2][1+7*i], parameters[2][2+7*i]; 35 | qic[i].w() = parameters[2][6+7*i]; qic[i].x() = parameters[2][3+7*i]; 36 | qic[i].y() = parameters[2][4+7*i]; qic[i].z() = parameters[2][5+7*i]; 37 | } 38 | 39 | 40 | // std::cout << "projection tic: " << parameters[2][7] << " " << parameters[2][8] << " " << parameters[2][9] << std::endl; 41 | 42 | //pts_i 是i时刻归一化相机坐标系下的3D坐标 43 | //第i帧相机坐标系下的的逆深度 44 | double inv_dep_i = parameters[3][0]; 45 | //第i帧相机坐标系下的3D坐标 46 | Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; 47 | //第i帧IMU坐标系下的3D坐标 48 | Eigen::Vector3d pts_imu_i = qic[cam_id] * pts_camera_i + tic[cam_id]; 49 | //世界坐标系下的3D坐标 50 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 51 | //第j帧imu坐标系下的3D坐标 52 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 53 | //第j帧相机坐标系下的3D坐标 54 | Eigen::Vector3d pts_camera_j = qic[cam_id].inverse() * (pts_imu_j - tic[cam_id]); 55 | Eigen::Map residual(residuals); 56 | 57 | #ifdef UNIT_SPHERE_ERROR 58 | residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); 59 | #else 60 | double dep_j = pts_camera_j.z(); 61 | residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); 62 | #endif 63 | 64 | residual = sqrt_info * residual; 65 | 66 | //reduce 表示残差residual对fci（pts_camera_j）的导数，同样根据不同的相机模型 67 | if (jacobians) 68 | { 69 | Eigen::Matrix3d Ri = Qi.toRotationMatrix(); 70 | Eigen::Matrix3d Rj = Qj.toRotationMatrix(); 71 | Eigen::Matrix3d ric = qic[cam_id].toRotationMatrix(); 72 | Eigen::Matrix reduce(2, 3); 73 | #ifdef UNIT_SPHERE_ERROR 74 | double norm = pts_camera_j.norm(); 75 | Eigen::Matrix3d norm_jaco; 76 | double x1, x2, x3; 77 | x1 = pts_camera_j(0); 78 | x2 = pts_camera_j(1); 79 | x3 = pts_camera_j(2); 80 | norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), 81 | - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 82 | - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); 83 | reduce = tangent_base * norm_jaco; 84 | #else 85 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 86 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 87 | #endif 88 | reduce = sqrt_info * reduce; 89 | 90 | // 残差项的Jacobian 91 | // 先求fci对各项的Jacobian，然后用链式法则乘起来 92 | // 对第i帧的位姿 pbi,qbi 2X7的矩阵 最后一项是0 93 | if (jacobians[0]) 94 | { 95 | Eigen::Map> jacobian_pose_i(jacobians[0]); 96 | 97 | Eigen::Matrix jaco_i; 98 | jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); 99 | jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); 100 | 101 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 102 | jacobian_pose_i.rightCols<1>().setZero(); 103 | } 104 | // 对第j帧的位姿 pbj,qbj 105 | if (jacobians[1]) 106 | { 107 | Eigen::Map> jacobian_pose_j(jacobians[1]); 108 | 109 | Eigen::Matrix jaco_j; 110 | jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); 111 | jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); 112 | 113 | jacobian_pose_j.leftCols<6>() = reduce * jaco_j; 114 | jacobian_pose_j.rightCols<1>().setZero(); 115 | } 116 | // 对相机到IMU的外参 pbc,qbc (qic,tic) 117 | if (jacobians[2]) 118 | { 119 | Eigen::Map> jacobian_ex_pose(jacobians[2]); 120 | Eigen::Matrix jaco_ex; 121 | jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); 122 | Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; 123 | jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + 124 | Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic[cam_id] + Pi - Pj) - tic[cam_id])); 125 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 126 | jacobian_ex_pose.rightCols<1>().setZero(); 127 | } 128 | // 对逆深度 \lambda (inv_dep_i) 129 | if (jacobians[3]) 130 | { 131 | Eigen::Map jacobian_feature(jacobians[3]); 132 | #if 1 133 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i); 134 | #else 135 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i; 136 | #endif 137 | } 138 | } 139 | sum_t += tic_toc.toc(); 140 | 141 | return true; 142 | } -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/projection_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> 11 | { 12 | public: 13 | ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, int _cam_id); 14 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 15 | void check(double **parameters); 16 | 17 | Eigen::Vector3d pts_i, pts_j; 18 | int cam_id; 19 | Eigen::Matrix tangent_base; 20 | static Eigen::Matrix2d sqrt_info; 21 | static double sum_t; 22 | }; 23 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/projection_td_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "projection_td_factor.h" 2 | 3 | Eigen::Matrix2d ProjectionTdFactor::sqrt_info; 4 | double ProjectionTdFactor::sum_t; 5 | 6 | ProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 7 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 8 | const double _td_i, const double _td_j, const double _row_i, const double _row_j) : 9 | pts_i(_pts_i), pts_j(_pts_j), 10 | td_i(_td_i), td_j(_td_j) 11 | { 12 | velocity_i.x() = _velocity_i.x(); 13 | velocity_i.y() = _velocity_i.y(); 14 | velocity_i.z() = 0; 15 | velocity_j.x() = _velocity_j.x(); 16 | velocity_j.y() = _velocity_j.y(); 17 | velocity_j.z() = 0; 18 | row_i = _row_i - ROW / 2; 19 | row_j = _row_j - ROW / 2; 20 | 21 | #ifdef UNIT_SPHERE_ERROR 22 | Eigen::Vector3d b1, b2; 23 | Eigen::Vector3d a = pts_j.normalized(); 24 | Eigen::Vector3d tmp(0, 0, 1); 25 | if(a == tmp) 26 | tmp << 1, 0, 0; 27 | b1 = (tmp - a * (a.transpose() * tmp)).normalized(); 28 | b2 = a.cross(b1); 29 | tangent_base.block<1, 3>(0, 0) = b1.transpose(); 30 | tangent_base.block<1, 3>(1, 0) = b2.transpose(); 31 | #endif 32 | }; 33 | 34 | bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 35 | { 36 | TicToc tic_toc; 37 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 38 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 39 | 40 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 41 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 42 | 43 | Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); 44 | Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 45 | 46 | double inv_dep_i = parameters[3][0]; 47 | 48 | double td = parameters[4][0]; 49 | 50 | Eigen::Vector3d pts_i_td, pts_j_td; 51 | pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i; 52 | pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j; 53 | Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; 54 | Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; 55 | Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; 56 | Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); 57 | Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); 58 | Eigen::Map residual(residuals); 59 | 60 | #ifdef UNIT_SPHERE_ERROR 61 | residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); 62 | #else 63 | double dep_j = pts_camera_j.z(); 64 | residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); 65 | #endif 66 | 67 | residual = sqrt_info * residual; 68 | 69 | if (jacobians) 70 | { 71 | Eigen::Matrix3d Ri = Qi.toRotationMatrix(); 72 | Eigen::Matrix3d Rj = Qj.toRotationMatrix(); 73 | Eigen::Matrix3d ric = qic.toRotationMatrix(); 74 | Eigen::Matrix reduce(2, 3); 75 | #ifdef UNIT_SPHERE_ERROR 76 | double norm = pts_camera_j.norm(); 77 | Eigen::Matrix3d norm_jaco; 78 | double x1, x2, x3; 79 | x1 = pts_camera_j(0); 80 | x2 = pts_camera_j(1); 81 | x3 = pts_camera_j(2); 82 | norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), 83 | - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 84 | - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); 85 | reduce = tangent_base * norm_jaco; 86 | #else 87 | reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 88 | 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); 89 | #endif 90 | reduce = sqrt_info * reduce; 91 | 92 | if (jacobians[0]) 93 | { 94 | Eigen::Map> jacobian_pose_i(jacobians[0]); 95 | 96 | Eigen::Matrix jaco_i; 97 | jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); 98 | jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); 99 | 100 | jacobian_pose_i.leftCols<6>() = reduce * jaco_i; 101 | jacobian_pose_i.rightCols<1>().setZero(); 102 | } 103 | 104 | if (jacobians[1]) 105 | { 106 | Eigen::Map> jacobian_pose_j(jacobians[1]); 107 | 108 | Eigen::Matrix jaco_j; 109 | jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); 110 | jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); 111 | 112 | jacobian_pose_j.leftCols<6>() = reduce * jaco_j; 113 | jacobian_pose_j.rightCols<1>().setZero(); 114 | } 115 | if (jacobians[2]) 116 | { 117 | Eigen::Map> jacobian_ex_pose(jacobians[2]); 118 | Eigen::Matrix jaco_ex; 119 | jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); 120 | Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; 121 | jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + 122 | Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); 123 | jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; 124 | jacobian_ex_pose.rightCols<1>().setZero(); 125 | } 126 | if (jacobians[3]) 127 | { 128 | Eigen::Map jacobian_feature(jacobians[3]); 129 | jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); 130 | } 131 | if (jacobians[4]) 132 | { 133 | Eigen::Map jacobian_td(jacobians[4]); 134 | jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0 + 135 | sqrt_info * velocity_j.head(2); 136 | } 137 | } 138 | sum_t += tic_toc.toc(); 139 | 140 | return true; 141 | } -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/factor/projection_td_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "../utility/utility.h" 7 | #include "../utility/tic_toc.h" 8 | #include "../parameters.h" 9 | 10 | //对imu-camera 时间戳不完全同步和 Rolling shutter 相机的支持 11 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 12 | { 13 | public: 14 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 15 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 16 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 17 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 18 | void check(double **parameters); 19 | 20 | Eigen::Vector3d pts_i, pts_j;//角点在归一化平面的坐标 21 | Eigen::Vector3d velocity_i, velocity_j;//角点在归一化平面的速度 22 | double td_i, td_j;//处理IMU数据时用到的时间同步误差 23 | Eigen::Matrix tangent_base; 24 | double row_i, row_j;//角点图像坐标的纵坐标 25 | static Eigen::Matrix2d sqrt_info; 26 | static double sum_t; 27 | }; 28 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/feature_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_MANAGER_H 2 | #define FEATURE_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | #include 11 | using namespace Eigen; 12 | 13 | #include 14 | #include 15 | 16 | #include "parameters.h" 17 | 18 | class FeaturePerFrame 19 | { 20 | public: 21 | FeaturePerFrame(const Eigen::Matrix &_point, double td) 22 | { 23 | point.x() = _point(0); 24 | point.y() = _point(1); 25 | point.z() = _point(2); 26 | uv.x() = _point(3); 27 | uv.y() = _point(4); 28 | velocity.x() = _point(5); 29 | velocity.y() = _point(6); 30 | depth = _point(7); 31 | cur_td = td; 32 | } 33 | double cur_td; 34 | Vector3d point; 35 | Vector2d uv; 36 | Vector2d velocity; 37 | double z; 38 | bool is_used; 39 | double parallax; 40 | MatrixXd A; 41 | VectorXd b; 42 | double dep_gradient; 43 | double depth; // lidar depth, initialized with -1 from feature points in feature tracker node 44 | }; 45 | 46 | class FeaturePerId 47 | { 48 | public: 49 | const int feature_id; 50 | const int cam_id; 51 | int start_frame; 52 | vector feature_per_frame; 53 | 54 | int used_num; 55 | bool is_outlier; 56 | bool is_margin; 57 | double estimated_depth; 58 | bool lidar_depth_flag; 59 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 60 | 61 | Vector3d gt_p; 62 | 63 | FeaturePerId(int _feature_id, int _start_frame, double _measured_depth, int _cam_id) 64 | : feature_id(_feature_id), start_frame(_start_frame), cam_id(_cam_id), 65 | used_num(0), estimated_depth(-1.0), lidar_depth_flag(false), solve_flag(0) 66 | { 67 | if (_measured_depth > 0) 68 | { 69 | estimated_depth = _measured_depth; 70 | lidar_depth_flag = true; 71 | } 72 | else 73 | { 74 | estimated_depth = -1; 75 | lidar_depth_flag = false; 76 | } 77 | } 78 | 79 | int endFrame(); 80 | }; 81 | 82 | class FeatureManager 83 | { 84 | public: 85 | FeatureManager(Matrix3d _Rs[]); 86 | 87 | void setRic(Matrix3d _ric[]); 88 | 89 | void clearState(); 90 | 91 | int getFeatureCount(); 92 | 93 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 94 | void debugShow(); 95 | vector> getCorresponding(int frame_count_l, int frame_count_r); 96 | 97 | //void updateDepth(const VectorXd &x); 98 | void setDepth(const VectorXd &x); 99 | void removeFailures(); 100 | void clearDepth(const VectorXd &x); 101 | VectorXd getDepthVector(); 102 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 103 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 104 | void removeBack(); 105 | void removeFront(int frame_count); 106 | void removeOutlier(); 107 | list feature; 108 | int last_track_num; 109 | 110 | private: 111 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 112 | const Matrix3d *Rs; 113 | Matrix3d ric[NUM_OF_CAM_ALL]; 114 | }; 115 | 116 | #endif -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/initial/initial_ex_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "initial_ex_rotation.h" 2 | 3 | InitialEXRotation::InitialEXRotation(){ 4 | frame_count = 0; 5 | Rc.push_back(Matrix3d::Identity()); 6 | Rc_g.push_back(Matrix3d::Identity()); 7 | Rimu.push_back(Matrix3d::Identity()); 8 | ric = Matrix3d::Identity(); 9 | } 10 | 11 | bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result) 12 | { 13 | frame_count++; 14 | Rc.push_back(solveRelativeR(corres)); 15 | Rimu.push_back(delta_q_imu.toRotationMatrix()); 16 | Rc_g.push_back(ric.inverse() * delta_q_imu * ric); 17 | 18 | Eigen::MatrixXd A(frame_count * 4, 4); 19 | A.setZero(); 20 | int sum_ok = 0; 21 | for (int i = 1; i <= frame_count; i++) 22 | { 23 | Quaterniond r1(Rc[i]); 24 | Quaterniond r2(Rc_g[i]); 25 | 26 | double angular_distance = 180 / M_PI * r1.angularDistance(r2); 27 | ROS_DEBUG( 28 | "%d %f", i, angular_distance); 29 | 30 | double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0; 31 | ++sum_ok; 32 | Matrix4d L, R; 33 | 34 | double w = Quaterniond(Rc[i]).w(); 35 | Vector3d q = Quaterniond(Rc[i]).vec(); 36 | L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q); 37 | L.block<3, 1>(0, 3) = q; 38 | L.block<1, 3>(3, 0) = -q.transpose(); 39 | L(3, 3) = w; 40 | 41 | Quaterniond R_ij(Rimu[i]); 42 | w = R_ij.w(); 43 | q = R_ij.vec(); 44 | R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q); 45 | R.block<3, 1>(0, 3) = q; 46 | R.block<1, 3>(3, 0) = -q.transpose(); 47 | R(3, 3) = w; 48 | 49 | A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); 50 | } 51 | 52 | JacobiSVD svd(A, ComputeFullU | ComputeFullV); 53 | Matrix x = svd.matrixV().col(3); 54 | Quaterniond estimated_R(x); 55 | ric = estimated_R.toRotationMatrix().inverse(); 56 | //cout << svd.singularValues().transpose() << endl; 57 | //cout << ric << endl; 58 | Vector3d ric_cov; 59 | ric_cov = svd.singularValues().tail<3>(); 60 | if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) 61 | { 62 | calib_ric_result = ric; 63 | return true; 64 | } 65 | else 66 | return false; 67 | } 68 | 69 | Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres) 70 | { 71 | if (corres.size() >= 9) 72 | { 73 | vector ll, rr; 74 | for (int i = 0; i < int(corres.size()); i++) 75 | { 76 | ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); 77 | rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); 78 | } 79 | cv::Mat E = cv::findFundamentalMat(ll, rr); 80 | cv::Mat_ R1, R2, t1, t2; 81 | decomposeE(E, R1, R2, t1, t2); 82 | 83 | if (determinant(R1) + 1.0 < 1e-09) 84 | { 85 | E = -E; 86 | decomposeE(E, R1, R2, t1, t2); 87 | } 88 | double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2)); 89 | double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2)); 90 | cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2; 91 | 92 | Matrix3d ans_R_eigen; 93 | for (int i = 0; i < 3; i++) 94 | for (int j = 0; j < 3; j++) 95 | ans_R_eigen(j, i) = ans_R_cv(i, j); 96 | return ans_R_eigen; 97 | } 98 | return Matrix3d::Identity(); 99 | } 100 | 101 | double InitialEXRotation::testTriangulation(const vector &l, 102 | const vector &r, 103 | cv::Mat_ R, cv::Mat_ t) 104 | { 105 | cv::Mat pointcloud; 106 | cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 107 | 0, 1, 0, 0, 108 | 0, 0, 1, 0); 109 | cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), 110 | R(1, 0), R(1, 1), R(1, 2), t(1), 111 | R(2, 0), R(2, 1), R(2, 2), t(2)); 112 | cv::triangulatePoints(P, P1, l, r, pointcloud); 113 | int front_count = 0; 114 | for (int i = 0; i < pointcloud.cols; i++) 115 | { 116 | double normal_factor = pointcloud.col(i).at(3); 117 | 118 | cv::Mat_ p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor); 119 | cv::Mat_ p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor); 120 | if (p_3d_l(2) > 0 && p_3d_r(2) > 0) 121 | front_count++; 122 | } 123 | ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols); 124 | return 1.0 * front_count / pointcloud.cols; 125 | } 126 | 127 | void InitialEXRotation::decomposeE(cv::Mat E, 128 | cv::Mat_ &R1, cv::Mat_ &R2, 129 | cv::Mat_ &t1, cv::Mat_ &t2) 130 | { 131 | cv::SVD svd(E, cv::SVD::MODIFY_A); 132 | cv::Matx33d W(0, -1, 0, 133 | 1, 0, 0, 134 | 0, 0, 1); 135 | cv::Matx33d Wt(0, 1, 0, 136 | -1, 0, 0, 137 | 0, 0, 1); 138 | R1 = svd.u * cv::Mat(W) * svd.vt; 139 | R2 = svd.u * cv::Mat(Wt) * svd.vt; 140 | t1 = svd.u.col(2); 141 | t2 = -svd.u.col(2); 142 | } 143 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/initial/initial_ex_rotation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../parameters.h" 5 | using namespace std; 6 | 7 | #include 8 | 9 | #include 10 | using namespace Eigen; 11 | #include 12 | 13 | /* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */ 14 | class InitialEXRotation 15 | { 16 | public: 17 | InitialEXRotation(); 18 | bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result); 19 | private: 20 | Matrix3d solveRelativeR(const vector> &corres); 21 | 22 | double testTriangulation(const vector &l, 23 | const vector &r, 24 | cv::Mat_ R, cv::Mat_ t); 25 | void decomposeE(cv::Mat E, 26 | cv::Mat_ &R1, cv::Mat_ &R2, 27 | cv::Mat_ &t1, cv::Mat_ &t2); 28 | int frame_count; 29 | 30 | vector< Matrix3d > Rc; 31 | vector< Matrix3d > Rimu; 32 | vector< Matrix3d > Rc_g; 33 | Matrix3d ric; 34 | }; 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/initial/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | using namespace std; 13 | 14 | 15 | 16 | struct SFMFeature 17 | { 18 | bool state; 19 | int id; 20 | vector> observation; 21 | double position[3]; 22 | double depth; 23 | }; 24 | 25 | struct ReprojectionError3D 26 | { 27 | ReprojectionError3D(double observed_u, double observed_v) 28 | :observed_u(observed_u), observed_v(observed_v) 29 | {} 30 | 31 | template 32 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 33 | { 34 | T p[3]; 35 | ceres::QuaternionRotatePoint(camera_R, point, p); 36 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 37 | T xp = p[0] / p[2]; 38 | T yp = p[1] / p[2]; 39 | residuals[0] = xp - T(observed_u); 40 | residuals[1] = yp - T(observed_v); 41 | return true; 42 | } 43 | 44 | static ceres::CostFunction* Create(const double observed_x, 45 | const double observed_y) 46 | { 47 | return (new ceres::AutoDiffCostFunction< 48 | ReprojectionError3D, 2, 4, 3, 3>( 49 | new ReprojectionError3D(observed_x,observed_y))); 50 | } 51 | 52 | double observed_u; 53 | double observed_v; 54 | }; 55 | 56 | class GlobalSFM 57 | { 58 | public: 59 | GlobalSFM(); 60 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 61 | const Matrix3d relative_R, const Vector3d relative_T, 62 | vector &sfm_f, map &sfm_tracked_points); 63 | 64 | private: 65 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 66 | 67 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 68 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 69 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 70 | int frame1, Eigen::Matrix &Pose1, 71 | vector &sfm_f); 72 | 73 | int feature_num; 74 | }; -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/initial/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | //#include 8 | #include 9 | using namespace Eigen; 10 | 11 | #include 12 | 13 | class MotionEstimator 14 | { 15 | public: 16 | 17 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 18 | 19 | private: 20 | double testTriangulation(const vector &l, 21 | const vector &r, 22 | cv::Mat_ R, cv::Mat_ t); 23 | void decomposeE(cv::Mat E, 24 | cv::Mat_ &R1, cv::Mat_ &R2, 25 | cv::Mat_ &t1, cv::Mat_ &t2); 26 | }; 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | const int WINDOW_SIZE = 10; 57 | 58 | 59 | const double FOCAL_LENGTH = 460.0; 60 | // TODO: move this to yaml 61 | const int NUM_OF_CAM_ALL = 3; 62 | extern int NUM_OF_CAM; 63 | 64 | const int NUM_OF_F = 1000; 65 | // #define UNIT_SPHERE_ERROR 1 66 | 67 | 68 | extern double INIT_DEPTH; 69 | extern double MIN_PARALLAX; 70 | extern int ESTIMATE_EXTRINSIC; 71 | 72 | extern double IMU_FREQ; 73 | extern double DELTA_T_IMU; 74 | extern double ACC_N, ACC_W; 75 | extern double GYR_N, GYR_W; 76 | 77 | extern std::vector RIC; 78 | extern std::vector TIC; 79 | extern Eigen::Vector3d G; 80 | 81 | extern double BIAS_ACC_THRESHOLD; 82 | extern double BIAS_GYR_THRESHOLD; 83 | extern double SOLVER_TIME; 84 | extern int NUM_ITERATIONS; 85 | extern std::string EX_CALIB_RESULT_PATH; 86 | extern std::string PROJECT_NAME; 87 | extern std::string IMU_TOPIC; 88 | extern double TD; 89 | extern double TR; 90 | extern int ESTIMATE_TD; 91 | extern int ROLLING_SHUTTER; 92 | extern double ROW, COL; 93 | 94 | extern int USE_LIDAR; 95 | extern int ALIGN_CAMERA_LIDAR_COORDINATE; 96 | 97 | 98 | 99 | extern double C_L_TX; 100 | extern double C_L_TY; 101 | extern double C_L_TZ; 102 | extern double C_L_RX; 103 | extern double C_L_RY; 104 | extern double C_L_RZ; 105 | 106 | extern double C_L_TX1; 107 | extern double C_L_TY1; 108 | extern double C_L_TZ1; 109 | extern double C_L_RX1; 110 | extern double C_L_RY1; 111 | extern double C_L_RZ1; 112 | 113 | extern double C_L_TX2; 114 | extern double C_L_TY2; 115 | extern double C_L_TZ2; 116 | extern double C_L_RX2; 117 | extern double C_L_RY2; 118 | extern double C_L_RZ2; 119 | 120 | extern Eigen::Matrix3d extRot_lidar2imu; 121 | extern Eigen::Vector3d extTrans_lidar2imu; 122 | 123 | void readParameters(ros::NodeHandle &n); 124 | 125 | enum SIZE_PARAMETERIZATION 126 | { 127 | SIZE_POSE = 7, 128 | SIZE_SPEEDBIAS = 9, 129 | SIZE_FEATURE = 1 130 | }; 131 | 132 | enum StateOrder 133 | { 134 | O_P = 0, 135 | O_R = 3, 136 | O_V = 6, 137 | O_BA = 9, 138 | O_BG = 12 139 | }; 140 | 141 | enum NoiseOrder 142 | { 143 | O_AN = 0, 144 | O_GN = 3, 145 | O_AW = 6, 146 | O_GW = 9 147 | }; 148 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class CameraPoseVisualization { 11 | public: 12 | std::string m_marker_ns; 13 | 14 | CameraPoseVisualization(float r, float g, float b, float a); 15 | 16 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 17 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 18 | void setScale(double s); 19 | void setLineWidth(double width); 20 | 21 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 22 | void reset(); 23 | 24 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 25 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 26 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 27 | private: 28 | std::vector m_markers; 29 | std_msgs::ColorRGBA m_image_boundary_color; 30 | std_msgs::ColorRGBA m_optical_center_connector_color; 31 | double m_scale; 32 | double m_line_width; 33 | 34 | static const Eigen::Vector3d imlt; 35 | static const Eigen::Vector3d imlb; 36 | static const Eigen::Vector3d imrt; 37 | static const Eigen::Vector3d imrb; 38 | static const Eigen::Vector3d oc ; 39 | static const Eigen::Vector3d lt0 ; 40 | static const Eigen::Vector3d lt1 ; 41 | static const Eigen::Vector3d lt2 ; 42 | }; 43 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/utility/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/utility/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 4 | { 5 | Eigen::Matrix3d R0; 6 | Eigen::Vector3d ng1 = g.normalized(); 7 | Eigen::Vector3d ng2{0, 0, 1.0}; 8 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 9 | double yaw = Utility::R2ypr(R0).x(); 10 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 11 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 12 | return R0; 13 | } 14 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/utility/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class Utility 13 | { 14 | public: 15 | template 16 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 17 | { 18 | typedef typename Derived::Scalar Scalar_t; 19 | 20 | Eigen::Quaternion dq; 21 | Eigen::Matrix half_theta = theta; 22 | half_theta /= static_cast(2.0); 23 | dq.w() = static_cast(1.0); 24 | dq.x() = half_theta.x(); 25 | dq.y() = half_theta.y(); 26 | dq.z() = half_theta.z(); 27 | return dq; 28 | } 29 | 30 | template 31 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 32 | { 33 | Eigen::Matrix ans; 34 | ans << typename Derived::Scalar(0), -q(2), q(1), 35 | q(2), typename Derived::Scalar(0), -q(0), 36 | -q(1), q(0), typename Derived::Scalar(0); 37 | return ans; 38 | } 39 | 40 | template 41 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 42 | { 43 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 44 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 45 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 46 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 47 | return q; 48 | } 49 | 50 | template 51 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 52 | { 53 | Eigen::Quaternion qq = positify(q); 54 | Eigen::Matrix ans; 55 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 56 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 57 | return ans; 58 | } 59 | 60 | template 61 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 62 | { 63 | Eigen::Quaternion pp = positify(p); 64 | Eigen::Matrix ans; 65 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 66 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 67 | return ans; 68 | } 69 | 70 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 71 | { 72 | Eigen::Vector3d n = R.col(0); 73 | Eigen::Vector3d o = R.col(1); 74 | Eigen::Vector3d a = R.col(2); 75 | 76 | Eigen::Vector3d ypr(3); 77 | double y = atan2(n(1), n(0)); 78 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 79 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 80 | ypr(0) = y; 81 | ypr(1) = p; 82 | ypr(2) = r; 83 | 84 | return ypr / M_PI * 180.0; 85 | } 86 | 87 | template 88 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 89 | { 90 | typedef typename Derived::Scalar Scalar_t; 91 | 92 | Scalar_t y = ypr(0) / 180.0 * M_PI; 93 | Scalar_t p = ypr(1) / 180.0 * M_PI; 94 | Scalar_t r = ypr(2) / 180.0 * M_PI; 95 | 96 | Eigen::Matrix Rz; 97 | Rz << cos(y), -sin(y), 0, 98 | sin(y), cos(y), 0, 99 | 0, 0, 1; 100 | 101 | Eigen::Matrix Ry; 102 | Ry << cos(p), 0., sin(p), 103 | 0., 1., 0., 104 | -sin(p), 0., cos(p); 105 | 106 | Eigen::Matrix Rx; 107 | Rx << 1., 0., 0., 108 | 0., cos(r), -sin(r), 109 | 0., sin(r), cos(r); 110 | 111 | return Rz * Ry * Rx; 112 | } 113 | 114 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g); 115 | 116 | template 117 | struct uint_ 118 | { 119 | }; 120 | 121 | template 122 | void unroller(const Lambda &f, const IterT &iter, uint_) 123 | { 124 | unroller(f, iter, uint_()); 125 | f(iter + N); 126 | } 127 | 128 | template 129 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 130 | { 131 | f(iter); 132 | } 133 | 134 | template 135 | static T normalizeAngle(const T& angle_degrees) { 136 | T two_pi(2.0 * 180); 137 | if (angle_degrees > 0) 138 | return angle_degrees - 139 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 140 | else 141 | return angle_degrees + 142 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 143 | }; 144 | }; 145 | 146 | class FileSystemHelper 147 | { 148 | public: 149 | 150 | /****************************************************************************** 151 | * Recursively create directory if `path` not exists. 152 | * Return 0 if success. 153 | *****************************************************************************/ 154 | static int createDirectoryIfNotExists(const char *path) 155 | { 156 | struct stat info; 157 | int statRC = stat(path, &info); 158 | if( statRC != 0 ) 159 | { 160 | if (errno == ENOENT) 161 | { 162 | printf("%s not exists, trying to create it \n", path); 163 | if (! createDirectoryIfNotExists(dirname(strdupa(path)))) 164 | { 165 | if (mkdir(path, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) 166 | { 167 | fprintf(stderr, "Failed to create folder %s \n", path); 168 | return 1; 169 | } 170 | else 171 | return 0; 172 | } 173 | else 174 | return 1; 175 | } // directory not exists 176 | if (errno == ENOTDIR) 177 | { 178 | fprintf(stderr, "%s is not a directory path \n", path); 179 | return 1; 180 | } // something in path prefix is not a dir 181 | return 1; 182 | } 183 | return ( info.st_mode & S_IFDIR ) ? 0 : 1; 184 | } 185 | }; 186 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_estimator/utility/visualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "CameraPoseVisualization.h" 17 | #include 18 | #include "../estimator.h" 19 | #include "../parameters.h" 20 | #include 21 | 22 | extern ros::Publisher pub_odometry; 23 | extern ros::Publisher pub_path, pub_pose; 24 | extern ros::Publisher pub_cloud, pub_map; 25 | extern ros::Publisher pub_key_poses; 26 | extern ros::Publisher pub_ref_pose, pub_cur_pose; 27 | extern ros::Publisher pub_key; 28 | extern nav_msgs::Path path; 29 | extern ros::Publisher pub_pose_graph; 30 | extern int IMAGE_ROW, IMAGE_COL; 31 | 32 | void registerPub(ros::NodeHandle &n); 33 | 34 | tf::Transform transformConversion(const tf::StampedTransform& t); 35 | 36 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header, const int &failureId); 37 | 38 | void printStatistics(const Estimator &estimator, double t); 39 | 40 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 41 | 42 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 43 | 44 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 45 | 46 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 47 | void pubCameraPose1(const Estimator &estimator, const std_msgs::Header &header); 48 | void pubCameraPose2(const Estimator &estimator, const std_msgs::Header &header); 49 | 50 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 51 | void pubPointCloud1(const Estimator &estimator, const std_msgs::Header &header); 52 | void pubPointCloud2(const Estimator &estimator, const std_msgs::Header &header); 53 | 54 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 55 | 56 | void pubKeyframe(const Estimator &estimator); 57 | 58 | void pubRelocalization(const Estimator &estimator); -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/Camera.cc: -------------------------------------------------------------------------------- 1 | #include "Camera.h" 2 | #include "ScaramuzzaCamera.h" 3 | 4 | #include 5 | 6 | namespace camodocal 7 | { 8 | 9 | Camera::Parameters::Parameters(ModelType modelType) 10 | : m_modelType(modelType) 11 | , m_imageWidth(0) 12 | , m_imageHeight(0) 13 | { 14 | switch (modelType) 15 | { 16 | case KANNALA_BRANDT: 17 | m_nIntrinsics = 8; 18 | break; 19 | case PINHOLE: 20 | m_nIntrinsics = 8; 21 | break; 22 | case SCARAMUZZA: 23 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 24 | break; 25 | case MEI: 26 | default: 27 | m_nIntrinsics = 9; 28 | } 29 | } 30 | 31 | Camera::Parameters::Parameters(ModelType modelType, 32 | const std::string& cameraName, 33 | int w, int h) 34 | : m_modelType(modelType) 35 | , m_cameraName(cameraName) 36 | , m_imageWidth(w) 37 | , m_imageHeight(h) 38 | { 39 | switch (modelType) 40 | { 41 | case KANNALA_BRANDT: 42 | m_nIntrinsics = 8; 43 | break; 44 | case PINHOLE: 45 | m_nIntrinsics = 8; 46 | break; 47 | case SCARAMUZZA: 48 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 49 | break; 50 | case MEI: 51 | default: 52 | m_nIntrinsics = 9; 53 | } 54 | } 55 | 56 | Camera::ModelType& 57 | Camera::Parameters::modelType(void) 58 | { 59 | return m_modelType; 60 | } 61 | 62 | std::string& 63 | Camera::Parameters::cameraName(void) 64 | { 65 | return m_cameraName; 66 | } 67 | 68 | int& 69 | Camera::Parameters::imageWidth(void) 70 | { 71 | return m_imageWidth; 72 | } 73 | 74 | int& 75 | Camera::Parameters::imageHeight(void) 76 | { 77 | return m_imageHeight; 78 | } 79 | 80 | Camera::ModelType 81 | Camera::Parameters::modelType(void) const 82 | { 83 | return m_modelType; 84 | } 85 | 86 | const std::string& 87 | Camera::Parameters::cameraName(void) const 88 | { 89 | return m_cameraName; 90 | } 91 | 92 | int 93 | Camera::Parameters::imageWidth(void) const 94 | { 95 | return m_imageWidth; 96 | } 97 | 98 | int 99 | Camera::Parameters::imageHeight(void) const 100 | { 101 | return m_imageHeight; 102 | } 103 | 104 | int 105 | Camera::Parameters::nIntrinsics(void) const 106 | { 107 | return m_nIntrinsics; 108 | } 109 | 110 | cv::Mat& 111 | Camera::mask(void) 112 | { 113 | return m_mask; 114 | } 115 | 116 | const cv::Mat& 117 | Camera::mask(void) const 118 | { 119 | return m_mask; 120 | } 121 | 122 | void 123 | Camera::estimateExtrinsics(const std::vector& objectPoints, 124 | const std::vector& imagePoints, 125 | cv::Mat& rvec, cv::Mat& tvec) const 126 | { 127 | std::vector Ms(imagePoints.size()); 128 | for (size_t i = 0; i < Ms.size(); ++i) 129 | { 130 | Eigen::Vector3d P; 131 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 132 | 133 | P /= P(2); 134 | 135 | Ms.at(i).x = P(0); 136 | Ms.at(i).y = P(1); 137 | } 138 | 139 | // assume unit focal length, zero principal point, and zero distortion 140 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 141 | } 142 | 143 | double 144 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 145 | { 146 | Eigen::Vector2d p1, p2; 147 | 148 | spaceToPlane(P1, p1); 149 | spaceToPlane(P2, p2); 150 | 151 | return (p1 - p2).norm(); 152 | } 153 | 154 | double 155 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 156 | const std::vector< std::vector >& imagePoints, 157 | const std::vector& rvecs, 158 | const std::vector& tvecs, 159 | cv::OutputArray _perViewErrors) const 160 | { 161 | int imageCount = objectPoints.size(); 162 | size_t pointsSoFar = 0; 163 | double totalErr = 0.0; 164 | 165 | bool computePerViewErrors = _perViewErrors.needed(); 166 | cv::Mat perViewErrors; 167 | if (computePerViewErrors) 168 | { 169 | _perViewErrors.create(imageCount, 1, CV_64F); 170 | perViewErrors = _perViewErrors.getMat(); 171 | } 172 | 173 | for (int i = 0; i < imageCount; ++i) 174 | { 175 | size_t pointCount = imagePoints.at(i).size(); 176 | 177 | pointsSoFar += pointCount; 178 | 179 | std::vector estImagePoints; 180 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 181 | estImagePoints); 182 | 183 | double err = 0.0; 184 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 185 | { 186 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 187 | } 188 | 189 | if (computePerViewErrors) 190 | { 191 | perViewErrors.at(i) = err / pointCount; 192 | } 193 | 194 | totalErr += err; 195 | } 196 | 197 | return totalErr / pointsSoFar; 198 | } 199 | 200 | double 201 | Camera::reprojectionError(const Eigen::Vector3d& P, 202 | const Eigen::Quaterniond& camera_q, 203 | const Eigen::Vector3d& camera_t, 204 | const Eigen::Vector2d& observed_p) const 205 | { 206 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 207 | 208 | Eigen::Vector2d p; 209 | spaceToPlane(P_cam, p); 210 | 211 | return (p - observed_p).norm(); 212 | } 213 | 214 | void 215 | Camera::projectPoints(const std::vector& objectPoints, 216 | const cv::Mat& rvec, 217 | const cv::Mat& tvec, 218 | std::vector& imagePoints) const 219 | { 220 | // project 3D object points to the image plane 221 | imagePoints.reserve(objectPoints.size()); 222 | 223 | //double 224 | cv::Mat R0; 225 | cv::Rodrigues(rvec, R0); 226 | 227 | Eigen::MatrixXd R(3,3); 228 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 229 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 230 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 231 | 232 | Eigen::Vector3d t; 233 | t << tvec.at(0), tvec.at(1), tvec.at(2); 234 | 235 | for (size_t i = 0; i < objectPoints.size(); ++i) 236 | { 237 | const cv::Point3f& objectPoint = objectPoints.at(i); 238 | 239 | // Rotate and translate 240 | Eigen::Vector3d P; 241 | P << objectPoint.x, objectPoint.y, objectPoint.z; 242 | 243 | P = R * P + t; 244 | 245 | Eigen::Vector2d p; 246 | spaceToPlane(P, p); 247 | 248 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 249 | } 250 | } 251 | 252 | } 253 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace camodocal 10 | { 11 | 12 | class Camera 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | enum ModelType 17 | { 18 | KANNALA_BRANDT, 19 | MEI, 20 | PINHOLE, 21 | SCARAMUZZA 22 | }; 23 | 24 | class Parameters 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | Parameters(ModelType modelType); 29 | 30 | Parameters(ModelType modelType, const std::string& cameraName, 31 | int w, int h); 32 | 33 | ModelType& modelType(void); 34 | std::string& cameraName(void); 35 | int& imageWidth(void); 36 | int& imageHeight(void); 37 | 38 | ModelType modelType(void) const; 39 | const std::string& cameraName(void) const; 40 | int imageWidth(void) const; 41 | int imageHeight(void) const; 42 | 43 | int nIntrinsics(void) const; 44 | 45 | virtual bool readFromYamlFile(const std::string& filename) = 0; 46 | virtual void writeToYamlFile(const std::string& filename) const = 0; 47 | 48 | protected: 49 | ModelType m_modelType; 50 | int m_nIntrinsics; 51 | std::string m_cameraName; 52 | int m_imageWidth; 53 | int m_imageHeight; 54 | }; 55 | 56 | virtual ModelType modelType(void) const = 0; 57 | virtual const std::string& cameraName(void) const = 0; 58 | virtual int imageWidth(void) const = 0; 59 | virtual int imageHeight(void) const = 0; 60 | 61 | virtual cv::Mat& mask(void); 62 | virtual const cv::Mat& mask(void) const; 63 | 64 | virtual void estimateIntrinsics(const cv::Size& boardSize, 65 | const std::vector< std::vector >& objectPoints, 66 | const std::vector< std::vector >& imagePoints) = 0; 67 | virtual void estimateExtrinsics(const std::vector& objectPoints, 68 | const std::vector& imagePoints, 69 | cv::Mat& rvec, cv::Mat& tvec) const; 70 | 71 | // Lift points from the image plane to the sphere 72 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 73 | //%output P 74 | 75 | // Lift points from the image plane to the projective space 76 | virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 77 | //%output P 78 | 79 | // Projects 3D points to the image plane (Pi function) 80 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; 81 | //%output p 82 | 83 | // Projects 3D points to the image plane (Pi function) 84 | // and calculates jacobian 85 | //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 86 | // Eigen::Matrix& J) const = 0; 87 | //%output p 88 | //%output J 89 | 90 | virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; 91 | //%output p 92 | 93 | //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; 94 | virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 95 | float fx = -1.0f, float fy = -1.0f, 96 | cv::Size imageSize = cv::Size(0, 0), 97 | float cx = -1.0f, float cy = -1.0f, 98 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; 99 | 100 | virtual int parameterCount(void) const = 0; 101 | 102 | virtual void readParameters(const std::vector& parameters) = 0; 103 | virtual void writeParameters(std::vector& parameters) const = 0; 104 | 105 | virtual void writeParametersToYamlFile(const std::string& filename) const = 0; 106 | 107 | virtual std::string parametersToString(void) const = 0; 108 | 109 | /** 110 | * \brief Calculates the reprojection distance between points 111 | * 112 | * \param P1 first 3D point coordinates 113 | * \param P2 second 3D point coordinates 114 | * \return euclidean distance in the plane 115 | */ 116 | double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; 117 | 118 | double reprojectionError(const std::vector< std::vector >& objectPoints, 119 | const std::vector< std::vector >& imagePoints, 120 | const std::vector& rvecs, 121 | const std::vector& tvecs, 122 | cv::OutputArray perViewErrors = cv::noArray()) const; 123 | 124 | double reprojectionError(const Eigen::Vector3d& P, 125 | const Eigen::Quaterniond& camera_q, 126 | const Eigen::Vector3d& camera_t, 127 | const Eigen::Vector2d& observed_p) const; 128 | 129 | void projectPoints(const std::vector& objectPoints, 130 | const cv::Mat& rvec, 131 | const cv::Mat& tvec, 132 | std::vector& imagePoints) const; 133 | protected: 134 | cv::Mat m_mask; 135 | }; 136 | 137 | typedef boost::shared_ptr CameraPtr; 138 | typedef boost::shared_ptr CameraConstPtr; 139 | 140 | } 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | #include "CameraFactory.h" 2 | 3 | #include 4 | 5 | 6 | #include "CataCamera.h" 7 | #include "EquidistantCamera.h" 8 | #include "PinholeCamera.h" 9 | #include "ScaramuzzaCamera.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | namespace camodocal 14 | { 15 | 16 | boost::shared_ptr CameraFactory::m_instance; 17 | 18 | CameraFactory::CameraFactory() 19 | { 20 | 21 | } 22 | 23 | boost::shared_ptr 24 | CameraFactory::instance(void) 25 | { 26 | if (m_instance.get() == 0) 27 | { 28 | m_instance.reset(new CameraFactory); 29 | } 30 | 31 | return m_instance; 32 | } 33 | 34 | CameraPtr 35 | CameraFactory::generateCamera(Camera::ModelType modelType, 36 | const std::string& cameraName, 37 | cv::Size imageSize) const 38 | { 39 | switch (modelType) 40 | { 41 | case Camera::KANNALA_BRANDT: 42 | { 43 | EquidistantCameraPtr camera(new EquidistantCamera); 44 | 45 | EquidistantCamera::Parameters params = camera->getParameters(); 46 | params.cameraName() = cameraName; 47 | params.imageWidth() = imageSize.width; 48 | params.imageHeight() = imageSize.height; 49 | camera->setParameters(params); 50 | return camera; 51 | } 52 | case Camera::PINHOLE: 53 | { 54 | PinholeCameraPtr camera(new PinholeCamera); 55 | 56 | PinholeCamera::Parameters params = camera->getParameters(); 57 | params.cameraName() = cameraName; 58 | params.imageWidth() = imageSize.width; 59 | params.imageHeight() = imageSize.height; 60 | camera->setParameters(params); 61 | return camera; 62 | } 63 | case Camera::SCARAMUZZA: 64 | { 65 | OCAMCameraPtr camera(new OCAMCamera); 66 | 67 | OCAMCamera::Parameters params = camera->getParameters(); 68 | params.cameraName() = cameraName; 69 | params.imageWidth() = imageSize.width; 70 | params.imageHeight() = imageSize.height; 71 | camera->setParameters(params); 72 | return camera; 73 | } 74 | case Camera::MEI: 75 | default: 76 | { 77 | CataCameraPtr camera(new CataCamera); 78 | 79 | CataCamera::Parameters params = camera->getParameters(); 80 | params.cameraName() = cameraName; 81 | params.imageWidth() = imageSize.width; 82 | params.imageHeight() = imageSize.height; 83 | camera->setParameters(params); 84 | return camera; 85 | } 86 | } 87 | } 88 | 89 | CameraPtr 90 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 91 | { 92 | cv::FileStorage fs(filename, cv::FileStorage::READ); 93 | 94 | if (!fs.isOpened()) 95 | { 96 | return CameraPtr(); 97 | } 98 | 99 | Camera::ModelType modelType = Camera::MEI; 100 | if (!fs["model_type"].isNone()) 101 | { 102 | std::string sModelType; 103 | fs["model_type"] >> sModelType; 104 | 105 | if (boost::iequals(sModelType, "kannala_brandt")) 106 | { 107 | modelType = Camera::KANNALA_BRANDT; 108 | } 109 | else if (boost::iequals(sModelType, "mei")) 110 | { 111 | modelType = Camera::MEI; 112 | } 113 | else if (boost::iequals(sModelType, "scaramuzza")) 114 | { 115 | modelType = Camera::SCARAMUZZA; 116 | } 117 | else if (boost::iequals(sModelType, "pinhole")) 118 | { 119 | modelType = Camera::PINHOLE; 120 | } 121 | else 122 | { 123 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 124 | return CameraPtr(); 125 | } 126 | } 127 | 128 | switch (modelType) 129 | { 130 | case Camera::KANNALA_BRANDT: 131 | { 132 | EquidistantCameraPtr camera(new EquidistantCamera); 133 | 134 | EquidistantCamera::Parameters params = camera->getParameters(); 135 | params.readFromYamlFile(filename); 136 | camera->setParameters(params); 137 | return camera; 138 | } 139 | case Camera::PINHOLE: 140 | { 141 | PinholeCameraPtr camera(new PinholeCamera); 142 | 143 | PinholeCamera::Parameters params = camera->getParameters(); 144 | params.readFromYamlFile(filename); 145 | camera->setParameters(params); 146 | return camera; 147 | } 148 | case Camera::SCARAMUZZA: 149 | { 150 | OCAMCameraPtr camera(new OCAMCamera); 151 | 152 | OCAMCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | case Camera::MEI: 158 | default: 159 | { 160 | CataCameraPtr camera(new CataCamera); 161 | 162 | CataCamera::Parameters params = camera->getParameters(); 163 | params.readFromYamlFile(filename); 164 | camera->setParameters(params); 165 | return camera; 166 | } 167 | } 168 | 169 | return CameraPtr(); 170 | } 171 | 172 | } 173 | 174 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "Camera.h" 8 | 9 | namespace camodocal 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camodocal 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef PINHOLECAMERA_H 2 | #define PINHOLECAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camodocal 11 | { 12 | 13 | class PinholeCamera: public Camera 14 | { 15 | public: 16 | class Parameters: public Camera::Parameters 17 | { 18 | public: 19 | Parameters(); 20 | Parameters(const std::string& cameraName, 21 | int w, int h, 22 | double k1, double k2, double p1, double p2, 23 | double fx, double fy, double cx, double cy); 24 | 25 | double& k1(void); 26 | double& k2(void); 27 | double& p1(void); 28 | double& p2(void); 29 | double& fx(void); 30 | double& fy(void); 31 | double& cx(void); 32 | double& cy(void); 33 | 34 | double xi(void) const; 35 | double k1(void) const; 36 | double k2(void) const; 37 | double p1(void) const; 38 | double p2(void) const; 39 | double fx(void) const; 40 | double fy(void) const; 41 | double cx(void) const; 42 | double cy(void) const; 43 | 44 | bool readFromYamlFile(const std::string& filename); 45 | void writeToYamlFile(const std::string& filename) const; 46 | 47 | Parameters& operator=(const Parameters& other); 48 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 49 | 50 | private: 51 | double m_k1; 52 | double m_k2; 53 | double m_p1; 54 | double m_p2; 55 | double m_fx; 56 | double m_fy; 57 | double m_cx; 58 | double m_cy; 59 | }; 60 | 61 | PinholeCamera(); 62 | 63 | /** 64 | * \brief Constructor from the projection model parameters 65 | */ 66 | PinholeCamera(const std::string& cameraName, 67 | int imageWidth, int imageHeight, 68 | double k1, double k2, double p1, double p2, 69 | double fx, double fy, double cx, double cy); 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | PinholeCamera(const Parameters& params); 74 | 75 | Camera::ModelType modelType(void) const; 76 | const std::string& cameraName(void) const; 77 | int imageWidth(void) const; 78 | int imageHeight(void) const; 79 | 80 | void estimateIntrinsics(const cv::Size& boardSize, 81 | const std::vector< std::vector >& objectPoints, 82 | const std::vector< std::vector >& imagePoints); 83 | 84 | // Lift points from the image plane to the sphere 85 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 86 | //%output P 87 | 88 | // Lift points from the image plane to the projective space 89 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 90 | //%output P 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 94 | //%output p 95 | 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const; 100 | //%output p 101 | //%output J 102 | 103 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 104 | //%output p 105 | 106 | template 107 | static void spaceToPlane(const T* const params, 108 | const T* const q, const T* const t, 109 | const Eigen::Matrix& P, 110 | Eigen::Matrix& p); 111 | 112 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 113 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 114 | Eigen::Matrix2d& J) const; 115 | 116 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 117 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 118 | float fx = -1.0f, float fy = -1.0f, 119 | cv::Size imageSize = cv::Size(0, 0), 120 | float cx = -1.0f, float cy = -1.0f, 121 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 122 | 123 | int parameterCount(void) const; 124 | 125 | const Parameters& getParameters(void) const; 126 | void setParameters(const Parameters& parameters); 127 | 128 | void readParameters(const std::vector& parameterVec); 129 | void writeParameters(std::vector& parameterVec) const; 130 | 131 | void writeParametersToYamlFile(const std::string& filename) const; 132 | 133 | std::string parametersToString(void) const; 134 | 135 | private: 136 | Parameters mParameters; 137 | 138 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 139 | bool m_noDistortion; 140 | }; 141 | 142 | typedef boost::shared_ptr PinholeCameraPtr; 143 | typedef boost::shared_ptr PinholeCameraConstPtr; 144 | 145 | template 146 | void 147 | PinholeCamera::spaceToPlane(const T* const params, 148 | const T* const q, const T* const t, 149 | const Eigen::Matrix& P, 150 | Eigen::Matrix& p) 151 | { 152 | T P_w[3]; 153 | P_w[0] = T(P(0)); 154 | P_w[1] = T(P(1)); 155 | P_w[2] = T(P(2)); 156 | 157 | // Convert quaternion from Eigen convention (x, y, z, w) 158 | // to Ceres convention (w, x, y, z) 159 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 160 | 161 | T P_c[3]; 162 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 163 | 164 | P_c[0] += t[0]; 165 | P_c[1] += t[1]; 166 | P_c[2] += t[2]; 167 | 168 | // project 3D object point to the image plane 169 | T k1 = params[0]; 170 | T k2 = params[1]; 171 | T p1 = params[2]; 172 | T p2 = params[3]; 173 | T fx = params[4]; 174 | T fy = params[5]; 175 | T alpha = T(0); //cameraParams.alpha(); 176 | T cx = params[6]; 177 | T cy = params[7]; 178 | 179 | // Transform to model plane 180 | T u = P_c[0] / P_c[2]; 181 | T v = P_c[1] / P_c[2]; 182 | 183 | T rho_sqr = u * u + v * v; 184 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 185 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 186 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 187 | 188 | u = L * u + du; 189 | v = L * v + dv; 190 | p(0) = fx * (u + alpha * v) + cx; 191 | p(1) = fy * v + cy; 192 | } 193 | 194 | } 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/camera_models/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camodocal 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC_0; 4 | std::string IMAGE_TOPIC_1; 5 | std::string IMAGE_TOPIC_2; 6 | std::string IMU_TOPIC; 7 | std::string POINT_CLOUD_TOPIC; 8 | std::string PROJECT_NAME; 9 | 10 | std::vector CAM_NAMES; 11 | std::string FISHEYE_MASK; 12 | int MAX_CNT; 13 | int MIN_DIST; 14 | int WINDOW_SIZE; 15 | int FREQ; 16 | double F_THRESHOLD; 17 | int SHOW_TRACK; 18 | int STEREO_TRACK; 19 | int EQUALIZE; 20 | int ROW; 21 | int COL; 22 | int FOCAL_LENGTH; 23 | int FISHEYE; 24 | bool PUB_THIS_FRAME; 25 | 26 | int NUM_OF_CAM; 27 | 28 | double C_L_TX; 29 | double C_L_TY; 30 | double C_L_TZ; 31 | double C_L_RX; 32 | double C_L_RY; 33 | double C_L_RZ; 34 | 35 | double C_L_TX1; 36 | double C_L_TY1; 37 | double C_L_TZ1; 38 | double C_L_RX1; 39 | double C_L_RY1; 40 | double C_L_RZ1; 41 | 42 | double C_L_TX2; 43 | double C_L_TY2; 44 | double C_L_TZ2; 45 | double C_L_RX2; 46 | double C_L_RY2; 47 | double C_L_RZ2; 48 | 49 | int USE_LIDAR; 50 | int LIDAR_SKIP; 51 | 52 | 53 | std::vector extRotV_lidar2imu; 54 | std::vector extTransV_lidar2imu; 55 | Eigen::Matrix3d extRot_lidar2imu; 56 | Eigen::Vector3d extTrans_lidar2imu; 57 | 58 | void readParameters(ros::NodeHandle &n) 59 | { 60 | // lidar parameters by sbq 61 | n.param("/PROJECT_NAME", PROJECT_NAME, "sam"); 62 | n.param>(PROJECT_NAME+ "/extrinsicRot", extRotV_lidar2imu, std::vector()); 63 | n.param>(PROJECT_NAME+ "/extrinsicTrans", extTransV_lidar2imu, std::vector()); 64 | extRot_lidar2imu = Eigen::Map>(extRotV_lidar2imu.data(), 3, 3); 65 | extTrans_lidar2imu = Eigen::Map>(extTransV_lidar2imu.data(), 3, 1); 66 | 67 | n.param(PROJECT_NAME+ "/NUM_OF_CAM", NUM_OF_CAM, 1); 68 | 69 | std::string config_file, config_file1, config_file2; 70 | n.getParam("vins_config_file", config_file); 71 | n.getParam("vins_config_file_1", config_file1); 72 | n.getParam("vins_config_file_2", config_file2); 73 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 74 | cv::FileStorage fsSettings1(config_file1, cv::FileStorage::READ); 75 | cv::FileStorage fsSettings2(config_file2, cv::FileStorage::READ); 76 | 77 | 78 | 79 | if(!fsSettings.isOpened() || !fsSettings1.isOpened() || !fsSettings2.isOpened()) 80 | { 81 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 82 | } 83 | 84 | // project name 85 | fsSettings["project_name"] >> PROJECT_NAME; 86 | std::string pkg_path = ros::package::getPath(PROJECT_NAME); 87 | 88 | // sensor topics 89 | fsSettings["image_topic"] >> IMAGE_TOPIC_0; 90 | fsSettings1["image_topic"] >> IMAGE_TOPIC_1; 91 | fsSettings2["image_topic"] >> IMAGE_TOPIC_2; 92 | fsSettings["imu_topic"] >> IMU_TOPIC; 93 | fsSettings["point_cloud_topic"] >> POINT_CLOUD_TOPIC; 94 | 95 | // lidar configurations 96 | fsSettings["use_lidar"] >> USE_LIDAR; 97 | fsSettings["lidar_skip"] >> LIDAR_SKIP; 98 | 99 | // feature and image settings 100 | MAX_CNT = fsSettings["max_cnt"]; 101 | MIN_DIST = fsSettings["min_dist"]; 102 | ROW = fsSettings["image_height"]; 103 | COL = fsSettings["image_width"]; 104 | FREQ = fsSettings["freq"]; 105 | F_THRESHOLD = fsSettings["F_threshold"]; 106 | SHOW_TRACK = fsSettings["show_track"]; 107 | EQUALIZE = fsSettings["equalize"]; 108 | 109 | C_L_TX = fsSettings["cam_to_lidar_tx"]; 110 | C_L_TY = fsSettings["cam_to_lidar_ty"]; 111 | C_L_TZ = fsSettings["cam_to_lidar_tz"]; 112 | C_L_RX = fsSettings["cam_to_lidar_rx"]; 113 | C_L_RY = fsSettings["cam_to_lidar_ry"]; 114 | C_L_RZ = fsSettings["cam_to_lidar_rz"]; 115 | 116 | C_L_TX1 = fsSettings1["cam_to_lidar_tx"]; 117 | C_L_TY1 = fsSettings1["cam_to_lidar_ty"]; 118 | C_L_TZ1 = fsSettings1["cam_to_lidar_tz"]; 119 | C_L_RX1 = fsSettings1["cam_to_lidar_rx"]; 120 | C_L_RY1 = fsSettings1["cam_to_lidar_ry"]; 121 | C_L_RZ1 = fsSettings1["cam_to_lidar_rz"]; 122 | 123 | C_L_TX2 = fsSettings2["cam_to_lidar_tx"]; 124 | C_L_TY2 = fsSettings2["cam_to_lidar_ty"]; 125 | C_L_TZ2 = fsSettings2["cam_to_lidar_tz"]; 126 | C_L_RX2 = fsSettings2["cam_to_lidar_rx"]; 127 | C_L_RY2 = fsSettings2["cam_to_lidar_ry"]; 128 | C_L_RZ2 = fsSettings2["cam_to_lidar_rz"]; 129 | 130 | // fisheye mask 131 | FISHEYE = fsSettings["fisheye"]; 132 | if (FISHEYE == 1) 133 | { 134 | std::string mask_name; 135 | fsSettings["fisheye_mask"] >> mask_name; 136 | FISHEYE_MASK = pkg_path + mask_name; 137 | } 138 | 139 | // camera config 140 | CAM_NAMES.push_back(config_file); 141 | CAM_NAMES.push_back(config_file1); 142 | CAM_NAMES.push_back(config_file2); 143 | 144 | WINDOW_SIZE = 20; 145 | STEREO_TRACK = false; 146 | FOCAL_LENGTH = 460; 147 | PUB_THIS_FRAME = false; 148 | 149 | if (FREQ == 0) 150 | FREQ = 100; 151 | 152 | fsSettings.release(); 153 | fsSettings1.release(); 154 | fsSettings2.release(); 155 | usleep(100); 156 | } 157 | 158 | float pointDistance(PointType p) 159 | { 160 | return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); 161 | } 162 | 163 | float pointDistance(PointType p1, PointType p2) 164 | { 165 | return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); 166 | } 167 | 168 | void publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) 169 | { 170 | if (thisPub->getNumSubscribers() == 0) 171 | return; 172 | sensor_msgs::PointCloud2 tempCloud; 173 | pcl::toROSMsg(*thisCloud, tempCloud); 174 | tempCloud.header.stamp = thisStamp; 175 | tempCloud.header.frame_id = thisFrame; 176 | thisPub->publish(tempCloud); 177 | } -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 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 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | using namespace std; 59 | 60 | typedef pcl::PointXYZI PointType; 61 | 62 | 63 | 64 | extern int ROW; 65 | extern int COL; 66 | extern int FOCAL_LENGTH; 67 | // TODO: move this to yaml 68 | const int NUM_OF_CAM_ALL = 2; 69 | extern int NUM_OF_CAM; 70 | extern int num_of_cam; 71 | 72 | 73 | extern std::string PROJECT_NAME; 74 | extern std::string IMAGE_TOPIC_0; 75 | extern std::string IMAGE_TOPIC_1; 76 | extern std::string IMAGE_TOPIC_2; 77 | extern std::string IMU_TOPIC; 78 | extern std::string POINT_CLOUD_TOPIC; 79 | 80 | extern int USE_LIDAR; 81 | extern int LIDAR_SKIP; 82 | 83 | extern std::string FISHEYE_MASK; 84 | extern std::vector CAM_NAMES; 85 | extern int MAX_CNT; 86 | extern int MIN_DIST; 87 | extern int WINDOW_SIZE; 88 | extern int FREQ; 89 | extern double F_THRESHOLD; 90 | extern int SHOW_TRACK; 91 | extern int STEREO_TRACK; 92 | extern int EQUALIZE; 93 | extern int FISHEYE; 94 | extern bool PUB_THIS_FRAME; 95 | 96 | extern double C_L_TX; 97 | extern double C_L_TY; 98 | extern double C_L_TZ; 99 | extern double C_L_RX; 100 | extern double C_L_RY; 101 | extern double C_L_RZ; 102 | 103 | extern double C_L_TX1; 104 | extern double C_L_TY1; 105 | extern double C_L_TZ1; 106 | extern double C_L_RX1; 107 | extern double C_L_RY1; 108 | extern double C_L_RZ1; 109 | 110 | extern double C_L_TX2; 111 | extern double C_L_TY2; 112 | extern double C_L_TZ2; 113 | extern double C_L_RX2; 114 | extern double C_L_RY2; 115 | extern double C_L_RZ2; 116 | 117 | extern Eigen::Matrix3d extRot_lidar2imu; 118 | extern Eigen::Vector3d extTrans_lidar2imu; 119 | 120 | 121 | void readParameters(ros::NodeHandle &n); 122 | 123 | float pointDistance(PointType p); 124 | 125 | float pointDistance(PointType p1, PointType p2); 126 | 127 | void publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame); -------------------------------------------------------------------------------- /src/visual_odometry/visual_feature/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/DBoW2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DBoW2.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: Generic include file for the DBoW2 classes and 6 | * the specialized vocabularies and databases 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DBoW2 Library 12 | * 13 | * DBoW2 library for C++: 14 | * Bag-of-word image database for image retrieval. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://doriangalvez.com 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries, 23 | * as well as the boost::dynamic_bitset class. 24 | * 25 | * \section citation Citation 26 | * If you use this software in academic works, please cite: 27 | 
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |
41 | * 42 | * \section license License 43 | * This file is licensed under a Creative Commons 44 | * Attribution-NonCommercial-ShareAlike 3.0 license. 45 | * This file can be freely used and users can use, download and edit this file 46 | * provided that credit is attributed to the original author. No users are 47 | * permitted to use this file for commercial purposes unless explicit permission 48 | * is given by the original author. Derivative works must be licensed using the 49 | * same or similar license. 50 | * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further 51 | * details. 52 | * 53 | */ 54 | 55 | #ifndef __D_T_DBOW2__ 56 | #define __D_T_DBOW2__ 57 | 58 | /// Includes all the data structures to manage vocabularies and image databases 59 | namespace DBoW2 60 | { 61 | } 62 | 63 | #include "TemplatedVocabulary.h" 64 | #include "TemplatedDatabase.h" 65 | #include "BowVector.h" 66 | #include "FeatureVector.h" 67 | #include "QueryResults.h" 68 | #include "FBrief.h" 69 | 70 | /// BRIEF Vocabulary 71 | typedef DBoW2::TemplatedVocabulary 72 | BriefVocabulary; 73 | 74 | /// BRIEF Database 75 | typedef DBoW2::TemplatedDatabase 76 | BriefDatabase; 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "FBrief.h" 15 | 16 | using namespace std; 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | void FBrief::meanValue(const std::vector &descriptors, 23 | FBrief::TDescriptor &mean) 24 | { 25 | mean.reset(); 26 | 27 | if(descriptors.empty()) return; 28 | 29 | const int N2 = descriptors.size() / 2; 30 | const int L = descriptors[0]->size(); 31 | 32 | vector counters(L, 0); 33 | 34 | vector::const_iterator it; 35 | for(it = descriptors.begin(); it != descriptors.end(); ++it) 36 | { 37 | const FBrief::TDescriptor &desc = **it; 38 | for(int i = 0; i < L; ++i) 39 | { 40 | if(desc[i]) counters[i]++; 41 | } 42 | } 43 | 44 | for(int i = 0; i < L; ++i) 45 | { 46 | if(counters[i] > N2) mean.set(i); 47 | } 48 | 49 | } 50 | 51 | // -------------------------------------------------------------------------- 52 | 53 | double FBrief::distance(const FBrief::TDescriptor &a, 54 | const FBrief::TDescriptor &b) 55 | { 56 | return (double)DVision::BRIEF::distance(a, b); 57 | } 58 | 59 | // -------------------------------------------------------------------------- 60 | 61 | std::string FBrief::toString(const FBrief::TDescriptor &a) 62 | { 63 | // from boost::bitset 64 | string s; 65 | to_string(a, s); // reversed 66 | return s; 67 | } 68 | 69 | // -------------------------------------------------------------------------- 70 | 71 | void FBrief::fromString(FBrief::TDescriptor &a, const std::string &s) 72 | { 73 | // from boost::bitset 74 | stringstream ss(s); 75 | ss >> a; 76 | } 77 | 78 | // -------------------------------------------------------------------------- 79 | 80 | void FBrief::toMat32F(const std::vector &descriptors, 81 | cv::Mat &mat) 82 | { 83 | if(descriptors.empty()) 84 | { 85 | mat.release(); 86 | return; 87 | } 88 | 89 | const int N = descriptors.size(); 90 | const int L = descriptors[0].size(); 91 | 92 | mat.create(N, L, CV_32F); 93 | 94 | for(int i = 0; i < N; ++i) 95 | { 96 | const TDescriptor& desc = descriptors[i]; 97 | float *p = mat.ptr(i); 98 | for(int j = 0; j < L; ++j, ++p) 99 | { 100 | *p = (desc[j] ? 1 : 0); 101 | } 102 | } 103 | } 104 | 105 | // -------------------------------------------------------------------------- 106 | 107 | } // namespace DBoW2 108 | 109 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_BRIEF__ 11 | #define __D_T_F_BRIEF__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | #include "../DVision/DVision.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Functions to manipulate BRIEF descriptors 23 | class FBrief: protected FClass 24 | { 25 | public: 26 | 27 | typedef DVision::BRIEF::bitset TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | static void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean); 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | 68 | }; 69 | 70 | } // namespace DBoW2 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.cpp 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include "QueryResults.h" 13 | 14 | using namespace std; 15 | 16 | namespace DBoW2 17 | { 18 | 19 | // --------------------------------------------------------------------------- 20 | 21 | ostream & operator<<(ostream& os, const Result& ret ) 22 | { 23 | os << ""; 24 | return os; 25 | } 26 | 27 | // --------------------------------------------------------------------------- 28 | 29 | ostream & operator<<(ostream& os, const QueryResults& ret ) 30 | { 31 | if(ret.size() == 1) 32 | os << "1 result:" << endl; 33 | else 34 | os << ret.size() << " results:" << endl; 35 | 36 | QueryResults::const_iterator rit; 37 | for(rit = ret.begin(); rit != ret.end(); ++rit) 38 | { 39 | os << *rit; 40 | if(rit + 1 != ret.end()) os << endl; 41 | } 42 | return os; 43 | } 44 | 45 | // --------------------------------------------------------------------------- 46 | 47 | void QueryResults::saveM(const std::string &filename) const 48 | { 49 | fstream f(filename.c_str(), ios::out); 50 | 51 | QueryResults::const_iterator qit; 52 | for(qit = begin(); qit != end(); ++qit) 53 | { 54 | f << qit->Id << " " << qit->Score << endl; 55 | } 56 | 57 | f.close(); 58 | } 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | } // namespace DBoW2 63 | 64 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.h 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_QUERY_RESULTS__ 11 | #define __D_T_QUERY_RESULTS__ 12 | 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | /// Id of entries of the database 18 | typedef unsigned int EntryId; 19 | 20 | /// Single result of a query 21 | class Result 22 | { 23 | public: 24 | 25 | /// Entry id 26 | EntryId Id; 27 | 28 | /// Score obtained 29 | double Score; 30 | 31 | /// debug 32 | int nWords; // words in common 33 | // !!! this is filled only by Bhatt score! 34 | // (and for BCMatching, BCThresholding then) 35 | 36 | double bhatScore, chiScore; 37 | /// debug 38 | 39 | // only done by ChiSq and BCThresholding 40 | double sumCommonVi; 41 | double sumCommonWi; 42 | double expectedChiScore; 43 | /// debug 44 | 45 | /** 46 | * Empty constructors 47 | */ 48 | inline Result(){} 49 | 50 | /** 51 | * Creates a result with the given data 52 | * @param _id entry id 53 | * @param _score score 54 | */ 55 | inline Result(EntryId _id, double _score): Id(_id), Score(_score){} 56 | 57 | /** 58 | * Compares the scores of two results 59 | * @return true iff this.score < r.score 60 | */ 61 | inline bool operator<(const Result &r) const 62 | { 63 | return this->Score < r.Score; 64 | } 65 | 66 | /** 67 | * Compares the scores of two results 68 | * @return true iff this.score > r.score 69 | */ 70 | inline bool operator>(const Result &r) const 71 | { 72 | return this->Score > r.Score; 73 | } 74 | 75 | /** 76 | * Compares the entry id of the result 77 | * @return true iff this.id == id 78 | */ 79 | inline bool operator==(EntryId id) const 80 | { 81 | return this->Id == id; 82 | } 83 | 84 | /** 85 | * Compares the score of this entry with a given one 86 | * @param s score to compare with 87 | * @return true iff this score < s 88 | */ 89 | inline bool operator<(double s) const 90 | { 91 | return this->Score < s; 92 | } 93 | 94 | /** 95 | * Compares the score of this entry with a given one 96 | * @param s score to compare with 97 | * @return true iff this score > s 98 | */ 99 | inline bool operator>(double s) const 100 | { 101 | return this->Score > s; 102 | } 103 | 104 | /** 105 | * Compares the score of two results 106 | * @param a 107 | * @param b 108 | * @return true iff a.Score > b.Score 109 | */ 110 | static inline bool gt(const Result &a, const Result &b) 111 | { 112 | return a.Score > b.Score; 113 | } 114 | 115 | /** 116 | * Compares the scores of two results 117 | * @return true iff a.Score > b.Score 118 | */ 119 | inline static bool ge(const Result &a, const Result &b) 120 | { 121 | return a.Score > b.Score; 122 | } 123 | 124 | /** 125 | * Returns true iff a.Score >= b.Score 126 | * @param a 127 | * @param b 128 | * @return true iff a.Score >= b.Score 129 | */ 130 | static inline bool geq(const Result &a, const Result &b) 131 | { 132 | return a.Score >= b.Score; 133 | } 134 | 135 | /** 136 | * Returns true iff a.Score >= s 137 | * @param a 138 | * @param s 139 | * @return true iff a.Score >= s 140 | */ 141 | static inline bool geqv(const Result &a, double s) 142 | { 143 | return a.Score >= s; 144 | } 145 | 146 | 147 | /** 148 | * Returns true iff a.Id < b.Id 149 | * @param a 150 | * @param b 151 | * @return true iff a.Id < b.Id 152 | */ 153 | static inline bool ltId(const Result &a, const Result &b) 154 | { 155 | return a.Id < b.Id; 156 | } 157 | 158 | /** 159 | * Prints a string version of the result 160 | * @param os ostream 161 | * @param ret Result to print 162 | */ 163 | friend std::ostream & operator<<(std::ostream& os, const Result& ret ); 164 | }; 165 | 166 | /// Multiple results from a query 167 | class QueryResults: public std::vector 168 | { 169 | public: 170 | 171 | /** 172 | * Multiplies all the scores in the vector by factor 173 | * @param factor 174 | */ 175 | inline void scaleScores(double factor); 176 | 177 | /** 178 | * Prints a string version of the results 179 | * @param os ostream 180 | * @param ret QueryResults to print 181 | */ 182 | friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret ); 183 | 184 | /** 185 | * Saves a matlab file with the results 186 | * @param filename 187 | */ 188 | void saveM(const std::string &filename) const; 189 | 190 | }; 191 | 192 | // -------------------------------------------------------------------------- 193 | 194 | inline void QueryResults::scaleScores(double factor) 195 | { 196 | for(QueryResults::iterator qit = begin(); qit != end(); ++qit) 197 | qit->Score *= factor; 198 | } 199 | 200 | // -------------------------------------------------------------------------- 201 | 202 | } // namespace TemplatedBoW 203 | 204 | #endif 205 | 206 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DBoW/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | }; 45 | 46 | /** 47 | * Macro for defining Scoring classes 48 | * @param NAME name of class 49 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 50 | * @param NORM type of norm to use when MUSTNORMALIZE 51 | */ 52 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 53 | NAME: public GeneralScoring \ 54 | { public: \ 55 | /** \ 56 | * Computes score between two vectors \ 57 | * @param v \ 58 | * @param w \ 59 | * @return score between v and w \ 60 | */ \ 61 | virtual double score(const BowVector &v, const BowVector &w) const; \ 62 | \ 63 | /** \ 64 | * Says if a vector must be normalized according to the scoring function \ 65 | * @param norm (out) if true, norm to use 66 | * @return true iff vectors must be normalized \ 67 | */ \ 68 | virtual inline bool mustNormalize(LNorm &norm) const \ 69 | { norm = NORM; return MUSTNORMALIZE; } \ 70 | } 71 | 72 | /// L1 Scoring object 73 | class __SCORING_CLASS(L1Scoring, true, L1); 74 | 75 | /// L2 Scoring object 76 | class __SCORING_CLASS(L2Scoring, true, L2); 77 | 78 | /// Chi square Scoring object 79 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 80 | 81 | /// KL divergence Scoring object 82 | class __SCORING_CLASS(KLScoring, true, L1); 83 | 84 | /// Bhattacharyya Scoring object 85 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 86 | 87 | /// Dot product Scoring object 88 | class __SCORING_CLASS(DotProductScoring, false, L1); 89 | 90 | #undef __SCORING_CLASS 91 | 92 | } // namespace DBoW2 93 | 94 | #endif 95 | 96 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DUtils/DException.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DException.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: general exception of the library 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #ifndef __D_EXCEPTION__ 14 | #define __D_EXCEPTION__ 15 | 16 | #include 17 | #include 18 | using namespace std; 19 | 20 | namespace DUtils { 21 | 22 | /// General exception 23 | class DException : 24 | public exception 25 | { 26 | public: 27 | /** 28 | * Creates an exception with a general error message 29 | */ 30 | DException(void) throw(): m_message("DUtils exception"){} 31 | 32 | /** 33 | * Creates an exception with a custom error message 34 | * @param msg: message 35 | */ 36 | DException(const char *msg) throw(): m_message(msg){} 37 | 38 | /** 39 | * Creates an exception with a custom error message 40 | * @param msg: message 41 | */ 42 | DException(const string &msg) throw(): m_message(msg){} 43 | 44 | /** 45 | * Destructor 46 | */ 47 | virtual ~DException(void) throw(){} 48 | 49 | /** 50 | * Returns the exception message 51 | */ 52 | virtual const char* what() const throw() 53 | { 54 | return m_message.c_str(); 55 | } 56 | 57 | protected: 58 | /// Error message 59 | string m_message; 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | 66 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DUtils/DUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DUtils.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: include file for including all the library functionalities 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DUtils Library 12 | * 13 | * DUtils library for C++: 14 | * Collection of classes with general utilities for C++ applications. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section license License 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU Lesser General Public License (LGPL) as 24 | * published by the Free Software Foundation, either version 3 of the License, 25 | * or any later version. 26 | * 27 | */ 28 | 29 | 30 | #pragma once 31 | 32 | #ifndef __D_UTILS__ 33 | #define __D_UTILS__ 34 | 35 | /// Several utilities for C++ programs 36 | namespace DUtils 37 | { 38 | } 39 | 40 | // Exception 41 | #include "DException.h" 42 | 43 | #include "Timestamp.h" 44 | // Random numbers 45 | #include "Random.h" 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/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 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BRIEF.cpp 3 | * Author: Dorian Galvez 4 | * Date: September 2010 5 | * Description: implementation of BRIEF (Binary Robust Independent 6 | * Elementary Features) descriptor by 7 | * Michael Calonder, Vincent Lepetitand Pascal Fua 8 | * + close binary tests (by Dorian Galvez-Lopez) 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include "BRIEF.h" 14 | #include "../DUtils/DUtils.h" 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace DVision; 20 | 21 | // ---------------------------------------------------------------------------- 22 | 23 | BRIEF::BRIEF(int nbits, int patch_size, Type type): 24 | m_bit_length(nbits), m_patch_size(patch_size), m_type(type) 25 | { 26 | assert(patch_size > 1); 27 | assert(nbits > 0); 28 | generateTestPoints(); 29 | } 30 | 31 | // ---------------------------------------------------------------------------- 32 | 33 | BRIEF::~BRIEF() 34 | { 35 | } 36 | 37 | // --------------------------------------------------------------------------- 38 | 39 | void BRIEF::compute(const cv::Mat &image, 40 | const std::vector &points, 41 | vector &descriptors, 42 | bool treat_image) const 43 | { 44 | const float sigma = 2.f; 45 | const cv::Size ksize(9, 9); 46 | 47 | cv::Mat im; 48 | if(treat_image) 49 | { 50 | cv::Mat aux; 51 | if(image.depth() == 3) 52 | { 53 | cv::cvtColor(image, aux, CV_RGB2GRAY); 54 | } 55 | else 56 | { 57 | aux = image; 58 | } 59 | 60 | cv::GaussianBlur(aux, im, ksize, sigma, sigma); 61 | 62 | } 63 | else 64 | { 65 | im = image; 66 | } 67 | 68 | assert(im.type() == CV_8UC1); 69 | assert(im.isContinuous()); 70 | 71 | // use im now 72 | const int W = im.cols; 73 | const int H = im.rows; 74 | 75 | descriptors.resize(points.size()); 76 | std::vector::iterator dit; 77 | 78 | std::vector::const_iterator kit; 79 | 80 | int x1, y1, x2, y2; 81 | 82 | dit = descriptors.begin(); 83 | for(kit = points.begin(); kit != points.end(); ++kit, ++dit) 84 | { 85 | dit->resize(m_bit_length); 86 | dit->reset(); 87 | 88 | for(unsigned int i = 0; i < m_x1.size(); ++i) 89 | { 90 | x1 = (int)(kit->pt.x + m_x1[i]); 91 | y1 = (int)(kit->pt.y + m_y1[i]); 92 | x2 = (int)(kit->pt.x + m_x2[i]); 93 | y2 = (int)(kit->pt.y + m_y2[i]); 94 | 95 | if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H 96 | && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H) 97 | { 98 | if( im.ptr(y1)[x1] < im.ptr(y2)[x2] ) 99 | { 100 | dit->set(i); 101 | } 102 | } // if (x,y)_1 and (x,y)_2 are in the image 103 | 104 | } // for each (x,y) 105 | } // for each keypoint 106 | } 107 | 108 | // --------------------------------------------------------------------------- 109 | 110 | void BRIEF::generateTestPoints() 111 | { 112 | m_x1.resize(m_bit_length); 113 | m_y1.resize(m_bit_length); 114 | m_x2.resize(m_bit_length); 115 | m_y2.resize(m_bit_length); 116 | 117 | const float g_mean = 0.f; 118 | const float g_sigma = 0.2f * (float)m_patch_size; 119 | const float c_sigma = 0.08f * (float)m_patch_size; 120 | 121 | float sigma2; 122 | if(m_type == RANDOM) 123 | sigma2 = g_sigma; 124 | else 125 | sigma2 = c_sigma; 126 | 127 | const int max_v = m_patch_size / 2; 128 | 129 | DUtils::Random::SeedRandOnce(); 130 | 131 | for(int i = 0; i < m_bit_length; ++i) 132 | { 133 | int x1, y1, x2, y2; 134 | 135 | do 136 | { 137 | x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 138 | } while( x1 > max_v || x1 < -max_v); 139 | 140 | do 141 | { 142 | y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma); 143 | } while( y1 > max_v || y1 < -max_v); 144 | 145 | float meanx, meany; 146 | if(m_type == RANDOM) 147 | meanx = meany = g_mean; 148 | else 149 | { 150 | meanx = x1; 151 | meany = y1; 152 | } 153 | 154 | do 155 | { 156 | x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2); 157 | } while( x2 > max_v || x2 < -max_v); 158 | 159 | do 160 | { 161 | y2 = DUtils::Random::RandomGaussianValue(meany, sigma2); 162 | } while( y2 > max_v || y2 < -max_v); 163 | 164 | m_x1[i] = x1; 165 | m_y1[i] = y1; 166 | m_x2[i] = x2; 167 | m_y2[i] = y2; 168 | } 169 | 170 | } 171 | 172 | // ---------------------------------------------------------------------------- 173 | 174 | 175 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BRIEF.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2011 5 | * Description: implementation of BRIEF (Binary Robust Independent 6 | * Elementary Features) descriptor by 7 | * Michael Calonder, Vincent Lepetit and Pascal Fua 8 | * + close binary tests (by Dorian Galvez-Lopez) 9 | * 10 | * If you use this code with the RANDOM_CLOSE descriptor version, please cite: 11 | @INPROCEEDINGS{GalvezIROS11, 12 | author={Galvez-Lopez, Dorian and Tardos, Juan D.}, 13 | booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on}, 14 | title={Real-time loop detection with bags of binary words}, 15 | year={2011}, 16 | month={sept.}, 17 | volume={}, 18 | number={}, 19 | pages={51 -58}, 20 | keywords={}, 21 | doi={10.1109/IROS.2011.6094885}, 22 | ISSN={2153-0858} 23 | } 24 | * 25 | * License: see the LICENSE.txt file 26 | * 27 | */ 28 | 29 | #ifndef __D_BRIEF__ 30 | #define __D_BRIEF__ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace DVision { 37 | 38 | /// BRIEF descriptor 39 | class BRIEF 40 | { 41 | public: 42 | 43 | /// Bitset type 44 | typedef boost::dynamic_bitset<> bitset; 45 | 46 | /// Type of pairs 47 | enum Type 48 | { 49 | RANDOM, // random pairs (Calonder's original version) 50 | RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11) 51 | }; 52 | 53 | public: 54 | 55 | /** 56 | * Creates the BRIEF a priori data for descriptors of nbits length 57 | * @param nbits descriptor length in bits 58 | * @param patch_size 59 | * @param type type of pairs to generate 60 | */ 61 | BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE); 62 | virtual ~BRIEF(); 63 | 64 | /** 65 | * Returns the descriptor length in bits 66 | * @return descriptor length in bits 67 | */ 68 | inline int getDescriptorLengthInBits() const 69 | { 70 | return m_bit_length; 71 | } 72 | 73 | /** 74 | * Returns the type of classifier 75 | */ 76 | inline Type getType() const 77 | { 78 | return m_type; 79 | } 80 | 81 | /** 82 | * Returns the size of the patch 83 | */ 84 | inline int getPatchSize() const 85 | { 86 | return m_patch_size; 87 | } 88 | 89 | /** 90 | * Returns the BRIEF descriptors of the given keypoints in the given image 91 | * @param image 92 | * @param points 93 | * @param descriptors 94 | * @param treat_image (default: true) if true, the image is converted to 95 | * grayscale if needed and smoothed. If not, it is assumed the image has 96 | * been treated by the user 97 | * @note this function is similar to BRIEF::compute 98 | */ 99 | inline void operator() (const cv::Mat &image, 100 | const std::vector &points, 101 | std::vector &descriptors, 102 | bool treat_image = true) const 103 | { 104 | compute(image, points, descriptors, treat_image); 105 | } 106 | 107 | /** 108 | * Returns the BRIEF descriptors of the given keypoints in the given image 109 | * @param image 110 | * @param points 111 | * @param descriptors 112 | * @param treat_image (default: true) if true, the image is converted to 113 | * grayscale if needed and smoothed. If not, it is assumed the image has 114 | * been treated by the user 115 | * @note this function is similar to BRIEF::operator() 116 | */ 117 | void compute(const cv::Mat &image, 118 | const std::vector &points, 119 | std::vector &descriptors, 120 | bool treat_image = true) const; 121 | 122 | /** 123 | * Exports the test pattern 124 | * @param x1 x1 coordinates of pairs 125 | * @param y1 y1 coordinates of pairs 126 | * @param x2 x2 coordinates of pairs 127 | * @param y2 y2 coordinates of pairs 128 | */ 129 | inline void exportPairs(std::vector &x1, std::vector &y1, 130 | std::vector &x2, std::vector &y2) const 131 | { 132 | x1 = m_x1; 133 | y1 = m_y1; 134 | x2 = m_x2; 135 | y2 = m_y2; 136 | } 137 | 138 | /** 139 | * Sets the test pattern 140 | * @param x1 x1 coordinates of pairs 141 | * @param y1 y1 coordinates of pairs 142 | * @param x2 x2 coordinates of pairs 143 | * @param y2 y2 coordinates of pairs 144 | */ 145 | inline void importPairs(const std::vector &x1, 146 | const std::vector &y1, const std::vector &x2, 147 | const std::vector &y2) 148 | { 149 | m_x1 = x1; 150 | m_y1 = y1; 151 | m_x2 = x2; 152 | m_y2 = y2; 153 | m_bit_length = x1.size(); 154 | } 155 | 156 | /** 157 | * Returns the Hamming distance between two descriptors 158 | * @param a first descriptor vector 159 | * @param b second descriptor vector 160 | * @return hamming distance 161 | */ 162 | inline static int distance(const bitset &a, const bitset &b) 163 | { 164 | return (a^b).count(); 165 | } 166 | 167 | protected: 168 | 169 | /** 170 | * Generates random points in the patch coordinates, according to 171 | * m_patch_size and m_bit_length 172 | */ 173 | void generateTestPoints(); 174 | 175 | protected: 176 | 177 | /// Descriptor length in bits 178 | int m_bit_length; 179 | 180 | /// Patch size 181 | int m_patch_size; 182 | 183 | /// Type of pairs 184 | Type m_type; 185 | 186 | /// Coordinates of test points relative to the center of the patch 187 | std::vector m_x1, m_x2; 188 | std::vector m_y1, m_y2; 189 | 190 | }; 191 | 192 | } // namespace DVision 193 | 194 | #endif 195 | 196 | 197 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/DVision/DVision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DVision.h 3 | * Project: DVision library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 4, 2010 6 | * Description: several functions for computer vision 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DVision Library 12 | * 13 | * DVision library for C++ and OpenCV: 14 | * Collection of classes with computer vision functionality 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. 23 | * 24 | * \section license License 25 | * This program is free software: you can redistribute it and/or modify 26 | * it under the terms of the GNU Lesser General Public License (LGPL) as 27 | * published by the Free Software Foundation, either version 3 of the License, 28 | * or any later version. 29 | * 30 | */ 31 | 32 | #ifndef __D_VISION__ 33 | #define __D_VISION__ 34 | 35 | 36 | /// Computer vision functions 37 | namespace DVision 38 | { 39 | } 40 | 41 | #include "BRIEF.h" 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.cpp: -------------------------------------------------------------------------------- 1 | #include "VocabularyBinary.hpp" 2 | #include 3 | using namespace std; 4 | 5 | VINSLoop::Vocabulary::Vocabulary() 6 | : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) { 7 | } 8 | 9 | VINSLoop::Vocabulary::~Vocabulary() { 10 | if (nodes != nullptr) { 11 | delete [] nodes; 12 | nodes = nullptr; 13 | } 14 | 15 | if (words != nullptr) { 16 | delete [] words; 17 | words = nullptr; 18 | } 19 | } 20 | 21 | void VINSLoop::Vocabulary::serialize(ofstream& stream) { 22 | stream.write((const char *)this, staticDataSize()); 23 | stream.write((const char *)nodes, sizeof(Node) * nNodes); 24 | stream.write((const char *)words, sizeof(Word) * nWords); 25 | } 26 | 27 | void VINSLoop::Vocabulary::deserialize(ifstream& stream) { 28 | stream.read((char *)this, staticDataSize()); 29 | 30 | nodes = new Node[nNodes]; 31 | stream.read((char *)nodes, sizeof(Node) * nNodes); 32 | 33 | words = new Word[nWords]; 34 | stream.read((char *)words, sizeof(Word) * nWords); 35 | } 36 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VocabularyBinary_hpp 2 | #define VocabularyBinary_hpp 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace VINSLoop { 9 | 10 | struct Node { 11 | int32_t nodeId; 12 | int32_t parentId; 13 | double weight; 14 | uint64_t descriptor[4]; 15 | }; 16 | 17 | struct Word { 18 | int32_t nodeId; 19 | int32_t wordId; 20 | }; 21 | 22 | struct Vocabulary { 23 | int32_t k; 24 | int32_t L; 25 | int32_t scoringType; 26 | int32_t weightingType; 27 | 28 | int32_t nNodes; 29 | int32_t nWords; 30 | 31 | Node* nodes; 32 | Word* words; 33 | 34 | Vocabulary(); 35 | ~Vocabulary(); 36 | 37 | void serialize(std::ofstream& stream); 38 | void deserialize(std::ifstream& stream); 39 | 40 | inline static size_t staticDataSize() { 41 | return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*); 42 | } 43 | }; 44 | 45 | } 46 | 47 | #endif /* VocabularyBinary_hpp */ 48 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | 5 | #define MIN_LOOP_NUM 25 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | using namespace DVision; 10 | 11 | class KeyFrame 12 | { 13 | public: 14 | 15 | double time_stamp; 16 | int index; 17 | 18 | // Pose in vins_world 19 | Eigen::Vector3d origin_vio_T; 20 | Eigen::Matrix3d origin_vio_R; 21 | 22 | cv::Mat image; 23 | cv::Mat thumbnail; 24 | 25 | vector point_3d; 26 | vector point_2d_uv; 27 | vector point_2d_norm; 28 | vector point_id; 29 | 30 | vector keypoints; 31 | vector keypoints_norm; 32 | vector window_keypoints; 33 | 34 | vector brief_descriptors; 35 | vector window_brief_descriptors; 36 | 37 | KeyFrame(double _time_stamp, int _index, 38 | Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, 39 | cv::Mat &_image, 40 | vector &_point_3d, 41 | vector &_point_2d_uv, vector &_point_2d_normal, 42 | vector &_point_id); 43 | 44 | bool findConnection(KeyFrame* old_kf); 45 | void computeWindowBRIEFPoint(); 46 | void computeBRIEFPoint(); 47 | 48 | int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b); 49 | 50 | bool searchInAera(const BRIEF::bitset window_descriptor, 51 | const std::vector &descriptors_old, 52 | const std::vector &keypoints_old, 53 | const std::vector &keypoints_old_norm, 54 | cv::Point2f &best_match, 55 | cv::Point2f &best_match_norm); 56 | 57 | void searchByBRIEFDes(std::vector &matched_2d_old, 58 | std::vector &matched_2d_old_norm, 59 | std::vector &status, 60 | const std::vector &descriptors_old, 61 | const std::vector &keypoints_old, 62 | const std::vector &keypoints_old_norm); 63 | 64 | 65 | void PnPRANSAC(const vector &matched_2d_old_norm, 66 | const std::vector &matched_3d, 67 | std::vector &status); 68 | }; -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/loop_detection.cpp: -------------------------------------------------------------------------------- 1 | #include "loop_detection.h" 2 | 3 | LoopDetector::LoopDetector(){} 4 | 5 | 6 | void LoopDetector::loadVocabulary(std::string voc_path) 7 | { 8 | voc = new BriefVocabulary(voc_path); 9 | db.setVocabulary(*voc, false, 0); 10 | } 11 | 12 | void LoopDetector::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) 13 | { 14 | int loop_index = -1; 15 | if (flag_detect_loop) 16 | { 17 | loop_index = detectLoop(cur_kf, cur_kf->index); 18 | } 19 | else 20 | { 21 | addKeyFrameIntoVoc(cur_kf); 22 | } 23 | 24 | // check loop if valid using ransan and pnp 25 | if (loop_index != -1) 26 | { 27 | KeyFrame* old_kf = getKeyFrame(loop_index); 28 | 29 | if (cur_kf->findConnection(old_kf)) 30 | { 31 | std_msgs::Float64MultiArray match_msg; 32 | match_msg.data.push_back(cur_kf->time_stamp); 33 | match_msg.data.push_back(old_kf->time_stamp); 34 | pub_match_msg.publish(match_msg); 35 | } 36 | } 37 | 38 | // add keyframe 39 | keyframelist.push_back(cur_kf); 40 | } 41 | 42 | KeyFrame* LoopDetector::getKeyFrame(int index) 43 | { 44 | list::iterator it = keyframelist.begin(); 45 | for (; it != keyframelist.end(); it++) 46 | { 47 | if((*it)->index == index) 48 | break; 49 | } 50 | if (it != keyframelist.end()) 51 | return *it; 52 | else 53 | return NULL; 54 | } 55 | 56 | int LoopDetector::detectLoop(KeyFrame* keyframe, int frame_index) 57 | { 58 | // put image into image_pool; for visualization 59 | cv::Mat compressed_image; 60 | if (DEBUG_IMAGE) 61 | { 62 | int feature_num = keyframe->keypoints.size(); 63 | cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); 64 | putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); 65 | image_pool[frame_index] = compressed_image; 66 | } 67 | //first query; then add this frame into database! 68 | QueryResults ret; 69 | db.query(keyframe->brief_descriptors, ret, 4, frame_index - 200); 70 | //printf("query time: %f", t_query.toc()); 71 | //cout << "Searching for Image " << frame_index << ". " << ret << endl; 72 | 73 | db.add(keyframe->brief_descriptors); 74 | //printf("add feature time: %f", t_add.toc()); 75 | // ret[0] is the nearest neighbour's score. threshold change with neighour score 76 | 77 | cv::Mat loop_result; 78 | if (DEBUG_IMAGE) 79 | { 80 | loop_result = compressed_image.clone(); 81 | if (ret.size() > 0) 82 | putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); 83 | } 84 | // visual loop result 85 | if (DEBUG_IMAGE) 86 | { 87 | for (unsigned int i = 0; i < ret.size(); i++) 88 | { 89 | int tmp_index = ret[i].Id; 90 | auto it = image_pool.find(tmp_index); 91 | cv::Mat tmp_image = (it->second).clone(); 92 | putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); 93 | cv::hconcat(loop_result, tmp_image, loop_result); 94 | } 95 | } 96 | // a good match with its nerghbour 97 | bool find_loop = false; 98 | if (ret.size() >= 1 && ret[0].Score > 0.05) 99 | { 100 | for (unsigned int i = 1; i < ret.size(); i++) 101 | { 102 | //if (ret[i].Score > ret[0].Score * 0.3) 103 | if (ret[i].Score > 0.015) 104 | { 105 | find_loop = true; 106 | 107 | if (DEBUG_IMAGE && 0) 108 | { 109 | int tmp_index = ret[i].Id; 110 | auto it = image_pool.find(tmp_index); 111 | cv::Mat tmp_image = (it->second).clone(); 112 | putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); 113 | cv::hconcat(loop_result, tmp_image, loop_result); 114 | } 115 | } 116 | 117 | } 118 | } 119 | 120 | if (DEBUG_IMAGE) 121 | { 122 | cv::imshow("loop_result", loop_result); 123 | cv::waitKey(20); 124 | } 125 | 126 | if (find_loop && frame_index > 50) 127 | { 128 | int min_index = -1; 129 | for (unsigned int i = 0; i < ret.size(); i++) 130 | { 131 | if (min_index == -1 || ((int)ret[i].Id < min_index && ret[i].Score > 0.015)) 132 | min_index = ret[i].Id; 133 | } 134 | return min_index; 135 | } 136 | else 137 | return -1; 138 | 139 | } 140 | 141 | void LoopDetector::addKeyFrameIntoVoc(KeyFrame* keyframe) 142 | { 143 | // put image into image_pool; for visualization 144 | cv::Mat compressed_image; 145 | if (DEBUG_IMAGE) 146 | { 147 | int feature_num = keyframe->keypoints.size(); 148 | cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); 149 | putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); 150 | image_pool[keyframe->index] = compressed_image; 151 | } 152 | 153 | db.add(keyframe->brief_descriptors); 154 | } 155 | 156 | void LoopDetector::visualizeKeyPoses(double time_cur) 157 | { 158 | if (keyframelist.empty() || pub_key_pose.getNumSubscribers() == 0) 159 | return; 160 | 161 | visualization_msgs::MarkerArray markerArray; 162 | 163 | int count = 0; 164 | int count_lim = 10; 165 | 166 | visualization_msgs::Marker markerNode; 167 | markerNode.header.frame_id = "vins_world"; 168 | markerNode.header.stamp = ros::Time().fromSec(time_cur); 169 | markerNode.action = visualization_msgs::Marker::ADD; 170 | markerNode.type = visualization_msgs::Marker::SPHERE_LIST; 171 | markerNode.ns = "keyframe_nodes"; 172 | markerNode.id = 0; 173 | markerNode.pose.orientation.w = 1; 174 | markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3; 175 | markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1; 176 | markerNode.color.a = 1; 177 | 178 | for (list::reverse_iterator rit = keyframelist.rbegin(); rit != keyframelist.rend(); ++rit) 179 | { 180 | if (count++ > count_lim) 181 | break; 182 | 183 | geometry_msgs::Point p; 184 | p.x = (*rit)->origin_vio_T.x(); 185 | p.y = (*rit)->origin_vio_T.y(); 186 | p.z = (*rit)->origin_vio_T.z(); 187 | markerNode.points.push_back(p); 188 | } 189 | 190 | markerArray.markers.push_back(markerNode); 191 | pub_key_pose.publish(markerArray); 192 | } -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/loop_detection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "parameters.h" 4 | #include "keyframe.h" 5 | 6 | using namespace DVision; 7 | using namespace DBoW2; 8 | 9 | class LoopDetector 10 | { 11 | public: 12 | 13 | BriefDatabase db; 14 | BriefVocabulary* voc; 15 | 16 | map image_pool; 17 | 18 | list keyframelist; 19 | 20 | LoopDetector(); 21 | void loadVocabulary(std::string voc_path); 22 | 23 | void addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop); 24 | void addKeyFrameIntoVoc(KeyFrame* keyframe); 25 | KeyFrame* getKeyFrame(int index); 26 | 27 | void visualizeKeyPoses(double time_cur); 28 | 29 | int detectLoop(KeyFrame* keyframe, int frame_index); 30 | }; 31 | -------------------------------------------------------------------------------- /src/visual_odometry/visual_loop/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include "ThirdParty/DBoW/DBoW2.h" 64 | #include "ThirdParty/DVision/DVision.h" 65 | #include "ThirdParty/DBoW/TemplatedDatabase.h" 66 | #include "ThirdParty/DBoW/TemplatedVocabulary.h" 67 | 68 | #include "../visual_feature/camera_models/CameraFactory.h" 69 | #include "../visual_feature/camera_models/CataCamera.h" 70 | #include "../visual_feature/camera_models/PinholeCamera.h" 71 | 72 | using namespace std; 73 | 74 | extern camodocal::CameraPtr m_camera; 75 | 76 | extern Eigen::Vector3d tic; 77 | extern Eigen::Matrix3d qic; 78 | 79 | extern string PROJECT_NAME; 80 | extern string IMAGE_TOPIC_0; 81 | 82 | extern int DEBUG_IMAGE; 83 | extern int LOOP_CLOSURE; 84 | extern double MATCH_IMAGE_SCALE; 85 | 86 | extern ros::Publisher pub_match_img; 87 | extern ros::Publisher pub_match_msg; 88 | extern ros::Publisher pub_key_pose; 89 | 90 | 91 | 92 | 93 | 94 | 95 | class BriefExtractor 96 | { 97 | public: 98 | 99 | DVision::BRIEF m_brief; 100 | 101 | virtual void operator()(const cv::Mat &im, vector &keys, vector &descriptors) const 102 | { 103 | m_brief.compute(im, keys, descriptors); 104 | } 105 | 106 | BriefExtractor(){}; 107 | 108 | BriefExtractor(const std::string &pattern_file) 109 | { 110 | // The DVision::BRIEF extractor computes a random pattern by default when 111 | // the object is created. 112 | // We load the pattern that we used to build the vocabulary, to make 113 | // the descriptors compatible with the predefined vocabulary 114 | 115 | // loads the pattern 116 | cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ); 117 | if(!fs.isOpened()) throw string("Could not open file ") + pattern_file; 118 | 119 | vector x1, y1, x2, y2; 120 | fs["x1"] >> x1; 121 | fs["x2"] >> x2; 122 | fs["y1"] >> y1; 123 | fs["y2"] >> y2; 124 | 125 | m_brief.importPairs(x1, y1, x2, y2); 126 | } 127 | }; 128 | 129 | extern BriefExtractor briefExtractor; --------------------------------------------------------------------------------