├── CMakeLists.txt ├── README.md ├── config ├── brief_k10L6.bin ├── brief_pattern.yml ├── daheng │ └── daheng_config_no_extrinsic.yaml ├── euroc │ └── euroc_config.yaml ├── fisheye_mask.jpg ├── iphone │ ├── iphone_config.yaml │ └── iphone_config_wind.yaml ├── kitti │ └── kitti.yaml └── mynteye │ └── mynteye_config.yaml ├── launch ├── config │ ├── robot.urdf.xacro │ └── vins_rviz_config.rviz ├── module_global.launch ├── module_loam.launch ├── module_rviz.launch ├── module_visual.launch └── run_fusion.launch ├── package.xml └── src ├── global_fusion ├── Factors.h ├── ThirdParty │ └── GeographicLib │ │ ├── CMakeLists.txt │ │ ├── include │ │ ├── Config.h │ │ ├── Constants.hpp │ │ ├── Geocentric.hpp │ │ ├── LocalCartesian.hpp │ │ └── Math.hpp │ │ └── src │ │ ├── Geocentric.cpp │ │ ├── LocalCartesian.cpp │ │ └── Math.cpp ├── globalOpt.cpp ├── globalOpt.h ├── globalOptNode.cpp ├── models │ ├── car.dae │ └── hummingbird.mesh └── tic_toc.h ├── lidar_loam ├── common.h ├── laserMapping.cpp ├── laserOdometry.cpp ├── lidarFactor.hpp ├── scanRegistration.cpp ├── segment │ ├── pointsCorrect.cpp │ ├── pointsCorrect.hpp │ ├── segment.cpp │ └── segment.hpp └── tic_toc.h └── visual_inertial ├── feature_tracker ├── 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_mask.cpp ├── parameters.cpp ├── parameters.h └── tic_toc.h ├── pose_graph ├── 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 ├── parameters.h ├── pose_graph.cpp ├── pose_graph.h ├── pose_graph_node.cpp └── utility │ ├── CameraPoseVisualization.cpp │ ├── CameraPoseVisualization.h │ ├── tic_toc.h │ ├── utility.cpp │ └── utility.h └── vins_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 /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(MultiSensor_fusion) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS "-std=c++11") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread -w") 8 | 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cv_bridge 12 | geometry_msgs 13 | message_generation 14 | message_runtime 15 | nav_msgs 16 | roscpp 17 | rospy 18 | sensor_msgs 19 | std_msgs 20 | tf 21 | roslib 22 | ) 23 | 24 | find_package(PCL REQUIRED) 25 | find_package(OpenCV REQUIRED) 26 | find_package(Ceres REQUIRED) 27 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 28 | 29 | 30 | ########### 31 | ## Build ## 32 | ########### 33 | catkin_package() 34 | 35 | add_subdirectory("src/global_fusion/ThirdParty/GeographicLib/") 36 | 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${CERES_INCLUDE_DIRS} 42 | ${Boost_INCLUDE_DIRS} 43 | "src/global_fusion/ThirdParty/GeographicLib/include/" 44 | ) 45 | # link_directories( 46 | # ${PCL_LIBRARY_DIRS} 47 | # ${OpenCV_LIBRARY_DIRS} 48 | # ) 49 | 50 | ## Visual_Inertial 51 | file(GLOB visual_feature_files 52 | "src/visual_inertial/feature_tracker/*.cpp" 53 | "src/visual_inertial/feature_tracker/camera_models/*.cc" 54 | ) 55 | 56 | file(GLOB visual_estimator_files 57 | "src/visual_inertial/vins_estimator/*.cpp" 58 | "src/visual_inertial/vins_estimator/factor/*.cpp" 59 | "src/visual_inertial/vins_estimator/initial/*.cpp" 60 | "src/visual_inertial/vins_estimator/utility/*.cpp" 61 | ) 62 | 63 | file(GLOB pose_graph_files 64 | "src/visual_inertial/pose_graph/*.cpp" 65 | "src/visual_inertial/pose_graph/utility/*.cpp" 66 | "src/visual_inertial/pose_graph/ThirdParty/*.cpp" 67 | "src/visual_inertial/pose_graph/ThirdParty/DBoW/*.cpp" 68 | "src/visual_inertial/pose_graph/ThirdParty/DUtils/*.cpp" 69 | "src/visual_inertial/pose_graph/ThirdParty/DVision/*.cpp" 70 | "src/visual_inertial/feature_tracker/camera_models/*.cc" 71 | ) 72 | 73 | ###Global_Fusion 74 | file(GLOB global_fusion_files 75 | "src/global_fusion/*.cpp" 76 | ) 77 | 78 | ####Visual_Inertial 79 | 80 | # visual feature 81 | add_executable(${PROJECT_NAME}_visual_feature ${visual_feature_files}) 82 | target_link_libraries(${PROJECT_NAME}_visual_feature ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 83 | 84 | # visual eatimator 85 | add_executable(${PROJECT_NAME}_visual_eatimator ${visual_estimator_files}) 86 | target_link_libraries(${PROJECT_NAME}_visual_eatimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 87 | 88 | # pose graph 89 | add_executable(${PROJECT_NAME}_pose_graph ${pose_graph_files}) 90 | target_link_libraries(${PROJECT_NAME}_pose_graph ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 91 | 92 | 93 | ####Lidar_Odometory 94 | # scan Registration 95 | file(GLOB scan_registration_files 96 | "src/lidar_loam/scanRegistration.cpp" 97 | "src/lidar_loam/segment/segment.cpp" 98 | "src/lidar_loam/segment/pointsCorrect.cpp" 99 | ) 100 | add_executable(${PROJECT_NAME}_scanRegistration ${scan_registration_files}) 101 | target_link_libraries(${PROJECT_NAME}_scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 102 | 103 | # laser Odometry 104 | add_executable(${PROJECT_NAME}_laserOdometry src/lidar_loam/laserOdometry.cpp) 105 | target_link_libraries(${PROJECT_NAME}_laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 106 | 107 | # laser Mapping 108 | add_executable(${PROJECT_NAME}_laserMapping src/lidar_loam/laserMapping.cpp) 109 | target_link_libraries(${PROJECT_NAME}_laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 110 | 111 | 112 | ###Global_Fusion 113 | 114 | add_executable(${PROJECT_NAME}_global_fusion ${global_fusion_files}) 115 | target_link_libraries(${PROJECT_NAME}_global_fusion ${catkin_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBS} libGeographiccc) 116 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MultiSensor_fusion 2 | 3 | **说明:使用视觉-惯性和纯激光里程计,通过因子图后处理全局位姿。其中平移的权重激光比例高,旋转的权重视觉-惯性比例高** 4 | 5 | **其中视觉前端使用mask-rcnn剔除动态特征点,激光前端采用lio_livox中的前景检测算法实现对动态点云的剔除,保证构建地图的精度** 6 | 7 | **传感器类型:大恒工业相机、Xsens惯性传感器、VLP-32激光雷达、松灵机器人底盘** 8 | 9 | ##Compilation 10 | 11 | cd ~/catkin_ws/src 12 | 13 | git clone https://github.com/GuoFeng-X/MultiSensor_fusion.git 14 | 15 | cd .. 16 | 17 | catkin_make 18 | 19 | ## Usage 20 | ### 1. 视觉前端运行 21 | 下载基于ROS话题发布的mask-rcnn前端代码。[mask-rcnn ros版本](https://download.csdn.net/download/qq_37568167/36765493) 22 | 23 | 拷贝上述代码到MultiSensor_fusion文件夹下 24 | 25 | cd catkin_ws/src/MultiSensor_fusion 26 | 27 | unzip Mask-RCNN 28 | 29 | cd script/mask_rcnn 30 | 31 | ./run_build.sh 32 | 33 | cd .. 34 | 35 | ./run_detect.sh 36 | 37 | ## 2. 视觉惯性里程计和激光里程计 38 | source devel/setup.bash 39 | 40 | roslaunch MultiSensor_fusion run_fusion.launch 41 | 42 | ## 3. 运行自己的数据集 43 | rosbag play xxx.bag --pause -r0.5 44 | 45 | 46 | ## Acknowledgements 47 | Thanks for following work: 48 | 49 | LOAM (LOAM: Lidar Odometry and Mapping in Real-time) 50 | 51 | VINS-Mono (VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator) 52 | -------------------------------------------------------------------------------- /config/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RichExplor/MultiSensor_fusion/c99f087082d9f1cf0edd1a16d8f29efef12d6143/config/brief_k10L6.bin -------------------------------------------------------------------------------- /config/daheng/daheng_config_no_extrinsic.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu/data" 5 | image_topic: "/galaxy_camera/image" 6 | output_path: "/home/lenovo/output/r2live" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 608 12 | image_height: 512 13 | distortion_parameters: 14 | k1: -0.10083195928893594 15 | k2: 0.07286601655353565 16 | p1: 0.0027750228530210998 17 | p2: 0.002709917974816327 18 | projection_parameters: 19 | fx: 369.6509500162731 20 | fy: 369.5130197849706 21 | cx: 314.51542051873264 22 | cy: 258.9054802015086 23 | 24 | 25 | # Extrinsic parameter between IMU and Camera. 26 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 27 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 28 | # 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. 29 | #If you choose 0 or 1, you should write down the following matrix. 30 | #Rotation from camera frame to imu frame, imu^R_cam 31 | extrinsicRotation: !!opencv-matrix 32 | rows: 3 33 | cols: 3 34 | dt: d 35 | data: [-0.01787795, -0.00545498, 0.9998253, 36 | -0.99981167, -0.00745345, -0.01791837, 37 | 0.00754989, -0.99995734, -0.0053207] 38 | #Translation from camera frame to imu frame, imu^T_cam 39 | extrinsicTranslation: !!opencv-matrix 40 | rows: 3 41 | cols: 1 42 | dt: d 43 | data: [0.04043489, -0.00593765, -0.01431643] 44 | 45 | #feature traker paprameters 46 | max_cnt: 200 # max feature number in feature tracking 47 | min_dist: 25 # min distance between two features 48 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 49 | F_threshold: 1.0 # ransac threshold (pixel) 50 | show_track: 1 # publish tracking image as topic 51 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 52 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 53 | 54 | #optimization parameters 55 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 56 | max_num_iterations: 8 # max solver itrations, to guarantee real time 57 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 58 | 59 | 60 | #imu parameters The more accurate parameters you provide, the better performance 61 | acc_n: 8.1204402909973231e-03 # accelerometer measurement noise standard deviation. #0.2 62 | gyr_n: 7.7340868706229941e-03 # gyroscope measurement noise standard deviation. #0.05 63 | acc_w: 1.4392145235071912e-04 # accelerometer bias random work noise standard deviation. #0.02 64 | gyr_w: 8.5869684731412183e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 65 | g_norm: 9.79754 # gravity magnitude 66 | 67 | #loop closure parameters 68 | loop_closure: 1 # start loop closure 69 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 70 | fast_relocalization: 0 # useful in real-time and large project 71 | pose_graph_save_path: "/home/lenovo/output/r2live/" # save and load path 72 | 73 | #unsynchronization parameters 74 | estimate_td: 0 # online estimate time offset between camera and imu 75 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 76 | 77 | #rolling shutter parameters 78 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 79 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 80 | 81 | #visualization parameters 82 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 83 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 84 | visualize_camera_size: 0.4 # size of camera marker in RVIZ -------------------------------------------------------------------------------- /config/euroc/euroc_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/lenovo/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 752 12 | image_height: 480 13 | distortion_parameters: 14 | k1: -2.917e-01 15 | k2: 8.228e-02 16 | p1: 5.333e-05 17 | p2: -1.578e-04 18 | projection_parameters: 19 | fx: 4.616e+02 20 | fy: 4.603e+02 21 | cx: 3.630e+02 22 | cy: 2.481e+02 23 | 24 | # Extrinsic parameter between IMU and Camera. 25 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 26 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 27 | # 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. 28 | #If you choose 0 or 1, you should write down the following matrix. 29 | #Rotation from camera frame to imu frame, imu^R_cam 30 | extrinsicRotation: !!opencv-matrix 31 | rows: 3 32 | cols: 3 33 | dt: d 34 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, 35 | 0.999557249008, 0.0149672133247, 0.025715529948, 36 | -0.0257744366974, 0.00375618835797, 0.999660727178] 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.0216401454975,-0.064676986768, 0.00981073058949] 43 | 44 | #feature traker paprameters 45 | max_cnt: 150 # max feature number in feature tracking 46 | min_dist: 30 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.81007 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/lenovo/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /config/fisheye_mask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RichExplor/MultiSensor_fusion/c99f087082d9f1cf0edd1a16d8f29efef12d6143/config/fisheye_mask.jpg -------------------------------------------------------------------------------- /config/iphone/iphone_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/lenovo/output/ADVIO/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 720 12 | image_height: 1280 13 | distortion_parameters: 14 | k1: 0.0478 15 | k2: 0.0339 16 | p1: -0.00033 17 | p2: -0.00091 18 | projection_parameters: 19 | fx: 1077.2 20 | fy: 1079.3 21 | cx: 362.145 22 | cy: 636.3873 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 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. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [0.9999763379093255, -0.004079205042965442, -0.005539287650170447, 34 | -0.004066386342107199, -0.9999890330121858, 0.0023234365646622014, 35 | -0.00554870467502187, -0.0023008567036498766, -0.9999819588046867] 36 | 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.008977668364731128, 0.07557012320238939, -0.005545773942541918] 43 | 44 | #feature traker paprameters 45 | max_cnt: 250 # max feature number in feature tracking 46 | min_dist: 15 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 4.8e-03 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 2.4e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 2.1e-04 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 5.1e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.79754 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/lenovo/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 0.013 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /config/iphone/iphone_config_wind.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu0" 5 | image_topic: "/cam0/image_raw" 6 | output_path: "/home/lenovo/output/ADVIO/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 720 12 | image_height: 1280 13 | distortion_parameters: 14 | k1: 0.0478 15 | k2: 0.0339 16 | p1: -0.00033 17 | p2: -0.00091 18 | projection_parameters: 19 | fx: 1077.2 20 | fy: 1079.3 21 | cx: 362.145 22 | cy: 636.3873 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 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. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [0.9999763379093255, -0.004079205042965442, -0.005539287650170447, 34 | -0.004066386342107199, -0.9999890330121858, 0.0023234365646622014, 35 | -0.00554870467502187, -0.0023008567036498766, -0.9999819588046867] 36 | 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.008977668364731128, 0.07557012320238939, -0.005545773942541918] 43 | 44 | #feature traker paprameters 45 | max_cnt: 250 # max feature number in feature tracking 46 | min_dist: 15 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 4.8e-03 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 2.4e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 2.1e-04 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 5.1e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.79754 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/lenovo/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 0.013 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /config/kitti/kitti.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/imu_raw" 5 | image_topic: "/kitti/camera_gray_left/image_raw" 6 | output_path: "/home/lenovo/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 1226 12 | image_height: 370 13 | distortion_parameters: 14 | k1: 0.0 15 | k2: 0.0 16 | p1: 0.0 17 | p2: 0.0 18 | projection_parameters: 19 | fx: 7.070912e+02 20 | fy: 7.070912e+02 21 | cx: 6.018873e+02 22 | cy: 1.831104e+02 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 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. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [0.00781297, -0.0042792, 0.99996, 34 | -0.999859, -0.014868, 0.00774856, 35 | 0.0148343, -0.99988, -0.00439476] 36 | 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [1.1439, -0.312718, 0.726546] 43 | 44 | #feature traker paprameters 45 | max_cnt: 200 # max feature number in feature tracking 46 | min_dist: 20 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 2.1657228523252730e-02 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 2.1923690143745844e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 3.8153871149178200e-03 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 1.4221215955051228e-04 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.79754 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/lenovo/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /config/mynteye/mynteye_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #common parameters 4 | imu_topic: "/mynteye/imu/data_raw" 5 | image_topic: "/mynteye/left/image_color" 6 | output_path: "/home/lenovo/output/" 7 | 8 | #camera calibration 9 | model_type: PINHOLE 10 | camera_name: camera 11 | image_width: 640 12 | image_height: 480 13 | distortion_parameters: 14 | k1: -0.2870635986328125 15 | k2: 0.06902313232421875 16 | p1: 0.000362396240234375 17 | p2: 0.000701904296875 18 | projection_parameters: 19 | fx: 349.199951171875 20 | fy: 349.199951171875 21 | cx: 322.2005615234375 22 | cy: 246.161865234375 23 | # Extrinsic parameter between IMU and Camera. 24 | estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 25 | # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. 26 | # 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. 27 | #If you choose 0 or 1, you should write down the following matrix. 28 | #Rotation from camera frame to imu frame, imu^R_cam 29 | extrinsicRotation: !!opencv-matrix 30 | rows: 3 31 | cols: 3 32 | dt: d 33 | data: [0.999894, -0.0105982, 0.0100214, 34 | -0.0106528, -0.999929, 0.0054139, 35 | 0.00996336, -0.00552008, -0.999935] 36 | 37 | #Translation from camera frame to imu frame, imu^T_cam 38 | extrinsicTranslation: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [-0.0487123, 0.00717749, -0.0292568] 43 | 44 | #feature traker paprameters 45 | max_cnt: 150 # max feature number in feature tracking 46 | min_dist: 30 # min distance between two features 47 | freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 48 | F_threshold: 1.0 # ransac threshold (pixel) 49 | show_track: 1 # publish tracking image as topic 50 | equalize: 1 # if image is too dark or light, trun on equalize to find enough features 51 | fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points 52 | 53 | #optimization parameters 54 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 55 | max_num_iterations: 8 # max solver itrations, to guarantee real time 56 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 57 | 58 | #imu parameters The more accurate parameters you provide, the better performance 59 | acc_n: 2.1657228523252730e-02 # accelerometer measurement noise standard deviation. #0.2 0.04 60 | gyr_n: 2.1923690143745844e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 61 | acc_w: 3.8153871149178200e-03 # accelerometer bias random work noise standard deviation. #0.02 62 | gyr_w: 1.4221215955051228e-04 # gyroscope bias random work noise standard deviation. #4.0e-5 63 | g_norm: 9.79754 # gravity magnitude 64 | 65 | #loop closure parameters 66 | loop_closure: 1 # start loop closure 67 | load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' 68 | fast_relocalization: 0 # useful in real-time and large project 69 | pose_graph_save_path: "/home/lenovo/output/pose_graph/" # save and load path 70 | 71 | #unsynchronization parameters 72 | estimate_td: 0 # online estimate time offset between camera and imu 73 | td: 0.013 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 74 | 75 | #rolling shutter parameters 76 | rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera 77 | rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet). 78 | 79 | #visualization parameters 80 | save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0 81 | visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results 82 | visualize_camera_size: 0.4 # size of camera marker in RVIZ 83 | -------------------------------------------------------------------------------- /launch/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/module_global.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/module_loam.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 | -------------------------------------------------------------------------------- /launch/module_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/module_visual.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 | -------------------------------------------------------------------------------- /launch/run_fusion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | MultiSensor_fusion 4 | 0.0.0 5 | The MultiSensor_fusion package 6 | 7 | 8 | 9 | 10 | lenovo 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | catkin 20 | 21 | cv_bridge 22 | geometry_msgs 23 | message_generation 24 | nav_msgs 25 | pcl_conversion 26 | roscpp 27 | rospy 28 | sensor_msgs 29 | std_msgs 30 | tf 31 | 32 | 33 | cv_bridge 34 | geometry_msgs 35 | message_runtime 36 | nav_msgs 37 | roscpp 38 | rospy 39 | sensor_msgs 40 | std_msgs 41 | tf 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/global_fusion/Factors.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #pragma once 13 | #include 14 | #include 15 | 16 | template inline 17 | void QuaternionInverse(const T q[4], T q_inverse[4]) 18 | { 19 | q_inverse[0] = q[0]; 20 | q_inverse[1] = -q[1]; 21 | q_inverse[2] = -q[2]; 22 | q_inverse[3] = -q[3]; 23 | }; 24 | 25 | 26 | struct TError 27 | { 28 | TError(double t_x, double t_y, double t_z, double var) 29 | :t_x(t_x), t_y(t_y), t_z(t_z), var(var){} 30 | 31 | template 32 | bool operator()(const T* ti, T* residuals) const 33 | { 34 | residuals[0] = (ti[0] - T(t_x)) / T(var); 35 | residuals[1] = (ti[1] - T(t_y)) / T(var); 36 | residuals[2] = (ti[2] - T(t_z)) / T(var); 37 | 38 | return true; 39 | } 40 | 41 | static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var) 42 | { 43 | return (new ceres::AutoDiffCostFunction< 44 | TError, 3, 3>( 45 | new TError(t_x, t_y, t_z, var))); 46 | } 47 | 48 | double t_x, t_y, t_z, var; 49 | 50 | }; 51 | 52 | struct RelativeRTError 53 | { 54 | RelativeRTError(double t_x, double t_y, double t_z, 55 | double q_w, double q_x, double q_y, double q_z, 56 | double t_var, double q_var) 57 | :t_x(t_x), t_y(t_y), t_z(t_z), 58 | q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z), 59 | t_var(t_var), q_var(q_var){} 60 | 61 | template 62 | bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const 63 | { 64 | T t_w_ij[3]; 65 | t_w_ij[0] = tj[0] - ti[0]; 66 | t_w_ij[1] = tj[1] - ti[1]; 67 | t_w_ij[2] = tj[2] - ti[2]; 68 | 69 | T i_q_w[4]; 70 | QuaternionInverse(w_q_i, i_q_w); 71 | 72 | T t_i_ij[3]; 73 | ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij); 74 | 75 | residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var); 76 | residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var); 77 | residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var); 78 | 79 | T relative_q[4]; 80 | relative_q[0] = T(q_w); 81 | relative_q[1] = T(q_x); 82 | relative_q[2] = T(q_y); 83 | relative_q[3] = T(q_z); 84 | 85 | T q_i_j[4]; 86 | ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j); 87 | 88 | T relative_q_inv[4]; 89 | QuaternionInverse(relative_q, relative_q_inv); 90 | 91 | T error_q[4]; 92 | ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 93 | 94 | residuals[3] = T(2) * error_q[1] / T(q_var); 95 | residuals[4] = T(2) * error_q[2] / T(q_var); 96 | residuals[5] = T(2) * error_q[3] / T(q_var); 97 | 98 | return true; 99 | } 100 | 101 | static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, 102 | const double q_w, const double q_x, const double q_y, const double q_z, 103 | const double t_var, const double q_var) 104 | { 105 | return (new ceres::AutoDiffCostFunction< 106 | RelativeRTError, 6, 4, 3, 4, 3>( 107 | new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var))); 108 | } 109 | 110 | double t_x, t_y, t_z, t_norm; 111 | double q_w, q_x, q_y, q_z; 112 | double t_var, q_var; 113 | 114 | }; 115 | 116 | 117 | struct RError 118 | { 119 | RError(double q_w, double q_x, double q_y, double q_z, 120 | double q_var) 121 | : q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z), 122 | q_var(q_var){} 123 | 124 | template 125 | bool operator()(const T* const w_q_i, const T* w_q_j, T* residuals) const 126 | { 127 | 128 | T i_q_w[4]; 129 | QuaternionInverse(w_q_i, i_q_w); 130 | 131 | T relative_q[4]; 132 | relative_q[0] = T(q_w); 133 | relative_q[1] = T(q_x); 134 | relative_q[2] = T(q_y); 135 | relative_q[3] = T(q_z); 136 | 137 | T q_i_j[4]; 138 | ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j); 139 | 140 | T relative_q_inv[4]; 141 | QuaternionInverse(relative_q, relative_q_inv); 142 | 143 | T error_q[4]; 144 | ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 145 | 146 | residuals[0] = T(2) * error_q[1] / T(q_var); 147 | residuals[1] = T(2) * error_q[2] / T(q_var); 148 | residuals[2] = T(2) * error_q[3] / T(q_var); 149 | 150 | return true; 151 | } 152 | 153 | static ceres::CostFunction* Create(const double q_w, const double q_x, const double q_y, const double q_z, 154 | const double q_var) 155 | { 156 | return (new ceres::AutoDiffCostFunction< 157 | RError, 3, 4, 4>( 158 | new RError( q_w, q_x, q_y, q_z, q_var))); 159 | } 160 | 161 | double q_w, q_x, q_y, q_z; 162 | double q_var; 163 | 164 | }; -------------------------------------------------------------------------------- /src/global_fusion/ThirdParty/GeographicLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project (GeographicLib) 2 | 3 | # Version information 4 | set (PROJECT_VERSION_MAJOR 1) 5 | set (PROJECT_VERSION_MINOR 49) 6 | set (PROJECT_VERSION_PATCH 0) 7 | set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") 8 | if (PROJECT_VERSION_PATCH GREATER 0) 9 | set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}") 10 | endif () 11 | 12 | # The library version tracks the numbering given by libtool in the 13 | # autoconf set up. 14 | set (LIBVERSION_API 17) 15 | set (LIBVERSION_BUILD 17.1.2) 16 | string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) 17 | string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER) 18 | 19 | cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16 20 | 21 | # (7) Set the default "real" precision. This should probably be left 22 | # at 2 (double). 23 | set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING 24 | "Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable") 25 | set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5) 26 | 27 | 28 | set (LIBNAME Geographic) 29 | 30 | include_directories( 31 | ./include/ 32 | ) 33 | 34 | add_library(libGeographiccc src/LocalCartesian.cpp 35 | src/Geocentric.cpp 36 | src/Math.cpp) -------------------------------------------------------------------------------- /src/global_fusion/ThirdParty/GeographicLib/include/Config.h: -------------------------------------------------------------------------------- 1 | // This will be overwritten by ./configure 2 | 3 | #define GEOGRAPHICLIB_VERSION_STRING "1.49" 4 | #define GEOGRAPHICLIB_VERSION_MAJOR 1 5 | #define GEOGRAPHICLIB_VERSION_MINOR 49 6 | #define GEOGRAPHICLIB_VERSION_PATCH 0 7 | 8 | // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler 9 | #define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1 10 | 11 | // Define WORDS_BIGENDIAN to be 1 if your machine is big endian 12 | /* #undef WORDS_BIGENDIAN */ 13 | -------------------------------------------------------------------------------- /src/global_fusion/ThirdParty/GeographicLib/src/LocalCartesian.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file LocalCartesian.cpp 3 | * \brief Implementation for GeographicLib::LocalCartesian class 4 | * 5 | * Copyright (c) Charles Karney (2008-2015) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #include "LocalCartesian.hpp" 11 | 12 | namespace GeographicLib { 13 | 14 | using namespace std; 15 | 16 | void LocalCartesian::Reset(real lat0, real lon0, real h0) { 17 | _lat0 = Math::LatFix(lat0); 18 | _lon0 = Math::AngNormalize(lon0); 19 | _h0 = h0; 20 | _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0); 21 | real sphi, cphi, slam, clam; 22 | Math::sincosd(_lat0, sphi, cphi); 23 | Math::sincosd(_lon0, slam, clam); 24 | Geocentric::Rotation(sphi, cphi, slam, clam, _r); 25 | } 26 | 27 | void LocalCartesian::MatrixMultiply(real M[dim2_]) const { 28 | // M = r' . M 29 | real t[dim2_]; 30 | copy(M, M + dim2_, t); 31 | for (size_t i = 0; i < dim2_; ++i) { 32 | size_t row = i / dim_, col = i % dim_; 33 | M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6]; 34 | } 35 | } 36 | 37 | void LocalCartesian::IntForward(real lat, real lon, real h, 38 | real& x, real& y, real& z, 39 | real M[dim2_]) const { 40 | real xc, yc, zc; 41 | _earth.IntForward(lat, lon, h, xc, yc, zc, M); 42 | xc -= _x0; yc -= _y0; zc -= _z0; 43 | x = _r[0] * xc + _r[3] * yc + _r[6] * zc; 44 | y = _r[1] * xc + _r[4] * yc + _r[7] * zc; 45 | z = _r[2] * xc + _r[5] * yc + _r[8] * zc; 46 | if (M) 47 | MatrixMultiply(M); 48 | } 49 | 50 | void LocalCartesian::IntReverse(real x, real y, real z, 51 | real& lat, real& lon, real& h, 52 | real M[dim2_]) const { 53 | real 54 | xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z, 55 | yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z, 56 | zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z; 57 | _earth.IntReverse(xc, yc, zc, lat, lon, h, M); 58 | if (M) 59 | MatrixMultiply(M); 60 | } 61 | 62 | } // namespace GeographicLib 63 | -------------------------------------------------------------------------------- /src/global_fusion/ThirdParty/GeographicLib/src/Math.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Math.cpp 3 | * \brief Implementation for GeographicLib::Math class 4 | * 5 | * Copyright (c) Charles Karney (2015) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #include "Math.hpp" 11 | 12 | #if defined(_MSC_VER) 13 | // Squelch warnings about constant conditional expressions 14 | # pragma warning (disable: 4127) 15 | #endif 16 | 17 | namespace GeographicLib { 18 | 19 | using namespace std; 20 | 21 | template T Math::eatanhe(T x, T es) { 22 | return es > T(0) ? es * atanh(es * x) : -es * atan(es * x); 23 | } 24 | 25 | template T Math::taupf(T tau, T es) { 26 | T tau1 = hypot(T(1), tau), 27 | sig = sinh( eatanhe(tau / tau1, es ) ); 28 | return hypot(T(1), sig) * tau - sig * tau1; 29 | } 30 | 31 | template T Math::tauf(T taup, T es) { 32 | static const int numit = 5; 33 | static const T tol = sqrt(numeric_limits::epsilon()) / T(10); 34 | T e2m = T(1) - sq(es), 35 | // To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use 36 | // tau = taup/_e2m as a starting guess. (This starting guess is the 37 | // geocentric latitude which, to first order in the flattening, is equal 38 | // to the conformal latitude.) Only 1 iteration is needed for |lat| < 39 | // 3.35 deg, otherwise 2 iterations are needed. If, instead, tau = taup 40 | // is used the mean number of iterations increases to 1.99 (2 iterations 41 | // are needed except near tau = 0). 42 | tau = taup/e2m, 43 | stol = tol * max(T(1), abs(taup)); 44 | // min iterations = 1, max iterations = 2; mean = 1.94 45 | for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) { 46 | T taupa = taupf(tau, es), 47 | dtau = (taup - taupa) * (1 + e2m * sq(tau)) / 48 | ( e2m * hypot(T(1), tau) * hypot(T(1), taupa) ); 49 | tau += dtau; 50 | if (!(abs(dtau) >= stol)) 51 | break; 52 | } 53 | return tau; 54 | } 55 | 56 | /// \cond SKIP 57 | // Instantiate 58 | template Math::real Math::eatanhe(Math::real, Math::real); 59 | template Math::real Math::taupf(Math::real, Math::real); 60 | template Math::real Math::tauf(Math::real, Math::real); 61 | /// \endcond 62 | 63 | } // namespace GeographicLib 64 | -------------------------------------------------------------------------------- /src/global_fusion/globalOpt.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | * 9 | * Author: Qin Tong (qintonguav@gmail.com) 10 | *******************************************************/ 11 | 12 | #pragma once 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "LocalCartesian.hpp" 24 | #include "tic_toc.h" 25 | 26 | using namespace std; 27 | 28 | class GlobalOptimization 29 | { 30 | public: 31 | GlobalOptimization(); 32 | ~GlobalOptimization(); 33 | void inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy); 34 | void inputLOAM(double t, double x, double y, double z, double posAccuracy); 35 | void inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ); 36 | void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ); 37 | nav_msgs::Path global_path; 38 | 39 | private: 40 | void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz); 41 | void optimize(); 42 | void updateGlobalPath(); 43 | 44 | // format t, tx,ty,tz,qw,qx,qy,qz 45 | map> localPoseMap; 46 | map> globalPoseMap; 47 | map> GPSPositionMap; 48 | bool initGPS; 49 | bool newGPS; 50 | GeographicLib::LocalCartesian geoConverter; 51 | std::mutex mPoseMap; 52 | Eigen::Matrix4d WGPS_T_WVIO; 53 | Eigen::Vector3d lastP; 54 | Eigen::Quaterniond lastQ; 55 | std::thread threadOpt; 56 | 57 | }; -------------------------------------------------------------------------------- /src/global_fusion/models/hummingbird.mesh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RichExplor/MultiSensor_fusion/c99f087082d9f1cf0edd1a16d8f29efef12d6143/src/global_fusion/models/hummingbird.mesh -------------------------------------------------------------------------------- /src/global_fusion/tic_toc.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | class TicToc 17 | { 18 | public: 19 | TicToc() 20 | { 21 | tic(); 22 | } 23 | 24 | void tic() 25 | { 26 | start = std::chrono::system_clock::now(); 27 | } 28 | 29 | double toc() 30 | { 31 | end = std::chrono::system_clock::now(); 32 | std::chrono::duration elapsed_seconds = end - start; 33 | return elapsed_seconds.count() * 1000; 34 | } 35 | 36 | private: 37 | std::chrono::time_point start, end; 38 | }; 39 | -------------------------------------------------------------------------------- /src/lidar_loam/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | #include 42 | 43 | typedef pcl::PointXYZI PointType; 44 | 45 | inline double rad2deg(double radians) 46 | { 47 | return radians * 180.0 / M_PI; 48 | } 49 | 50 | inline double deg2rad(double degrees) 51 | { 52 | return degrees * M_PI / 180.0; 53 | } 54 | -------------------------------------------------------------------------------- /src/lidar_loam/lidarFactor.hpp: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | struct LidarEdgeFactor 13 | { 14 | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 15 | Eigen::Vector3d last_point_b_, double s_) 16 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} 17 | 18 | template 19 | bool operator()(const T *q, const T *t, T *residual) const 20 | { 21 | 22 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 23 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 24 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 25 | 26 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 27 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 28 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 29 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 30 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 31 | 32 | Eigen::Matrix lp; 33 | lp = q_last_curr * cp + t_last_curr; 34 | 35 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 36 | Eigen::Matrix de = lpa - lpb; 37 | 38 | residual[0] = nu.x() / de.norm(); 39 | residual[1] = nu.y() / de.norm(); 40 | residual[2] = nu.z() / de.norm(); 41 | 42 | return true; 43 | } 44 | 45 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 46 | const Eigen::Vector3d last_point_b_, const double s_) 47 | { 48 | return (new ceres::AutoDiffCostFunction< 49 | LidarEdgeFactor, 3, 4, 3>( 50 | new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); 51 | } 52 | 53 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 54 | double s; 55 | }; 56 | 57 | struct LidarPlaneFactor 58 | { 59 | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 60 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) 61 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 62 | last_point_m(last_point_m_), s(s_) 63 | { 64 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 65 | ljm_norm.normalize(); 66 | } 67 | 68 | template 69 | bool operator()(const T *q, const T *t, T *residual) const 70 | { 71 | 72 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 73 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 74 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 75 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 76 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 77 | 78 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 79 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 80 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 81 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 82 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 83 | 84 | Eigen::Matrix lp; 85 | lp = q_last_curr * cp + t_last_curr; 86 | 87 | residual[0] = (lp - lpj).dot(ljm); 88 | 89 | return true; 90 | } 91 | 92 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 93 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 94 | const double s_) 95 | { 96 | return (new ceres::AutoDiffCostFunction< 97 | LidarPlaneFactor, 1, 4, 3>( 98 | new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); 99 | } 100 | 101 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 102 | Eigen::Vector3d ljm_norm; 103 | double s; 104 | }; 105 | 106 | struct LidarPlaneNormFactor 107 | { 108 | 109 | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 110 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 111 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 112 | 113 | template 114 | bool operator()(const T *q, const T *t, T *residual) const 115 | { 116 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 117 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 118 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 119 | Eigen::Matrix point_w; 120 | point_w = q_w_curr * cp + t_w_curr; 121 | 122 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 123 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 124 | return true; 125 | } 126 | 127 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 128 | const double negative_OA_dot_norm_) 129 | { 130 | return (new ceres::AutoDiffCostFunction< 131 | LidarPlaneNormFactor, 1, 4, 3>( 132 | new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 133 | } 134 | 135 | Eigen::Vector3d curr_point; 136 | Eigen::Vector3d plane_unit_norm; 137 | double negative_OA_dot_norm; 138 | }; 139 | 140 | 141 | struct LidarDistanceFactor 142 | { 143 | 144 | LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 145 | : curr_point(curr_point_), closed_point(closed_point_){} 146 | 147 | template 148 | bool operator()(const T *q, const T *t, T *residual) const 149 | { 150 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 151 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 152 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 153 | Eigen::Matrix point_w; 154 | point_w = q_w_curr * cp + t_w_curr; 155 | 156 | 157 | residual[0] = point_w.x() - T(closed_point.x()); 158 | residual[1] = point_w.y() - T(closed_point.y()); 159 | residual[2] = point_w.z() - T(closed_point.z()); 160 | return true; 161 | } 162 | 163 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_) 164 | { 165 | return (new ceres::AutoDiffCostFunction< 166 | LidarDistanceFactor, 3, 4, 3>( 167 | new LidarDistanceFactor(curr_point_, closed_point_))); 168 | } 169 | 170 | Eigen::Vector3d curr_point; 171 | Eigen::Vector3d closed_point; 172 | }; -------------------------------------------------------------------------------- /src/lidar_loam/segment/pointsCorrect.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _COMMON_HPP 2 | #define _CONNON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | 19 | using namespace std; 20 | 21 | typedef struct 22 | { 23 | Eigen::Matrix3f eigenVectorsPCA; 24 | Eigen::Vector3f eigenValuesPCA; 25 | std::vector neibors; 26 | } SNeiborPCA_cor; 27 | 28 | int GetNeiborPCA_cor(SNeiborPCA_cor &npca, pcl::PointCloud::Ptr cloud, pcl::KdTreeFLANN kdtree, pcl::PointXYZ searchPoint, float fSearchRadius); 29 | int FilterGndForPos_cor(float* outPoints,float*inPoints,int inNum); 30 | int CalGndPos_cor(float *gnd, float *fPoints,int pointNum,float fSearchRadius); 31 | int GetRTMatrix_cor(float *RTM, float *v0, float *v1); 32 | int CorrectPoints_cor(float *fPoints,int pointNum,float *gndPos); 33 | int GetGndPos(float *pos, float *fPoints,int pointNum); 34 | #endif 35 | -------------------------------------------------------------------------------- /src/lidar_loam/segment/segment.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SEGMENT_HPP 2 | #define _SEGMENT_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "pointsCorrect.hpp" 22 | 23 | using namespace std; 24 | 25 | #define SELF_CALI_FRAMES 20 26 | 27 | #define GND_IMG_NX 150 28 | #define GND_IMG_NY 400 29 | #define GND_IMG_DX 0.2 30 | #define GND_IMG_DY 0.2 31 | #define GND_IMG_OFFX 40 32 | #define GND_IMG_OFFY 40 33 | 34 | #define GND_IMG_NX1 24 35 | #define GND_IMG_NY1 20 36 | #define GND_IMG_DX1 4 37 | #define GND_IMG_DY1 4 38 | #define GND_IMG_OFFX1 40 39 | #define GND_IMG_OFFY1 40 40 | 41 | #define DN_SAMPLE_IMG_NX 600 //(GND_IMG_NX) 42 | #define DN_SAMPLE_IMG_NY 200 //(GND_IMG_NY) 43 | #define DN_SAMPLE_IMG_NZ 100 44 | #define DN_SAMPLE_IMG_DX 0.4 //(GND_IMG_DX) 45 | #define DN_SAMPLE_IMG_DY 0.4 //(GND_IMG_DY) 46 | #define DN_SAMPLE_IMG_DZ 0.2 47 | #define DN_SAMPLE_IMG_OFFX 40 //(GND_IMG_OFFX) 48 | #define DN_SAMPLE_IMG_OFFY 40 //(GND_IMG_OFFY) 49 | #define DN_SAMPLE_IMG_OFFZ 2.5//2.5 50 | 51 | #define FREE_ANG_NUM 360 52 | #define FREE_PI (3.14159265) 53 | #define FREE_DELTA_ANG (FREE_PI*2/FREE_ANG_NUM) 54 | 55 | typedef struct 56 | { 57 | Eigen::Matrix3f eigenVectorsPCA; 58 | Eigen::Vector3f eigenValuesPCA; 59 | std::vector neibors; 60 | } SNeiborPCA; 61 | 62 | typedef struct 63 | { 64 | // basic 65 | int pnum; 66 | float xmin; 67 | float xmax; 68 | float ymin; 69 | float ymax; 70 | float zmin; 71 | float zmax; 72 | float zmean; 73 | 74 | // pca 75 | float d0[3]; 76 | float d1[3]; 77 | 78 | float center[3]; 79 | 80 | float obb[8]; 81 | 82 | int cls;//类别 83 | 84 | } SClusterFeature; 85 | 86 | int FilterGndForPos(float* outPoints,float*inPoints,int inNum); 87 | int CalNomarls(float *nvects, float *fPoints,int pointNum,float fSearchRadius); 88 | 89 | int CalGndPos(float *gnd, float *fPoints,int pointNum,float fSearchRadius); 90 | 91 | int GetNeiborPCA(SNeiborPCA &npca, pcl::PointCloud::Ptr cloud, 92 | pcl::KdTreeFLANN kdtree, pcl::PointXYZ searchPoint, float fSearchRadius); 93 | 94 | 95 | int CorrectPoints(float *fPoints,int pointNum,float *gndPos); 96 | 97 | // 地面上物体分割 98 | int AbvGndSeg(int *pLabel, float *fPoints, int pointNum); 99 | int SegBG(int *pLabel, pcl::PointCloud::Ptr cloud, pcl::KdTreeFLANN &kdtree, float fSearchRadius); 100 | 101 | SClusterFeature FindACluster(int *pLabel, int seedId, int labelId, pcl::PointCloud::Ptr cloud, pcl::KdTreeFLANN &kdtree, float fSearchRadius, float thrHeight); 102 | int SegObjects(int *pLabel, pcl::PointCloud::Ptr cloud, pcl::KdTreeFLANN &kdtree, float fSearchRadius); 103 | 104 | int CalFreeRegion(float *pFreeDis, float *fPoints,int *pLabel,int pointNum); 105 | int FreeSeg(float *fPoints,int *pLabel,int pointNum); 106 | 107 | int CompleteObjects(int *pLabel, pcl::PointCloud::Ptr cloud, pcl::KdTreeFLANN &kdtree, float fSearchRadius); 108 | int ExpandObjects(int *pLabel, float* fPoints, int pointNum, float fSearchRadius); 109 | int ExpandBG(int *pLabel, float* fPoints, int pointNum, float fSearchRadius); 110 | 111 | // 地面分割 112 | int GndSeg(int* pLabel,float *fPoints,int pointNum,float fSearchRadius); 113 | 114 | 115 | class PCSeg 116 | { 117 | public: 118 | PCSeg(); 119 | // functions 120 | int DoSeg(int *pLabel, float* fPoints1, int pointNum); 121 | int GetMainVectors(float*fPoints, int* pLabel, int pointNum); 122 | int EncodeFeatures(float *pFeas); 123 | int DoBoundaryDetection(float* fPoints1,int *pLabel1,int pointNum); 124 | int DoTrafficLineDet(float *fPoints1,int *pLabel1,int pointNum); 125 | 126 | int CorrectPoints(float *fPoints,int pointNum,float *gndPos); 127 | 128 | 129 | float *PrePoints; 130 | int numPrePoints = 0; 131 | 132 | float gnd_pos[100*6]; 133 | int gnd_vet_len = 0; 134 | 135 | int laneType=0; 136 | float lanePosition[2] = {0}; 137 | 138 | // vars 139 | unsigned char *pVImg; 140 | float gndPos[6]; 141 | int posFlag; 142 | 143 | // cluster features 144 | float pVectors[256*3]; 145 | float pCenters[256*3];//重心 146 | int pnum[256]; 147 | 148 | int objClass[256]; 149 | float zs[256]; 150 | float pOBBs[256*8]; 151 | 152 | float CBBox[256*7];//(a,x,y,z,l,w,h) 153 | 154 | int clusterNum; 155 | 156 | float *corPoints; 157 | int corNum; 158 | 159 | ~PCSeg(); 160 | 161 | }; 162 | 163 | SClusterFeature CalBBox(float *fPoints,int pointNum); 164 | SClusterFeature CalOBB(float *fPoints,int pointNum); 165 | 166 | #endif 167 | -------------------------------------------------------------------------------- /src/lidar_loam/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; -------------------------------------------------------------------------------- /src/visual_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/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_inertial/feature_tracker/feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "camera_models/CameraFactory.h" 13 | #include "camera_models/CataCamera.h" 14 | #include "camera_models/PinholeCamera.h" 15 | 16 | #include "parameters.h" 17 | #include "tic_toc.h" 18 | 19 | using namespace std; 20 | using namespace camodocal; 21 | using namespace Eigen; 22 | 23 | bool inBorder(const cv::Point2f &pt); 24 | 25 | void reduceVector(vector &v, vector status); 26 | void reduceVector(vector &v, vector status); 27 | 28 | 29 | class FeatureTracker 30 | { 31 | public: 32 | FeatureTracker(); 33 | 34 | void readImage(const cv::Mat &_img,double _cur_time); 35 | void readImage_mask(const cv::Mat &_img, const cv::Mat& _mask_img, double _cur_time); 36 | 37 | void setMask(); 38 | 39 | void addPoints(); 40 | 41 | bool updateID(unsigned int i); 42 | 43 | void readIntrinsicParameter(const string &calib_file); 44 | 45 | void showUndistortion(const string &name); 46 | 47 | void rejectWithF(); 48 | void rejectWithF_mask(); 49 | 50 | void undistortedPoints(); 51 | 52 | cv::Mat mask_img; 53 | cv::Mat mask; 54 | cv::Mat fisheye_mask; 55 | cv::Mat prev_img, cur_img, forw_img; 56 | vector n_pts; 57 | vector prev_pts, cur_pts, forw_pts; 58 | vector cur_pts_mask, forw_pts_mask; //定义mask点 59 | vector prev_un_pts, cur_un_pts; 60 | vector pts_velocity; 61 | vector ids; 62 | vector track_cnt; 63 | map cur_un_pts_map; 64 | map prev_un_pts_map; 65 | camodocal::CameraPtr m_camera; 66 | double cur_time; 67 | double prev_time; 68 | 69 | static int n_id; 70 | 71 | double feture_tracker_cost_ = 0.0; 72 | double feture_detect_cost_ = 0.0; 73 | 74 | double limit_dis_epi = 1.0; 75 | }; 76 | -------------------------------------------------------------------------------- /src/visual_inertial/feature_tracker/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | std::string IMAGE_TOPIC; 4 | std::string IMU_TOPIC; 5 | std::vector CAM_NAMES; 6 | std::string FISHEYE_MASK; 7 | int MAX_CNT; 8 | int MIN_DIST; 9 | int WINDOW_SIZE; 10 | int FREQ; 11 | double F_THRESHOLD; 12 | int SHOW_TRACK; 13 | int STEREO_TRACK; 14 | int EQUALIZE; 15 | int ROW; 16 | int COL; 17 | int FOCAL_LENGTH; 18 | int FISHEYE; 19 | bool PUB_THIS_FRAME; 20 | 21 | template 22 | T readParam(ros::NodeHandle &n, std::string name) 23 | { 24 | T ans; 25 | if (n.getParam(name, ans)) 26 | { 27 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 28 | } 29 | else 30 | { 31 | ROS_ERROR_STREAM("Failed to load " << name); 32 | n.shutdown(); 33 | } 34 | return ans; 35 | } 36 | 37 | void readParameters(ros::NodeHandle &n) 38 | { 39 | std::string config_file; 40 | config_file = readParam(n, "config_file"); 41 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 42 | if(!fsSettings.isOpened()) 43 | { 44 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 45 | } 46 | std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); 47 | 48 | fsSettings["image_topic"] >> IMAGE_TOPIC; 49 | fsSettings["imu_topic"] >> IMU_TOPIC; 50 | MAX_CNT = fsSettings["max_cnt"]; 51 | MIN_DIST = fsSettings["min_dist"]; 52 | ROW = fsSettings["image_height"]; 53 | COL = fsSettings["image_width"]; 54 | FREQ = fsSettings["freq"]; 55 | F_THRESHOLD = fsSettings["F_threshold"]; 56 | SHOW_TRACK = fsSettings["show_track"]; 57 | EQUALIZE = fsSettings["equalize"]; 58 | FISHEYE = fsSettings["fisheye"]; 59 | if (FISHEYE == 1) 60 | FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; 61 | CAM_NAMES.push_back(config_file); 62 | 63 | WINDOW_SIZE = 20; 64 | STEREO_TRACK = false; 65 | FOCAL_LENGTH = 460; 66 | PUB_THIS_FRAME = false; 67 | 68 | if (FREQ == 0) 69 | FREQ = 100; 70 | 71 | fsSettings.release(); 72 | 73 | 74 | } 75 | -------------------------------------------------------------------------------- /src/visual_inertial/feature_tracker/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | extern int ROW; 6 | extern int COL; 7 | extern int FOCAL_LENGTH; 8 | const int NUM_OF_CAM = 1; 9 | 10 | 11 | extern std::string IMAGE_TOPIC; 12 | extern std::string IMU_TOPIC; 13 | extern std::string FISHEYE_MASK; 14 | extern std::vector CAM_NAMES; 15 | extern int MAX_CNT; 16 | extern int MIN_DIST; 17 | extern int WINDOW_SIZE; 18 | extern int FREQ; 19 | extern double F_THRESHOLD; 20 | extern int SHOW_TRACK; 21 | extern int STEREO_TRACK; 22 | extern int EQUALIZE; 23 | extern int FISHEYE; 24 | extern bool PUB_THIS_FRAME; 25 | 26 | void readParameters(ros::NodeHandle &n); 27 | -------------------------------------------------------------------------------- /src/visual_inertial/feature_tracker/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "../feature_tracker/camera_models/CameraFactory.h" 8 | #include "../feature_tracker/camera_models/CataCamera.h" 9 | #include "../feature_tracker/camera_models/PinholeCamera.h" 10 | #include "utility/tic_toc.h" 11 | #include "utility/utility.h" 12 | #include "parameters.h" 13 | #include "ThirdParty/DBoW/DBoW2.h" 14 | #include "ThirdParty/DVision/DVision.h" 15 | 16 | #define MIN_LOOP_NUM 25 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | using namespace DVision; 21 | 22 | 23 | class BriefExtractor 24 | { 25 | public: 26 | virtual void operator()(const cv::Mat &im, vector &keys, vector &descriptors) const; 27 | BriefExtractor(const std::string &pattern_file); 28 | 29 | DVision::BRIEF m_brief; 30 | }; 31 | 32 | class KeyFrame 33 | { 34 | public: 35 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image, 36 | vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_normal, 37 | vector &_point_id, int _sequence); 38 | KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i, 39 | cv::Mat &_image, int _loop_index, Eigen::Matrix &_loop_info, 40 | vector &_keypoints, vector &_keypoints_norm, vector &_brief_descriptors); 41 | bool findConnection(KeyFrame* old_kf); 42 | void computeWindowBRIEFPoint(); 43 | void computeBRIEFPoint(); 44 | //void extractBrief(); 45 | int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b); 46 | bool searchInAera(const BRIEF::bitset window_descriptor, 47 | const std::vector &descriptors_old, 48 | const std::vector &keypoints_old, 49 | const std::vector &keypoints_old_norm, 50 | cv::Point2f &best_match, 51 | cv::Point2f &best_match_norm); 52 | void searchByBRIEFDes(std::vector &matched_2d_old, 53 | std::vector &matched_2d_old_norm, 54 | std::vector &status, 55 | const std::vector &descriptors_old, 56 | const std::vector &keypoints_old, 57 | const std::vector &keypoints_old_norm); 58 | void FundmantalMatrixRANSAC(const std::vector &matched_2d_cur_norm, 59 | const std::vector &matched_2d_old_norm, 60 | vector &status); 61 | void PnPRANSAC(const vector &matched_2d_old_norm, 62 | const std::vector &matched_3d, 63 | std::vector &status, 64 | Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old); 65 | void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 66 | void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i); 67 | void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); 68 | void updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i); 69 | void updateLoop(Eigen::Matrix &_loop_info); 70 | 71 | Eigen::Vector3d getLoopRelativeT(); 72 | double getLoopRelativeYaw(); 73 | Eigen::Quaterniond getLoopRelativeQ(); 74 | 75 | 76 | 77 | double time_stamp; 78 | int index; 79 | int local_index; 80 | Eigen::Vector3d vio_T_w_i; 81 | Eigen::Matrix3d vio_R_w_i; 82 | Eigen::Vector3d T_w_i; 83 | Eigen::Matrix3d R_w_i; 84 | Eigen::Vector3d origin_vio_T; 85 | Eigen::Matrix3d origin_vio_R; 86 | cv::Mat image; 87 | cv::Mat thumbnail; 88 | vector point_3d; 89 | vector point_2d_uv; 90 | vector point_2d_norm; 91 | vector point_id; 92 | vector keypoints; 93 | vector keypoints_norm; 94 | vector window_keypoints; 95 | vector brief_descriptors; 96 | vector window_brief_descriptors; 97 | bool has_fast_point; 98 | int sequence; 99 | 100 | bool has_loop; 101 | int loop_index; 102 | Eigen::Matrix loop_info; 103 | }; 104 | 105 | -------------------------------------------------------------------------------- /src/visual_inertial/pose_graph/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../feature_tracker/camera_models/CameraFactory.h" 4 | #include "../feature_tracker/camera_models/CataCamera.h" 5 | #include "../feature_tracker/camera_models/PinholeCamera.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | extern camodocal::CameraPtr m_camera; 14 | extern Eigen::Vector3d tic; 15 | extern Eigen::Matrix3d qic; 16 | extern ros::Publisher pub_match_img; 17 | extern ros::Publisher pub_match_points; 18 | extern int VISUALIZATION_SHIFT_X; 19 | extern int VISUALIZATION_SHIFT_Y; 20 | extern std::string BRIEF_PATTERN_FILE; 21 | extern std::string POSE_GRAPH_SAVE_PATH; 22 | extern int ROW; 23 | extern int COL; 24 | extern std::string VINS_RESULT_PATH; 25 | extern int DEBUG_IMAGE; 26 | extern int FAST_RELOCALIZATION; 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/visual_inertial/pose_graph/utility/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "../parameters.h" 11 | 12 | class CameraPoseVisualization { 13 | public: 14 | std::string m_marker_ns; 15 | 16 | CameraPoseVisualization(float r, float g, float b, float a); 17 | 18 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 19 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 20 | void setScale(double s); 21 | void setLineWidth(double width); 22 | 23 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 24 | void reset(); 25 | 26 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 27 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 28 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 29 | //void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src); 30 | void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header); 31 | private: 32 | std::vector m_markers; 33 | std_msgs::ColorRGBA m_image_boundary_color; 34 | std_msgs::ColorRGBA m_optical_center_connector_color; 35 | double m_scale; 36 | double m_line_width; 37 | visualization_msgs::Marker image; 38 | int LOOP_EDGE_NUM; 39 | int tmp_loop_edge_num; 40 | 41 | static const Eigen::Vector3d imlt; 42 | static const Eigen::Vector3d imlb; 43 | static const Eigen::Vector3d imrt; 44 | static const Eigen::Vector3d imrb; 45 | static const Eigen::Vector3d oc ; 46 | static const Eigen::Vector3d lt0 ; 47 | static const Eigen::Vector3d lt1 ; 48 | static const Eigen::Vector3d lt2 ; 49 | }; 50 | -------------------------------------------------------------------------------- /src/visual_inertial/pose_graph/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_inertial/pose_graph/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_inertial/pose_graph/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_inertial/vins_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 | 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, const std_msgs::Header &header); 36 | void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); 37 | 38 | // internal 39 | void clearState(); 40 | bool initialStructure(); 41 | bool visualInitialAlign(); 42 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l); 43 | void slideWindow(); 44 | void solveOdometry(); 45 | void slideWindowNew(); 46 | void slideWindowOld(); 47 | void optimization(); 48 | void vector2double(); 49 | void double2vector(); 50 | bool failureDetection(); 51 | 52 | 53 | enum SolverFlag 54 | { 55 | INITIAL, 56 | NON_LINEAR 57 | }; 58 | 59 | enum MarginalizationFlag 60 | { 61 | MARGIN_OLD = 0, 62 | MARGIN_SECOND_NEW = 1 63 | }; 64 | 65 | SolverFlag solver_flag; 66 | MarginalizationFlag marginalization_flag; 67 | Vector3d g; 68 | MatrixXd Ap[2], backup_A; 69 | VectorXd bp[2], backup_b; 70 | 71 | Matrix3d ric[NUM_OF_CAM]; 72 | Vector3d tic[NUM_OF_CAM]; 73 | 74 | Vector3d Ps[(WINDOW_SIZE + 1)]; 75 | Vector3d Vs[(WINDOW_SIZE + 1)]; 76 | Matrix3d Rs[(WINDOW_SIZE + 1)]; 77 | Vector3d Bas[(WINDOW_SIZE + 1)]; 78 | Vector3d Bgs[(WINDOW_SIZE + 1)]; 79 | double td; 80 | 81 | Matrix3d back_R0, last_R, last_R0; 82 | Vector3d back_P0, last_P, last_P0; 83 | std_msgs::Header Headers[(WINDOW_SIZE + 1)]; 84 | 85 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 86 | Vector3d acc_0, gyr_0; 87 | 88 | vector dt_buf[(WINDOW_SIZE + 1)]; 89 | vector linear_acceleration_buf[(WINDOW_SIZE + 1)]; 90 | vector angular_velocity_buf[(WINDOW_SIZE + 1)]; 91 | 92 | int frame_count; 93 | int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; 94 | 95 | FeatureManager f_manager; 96 | MotionEstimator m_estimator; 97 | InitialEXRotation initial_ex_rotation; 98 | 99 | bool first_imu; 100 | bool is_valid, is_key; 101 | bool failure_occur; 102 | 103 | vector point_cloud; 104 | vector margin_cloud; 105 | vector key_poses; 106 | double initial_timestamp; 107 | 108 | 109 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; 110 | double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; 111 | double para_Feature[NUM_OF_F][SIZE_FEATURE]; 112 | double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE]; 113 | double para_Retrive_Pose[SIZE_POSE]; 114 | double para_Td[1][1]; 115 | double para_Tr[1][1]; 116 | 117 | int loop_window_index; 118 | 119 | MarginalizationInfo *last_marginalization_info; 120 | vector last_marginalization_parameter_blocks; 121 | 122 | map all_image_frame; 123 | IntegrationBase *tmp_pre_integration; 124 | 125 | //relocalization variable 126 | bool relocalization_info; 127 | double relo_frame_stamp; 128 | double relo_frame_index; 129 | int relo_frame_local_index; 130 | vector match_points; 131 | double relo_Pose[SIZE_POSE]; 132 | Matrix3d drift_correct_r; 133 | Vector3d drift_correct_t; 134 | Vector3d prev_relo_t; 135 | Matrix3d prev_relo_r; 136 | Vector3d relo_relative_t; 137 | Quaterniond relo_relative_q; 138 | double relo_relative_yaw; 139 | }; 140 | -------------------------------------------------------------------------------- /src/visual_inertial/vins_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_inertial/vins_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_inertial/vins_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_inertial/vins_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); 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 | Eigen::Matrix tangent_base; 19 | static Eigen::Matrix2d sqrt_info; 20 | static double sum_t; 21 | }; 22 | -------------------------------------------------------------------------------- /src/visual_inertial/vins_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 | class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1> 11 | { 12 | public: 13 | ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, 14 | const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j, 15 | const double _td_i, const double _td_j, const double _row_i, const double _row_j); 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | void check(double **parameters); 18 | 19 | Eigen::Vector3d pts_i, pts_j; 20 | Eigen::Vector3d velocity_i, velocity_j; 21 | double td_i, td_j; 22 | Eigen::Matrix tangent_base; 23 | double row_i, row_j; 24 | static Eigen::Matrix2d sqrt_info; 25 | static double sum_t; 26 | }; 27 | -------------------------------------------------------------------------------- /src/visual_inertial/vins_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 | cur_td = td; 31 | } 32 | double cur_td; 33 | Vector3d point; 34 | Vector2d uv; 35 | Vector2d velocity; 36 | double z; 37 | bool is_used; 38 | double parallax; 39 | MatrixXd A; 40 | VectorXd b; 41 | double dep_gradient; 42 | }; 43 | 44 | class FeaturePerId 45 | { 46 | public: 47 | const int feature_id; 48 | int start_frame; 49 | vector feature_per_frame; 50 | 51 | int used_num; 52 | bool is_outlier; 53 | bool is_margin; 54 | double estimated_depth; 55 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 56 | 57 | Vector3d gt_p; 58 | 59 | FeaturePerId(int _feature_id, int _start_frame) 60 | : feature_id(_feature_id), start_frame(_start_frame), 61 | used_num(0), estimated_depth(-1.0), solve_flag(0) 62 | { 63 | } 64 | 65 | int endFrame(); 66 | }; 67 | 68 | class FeatureManager 69 | { 70 | public: 71 | FeatureManager(Matrix3d _Rs[]); 72 | 73 | void setRic(Matrix3d _ric[]); 74 | 75 | void clearState(); 76 | 77 | int getFeatureCount(); 78 | 79 | bool addFeatureCheckParallax(int frame_count, const map>>> &image, double td); 80 | void debugShow(); 81 | vector> getCorresponding(int frame_count_l, int frame_count_r); 82 | 83 | //void updateDepth(const VectorXd &x); 84 | void setDepth(const VectorXd &x); 85 | void removeFailures(); 86 | void clearDepth(const VectorXd &x); 87 | VectorXd getDepthVector(); 88 | void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]); 89 | void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P); 90 | void removeBack(); 91 | void removeFront(int frame_count); 92 | void removeOutlier(); 93 | list feature; 94 | int last_track_num; 95 | 96 | private: 97 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 98 | const Matrix3d *Rs; 99 | Matrix3d ric[NUM_OF_CAM]; 100 | }; 101 | 102 | #endif -------------------------------------------------------------------------------- /src/visual_inertial/vins_estimator/initial/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "../factor/imu_factor.h" 5 | #include "../utility/utility.h" 6 | #include 7 | #include 8 | #include "../feature_manager.h" 9 | 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | class ImageFrame 14 | { 15 | public: 16 | ImageFrame(){}; 17 | ImageFrame(const map>>>& _points, double _t):t{_t},is_key_frame{false} 18 | { 19 | points = _points; 20 | }; 21 | map> > > points; 22 | double t; 23 | Matrix3d R; 24 | Vector3d T; 25 | IntegrationBase *pre_integration; 26 | bool is_key_frame; 27 | }; 28 | 29 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); -------------------------------------------------------------------------------- /src/visual_inertial/vins_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_inertial/vins_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_inertial/vins_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_inertial/vins_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_inertial/vins_estimator/parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters.h" 2 | 3 | double INIT_DEPTH; 4 | double MIN_PARALLAX; 5 | double ACC_N, ACC_W; 6 | double GYR_N, GYR_W; 7 | 8 | std::vector RIC; 9 | std::vector TIC; 10 | 11 | Eigen::Vector3d G{0.0, 0.0, 9.8}; 12 | 13 | double BIAS_ACC_THRESHOLD; 14 | double BIAS_GYR_THRESHOLD; 15 | double SOLVER_TIME; 16 | int NUM_ITERATIONS; 17 | int ESTIMATE_EXTRINSIC; 18 | int ESTIMATE_TD; 19 | int ROLLING_SHUTTER; 20 | std::string EX_CALIB_RESULT_PATH; 21 | std::string VINS_RESULT_PATH; 22 | std::string IMU_TOPIC; 23 | double ROW, COL; 24 | double TD, TR; 25 | 26 | template 27 | T readParam(ros::NodeHandle &n, std::string name) 28 | { 29 | T ans; 30 | if (n.getParam(name, ans)) 31 | { 32 | ROS_INFO_STREAM("Loaded " << name << ": " << ans); 33 | } 34 | else 35 | { 36 | ROS_ERROR_STREAM("Failed to load " << name); 37 | n.shutdown(); 38 | } 39 | return ans; 40 | } 41 | 42 | void readParameters(ros::NodeHandle &n) 43 | { 44 | std::string config_file; 45 | config_file = readParam(n, "config_file"); 46 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 47 | if(!fsSettings.isOpened()) 48 | { 49 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 50 | } 51 | 52 | fsSettings["imu_topic"] >> IMU_TOPIC; 53 | 54 | SOLVER_TIME = fsSettings["max_solver_time"]; 55 | NUM_ITERATIONS = fsSettings["max_num_iterations"]; 56 | MIN_PARALLAX = fsSettings["keyframe_parallax"]; 57 | MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH; 58 | 59 | std::string OUTPUT_PATH; 60 | fsSettings["output_path"] >> OUTPUT_PATH; 61 | VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.txt"; 62 | std::cout << "result path " << VINS_RESULT_PATH << std::endl; 63 | 64 | // create folder if not exists 65 | FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str()); 66 | 67 | std::ofstream fout(VINS_RESULT_PATH, std::ios::out); 68 | fout.close(); 69 | 70 | ACC_N = fsSettings["acc_n"]; 71 | ACC_W = fsSettings["acc_w"]; 72 | GYR_N = fsSettings["gyr_n"]; 73 | GYR_W = fsSettings["gyr_w"]; 74 | G.z() = fsSettings["g_norm"]; 75 | ROW = fsSettings["image_height"]; 76 | COL = fsSettings["image_width"]; 77 | ROS_INFO("ROW: %f COL: %f ", ROW, COL); 78 | 79 | ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"]; 80 | if (ESTIMATE_EXTRINSIC == 2) 81 | { 82 | ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param"); 83 | RIC.push_back(Eigen::Matrix3d::Identity()); 84 | TIC.push_back(Eigen::Vector3d::Zero()); 85 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 86 | 87 | } 88 | else 89 | { 90 | if ( ESTIMATE_EXTRINSIC == 1) 91 | { 92 | ROS_WARN(" Optimize extrinsic param around initial guess!"); 93 | EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv"; 94 | } 95 | if (ESTIMATE_EXTRINSIC == 0) 96 | ROS_WARN(" fix extrinsic param "); 97 | 98 | cv::Mat cv_R, cv_T; 99 | fsSettings["extrinsicRotation"] >> cv_R; 100 | fsSettings["extrinsicTranslation"] >> cv_T; 101 | Eigen::Matrix3d eigen_R; 102 | Eigen::Vector3d eigen_T; 103 | cv::cv2eigen(cv_R, eigen_R); 104 | cv::cv2eigen(cv_T, eigen_T); 105 | Eigen::Quaterniond Q(eigen_R); 106 | eigen_R = Q.normalized(); 107 | RIC.push_back(eigen_R); 108 | TIC.push_back(eigen_T); 109 | ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]); 110 | ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose()); 111 | 112 | } 113 | 114 | INIT_DEPTH = 5.0; 115 | BIAS_ACC_THRESHOLD = 0.1; 116 | BIAS_GYR_THRESHOLD = 0.1; 117 | 118 | TD = fsSettings["td"]; 119 | ESTIMATE_TD = fsSettings["estimate_td"]; 120 | if (ESTIMATE_TD) 121 | ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD); 122 | else 123 | ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD); 124 | 125 | ROLLING_SHUTTER = fsSettings["rolling_shutter"]; 126 | if (ROLLING_SHUTTER) 127 | { 128 | TR = fsSettings["rolling_shutter_tr"]; 129 | ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR); 130 | } 131 | else 132 | { 133 | TR = 0; 134 | } 135 | 136 | fsSettings.release(); 137 | } 138 | -------------------------------------------------------------------------------- /src/visual_inertial/vins_estimator/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "utility/utility.h" 7 | #include 8 | #include 9 | #include 10 | 11 | const double FOCAL_LENGTH = 460.0; 12 | const int WINDOW_SIZE = 10; 13 | const int NUM_OF_CAM = 1; 14 | const int NUM_OF_F = 1000; 15 | //#define UNIT_SPHERE_ERROR 16 | 17 | extern double INIT_DEPTH; 18 | extern double MIN_PARALLAX; 19 | extern int ESTIMATE_EXTRINSIC; 20 | 21 | extern double ACC_N, ACC_W; 22 | extern double GYR_N, GYR_W; 23 | 24 | extern std::vector RIC; 25 | extern std::vector TIC; 26 | extern Eigen::Vector3d G; 27 | 28 | extern double BIAS_ACC_THRESHOLD; 29 | extern double BIAS_GYR_THRESHOLD; 30 | extern double SOLVER_TIME; 31 | extern int NUM_ITERATIONS; 32 | extern std::string EX_CALIB_RESULT_PATH; 33 | extern std::string VINS_RESULT_PATH; 34 | extern std::string IMU_TOPIC; 35 | extern double TD; 36 | extern double TR; 37 | extern int ESTIMATE_TD; 38 | extern int ROLLING_SHUTTER; 39 | extern double ROW, COL; 40 | 41 | 42 | void readParameters(ros::NodeHandle &n); 43 | 44 | enum SIZE_PARAMETERIZATION 45 | { 46 | SIZE_POSE = 7, 47 | SIZE_SPEEDBIAS = 9, 48 | SIZE_FEATURE = 1 49 | }; 50 | 51 | enum StateOrder 52 | { 53 | O_P = 0, 54 | O_R = 3, 55 | O_V = 6, 56 | O_BA = 9, 57 | O_BG = 12 58 | }; 59 | 60 | enum NoiseOrder 61 | { 62 | O_AN = 0, 63 | O_GN = 3, 64 | O_AW = 6, 65 | O_GW = 9 66 | }; 67 | -------------------------------------------------------------------------------- /src/visual_inertial/vins_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_inertial/vins_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_inertial/vins_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_inertial/vins_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 | void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header); 35 | 36 | void printStatistics(const Estimator &estimator, double t); 37 | 38 | void pubOdometry(const Estimator &estimator, const std_msgs::Header &header); 39 | 40 | void pubInitialGuess(const Estimator &estimator, const std_msgs::Header &header); 41 | 42 | void pubKeyPoses(const Estimator &estimator, const std_msgs::Header &header); 43 | 44 | void pubCameraPose(const Estimator &estimator, const std_msgs::Header &header); 45 | 46 | void pubPointCloud(const Estimator &estimator, const std_msgs::Header &header); 47 | 48 | void pubTF(const Estimator &estimator, const std_msgs::Header &header); 49 | 50 | void pubKeyframe(const Estimator &estimator); 51 | 52 | void pubRelocalization(const Estimator &estimator); --------------------------------------------------------------------------------