├── .gitignore ├── .gitmodules ├── README.md ├── assets ├── erl_d455_car_demo.gif ├── erl_d455_car_demo_640x480.gif └── erl_d455_demo.gif ├── config ├── euroc.yaml ├── object_feat_all.yaml ├── object_feat_erl.yaml └── rs_d455.yaml ├── doc └── object_mapping.md ├── licenses ├── LICENCE_VINS_MONO.txt ├── LICENSE_MSCKF_VIO.txt └── LICENSE_ORB_SLAM2.txt ├── ros_wrapper ├── .catkin_tools │ ├── README │ ├── VERSION │ └── profiles │ │ └── default │ │ ├── build.yaml │ │ ├── devel_collisions.txt │ │ └── packages │ │ ├── catkin_tools_prebuild │ │ ├── devel_manifest.txt │ │ └── package.xml │ │ ├── darknet_ros │ │ ├── devel_manifest.txt │ │ └── package.xml │ │ └── darknet_ros_msgs │ │ ├── devel_manifest.txt │ │ └── package.xml ├── .gitignore └── src │ ├── CMakeLists.txt │ ├── darknet_ros_bridge │ ├── CMakeLists.txt │ ├── package.xml │ └── scripts │ │ └── darknet_ros_bridge.py │ ├── orcvio │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake │ │ └── FindSuiteSparse.cmake │ ├── config │ │ ├── camchain-imucam-d455.yaml │ │ ├── camchain-imucam-erl.yaml │ │ ├── camchain-imucam-euroc-noextrinsics.yaml │ │ ├── camchain-imucam-euroc.yaml │ │ ├── camchain-imucam-fla.yaml │ │ ├── camchain-imucam-unity-gq.yaml │ │ └── euroc_config │ │ │ ├── MH_01_easy.yaml │ │ │ └── V1_01_easy.yaml │ ├── include │ │ ├── initializer │ │ │ ├── DynamicInitializer.h │ │ │ ├── FlexibleInitializer.h │ │ │ ├── ImuPreintegration.h │ │ │ ├── StaticInitializer.h │ │ │ ├── feature_manager.h │ │ │ ├── initial_alignment.h │ │ │ ├── initial_sfm.h │ │ │ ├── math_utils.h │ │ │ └── solve_5pts.h │ │ └── orcvio │ │ │ ├── dataset_reader.h │ │ │ ├── euroc_gt.h │ │ │ ├── feature.h │ │ │ ├── feature_ov.h │ │ │ ├── image_processor.h │ │ │ ├── image_processor_nodelet.h │ │ │ ├── orcvio.h │ │ │ ├── orcvio_nodelet.h │ │ │ ├── state.h │ │ │ └── utils.h │ ├── launch │ │ ├── image_processor_d455.launch │ │ ├── image_processor_euroc.launch │ │ ├── image_processor_fla.launch │ │ ├── image_processor_unity_gq.launch │ │ ├── orcvio_d455.launch │ │ ├── orcvio_euroc.launch │ │ ├── orcvio_euroc_eval.launch │ │ ├── orcvio_fla.launch │ │ ├── orcvio_unity_gq_husky.launch │ │ └── reset.launch │ ├── msg │ │ ├── CameraMeasurement.msg │ │ ├── FeatureMeasurement.msg │ │ └── TrackingInfo.msg │ ├── nodelets.xml │ ├── package.xml │ ├── rviz │ │ ├── rviz_d455.rviz │ │ ├── rviz_euroc_config.rviz │ │ ├── rviz_fla_config.rviz │ │ └── rviz_unity_gq.rviz │ ├── scripts │ │ └── batch_run_euroc.py │ ├── src │ │ ├── DynamicInitializer.cpp │ │ ├── FlexibleInitializer.cpp │ │ ├── StaticInitializer.cpp │ │ ├── euroc_gt.cpp │ │ ├── feature_manager.cpp │ │ ├── feature_ov.cpp │ │ ├── image_processor.cpp │ │ ├── image_processor_nodelet.cpp │ │ ├── initial_alignment.cpp │ │ ├── initial_sfm.cpp │ │ ├── orcvio.cpp │ │ ├── orcvio_nodelet.cpp │ │ ├── publish_euroc_gt.cpp │ │ ├── solve_5pts.cpp │ │ └── utils.cpp │ └── test │ │ ├── feature_initialization_test.cpp │ │ └── math_utils_test.cpp │ ├── orcvio_mapping │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake │ │ ├── FindEigen3.cmake │ │ └── FindSuiteSparse.cmake │ ├── config │ │ ├── medfield.yaml │ │ ├── object_feat_all.yaml │ │ ├── object_feat_erl.yaml │ │ └── object_feat_unity_gq.yaml │ ├── include │ │ ├── ObjectInitNode.h │ │ └── orcvio │ │ │ ├── feat │ │ │ ├── Feature.h │ │ │ ├── FeatureInitializer.h │ │ │ ├── FeatureInitializerOptions.h │ │ │ ├── feature.hpp │ │ │ ├── feature_msg.h │ │ │ └── kf.h │ │ │ ├── imu_state.h │ │ │ ├── obj │ │ │ ├── ObjectFeature.h │ │ │ ├── ObjectFeatureInitializer.h │ │ │ ├── ObjectLM.h │ │ │ ├── ObjectLMLite.h │ │ │ ├── ObjectResJacCam.h │ │ │ └── ObjectState.h │ │ │ ├── plot │ │ │ └── matplotlibcpp.h │ │ │ └── utils │ │ │ ├── EigenLevenbergMarquardt.h │ │ │ ├── EigenLevenbergMarquardt │ │ │ ├── LMcovar.h │ │ │ ├── LMonestep.h │ │ │ ├── LMpar.h │ │ │ ├── LMqrsolv.h │ │ │ └── LevenbergMarquardt.h │ │ │ ├── EigenNumericalDiff.h │ │ │ ├── math_utils.hpp │ │ │ └── se3_ops.hpp │ ├── launch │ │ ├── om_d455.launch │ │ ├── om_unity_gq.launch │ │ ├── standalone_object_mapping_dcist.launch │ │ └── standalone_object_mapping_erl.launch │ ├── package.xml │ └── src │ │ ├── ObjectInitNode.cpp │ │ ├── orcvio │ │ ├── feat │ │ │ ├── Feature.cpp │ │ │ ├── FeatureInitializer.cpp │ │ │ └── kf.cpp │ │ └── obj │ │ │ ├── ObjectFeature.cpp │ │ │ ├── ObjectFeatureInitializer.cpp │ │ │ ├── ObjectLM.cpp │ │ │ ├── ObjectLMLite.cpp │ │ │ ├── ObjectResJacCam.cpp │ │ │ └── ObjectState.cpp │ │ └── orcvio_mapping_node.cpp │ ├── sort_ros │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include │ │ └── sort_ros │ │ │ ├── Hungarian.h │ │ │ ├── KalmanTracker.h │ │ │ └── sort_tracking.h │ ├── launch │ │ ├── display_dcist.rviz │ │ ├── sort_ros_test_d455.launch │ │ ├── sort_ros_test_gdb.launch │ │ ├── sort_ros_test_kitti.launch │ │ ├── sort_ros_test_tum.launch │ │ └── sort_ros_test_visma.launch │ ├── msg │ │ ├── TrackedBoundingBox.msg │ │ └── TrackedBoundingBoxes.msg │ ├── nodelet_plugins.xml │ ├── package.xml │ ├── readme.md │ └── src │ │ ├── Hungarian.cpp │ │ ├── KalmanTracker.cpp │ │ ├── main.cpp │ │ ├── sort_ros_node.cpp │ │ ├── sort_ros_nodelet.cpp │ │ ├── sort_tracking.cpp │ │ └── test_kf.cpp │ └── wm_od_interface_msgs │ ├── CMakeLists.txt │ ├── msg │ ├── KeypointDetection.msg │ ├── KeypointDetections.msg │ ├── ObjectDetectionRequest.msg │ ├── ROI2d.msg │ ├── ira_det.msg │ ├── ira_dets.msg │ └── ira_request.msg │ └── package.xml └── scripts ├── convert_csv_to_txt.py ├── process_rosbag ├── add_sim_imu.py ├── bbox_msg_template.msg ├── check_barrier.py ├── check_timestamp.py ├── convert_detection_to_bboxes.py └── save_zs_from_rosbag.py ├── save_pose_from_tf.py ├── tf_bag.py └── traj_eval.py /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | devel 3 | cache 4 | .vscode 5 | 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Compiled Dynamic libraries 20 | *.so 21 | *.dylib 22 | *.dll 23 | 24 | # Fortran module files 25 | *.mod 26 | *.smod 27 | 28 | # Compiled Static libraries 29 | *.lai 30 | *.la 31 | *.a 32 | *.lib 33 | 34 | # Executables 35 | *.exe 36 | *.out 37 | *.app -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ros_wrapper/src/catkin_simple"] 2 | path = ros_wrapper/src/catkin_simple 3 | url = https://github.com/catkin/catkin_simple 4 | [submodule "ros_wrapper/src/darknet_ros"] 5 | path = ros_wrapper/src/darknet_ros 6 | url = https://github.com/leggedrobotics/darknet_ros 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## About 2 | 3 | - Object residual constrained Visual-Inertial Odometry (OrcVIO) is a visual-inertial odometry pipeline, which is tightly coupled with tracking and optimization over structured object models. It provides accurate trajectory estimation and large-scale object-level mapping from online **Stereo+IMU** data. 4 | 5 | - OrcVIO-Lite only uses **bounding boxs** and no keypoints. The object mapping module and VIO module are implemented in separate ROS nodelets and are decoupled. 6 | 7 | - [Project website](https://moshan.cf/orcvio_githubpage/) 8 | 9 | ### Citation 10 | 11 | ``` 12 | @inproceedings{shan2020orcvio, 13 | title={OrcVIO: Object residual constrained Visual-Inertial Odometry}, 14 | author={Shan, Mo and Feng, Qiaojun and Atanasov, Nikolay}, 15 | booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 16 | pages={5104--5111}, 17 | year={2020}, 18 | organization={IEEE} 19 | } 20 | ``` 21 | 22 | ## 1. Prerequisites 23 | 24 | This repository was tested on Ubuntu 18.04 with [ROS Melodic](http://wiki.ros.org/melodic/Installation). 25 | 26 | The core algorithm depends on `Eigen`, `Boost`, `Suitesparse`, `Ceres`, `OpenCV`, `Sophus`, `GTest` 27 | 28 | 29 | ## 2. Installation 30 | 31 | ### ROS version 32 | 33 | - Environment is `Ubuntu 18.04` with ROS `Melodic` 34 | - The ROS version also depends on [catkin simple](https://github.com/catkin/catkin_simple), please put it in the `ros_wrapper/src` folder 35 | 36 | ``` 37 | $ git clone --recursive https://github.com/shanmo/OrcVIO-Stereo-Mapping.git 38 | $ cd OrcVIO-Stereo-Mapping/ros_wrapper 39 | $ catkin_make 40 | $ source ./devel/setup.bash 41 | ``` 42 | 43 | ## 3. Demo 44 | 45 | - Download [ERL indoor hand-held dataset (chairs)](https://www.dropbox.com/s/mwmv5ql3ht1i61n/d455_one_way_demo_bboxes.bag?dl=0), which was collected with [Realsense D455](https://www.intelrealsense.com/depth-camera-d455/) in Existential Robotics Lab, University of California San Diego 46 | 47 | - Please refer to [wiki](https://github.com/shanmo/OrcVIO-Stereo-Mapping/wiki) regarding how to setup D455 48 | 49 | - Run `roslaunch orcvio orcvio_d455.launch` generates the result below 50 | 51 | ![demo](assets/erl_d455_demo.gif) 52 | 53 | - Download [ERL indoor robotic car dataset](https://www.dropbox.com/s/uzauaeegf8i82kg/d455_car_demo_new_bboxes.bag?dl=0), change the rosbag path, and run `roslaunch orcvio orcvio_d455.launch` to get the demos like below 54 | 55 | ![demo](assets/erl_d455_car_demo.gif) 56 | 57 | ![demo](assets/erl_d455_car_demo_640x480.gif) 58 | 59 | ## License 60 | 61 | ``` 62 | MIT License 63 | Copyright (c) 2021 ERL at UCSD 64 | ``` 65 | 66 | ## Reference 67 | 68 | - [LARVIO](https://github.com/PetWorm/LARVIO) 69 | - [MSCKF](https://github.com/KumarRobotics/msckf_vio) 70 | - [OpenVINS](https://github.com/rpng/open_vins) 71 | - [OpenVINS eval](https://github.com/symao/open_vins) 72 | -------------------------------------------------------------------------------- /assets/erl_d455_car_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Stereo-Mapping/e7716dc21cdc00c67e40dee85860a25d57c979ba/assets/erl_d455_car_demo.gif -------------------------------------------------------------------------------- /assets/erl_d455_car_demo_640x480.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Stereo-Mapping/e7716dc21cdc00c67e40dee85860a25d57c979ba/assets/erl_d455_car_demo_640x480.gif -------------------------------------------------------------------------------- /assets/erl_d455_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Stereo-Mapping/e7716dc21cdc00c67e40dee85860a25d57c979ba/assets/erl_d455_demo.gif -------------------------------------------------------------------------------- /config/euroc.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # switches 4 | if_FEJ: 0 # 0(false) or 1(true) 5 | estimate_extrin: 0 # 0(false) or 1(true) 6 | estimate_td: 0 # 0(false) or 1(true) 7 | calib_imu_instrinsic: 0 # 0(false) or 1(true) 8 | 9 | # camera instrinsic 10 | camera_model: "pinhole" # only support "pinhole" 11 | distortion_model: "radtan" # only support "radtan" and "equidistant" 12 | resolution_width: 752 13 | resolution_height: 480 14 | intrinsics: 15 | fx: 458.654 16 | fy: 457.296 17 | cx: 367.215 18 | cy: 248.375 19 | distortion_coeffs: 20 | k1: -0.28340811 21 | k2: 0.07395907 22 | p1: 0.00019359 23 | p2: 1.76187114e-05 24 | 25 | # imu-camera extrinsic, including spacial and temporal parameters 26 | T_cam_imu: !!opencv-matrix 27 | rows: 4 28 | cols: 4 29 | dt: d 30 | data: 31 | [0.014865542981794, 0.999557249008346, -0.025774436697440, 0.065222909535531, 32 | -0.999880929698575, 0.014967213324719, 0.003756188357967, -0.020706385492719, 33 | 0.004140296794224, 0.025715529947966, 0.999660727177902, -0.008054602460030, 34 | 0, 0, 0, 1.000000000000000] 35 | td: 0.0 36 | 37 | # TODO: if calibrate camera instrinsic online 38 | 39 | # visual front-end parameters 40 | pyramid_levels: 2 41 | patch_size: 21 42 | fast_threshold: 30 43 | max_iteration: 30 44 | track_precision: 0.01 45 | ransac_threshold: 1 46 | max_features_num: 200 47 | min_distance: 20 48 | flag_equalize: 1 # 0(false) or 1(true) 49 | pub_frequency: 10 50 | 51 | # window size 52 | sw_size: 20 53 | 54 | # online reset thresholds 55 | position_std_threshold: 8.0 56 | rotation_threshold: 0.2618 57 | translation_threshold: 0.4 58 | tracking_rate_threshold: 0.5 59 | 60 | # feature triangulation parameters 61 | least_observation_number: 3 62 | max_track_len: 6 63 | feature_translation_threshold: -1.0 64 | 65 | # imu and camera measurement noise parameters 66 | noise_gyro: 0.004 67 | noise_acc: 0.08 68 | noise_gyro_bias: 2e-6 69 | noise_acc_bias: 4e-5 70 | noise_feature: 0.008 71 | 72 | # filter intial covariance 73 | initial_covariance_orientation: 4e-4 74 | initial_covariance_velocity: 0.25 75 | initial_covariance_position: 1.0 76 | initial_covariance_gyro_bias: 4e-4 77 | initial_covariance_acc_bias: 0.01 78 | initial_covariance_extrin_rot: 3.0462e-8 79 | initial_covariance_extrin_trans: 9e-8 80 | 81 | # fej settings 82 | reset_fej_threshold: 10.11 83 | 84 | # zupt settings 85 | if_ZUPT_valid: 1 # 0(false) or 1(true) 86 | 87 | if_use_feature_zupt_flag: 0 # 0(false) or 1(true) 88 | zupt_max_feature_dis: 2e-3 89 | 90 | zupt_noise_v: 1e-2 # std 91 | zupt_noise_p: 1e-2 92 | zupt_noise_q: 3.4e-2 93 | 94 | # static initialization setting 95 | static_duration: 1.0 96 | 97 | # measurement rate 98 | imu_rate: 200 99 | img_rate: 20 100 | 101 | # augmented feature state settings 102 | # a positive value is hybrid msckf 103 | max_features_in_one_grid: 1 # pure msckf if set to 0 104 | aug_grid_rows: 5 105 | aug_grid_cols: 6 106 | feature_idp_dim: 1 # 1 or 3 107 | 108 | # if apply Schmidt filter 109 | use_schmidt: 0 # 0(false) or 1(true) 110 | # left or right perturbation 111 | use_left_perturbation_flag: 0 # 0(right) or 1(left) 112 | # use euler method or closed form covariance propagation 113 | use_closed_form_cov_prop_flag: 1 # 0(euler method) or 1(closed form) 114 | # use original larvio 115 | use_larvio_flag: 0 # 0(orcvio) or 1(larvio) 116 | 117 | # the threshold for chi square 118 | chi_square_threshold_feat: 0.95 119 | # the threshold for cost in feature LM 120 | feature_cost_threshold: 4.7673e-04 121 | # the threshold for distance check in feature LM 122 | init_final_dist_threshold: 1e2 123 | # the flag whether we discard large update 124 | discard_large_update_flag: 0 125 | 126 | # whether to use object residual to update camera pose 127 | use_object_residual_update_cam_pose_flag: 0 # 0(not use) or 1(use) 128 | 129 | # whether we skip the update step 130 | prediction_only_flag: 0 131 | 132 | # if using ground-truth for initilization 133 | initial_use_gt: 0 134 | -------------------------------------------------------------------------------- /config/rs_d455.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # switches 4 | if_FEJ: 0 # 0(false) or 1(true) 5 | estimate_extrin: 0 # 0(false) or 1(true) 6 | estimate_td: 0 # 0(false) or 1(true) 7 | calib_imu_instrinsic: 0 # 0(false) or 1(true) 8 | 9 | # camera instrinsic 10 | camera_model: "pinhole" # only support "pinhole" 11 | distortion_model: "radtan" # only support "radtan" and "equidistant" 12 | resolution_width: 640 13 | resolution_height: 480 14 | intrinsics: 15 | fx: 387.47454833984375 16 | fy: 387.47454833984375 17 | cx: 316.1062927246094 18 | cy: 238.7586212158203 19 | distortion_coeffs: 20 | k1: 0.0 21 | k2: 0.0 22 | p1: 0.0 23 | p2: 0.0 24 | 25 | # imu-camera extrinsic, including spacial and temporal parameters 26 | # from IMU to camera, same with Kalibr 27 | T_cam_imu: !!opencv-matrix 28 | rows: 4 29 | cols: 4 30 | dt: d 31 | data: 32 | [0.9999887972997616, 0.004225660403089795, -0.002132854691210053, -0.06510852338269565, 33 | -0.004224561368945015, 0.9999909415437157, 0.0005195303196076303, -0.004589801762755343, 34 | 0.0021350307295388425, -0.0005105141239311359, 0.9999975905066537, -0.01765349149011269, 35 | 0., 0., 0., 1. ] 36 | 37 | td: 0 38 | 39 | # TODO: if calibrate camera instrinsic online 40 | 41 | # visual front-end parameters 42 | pyramid_levels: 2 43 | patch_size: 21 44 | fast_threshold: 30 45 | max_iteration: 30 46 | track_precision: 0.01 47 | ransac_threshold: 1 48 | max_features_num: 200 49 | min_distance: 20 50 | flag_equalize: 1 # 0(false) or 1(true) 51 | pub_frequency: 10 52 | 53 | # window size 54 | sw_size: 20 55 | 56 | # online reset thresholds 57 | position_std_threshold: 8.0 58 | rotation_threshold: 0.2618 59 | translation_threshold: 0.4 60 | tracking_rate_threshold: 0.5 61 | 62 | # feature triangulation parameters 63 | least_observation_number: 3 64 | max_track_len: 6 65 | feature_translation_threshold: -1.0 66 | 67 | # imu and camera measurement noise parameters 68 | noise_gyro: 0.004 69 | noise_acc: 0.08 70 | noise_gyro_bias: 2e-6 71 | noise_acc_bias: 4e-5 72 | noise_feature: 0.008 73 | 74 | # filter intial covariance 75 | initial_covariance_orientation: 4e-4 76 | initial_covariance_velocity: 0.25 77 | initial_covariance_position: 1.0 78 | initial_covariance_gyro_bias: 4e-4 79 | initial_covariance_acc_bias: 0.01 80 | initial_covariance_extrin_rot: 3.0462e-8 81 | initial_covariance_extrin_trans: 9e-8 82 | 83 | # fej settings 84 | reset_fej_threshold: 10.11 85 | 86 | # zupt settings 87 | if_ZUPT_valid: 1 # 0(false) or 1(true) 88 | 89 | if_use_feature_zupt_flag: 0 # 0(false) or 1(true) 90 | zupt_max_feature_dis: 2e-3 91 | 92 | zupt_noise_v: 1e-2 # std 93 | zupt_noise_p: 1e-2 94 | zupt_noise_q: 3.4e-2 95 | 96 | # static initialization setting 97 | static_duration: 1.0 98 | 99 | # measurement rate 100 | imu_rate: 200 101 | img_rate: 30 102 | 103 | # augmented feature state settings 104 | # a positive value is hybrid msckf 105 | max_features_in_one_grid: 0 # pure msckf if set to 0 106 | aug_grid_rows: 5 107 | aug_grid_cols: 6 108 | feature_idp_dim: 1 # 1 or 3 109 | 110 | # if apply Schmidt filter 111 | use_schmidt: 0 # 0(false) or 1(true) 112 | # left or right perturbation 113 | use_left_perturbation_flag: 0 # 0(right) or 1(left) 114 | # use euler method or closed form covariance propagation 115 | use_closed_form_cov_prop_flag: 1 # 0(euler method) or 1(closed form) 116 | # use original larvio 117 | use_larvio_flag: 0 # 0(orcvio) or 1(larvio) 118 | 119 | # the threshold for chi square 120 | chi_square_threshold_feat: 0.95 121 | # the threshold for cost in feature LM 122 | feature_cost_threshold: 4.7673e-04 123 | # the threshold for distance check in feature LM 124 | init_final_dist_threshold: 1e1 125 | # the flag whether we discard large update 126 | discard_large_update_flag: 1 127 | 128 | # whether to use object residual to update camera pose 129 | use_object_residual_update_cam_pose_flag: 0 # 0(not use) or 1(use) 130 | 131 | # whether we skip the update step 132 | prediction_only_flag: 0 133 | 134 | # if using ground-truth for initilization 135 | initial_use_gt: 0 136 | -------------------------------------------------------------------------------- /doc/object_mapping.md: -------------------------------------------------------------------------------- 1 | ## hyperparameters 2 | 3 | - in `OrcVIO-Object-Mapping/src/orcvio/obj/ObjectFeatureInitializer.cpp`, an initial scale `Eigen::Vector3d empirical_bbox_scale` is defined to accommodate the fact that the bounding box does not enclose the object tightly, this needs to be tuned for each type of object detection 4 | 5 | - in `OrcVIO-Object-Mapping/src/ObjectInitNode.cpp`, object distance thresholds `min_dist_btw_objects` and `max_dist_btw_cam_obj` are used to prune the objects that are too close to each other (possibly duplicates of the same object), and the object that is too far from the camera (possibly wrong object detection) 6 | 7 | - in `orcvio_mapping/launch/om_unity_gq.launch`, there are parameters related with the object tracking thresholds, `max_object_feature_track` and `min_object_feature_track`, which refers to the maximum/minimum number of frames to track an object 8 | * if an object is lost before `min_object_feature_track` is reached, then it is discarded 9 | * if an object is tracked long enough, which exceeds `max_object_feature_track`, then the object will be reconstructed and no longer tracked -------------------------------------------------------------------------------- /licenses/LICENSE_MSCKF_VIO.txt: -------------------------------------------------------------------------------- 1 | COPYRIGHT AND PERMISSION NOTICE 2 | Penn Software MSCKF_VIO 3 | Copyright (C) 2017 The Trustees of the University of Pennsylvania 4 | All rights reserved. 5 | 6 | The Trustees of the University of Pennsylvania (“Penn”) and Ke Sun, Kartik Mohta, the developer (“Developer”) of Penn Software MSCKF_VIO (“Software”) give recipient (“Recipient”) and Recipient’s Institution (“Institution”) permission to use, copy, and modify the software in source and binary forms, with or without modification for non-profit research purposes only provided that the following conditions are met: 7 | 8 | 1) All copies of Software in binary form and/or source code, related documentation and/or other materials provided with the Software must reproduce and retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 10 | 11 | 2) Recipient shall have the right to create modifications of the Software (“Modifications”) for their internal research and academic purposes only. 12 | 13 | 14 | 3) All copies of Modifications in binary form and/or source code and related documentation must reproduce and retain the above copyright notice, this list of conditions and the following disclaimer. 15 | 16 | 17 | 4) Recipient and Institution shall not distribute Software or Modifications to any third parties without the prior written approval of Penn. 18 | 19 | 20 | 5) Recipient will provide the Developer with feedback on the use of the Software and Modifications, if any, in their research. The Developers and Penn are permitted to use any information Recipient provides in making changes to the Software. All feedback, bug reports and technical questions shall be sent to: 21 | sunke@seas.upenn.edu 22 | kmohta@seas.upenn.edu 23 | 24 | 25 | 6) Recipient acknowledges that the Developers, Penn and its licensees may develop modifications to Software that may be substantially similar to Recipient’s modifications of Software, and that the Developers, Penn and its licensees shall not be constrained in any way by Recipient in Penn’s or its licensees’ use or management of such modifications. Recipient acknowledges the right of the Developers and Penn to prepare and publish modifications to Software that may be substantially similar or functionally equivalent to your modifications and improvements, and if Recipient or Institution obtains patent protection for any modification or improvement to Software, Recipient and Institution agree not to allege or enjoin infringement of their patent by the Developers, Penn or any of Penn’s licensees obtaining modifications or improvements to Software from the Penn or the Developers. 26 | 27 | 28 | 7) Recipient and Developer will acknowledge in their respective publications the contributions made to each other’s research involving or based on the Software. The current citations for Software are: 29 | 30 | 31 | 8) Any party desiring a license to use the Software and/or Modifications for commercial purposes shall contact The Penn Center for Innovation at 215-898-9591. 32 | 33 | 34 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS, CONTRIBUTORS, AND THE TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER, CONTRIBUTORS OR THE TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | 36 | 37 | -------------------------------------------------------------------------------- /licenses/LICENSE_ORB_SLAM2.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt). 2 | Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2. 3 | 4 | For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | https://github.com/raulmur/ORB_SLAM2 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/README: -------------------------------------------------------------------------------- 1 | # Catkin Tools Metadata 2 | 3 | This directory was generated by catkin_tools and it contains persistent 4 | configuration information used by the `catkin` command and its sub-commands. 5 | 6 | Each subdirectory of the `profiles` directory contains a set of persistent 7 | configuration options for separate profiles. The default profile is called 8 | `default`. If another profile is desired, it can be described in the 9 | `profiles.yaml` file in this directory. 10 | 11 | Please see the catkin_tools documentation before editing any files in this 12 | directory. Most actions can be performed with the `catkin` command-line 13 | program. 14 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/VERSION: -------------------------------------------------------------------------------- 1 | 0.4.5 -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/build.yaml: -------------------------------------------------------------------------------- 1 | blacklist: [] 2 | build_space: build 3 | catkin_make_args: [] 4 | cmake_args: 5 | - -DCMAKE_BUILD_TYPE=Release 6 | devel_layout: linked 7 | devel_space: devel 8 | extend_path: null 9 | install: false 10 | install_space: install 11 | isolate_install: false 12 | jobs_args: [] 13 | log_space: logs 14 | make_args: [] 15 | source_space: src 16 | use_env_cache: false 17 | use_internal_make_jobserver: true 18 | whitelist: [] 19 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/devel_collisions.txt: -------------------------------------------------------------------------------- 1 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./cmake.lock 4 2 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt: -------------------------------------------------------------------------------- 1 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/build/catkin_tools_prebuild 2 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/_setup_util.py /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./_setup_util.py 3 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/cmake.lock /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./cmake.lock 4 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/setup.zsh /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./setup.zsh 5 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./local_setup.zsh 6 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/setup.sh /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./setup.sh 7 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/env.sh /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./env.sh 8 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/local_setup.bash /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./local_setup.bash 9 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/local_setup.sh /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./local_setup.sh 10 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/setup.bash /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./setup.bash 11 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/lib/pkgconfig/catkin_tools_prebuild.pc 12 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake 13 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake 14 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml: -------------------------------------------------------------------------------- 1 | 2 | catkin_tools_prebuild 3 | 4 | This package is used to generate catkin setup files. 5 | 6 | 0.0.0 7 | BSD 8 | jbohren 9 | catkin 10 | 11 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/packages/darknet_ros/devel_manifest.txt: -------------------------------------------------------------------------------- 1 | darknet_ros/darknet_ros 2 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/cmake.lock /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/./cmake.lock 3 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/lib/libdarknet_ros_lib.so /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/lib/libdarknet_ros_lib.so 4 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/lib/libdarknet_ros_nodelet.so /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/lib/libdarknet_ros_nodelet.so 5 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/lib/pkgconfig/darknet_ros.pc /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/lib/pkgconfig/darknet_ros.pc 6 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/lib/darknet_ros/darknet_ros /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/lib/darknet_ros/darknet_ros 7 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/share/darknet_ros/cmake/darknet_rosConfig.cmake /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/share/darknet_ros/cmake/darknet_rosConfig.cmake 8 | /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/.private/darknet_ros/share/darknet_ros/cmake/darknet_rosConfig-version.cmake /home/erl/repos/OrcVIO-Lite/ros_wrapper/devel/share/darknet_ros/cmake/darknet_rosConfig-version.cmake 9 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/packages/darknet_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros 4 | 1.1.5 5 | Darknet is an open source neural network framework that runs on CPU and GPU. You only look once (YOLO) is a state-of-the-art, real-time object detection system. 6 | Marko Bjelonic 7 | BSD 8 | https://github.com/leggedrobotics/darknet_ros 9 | Marko Bjelonic 10 | 11 | catkin 12 | boost 13 | libopencv-dev 14 | libx11 15 | libxt-dev 16 | libxext 17 | 18 | roscpp 19 | rospy 20 | std_msgs 21 | image_transport 22 | cv_bridge 23 | sensor_msgs 24 | message_generation 25 | darknet_ros_msgs 26 | actionlib 27 | nodelet 28 | 29 | 30 | rostest 31 | wget 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /ros_wrapper/.catkin_tools/profiles/default/packages/darknet_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros_msgs 4 | 1.1.5 5 | Darknet is an open source neural network framework that runs on CPU and GPU. You only look once (YOLO) is a state-of-the-art, real-time object detection system. 6 | Marko Bjelonic 7 | BSD 8 | https://github.com/leggedrobotics/darknet_ros 9 | Marko Bjelonic 10 | 11 | catkin 12 | 13 | actionlib_msgs 14 | geometry_msgs 15 | sensor_msgs 16 | message_generation 17 | std_msgs 18 | 19 | actionlib_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | message_runtime 23 | std_msgs 24 | 25 | -------------------------------------------------------------------------------- /ros_wrapper/.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | .catkin_workspace -------------------------------------------------------------------------------- /ros_wrapper/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros_bridge 4 | 0.0.0 5 | The darknet_ros_bridge package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | darknet_ros_msgs 53 | geometry_msgs 54 | wm_od_interface_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Mo Shan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-d455.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [0.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689] 4 | - [0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053] 5 | - [0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [0.009694051100341156, -0.008084564032660335, 0.00022908109336942359, 10 | 0.0023177007244794693] 11 | distortion_model: radtan 12 | intrinsics: [384.17102476497377, 384.3372186440579, 323.7997751702461, 238.91110409779023] 13 | resolution: [640, 480] 14 | rostopic: /camera/infra1/image_rect_raw 15 | timeshift_cam_imu: 0.021933037220846576 16 | cam1: 17 | T_cam_imu: 18 | - [0.9999698968492514, -0.006847293164047317, -0.003649653630584302, -0.041744724619478486] 19 | - [0.006822507941589594, 0.9999538711144581, -0.006760845257577333, -0.02180719301602511] 20 | - [0.0036957787656450997, 0.006735741943954643, 0.9999704850643243, -0.023163825057369577] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | T_cn_cnm1: 23 | - [0.9999996707137908, 5.3416799524852884e-05, 0.0008097647507470238, -0.0504333570150308] 24 | - [-5.3648173678250714e-05, 0.9999999577455292, 0.00028571105951359323, -5.0779630172412783e-05] 25 | - [-0.0008097494547604562, -0.0002857544078328687, 0.9999996313250495, -0.00037767141222754295] 26 | - [0.0, 0.0, 0.0, 1.0] 27 | cam_overlaps: [0] 28 | camera_model: pinhole 29 | distortion_coeffs: [0.008722147503810068, -0.0071965817620313855, 0.00020312551594745345, 30 | 0.002072988775825771] 31 | distortion_model: radtan 32 | intrinsics: [384.31545240848607, 384.4493157119672, 323.8563013205691, 239.01554278327566] 33 | resolution: [640, 480] 34 | rostopic: /camera/infra2/image_rect_raw 35 | timeshift_cam_imu: 0.021948018259291602 36 | 37 | T_imu_body: 38 | [1.0000, 0.0000, 0.0000, 0.0000, 39 | 0.0000, 1.0000, 0.0000, 0.0000, 40 | 0.0000, 0.0000, 1.0000, 0.0000, 41 | 0.0000, 0.0000, 0.0000, 1.0000] -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-erl.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [0.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689] 4 | - [0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053] 5 | - [0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [0.009694051100341156, -0.008084564032660335, 0.00022908109336942359, 10 | 0.0023177007244794693] 11 | distortion_model: radtan 12 | intrinsics: [384.17102476497377, 384.3372186440579, 323.7997751702461, 238.91110409779023] 13 | resolution: [640, 480] 14 | rostopic: /camera/infra1/image_rect_raw 15 | timeshift_cam_imu: 0.021933037220846576 16 | cam1: 17 | T_cam_imu: 18 | - [0.9999698968492514, -0.006847293164047317, -0.003649653630584302, -0.041744724619478486] 19 | - [0.006822507941589594, 0.9999538711144581, -0.006760845257577333, -0.02180719301602511] 20 | - [0.0036957787656450997, 0.006735741943954643, 0.9999704850643243, -0.023163825057369577] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | T_cn_cnm1: 23 | - [0.9999996707137908, 5.3416799524852884e-05, 0.0008097647507470238, -0.0504333570150308] 24 | - [-5.3648173678250714e-05, 0.9999999577455292, 0.00028571105951359323, -5.0779630172412783e-05] 25 | - [-0.0008097494547604562, -0.0002857544078328687, 0.9999996313250495, -0.00037767141222754295] 26 | - [0.0, 0.0, 0.0, 1.0] 27 | cam_overlaps: [0] 28 | camera_model: pinhole 29 | distortion_coeffs: [0.008722147503810068, -0.0071965817620313855, 0.00020312551594745345, 30 | 0.002072988775825771] 31 | distortion_model: radtan 32 | intrinsics: [384.31545240848607, 384.4493157119672, 323.8563013205691, 239.01554278327566] 33 | resolution: [640, 480] 34 | rostopic: /camera/infra2/image_rect_raw 35 | timeshift_cam_imu: 0.021948018259291602 36 | 37 | T_imu_body: 38 | [1.0000, 0.0000, 0.0000, 0.0000, 39 | 0.0000, 1.0000, 0.0000, 0.0000, 40 | 0.0000, 0.0000, 1.0000, 0.0000, 41 | 0.0000, 0.0000, 0.0000, 1.0000] -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-euroc-noextrinsics.yaml: -------------------------------------------------------------------------------- 1 | # The modifications of the output file from Kalibr: 2 | # 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix. 3 | # 2. Add the T_imu_body at the end of the calibration file (usually set to identity). 4 | cam0: 5 | T_cam_imu: 6 | [0.0, 1.0, 0.0, 0.05, 7 | -1.0, 0.0, 0.0, 0.0, 8 | 0.0, 0.0, 1.0, 0.0, 9 | 0.0, 0.0, 0.0, 1.0] 10 | camera_model: pinhole 11 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 12 | distortion_model: radtan 13 | intrinsics: [458.654, 457.296, 367.215, 248.375] 14 | resolution: [752, 480] 15 | timeshift_cam_imu: 0.0 16 | cam1: 17 | T_cam_imu: 18 | [0.012555267089103, 0.999598781151433, -0.025389800891747, -0.044901980682509, 19 | -0.999755099723116, 0.013011905181504, 0.017900583825251, -0.020569771258915, 20 | 0.018223771455443, 0.025158836311552, 0.999517347077547, -0.008638135126028, 21 | 0, 0, 0, 1.000000000000000] 22 | T_cn_cnm1: 23 | [0.999997256477881, 0.002312067192424, 0.000376008102415, -0.110073808127187, 24 | -0.002317135723281, 0.999898048506644, 0.014089835846648, 0.000399121547014, 25 | -0.000343393120525, -0.014090668452714, 0.999900662637729, -0.000853702503357, 26 | 0, 0, 0, 1.000000000000000] 27 | camera_model: pinhole 28 | distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 29 | distortion_model: radtan 30 | intrinsics: [457.587, 456.134, 379.999, 255.238] 31 | resolution: [752, 480] 32 | timeshift_cam_imu: 0.0 33 | T_imu_body: 34 | [1.0000, 0.0000, 0.0000, 0.0000, 35 | 0.0000, 1.0000, 0.0000, 0.0000, 36 | 0.0000, 0.0000, 1.0000, 0.0000, 37 | 0.0000, 0.0000, 0.0000, 1.0000] 38 | 39 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-euroc.yaml: -------------------------------------------------------------------------------- 1 | # The modifications of the output file from Kalibr: 2 | # 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix. 3 | # 2. Add the T_imu_body at the end of the calibration file (usually set to identity). 4 | cam0: 5 | T_cam_imu: 6 | [0.014865542981794, 0.999557249008346, -0.025774436697440, 0.065222909535531, 7 | -0.999880929698575, 0.014967213324719, 0.003756188357967, -0.020706385492719, 8 | 0.004140296794224, 0.025715529947966, 0.999660727177902, -0.008054602460030, 9 | 0, 0, 0, 1.000000000000000] 10 | camera_model: pinhole 11 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 12 | distortion_model: radtan 13 | intrinsics: [458.654, 457.296, 367.215, 248.375] 14 | resolution: [752, 480] 15 | timeshift_cam_imu: 0.0 16 | cam1: 17 | T_cam_imu: 18 | [0.012555267089103, 0.999598781151433, -0.025389800891747, -0.044901980682509, 19 | -0.999755099723116, 0.013011905181504, 0.017900583825251, -0.020569771258915, 20 | 0.018223771455443, 0.025158836311552, 0.999517347077547, -0.008638135126028, 21 | 0, 0, 0, 1.000000000000000] 22 | T_cn_cnm1: 23 | [0.999997256477881, 0.002312067192424, 0.000376008102415, -0.110073808127187, 24 | -0.002317135723281, 0.999898048506644, 0.014089835846648, 0.000399121547014, 25 | -0.000343393120525, -0.014090668452714, 0.999900662637729, -0.000853702503357, 26 | 0, 0, 0, 1.000000000000000] 27 | camera_model: pinhole 28 | distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 29 | distortion_model: radtan 30 | intrinsics: [457.587, 456.134, 379.999, 255.238] 31 | resolution: [752, 480] 32 | timeshift_cam_imu: 0.0 33 | T_imu_body: 34 | [1.0000, 0.0000, 0.0000, 0.0000, 35 | 0.0000, 1.0000, 0.0000, 0.0000, 36 | 0.0000, 0.0000, 1.0000, 0.0000, 37 | 0.0000, 0.0000, 0.0000, 1.0000] 38 | 39 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-fla.yaml: -------------------------------------------------------------------------------- 1 | # The modifications of the output file from Kalibr: 2 | # 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix. 3 | # 2. Add the T_imu_body at the end of the calibration file (usually set to identity). 4 | cam0: 5 | T_cam_imu: 6 | [0.9999354187625251, -0.008299416281612066, 0.007763890364879687, 0.1058254769257812, 7 | -0.008323202950480819, -0.9999607512746784, 0.003036478688450292, -0.017679034754829417, 8 | 0.0077383846414136375, -0.0031009030240912025, -0.9999652502980172, -0.008890355463642424, 9 | 0.0, 0.0, 0.0, 1.0] 10 | camera_model: pinhole 11 | distortion_coeffs: [-0.0147, -0.00584, 0.00715, -0.00464] 12 | distortion_model: equidistant 13 | intrinsics: [606.57934, 606.73233, 474.93081, 402.27722] 14 | resolution: [960, 800] 15 | rostopic: /sync/cam0/image_raw 16 | cam1: 17 | T_cam_imu: 18 | [0.9999415499962613, -0.003314180239269491, 0.010291394483555276, -0.09412457355264209, 19 | -0.0029741561536762054, -0.9994548489877669, -0.032880985843089204, -0.016816910884051604, 20 | 0.010394757632964142, 0.03284845573511056, -0.9994062877376598, -0.008908185988981819, 21 | 0.0, 0.0, 0.0, 1.0] 22 | T_cn_cnm1: 23 | [0.9999843700393054, -0.004977416649937181, -0.0025428275521419763, -0.200059, 24 | 0.005065643350062822, 0.9993405242433947, 0.03595604029590122, 0.000634053, 25 | 0.002362182447858024, -0.03596835970409878, 0.9993501279159637, -0.000909473, 26 | 0.0, 0.0, 0.0, 1.0] 27 | camera_model: pinhole 28 | distortion_coeffs: [-0.01565, -0.00026, -0.00454, 0.00311] 29 | distortion_model: equidistant 30 | intrinsics: [605.7278, 605.75661, 481.98865, 417.83476] 31 | resolution: [960, 800] 32 | rostopic: /sync/cam1/image_raw 33 | T_imu_body: 34 | [1.0000, 0.0000, 0.0000, 0.0000, 35 | 0.0000, 1.0000, 0.0000, 0.0000, 36 | 0.0000, 0.0000, 1.0000, 0.0000, 37 | 0.0000, 0.0000, 0.0000, 1.0000] 38 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/camchain-imucam-unity-gq.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [0.0, -1.0, 0.0, 0.1] 4 | - [0.0, 0.0, -1.0, 0.0] 5 | - [1.0, 0.0, 0.0, -0.1] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [0, 0, 0, 0] 10 | distortion_model: radtan 11 | intrinsics: [260.99805320956386, 260.99805320956386, 320, 240] 12 | resolution: [640, 480] 13 | rostopic: /husky1/camera_left/image 14 | timeshift_cam_imu: 0.0 15 | cam1: 16 | T_cam_imu: 17 | - [0.0, -1.0, 0.0, -0.1] 18 | - [0.0, 0.0, -1.0, 0] 19 | - [1.0, 0.0, 0.0, -0.1] 20 | - [0.0, 0.0, 0.0, 1.0] 21 | T_cn_cnm1: 22 | - [1.0, 0.0, 0.0, -0.2] 23 | - [0.0, 1.0, 0.0, 0.0] 24 | - [0.0, 0.0, 1.0, 0.0] 25 | - [0.0, 0.0, 0.0, 1.0] 26 | cam_overlaps: [0] 27 | camera_model: pinhole 28 | distortion_coeffs: [0, 0, 0, 0] 29 | distortion_model: radtan 30 | intrinsics: [260.99805320956386, 260.99805320956386, 320, 240] 31 | resolution: [640, 480] 32 | rostopic: /husky1/camera_right/image 33 | timeshift_cam_imu: 0.0 34 | 35 | T_imu_body: 36 | [1.0000, 0.0000, 0.0000, 0.0000, 37 | 0.0000, 1.0000, 0.0000, 0.0000, 38 | 0.0000, 0.0000, 1.0000, 0.0000, 39 | 0.0000, 0.0000, 0.0000, 1.0000] 40 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/euroc_config/MH_01_easy.yaml: -------------------------------------------------------------------------------- 1 | path_gt: "/home/erl/dcist_vio_workspace/stereo_orcvio_ws/src/slam_groundtruth_comparison/data/euroc_mav/MH_01_easy.csv" 2 | 3 | gtTslam: 4 | [0.328175, -0.944612, 0.00311312, 5.02224, 5 | 0.944259, 0.328139, 0.0264596, -1.85008, 6 | -0.0260156, -0.00574378, 0.999645, 0.453058, 7 | 0.0, 0.0, 0.0, 1.0] -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/config/euroc_config/V1_01_easy.yaml: -------------------------------------------------------------------------------- 1 | path_gt: "/home/erl/dcist_vio_workspace/stereo_orcvio_ws/src/slam_groundtruth_comparison/data/euroc_mav/V1_01_easy.csv" 2 | 3 | gtTslam: 4 | [-0.973632, 0.222831, -0.0488523, 0.811762, 5 | -0.223219, -0.974765, 0.00256102, 2.33515, 6 | -0.0470488, 0.0133983, 0.998803, 0.857574, 7 | 0.0, 0.0, 0.0, 1.0] -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/FlexibleInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "initializer/StaticInitializer.h" 9 | #include "initializer/DynamicInitializer.h" 10 | 11 | namespace orcvio { 12 | 13 | class FlexibleInitializer 14 | { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | // Constructor 19 | FlexibleInitializer() = delete; 20 | FlexibleInitializer(const double& acc_n_, const double& acc_w_, const double& gyr_n_, 21 | const double& gyr_w_, const Eigen::Matrix3d& R_c2b, 22 | const Eigen::Vector3d& t_bc_b) { 23 | 24 | staticInitPtr.reset(new StaticInitializer()); 25 | dynamicInitPtr.reset(new DynamicInitializer(acc_n_, acc_w_, gyr_n_, 26 | gyr_w_, R_c2b, t_bc_b)); 27 | 28 | } 29 | 30 | // Destructor 31 | ~FlexibleInitializer(){} 32 | 33 | // Interface for trying to initialize 34 | bool tryIncInit(std::vector& imu_msg_buffer, 35 | CameraMeasurementConstPtr img_msg, IMUState& imu_state); 36 | 37 | private: 38 | 39 | // Inclinometer-initializer 40 | boost::shared_ptr staticInitPtr; 41 | // Dynamic initializer 42 | boost::shared_ptr dynamicInitPtr; 43 | 44 | 45 | }; 46 | 47 | } -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/StaticInitializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "orcvio/state.h" 14 | #include "orcvio/feature.h" 15 | #include "orcvio/CameraMeasurement.h" 16 | 17 | namespace orcvio { 18 | 19 | class StaticInitializer 20 | { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | // Constructor 25 | StaticInitializer() : 26 | max_feature_dis(2e-3), 27 | bInit(false), 28 | lower_time_bound(0.0) 29 | { 30 | staticImgCounter = 0; 31 | init_features.clear(); 32 | gyro_bias = Eigen::Vector3d::Zero(); 33 | acc_bias = Eigen::Vector3d::Zero(); 34 | position = Eigen::Vector3d::Zero(); 35 | velocity = Eigen::Vector3d::Zero(); 36 | 37 | orientation = Eigen::Matrix3d::Identity(); 38 | 39 | // FIXEME: load from launch file 40 | static_Num = 20; 41 | } 42 | 43 | // Destructor 44 | ~StaticInitializer(){} 45 | 46 | // Interface for trying to initialize 47 | bool tryIncInit(const std::vector& imu_msg_buffer, 48 | CameraMeasurementConstPtr img_msg); 49 | 50 | // Assign the initial state if initialized successfully 51 | void assignInitialState(std::vector& imu_msg_buffer, IMUState& imu_state); 52 | 53 | // If initialized 54 | bool ifInitialized() { 55 | return bInit; 56 | } 57 | 58 | private: 59 | 60 | // Time lower bound for used imu data. 61 | double lower_time_bound; 62 | 63 | // Maximum feature distance allowed bewteen static images 64 | double max_feature_dis; 65 | 66 | // Number of consecutive image for trigger static initializer 67 | unsigned int static_Num; 68 | 69 | // Defined type for initialization 70 | typedef std::map, 71 | Eigen::aligned_allocator< 72 | std::pair > > InitFeatures; 73 | InitFeatures init_features; 74 | 75 | // Counter for static images that will be used in inclinometer-initializer 76 | unsigned int staticImgCounter; 77 | 78 | // Initialize results 79 | double state_time; 80 | Eigen::Vector3d gyro_bias; 81 | Eigen::Vector3d acc_bias; 82 | 83 | // Eigen::Vector4d orientation; 84 | Eigen::Matrix3d orientation; 85 | 86 | Eigen::Vector3d position; 87 | Eigen::Vector3d velocity; 88 | 89 | // Flag indicating if initialized 90 | bool bInit; 91 | 92 | /** 93 | * @brief initializegravityAndBias 94 | * Initialize the IMU bias and initial orientation based on the first few IMU readings. 95 | * @details take the average of first N frames, average acceleration norm is gravity norm, 96 | * average angular velocity is gyro bias 97 | * IMU needs to be static for the first N frames 98 | */ 99 | void initializeGravityAndBias(const double& time_bound, 100 | const std::vector& imu_msg_buffer) noexcept; 101 | }; 102 | 103 | } // namespace orcvio -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/feature_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "orcvio/FeatureMeasurement.h" 11 | #include "orcvio/CameraMeasurement.h" 12 | 13 | namespace { 14 | 15 | const int WINDOW_SIZE = 10; 16 | const double MIN_PARALLAX = 10/460; 17 | 18 | } 19 | 20 | namespace orcvio { 21 | 22 | class FeaturePerFrame 23 | { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | FeaturePerFrame(const FeatureMeasurement& _point, double td) 28 | { 29 | point.x() = _point.u0; 30 | point.y() = _point.v0; 31 | point.z() = 1; 32 | cur_td = td; 33 | } 34 | double cur_td; 35 | Eigen::Vector3d point; 36 | double z; 37 | bool is_used; 38 | double parallax; 39 | Eigen::MatrixXd A; 40 | Eigen::VectorXd b; 41 | double dep_gradient; 42 | }; 43 | 44 | class FeaturePerId 45 | { 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | const int feature_id; 50 | int start_frame; 51 | std::vector feature_per_frame; 52 | 53 | int used_num; 54 | bool is_outlier; 55 | bool is_margin; 56 | double estimated_depth; 57 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 58 | 59 | Eigen::Vector3d gt_p; 60 | 61 | FeaturePerId(int _feature_id, int _start_frame) 62 | : feature_id(_feature_id), start_frame(_start_frame), 63 | used_num(0), estimated_depth(-1.0), solve_flag(0) 64 | { 65 | } 66 | 67 | int endFrame(); 68 | }; 69 | 70 | class FeatureManager 71 | { 72 | public: 73 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 74 | 75 | FeatureManager(){}; 76 | 77 | void setRic(const Eigen::Matrix3d& _ric); 78 | 79 | void clearState(); 80 | 81 | int getFeatureCount(); 82 | 83 | bool addFeatureCheckParallax(int frame_count, CameraMeasurementConstPtr image, double td); 84 | 85 | std::vector> getCorresponding(int frame_count_l, int frame_count_r); 86 | 87 | //void updateDepth(const VectorXd &x); 88 | void setDepth(const Eigen::VectorXd &x); 89 | void removeFailures(); 90 | void clearDepth(const Eigen::VectorXd &x); 91 | Eigen::VectorXd getDepthVector(); 92 | void removeBack(); 93 | void removeFront(int frame_count); 94 | void removeOutlier(); 95 | std::list feature; 96 | int last_track_num; 97 | 98 | private: 99 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 100 | Eigen::Matrix3d ric; 101 | }; 102 | 103 | } 104 | 105 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/initial_alignment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "orcvio/feature.h" 10 | #include "orcvio/CameraMeasurement.h" 11 | #include "initializer/ImuPreintegration.h" 12 | #include "initializer/feature_manager.h" 13 | 14 | namespace orcvio { 15 | 16 | class ImageFrame 17 | { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | ImageFrame(){}; 22 | ImageFrame(CameraMeasurementConstPtr _points, const double& td):is_key_frame{false} 23 | { 24 | t = _points->header.stamp.toSec() + td; 25 | for (const auto& pt : _points->features) 26 | { 27 | // only x, y values are used 28 | double x = pt.u0; 29 | double y = pt.v0; 30 | double z = 1; 31 | Eigen::Vector3d xyz; 32 | xyz << x, y, z; 33 | points[pt.id] = xyz; 34 | } 35 | }; 36 | // points are used in initialStructure 37 | // only x, y values are used 38 | std::map points; 39 | double t; 40 | Eigen::Matrix3d R; 41 | Eigen::Vector3d T; 42 | boost::shared_ptr pre_integration; 43 | bool is_key_frame; 44 | }; 45 | 46 | bool VisualIMUAlignment(std::map &all_image_frame, Eigen::Vector3d* Bgs, Eigen::Vector3d &g, Eigen::VectorXd &x, const Eigen::Vector3d& TIC); 47 | 48 | } 49 | 50 | 51 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/initial_sfm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace orcvio { 14 | 15 | struct SFMFeature 16 | { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | bool state; 20 | int id; 21 | std::vector> observation; 22 | double position[3]; 23 | double depth; 24 | }; 25 | 26 | struct ReprojectionError3D 27 | { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | ReprojectionError3D(double observed_u, double observed_v) 31 | :observed_u(observed_u), observed_v(observed_v) 32 | {} 33 | 34 | template 35 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 36 | { 37 | T p[3]; 38 | ceres::QuaternionRotatePoint(camera_R, point, p); 39 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 40 | T xp = p[0] / p[2]; 41 | T yp = p[1] / p[2]; 42 | residuals[0] = xp - T(observed_u); 43 | residuals[1] = yp - T(observed_v); 44 | return true; 45 | } 46 | 47 | static ceres::CostFunction* Create(const double observed_x, 48 | const double observed_y) 49 | { 50 | return (new ceres::AutoDiffCostFunction< 51 | ReprojectionError3D, 2, 4, 3, 3>( 52 | new ReprojectionError3D(observed_x,observed_y))); 53 | } 54 | 55 | double observed_u; 56 | double observed_v; 57 | }; 58 | 59 | class GlobalSFM 60 | { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | 64 | GlobalSFM(); 65 | bool construct(int frame_num, Eigen::Quaterniond* q, Eigen::Vector3d* T, int l, 66 | const Eigen::Matrix3d relative_R, const Eigen::Vector3d relative_T, 67 | std::vector &sfm_f, std::map &sfm_tracked_points); 68 | 69 | private: 70 | bool solveFrameByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, int i, std::vector &sfm_f); 71 | 72 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 73 | Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d); 74 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 75 | int frame1, Eigen::Matrix &Pose1, 76 | std::vector &sfm_f); 77 | 78 | int feature_num; 79 | }; 80 | 81 | } 82 | 83 | 84 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/math_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace orcvio { 8 | 9 | /* 10 | * @brief Convert the vector part of a quaternion to a 11 | * full quaternion. 12 | * @note This function is useful to convert delta quaternion 13 | * which is usually a 3x1 vector to a full quaternion. 14 | * For more details, check Section 3.2 "Kalman Filter Update" in 15 | * "Indirect Kalman Filter for 3D Attitude Estimation: 16 | * A Tutorial for quaternion Algebra". 17 | */ 18 | inline Eigen::Quaterniond getSmallAngleQuaternion( 19 | const Eigen::Vector3d& dtheta) { 20 | 21 | Eigen::Vector3d dq = dtheta / 2.0; 22 | Eigen::Quaterniond q; 23 | double dq_square_norm = dq.squaredNorm(); 24 | 25 | if (dq_square_norm <= 1) { 26 | q.x() = dq(0); 27 | q.y() = dq(1); 28 | q.z() = dq(2); 29 | q.w() = std::sqrt(1-dq_square_norm); 30 | } else { 31 | q.x() = dq(0); 32 | q.y() = dq(1); 33 | q.z() = dq(2); 34 | q.w() = 1; 35 | q.normalize(); 36 | } 37 | 38 | return q; 39 | 40 | } 41 | 42 | 43 | /* 44 | * @brief Convert a quaternion to the corresponding rotation matrix 45 | * @note Pay attention to the convention used. The function follows the 46 | * conversion in "Indirect Kalman Filter for 3D Attitude Estimation: 47 | * A Tutorial for Quaternion Algebra", Equation (78). 48 | * 49 | * The input quaternion should be in the form 50 | * [q1, q2, q3, q4(scalar)]^T 51 | */ 52 | inline Eigen::Matrix3d quaternionToRotation( 53 | const Eigen::Vector4d& q) { 54 | // Hamilton 55 | const double& qw = q(3); 56 | const double& qx = q(0); 57 | const double& qy = q(1); 58 | const double& qz = q(2); 59 | Eigen::Matrix3d R; 60 | R(0, 0) = 1-2*(qy*qy+qz*qz); R(0, 1) = 2*(qx*qy-qw*qz); R(0, 2) = 2*(qx*qz+qw*qy); 61 | R(1, 0) = 2*(qx*qy+qw*qz); R(1, 1) = 1-2*(qx*qx+qz*qz); R(1, 2) = 2*(qy*qz-qw*qx); 62 | R(2, 0) = 2*(qx*qz-qw*qy); R(2, 1) = 2*(qy*qz+qw*qx); R(2, 2) = 1-2*(qx*qx+qy*qy); 63 | 64 | return R; 65 | } 66 | 67 | 68 | } -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/initializer/solve_5pts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | namespace orcvio { 9 | 10 | class MotionEstimator 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | bool solveRelativeRT(const std::vector> &corres, Eigen::Matrix3d &R, Eigen::Vector3d &T); 16 | }; 17 | 18 | } 19 | 20 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/orcvio/euroc_gt.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "dataset_reader.h" 20 | 21 | // ref https://gist.github.com/kartikmohta/67cafc968ba5146bc6dcaf721e61b914 22 | 23 | namespace orcvio 24 | { 25 | 26 | /** 27 | * @brief This class takes in published euroc gt and converts it to path for rviz 28 | */ 29 | class EuRoCPub { 30 | 31 | public: 32 | 33 | /** 34 | * @brief Default constructor 35 | */ 36 | EuRoCPub(ros::NodeHandle& nh, std::string path_gt, std::string topic_est) { 37 | 38 | DatasetReader::load_gt_file(path_gt, gt_states); 39 | pub_gt_path = nh.advertise("/orcvio/gt_path", 2); 40 | sub_est_pose = nh.subscribe(topic_est, 9999, &EuRoCPub::gt_odom_path_cb, this); 41 | 42 | nh.param("output_dir_traj", output_dir_traj, "./cache/"); 43 | if (!boost::filesystem::exists(output_dir_traj)) 44 | { 45 | // if this dir does not exist, create the dir 46 | const char *path = output_dir_traj.c_str(); 47 | boost::filesystem::path dir(path); 48 | if (boost::filesystem::create_directories(dir)) 49 | { 50 | std::cerr << "Directory Created: " << output_dir_traj << std::endl; 51 | } 52 | } 53 | fStateToSave.open((output_dir_traj+"stamped_groundtruth.txt").c_str(), std::ofstream::trunc); 54 | 55 | first_pose_flag = true; 56 | 57 | std::vector matrix_gtTslam; 58 | std::vector matrix_gtTslam_default = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; 59 | nh.param>("gtTslam", matrix_gtTslam, matrix_gtTslam_default); 60 | int i = 0; 61 | gtTslam.matrix() << matrix_gtTslam.at(0), matrix_gtTslam.at(1), matrix_gtTslam.at(2), matrix_gtTslam.at(3), 62 | matrix_gtTslam.at(4), matrix_gtTslam.at(5), matrix_gtTslam.at(6), matrix_gtTslam.at(7), 63 | matrix_gtTslam.at(8), matrix_gtTslam.at(9), matrix_gtTslam.at(10), matrix_gtTslam.at(11), 64 | matrix_gtTslam.at(12), matrix_gtTslam.at(13), matrix_gtTslam.at(14), matrix_gtTslam.at(15); 65 | 66 | slamTgt = gtTslam.inverse(); 67 | 68 | }; 69 | 70 | /** 71 | * @brief Default desctructor 72 | */ 73 | ~EuRoCPub() 74 | { 75 | fStateToSave.close(); 76 | } 77 | 78 | /** 79 | * @brief callback function to convert odometry to path 80 | * @param a pointer to the gt odometry 81 | */ 82 | void gt_odom_path_cb(const nav_msgs::Odometry::ConstPtr &est_odom_ptr); 83 | 84 | nav_msgs::Path path; 85 | 86 | Eigen::Matrix3d R0; 87 | Eigen::Vector3d t0; 88 | 89 | ros::Publisher pub_gt_path; 90 | // Our groundtruth states 91 | std::map> gt_states; 92 | ros::Subscriber sub_est_pose; 93 | 94 | std::string output_dir_traj; 95 | std::ofstream fStateToSave; 96 | 97 | bool first_pose_flag; 98 | 99 | // for aligning gt and orcvio 100 | Eigen::Isometry3d gtTslam, slamTgt; 101 | }; 102 | 103 | } 104 | 105 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/orcvio/image_processor_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace orcvio { 15 | class ImageProcessorNodelet : public nodelet::Nodelet { 16 | public: 17 | ImageProcessorNodelet() { return; } 18 | ~ImageProcessorNodelet() { return; } 19 | 20 | private: 21 | virtual void onInit(); 22 | ImageProcessorPtr img_processor_ptr; 23 | }; 24 | } // end namespace orcvio 25 | 26 | 27 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/orcvio/orcvio_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace orcvio { 15 | class OrcVioNodelet : public nodelet::Nodelet { 16 | public: 17 | OrcVioNodelet() { return; } 18 | ~OrcVioNodelet() { return; } 19 | 20 | private: 21 | virtual void onInit(); 22 | OrcVioPtr orcvio_ptr; 23 | }; 24 | } // end namespace orcvio 25 | 26 | 27 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/orcvio/state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define GRAVITY_ACCELERATION 9.81 16 | 17 | namespace orcvio { 18 | 19 | /** 20 | * @brief IMUState State for IMU 21 | */ 22 | struct IMUState { 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | typedef long long int StateIDType; 26 | 27 | // An unique identifier for the IMU state. 28 | StateIDType id; 29 | 30 | // id for next IMU state 31 | static StateIDType next_id; 32 | 33 | // Time when the state is recorded 34 | double time; 35 | 36 | // Orientation q_I_G 37 | // Take a vector from the world frame to the IMU (body) frame. 38 | Eigen::Matrix3d orientation; 39 | 40 | // Position of the IMU (body) frame in the world frame. 41 | Eigen::Vector3d position; 42 | 43 | // Velocity of the IMU (body) frame in the world frame. 44 | Eigen::Vector3d velocity; 45 | 46 | // Bias for measured angular velocity and acceleration. 47 | Eigen::Vector3d gyro_bias; 48 | Eigen::Vector3d acc_bias; 49 | 50 | // Transformation between the IMU and the left camera (cam0) 51 | Eigen::Matrix3d R_imu_cam0; 52 | Eigen::Vector3d t_cam0_imu; 53 | 54 | 55 | // Process noise 56 | static double gyro_noise; 57 | static double acc_noise; 58 | static double gyro_bias_noise; 59 | static double acc_bias_noise; 60 | 61 | // Gravity vector in the world frame 62 | static Eigen::Vector3d gravity; 63 | 64 | // Transformation offset from the IMU frame to the body frame. 65 | // The transformation takes a vector from the IMU frame to the body frame. 66 | // The z axis of the body frame should point upwards. 67 | // Normally, this transform should be identity. 68 | static Eigen::Isometry3d T_imu_body; 69 | 70 | IMUState() : id(0), 71 | time(0), 72 | orientation(Eigen::Matrix3d::Identity()), 73 | position(Eigen::Vector3d::Zero()), 74 | velocity(Eigen::Vector3d::Zero()), 75 | gyro_bias(Eigen::Vector3d::Zero()), 76 | acc_bias(Eigen::Vector3d::Zero()) 77 | {} 78 | 79 | IMUState(const StateIDType& new_id) : 80 | id(new_id), 81 | time(0), 82 | orientation(Eigen::Matrix3d::Identity()), 83 | position(Eigen::Vector3d::Zero()), 84 | velocity(Eigen::Vector3d::Zero()), 85 | gyro_bias(Eigen::Vector3d::Zero()), 86 | acc_bias(Eigen::Vector3d::Zero()) 87 | {} 88 | }; 89 | 90 | typedef IMUState::StateIDType StateIDType; 91 | 92 | /* 93 | * @brief CAMState Stored camera state in order to form measurement model. 94 | */ 95 | struct CAMState { 96 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 97 | 98 | // An unique identifier for the CAM state. 99 | StateIDType id; 100 | 101 | // Time when the state is recorded 102 | double time; 103 | 104 | // Orientation Take a vector from the camera frame to the world frame. 105 | Eigen::Matrix3d orientation; 106 | 107 | // First estimated Jacobian of orientation; 108 | Eigen::Matrix3d orientation_FEJ; 109 | 110 | // Position of the camera frame in the world frame. 111 | Eigen::Vector3d position; 112 | 113 | // First estimated Jacobian of position; 114 | Eigen::Vector3d position_FEJ; 115 | 116 | // Takes a vector from the cam0 frame to the cam1 frame. 117 | static Eigen::Isometry3d T_cam0_cam1; 118 | 119 | CAMState() : 120 | id(0), 121 | time(0), 122 | orientation(Eigen::Matrix3d::Identity()), 123 | orientation_FEJ(Eigen::Matrix3d::Identity()), 124 | position(Eigen::Vector3d::Zero()), 125 | position_FEJ(Eigen::Vector3d::Zero()) 126 | {} 127 | 128 | CAMState(const StateIDType& new_id) : 129 | id(new_id), 130 | time(0), 131 | orientation(Eigen::Matrix3d::Identity()), 132 | orientation_FEJ(Eigen::Matrix3d::Identity()), 133 | position(Eigen::Vector3d::Zero()), 134 | position_FEJ(Eigen::Vector3d::Zero()) 135 | {} 136 | }; 137 | 138 | typedef std::map, 139 | Eigen::aligned_allocator > > 140 | CamStateServer; 141 | 142 | } // namespace orcvio 143 | 144 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/orcvio/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace orcvio { 21 | 22 | /* 23 | * @brief utilities for orcvio 24 | */ 25 | namespace utils { 26 | 27 | Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) noexcept; 28 | 29 | Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field) noexcept; 30 | 31 | cv::Mat getTransformCV(const ros::NodeHandle &nh, const std::string &field); 32 | 33 | cv::Mat getVec16Transform(const ros::NodeHandle &nh, const std::string &field); 34 | 35 | cv::Mat getKalibrStyleTransform(const ros::NodeHandle &nh, const std::string &field); 36 | 37 | /* 38 | functions used in propagation start 39 | */ 40 | 41 | Eigen::Matrix3d Hl_operator(const Eigen::Vector3d& gyro) noexcept; 42 | Eigen::Matrix3d Jl_operator(const Eigen::Vector3d& gyro) noexcept; 43 | 44 | /* 45 | functions used in propagation end 46 | */ 47 | 48 | 49 | /* 50 | functions used in update start 51 | */ 52 | 53 | Eigen::Matrix get_cam_wrt_imu_se3_jacobian(const Eigen::Matrix3d& R_b2c, const Eigen::Vector3d& t_c_b, const Eigen::Matrix3d& R_w2c) noexcept; 54 | Eigen::Matrix odotOperator(const Eigen::Vector4d& x) noexcept; 55 | 56 | /* 57 | functions used in update end 58 | */ 59 | 60 | } // namespace utils 61 | 62 | } // namespace orcvio -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/image_processor_d455.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/image_processor_euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/image_processor_fla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/image_processor_unity_gq.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/orcvio_d455.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 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 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/orcvio_euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 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 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 85 | ?> 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/orcvio_euroc_eval.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 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 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/orcvio_fla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/orcvio_unity_gq_husky.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 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 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/reset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/msg/CameraMeasurement.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # All features on the current image, 3 | # including tracked ones and newly detected ones. 4 | FeatureMeasurement[] features 5 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/msg/FeatureMeasurement.msg: -------------------------------------------------------------------------------- 1 | uint64 id 2 | # Normalized feature coordinates (with identity intrinsic matrix) 3 | float64 u0 # horizontal coordinate in cam0 4 | float64 v0 # vertical coordinate in cam0 5 | float64 u1 # horizontal coordinate in cam0 6 | float64 v1 # vertical coordinate in cam0 7 | 8 | # feature pixel coordinates 9 | float64 pu0 10 | float64 pv0 11 | float64 pu1 12 | float64 pv1 13 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/msg/TrackingInfo.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Number of features after each outlier removal step. 4 | int16 before_tracking 5 | int16 after_tracking 6 | int16 after_matching 7 | int16 after_ransac 8 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | OrcVIO Object residual constrained Visual-Inertial Odometry 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | Detect and track features in image sequence. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | orcvio 5 | 0.0.1 6 | OrcVIO Object residual constrained Visual-Inertial Odometry 7 | 8 | Mo Shan 9 | MIT Software License 10 | 11 | Mo Shan 12 | 13 | catkin 14 | 15 | roscpp 16 | std_msgs 17 | tf 18 | nav_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | eigen_conversions 22 | tf_conversions 23 | random_numbers 24 | nodelet 25 | image_transport 26 | cv_bridge 27 | message_filters 28 | pcl_conversions 29 | pcl_ros 30 | std_srvs 31 | message_generation 32 | message_runtime 33 | 34 | libpcl-all-dev 35 | libpcl-all 36 | suitesparse 37 | 38 | rosunit 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/scripts/batch_run_euroc.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | #-------------------------------------------------------------------------------------------- 5 | # for EuRoC dataset start 6 | #-------------------------------------------------------------------------------------------- 7 | 8 | euroc_dataset_path = '/media/erl/disk2/euroc/' 9 | euroc_csv_path = '/home/erl/dcist_vio_workspace/stereo_orcvio_ws/src/slam_groundtruth_comparison/data/euroc_mav/' 10 | 11 | #-------------------------------------------------------------------------------------------- 12 | # for EuRoC dataset end 13 | #-------------------------------------------------------------------------------------------- 14 | 15 | orcvio_root_path = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '..', '')) 16 | euroc_cache_path = orcvio_root_path + "/cache/euroc/" 17 | launch_file_name = "orcvio_euroc_eval.launch" 18 | launch_file = orcvio_root_path + '/launch/' + launch_file_name 19 | bag_dir = euroc_dataset_path 20 | 21 | keywords_to_avoid = { 22 | "output_dir_traj", "output_dir_log", "rosbag" 23 | } 24 | 25 | # bag_name:bag_start in sec 26 | bag_list = { 27 | 'MH_01_easy':40, 28 | 'MH_02_easy':35, 29 | 'MH_03_medium':15, 30 | 'MH_04_difficult':20, 31 | 'MH_05_difficult':20, 32 | 'V1_01_easy':0, 33 | 'V1_02_medium':0, 34 | 'V1_03_difficult':0, 35 | 'V2_01_easy':0, 36 | 'V2_02_medium':0 37 | } 38 | 39 | run_cmd = 'roslaunch orcvio ' + launch_file_name 40 | 41 | def run_once(): 42 | os.system(run_cmd) 43 | return 44 | 45 | # [name:(type,value)] 46 | def modify_launch(params): 47 | lines = open(launch_file, "r").readlines() 48 | fp = open(launch_file, "w") 49 | for line in lines: 50 | valid_flag = True 51 | for name in params.keys(): 52 | if name in line: 53 | for kw in keywords_to_avoid: 54 | if kw in line: 55 | valid_flag = False 56 | if (valid_flag): 57 | a, b = params[name] 58 | if (name == 'path_gt'): 59 | line = ' \n'%(name,a,b) 60 | else: 61 | line = ' \n'%(name,b) 62 | break 63 | fp.write(line) 64 | fp.close() 65 | 66 | def loop_rosbag(): 67 | params = {} 68 | for bag, start_ts in bag_list.items(): 69 | fbag = os.path.join(bag_dir, bag + '.bag') 70 | fcsv = euroc_csv_path + "%s.csv"%bag 71 | fresult = os.path.join(euroc_cache_path, bag) + "/" 72 | if os.path.exists(fbag): 73 | params['path_gt'] = ('string', fcsv) 74 | params['path_bag'] = ('string', fbag) 75 | params['bag_start'] = ('string', str(start_ts)) 76 | params['path_traj'] = ('string', fresult) 77 | modify_launch(params) 78 | print("run {}".format(bag)) 79 | res = run_once() 80 | return 81 | 82 | if __name__ == '__main__': 83 | 84 | # clean roslog 85 | os.system('rm -rf ~/.ros/log') 86 | 87 | loop_rosbag() -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/FlexibleInitializer.cpp: -------------------------------------------------------------------------------- 1 | #include "initializer/FlexibleInitializer.h" 2 | 3 | namespace orcvio { 4 | 5 | bool FlexibleInitializer::tryIncInit(std::vector& imu_msg_buffer, 6 | CameraMeasurementConstPtr img_msg, IMUState& imu_state) { 7 | 8 | if(staticInitPtr->tryIncInit(imu_msg_buffer, img_msg)) { 9 | staticInitPtr->assignInitialState(imu_msg_buffer, imu_state); 10 | return true; 11 | } 12 | else if (dynamicInitPtr->tryDynInit(imu_msg_buffer, img_msg)) { 13 | dynamicInitPtr->assignInitialState(imu_msg_buffer, imu_state); 14 | return true; 15 | } 16 | 17 | return false; 18 | } 19 | 20 | 21 | } 22 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/euroc_gt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "orcvio/euroc_gt.h" 13 | 14 | namespace orcvio 15 | { 16 | 17 | // this subscribes to the estimated pose 18 | void EuRoCPub::gt_odom_path_cb(const nav_msgs::Odometry::ConstPtr &est_odom_ptr){ 19 | 20 | double timestamp = est_odom_ptr->header.stamp.toSec(); 21 | // Our groundtruth state 22 | Eigen::Matrix state_gt; 23 | 24 | // Check that we have the timestamp in our GT file [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel] 25 | if(!DatasetReader::get_gt_state(timestamp, state_gt, gt_states)) { 26 | return; 27 | } 28 | 29 | Eigen::Vector4d q_gt; 30 | Eigen::Vector3d t_gt; 31 | 32 | q_gt << state_gt(1,0),state_gt(2,0),state_gt(3,0),state_gt(4,0); 33 | t_gt << state_gt(5,0),state_gt(6,0),state_gt(7,0); 34 | 35 | Eigen::Quaterniond q_in = Eigen::Quaterniond(q_gt); 36 | 37 | if (first_pose_flag) 38 | { 39 | t0 = t_gt; 40 | // convert to rotation matrix 41 | R0 = q_in.normalized().toRotationMatrix(); 42 | 43 | // align the estimated pose and groundtruth pose 44 | t0 = slamTgt.linear() * t0 + slamTgt.translation(); 45 | R0 = slamTgt.linear() * R0; 46 | 47 | first_pose_flag = false; 48 | } 49 | 50 | Eigen::Vector3d t1, t1_new; 51 | t1 = t_gt; 52 | 53 | // convert to rotation matrix 54 | Eigen::Matrix3d R1, R_new; 55 | R1 = q_in.normalized().toRotationMatrix(); 56 | 57 | // align the estimated pose and groundtruth pose 58 | t1_new = slamTgt.linear() * t1 + slamTgt.translation(); 59 | // initial position is 0 60 | t1_new = t1_new - t0; 61 | // NOTE, rotation is not identity since orcvio initializes using gravity 62 | R_new = slamTgt.linear() * R1; 63 | 64 | // convert to quaternion 65 | Eigen::Quaterniond q1_normalized = Eigen::Quaterniond(R_new); 66 | // normalize the quaternion 67 | q1_normalized = q1_normalized.normalized(); 68 | 69 | geometry_msgs::PoseStamped cur_pose; 70 | cur_pose.header = est_odom_ptr->header; 71 | cur_pose.header.frame_id = "/global"; 72 | 73 | cur_pose.pose.position.x = t1_new(0,0); 74 | cur_pose.pose.position.y = t1_new(1,0); 75 | cur_pose.pose.position.z = t1_new(2,0); 76 | 77 | cur_pose.pose.orientation.x = q1_normalized.x(); 78 | cur_pose.pose.orientation.y = q1_normalized.y(); 79 | cur_pose.pose.orientation.z = q1_normalized.z(); 80 | cur_pose.pose.orientation.w = q1_normalized.w(); 81 | 82 | path.header = cur_pose.header; 83 | path.header.frame_id = "/global"; 84 | path.poses.push_back(cur_pose); 85 | 86 | pub_gt_path.publish(path); 87 | 88 | // save the pose to txt for trajectory evaluation 89 | // ============================ 90 | // TUM format 91 | // timestamp tx ty tz qx qy qz qw 92 | fStateToSave << std::fixed << std::setprecision(3) << cur_pose.header.stamp.toSec(); 93 | fStateToSave << " " 94 | << cur_pose.pose.position.x << " " << cur_pose.pose.position.y << " " << cur_pose.pose.position.z << " " 95 | << cur_pose.pose.orientation.x << " " << cur_pose.pose.orientation.y << " " << cur_pose.pose.orientation.z << " " << cur_pose.pose.orientation.w << std::endl; 96 | 97 | 98 | return; 99 | 100 | } 101 | 102 | } -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/image_processor_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #include 9 | 10 | namespace orcvio { 11 | 12 | void ImageProcessorNodelet::onInit() { 13 | img_processor_ptr.reset(new ImageProcessor(getPrivateNodeHandle())); 14 | if (!img_processor_ptr->initialize()) { 15 | ROS_ERROR("Cannot initialize Image Processor..."); 16 | return; 17 | } 18 | return; 19 | } 20 | 21 | PLUGINLIB_EXPORT_CLASS(orcvio::ImageProcessorNodelet, nodelet::Nodelet); 22 | 23 | } // end namespace orcvio 24 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/orcvio_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #include 9 | 10 | namespace orcvio { 11 | 12 | void OrcVioNodelet::onInit() { 13 | orcvio_ptr.reset(new OrcVio(getPrivateNodeHandle())); 14 | if (!orcvio_ptr->initialize()) { 15 | ROS_ERROR("Cannot initialize OrcVIO..."); 16 | return; 17 | } 18 | return; 19 | } 20 | 21 | PLUGINLIB_EXPORT_CLASS(orcvio::OrcVioNodelet, nodelet::Nodelet); 22 | 23 | } // end namespace orcvio 24 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/publish_euroc_gt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "orcvio/euroc_gt.h" 6 | 7 | using namespace orcvio; 8 | 9 | int main(int argc, char **argv) { 10 | 11 | // Create ros node 12 | ros::init(argc, argv, "publish_gt_path"); 13 | ros::NodeHandle nh("~"); 14 | 15 | // Get parameters to subscribe 16 | std::string path_gt, topic_est; 17 | nh.getParam("path_gt", path_gt); 18 | nh.getParam("topic_pose_est", topic_est); 19 | 20 | // Debug 21 | ROS_INFO("Done reading config values"); 22 | ROS_INFO(" - path for gt pose = %s", path_gt.c_str()); 23 | ROS_INFO(" - topic for estimated pose = %s", topic_est.c_str()); 24 | 25 | EuRoCPub euroc_pub(nh, path_gt, topic_est); 26 | 27 | // Done! 28 | ros::spin(); 29 | return EXIT_SUCCESS; 30 | 31 | } -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/test/feature_initialization_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "orcvio/cam_state.h" 19 | #include "orcvio/feature.h" 20 | 21 | using namespace std; 22 | using namespace Eigen; 23 | using namespace orcvio; 24 | 25 | // Static member variables in CAMState class 26 | Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); 27 | 28 | // Static member variables in Feature class 29 | Feature::OptimizationConfig Feature::optimization_config; 30 | 31 | TEST(FeatureInitializeTest, sphereDistribution) { 32 | // Set the real feature at the origin of the world frame. 33 | Vector3d feature(0.5, 0.0, 0.0); 34 | 35 | // Add 6 camera poses, all of which are able to see the 36 | // feature at the origin. For simplicity, the six camera 37 | // view are located at the six intersections between a 38 | // unit sphere and the coordinate system. And the z axes 39 | // of the camera frames are facing the origin. 40 | vector cam_poses(6); 41 | // Positive x axis. 42 | cam_poses[0].linear() << 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0, 0.0; 43 | cam_poses[0].translation() << 1.0, 0.0, 0.0; 44 | // Positive y axis. 45 | cam_poses[1].linear() << -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, -1.0, 0.0; 46 | cam_poses[1].translation() << 0.0, 1.0, 0.0; 47 | // Negative x axis. 48 | cam_poses[2].linear() << 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0; 49 | cam_poses[2].translation() << -1.0, 0.0, 0.0; 50 | // Negative y axis. 51 | cam_poses[3].linear() << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0; 52 | cam_poses[3].translation() << 0.0, -1.0, 0.0; 53 | // Positive z axis. 54 | cam_poses[4].linear() << 0.0, -1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0; 55 | cam_poses[4].translation() << 0.0, 0.0, 1.0; 56 | // Negative z axis. 57 | cam_poses[5].linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; 58 | cam_poses[5].translation() << 0.0, 0.0, -1.0; 59 | 60 | // Set the camera states 61 | CamStateServer cam_states; 62 | for (int i = 0; i < 6; ++i) { 63 | CAMState new_cam_state; 64 | new_cam_state.id = i; 65 | new_cam_state.time = static_cast(i); 66 | new_cam_state.orientation = Matrix3d(cam_poses[i].linear()); 67 | new_cam_state.position = cam_poses[i].translation(); 68 | cam_states[new_cam_state.id] = new_cam_state; 69 | } 70 | 71 | // Compute measurements. 72 | random_numbers::RandomNumberGenerator noise_generator; 73 | vector > measurements(6); 74 | for (int i = 0; i < 6; ++i) { 75 | Isometry3d cam_pose_inv = cam_poses[i].inverse(); 76 | Vector3d p = cam_pose_inv.linear() * feature + cam_pose_inv.translation(); 77 | double u = p(0) / p(2) + noise_generator.gaussian(0.0, 0.01); 78 | double v = p(1) / p(2) + noise_generator.gaussian(0.0, 0.01); 79 | //double u = p(0) / p(2); 80 | //double v = p(1) / p(2); 81 | measurements[i] = Vector4d(u, v, u, v); 82 | } 83 | 84 | for (int i = 0; i < 6; ++i) { 85 | cout << "pose " << i << ":" << endl; 86 | cout << "orientation: " << endl; 87 | cout << cam_poses[i].linear() << endl; 88 | cout << "translation: " << endl; 89 | cout << cam_poses[i].translation().transpose() << endl; 90 | cout << "measurement: " << endl; 91 | cout << measurements[i].transpose() << endl; 92 | cout << endl; 93 | } 94 | 95 | // Initialize a feature object. 96 | Feature feature_object; 97 | for (int i = 0; i < 6; ++i) feature_object.observations[i] = measurements[i]; 98 | 99 | // Compute the 3d position of the feature. 100 | feature_object.initializePosition(cam_states); 101 | 102 | // Check the difference between the computed 3d 103 | // feature position and the groud truth. 104 | cout << "ground truth position: " << feature.transpose() << endl; 105 | cout << "estimated position: " << feature_object.position.transpose() << endl; 106 | Eigen::Vector3d error = feature_object.position - feature; 107 | EXPECT_NEAR(error.norm(), 0, 0.05); 108 | } 109 | 110 | int main(int argc, char** argv) { 111 | testing::InitGoogleTest(&argc, argv); 112 | return RUN_ALL_TESTS(); 113 | } 114 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/test/math_utils_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * UCSD Software ORCVIO 4 | * Copyright (C) 2021 5 | * All rights reserved. 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "orcvio/utils.h" 14 | 15 | using namespace std; 16 | using namespace Eigen; 17 | using namespace orcvio; 18 | 19 | TEST(MathUtilsTest, skewSymmetric) { 20 | Vector3d w(1.0, 2.0, 3.0); 21 | Matrix3d w_hat = utils::skewSymmetric(w); 22 | Vector3d zero_vector = w_hat * w; 23 | 24 | FullPivLU lu_helper(w_hat); 25 | EXPECT_EQ(lu_helper.rank(), 2); 26 | EXPECT_DOUBLE_EQ(zero_vector.norm(), 0.0); 27 | return; 28 | } 29 | 30 | 31 | int main(int argc, char** argv) { 32 | testing::InitGoogleTest(&argc, argv); 33 | return RUN_ALL_TESTS(); 34 | } 35 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(orcvio_mapping) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") 6 | # set(CMAKE_BUILD_TYPE "Debug") 7 | #set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g -ggdb") 8 | 9 | add_compile_options(-std=c++11) 10 | 11 | # Modify cmake module path if new .cmake files are required 12 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 13 | 14 | find_package(catkin REQUIRED COMPONENTS 15 | roscpp 16 | std_msgs 17 | tf 18 | nav_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | eigen_conversions 22 | tf_conversions 23 | tf2_ros 24 | nodelet 25 | image_transport 26 | cv_bridge 27 | message_filters 28 | pcl_conversions 29 | pcl_ros 30 | std_srvs 31 | sort_ros 32 | ) 33 | find_package(arl_perception_msgs QUIET) 34 | if (${arl_perception_msgs_FOUND}) 35 | ## message("arl_perception_msgs_FOUND; ${arl_perception_msgs_FOUND}") 36 | add_definitions("-DENABLE_ARL_PERCEPTION_MSGS") 37 | include_directories(${arl_perception_msgs_INCLUDE_DIRS}) 38 | endif() 39 | 40 | ## System dependencies are found with CMake's conventions 41 | find_package(Boost REQUIRED) 42 | find_package(Eigen3 REQUIRED) 43 | find_package(OpenCV REQUIRED) 44 | find_package(SuiteSparse REQUIRED) 45 | find_package(Ceres REQUIRED) 46 | find_package(Sophus QUIET) 47 | if (NOT ${Sophus_FOUND}) 48 | find_package(sophus REQUIRED) 49 | endif() 50 | 51 | ################################### 52 | ## catkin specific configuration ## 53 | ################################### 54 | catkin_package( 55 | INCLUDE_DIRS include 56 | CATKIN_DEPENDS 57 | roscpp std_msgs tf nav_msgs sensor_msgs geometry_msgs 58 | eigen_conversions tf_conversions message_runtime 59 | image_transport cv_bridge message_filters pcl_conversions 60 | pcl_ros std_srvs 61 | DEPENDS Boost EIGEN3 OpenCV SUITESPARSE 62 | ) 63 | 64 | ########### 65 | ## Build ## 66 | ########### 67 | 68 | include_directories( 69 | include 70 | ${catkin_INCLUDE_DIRS} 71 | ${EIGEN3_INCLUDE_DIR} 72 | ${Boost_INCLUDE_DIR} 73 | ${OpenCV_INCLUDE_DIRS} 74 | ${SUITESPARSE_INCLUDE_DIRS} 75 | ${CERES_INCLUDE_DIRS} 76 | ${Sophus_INCLUDE_DIRS} 77 | ) 78 | 79 | 80 | # Object Mapper 81 | add_library(orcvio_mapping_core 82 | src/ObjectInitNode.cpp 83 | src/orcvio/obj/ObjectFeature.cpp 84 | src/orcvio/obj/ObjectFeatureInitializer.cpp 85 | src/orcvio/obj/ObjectLM.cpp 86 | src/orcvio/obj/ObjectLMLite.cpp 87 | src/orcvio/obj/ObjectResJacCam.cpp 88 | src/orcvio/obj/ObjectState.cpp 89 | src/orcvio/feat/kf.cpp 90 | src/orcvio/feat/Feature.cpp 91 | src/orcvio/feat/FeatureInitializer.cpp 92 | ) 93 | 94 | add_dependencies(orcvio_mapping_core 95 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 96 | ${catkin_EXPORTED_TARGETS} 97 | ) 98 | 99 | target_link_libraries(orcvio_mapping_core 100 | ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} 101 | ${arl_perception_msgs_LIBRARIES} 102 | ) 103 | 104 | # Object Mapper node 105 | add_executable(orcvio_mapping_node 106 | src/orcvio_mapping_node.cpp 107 | ) 108 | 109 | add_dependencies(orcvio_mapping_node 110 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 111 | ${catkin_EXPORTED_TARGETS} 112 | ) 113 | target_link_libraries(orcvio_mapping_node 114 | orcvio_mapping_core 115 | ${catkin_LIBRARIES} 116 | ) 117 | 118 | 119 | 120 | ############# 121 | ## Install ## 122 | ############# 123 | 124 | #install(TARGETS 125 | # orcvio_mapping_node 126 | # orcvio_mapping_node 127 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 129 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 130 | #) 131 | 132 | #install(DIRECTORY include/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # PATTERN "*_nodelet.h" EXCLUDE 135 | #) 136 | 137 | #install(DIRECTORY 138 | # launch 139 | # ${PROJECT_SOURCE_DIR}/../../../config 140 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 141 | # ) 142 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 ERL 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/README.md: -------------------------------------------------------------------------------- 1 | # about 2 | 3 | This repo is used in OrcVIO [project website](http://me-llamo-sean.cf/orcvio_githubpage/), [Journal version](https://arxiv.org/abs/2007.15107) 4 | 5 | If you find this repo useful, kindly cite our publications 6 | 7 | ``` 8 | @article{orcvio, 9 | title={OrcVIO: Object residual constrained Visual-Inertial Odometry}, 10 | author={M. {Shan} and Q. {Feng} and N. {Atanasov}}, 11 | url = {http://erl.ucsd.edu/pages/orcvio.html}, 12 | pdf = {https://arxiv.org/abs/2007.15107}, 13 | journal={IEEE Transactions on Robotics}, 14 | volume={}, 15 | number={}, 16 | pages={}, 17 | year={2021}, 18 | publisher={IEEE} 19 | } 20 | ``` 21 | 22 | # orcvio mapping demo 23 | 24 | - Download rosbag: [link]( https://www.dropbox.com/sh/bovb04lrb0l745r/AAA69peH_ELWLNzvkTwoeRB4a?dl=0&lst=&preview=indoor_vio5_2020-11-03-18-44-15.bag) 25 | 26 | - Run 27 | 28 | ``` 29 | roslaunch orcvio_mapping orcvio_object_mapping_dcist.launch rosbag_path:=/Download Folder/darknet_sort-ros_2021-04-11-19-46-21.bag 30 | ``` 31 | 32 | * Parameters 33 | 34 | * min_dist_btw_objects: minimum distance between two objects. 35 | 36 | * max_dist_btw_cam_obj: maximum distance between the first viewpoint and object. 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/config/medfield.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # output directory 4 | output_dir: "/home/erl/Workspace/orcvio-lite/cache/" 5 | 6 | # switches 7 | if_FEJ: 0 # 0(false) or 1(true) 8 | estimate_extrin: 0 # 0(false) or 1(true) 9 | estimate_td: 0 # 0(false) or 1(true) 10 | calib_imu_instrinsic: 0 # 0(false) or 1(true) 11 | 12 | # camera instrinsic 13 | camera_model: "pinhole" # only support "pinhole" 14 | distortion_model: "radtan" # only support "radtan" and "equidistant" 15 | resolution_width: 640 16 | resolution_height: 512 17 | intrinsics: 18 | fx: 484.0 19 | fy: 484.0 20 | cx: 320.0 21 | cy: 262.5 22 | distortion_coeffs: 23 | k1: 0 24 | k2: 0 25 | p1: 0 26 | p2: 0 27 | 28 | # imu-camera extrinsic, including spacial and temporal parameters 29 | T_cam_imu: !!opencv-matrix 30 | rows: 4 31 | cols: 4 32 | dt: d 33 | data: 34 | [ 35 | 0, -1, 0, 0.0, 36 | 0.0, 0, -1, 0, 37 | 1, 0, 0, 0, 38 | 0., 0., 0., 1. 39 | ] 40 | td: 0 41 | 42 | # TODO: if calibrate camera instrinsic online 43 | 44 | # visual front-end parameters 45 | pyramid_levels: 2 46 | patch_size: 21 47 | fast_threshold: 30 48 | max_iteration: 30 49 | track_precision: 0.01 50 | ransac_threshold: 1 51 | max_features_num: 200 52 | min_distance: 20 53 | flag_equalize: 1 # 0(false) or 1(true) 54 | pub_frequency: 10 55 | 56 | # window size 57 | sw_size: 20 58 | 59 | # online reset thresholds 60 | position_std_threshold: 8.0 61 | rotation_threshold: 0.2618 62 | translation_threshold: 0.4 63 | tracking_rate_threshold: 0.5 64 | 65 | # feature triangulation parameters 66 | least_observation_number: 3 67 | max_track_len: 6 68 | feature_translation_threshold: -1.0 69 | 70 | # imu and camera measurement noise parameters 71 | noise_gyro: 0.04 72 | noise_acc: 0.8 73 | noise_gyro_bias: 2e-5 74 | noise_acc_bias: 4e-4 75 | noise_feature: 0.008 76 | 77 | # filter intial covariance 78 | initial_covariance_orientation: 4e-4 79 | initial_covariance_velocity: 0.25 80 | initial_covariance_position: 1.0 81 | initial_covariance_gyro_bias: 4e-4 82 | initial_covariance_acc_bias: 0.01 83 | initial_covariance_extrin_rot: 3.0462e-8 84 | initial_covariance_extrin_trans: 9e-8 85 | 86 | # fej settings 87 | reset_fej_threshold: 10.11 88 | 89 | # zupt settings 90 | if_ZUPT_valid: 1 # 0(false) or 1(true) 91 | 92 | if_use_feature_zupt_flag: 0 # 0(false) or 1(true) 93 | zupt_max_feature_dis: 2e-3 94 | 95 | zupt_noise_v: 1e-2 # std 96 | zupt_noise_p: 1e-2 97 | zupt_noise_q: 3.4e-2 98 | 99 | # static initialization setting 100 | static_duration: 1.0 101 | 102 | # measurement rate 103 | # imu_rate: 100 104 | imu_rate: 200 105 | img_rate: 60 106 | 107 | # augmented feature state settings 108 | # a positive value is hybrid msckf 109 | max_features_in_one_grid: 0 # pure msckf if set to 0 110 | aug_grid_rows: 5 111 | aug_grid_cols: 6 112 | feature_idp_dim: 1 # 1 or 3 113 | 114 | # if apply Schmidt filter 115 | use_schmidt: 0 # 0(false) or 1(true) 116 | # left or right perturbation 117 | use_left_perturbation_flag: 0 # 0(right) or 1(left) 118 | # use euler method or closed form covariance propagation 119 | use_closed_form_cov_prop_flag: 1 # 0(euler method) or 1(closed form) 120 | # use original larvio 121 | use_larvio_flag: 0 # 0(orcvio) or 1(larvio) 122 | 123 | # the threshold for chi square 124 | chi_square_threshold_feat: 0.95 125 | # the threshold for cost in feature LM 126 | feature_cost_threshold: 4.7673e-04 127 | # the threshold for distance check in feature LM 128 | init_final_dist_threshold: 1e1 129 | # the flag whether we discard large update 130 | discard_large_update_flag: 0 131 | 132 | # if using ground-truth for initilization 133 | initial_use_gt: 0 -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/config/object_feat_erl.yaml: -------------------------------------------------------------------------------- 1 | object_classes: 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # for chair category 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | chair: 8 | keypoints_num: 10 9 | object_keypoints_mean: 10 | [-0.16260029, -0.18636511, 0.16428348, 0.18096888, -0.174444 , 11 | -0.19166636, 0.1767874 , 0.18786542, -0.19818088, 0.19658388, 12 | 0.17052046, 0.13219248, 0.17011265, 0.13199527, -0.19604203, 13 | -0.21056669, -0.19605567, -0.20875473, 0.19473798, 0.19352854, 14 | -0.44795494, -0.00819907, -0.44773116, -0.00803745, -0.44755495, 15 | -0.01097987, -0.44755495, -0.01101388, 0.55173099, 0.55204506] 16 | 17 | object_mean_shape: 18 | [0.4, 0.4, 1] 19 | 20 | aliases: 21 | - bench 22 | marker_color: [1.0, 0.0, 0.0] 23 | 24 | #-------------------------------------------------------------------------------------------- 25 | # other configs 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | # whether to fine tune object pose using Levenberg Marquardt 29 | # do_fine_tune_object_pose_using_lm: false 30 | do_fine_tune_object_pose_using_lm: true 31 | 32 | # The directory where result files are saved per object 33 | result_dir_path_object_map: "/home/erl/repos/OrcVIO-Lite/cache/object_map/" 34 | 35 | #-------------------------------------------------------------------------------------------- 36 | # groundtruth object states 37 | #-------------------------------------------------------------------------------------------- 38 | 39 | # note, these cannot be empty even if we don't have groundtruth object states 40 | 41 | total_object_num: 22 42 | 43 | objects_ids_gt: [1, 4, 7, 10, 13, 16, 1, 2, 3, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 44 | 15, 16, 17] 45 | 46 | objects_class_gt: [chair, chair, chair, chair, chair, chair, chair, chair, tvmonitor, tvmonitor, tvmonitor, tvmonitor, 47 | tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitors] 48 | 49 | objects_rotation_gt: 50 | [2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 51 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 52 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 53 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 54 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 55 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 56 | -0.0, -0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 57 | 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 58 | 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 59 | 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 60 | 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 61 | 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0008028513696686779, 62 | 0.9999996777147871, 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 63 | 0.0, 1.0, 0.0008028513696686779, 0.9999996777147871, 0.0, -0.9999996777147871, 64 | 0.0008028513696686779, -0.0, -0.0, 0.0, 1.0, 0.0008028513696686779, 0.9999996777147871, 65 | 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 0.0, 1.0, 0.0008028513696686779, 66 | 0.9999996777147871, 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 67 | 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 68 | 0.0, 0.0, 0.0, 1.0] 69 | 70 | objects_translation_gt: 71 | [5.0, 5.5, 0.0, 20.0, 0.5, 0.0, 35.0, 5.5, 0.0, -35.0, 5.5, 72 | 0.0, -15.0, 5.5, 0.0, -30.0, 0.5, 0.0, -49.09, -10.76, 1.57, -29.59, -14.41, 1.57, 73 | -26.561, -14.41, 1.57, -8.986, 10.935, 1.602, -5.21, 10.935, 1.602, -5.547, -4.08, 74 | 1.602, -1.66, -4.08, 1.602, 12.78, -4.519, 1.602, 12.461, 17.475, 1.602, 15.332, 75 | 17.475, 1.602, 17.3, 12.505, 1.602, 17.3, 15.349, 1.602, 27.47, -5.178, 1.602, 76 | 27.47, -8.96, 1.602, -49.14, -10.74, 1.63, -38.582, 13.98, 1.63] 77 | 78 | first_uav_translation_gt: 79 | [1, 1, 0.3] 80 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/feat/Feature.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_H 2 | #define FEATURE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace orcvio 10 | { 11 | 12 | /** 13 | * @brief Sparse feature class used to collect measurements 14 | * 15 | * This feature class allows for holding of all tracking information for a given feature. 16 | * Each feature has a unique ID assigned to it, and should have a set of feature tracks alongside it. 17 | * See the FeatureDatabase class for details on how we load information into this, and how we delete features. 18 | */ 19 | class Feature 20 | { 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | /// Unique ID of this feature 26 | size_t featid; 27 | 28 | /// If this feature should be deleted 29 | bool to_delete; 30 | 31 | /// UV coordinates that this feature has been seen from (mapped by camera ID) 32 | std::unordered_map> uvs; 33 | 34 | /// UV normalized coordinates that this feature has been seen from (mapped by camera ID) 35 | std::unordered_map> uvs_norm; 36 | 37 | /// Timestamps of each UV measurement (mapped by camera ID) 38 | std::unordered_map> timestamps; 39 | 40 | /// What camera ID our pose is anchored in!! By default the first measurement is the anchor. 41 | int anchor_cam_id = -1; 42 | 43 | /// Timestamp of anchor clone 44 | double anchor_clone_timestamp; 45 | 46 | /// Triangulated position of this feature, in the anchor frame 47 | Eigen::Vector3d p_FinA; 48 | 49 | /// Triangulated position of this feature, in the global frame 50 | Eigen::Vector3d p_FinG; 51 | 52 | /** 53 | * @brief Remove measurements that do not occur at passed timestamps. 54 | * 55 | * Given a series of valid timestamps, this will remove all measurements that have not occurred at these times. 56 | * This would normally be used to ensure that the measurements that we have occur at our clone times. 57 | * 58 | * @param valid_times Vector of timestamps that our measurements must occur at 59 | */ 60 | void clean_old_measurements(std::vector valid_times); 61 | 62 | /** 63 | * @brief Remove measurements that are older then the specified timestamp. 64 | * 65 | * Given a valid timestamp, this will remove all measurements that have occured earlier then this. 66 | * 67 | * @param timestamp Timestamps that our measurements must occur after 68 | */ 69 | void clean_older_measurements(double timestamp); 70 | 71 | }; // end of Feature 72 | 73 | } // namespace orcvio 74 | 75 | #endif /* FEATURE_H */ -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/feat/FeatureInitializerOptions.h: -------------------------------------------------------------------------------- 1 | #ifndef INITIALIZEROPTIONS_H 2 | #define INITIALIZEROPTIONS_H 3 | 4 | namespace orcvio { 5 | 6 | /** 7 | * @brief Struct which stores all our feature initializer options 8 | */ 9 | struct FeatureInitializerOptions { 10 | 11 | /// Max runs for Gauss Newton 12 | int max_runs = 20; 13 | 14 | /// Init lambda for LM optimization 15 | double init_lamda = 1e-3; 16 | 17 | /// Max lambda for LM optimization 18 | double max_lamda = 1e10; 19 | 20 | /// Cutoff for dx increment to consider as converged 21 | double min_dx = 1e-6; 22 | 23 | /// Cutoff for cost decrement to consider as converged 24 | double min_dcost = 1e-6; 25 | 26 | /// Multiplier to increase/decrease lambda 27 | double lam_mult = 10; 28 | 29 | /// Minimum distance to accept triangulated features 30 | double min_dist = 0.25; 31 | 32 | /// Minimum distance to accept triangulated features 33 | double max_dist = 40; 34 | 35 | /// Max baseline ratio to accept triangulated features 36 | double max_baseline = 40; 37 | 38 | /// Max condition number of linear triangulation matrix accept triangulated features 39 | double max_cond_number = 1000; 40 | 41 | /// Nice print function of what parameters we have loaded 42 | void print() { 43 | printf("\t- max_runs: %d\n", max_runs); 44 | printf("\t- init_lamda: %.3f\n", init_lamda); 45 | printf("\t- max_lamda: %.3f\n", max_lamda); 46 | printf("\t- min_dx: %.7f\n", min_dx); 47 | printf("\t- min_dcost: %.7f\n", min_dcost); 48 | printf("\t- lam_mult: %.3f\n", lam_mult); 49 | printf("\t- min_dist: %.3f\n", min_dist); 50 | printf("\t- max_dist: %.3f\n", max_dist); 51 | printf("\t- max_baseline: %.3f\n", max_baseline); 52 | printf("\t- max_cond_number: %.3f\n", max_cond_number); 53 | } 54 | 55 | }; 56 | 57 | } 58 | 59 | #endif // INITIALIZEROPTIONS_H 60 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/feat/feature_msg.h: -------------------------------------------------------------------------------- 1 | // 2 | // Managing the image processer and the estimator. 3 | // 4 | 5 | #ifndef FEATURE_MSG_H 6 | #define FEATURE_MSG_H 7 | 8 | #include 9 | #include 10 | 11 | namespace orcvio 12 | { 13 | 14 | // Measurement for one features 15 | class MonoFeatureMeasurement 16 | { 17 | public: 18 | MonoFeatureMeasurement() 19 | : id(0), u(0.0), v(0.0), u_init(0.0), v_init(0.0), u_vel(0.0), v_vel(0.0), u_init_vel(0.0), v_init_vel(0.0) 20 | { 21 | } 22 | 23 | // id 24 | unsigned long long int id; 25 | // Normalized feature coordinates (with identity intrinsic matrix) 26 | double u; // horizontal coordinate 27 | double v; // vertical coordinate 28 | // Normalized feature coordinates (with identity intrinsic matrix) in initial frame of this feature 29 | //# (meaningful if this is the first msg of this feature id) 30 | double u_init; 31 | double v_init; 32 | // Velocity of current normalized feature coordinate 33 | double u_vel; 34 | double v_vel; 35 | // Velocity of initial normalized feature coordinate 36 | double u_init_vel; 37 | double v_init_vel; 38 | }; 39 | 40 | // Measurements for features in one camera img 41 | class MonoCameraMeasurement 42 | { 43 | public: 44 | double timeStampToSec; 45 | // All features on the current image, 46 | // including tracked ones and newly detected ones. 47 | std::vector features; 48 | }; 49 | 50 | // typedef boost::shared_ptr MonoCameraMeasurementPtr; 51 | typedef MonoCameraMeasurement *MonoCameraMeasurementPtr; 52 | 53 | } // namespace orcvio 54 | 55 | #endif //FEATURE_MSG_H -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/feat/kf.h: -------------------------------------------------------------------------------- 1 | #ifndef KF_H_ 2 | #define KF_H_ 3 | 4 | #include "Eigen/Dense" 5 | 6 | #include 7 | 8 | using namespace orcvio; 9 | 10 | // we only measure the 2d position 11 | class MeasurementPackage { 12 | public: 13 | double timestamp_; 14 | Eigen::VectorXd raw_measurements_; 15 | }; 16 | 17 | class KalmanFilter { 18 | 19 | private: 20 | /** 21 | * Common calculation for KF and EKF. 22 | * @param y = residual. 23 | */ 24 | void UpdateWithResidual(const Eigen::VectorXd &y); 25 | 26 | // check whether the tracking toolbox was initialized or not (first measurement) 27 | bool is_initialized_; 28 | 29 | // previous timestamp 30 | double previous_timestamp_; 31 | 32 | public: 33 | 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | /** 37 | * Constructor. 38 | */ 39 | KalmanFilter(); 40 | 41 | /** 42 | * Destructor. 43 | */ 44 | ~KalmanFilter(); 45 | 46 | /** 47 | * Run the whole flow of the Kalman Filter from here. 48 | */ 49 | void ProcessMeasurement(const MeasurementPackage &measurement_pack); 50 | 51 | // state vector is x, y, vx, vy 52 | Eigen::VectorXd x_; 53 | 54 | // state covariance matrix 55 | Eigen::MatrixXd P_; 56 | 57 | // state transition matrix 58 | Eigen::MatrixXd F_; 59 | 60 | // process covariance matrix 61 | Eigen::MatrixXd Q_; 62 | 63 | // measurement matrix 64 | Eigen::MatrixXd H_; 65 | 66 | // measurement covariance matrix 67 | Eigen::MatrixXd R_; 68 | 69 | /** 70 | * Prediction Predicts the state and the state covariance 71 | * using the process model 72 | * @param delta_T Time between k and k+1 in s 73 | */ 74 | void Predict(); 75 | 76 | /** 77 | * Updates the state by using standard Kalman Filter equations 78 | * @param z The measurement at k+1 79 | */ 80 | void Update(const Eigen::VectorXd &z); 81 | 82 | // to keep all tracked kps 83 | vector_eigen kp_history; 84 | 85 | }; 86 | 87 | #endif /* KF_H_ */ -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/obj/ObjectState.h: -------------------------------------------------------------------------------- 1 | #ifndef OBJ_STATE_H 2 | #define OBJ_STATE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace orcvio 11 | { 12 | 13 | struct ObjectState { 14 | 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | int object_id; 18 | std::string object_class; 19 | 20 | Eigen::Vector3d ellipsoid_shape; 21 | Eigen::Matrix4d object_pose; 22 | Eigen::MatrixX3d object_keypoints_shape_global_frame; 23 | 24 | /// Default constructor 25 | ObjectState() { 26 | 27 | } 28 | 29 | }; 30 | 31 | /** 32 | * @brief transform the mean shape to global frame using object pose 33 | * @param object_keypoints_mean_shape size 12x3, the mean shape of keypoints 34 | * @param object_pose size 4x4, the object pose from object frame to global frame 35 | * @return size 12x3, object keypoints shape in global frame 36 | */ 37 | Eigen::MatrixX3d transform_mean_keypoints_to_global(const Eigen::MatrixX3d& object_keypoints_mean_shape, const Eigen::Matrix4d& object_pose); 38 | 39 | /** 40 | * @brief save the object state to file 41 | * @param an ObjectState structure that holds the object states 42 | * @param the timestamps of observations 43 | * @param the file name to save the results 44 | */ 45 | void save_object_state_to_file(const ObjectState & object_state, const std::vector& timestamps, 46 | std::string filepath_format = "/home/erl/moshan/open_orcvio/catkin_ws_openvins/src/open_vins/results/object_state_%d.txt" ); 47 | 48 | } 49 | 50 | #endif /* OBJ_STATE_H */ 51 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/utils/EigenLevenbergMarquardt.h: -------------------------------------------------------------------------------- 1 | #ifndef OV_CORE_EIGENLEVENBERGMARUQARDT_H 2 | #define OV_CORE_EIGENLEVENBERGMARUQARDT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include "Eigen/SparseCore" 11 | 12 | #include "EigenLevenbergMarquardt/LMqrsolv.h" 13 | #include "EigenLevenbergMarquardt/LMcovar.h" 14 | #include "EigenLevenbergMarquardt/LMpar.h" 15 | #include "EigenLevenbergMarquardt/LevenbergMarquardt.h" 16 | #include "EigenLevenbergMarquardt/LMonestep.h" 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/utils/EigenLevenbergMarquardt/LMcovar.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This code initially comes from MINPACK whose original authors are: 5 | // Copyright Jorge More - Argonne National Laboratory 6 | // Copyright Burt Garbow - Argonne National Laboratory 7 | // Copyright Ken Hillstrom - Argonne National Laboratory 8 | // 9 | // This Source Code Form is subject to the terms of the Minpack license 10 | // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 11 | 12 | #ifndef EIGEN_LMCOVAR_H 13 | #define EIGEN_LMCOVAR_H 14 | 15 | namespace Eigen { 16 | 17 | namespace internal { 18 | 19 | template 20 | void covar( 21 | Matrix< Scalar, Dynamic, Dynamic > &r, 22 | const VectorXi& ipvt, 23 | Scalar tol = std::sqrt(NumTraits::epsilon()) ) 24 | { 25 | using std::abs; 26 | /* Local variables */ 27 | Index i, j, k, l, ii, jj; 28 | bool sing; 29 | Scalar temp; 30 | 31 | /* Function Body */ 32 | const Index n = r.cols(); 33 | const Scalar tolr = tol * abs(r(0,0)); 34 | Matrix< Scalar, Dynamic, 1 > wa(n); 35 | eigen_assert(ipvt.size()==n); 36 | 37 | /* form the inverse of r in the full upper triangle of r. */ 38 | l = -1; 39 | for (k = 0; k < n; ++k) 40 | if (abs(r(k,k)) > tolr) { 41 | r(k,k) = 1. / r(k,k); 42 | for (j = 0; j <= k-1; ++j) { 43 | temp = r(k,k) * r(j,k); 44 | r(j,k) = 0.; 45 | r.col(k).head(j+1) -= r.col(j).head(j+1) * temp; 46 | } 47 | l = k; 48 | } 49 | 50 | /* form the full upper triangle of the inverse of (r transpose)*r */ 51 | /* in the full upper triangle of r. */ 52 | for (k = 0; k <= l; ++k) { 53 | for (j = 0; j <= k-1; ++j) 54 | r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k); 55 | r.col(k).head(k+1) *= r(k,k); 56 | } 57 | 58 | /* form the full lower triangle of the covariance matrix */ 59 | /* in the strict lower triangle of r and in wa. */ 60 | for (j = 0; j < n; ++j) { 61 | jj = ipvt[j]; 62 | sing = j > l; 63 | for (i = 0; i <= j; ++i) { 64 | if (sing) 65 | r(i,j) = 0.; 66 | ii = ipvt[i]; 67 | if (ii > jj) 68 | r(ii,jj) = r(i,j); 69 | if (ii < jj) 70 | r(jj,ii) = r(i,j); 71 | } 72 | wa[jj] = r(j,j); 73 | } 74 | 75 | /* symmetrize the covariance matrix in r. */ 76 | r.topLeftCorner(n,n).template triangularView() = r.topLeftCorner(n,n).transpose(); 77 | r.diagonal() = wa; 78 | } 79 | 80 | } // end namespace internal 81 | 82 | } // end namespace Eigen 83 | 84 | #endif // EIGEN_LMCOVAR_H 85 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/include/orcvio/utils/EigenNumericalDiff.h: -------------------------------------------------------------------------------- 1 | // -*- coding: utf-8 2 | // vim: set fileencoding=utf-8 3 | 4 | // This file is part of Eigen, a lightweight C++ template library 5 | // for linear algebra. 6 | // 7 | // Copyright (C) 2009 Thomas Capricelli 8 | // 9 | // This Source Code Form is subject to the terms of the Mozilla 10 | // Public License v. 2.0. If a copy of the MPL was not distributed 11 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 12 | 13 | #ifndef EIGENNUMERICALDIFF_H 14 | #define EIGENNUMERICALDIFF_H 15 | 16 | namespace EigenNumericalDiff { 17 | 18 | using namespace Eigen; 19 | 20 | enum NumericalDiffMode { 21 | Forward, 22 | Central 23 | }; 24 | 25 | 26 | /** 27 | * This class allows you to add a method df() to your functor, which will 28 | * use numerical differentiation to compute an approximate of the 29 | * derivative for the functor. Of course, if you have an analytical form 30 | * for the derivative, you should rather implement df() by yourself. 31 | * 32 | * More information on 33 | * http://en.wikipedia.org/wiki/Numerical_differentiation 34 | * 35 | * Currently only "Forward" and "Central" scheme are implemented. 36 | */ 37 | template 38 | class NumericalDiff : public _Functor 39 | { 40 | public: 41 | typedef _Functor Functor; 42 | typedef typename Functor::Scalar Scalar; 43 | typedef typename Functor::InputType InputType; 44 | typedef typename Functor::DiffType DiffType; 45 | typedef typename Functor::ValueType ValueType; 46 | typedef typename Functor::JacobianType JacobianType; 47 | 48 | NumericalDiff(Scalar _epsfcn=0.) : Functor(), epsfcn(_epsfcn) {} 49 | NumericalDiff(const Functor& f, Scalar _epsfcn=0.) : Functor(f), epsfcn(_epsfcn) {} 50 | 51 | // forward constructors 52 | template 53 | NumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {} 54 | template 55 | NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {} 56 | template 57 | NumericalDiff(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2), epsfcn(0) {} 58 | 59 | enum { 60 | InputsAtCompileTime = Functor::InputsAtCompileTime, 61 | ValuesAtCompileTime = Functor::ValuesAtCompileTime 62 | }; 63 | 64 | /** 65 | * return the number of evaluation of functor 66 | */ 67 | int df(const InputType& _x, JacobianType &jac) const 68 | { 69 | using std::sqrt; 70 | using std::abs; 71 | /* Local variables */ 72 | Scalar h; 73 | int nfev=0; 74 | const typename InputType::Index n = _x.size(); 75 | const Scalar eps = sqrt(((std::max)(epsfcn,NumTraits::epsilon() ))); 76 | ValueType val1, val2; 77 | InputType x = _x; 78 | // TODO : we should do this only if the size is not already known 79 | val1.resize(Functor::values()); 80 | val2.resize(Functor::values()); 81 | 82 | // initialization 83 | switch(mode) { 84 | case Forward: 85 | // compute f(x) 86 | Functor::operator()(x, val1); nfev++; 87 | break; 88 | case Central: 89 | // do nothing 90 | break; 91 | default: 92 | eigen_assert(false); 93 | }; 94 | 95 | // Function Body 96 | for (int j = 0; j < n; ++j) { 97 | h = eps;// * abs(x[j]); 98 | if (h == 0.) { 99 | h = eps; 100 | } 101 | DiffType dx(x.size()); dx.setZero(); 102 | dx[j] = h; 103 | switch(mode) { 104 | case Forward: 105 | x += dx; 106 | Functor::operator()(x, val2); 107 | // assert(val2.allFinite()); 108 | nfev++; 109 | x = _x; 110 | jac.col(j) = (val2-val1)/h; 111 | // assert(!Eigen::isnan(jac.col(j).array()).any()); 112 | // assert(!Eigen::isinf(jac.col(j).array()).any()); 113 | // assert(jac.col(j).allFinite()); 114 | break; 115 | case Central: 116 | x += dx; 117 | Functor::operator()(x, val2); nfev++; 118 | x += (-2)*dx; 119 | Functor::operator()(x, val1); nfev++; 120 | x = _x; 121 | jac.col(j) = (val2-val1)/(2*h); 122 | assert(jac.allFinite()); 123 | break; 124 | default: 125 | eigen_assert(false); 126 | }; 127 | } 128 | return nfev; 129 | } 130 | private: 131 | Scalar epsfcn; 132 | 133 | NumericalDiff& operator=(const NumericalDiff&); 134 | }; 135 | 136 | } // end namespace Eigen 137 | 138 | //vim: ai ts=4 sts=4 et sw=4 139 | #endif // EIGEN_NUMERICAL_DIFF_H 140 | 141 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/launch/om_d455.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | [ 20 | 0.9999887972997616, 0.004225660403089795, -0.002132854691210053, -0.06510852338269565, 21 | -0.004224561368945015, 0.9999909415437157, 0.0005195303196076303, -0.004589801762755343, 22 | 0.0021350307295388425, -0.0005105141239311359, 0.9999975905066537, -0.01765349149011269, 23 | 0.0, 0.0, 0.0, 1.0 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/launch/om_unity_gq.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | [ 20 | 0.0, -1.0, 0.0, 0.1, 21 | 0.0, 0.0, -1.0, 0, 22 | 1.0, 0.0, 0.0, -0.1, 23 | 0.0, 0.0, 0.0, 1.0 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/launch/standalone_object_mapping_dcist.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | [ 21 | 0.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689, 22 | 0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053, 23 | 0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582, 24 | 0., 0., 0., 1. 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/launch/standalone_object_mapping_erl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | [ 20 | 0.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689, 21 | 0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053, 22 | 0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582, 23 | 0., 0., 0., 1. 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | orcvio_mapping 5 | 0.0.1 6 | OrcVIO: Object residual constrained Visual-Inertial Odometry 7 | 8 | Mo Shan 9 | 10 | TODO 11 | 12 | Mo Shan 13 | 14 | catkin 15 | 16 | roscpp 17 | std_msgs 18 | tf 19 | nav_msgs 20 | sensor_msgs 21 | geometry_msgs 22 | eigen_conversions 23 | tf_conversions 24 | tf2_ros 25 | image_transport 26 | cv_bridge 27 | message_filters 28 | pcl_conversions 29 | pcl_ros 30 | std_srvs 31 | message_runtime 32 | 33 | sort_ros 34 | 35 | libpcl-all-dev 36 | suitesparse 37 | libceres-dev 38 | sophus 39 | 40 | rosunit 41 | 42 | 43 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/src/orcvio/feat/Feature.cpp: -------------------------------------------------------------------------------- 1 | #include "orcvio/feat/Feature.h" 2 | 3 | using namespace orcvio; 4 | 5 | void Feature::clean_old_measurements(std::vector valid_times) 6 | { 7 | 8 | // Loop through each of the cameras we have 9 | for (auto const &pair : timestamps) 10 | { 11 | 12 | // Assert that we have all the parts of a measurement 13 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 14 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 15 | 16 | // Our iterators 17 | auto it1 = timestamps[pair.first].begin(); 18 | auto it2 = uvs[pair.first].begin(); 19 | auto it3 = uvs_norm[pair.first].begin(); 20 | 21 | // Loop through measurement times, remove ones that are not in our timestamps 22 | while (it1 != timestamps[pair.first].end()) 23 | { 24 | if (std::find(valid_times.begin(), valid_times.end(), *it1) == valid_times.end()) 25 | { 26 | it1 = timestamps[pair.first].erase(it1); 27 | it2 = uvs[pair.first].erase(it2); 28 | it3 = uvs_norm[pair.first].erase(it3); 29 | } 30 | else 31 | { 32 | ++it1; 33 | ++it2; 34 | ++it3; 35 | } 36 | } 37 | } 38 | } 39 | 40 | void Feature::clean_older_measurements(double timestamp) 41 | { 42 | 43 | // Loop through each of the cameras we have 44 | for (auto const &pair : timestamps) 45 | { 46 | 47 | // Assert that we have all the parts of a measurement 48 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 49 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 50 | 51 | // Our iterators 52 | auto it1 = timestamps[pair.first].begin(); 53 | auto it2 = uvs[pair.first].begin(); 54 | auto it3 = uvs_norm[pair.first].begin(); 55 | 56 | // Loop through measurement times, remove ones that are older then the specified one 57 | while (it1 != timestamps[pair.first].end()) 58 | { 59 | if (*it1 <= timestamp) 60 | { 61 | it1 = timestamps[pair.first].erase(it1); 62 | it2 = uvs[pair.first].erase(it2); 63 | it3 = uvs_norm[pair.first].erase(it3); 64 | } 65 | else 66 | { 67 | ++it1; 68 | ++it2; 69 | ++it3; 70 | } 71 | } 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/src/orcvio/feat/kf.cpp: -------------------------------------------------------------------------------- 1 | #include "orcvio/feat/kf.h" 2 | 3 | using Eigen::MatrixXd; 4 | using Eigen::VectorXd; 5 | 6 | KalmanFilter::KalmanFilter() { 7 | 8 | is_initialized_ = false; 9 | previous_timestamp_ = 0; 10 | 11 | // initializing matrices 12 | x_ = VectorXd(4); 13 | R_ = MatrixXd(2, 2); 14 | H_ = MatrixXd(2, 4); 15 | H_ << 1, 0, 0, 0, 16 | 0, 1, 0, 0; 17 | 18 | //measurement covariance matrix 19 | R_ << 0.0225, 0, 20 | 0, 0.0225; 21 | 22 | // Initializing P 23 | P_ = MatrixXd(4, 4); 24 | P_ << 1, 0, 0, 0, 25 | 0, 1, 0, 0, 26 | 0, 0, 1000, 0, 27 | 0, 0, 0, 1000; 28 | 29 | F_ = MatrixXd(4, 4); 30 | Q_ = MatrixXd(4, 4); 31 | 32 | } 33 | 34 | KalmanFilter::~KalmanFilter() {} 35 | 36 | void KalmanFilter::Predict() { 37 | x_ = F_ * x_ ; 38 | MatrixXd Ft = F_.transpose(); 39 | P_ = F_ * P_ * Ft + Q_; 40 | } 41 | 42 | void KalmanFilter::Update(const VectorXd &z) { 43 | /** 44 | * update the state by using Kalman Filter equations 45 | */ 46 | VectorXd y = z - H_ * x_; 47 | UpdateWithResidual(y); 48 | } 49 | 50 | void KalmanFilter::UpdateWithResidual(const VectorXd &y){ 51 | MatrixXd Ht = H_.transpose(); 52 | MatrixXd S = H_ * P_ * Ht + R_; 53 | MatrixXd Si = S.inverse(); 54 | MatrixXd K = P_ * Ht * Si; 55 | // New state 56 | x_ = x_ + (K * y); 57 | int x_size = x_.size(); 58 | MatrixXd I = MatrixXd::Identity(x_size, x_size); 59 | P_ = (I - K * H_) * P_; 60 | } 61 | 62 | void KalmanFilter::ProcessMeasurement(const MeasurementPackage &meas_package) { 63 | 64 | /***************************************************************************** 65 | * Initialization 66 | ****************************************************************************/ 67 | if (!is_initialized_) { 68 | /** 69 | Initialize state. 70 | */ 71 | // No velocity and coordinates are cartesian already. 72 | const double v_init = 3 / 0.1; 73 | x_ << meas_package.raw_measurements_[0], meas_package.raw_measurements_[1], v_init, v_init; 74 | 75 | // Saving first timestamp in seconds 76 | previous_timestamp_ = meas_package.timestamp_ ; 77 | // done initializing, no need to predict or update 78 | is_initialized_ = true; 79 | 80 | return; 81 | } 82 | 83 | /** 84 | * Update the state transition matrix F according to the new elapsed time. 85 | - Time is measured in seconds. 86 | * Update the process noise covariance matrix. 87 | * Use noise_ax = 9 and noise_ay = 9 for your Q matrix. 88 | */ 89 | double dt = (meas_package.timestamp_ - previous_timestamp_) / 1; 90 | previous_timestamp_ = meas_package.timestamp_; 91 | 92 | // State transition matrix update 93 | F_ << 1, 0, dt, 0, 94 | 0, 1, 0, dt, 95 | 0, 0, 1, 0, 96 | 0, 0, 0, 1; 97 | 98 | // Noise covariance matrix computation 99 | // Noise values from the task 100 | double noise_ax = 9.0; 101 | double noise_ay = 9.0; 102 | 103 | double dt_2 = dt * dt; //dt^2 104 | double dt_3 = dt_2 * dt; //dt^3 105 | double dt_4 = dt_3 * dt; //dt^4 106 | double dt_4_4 = dt_4 / 4; //dt^4/4 107 | double dt_3_2 = dt_3 / 2; //dt^3/2 108 | 109 | Q_ << dt_4_4 * noise_ax, 0, dt_3_2 * noise_ax, 0, 110 | 0, dt_4_4 * noise_ay, 0, dt_3_2 * noise_ay, 111 | dt_3_2 * noise_ax, 0, dt_2 * noise_ax, 0, 112 | 0, dt_3_2 * noise_ay, 0, dt_2 * noise_ay; 113 | 114 | /***************************************************************************** 115 | * Prediction 116 | ****************************************************************************/ 117 | 118 | Predict(); 119 | 120 | /***************************************************************************** 121 | * Update 122 | ****************************************************************************/ 123 | 124 | /** 125 | * Use the sensor type to perform the update step. 126 | * Update the state and covariance matrices. 127 | */ 128 | 129 | if (meas_package.raw_measurements_[0] == 0 && meas_package.raw_measurements_[1] == 0) { 130 | // skip update when this keypoint is not detected 131 | // note that 0, 0 is the dummy value in this case 132 | } else { 133 | Update(meas_package.raw_measurements_); 134 | } 135 | 136 | } 137 | 138 | 139 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/src/orcvio/obj/ObjectState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "orcvio/obj/ObjectState.h" 11 | 12 | using namespace boost::filesystem; 13 | 14 | namespace orcvio 15 | { 16 | 17 | Eigen::MatrixX3d transform_mean_keypoints_to_global(const Eigen::MatrixX3d& object_keypoints_mean_shape, const Eigen::Matrix4d& object_pose) 18 | { 19 | 20 | // get the number of keypoints for this object class 21 | const int kps_num = object_keypoints_mean_shape.rows(); 22 | 23 | Eigen::MatrixX3d object_keypoints_shape_global_frame; 24 | object_keypoints_shape_global_frame = Eigen::MatrixXd::Zero(kps_num, 3); 25 | 26 | Eigen::Matrix3d wRq = object_pose.block(0, 0, 3, 3); 27 | Eigen::Vector3d wPq = object_pose.block(0, 3, 3, 1); 28 | 29 | // std::cout << "wRq " << wRq << std::endl; 30 | // std::cout << "wPq " << wPq.transpose() << std::endl; 31 | 32 | for (int i = 0; i < kps_num; ++i) 33 | { 34 | // std::cout << "keypoint position in object frame " << object_keypoints_mean_shape.row(i) << std::endl; 35 | Eigen::Vector3d keypoint_global_frame = wRq * object_keypoints_mean_shape.row(i).transpose() + wPq; 36 | // std::cout << "keypoint position in global frame " << keypoint_global_frame.transpose() << std::endl; 37 | object_keypoints_shape_global_frame.row(i) = keypoint_global_frame.transpose(); 38 | } 39 | 40 | return object_keypoints_shape_global_frame; 41 | 42 | } 43 | 44 | void save_object_state_to_file(const ObjectState & object_state, const std::vector& timestamps, 45 | std::string filepath_format) 46 | { 47 | boost::format boost_filepath_format(filepath_format); 48 | std::ofstream file((boost_filepath_format % object_state.object_id).str()); 49 | 50 | if (file.is_open()) 51 | { 52 | file << "object id:\n" << object_state.object_id << '\n'; 53 | file << "object class:\n" << object_state.object_class << '\n'; 54 | file << "wTq:\n" << object_state.object_pose << '\n'; 55 | file << "keypoints in global frame:\n" << object_state.object_keypoints_shape_global_frame << '\n'; 56 | file << "ellipsoid shape:\n" << object_state.ellipsoid_shape << '\n'; 57 | file << "observation timestamps:\n"; 58 | for (const auto& time: timestamps) 59 | { 60 | file << time << " "; 61 | } 62 | } 63 | } 64 | 65 | } 66 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio_mapping/src/orcvio_mapping_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ObjectInitNode.h" 3 | 4 | using namespace orcvio_mapping; 5 | 6 | int main(int argc, char **argv) { 7 | 8 | // Create ros node 9 | ros::init(argc, argv, "orcvio_mapping"); 10 | ros::NodeHandle nh("~"); 11 | 12 | // Debug 13 | // ROS_INFO("Done reading config values"); 14 | 15 | ObjectInitNode object_mapper(nh); 16 | 17 | // Done! 18 | ros::spin(); 19 | return EXIT_SUCCESS; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | data/ 3 | output/ -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.8) 2 | 3 | # Project name 4 | project(sort_ros) 5 | 6 | IF(NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | # Find catkin (the ROS build system) 11 | find_package(catkin_simple REQUIRED) 12 | 13 | # Include libraries 14 | find_package(OpenCV 3 REQUIRED) 15 | 16 | # display message to user 17 | message(STATUS "OPENCV VERSION: " ${OpenCV_VERSION}) 18 | 19 | catkin_simple(ALL_DEPS_REQUIRED) 20 | 21 | # Try to compile with c++11 22 | # http://stackoverflow.com/a/25836953 23 | include(CheckCXXCompilerFlag) 24 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 25 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 26 | if(COMPILER_SUPPORTS_CXX11) 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 28 | elseif(COMPILER_SUPPORTS_CXX0X) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 30 | else() 31 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 32 | endif() 33 | 34 | # Enable compile optimizations 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 36 | 37 | # Enable debug flags (use if you want to debug in gdb) 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") 39 | 40 | 41 | # Set link libraries used by all binaries 42 | list(APPEND thirdparty_libraries 43 | ${OpenCV_LIBRARIES} 44 | ) 45 | 46 | ################################################## 47 | # Make the core library 48 | ################################################## 49 | cs_add_library(sort_core_lib 50 | src/Hungarian.cpp 51 | src/KalmanTracker.cpp 52 | src/sort_tracking.cpp 53 | src/sort_ros_nodelet.cpp 54 | ) 55 | target_link_libraries(sort_core_lib ${thirdparty_libraries}) 56 | 57 | ################################################## 58 | # Make binary files! 59 | ################################################## 60 | cs_add_executable(sort_ros src/main.cpp) 61 | target_link_libraries(sort_ros sort_core_lib ${thirdparty_libraries}) 62 | 63 | cs_add_executable(test_kf src/test_kf.cpp) 64 | target_link_libraries(test_kf sort_core_lib ${thirdparty_libraries}) 65 | 66 | ## Specify libraries to link a library or executable target against 67 | cs_add_executable(sort_ros_node src/sort_ros_node.cpp) 68 | target_link_libraries(sort_ros_node sort_core_lib ${catkin_LIBRARIES}) 69 | 70 | cs_install() 71 | cs_export() 72 | install(FILES nodelet_plugins.xml 73 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 74 | ) 75 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/include/sort_ros/Hungarian.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | class HungarianAlgorithm 11 | { 12 | 13 | public: 14 | 15 | HungarianAlgorithm(); 16 | ~HungarianAlgorithm(); 17 | 18 | double Solve(vector>& DistMatrix, vector& Assignment); 19 | 20 | private: 21 | 22 | void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns); 23 | void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns); 24 | void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows); 25 | 26 | void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); 27 | void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); 28 | void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); 29 | void step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col); 30 | void step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); 31 | 32 | }; 33 | 34 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/include/sort_ros/KalmanTracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "opencv2/video/tracking.hpp" 4 | #include "opencv2/highgui/highgui.hpp" 5 | 6 | using namespace std; 7 | using namespace cv; 8 | 9 | #define StateType Rect_ 10 | 11 | // This class represents the internel state of individual tracked objects observed as bounding box. 12 | class KalmanTracker 13 | { 14 | 15 | public: 16 | 17 | KalmanTracker(StateType initRect) 18 | { 19 | init_kf(initRect); 20 | m_time_since_update = 0; 21 | m_hits = 0; 22 | m_hit_streak = 0; 23 | m_age = 0; 24 | m_id = kf_count; 25 | kf_count++; 26 | lost_flag = false; 27 | } 28 | 29 | ~KalmanTracker() 30 | { 31 | m_history.clear(); 32 | } 33 | 34 | StateType predict(); 35 | void update(StateType stateMat); 36 | 37 | StateType get_state(); 38 | StateType get_rect_xysr(float cx, float cy, float s, float r); 39 | 40 | void update_centroid_history(); 41 | 42 | static int kf_count; 43 | 44 | int m_time_since_update; 45 | int m_hits; 46 | int m_hit_streak; 47 | int m_age; 48 | int m_id; 49 | 50 | string object_class; 51 | bool lost_flag; 52 | vector centroid_history; 53 | 54 | private: 55 | 56 | void init_kf(StateType stateMat); 57 | 58 | KalmanFilter kf; 59 | Mat measurement; 60 | 61 | vector m_history; 62 | }; 63 | 64 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/include/sort_ros/sort_tracking.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "sort_ros/Hungarian.h" 9 | #include "sort_ros/KalmanTracker.h" 10 | 11 | #include "opencv2/video/tracking.hpp" 12 | #include "opencv2/highgui/highgui.hpp" 13 | 14 | using namespace std; 15 | using namespace cv; 16 | 17 | namespace sort_ros { 18 | 19 | // global variables for counting colors 20 | #define CNUM 20 21 | 22 | typedef struct TrackingBox 23 | { 24 | vector frame; 25 | int id; 26 | Rect_ box; 27 | string object_class; 28 | 29 | }TrackingBox; 30 | 31 | struct Config { 32 | int max_age; 33 | int min_hits; 34 | double iou_threshold; 35 | bool use_centroid_dist_flag; 36 | double centroid_dist_threshold; 37 | }; 38 | 39 | 40 | // Computes IOU between two bounding boxes 41 | double GetIOU(Rect_ bb_test, Rect_ bb_gt); 42 | 43 | // Computes centroid distance between two bounding boxes 44 | double GetCentroidDist(Rect_ bb_test, Rect_ bb_gt); 45 | 46 | // Computes the centroid of a bounding box 47 | Point2f GetRectCenter(Rect_ bb); 48 | 49 | class SortTracker { 50 | 51 | public: 52 | 53 | SortTracker() : frame_count(0), max_age(3), min_hits(5), trkNum(0), detNum(0), iou_threshold(0.3) 54 | { 55 | gen_class(); 56 | gen_colors(); 57 | } 58 | 59 | void update(vector detFrameData); 60 | void remove_lost_trackers(); 61 | void set_config(Config config); 62 | 63 | void gen_colors(); 64 | Mat draw_bbox(Mat img); 65 | Mat draw_centroids(Mat img); 66 | 67 | void gen_class(); 68 | bool check_valid_class(string label); 69 | 70 | // we only keep the bbox that is currently tracked 71 | // if a bbox gets lost we publish it and remove it from trackers 72 | vector trackers; 73 | 74 | private: 75 | 76 | int frame_count; 77 | int max_age; 78 | int min_hits; 79 | 80 | unsigned int trkNum; 81 | unsigned int detNum; 82 | 83 | double iou_threshold; 84 | bool use_centroid_dist_flag; 85 | double centroid_dist_threshold; 86 | 87 | vector> predictedBoxes; 88 | vector> distMatrix; 89 | vector assignment; 90 | 91 | set unmatchedDetections; 92 | set unmatchedTrajectories; 93 | set allItems; 94 | set matchedItems; 95 | 96 | vector matchedPairs; 97 | 98 | Scalar_ randColor[CNUM]; 99 | set class_labels; 100 | }; 101 | 102 | } 103 | 104 | 105 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/display_dcist.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 140 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Image 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz/Image 36 | Enabled: true 37 | Image Topic: /SortRos/detection_image 38 | Max Value: 1 39 | Median window: 5 40 | Min Value: 0 41 | Name: Image 42 | Normalize Range: true 43 | Queue Size: 2 44 | Transport Hint: compressed 45 | Unreliable: false 46 | Value: true 47 | - Class: rviz/Image 48 | Enabled: true 49 | Image Topic: /acl_jackal/detector_manager_node/object_visualization 50 | Max Value: 1 51 | Median window: 5 52 | Min Value: 0 53 | Name: Image 54 | Normalize Range: true 55 | Queue Size: 2 56 | Transport Hint: raw 57 | Unreliable: false 58 | Value: true 59 | Enabled: true 60 | Global Options: 61 | Background Color: 48; 48; 48 62 | Default Light: false 63 | Fixed Frame: acl_jackal/chassis_link 64 | Frame Rate: 30 65 | Name: root 66 | Tools: 67 | - Class: rviz/Interact 68 | Hide Inactive Objects: true 69 | - Class: rviz/MoveCamera 70 | - Class: rviz/Select 71 | - Class: rviz/FocusCamera 72 | - Class: rviz/Measure 73 | - Class: rviz/SetInitialPose 74 | Theta std deviation: 0.2617993950843811 75 | Topic: /initialpose 76 | X std deviation: 0.5 77 | Y std deviation: 0.5 78 | - Class: rviz/SetGoal 79 | Topic: /move_base_simple/goal 80 | - Class: rviz/PublishPoint 81 | Single click: true 82 | Topic: /clicked_point 83 | Value: true 84 | Views: 85 | Current: 86 | Class: rviz/Orbit 87 | Distance: 10 88 | Enable Stereo Rendering: 89 | Stereo Eye Separation: 0.05999999865889549 90 | Stereo Focal Distance: 1 91 | Swap Stereo Eyes: false 92 | Value: false 93 | Focal Point: 94 | X: 0 95 | Y: 0 96 | Z: 0 97 | Focal Shape Fixed Size: true 98 | Focal Shape Size: 0.05000000074505806 99 | Invert Z Axis: false 100 | Name: Current View 101 | Near Clip Distance: 0.009999999776482582 102 | Pitch: 0.785398006439209 103 | Target Frame: 104 | Value: Orbit (rviz) 105 | Yaw: 0.785398006439209 106 | Saved: ~ 107 | Window Geometry: 108 | Displays: 109 | collapsed: false 110 | Height: 1025 111 | Hide Left Dock: false 112 | Hide Right Dock: false 113 | Image: 114 | collapsed: false 115 | QMainWindow State: 000000ff00000000fd000000040000000000000717000003a7fc020000000cfb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000003d000001d20000001600fffffffb0000000a0049006d0061006700650100000215000001cf0000001600fffffffb000000100044006900730070006c00610079007302000000430000006700000717000000cbfb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000105000000db0000000000000000fb0000000a0049006d006100670065010000014e0000032c0000000000000000000000010000010f00000452fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000452000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000020000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 116 | Selection: 117 | collapsed: false 118 | Time: 119 | collapsed: false 120 | Tool Properties: 121 | collapsed: false 122 | Views: 123 | collapsed: false 124 | Width: 1853 125 | X: 67 126 | Y: 27 127 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/sort_ros_test_d455.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/sort_ros_test_gdb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/sort_ros_test_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/sort_ros_test_tum.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/launch/sort_ros_test_visma.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/msg/TrackedBoundingBox.msg: -------------------------------------------------------------------------------- 1 | int64 xmin 2 | int64 ymin 3 | int64 xmax 4 | int64 ymax 5 | int16 id 6 | string Class 7 | bool lost_flag -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/msg/TrackedBoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | TrackedBoundingBox[] bounding_boxes 3 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | This is a sort ROS Nodelet 7 | 8 | 9 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sort_ros 5 | 1.0.0 6 | 7 | Core algorithms for visual-inertial navigation algorithms. 8 | 9 | 10 | 11 | Mo Shan 12 | 13 | 14 | GNU General Public License v3.0 15 | 16 | 17 | catkin 18 | 19 | 20 | cmake_modules 21 | roscpp 22 | rosbag 23 | tf 24 | std_msgs 25 | geometry_msgs 26 | sensor_msgs 27 | nav_msgs 28 | visualization_msgs 29 | cv_bridge 30 | darknet_ros_msgs 31 | nodelet 32 | image_transport 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/readme.md: -------------------------------------------------------------------------------- 1 | ## about 2 | 3 | * this repo implements bbox tracking using sort 4 | * original version uses IOU, but if the overlap is 0, cannot re-identify the same object and lead to ID switch 5 | * this version add centroid distance, so that we could use a threshold to determine whether the two centroids belong to the same object 6 | 7 | ## dependencies 8 | 9 | - opencv 10 | - [catkin simple](https://github.com/catkin/catkin_simple) 11 | - [darknet ros](https://github.com/leggedrobotics/darknet_ros) 12 | - [darknet ros bridge](https://github.com/moshanATucsd/OrcVIO_Lite/tree/master/ros_wrapper/src/darknet_ros_bridge) 13 | - [wm_od_interface_msgs](https://gitlab.sitcore.net/aimm/phoenix-r1/-/tree/master/src/common_msgs/wm_od_interface_msgs) 14 | 15 | ## notes 16 | 17 | * we only track valid class, e.g. barrel, which can be set in `gen_class` function 18 | * the `centroid_dist_threshold` determines whether we consider two centroids belong to same object, which can be tuned in launch file. set to a large value if we only need to detect barrels to accommodate jackal's aggressive motion 19 | * the output topic is `/SortRos/tracked_bbox`, here is the output from `rostopic echo /SortRos/tracked_bbox` 20 | 21 | ``` 22 | header: 23 | seq: 49 24 | stamp: 25 | secs: 1317067372 26 | nsecs: 169100046 27 | frame_id: "tracked bboxes" 28 | bounding_boxes: 29 | - 30 | xmin: 433 31 | ymin: 164 32 | xmax: 455 33 | ymax: 453 34 | id: 18 35 | Class: "car" 36 | lost_flag: False 37 | - 38 | xmin: 19 39 | ymin: 159 40 | xmax: 69 41 | ymax: 51 42 | id: 21 43 | Class: "car" 44 | lost_flag: False 45 | - 46 | xmin: 117 47 | ymin: 159 48 | xmax: 181 49 | ymax: 150 50 | id: 23 51 | Class: "car" 52 | lost_flag: False 53 | - 54 | xmin: 313 55 | ymin: 171 56 | xmax: 347 57 | ymax: 338 58 | id: 25 59 | Class: "car" 60 | lost_flag: True 61 | - 62 | xmin: 201 63 | ymin: 162 64 | xmax: 261 65 | ymax: 232 66 | id: 27 67 | Class: "car" 68 | lost_flag: False 69 | - 70 | xmin: 538 71 | ymin: 166 72 | xmax: 552 73 | ymax: 550 74 | id: 28 75 | Class: "car" 76 | lost_flag: False 77 | - 78 | xmin: 535 79 | ymin: 171 80 | xmax: 548 81 | ymax: 546 82 | id: 29 83 | Class: "car" 84 | lost_flag: False 85 | ``` 86 | 87 | ## references 88 | 89 | - https://github.com/tryolabs/norfair -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/src/KalmanTracker.cpp: -------------------------------------------------------------------------------- 1 | #include "sort_ros/KalmanTracker.h" 2 | 3 | // tracking id relies on this, so we have to reset it in each seq. 4 | int KalmanTracker::kf_count = 0; 5 | 6 | // initialize Kalman filter 7 | void KalmanTracker::init_kf(StateType stateMat) 8 | { 9 | int stateNum = 7; 10 | int measureNum = 4; 11 | kf = KalmanFilter(stateNum, measureNum, 0); 12 | 13 | // State Transition Matrix A 14 | // Note: set dT at each processing step! 15 | // [ 1 0 0 0 1 0 0 ] 16 | // [ 0 1 0 0 0 1 0 ] 17 | // [ 0 0 1 0 0 0 1 ] 18 | // [ 0 0 0 1 0 0 0 ] 19 | // [ 0 0 0 0 1 0 0 ] 20 | // [ 0 0 0 0 0 1 0 ] 21 | // [ 0 0 0 0 0 0 1 ] 22 | const float dT = 1.0; 23 | setIdentity(kf.transitionMatrix); 24 | kf.transitionMatrix.at(4) = dT; 25 | kf.transitionMatrix.at(12) = dT; 26 | kf.transitionMatrix.at(20) = dT; 27 | setIdentity(kf.processNoiseCov, Scalar::all(1e-2)); 28 | 29 | measurement = Mat::zeros(measureNum, 1, CV_32F); 30 | setIdentity(kf.measurementMatrix); 31 | setIdentity(kf.measurementNoiseCov, Scalar::all(1e-2)); 32 | 33 | // give high uncertainty to the unobservable initial velocities 34 | setIdentity(kf.errorCovPost, Scalar::all(1)); 35 | 36 | // initialize state vector with bounding box in [cx,cy,s,r] style 37 | kf.statePost.at(0, 0) = stateMat.x + stateMat.width / 2; 38 | kf.statePost.at(1, 0) = stateMat.y + stateMat.height / 2; 39 | kf.statePost.at(2, 0) = stateMat.area(); 40 | kf.statePost.at(3, 0) = stateMat.width / stateMat.height; 41 | } 42 | 43 | // Predict the estimated bounding box. 44 | StateType KalmanTracker::predict() 45 | { 46 | // predict 47 | // assert(kf.statePost.type() == CV_32F); 48 | 49 | Mat p = kf.predict(); 50 | m_age += 1; 51 | 52 | if (m_time_since_update > 0) 53 | m_hit_streak = 0; 54 | m_time_since_update += 1; 55 | 56 | StateType predictBox = get_rect_xysr(p.at(0, 0), p.at(1, 0), p.at(2, 0), p.at(3, 0)); 57 | 58 | update_centroid_history(); 59 | 60 | m_history.push_back(predictBox); 61 | return m_history.back(); 62 | } 63 | 64 | // Update the state vector with observed bounding box. 65 | void KalmanTracker::update(StateType stateMat) 66 | { 67 | m_time_since_update = 0; 68 | m_history.clear(); 69 | m_hits += 1; 70 | m_hit_streak += 1; 71 | 72 | // measurement 73 | measurement.at(0, 0) = stateMat.x + stateMat.width / 2; 74 | measurement.at(1, 0) = stateMat.y + stateMat.height / 2; 75 | measurement.at(2, 0) = stateMat.area(); 76 | measurement.at(3, 0) = stateMat.width / stateMat.height; 77 | 78 | // update 79 | kf.correct(measurement); 80 | 81 | update_centroid_history(); 82 | } 83 | 84 | // Return the current state vector 85 | StateType KalmanTracker::get_state() 86 | { 87 | Mat s = kf.statePost; 88 | return get_rect_xysr(s.at(0, 0), s.at(1, 0), s.at(2, 0), s.at(3, 0)); 89 | } 90 | 91 | // Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style. 92 | StateType KalmanTracker::get_rect_xysr(float cx, float cy, float s, float r) 93 | { 94 | // handle the case when s * r is negative 95 | float w = sqrt(abs(s * r)); 96 | float h = s / w; 97 | float x = (cx - w / 2); 98 | float y = (cy - h / 2); 99 | 100 | if (x < 0 && cx > 0) 101 | x = 0; 102 | if (y < 0 && cy > 0) 103 | y = 0; 104 | 105 | return StateType(x, y, w, h); 106 | } 107 | 108 | void KalmanTracker::update_centroid_history() 109 | { 110 | Mat s = kf.statePost; 111 | Point2f cur_centroid(s.at(0, 0), s.at(1, 0)); 112 | centroid_history.emplace_back(cur_centroid); 113 | } 114 | -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/src/sort_ros_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "nodelet/loader.h" 3 | 4 | int main(int argc, char **argv){ 5 | 6 | ros::init(argc, argv, "sort_ros"); 7 | nodelet::Loader nodelet; 8 | nodelet::M_string remap(ros::names::getRemappings()); 9 | nodelet::V_string nargv; 10 | std::string nodelet_name = ros::this_node::getName(); 11 | nodelet.load(nodelet_name, "sort_ros/SortRos", remap, nargv); 12 | ros::spin(); 13 | 14 | return 0; 15 | 16 | } -------------------------------------------------------------------------------- /ros_wrapper/src/sort_ros/src/test_kf.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/video/tracking.hpp" 2 | #include "opencv2/highgui/highgui.hpp" 3 | 4 | using namespace std; 5 | using namespace cv; 6 | 7 | // -------------------------------------------------------------------- 8 | // Kalman Filter Demonstrating, a 2-d ball demo 9 | // -------------------------------------------------------------------- 10 | 11 | const int winHeight = 600; 12 | const int winWidth = 800; 13 | 14 | Point mousePosition = Point(winWidth >> 1, winHeight >> 1); 15 | 16 | // mouse event callback 17 | void mouseEvent(int event, int x, int y, int flags, void *param) 18 | { 19 | if (event == CV_EVENT_MOUSEMOVE) { 20 | mousePosition = Point(x, y); 21 | } 22 | } 23 | 24 | void TestKF(); 25 | 26 | int main() 27 | { 28 | TestKF(); 29 | 30 | return 0; 31 | } 32 | 33 | void TestKF() 34 | { 35 | 36 | int stateNum = 4; 37 | int measureNum = 2; 38 | KalmanFilter kf = KalmanFilter(stateNum, measureNum, 0); 39 | 40 | // initialization 41 | Mat processNoise(stateNum, 1, CV_32F); 42 | Mat measurement = Mat::zeros(measureNum, 1, CV_32F); 43 | 44 | // kf.transitionMatrix = *(Mat_(stateNum, stateNum) << 45 | // 1, 0, 1, 0, 46 | // 0, 1, 0, 1, 47 | // 0, 0, 1, 0, 48 | // 0, 0, 0, 1); 49 | cv::setIdentity(kf.transitionMatrix); 50 | kf.transitionMatrix.at(2) = 1.0f; 51 | kf.transitionMatrix.at(7) = 1.0f; 52 | 53 | setIdentity(kf.measurementMatrix); 54 | setIdentity(kf.processNoiseCov, Scalar::all(1e-2)); 55 | setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1)); 56 | setIdentity(kf.errorCovPost, Scalar::all(1)); 57 | 58 | randn(kf.statePost, Scalar::all(0), Scalar::all(winHeight)); 59 | 60 | namedWindow("Kalman"); 61 | setMouseCallback("Kalman", mouseEvent); 62 | Mat img(winHeight, winWidth, CV_8UC3); 63 | 64 | while (1) 65 | { 66 | // predict 67 | Mat prediction = kf.predict(); 68 | Point predictPt = Point(prediction.at(0, 0), prediction.at(1, 0)); 69 | // generate measurement 70 | Point statePt = mousePosition; 71 | measurement.at(0, 0) = statePt.x; 72 | measurement.at(1, 0) = statePt.y; 73 | // update 74 | kf.correct(measurement); 75 | // visualization 76 | img.setTo(Scalar(255, 255, 255)); 77 | circle(img, predictPt, 8, CV_RGB(0, 255, 0), -1); // predicted point as green 78 | circle(img, statePt, 8, CV_RGB(255, 0, 0), -1); // current position as red 79 | imshow("Kalman", img); 80 | char code = (char)waitKey(100); 81 | if (code == 27 || code == 'q' || code == 'Q') 82 | break; 83 | } 84 | destroyWindow("Kalman"); 85 | } 86 | 87 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/KeypointDetection.msg: -------------------------------------------------------------------------------- 1 | string obj_name 2 | int32[] x 3 | int32[] y 4 | float32[] probabilities 5 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/KeypointDetections.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | KeypointDetection[] detections 3 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/ObjectDetectionRequest.msg: -------------------------------------------------------------------------------- 1 | #message for triggering object detection of a specific object 2 | #contains object type to trigger corresponding node and region of interest to constrain search 3 | Header header 4 | string obj_type #object type, reference to the object in the database 5 | #geometry_msgs/Pose coarse_pose #coarse pose from 2d detector 6 | wm_od_interface_msgs/ROI2d region2d #image region of interest used to constrain object detection 7 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/ROI2d.msg: -------------------------------------------------------------------------------- 1 | # a 2d region of interest 2 | # values are between 0 and 1 from the top-left of the image 3 | Header header 4 | 5 | float32 top_left_x 6 | float32 top_left_y 7 | 8 | float32 bottom_right_x 9 | float32 bottom_right_y 10 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/ira_det.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string obj_name 3 | uint32 id 4 | geometry_msgs/Pose pose 5 | geometry_msgs/Polygon bbox 6 | float32 confidence 7 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/ira_dets.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string filename 3 | uint32 serial 4 | ira_det[] dets 5 | uint8 n_dets 6 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/msg/ira_request.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string filename 3 | uint32 serial 4 | string debug 5 | -------------------------------------------------------------------------------- /ros_wrapper/src/wm_od_interface_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | wm_od_interface_msgs 4 | 0.0.0 5 | The wm_od_interface_msgs package 6 | 7 | 8 | 9 | 10 | rcta 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /scripts/convert_csv_to_txt.py: -------------------------------------------------------------------------------- 1 | import csv 2 | def main(incsv, outtxt, skipfirst=1): 3 | with open(incsv) as infile: 4 | inreader = csv.reader(infile) 5 | with open(outtxt, 'w') as outfile: 6 | outwriter = csv.writer( 7 | outfile, delimiter=" ", quoting=csv.QUOTE_MINIMAL) 8 | for linenum, row in enumerate(inreader): 9 | if linenum >= skipfirst: 10 | row[0] = int(row[0]) / 1e9 11 | outwriter.writerow(row) 12 | 13 | if __name__ == '__main__': 14 | import sys 15 | main(sys.argv[1], sys.argv[2]) 16 | 17 | -------------------------------------------------------------------------------- /scripts/process_rosbag/add_sim_imu.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import rospy 3 | from sensor_msgs.msg import Imu 4 | 5 | # change these three parameters 6 | root_dir = "/home/erl/moshan/open_orcvio/catkin_ws_openvins/" 7 | input_bag = root_dir + "data/with_keypoints_original.bag" 8 | output_bag = root_dir + "data/with_keypoints.bag" 9 | input_imu = root_dir + "src/open_vins/results/imu.txt" 10 | 11 | f_imu = open(input_imu,"r") 12 | imu_records = f_imu.readlines() 13 | f_imu.close() 14 | imu_pointer = 0 15 | imu_next = imu_records[imu_pointer].split() 16 | 17 | with rosbag.Bag(output_bag, 'w') as outbag: 18 | for topic, msg, t in rosbag.Bag(input_bag).read_messages(): 19 | if imu_pointer rospy.Time.from_sec((float(imu_next[0]))): 20 | imu_msg = Imu() 21 | imu_msg.header.stamp = rospy.Time.from_sec((float(imu_next[0]))) 22 | imu_msg.header.frame_id = "husky/imu_sim" 23 | imu_msg.angular_velocity.x = float(imu_next[1]) 24 | imu_msg.angular_velocity.y = float(imu_next[2]) 25 | imu_msg.angular_velocity.z = float(imu_next[3]) 26 | imu_msg.linear_acceleration.x = float(imu_next[4]) 27 | imu_msg.linear_acceleration.y = float(imu_next[5]) 28 | imu_msg.linear_acceleration.z = float(imu_next[6]) 29 | imu_pointer += 1 30 | if imu_pointer 20: 11 | break 12 | import yaml 13 | print(yaml.dump(barrier_data)) -------------------------------------------------------------------------------- /scripts/process_rosbag/check_timestamp.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | bag = rosbag.Bag("/media/erl/disk1/orcvio/opcity_smooth/docker_compose_opcity_20cars_quad.bag") 3 | image_t, det_t, odom_t = [], [], [] 4 | 5 | for topic, msg, t in bag.read_messages(topics=["/falcon/cam_left/image_raw", "/starmap/keypoints", "/unity_command/ground_truth/falcon/pose"]): 6 | if topic == "/falcon/cam_left/image_raw": image_t.append(msg.header.stamp) 7 | if topic == "/starmap/keypoints": det_t.append(msg.header.stamp) 8 | if topic == "/unity_command/ground_truth/falcon/pose": odom_t.append(msg.header.stamp) 9 | 10 | print("common image and det: ", len(set(image_t) & set(det_t))) 11 | print("common odom and det:", len(set(odom_t) & set(det_t))) 12 | print("common image and odom:", len(set(odom_t) & set(image_t))); 13 | print("total det:", len(det_t)) 14 | print("total odom:", len(odom_t)) -------------------------------------------------------------------------------- /scripts/process_rosbag/convert_detection_to_bboxes.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import rosbag 4 | import numpy as np 5 | import copy 6 | import argparse 7 | from geometry_msgs.msg import PoseStamped 8 | 9 | from sort_ros.msg import TrackedBoundingBoxes 10 | 11 | def scriptdir(f=__file__): 12 | return os.path.dirname(f) 13 | 14 | def rel2abs(relpath, refdir=scriptdir()): 15 | return os.path.join(refdir, relpath) 16 | 17 | if __name__ == "__main__": 18 | 19 | # Settings. 20 | parser = argparse.ArgumentParser(description='Convert the kpts message.', 21 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 22 | 23 | parser.add_argument('--input_bag', type=str, default="/media/erl/disk1/orcvio/opcity_40cars/opcity_40cars_quad.bag", 24 | help='Input Rosbag') 25 | parser.add_argument('--output_bag', type=str, default='/media/erl/disk1/orcvio/opcity_40cars/opcity_40cars_quad_new.bag', 26 | help='Output Rosbag') 27 | 28 | parser.add_argument('--origin_kpts_topic', type=str, default='/husky1/detection', 29 | help='The orignal topic name of the kpts info. /ns/detection') 30 | parser.add_argument('--bbox_topic', type=str, default='/sort_ros/tracked_bounding_boxes', 31 | help='The topic name of the bbox info.') 32 | parser.add_argument('--template_bag', type=str, default='/media/erl/disk1/orcvio/rosbags_old/arl_husky_overpasscity_threecar_640480_pose_kps.bag', 33 | help='A rosbag having TrackedBoundingBoxes msg inside') 34 | 35 | # one example is 36 | # python convert_detection_to_bboxes.py --template_bag /media/erl/disk1/orcvio/mit_rosbags/mit_medfield_modified_with_keypoints.bag --input_bag /media/erl/disk1/orcvio/opcity_smooth/docker_compose_opcity_20cars_quad_no_occ_with_doors.bag --output_bag /media/erl/disk1/orcvio/opcity_smooth/docker_compose_opcity_car_door_quad_bboxes_only.bag 37 | 38 | args = parser.parse_args() 39 | input_bag = args.input_bag 40 | output_bag = args.output_bag 41 | template_bag = args.template_bag 42 | 43 | origin_kpts_topic = args.origin_kpts_topic 44 | bbox_topic = args.bbox_topic 45 | 46 | # copy the bboxes message 47 | # the lines below are copy a template for the special topic message 48 | for topic, msg, t in rosbag.Bag(template_bag).read_messages(): 49 | if topic == bbox_topic: 50 | if len(msg.bounding_boxes) == 0: 51 | continue 52 | bbox_msg_template = copy.deepcopy(msg) 53 | break 54 | bbox_msg_template.serialize(open(rel2abs('bbox_msg_template.msg'), 'w')) 55 | 56 | bbox_msg_template.header.frame_id = origin_kpts_topic 57 | det_template = bbox_msg_template.bounding_boxes[0] 58 | 59 | closest_t = -1 60 | 61 | with rosbag.Bag(output_bag, 'w') as outbag: 62 | for topic, msg, t in rosbag.Bag(input_bag).read_messages(): 63 | # convert detection to starmap keypoint topic 64 | if topic == origin_kpts_topic: 65 | 66 | bbox_msg_template.header = msg.header 67 | bbox_msg_template.bounding_boxes = [] 68 | 69 | for detection in msg.detections: 70 | 71 | obj_det = copy.deepcopy(det_template) 72 | obj_det.xmin = detection.x_min 73 | obj_det.ymin = detection.y_min 74 | obj_det.xmax = detection.x_max 75 | obj_det.ymax = detection.y_max 76 | obj_det.id = detection.object_id 77 | # obj_det.Class = detection.class_name 78 | obj_det.Class = "car" 79 | obj_det.lost_flag = True 80 | 81 | bbox_msg_template.bounding_boxes.append(obj_det) 82 | 83 | outbag.write(bbox_topic, bbox_msg_template, t=t) 84 | 85 | else: 86 | 87 | if hasattr(msg, "header"): 88 | # closest_t = msg.header.stamp 89 | #outbag.write(topic, msg, t=msg.header.stamp) 90 | closest_t = t 91 | outbag.write(topic, msg, t=t) 92 | else: 93 | if closest_t == -1: 94 | continue 95 | else: 96 | outbag.write(topic, msg, t=closest_t) 97 | 98 | 99 | -------------------------------------------------------------------------------- /scripts/save_pose_from_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rosbag 3 | from tf_bag import BagTfTransformer 4 | 5 | import numpy as np 6 | import os 7 | from pyquaternion import Quaternion 8 | 9 | def save_pose(timestamps, positions, rotations, result_dir): 10 | """ 11 | save positions, rotations of the trajectory 12 | :param timestamps: list of timestamps 13 | :param positions: list of positions 14 | :param rotations: list of rotations 15 | :param result_dir: directory of results 16 | """ 17 | 18 | with open(result_dir, "w") as f: 19 | 20 | for state_id in range(0, len(timestamps)): 21 | 22 | timestamp = timestamps[state_id] 23 | s1 = " ".join(map(str, positions[state_id])) 24 | q = rotations[state_id] 25 | s2 = " ".join(map(str, q)) 26 | 27 | f.write("%s %s %s\n" % (timestamp, s1, s2)) 28 | 29 | if __name__ == "__main__": 30 | 31 | root_dir = "/media/erl/disk1/orcvio/mit_rosbags/phoenix_stack/" 32 | input_bag = root_dir + "medfield_ft5_2020-11-06-15-44-24.bag" 33 | result_dir = "/home/erl/Workspace/orcvio-lite/cache/stamped_groundtruth.txt" 34 | 35 | bag_transformer = BagTfTransformer(input_bag) 36 | 37 | # frame1_id = 'acl_jackal/forward_color_optical_frame' 38 | # frame1_id = 'acl_jackal/base' 39 | # frame2_id = 'acl_jackal/map' 40 | 41 | frame1_id = 'acl_jackal/map' 42 | frame2_id = 'acl_jackal/base' 43 | 44 | timestamps = [] 45 | positions = [] 46 | rotations = [] 47 | 48 | for topic, msg, time in rosbag.Bag(input_bag).read_messages(): 49 | timestamps.append(time.to_sec()) 50 | translation, quaternion = bag_transformer.lookupTransform(frame1_id, frame2_id, time) 51 | positions.append(translation) 52 | rotations.append(quaternion) 53 | 54 | # bag_transformer.plotTranslation(frame1_id, frame2_id) 55 | save_pose(timestamps, positions, rotations, result_dir) -------------------------------------------------------------------------------- /scripts/traj_eval.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os, glob, sys 3 | import os.path as path 4 | import shutil 5 | 6 | sys.path.insert(0, os.path.dirname(os.path.realpath(__file__)) + 7 | "/rpg_trajectory_evaluation/" + "src/rpg_trajectory_evaluation") 8 | 9 | from trajectory import Trajectory 10 | 11 | class TrajEval(): 12 | """ 13 | this class evaluates the trajectory wrt groundtruth 14 | """ 15 | 16 | def __init__(self, result_dir): 17 | 18 | # ref http://www.cvlibs.net/datasets/kitti/eval_odometry.php 19 | self.subtraj_lengths = [x * 100 for x in range(1, 9)] 20 | 21 | self.result_dir = result_dir 22 | 23 | def evaluate_and_plot(self): 24 | """ 25 | evaluate the poses and show the plot 26 | """ 27 | 28 | # should contain the groundtruth, trajectory estimate and 29 | # optionally the evaluation configuration as mentioned above. 30 | 31 | # remove old results 32 | folder_name = self.result_dir + 'plots/' 33 | if os.path.isdir(folder_name): 34 | shutil.rmtree(folder_name, ignore_errors=True) 35 | 36 | folder_name = self.result_dir + 'saved_results/' 37 | if os.path.isdir(folder_name): 38 | shutil.rmtree(folder_name, ignore_errors=True) 39 | 40 | evo_cmd = 'python ' + './rpg_trajectory_evaluation/scripts/analyze_trajectory_single.py ' \ 41 | + self.result_dir 42 | 43 | os.system(evo_cmd) 44 | 45 | if __name__ == "__main__": 46 | 47 | result_dir = "/home/vdhiman/.cache/orcvio_cpp" 48 | TE = TrajEval(result_dir) 49 | TE.evaluate_and_plot() 50 | 51 | 52 | --------------------------------------------------------------------------------