├── .DS_Store ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── README.md ├── app └── orcvioMain.cpp ├── assets ├── erl_realsense_demo.gif └── orcvio-lite-unity-demo.gif ├── cmake ├── FindEigen3.cmake └── FindSuiteSparse.cmake ├── config ├── euroc.yaml ├── kitti.yaml ├── object_feat_all.yaml ├── object_feat_erl.yaml └── rs_d435i.yaml ├── include ├── Initializer │ ├── DynamicInitializer.h │ ├── FlexibleInitializer.h │ ├── ImuPreintegration.h │ ├── StaticInitializer.h │ ├── feature_manager.h │ ├── initial_alignment.h │ ├── initial_sfm.h │ └── solve_5pts.h ├── ORB │ └── ORBDescriptor.h ├── orcvio │ ├── dataset_reader.h │ ├── feat │ │ ├── Feature.h │ │ ├── FeatureInitializer.h │ │ ├── FeatureInitializerOptions.h │ │ ├── feature.hpp │ │ ├── feature_msg.h │ │ └── kf.h │ ├── image_processor.h │ ├── imu_state.h │ ├── obj │ │ ├── ObjectFeature.h │ │ ├── ObjectFeatureInitializer.h │ │ ├── ObjectLM.h │ │ ├── ObjectLMLite.h │ │ ├── ObjectResJacCam.h │ │ └── ObjectState.h │ ├── orcvio.h │ ├── plot │ │ └── matplotlibcpp.h │ ├── tests │ │ └── test_utils.h │ └── utils │ │ ├── EigenLevenbergMarquardt.h │ │ ├── EigenLevenbergMarquardt │ │ ├── LMcovar.h │ │ ├── LMonestep.h │ │ ├── LMpar.h │ │ ├── LMqrsolv.h │ │ └── LevenbergMarquardt.h │ │ ├── EigenNumericalDiff.h │ │ ├── math_utils.hpp │ │ └── se3_ops.hpp ├── sensors │ ├── ImageData.hpp │ └── ImuData.hpp ├── utils │ └── DataReader.hpp └── visualization │ └── visualize.hpp ├── licenses ├── LICENCE_VINS_MONO.txt ├── LICENSE_MSCKF_VIO.txt └── LICENSE_ORB_SLAM2.txt ├── requirements.md ├── ros_wrapper ├── .gitignore └── src │ ├── CMakeLists.txt │ ├── darknet_ros_bridge │ ├── CMakeLists.txt │ ├── package.xml │ └── scripts │ │ └── darknet_ros_bridge.py │ ├── darknet_ros_msgs │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── action │ │ └── CheckForObjects.action │ ├── msg │ │ ├── BoundingBox.msg │ │ ├── BoundingBoxes.msg │ │ └── ObjectCount.msg │ └── package.xml │ ├── orcvio │ ├── CMakeLists.txt │ ├── include │ │ ├── ObjectInitNode.h │ │ ├── ObjectMapper_nodelet.h │ │ ├── System.h │ │ ├── System_nodelet.h │ │ └── gt_odom.h │ ├── launch │ │ ├── realsense_d435i │ │ │ ├── orcvio_mapping_rs_d435i.launch │ │ │ └── orcvio_vio_rs_d435i.launch │ │ └── rviz │ │ │ ├── orcvio_kitti_object.rviz │ │ │ ├── orcvio_mapping_rs_d435i.rviz │ │ │ └── orcvio_vio_rs_d435i.rviz │ ├── nodelet_plugins.xml │ ├── package.xml │ └── src │ │ ├── ObjectInitNode.cpp │ │ ├── ObjectMapper_nodelet.cpp │ │ ├── System.cpp │ │ ├── System_nodelet.cpp │ │ ├── gt_odom.cpp │ │ └── publish_gt_path.cpp │ ├── sort_ros │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include │ │ └── sort_ros │ │ │ ├── Hungarian.h │ │ │ ├── KalmanTracker.h │ │ │ └── sort_tracking.h │ ├── launch │ │ ├── display.rviz │ │ ├── display_dcist.rviz │ │ ├── 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 ├── run_euroc.sh ├── scripts ├── convert_csv_to_txt.py ├── save_pose_from_tf.py ├── tf_bag.py └── traj_eval.py └── src ├── DynamicInitializer.cpp ├── FlexibleInitializer.cpp ├── ORBDescriptor.cpp ├── StaticInitializer.cpp ├── feat ├── Feature.cpp ├── FeatureInitializer.cpp └── kf.cpp ├── feature_manager.cpp ├── image_processor.cpp ├── initial_alignment.cpp ├── initial_sfm.cpp ├── obj ├── ObjectFeature.cpp ├── ObjectFeatureInitializer.cpp ├── ObjectLM.cpp ├── ObjectLMLite.cpp ├── ObjectResJacCam.cpp └── ObjectState.cpp ├── orcvio.cpp ├── solve_5pts.cpp └── tests ├── data ├── one_car │ ├── frame_0.h5 │ ├── frame_1.h5 │ ├── frame_10.h5 │ ├── frame_11.h5 │ ├── frame_12.h5 │ ├── frame_13.h5 │ ├── frame_14.h5 │ ├── frame_15.h5 │ ├── frame_16.h5 │ ├── frame_17.h5 │ ├── frame_18.h5 │ ├── frame_19.h5 │ ├── frame_2.h5 │ ├── frame_20.h5 │ ├── frame_21.h5 │ ├── frame_22.h5 │ ├── frame_23.h5 │ ├── frame_24.h5 │ ├── frame_25.h5 │ ├── frame_26.h5 │ ├── frame_27.h5 │ ├── frame_28.h5 │ ├── frame_29.h5 │ ├── frame_3.h5 │ ├── frame_30.h5 │ ├── frame_31.h5 │ ├── frame_32.h5 │ ├── frame_33.h5 │ ├── frame_34.h5 │ ├── frame_35.h5 │ ├── frame_36.h5 │ ├── frame_37.h5 │ ├── frame_38.h5 │ ├── frame_39.h5 │ ├── frame_4.h5 │ ├── frame_40.h5 │ ├── frame_41.h5 │ ├── frame_42.h5 │ ├── frame_43.h5 │ ├── frame_44.h5 │ ├── frame_45.h5 │ ├── frame_46.h5 │ ├── frame_5.h5 │ ├── frame_6.h5 │ ├── frame_7.h5 │ ├── frame_8.h5 │ └── frame_9.h5 ├── one_car_no_zb │ ├── frame_0.h5 │ ├── frame_1.h5 │ ├── frame_10.h5 │ ├── frame_11.h5 │ ├── frame_12.h5 │ ├── frame_13.h5 │ ├── frame_14.h5 │ ├── frame_15.h5 │ ├── frame_16.h5 │ ├── frame_17.h5 │ ├── frame_18.h5 │ ├── frame_19.h5 │ ├── frame_2.h5 │ ├── frame_20.h5 │ ├── frame_21.h5 │ ├── frame_22.h5 │ ├── frame_23.h5 │ ├── frame_24.h5 │ ├── frame_25.h5 │ ├── frame_26.h5 │ ├── frame_27.h5 │ ├── frame_28.h5 │ ├── frame_29.h5 │ ├── frame_3.h5 │ ├── frame_30.h5 │ ├── frame_31.h5 │ ├── frame_32.h5 │ ├── frame_33.h5 │ ├── frame_34.h5 │ ├── frame_35.h5 │ ├── frame_36.h5 │ ├── frame_37.h5 │ ├── frame_38.h5 │ ├── frame_39.h5 │ ├── frame_4.h5 │ ├── frame_40.h5 │ ├── frame_41.h5 │ ├── frame_42.h5 │ ├── frame_43.h5 │ ├── frame_44.h5 │ ├── frame_45.h5 │ ├── frame_46.h5 │ ├── frame_5.h5 │ ├── frame_6.h5 │ ├── frame_7.h5 │ ├── frame_8.h5 │ └── frame_9.h5 ├── test_error_bbox_quadric.h5 ├── test_error_deform_reg.h5 ├── test_error_feature_quadric.h5 └── test_error_mean_shape_reg.h5 ├── test_kabsch.cpp ├── test_levenberg_marquardt.cpp ├── test_object_init_multiframe.cpp ├── test_object_lm.cpp ├── test_object_lm_multiframe.cpp ├── test_se3.cpp ├── test_state_update.cpp └── test_utils.cpp /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/.DS_Store -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | cmake_policy(SET CMP0054 NEW) 3 | cmake_policy(SET CMP0057 NEW) 4 | 5 | project(orcvio) 6 | include(CTest) 7 | 8 | option(BUILD_SHARED_LIBS "Whether to build shared libs" 9 | True) 10 | 11 | option(CMAKE_BUILD_TYPE "Release or Debug" "Release") 12 | set(CMAKE_CXX_STANDARD 11) 13 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 14 | #set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 15 | 16 | find_package(OpenCV REQUIRED) 17 | find_package(Boost REQUIRED filesystem) 18 | find_package(Ceres REQUIRED) 19 | find_package(Eigen3 REQUIRED) 20 | find_package(SuiteSparse REQUIRED) 21 | find_package(Pangolin REQUIRED) 22 | find_package(Sophus REQUIRED) 23 | 24 | include_directories( 25 | ${PROJECT_SOURCE_DIR}/include 26 | ${OpenCV_INCLUDE_DIRS} 27 | ${SUITESPARSE_INCLUDE_DIRS} 28 | ${CERES_INCLUDE_DIRS} 29 | ${Boost_INCLUDE_DIR} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ${Pangolin_INCLUDE_DIRS} 32 | ${Sophus_INCLUDE_DIRS} 33 | ) 34 | 35 | # Image processor 36 | add_library(${PROJECT_NAME}_image_processor 37 | ${PROJECT_SOURCE_DIR}/src/image_processor.cpp 38 | ${PROJECT_SOURCE_DIR}/src/ORBDescriptor.cpp 39 | ) 40 | target_link_libraries(${PROJECT_NAME}_image_processor 41 | ${OpenCV_LIBRARIES} 42 | ) 43 | 44 | # Initializer 45 | # -- Static Initializer 46 | add_library(${PROJECT_NAME}_static_initializer 47 | ${PROJECT_SOURCE_DIR}/src/StaticInitializer.cpp 48 | ) 49 | target_link_libraries(${PROJECT_NAME}_static_initializer 50 | ${SUITESPARSE_LIBRARIES} 51 | ) 52 | # -- Dynamic Initializer 53 | add_library(${PROJECT_NAME}_dynamic_initializer 54 | ${PROJECT_SOURCE_DIR}/src/StaticInitializer.cpp 55 | ${PROJECT_SOURCE_DIR}/src/DynamicInitializer.cpp 56 | ${PROJECT_SOURCE_DIR}/src/feature_manager.cpp 57 | ${PROJECT_SOURCE_DIR}/src/initial_alignment.cpp 58 | ${PROJECT_SOURCE_DIR}/src/initial_sfm.cpp 59 | ${PROJECT_SOURCE_DIR}/src/solve_5pts.cpp 60 | ) 61 | target_link_libraries(${PROJECT_NAME}_dynamic_initializer 62 | ${SUITESPARSE_LIBRARIES} 63 | ${CERES_LIBRARIES} 64 | ${OpenCV_LIBRARIES} 65 | ) 66 | # -- Flexible Initializer 67 | add_library(${PROJECT_NAME}_flexible_initializer 68 | ${PROJECT_SOURCE_DIR}/src/FlexibleInitializer.cpp 69 | ) 70 | target_link_libraries(${PROJECT_NAME}_flexible_initializer 71 | ${PROJECT_NAME}_static_initializer 72 | ${PROJECT_NAME}_dynamic_initializer 73 | ) 74 | 75 | # Estimator 76 | add_library(${PROJECT_NAME}_estimator 77 | ${PROJECT_SOURCE_DIR}/src/orcvio.cpp 78 | ) 79 | target_link_libraries(${PROJECT_NAME}_estimator 80 | ${PROJECT_NAME}_flexible_initializer 81 | ${SUITESPARSE_LIBRARIES} 82 | ${OpenCV_LIBRARIES} 83 | ) 84 | 85 | # App 86 | add_executable(orcvio app/orcvioMain.cpp) 87 | target_link_libraries(orcvio 88 | ${PROJECT_NAME}_image_processor 89 | ${PROJECT_NAME}_estimator 90 | ${OpenCV_LIBS} 91 | ${Pangolin_LIBRARIES} 92 | ) 93 | 94 | file(GLOB mappingFiles 95 | LIST_DIRECTORIES false 96 | ${PROJECT_SOURCE_DIR}/src/obj/*.cpp 97 | ${PROJECT_SOURCE_DIR}/src/feat/*.cpp) 98 | 99 | add_library(${PROJECT_NAME}_mapping 100 | ${mappingFiles}) 101 | target_link_libraries(${PROJECT_NAME}_mapping 102 | ${OpenCV_LIBS}) 103 | 104 | if(BUILD_TESTING) 105 | 106 | find_package(GTest REQUIRED) 107 | find_package(PythonLibs 2.7) 108 | if(PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) 109 | add_definitions(-DHAVE_PYTHONLIBS=1) 110 | message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) 111 | message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) 112 | message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) 113 | endif() 114 | include_directories(${PYTHON_INCLUDE_DIRS}) 115 | 116 | add_executable(${PROJECT_NAME}_tests 117 | src/tests/test_utils.cpp 118 | src/tests/test_se3.cpp 119 | src/tests/test_kabsch.cpp 120 | src/tests/test_state_update.cpp 121 | src/tests/test_object_init_multiframe.cpp 122 | src/tests/test_levenberg_marquardt.cpp 123 | src/tests/test_object_lm.cpp 124 | src/tests/test_object_lm_multiframe.cpp) 125 | 126 | target_link_libraries(${PROJECT_NAME}_tests 127 | ${PROJECT_NAME}_mapping 128 | ${PROJECT_NAME}_estimator 129 | ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} GTest::GTest 130 | GTest::Main) 131 | 132 | add_test( 133 | ${PROJECT_NAME}_tests 134 | WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}) 135 | 136 | # for line coverage 137 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage -fprofile-arcs -ftest-coverage -std=c++11 -g") 138 | # add_custom_target(cov COMMAND ${PROJECT_SOURCE_DIR}/build_scripts/generate_coverage_report -r "make") 139 | 140 | endif() # BUILD_TESTING 141 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OrcVIO-Lite 2 | 3 | ### About 4 | 5 | - 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 **Mono+IMU** data. 6 | 7 | - 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. 8 | 9 | - Related publication: [OrcVIO: Object residual constrained Visual-Inertial Odometry](https://arxiv.org/pdf/2007.15107.pdf), this is the journal version submitted to T-RO. [Project website](https://moshan.cf/orcvio_githubpage/) 10 | 11 | ### Citation 12 | 13 | ``` 14 | @inproceedings{shan2020orcvio, 15 | title={OrcVIO: Object residual constrained Visual-Inertial Odometry}, 16 | author={Shan, Mo and Feng, Qiaojun and Atanasov, Nikolay}, 17 | booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 18 | pages={5104--5111}, 19 | year={2020}, 20 | organization={IEEE} 21 | } 22 | ``` 23 | 24 | ## 1. Prerequisites 25 | 26 | This repository was tested on Ubuntu 18.04 with [ROS Melodic](http://wiki.ros.org/melodic/Installation). 27 | 28 | The core algorithm depends on `Eigen`, `Boost`, `Suitesparse`, `Ceres`, `OpenCV`, `Sophus`, `GTest` 29 | 30 | 31 | ## 2. Installation 32 | 33 | ### 2.1 Non-ROS version 34 | 35 | ``` 36 | $ git clone --recursive https://github.com/shanmo/OrcVIO-Lite.git 37 | $ cd OrcVIO-Lite 38 | $ mkdir build 39 | $ cd build 40 | $ cmake -D CMAKE_BUILD_TYPE=Release .. 41 | $ make 42 | ``` 43 | 44 | ### 2.2 ROS version 45 | 46 | - Environment is `Ubuntu 18.04` with ROS `Melodic` 47 | - The ROS version also depends on [catkin simple](https://github.com/catkin/catkin_simple), please put it in the `ros_wrapper/src` folder 48 | 49 | ``` 50 | $ git clone --recursive https://github.com/shanmo/OrcVIO-Lite.git 51 | $ cd OrcVIO-Lite/ros_wrapper 52 | $ catkin_make 53 | $ source ./devel/setup.bash 54 | ``` 55 | 56 | ## 3. Evaluation 57 | 58 | ### 3.1 Non-ROS Version 59 | 60 | - **Download dataset** 61 | 62 | Download [EuRoC MAV Dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) to ***PATH_TO_EUROC_DATASET/***. 63 | 64 | - **VIO** 65 | 66 | ``` 67 | $ cd PATH_TO_ORCVIO_LITE/ 68 | $ ./run_euroc.sh PATH_TO_EUROC_DATASET/ 69 | ``` 70 | 71 | 72 | 73 | ### 3.2 ROS Version 74 | 75 | - **Download dataset** 76 | 77 | [ERL indoor dataset (chairs, monitors)](https://www.dropbox.com/s/xe5ykbylawhanft/erl_lite_realsense_demo.bag?dl=0) 78 | 79 | * Indoor rosbags were collected with [Realsense D435i](https://www.intelrealsense.com/depth-camera-d435i/) in Existential Robotics Lab, University of California San Diego. 80 | 81 | - **VIO** 82 | 83 | ``` 84 | $ cd OrcVIO_Lite/ros_wrapper/ 85 | $ roslaunch orcvio orcvio_vio_rs_d435i.launch path_bag:=PATH_TO_ERL_DATASET/ 86 | ``` 87 | 88 | - **VIO & Mapping** 89 | 90 | ``` 91 | $ cd OrcVIO_Lite/ros_wrapper/ 92 | $ roslaunch orcvio orcvio_mapping_rs_d435i.launch path_bag:=PATH_TO_ERL_DATASET/ 93 | ``` 94 | 95 | - `rviz` visualization 96 | > ![demo](assets/erl_realsense_demo.gif) 97 | ![demo2](assets/orcvio-lite-unity-demo.gif) 98 | 99 | ## 4. Notes 100 | 101 | - The path to save the object map is set in `config/object_feat_erl.yaml` by `result_dir_path_object_map` 102 | 103 | 104 | 105 | 106 | ## License 107 | 108 | ``` 109 | MIT License 110 | Copyright (c) 2021 ERL at UCSD 111 | ``` 112 | 113 | 114 | 115 | ## Reference 116 | 117 | - [LARVIO](https://github.com/PetWorm/LARVIO) 118 | - [MSCKF](https://github.com/KumarRobotics/msckf_vio) 119 | - [OpenVINS](https://github.com/rpng/open_vins) 120 | - [OpenVINS eval](https://github.com/symao/open_vins) 121 | -------------------------------------------------------------------------------- /assets/erl_realsense_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/assets/erl_realsense_demo.gif -------------------------------------------------------------------------------- /assets/orcvio-lite-unity-demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/assets/orcvio-lite-unity-demo.gif -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/kitti.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 | 14 | # 2011 09 30 15 | camera_model: "pinhole" # only support "pinhole" 16 | distortion_model: "radtan" # only support "radtan" and "equidistant" 17 | resolution_width: 1226 18 | resolution_height: 370 19 | intrinsics: 20 | fx: 707.0912 21 | fy: 707.0912 22 | cx: 601.8873 23 | cy: 183.1104 24 | distortion_coeffs: 25 | k1: 0.0 26 | k2: 0.0 27 | p1: 0.0 28 | p2: 0.0 29 | # imu-camera extrinsic, including spacial and temporal parameters 30 | T_cam_imu: !!opencv-matrix 31 | rows: 4 32 | cols: 4 33 | dt: d 34 | data: 35 | [-0.00108864, -0.99997635, 0.00678718, -0.25008612, 36 | -0.00851207, -0.00677767, -0.99994087, 0.73412053, 37 | 0.99996318, -0.00114634, -0.00850449, -1.13803616, 38 | 0.0, 0.0, 0.0, 1.0] 39 | td: 0.007 40 | 41 | # TODO: if calibrate camera instrinsic online 42 | 43 | # visual front-end parameters 44 | pyramid_levels: 2 45 | patch_size: 21 46 | fast_threshold: 10 47 | max_iteration: 30 48 | track_precision: 0.01 49 | ransac_threshold: 1 50 | max_features_num: 200 51 | min_distance: 20 52 | flag_equalize: 1 # 0(false) or 1(true) 53 | pub_frequency: 10 54 | 55 | # window size 56 | sw_size: 20 57 | 58 | # online reset thresholds 59 | position_std_threshold: 8.0 60 | rotation_threshold: 0.2618 61 | translation_threshold: 0.4 62 | tracking_rate_threshold: 0.5 63 | 64 | # feature triangulation parameters 65 | least_observation_number: 3 66 | max_track_len: 6 67 | feature_translation_threshold: -1.0 68 | 69 | # imu and camera measurement noise parameters 70 | noise_gyro: 2.0000e-5 71 | noise_acc: 3.0000e-3 72 | noise_gyro_bias: 2.0000e-4 73 | noise_acc_bias: 2.0000e-3 74 | noise_feature: 0.008 75 | 76 | # filter intial values 77 | initial_covariance_orientation: 4e-4 78 | initial_covariance_velocity: 0.25 79 | initial_covariance_position: 1.0 80 | initial_covariance_gyro_bias: 4e-4 81 | initial_covariance_acc_bias: 0.01 82 | initial_covariance_extrin_rot: 3.0462e-8 83 | initial_covariance_extrin_trans: 9e-8 84 | 85 | # fej settings 86 | reset_fej_threshold: 10.11 87 | 88 | # zupt settings 89 | if_ZUPT_valid: 1 # 0(false) or 1(true) 90 | # if_ZUPT_valid: 0 # 0(false) or 1(true) 91 | if_use_feature_zupt_flag: 0 # 0(false) or 1(true) 92 | # if_use_feature_zupt_flag: 1 # 0(false) or 1(true) 93 | zupt_max_feature_dis: 2e-3 94 | zupt_noise_v: 1e-2 # std 95 | zupt_noise_p: 1e-2 96 | zupt_noise_q: 3.4e-2 97 | 98 | # static initialization setting 99 | static_duration: 1.0 100 | 101 | # measurement rate 102 | imu_rate: 250 103 | img_rate: 10 104 | 105 | # augmented feature state settings 106 | max_features_in_one_grid: 1 # pure msckf if set to 0 107 | aug_grid_rows: 5 108 | aug_grid_cols: 6 109 | feature_idp_dim: 1 # 1 or 3 110 | 111 | # if apply Schmidt filter 112 | use_schmidt: 0 # 0(false) or 1(true) 113 | # left or right perturbation 114 | use_left_perturbation_flag: 0 # 0(right) or 1(left) 115 | # use_left_perturbation_flag: 1 # 0(right) or 1(left) 116 | # use euler method or closed form covariance propagation 117 | use_closed_form_cov_prop_flag: 1 # 0(euler method) or 1(closed form) 118 | # use original larvio 119 | use_larvio_flag: 0 # 0(orcvio) or 1(larvio) 120 | # use_larvio_flag: 1 # 0(orcvio) or 1(larvio) 121 | 122 | # the threshold for chi square 123 | chi_square_threshold_feat: 0.95 124 | # the threshold for cost in feature LM 125 | feature_cost_threshold: 1e3 126 | # the threshold for distance check in feature LM 127 | init_final_dist_threshold: 1e3 128 | # the flag whether we discard large update 129 | discard_large_update_flag: 0 130 | 131 | # if using ground-truth for initilization 132 | initial_use_gt: 1 133 | 134 | # we always set initial position to zero, 135 | # and initial quat to identity 136 | initial_pos: !!opencv-matrix 137 | rows: 3 138 | cols: 1 139 | dt: d 140 | data: [0,0,0] 141 | initial_quat: !!opencv-matrix 142 | rows: 4 143 | cols: 1 144 | dt: d 145 | data: [0.0, 0.0, 0.0, 1] 146 | initial_ba: !!opencv-matrix 147 | rows: 3 148 | cols: 1 149 | dt: d 150 | data: [0.0, 0.0, 0.0] 151 | initial_bg: !!opencv-matrix 152 | rows: 3 153 | cols: 1 154 | dt: d 155 | data: [0.0, 0.0, 0.0] 156 | 157 | # setting for odometry 06 158 | initial_state_time: 0.4 159 | initial_vel: !!opencv-matrix 160 | rows: 3 161 | cols: 1 162 | dt: d 163 | data: [11.50625649630200, 0.00508098927674, 0.05999312318407] 164 | 165 | -------------------------------------------------------------------------------- /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 | # for monitor category 26 | #-------------------------------------------------------------------------------------------- 27 | 28 | tvmonitor: 29 | keypoints_num: 8 30 | object_keypoints_mean: 31 | [-0.24979928, 0.25018418, -0.24980799, 0.25019201, -0.07860816, 32 | 0.07860816, -0.07860816, 0.07860816, 33 | -0.02643643, -0.0264397 , -0.02643432, -0.02643994, 0.02356006, 34 | 0.02356006, 0.02356006, 0.02356006, 35 | -0.22036003, -0.22037013, 0.27962987, 0.27961471, -0.07454422, 36 | -0.07454422, 0.07585024, 0.07585024] 37 | object_mean_shape: 38 | [0.5, 0.05, 0.5] 39 | aliases: 40 | - computer 41 | - laptop 42 | marker_color: [0.0, 0.0, 1.0] 43 | 44 | 45 | #-------------------------------------------------------------------------------------------- 46 | # other configs 47 | #-------------------------------------------------------------------------------------------- 48 | 49 | # whether to fine tune object pose using Levenberg Marquardt 50 | # do_fine_tune_object_pose_using_lm: false 51 | do_fine_tune_object_pose_using_lm: true 52 | 53 | # The directory where result files are saved per object 54 | # result_dir_path_object_map: ".ros/log/orcvio_cpp/object_map/" 55 | result_dir_path_object_map: "/home/erl/orcvio/OrcVIO-Lite/cache/object_map/" 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # groundtruth object states 59 | #-------------------------------------------------------------------------------------------- 60 | 61 | # note, these cannot be empty even if we don't have groundtruth object states 62 | 63 | total_object_num: 22 64 | 65 | objects_ids_gt: [1, 4, 7, 10, 13, 16, 1, 2, 3, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 66 | 15, 16, 17] 67 | 68 | objects_class_gt: [chair, chair, chair, chair, chair, chair, chair, chair, tvmonitor, tvmonitor, tvmonitor, tvmonitor, 69 | tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitor, tvmonitors] 70 | 71 | objects_rotation_gt: 72 | [2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 73 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 74 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 75 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 76 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 77 | -0.0, -0.0, 0.0, 1.0, 2.220446049250313e-16, 1.0, 0.0, -1.0, 2.220446049250313e-16, 78 | -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, 79 | 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, 80 | 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, 81 | 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, 82 | 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, 83 | 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, 84 | 0.9999996777147871, 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 85 | 0.0, 1.0, 0.0008028513696686779, 0.9999996777147871, 0.0, -0.9999996777147871, 86 | 0.0008028513696686779, -0.0, -0.0, 0.0, 1.0, 0.0008028513696686779, 0.9999996777147871, 87 | 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 0.0, 1.0, 0.0008028513696686779, 88 | 0.9999996777147871, 0.0, -0.9999996777147871, 0.0008028513696686779, -0.0, -0.0, 89 | 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, 90 | 0.0, 0.0, 0.0, 1.0] 91 | 92 | objects_translation_gt: 93 | [5.0, 5.5, 0.0, 20.0, 0.5, 0.0, 35.0, 5.5, 0.0, -35.0, 5.5, 94 | 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, 95 | -26.561, -14.41, 1.57, -8.986, 10.935, 1.602, -5.21, 10.935, 1.602, -5.547, -4.08, 96 | 1.602, -1.66, -4.08, 1.602, 12.78, -4.519, 1.602, 12.461, 17.475, 1.602, 15.332, 97 | 17.475, 1.602, 17.3, 12.505, 1.602, 17.3, 15.349, 1.602, 27.47, -5.178, 1.602, 98 | 27.47, -8.96, 1.602, -49.14, -10.74, 1.63, -38.582, 13.98, 1.63] 99 | 100 | first_uav_translation_gt: 101 | [1, 1, 0.3] 102 | -------------------------------------------------------------------------------- /config/rs_d435i.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: 384.17102476497377 16 | fy: 384.3372186440579 17 | cx: 323.7997751702461 18 | cy: 238.91110409779023 19 | distortion_coeffs: 20 | k1: 0.009694051100341156 21 | k2: -0.008084564032660335 22 | p1: 0.00022908109336942359 23 | p2: 0.0023177007244794693 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.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689, 33 | 0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053, 34 | 0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582, 35 | 0., 0., 0., 1. ] 36 | td: 0 37 | 38 | # TODO: if calibrate camera instrinsic online 39 | 40 | # visual front-end parameters 41 | pyramid_levels: 2 42 | patch_size: 21 43 | fast_threshold: 30 44 | max_iteration: 30 45 | track_precision: 0.01 46 | ransac_threshold: 1 47 | max_features_num: 200 48 | min_distance: 20 49 | flag_equalize: 1 # 0(false) or 1(true) 50 | pub_frequency: 10 51 | 52 | # window size 53 | sw_size: 20 54 | 55 | # online reset thresholds 56 | position_std_threshold: 8.0 57 | rotation_threshold: 0.2618 58 | translation_threshold: 0.4 59 | tracking_rate_threshold: 0.5 60 | 61 | # feature triangulation parameters 62 | least_observation_number: 3 63 | max_track_len: 6 64 | feature_translation_threshold: -1.0 65 | 66 | # imu and camera measurement noise parameters 67 | noise_gyro: 0.004 68 | noise_acc: 0.08 69 | noise_gyro_bias: 2e-6 70 | noise_acc_bias: 4e-5 71 | noise_feature: 0.008 72 | 73 | # filter intial covariance 74 | initial_covariance_orientation: 4e-4 75 | initial_covariance_velocity: 0.25 76 | initial_covariance_position: 1.0 77 | initial_covariance_gyro_bias: 4e-4 78 | initial_covariance_acc_bias: 0.01 79 | initial_covariance_extrin_rot: 3.0462e-8 80 | initial_covariance_extrin_trans: 9e-8 81 | 82 | # fej settings 83 | reset_fej_threshold: 10.11 84 | 85 | # zupt settings 86 | if_ZUPT_valid: 1 # 0(false) or 1(true) 87 | 88 | if_use_feature_zupt_flag: 0 # 0(false) or 1(true) 89 | zupt_max_feature_dis: 2e-3 90 | 91 | zupt_noise_v: 1e-2 # std 92 | zupt_noise_p: 1e-2 93 | zupt_noise_q: 3.4e-2 94 | 95 | # static initialization setting 96 | static_duration: 1.0 97 | 98 | # measurement rate 99 | imu_rate: 200 100 | img_rate: 30 101 | 102 | # augmented feature state settings 103 | # a positive value is hybrid msckf 104 | max_features_in_one_grid: 0 # pure msckf if set to 0 105 | aug_grid_rows: 5 106 | aug_grid_cols: 6 107 | feature_idp_dim: 1 # 1 or 3 108 | 109 | # if apply Schmidt filter 110 | use_schmidt: 0 # 0(false) or 1(true) 111 | # left or right perturbation 112 | use_left_perturbation_flag: 0 # 0(right) or 1(left) 113 | # use euler method or closed form covariance propagation 114 | use_closed_form_cov_prop_flag: 1 # 0(euler method) or 1(closed form) 115 | # use original larvio 116 | use_larvio_flag: 0 # 0(orcvio) or 1(larvio) 117 | 118 | # the threshold for chi square 119 | chi_square_threshold_feat: 0.95 120 | # the threshold for cost in feature LM 121 | feature_cost_threshold: 4.7673e-04 122 | # the threshold for distance check in feature LM 123 | init_final_dist_threshold: 1e1 124 | # the flag whether we discard large update 125 | discard_large_update_flag: 1 126 | 127 | # whether to use object residual to update camera pose 128 | use_object_residual_update_cam_pose_flag: 0 # 0(not use) or 1(use) 129 | 130 | # whether we skip the update step 131 | prediction_only_flag: 0 132 | 133 | # if using ground-truth for initilization 134 | initial_use_gt: 0 135 | -------------------------------------------------------------------------------- /include/Initializer/FlexibleInitializer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 19-8-13. 3 | // A flexible initializer that can automatically initialize in case of static or dynamic scene. 4 | // 5 | 6 | #ifndef FLEXIBLE_INITIALIZER_H 7 | #define FLEXIBLE_INITIALIZER_H 8 | 9 | #include 10 | #include 11 | 12 | #include "Initializer/StaticInitializer.h" 13 | #include "Initializer/DynamicInitializer.h" 14 | 15 | #include "sensors/ImuData.hpp" 16 | 17 | using namespace std; 18 | 19 | namespace orcvio { 20 | 21 | class FlexibleInitializer 22 | { 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | 26 | // Constructor 27 | FlexibleInitializer() = delete; 28 | FlexibleInitializer(const double& max_feature_dis_, const int& static_Num_, 29 | const double& td_, const Eigen::Matrix3d& Ma_, 30 | const Eigen::Matrix3d& Tg_, const Eigen::Matrix3d& As_, 31 | const double& acc_n_, const double& acc_w_, const double& gyr_n_, 32 | const double& gyr_w_, const Eigen::Matrix3d& R_c2b, 33 | const Eigen::Vector3d& t_bc_b, const double& imu_img_timeTh_) { 34 | 35 | staticInitPtr.reset(new StaticInitializer( 36 | max_feature_dis_, static_Num_, td_, Ma_, Tg_, As_)); 37 | 38 | dynamicInitPtr.reset(new DynamicInitializer( 39 | td_, Ma_, Tg_, As_, acc_n_, acc_w_, gyr_n_, 40 | gyr_w_, R_c2b, t_bc_b, imu_img_timeTh_)); 41 | } 42 | 43 | // Destructor 44 | ~FlexibleInitializer(){} 45 | 46 | // Interface for trying to initialize 47 | bool tryIncInit(std::vector& imu_msg_buffer, 48 | MonoCameraMeasurementPtr img_msg, 49 | Eigen::Vector3d& m_gyro_old, Eigen::Vector3d& m_acc_old, 50 | IMUState& imu_state); 51 | 52 | 53 | private: 54 | 55 | // Inclinometer-initializer 56 | boost::shared_ptr staticInitPtr; 57 | // Dynamic initializer 58 | boost::shared_ptr dynamicInitPtr; 59 | 60 | 61 | }; 62 | 63 | } 64 | 65 | 66 | #endif //FLEXIBLE_INITIALIZER_H 67 | -------------------------------------------------------------------------------- /include/Initializer/StaticInitializer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 19-8-13. 3 | // A inclinometer-initializer utilizing the static scene. 4 | // 5 | 6 | #ifndef STATIC_INITIALIZER_H 7 | #define STATIC_INITIALIZER_H 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "orcvio/imu_state.h" 21 | 22 | #include "sensors/ImuData.hpp" 23 | 24 | using namespace std; 25 | 26 | namespace orcvio { 27 | 28 | class StaticInitializer 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | // Constructor 34 | StaticInitializer() = delete; 35 | StaticInitializer(const double& max_feature_dis_, const int& static_Num_, const double& td_, 36 | const Eigen::Matrix3d& Ma_, const Eigen::Matrix3d& Tg_, const Eigen::Matrix3d& As_) : 37 | max_feature_dis(max_feature_dis_), static_Num(static_Num_), td(td_), 38 | bInit(false), state_time(0.0), lower_time_bound(0.0) { 39 | staticImgCounter = 0; 40 | init_features.clear(); 41 | Ma = Ma_; 42 | Tg = Tg_; 43 | As = As_; 44 | gyro_bias = Eigen::Vector3d::Zero(); 45 | acc_bias = Eigen::Vector3d::Zero(); 46 | position = Eigen::Vector3d::Zero(); 47 | velocity = Eigen::Vector3d::Zero(); 48 | 49 | // orientation = Eigen::Vector4d(0.0, 0.0, 0.0, 1.0); 50 | orientation = Eigen::Matrix3d::Identity(); 51 | } 52 | 53 | // Destructor 54 | ~StaticInitializer(){} 55 | 56 | // Interface for trying to initialize 57 | bool tryIncInit(const std::vector& imu_msg_buffer, 58 | MonoCameraMeasurementPtr img_msg); 59 | 60 | // Assign the initial state if initialized successfully 61 | void assignInitialState(std::vector& imu_msg_buffer, 62 | Eigen::Vector3d& m_gyro_old, Eigen::Vector3d& m_acc_old, IMUState& imu_state); 63 | 64 | // If initialized 65 | bool ifInitialized() { 66 | return bInit; 67 | } 68 | 69 | private: 70 | 71 | // Time lower bound for used imu data. 72 | double lower_time_bound; 73 | 74 | // Maximum feature distance allowed bewteen static images 75 | double max_feature_dis; 76 | 77 | // Number of consecutive image for trigger static initializer 78 | unsigned int static_Num; 79 | 80 | // Defined type for initialization 81 | typedef std::map, 82 | Eigen::aligned_allocator< 83 | std::pair > > InitFeatures; 84 | InitFeatures init_features; 85 | 86 | // Counter for static images that will be used in inclinometer-initializer 87 | unsigned int staticImgCounter; 88 | 89 | // Error bewteen timestamp of imu and img 90 | double td; 91 | 92 | // Imu instrinsic 93 | Eigen::Matrix3d Ma; 94 | Eigen::Matrix3d Tg; 95 | Eigen::Matrix3d As; 96 | 97 | // Initialize results 98 | double state_time; 99 | Eigen::Vector3d gyro_bias; 100 | Eigen::Vector3d acc_bias; 101 | 102 | // Eigen::Vector4d orientation; 103 | Eigen::Matrix3d orientation; 104 | 105 | Eigen::Vector3d position; 106 | Eigen::Vector3d velocity; 107 | 108 | // Flag indicating if initialized 109 | bool bInit; 110 | 111 | // initialize rotation and gyro bias by static imu datas 112 | void initializeGravityAndBias(const double& time_bound, 113 | const std::vector& imu_msg_buffer); 114 | }; 115 | 116 | } // namespace orcvio 117 | 118 | 119 | #endif //STATIC_INITIALIZER_H 120 | -------------------------------------------------------------------------------- /include/Initializer/feature_manager.h: -------------------------------------------------------------------------------- 1 | // Feature manager. 2 | // The original file belong to VINS-MONO (https://github.com/HKUST-Aerial-Robotics/VINS-Mono). 3 | 4 | #ifndef FEATURE_MANAGER_H 5 | #define FEATURE_MANAGER_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace std; 12 | 13 | #include 14 | using namespace Eigen; 15 | 16 | #include 17 | 18 | 19 | namespace orcvio { 20 | 21 | const int WINDOW_SIZE = 10; 22 | const double MIN_PARALLAX = 10/460; 23 | 24 | class FeaturePerFrame 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | FeaturePerFrame(const MonoFeatureMeasurement& _point, double td) 30 | { 31 | point.x() = _point.u+_point.u_vel*td; 32 | point.y() = _point.v+_point.v_vel*td; 33 | point.z() = 1; 34 | velocity.x() = _point.u_vel; 35 | velocity.y() = _point.v_vel; 36 | cur_td = td; 37 | } 38 | double cur_td; 39 | Vector3d point; 40 | Vector2d velocity; 41 | double z; 42 | bool is_used; 43 | double parallax; 44 | MatrixXd A; 45 | VectorXd b; 46 | double dep_gradient; 47 | }; 48 | 49 | class FeaturePerId 50 | { 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | const int feature_id; 55 | int start_frame; 56 | vector feature_per_frame; 57 | 58 | int used_num; 59 | bool is_outlier; 60 | bool is_margin; 61 | double estimated_depth; 62 | int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail; 63 | 64 | Vector3d gt_p; 65 | 66 | FeaturePerId(int _feature_id, int _start_frame) 67 | : feature_id(_feature_id), start_frame(_start_frame), 68 | used_num(0), estimated_depth(-1.0), solve_flag(0) 69 | { 70 | } 71 | 72 | int endFrame(); 73 | }; 74 | 75 | class FeatureManager 76 | { 77 | public: 78 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 79 | 80 | FeatureManager(){}; 81 | 82 | void setRic(const Matrix3d& _ric); 83 | 84 | void clearState(); 85 | 86 | int getFeatureCount(); 87 | 88 | bool addFeatureCheckParallax(int frame_count, MonoCameraMeasurementPtr image, double td); 89 | vector> getCorresponding(int frame_count_l, int frame_count_r); 90 | 91 | //void updateDepth(const VectorXd &x); 92 | void setDepth(const VectorXd &x); 93 | void removeFailures(); 94 | void clearDepth(const VectorXd &x); 95 | VectorXd getDepthVector(); 96 | void removeBack(); 97 | void removeFront(int frame_count); 98 | void removeOutlier(); 99 | list feature; 100 | int last_track_num; 101 | 102 | private: 103 | double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count); 104 | Matrix3d ric; 105 | }; 106 | 107 | } 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /include/Initializer/initial_alignment.h: -------------------------------------------------------------------------------- 1 | // 2 | // Added by xiaochen at 19-8-16. 3 | // Type and methods for initial alignment. 4 | // The original file belong to VINS-MONO (https://github.com/HKUST-Aerial-Robotics/VINS-Mono). 5 | // 6 | 7 | #ifndef INITIAL_ALIGNMENT_H 8 | #define INITIAL_ALIGNMENT_H 9 | 10 | // #pragma once 11 | #include 12 | #include 13 | #include "Initializer/ImuPreintegration.h" 14 | #include "Initializer/feature_manager.h" 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | using namespace Eigen; 22 | using namespace std; 23 | 24 | namespace orcvio { 25 | 26 | class ImageFrame 27 | { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | ImageFrame(){}; 32 | ImageFrame(MonoCameraMeasurementPtr _points, const double& td):is_key_frame{false} 33 | { 34 | t = _points->timeStampToSec+td; 35 | for (const auto& pt : _points->features) 36 | { 37 | double x = pt.u+pt.u_vel*td; 38 | double y = pt.v+pt.v_vel*td; 39 | double z = 1; 40 | double p_u = pt.u; 41 | double p_v = pt.v; 42 | double velocity_x = pt.u_vel; 43 | double velocity_y = pt.v_vel; 44 | Eigen::Matrix xyz_uv_velocity; 45 | xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y; 46 | points[pt.id] = xyz_uv_velocity; 47 | } 48 | }; 49 | map> points; 50 | double t; 51 | Matrix3d R; 52 | Vector3d T; 53 | boost::shared_ptr pre_integration; 54 | bool is_key_frame; 55 | }; 56 | 57 | bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x, const Vector3d& TIC); 58 | 59 | } 60 | 61 | 62 | #endif //INITIAL_ALIGNMENT_H 63 | -------------------------------------------------------------------------------- /include/Initializer/initial_sfm.h: -------------------------------------------------------------------------------- 1 | // 2 | // Added by xiaochen at 19-8-16. 3 | // Type and methods for initial sfm. 4 | // The original file belong to VINS-MONO (https://github.com/HKUST-Aerial-Robotics/VINS-Mono). 5 | // 6 | 7 | #ifndef INITIAL_SFM_H 8 | #define INITIAL_SFM_H 9 | 10 | // #pragma once 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace Eigen; 22 | using namespace std; 23 | 24 | namespace orcvio { 25 | 26 | struct SFMFeature 27 | { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | bool state; 31 | int id; 32 | vector> observation; 33 | double position[3]; 34 | double depth; 35 | }; 36 | 37 | struct ReprojectionError3D 38 | { 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | ReprojectionError3D(double observed_u, double observed_v) 42 | :observed_u(observed_u), observed_v(observed_v) 43 | {} 44 | 45 | template 46 | bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const 47 | { 48 | T p[3]; 49 | ceres::QuaternionRotatePoint(camera_R, point, p); 50 | p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; 51 | T xp = p[0] / p[2]; 52 | T yp = p[1] / p[2]; 53 | residuals[0] = xp - T(observed_u); 54 | residuals[1] = yp - T(observed_v); 55 | return true; 56 | } 57 | 58 | static ceres::CostFunction* Create(const double observed_x, 59 | const double observed_y) 60 | { 61 | return (new ceres::AutoDiffCostFunction< 62 | ReprojectionError3D, 2, 4, 3, 3>( 63 | new ReprojectionError3D(observed_x,observed_y))); 64 | } 65 | 66 | double observed_u; 67 | double observed_v; 68 | }; 69 | 70 | class GlobalSFM 71 | { 72 | public: 73 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 74 | 75 | GlobalSFM(); 76 | bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l, 77 | const Matrix3d relative_R, const Vector3d relative_T, 78 | vector &sfm_f, map &sfm_tracked_points); 79 | 80 | private: 81 | bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector &sfm_f); 82 | 83 | void triangulatePoint(Eigen::Matrix &Pose0, Eigen::Matrix &Pose1, 84 | Vector2d &point0, Vector2d &point1, Vector3d &point_3d); 85 | void triangulateTwoFrames(int frame0, Eigen::Matrix &Pose0, 86 | int frame1, Eigen::Matrix &Pose1, 87 | vector &sfm_f); 88 | 89 | int feature_num; 90 | }; 91 | 92 | } 93 | 94 | 95 | #endif //INITIAL_SFM_H -------------------------------------------------------------------------------- /include/Initializer/solve_5pts.h: -------------------------------------------------------------------------------- 1 | // 2 | // Added by xiaochen at 19-8-16. 3 | // Solving relative motion. 4 | // The original file belong to VINS-MONO (https://github.com/HKUST-Aerial-Robotics/VINS-Mono). 5 | // 6 | 7 | #ifndef SOLVE_5PTS_H 8 | #define SOLVE_5PTS_H 9 | 10 | // #pragma once 11 | 12 | #include 13 | 14 | using namespace std; 15 | 16 | #include 17 | #include 18 | 19 | using namespace Eigen; 20 | 21 | 22 | namespace orcvio { 23 | 24 | class MotionEstimator 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | bool solveRelativeRT(const vector> &corres, Matrix3d &R, Vector3d &T); 30 | }; 31 | 32 | } 33 | 34 | 35 | #endif //SOLVE_5PTS_H -------------------------------------------------------------------------------- /include/ORB/ORBDescriptor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 18-8-27. 3 | // Functions for calculate ORB descriptor of an arbitrary image point. 4 | // Stems from ORB_SLAM2: ORBextractor.cc and opencv/orb.cpp 5 | // 6 | 7 | #ifndef LKTTRACKER_ORBDESCRIPTOR_H 8 | #define LKTTRACKER_ORBDESCRIPTOR_H 9 | 10 | // #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | namespace orcvio { 17 | 18 | class ORBdescriptor // this class is created by QXC based on ORB_SLAM2::ORBextractor 19 | { 20 | public: 21 | 22 | ORBdescriptor(const cv::Mat& _image, float _scaleFactor, int _nlevels, 23 | int _edgeThreshold=31, int _patchSize=31); 24 | 25 | ~ORBdescriptor(){} 26 | 27 | // modified based on ORBextractor::computeDescriptors by QXC 28 | // @INPUTs: 29 | // @pts: cv::Point object of key points in source image, i.e. image at level 0 30 | // @levels: level of @image in pyramid 31 | // @OUTPUTs 32 | // @descriptors: each row store the descriptor of corresponding key point 33 | // @BRIEF: 34 | // This function calculate the descriptors of keypoints of same level. 35 | // Should be called after calling function initializeLayerAndPyramid(). 36 | bool computeDescriptors(const vector& pts, 37 | const vector& levels, 38 | cv::Mat& descriptors); 39 | 40 | // copy from ORBmatcher.cc, below is original comment 41 | // Bit set count operation from 42 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 43 | static int computeDescriptorDistance(const cv::Mat &a, const cv::Mat &b) 44 | { 45 | const int *pa = a.ptr(); 46 | const int *pb = b.ptr(); 47 | 48 | int dist=0; 49 | 50 | for(int i=0; i<8; i++, pa++, pb++) 51 | { 52 | unsigned int v = *pa ^ *pb; 53 | v = v - ((v >> 1) & 0x55555555); 54 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 55 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 56 | } 57 | 58 | return dist; 59 | } 60 | 61 | static const float factorPI; 62 | 63 | static const int TH_HIGH; // threshold for worst descriptor distance in all levels 64 | static const int TH_LOW; // threshold for best descriptor distance in all levels 65 | 66 | private: 67 | 68 | static const int HARRIS_BLOCK_SIZE; 69 | 70 | int patchSize; 71 | int halfPatchSize; 72 | int edgeThreshold; 73 | 74 | double scaleFactor; 75 | int nlevels; 76 | 77 | vector pattern; 78 | vector umax; 79 | 80 | vector mvScaleFactor; 81 | vector mvInvScaleFactor; 82 | vector mvLayerInfo; 83 | vector mvLayerScale; 84 | cv::Mat mImagePyramid; 85 | cv::Mat mBluredImagePyramid; 86 | 87 | private: 88 | // compute descriptor of a key point and save it into @desc 89 | void computeOrbDescriptor(const cv::KeyPoint& kpt, 90 | uchar* desc); 91 | 92 | // copy from opencv/orb.cpp 93 | void makeRandomPattern(int patchSize, cv::Point* pattern_, int npoints); 94 | 95 | // initialize layer information and image of pyramid 96 | void initializeLayerAndPyramid(const cv::Mat& image); 97 | 98 | public: 99 | // calculate Angle for an ordinary point 100 | float IC_Angle(const int& levels, const cv::Point2f& pt); 101 | }; 102 | 103 | } 104 | 105 | 106 | #endif //LKTTRACKER_ORBDESCRIPTOR_H -------------------------------------------------------------------------------- /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 | * @brief Sparse feature class used to collect measurements 13 | * 14 | * This feature class allows for holding of all tracking information for a given feature. 15 | * Each feature has a unique ID assigned to it, and should have a set of feature tracks alongside it. 16 | * See the FeatureDatabase class for details on how we load information into this, and how we delete features. 17 | */ 18 | class Feature { 19 | 20 | public: 21 | 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | /// Unique ID of this feature 25 | size_t featid; 26 | 27 | /// If this feature should be deleted 28 | bool to_delete; 29 | 30 | /// UV coordinates that this feature has been seen from (mapped by camera ID) 31 | std::unordered_map> uvs; 32 | 33 | /// UV normalized coordinates that this feature has been seen from (mapped by camera ID) 34 | std::unordered_map> uvs_norm; 35 | 36 | /// Timestamps of each UV measurement (mapped by camera ID) 37 | std::unordered_map> timestamps; 38 | 39 | /// What camera ID our pose is anchored in!! By default the first measurement is the anchor. 40 | int anchor_cam_id = -1; 41 | 42 | /// Timestamp of anchor clone 43 | double anchor_clone_timestamp; 44 | 45 | /// Triangulated position of this feature, in the anchor frame 46 | Eigen::Vector3d p_FinA; 47 | 48 | /// Triangulated position of this feature, in the global frame 49 | Eigen::Vector3d p_FinG; 50 | 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 | } // end of orcvio 74 | 75 | #endif /* FEATURE_H */ -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | // Measurement for one features 14 | class MonoFeatureMeasurement { 15 | public: 16 | MonoFeatureMeasurement() 17 | : id(0) 18 | , u(0.0) 19 | , v(0.0) 20 | , u_init(0.0) 21 | , v_init(0.0) 22 | , u_vel(0.0) 23 | , v_vel(0.0) 24 | , u_init_vel(0.0) 25 | , v_init_vel(0.0) { 26 | } 27 | 28 | // id 29 | unsigned long long int id; 30 | // Normalized feature coordinates (with identity intrinsic matrix) 31 | double u; // horizontal coordinate 32 | double v; // vertical coordinate 33 | // Normalized feature coordinates (with identity intrinsic matrix) in initial frame of this feature 34 | //# (meaningful if this is the first msg of this feature id) 35 | double u_init; 36 | double v_init; 37 | // Velocity of current normalized feature coordinate 38 | double u_vel; 39 | double v_vel; 40 | // Velocity of initial normalized feature coordinate 41 | double u_init_vel; 42 | double v_init_vel; 43 | }; 44 | 45 | // Measurements for features in one camera img 46 | class MonoCameraMeasurement { 47 | public: 48 | double timeStampToSec; 49 | // All features on the current image, 50 | // including tracked ones and newly detected ones. 51 | std::vector features; 52 | }; 53 | 54 | // typedef boost::shared_ptr MonoCameraMeasurementPtr; 55 | typedef MonoCameraMeasurement* MonoCameraMeasurementPtr; 56 | 57 | } 58 | 59 | #endif //FEATURE_MSG_H -------------------------------------------------------------------------------- /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_ */ -------------------------------------------------------------------------------- /include/orcvio/imu_state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT AND PERMISSION NOTICE 3 | * Penn Software MSCKF_VIO 4 | * Copyright (C) 2017 The Trustees of the University of Pennsylvania 5 | * All rights reserved. 6 | */ 7 | 8 | // The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/) 9 | // Some changes have been made to use it in orcvio 10 | 11 | 12 | #ifndef IMU_STATE_H 13 | #define IMU_STATE_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define GRAVITY_ACCELERATION 9.81 21 | 22 | namespace orcvio { 23 | 24 | typedef long long int FeatureIDType; 25 | 26 | /* 27 | * @brief IMUState State for IMU 28 | */ 29 | struct IMUState { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 31 | typedef long long int StateIDType; 32 | 33 | // An unique identifier for the IMU state. 34 | StateIDType id; 35 | 36 | // id for next IMU state 37 | static StateIDType next_id; 38 | 39 | // Time when the state is recorded 40 | double time; 41 | 42 | // Time interval to the nearest image 43 | double dt; 44 | 45 | // // Orientation 46 | // // Take a quaternion from the world frame to 47 | // // the IMU (body) frame. 48 | // Eigen::Vector4d orientation; 49 | 50 | // Orientation 51 | // rotation matrix from the IMU (body) frame to world frame 52 | Eigen::Matrix3d orientation; 53 | 54 | // Position of the IMU (body) frame 55 | // in the world frame. 56 | Eigen::Vector3d position; 57 | 58 | // Velocity of the IMU (body) frame 59 | // in the world frame. 60 | Eigen::Vector3d velocity; 61 | 62 | // Bias for measured angular velocity 63 | // and acceleration. 64 | Eigen::Vector3d gyro_bias; 65 | Eigen::Vector3d acc_bias; 66 | 67 | // Transformation between the IMU and the 68 | // left camera (cam0) 69 | Eigen::Matrix3d R_imu_cam0; 70 | Eigen::Vector3d t_cam0_imu; 71 | 72 | // Gravity vector in the world frame 73 | static Eigen::Vector3d gravity; 74 | 75 | // Transformation offset from the IMU frame to 76 | // the body frame. The transformation takes a 77 | // vector from the IMU frame to the body frame. 78 | // The z axis of the body frame should point upwards. 79 | // Normally, this transform should be identity. 80 | static Eigen::Isometry3d T_imu_body; 81 | 82 | IMUState(): id(0), time(0), 83 | orientation(Eigen::Matrix3d::Identity()), 84 | position(Eigen::Vector3d::Zero()), 85 | velocity(Eigen::Vector3d::Zero()), 86 | gyro_bias(Eigen::Vector3d::Zero()), 87 | acc_bias(Eigen::Vector3d::Zero()) {} 88 | 89 | IMUState(const StateIDType& new_id): id(new_id), time(0), 90 | orientation(Eigen::Matrix3d::Identity()), 91 | position(Eigen::Vector3d::Zero()), 92 | velocity(Eigen::Vector3d::Zero()), 93 | gyro_bias(Eigen::Vector3d::Zero()), 94 | acc_bias(Eigen::Vector3d::Zero()) {} 95 | 96 | }; 97 | 98 | typedef IMUState::StateIDType StateIDType; 99 | 100 | /* 101 | * @brief IMUState_aug Augmented state for IMU, including only orientation and position 102 | */ 103 | struct IMUState_Aug { 104 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 105 | 106 | // An unique identifier for the IMU state. 107 | StateIDType id; 108 | 109 | // Time when the state is recorded 110 | double time; 111 | 112 | // Time interval to the nearest image 113 | double dt; 114 | 115 | // Orientation 116 | // rotation matrix from the IMU (body) frame to the world frame 117 | Eigen::Matrix3d orientation; 118 | 119 | // Position of the IMU (body) frame 120 | // in the world frame. 121 | Eigen::Vector3d position; 122 | 123 | // First estimated Jacobian of position; 124 | Eigen::Vector3d position_FEJ; 125 | 126 | // Transformation between the IMU and the 127 | // left camera (cam0) 128 | Eigen::Matrix3d R_imu_cam0; 129 | Eigen::Vector3d t_cam0_imu; 130 | 131 | // Corresponding camera pose 132 | Eigen::Matrix3d orientation_cam; 133 | 134 | Eigen::Vector3d position_cam; 135 | 136 | // Store the feature id whose anchor frame is 137 | // corresponding to this imu state. 138 | std::vector FeatureIDs; 139 | 140 | IMUState_Aug(): id(0), time(0), 141 | orientation(Eigen::Matrix3d::Identity()), 142 | position(Eigen::Vector3d::Zero()) {} 143 | 144 | IMUState_Aug(const StateIDType& new_id): id(new_id), time(0), 145 | orientation(Eigen::Matrix3d::Identity()), 146 | position(Eigen::Vector3d::Zero()) {} 147 | 148 | }; 149 | 150 | // for augmented imu states, added by QXC 151 | typedef std::map, 152 | Eigen::aligned_allocator< 153 | std::pair > > IMUStateServer; 154 | 155 | } // namespace orcvio 156 | 157 | #endif // IMU_STATE_H 158 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/orcvio/tests/test_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef TEST_UTILS_H 2 | #define TEST_UTILS_H 1 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "orcvio/obj/ObjectFeatureInitializer.h" 10 | #include "orcvio/obj/ObjectFeature.h" 11 | #include "orcvio/feat/FeatureInitializer.h" 12 | 13 | 14 | namespace orcvio { 15 | int load_multi_frame_test_data(const std::string& filename, 16 | std::unordered_map>& clones_cam, 17 | std::shared_ptr& obj_obs_ptr, 18 | Eigen::Matrix4d& wTq_gt, 19 | Eigen::Vector3d& object_mean_shape, 20 | Eigen::MatrixX3d& object_keypoints_mean); 21 | const std::string ensure_path_exists(const std::string& filepath); 22 | } // namespace orcvio 23 | 24 | #endif // TEST_UTILS_H 25 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/orcvio/utils/EigenLevenbergMarquardt/LMpar.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 EIGENLEVENBERGMARQUARDT_LMPAR_H 13 | #define EIGENLEVENBERGMARQUARDT_LMPAR_H 14 | 15 | namespace EigenLevenbergMarquardt { 16 | 17 | namespace internal { 18 | using namespace Eigen; 19 | using namespace Eigen::internal; 20 | 21 | template 22 | void lmpar2( 23 | const QRSolver &qr, 24 | const VectorType &diag, 25 | const VectorType &qtb, 26 | typename VectorType::Scalar m_delta, 27 | typename VectorType::Scalar &par, 28 | VectorType &x) 29 | 30 | { 31 | using std::sqrt; 32 | using std::abs; 33 | typedef typename QRSolver::MatrixType MatrixType; 34 | typedef typename QRSolver::Scalar Scalar; 35 | // typedef typename QRSolver::StorageIndex StorageIndex; 36 | 37 | /* Local variables */ 38 | Index j; 39 | Scalar fp; 40 | Scalar parc, parl; 41 | Index iter; 42 | Scalar temp, paru; 43 | Scalar gnorm; 44 | Scalar dxnorm; 45 | 46 | // Make a copy of the triangular factor. 47 | // This copy is modified during call the qrsolv 48 | MatrixType s; 49 | s = qr.matrixR(); 50 | 51 | /* Function Body */ 52 | const Scalar dwarf = (std::numeric_limits::min)(); 53 | const Index n = qr.matrixR().cols(); 54 | eigen_assert(n==diag.size()); 55 | eigen_assert(n==qtb.size()); 56 | 57 | VectorType wa1, wa2; 58 | 59 | /* compute and store in x the gauss-newton direction. if the */ 60 | /* jacobian is rank-deficient, obtain a least squares solution. */ 61 | 62 | // const Index rank = qr.nonzeroPivots(); // exactly double(0.) 63 | const Index rank = qr.rank(); // use a threshold 64 | wa1 = qtb; 65 | wa1.tail(n-rank).setZero(); 66 | //FIXME There is no solve in place for sparse triangularView 67 | wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView().solve(qtb.head(rank)); 68 | 69 | x = qr.colsPermutation()*wa1; 70 | 71 | /* initialize the iteration counter. */ 72 | /* evaluate the function at the origin, and test */ 73 | /* for acceptance of the gauss-newton direction. */ 74 | iter = 0; 75 | wa2 = diag.cwiseProduct(x); 76 | dxnorm = wa2.blueNorm(); 77 | fp = dxnorm - m_delta; 78 | if (fp <= Scalar(0.1) * m_delta) { 79 | par = 0; 80 | return; 81 | } 82 | 83 | /* if the jacobian is not rank deficient, the newton */ 84 | /* step provides a lower bound, parl, for the zero of */ 85 | /* the function. otherwise set this bound to zero. */ 86 | parl = 0.; 87 | if (rank==n) { 88 | wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm; 89 | s.topLeftCorner(n,n).transpose().template triangularView().solveInPlace(wa1); 90 | temp = wa1.blueNorm(); 91 | parl = fp / m_delta / temp / temp; 92 | } 93 | 94 | /* calculate an upper bound, paru, for the zero of the function. */ 95 | for (j = 0; j < n; ++j) 96 | wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)]; 97 | 98 | gnorm = wa1.stableNorm(); 99 | paru = gnorm / m_delta; 100 | if (paru == 0.) 101 | paru = dwarf / (std::min)(m_delta,Scalar(0.1)); 102 | 103 | /* if the input par lies outside of the interval (parl,paru), */ 104 | /* set par to the closer endpoint. */ 105 | par = (std::max)(par,parl); 106 | par = (std::min)(par,paru); 107 | if (par == 0.) 108 | par = gnorm / dxnorm; 109 | 110 | /* beginning of an iteration. */ 111 | while (true) { 112 | ++iter; 113 | 114 | /* evaluate the function at the current value of par. */ 115 | if (par == 0.) 116 | par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */ 117 | wa1 = sqrt(par)* diag; 118 | 119 | VectorType sdiag(n); 120 | lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag); 121 | 122 | wa2 = diag.cwiseProduct(x); 123 | dxnorm = wa2.blueNorm(); 124 | temp = fp; 125 | fp = dxnorm - m_delta; 126 | 127 | /* if the function is small enough, accept the current value */ 128 | /* of par. also test for the exceptional cases where parl */ 129 | /* is zero or the number of iterations has reached 10. */ 130 | if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) 131 | break; 132 | 133 | /* compute the newton correction. */ 134 | wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 135 | // we could almost use this here, but the diagonal is outside qr, in sdiag[] 136 | for (j = 0; j < n; ++j) { 137 | wa1[j] /= sdiag[j]; 138 | temp = wa1[j]; 139 | for (Index i = j+1; i < n; ++i) 140 | wa1[i] -= s.coeff(i,j) * temp; 141 | } 142 | temp = wa1.blueNorm(); 143 | parc = fp / m_delta / temp / temp; 144 | 145 | /* depending on the sign of the function, update parl or paru. */ 146 | if (fp > 0.) 147 | parl = (std::max)(parl,par); 148 | if (fp < 0.) 149 | paru = (std::min)(paru,par); 150 | 151 | /* compute an improved estimate for par. */ 152 | par = (std::max)(parl,par+parc); 153 | } 154 | if (iter == 0) 155 | par = 0.; 156 | return; 157 | } 158 | } // end namespace internal 159 | 160 | } // end namespace Eigen 161 | 162 | #endif // EIGEN_LMPAR_H 163 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /include/sensors/ImageData.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Descripttion: Type of image sensor data 3 | * @Author: Xiaochen Qiu 4 | */ 5 | 6 | 7 | #ifndef IMAGE_DATA_HPP 8 | #define IMAGE_DATA_HPP 9 | 10 | 11 | #include "opencv2/core.hpp" 12 | #include "boost/shared_ptr.hpp" 13 | 14 | namespace orcvio { 15 | 16 | struct ImgData { 17 | double timeStampToSec; 18 | cv::Mat image; 19 | }; 20 | 21 | typedef boost::shared_ptr ImageDataPtr; 22 | 23 | } 24 | 25 | 26 | #endif // IMAGE_DATA_HPP -------------------------------------------------------------------------------- /include/sensors/ImuData.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Descripttion: Types of IMU sensor data. 3 | * @Author: Xiaochen Qiu 4 | */ 5 | 6 | 7 | #ifndef IMU_DATA_HPP 8 | #define IMU_DATA_HPP 9 | 10 | 11 | #include "Eigen/Core" 12 | #include "Eigen/Dense" 13 | 14 | namespace orcvio { 15 | 16 | struct ImuData { 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | ImuData (double t, double wx, double wy, double wz, 20 | double ax, double ay, double az) { 21 | timeStampToSec = t; 22 | angular_velocity[0] = wx; 23 | angular_velocity[1] = wy; 24 | angular_velocity[2] = wz; 25 | linear_acceleration[0] = ax; 26 | linear_acceleration[1] = ay; 27 | linear_acceleration[2] = az; 28 | } 29 | 30 | ImuData (double t, const Eigen::Vector3d& omg, const Eigen::Vector3d& acc) { 31 | timeStampToSec = t; 32 | angular_velocity = omg; 33 | linear_acceleration = acc; 34 | } 35 | 36 | double timeStampToSec; 37 | Eigen::Vector3d angular_velocity; 38 | Eigen::Vector3d linear_acceleration; 39 | }; 40 | 41 | } 42 | 43 | 44 | #endif // IMU_DATA_HPP -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /requirements.md: -------------------------------------------------------------------------------- 1 | ## how to install the required libraries 2 | 3 | - sophus 4 | 5 | ``` 6 | git clone https://github.com/strasdat/Sophus.git 7 | cd Sophus 8 | git checkout v1.0.0 9 | mkdir build 10 | cd build 11 | cmake .. 12 | make 13 | ``` 14 | -------------------------------------------------------------------------------- /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/darknet_ros_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package darknet_ros_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.4 (2019-03-03) 6 | ------------------ 7 | 8 | 1.1.3 (2018-04-26) 9 | ------------------ 10 | * Fixed formatting part 2. 11 | * Merge branch 'firephinx-master' 12 | * Merge branch 'master' of https://github.com/firephinx/darknet_ros into firephinx-master 13 | * Added rgb_image_header to BoundingBoxes msg. 14 | * Merge pull request `#57 `_ from leggedrobotics/devel/threads 15 | Devel/threads 16 | * Adapted package description. 17 | * Merge branch 'master' into devel/threads 18 | * Update package.xml 19 | * Contributors: Kevin Zhang, Marko Bjelonic 20 | 21 | 1.1.2 (2018-01-06) 22 | ------------------ 23 | * First release of darknet_ros_msgs. 24 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | 3 | project(darknet_ros_msgs) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(catkin REQUIRED 8 | COMPONENTS 9 | actionlib_msgs 10 | geometry_msgs 11 | sensor_msgs 12 | std_msgs 13 | message_generation 14 | ) 15 | 16 | add_message_files( 17 | FILES 18 | BoundingBox.msg 19 | BoundingBoxes.msg 20 | ObjectCount.msg 21 | ) 22 | 23 | add_action_files( 24 | FILES 25 | CheckForObjects.action 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | actionlib_msgs 31 | geometry_msgs 32 | sensor_msgs 33 | std_msgs 34 | ) 35 | 36 | catkin_package( 37 | CATKIN_DEPENDS 38 | actionlib_msgs 39 | geometry_msgs 40 | sensor_msgs 41 | message_runtime 42 | std_msgs 43 | ) 44 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/action/CheckForObjects.action: -------------------------------------------------------------------------------- 1 | # Check if objects in image 2 | 3 | # Goal definition 4 | int16 id 5 | sensor_msgs/Image image 6 | 7 | --- 8 | # Result definition 9 | int16 id 10 | darknet_ros_msgs/BoundingBoxes bounding_boxes 11 | 12 | --- 13 | # Feedback definition 14 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | float64 probability 2 | int64 xmin 3 | int64 ymin 4 | int64 xmax 5 | int64 ymax 6 | int16 id 7 | string Class 8 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/msg/BoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Header image_header 3 | BoundingBox[] bounding_boxes 4 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/msg/ObjectCount.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int8 count 3 | -------------------------------------------------------------------------------- /ros_wrapper/src/darknet_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | darknet_ros_msgs 4 | 1.1.4 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/src/orcvio/include/ObjectMapper_nodelet.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 19-8-21. 3 | // Nodelet for system manager. 4 | // 5 | 6 | #ifndef OBJECTMAPPER_NODELET_H 7 | #define OBJECTMAPPER_NODELET_H 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace orcvio { 14 | class ObjectMapperNodelet : public nodelet::Nodelet { 15 | public: 16 | ObjectMapperNodelet() { return; } 17 | ~ObjectMapperNodelet() { 18 | std::cout << "in ~ObjectMapperNodelet()" << std::endl; 19 | return; 20 | } 21 | 22 | private: 23 | virtual void onInit(); 24 | ObjectInitPtr object_init_ptr; 25 | }; 26 | } // end namespace orcvio 27 | 28 | #endif //OBJECTMAPPER_NODELET_H 29 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/System.h: -------------------------------------------------------------------------------- 1 | // 2 | // Managing the image processer and the estimator. 3 | // 4 | 5 | #ifndef SYSTEM_H 6 | #define SYSTEM_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "sensors/ImuData.hpp" 34 | #include "sensors/ImageData.hpp" 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | namespace orcvio { 45 | 46 | /* 47 | * @brief Manager of the system. 48 | */ 49 | class System { 50 | 51 | public: 52 | 53 | // Constructor 54 | System(ros::NodeHandle& n); 55 | // Disable copy and assign constructors. 56 | System(const ImageProcessor&) = delete; 57 | System operator=(const System&) = delete; 58 | 59 | // Destructor. 60 | ~System(); 61 | 62 | // Initialize the object. 63 | bool initialize(); 64 | 65 | typedef boost::shared_ptr Ptr; 66 | typedef boost::shared_ptr ConstPtr; 67 | 68 | private: 69 | 70 | // Ros node handle. 71 | ros::NodeHandle nh; 72 | 73 | // Subscribers. 74 | ros::Subscriber img_sub; 75 | ros::Subscriber imu_sub; 76 | 77 | // Publishers. 78 | image_transport::Publisher vis_img_pub; 79 | ros::Publisher odom_pub; 80 | ros::Publisher stable_feature_pub; 81 | ros::Publisher active_feature_pub; 82 | ros::Publisher msckf_feature_pub; 83 | ros::Publisher path_pub; 84 | 85 | ros::Publisher poseout_pub; 86 | bool do_publish_tf_; 87 | tf2_ros::TransformBroadcaster poseout_tf2_broadcaster_; 88 | tf2_ros::Buffer tf2_buffer_; 89 | std::unique_ptr tf2_listener_; 90 | 91 | unsigned int poses_seq_out = 0; 92 | 93 | // Msgs to be published. 94 | std::queue header_buffer_; // buffer for heads of msgs to be published 95 | 96 | // Msgs to be published. 97 | nav_msgs::Odometry odom_msg; 98 | pcl::PointCloud::Ptr stable_feature_msg_ptr; 99 | pcl::PointCloud::Ptr active_feature_msg_ptr; 100 | pcl::PointCloud::Ptr msckf_feature_msg_ptr; 101 | 102 | nav_msgs::Path path_msg; 103 | 104 | // Frame id 105 | std::string fixed_frame_id; 106 | std::string base_frame_id_; 107 | std::string camera_frame_id_; 108 | 109 | // Pointer for image processer. 110 | ImageProcessorPtr ImgProcesser; 111 | 112 | // Pointer for estimator. 113 | OrcVIOPtr Estimator; 114 | 115 | // Directory for files 116 | std::string config_file; 117 | std::string result_file; 118 | 119 | // for saving trajectory 120 | std::string output_dir_traj; 121 | std::ofstream fStateToSave; 122 | 123 | // Our groundtruth states 124 | std::map> gt_states; 125 | 126 | double summed_rmse_ori; 127 | double summed_rmse_pos; 128 | double summed_nees_ori; 129 | double summed_nees_pos; 130 | size_t summed_number; 131 | bool first_pose_flag; 132 | Eigen::Matrix4d T_from_est_to_gt; 133 | 134 | // flags 135 | // whether we load groundtruth or receive a rostopic 136 | int load_groundtruth_flag; 137 | 138 | // IMU message buffer. 139 | std::vector imu_msg_buffer_; 140 | std::mutex imu_msg_buffer_mutex_; 141 | 142 | // Img message buffer. 143 | std::queue img_msg_buffer_; 144 | 145 | /* 146 | * @brief loadParameters 147 | * Load parameters from the parameter server. 148 | */ 149 | bool loadParameters(); 150 | 151 | /* 152 | * @brief createRosIO 153 | * Create ros publisher and subscirbers. 154 | */ 155 | bool createRosIO(); 156 | 157 | /* 158 | * @brief imageCallback 159 | * Callback function for the monocular images. 160 | * @param image msg. 161 | */ 162 | void imageCallback(const sensor_msgs::ImageConstPtr& msg); 163 | 164 | /* 165 | * @brief imuCallback 166 | * Callback function for the imu message. 167 | * @param msg IMU msg. 168 | */ 169 | void imuCallback(const sensor_msgs::ImuConstPtr& msg); 170 | 171 | /* 172 | * @brief function to convert Float64MultiArray to eigen 173 | * @param Float64MultiArray msg. 174 | */ 175 | Eigen::MatrixXd msgToEigen(const std_msgs::Float64MultiArray& msg); 176 | 177 | /* 178 | * @brief publish Publish the results of VIO. 179 | * @param time The time stamp of output msgs. 180 | */ 181 | void publishVIO(const ros::Time& time); 182 | 183 | /* 184 | * @brief publish Publish the groundtruth 185 | * @param time The time stamp of output msgs. 186 | */ 187 | void publishGroundtruth(const ros::Time& time); 188 | 189 | /** 190 | * @brief Get the root of the tf tree 191 | * @param frame_id one of the starting frames 192 | * @param time the time when the tf tree should be sampled 193 | */ 194 | const std::string find_tf_tree_root(const std::string& frame_id, const ros::Time& time); 195 | 196 | }; 197 | 198 | typedef System::Ptr SystemPtr; 199 | typedef System::ConstPtr SystemConstPtr; 200 | 201 | } // end namespace orcvio 202 | 203 | 204 | #endif //SYSTEM_H 205 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/System_nodelet.h: -------------------------------------------------------------------------------- 1 | // 2 | // Nodelet for system manager. 3 | // 4 | 5 | #ifndef SYSTEM_NODELET_H 6 | #define SYSTEM_NODELET_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace orcvio { 13 | class SystemNodelet : public nodelet::Nodelet { 14 | public: 15 | SystemNodelet() { return; } 16 | ~SystemNodelet() { 17 | // debug log 18 | std::cout << "in ~SystemNodelet()" << std::endl; 19 | return; } 20 | 21 | private: 22 | virtual void onInit(); 23 | SystemPtr system_ptr; 24 | }; 25 | } // end namespace orcvio 26 | 27 | #endif //SYSTEM_NODELET_H 28 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/include/gt_odom.h: -------------------------------------------------------------------------------- 1 | #ifndef GTODOM_H 2 | #define GTODOM_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | // ref https://gist.github.com/kartikmohta/67cafc968ba5146bc6dcaf721e61b914 17 | 18 | namespace orcvio 19 | { 20 | 21 | /** 22 | * @brief This class takes in published gt odometry and converts it to path for rviz 23 | */ 24 | class OdomToPath { 25 | 26 | public: 27 | 28 | /** 29 | * @brief Default constructor 30 | */ 31 | OdomToPath(ros::NodeHandle& nh, std::string topic) { 32 | 33 | sub_gt_pose = nh.subscribe(topic, 9999, &OdomToPath::gt_odom_path_cb, this); 34 | pub_gt_path = nh.advertise("/orcvio/gt_path", 2); 35 | 36 | first_pose_flag = true; 37 | 38 | set_first_pose_as_origin_flag = true; 39 | 40 | nh.param("output_dir_traj", output_dir_traj, "./cache"); 41 | fStateToSave.open((output_dir_traj+"/stamped_groundtruth.txt").c_str(), std::ofstream::trunc); 42 | 43 | }; 44 | 45 | /** 46 | * @brief Default desctructor 47 | */ 48 | ~OdomToPath() 49 | { 50 | fStateToSave.close(); 51 | } 52 | 53 | /** 54 | * @brief callback function to convert odometry to path 55 | * @param a pointer to the gt odometry 56 | */ 57 | void gt_odom_path_cb(const geometry_msgs::PoseStamped::ConstPtr >_odom_ptr); 58 | 59 | nav_msgs::Path path; 60 | 61 | bool first_pose_flag; 62 | bool set_first_pose_as_origin_flag; 63 | 64 | Eigen::Matrix3d R0; 65 | Eigen::Vector3d p0; 66 | 67 | ros::Subscriber sub_gt_pose; 68 | ros::Publisher pub_gt_path; 69 | 70 | std::string output_dir_traj; 71 | std::ofstream fStateToSave; 72 | 73 | }; 74 | 75 | } 76 | 77 | #endif -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/realsense_d435i/orcvio_mapping_rs_d435i.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 48 | 49 | 50 | 55 | 56 | 57 | 58 | 59 | 60 | [ 61 | 0.9999662089030257, -0.006906390871642436, -0.004459015276860498, 0.008708247801839689, 62 | 0.006874866759746373, 0.9999515383335019, -0.007046785898527633, -0.02174943710377053, 63 | 0.004507467043353249, 0.007015892643993886, 0.9999652293911333, -0.022785325544131582, 64 | 0., 0., 0., 1. 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 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/launch/realsense_d435i/orcvio_vio_rs_d435i.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 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 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | System manager nodelet for organize the visual 8 | front-end and filter back-end. 9 | 10 | 11 | 12 | 15 | 16 | Object mapper nodelet for object LM. 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /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 | 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 | nodelet 26 | image_transport 27 | cv_bridge 28 | message_filters 29 | pcl_conversions 30 | pcl_ros 31 | std_srvs 32 | message_runtime 33 | 34 | sort_ros 35 | 36 | libpcl-all-dev 37 | suitesparse 38 | libceres-dev 39 | sophus 40 | 41 | rosunit 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/ObjectMapper_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace orcvio { 4 | 5 | void ObjectMapperNodelet::onInit() { 6 | 7 | object_init_ptr.reset(new ObjectInitNode(getPrivateNodeHandle())); 8 | NODELET_WARN("Initialized the object mapper Nodelet"); 9 | 10 | return; 11 | } 12 | 13 | PLUGINLIB_EXPORT_CLASS(orcvio::ObjectMapperNodelet, nodelet::Nodelet); 14 | 15 | } // end namespace orcvio 16 | 17 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/System_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace orcvio { 4 | 5 | void SystemNodelet::onInit() { 6 | system_ptr.reset(new System(getPrivateNodeHandle())); 7 | if (!system_ptr->initialize()) { 8 | ROS_ERROR("Cannot initialize System Manager..."); 9 | return; 10 | } 11 | return; 12 | } 13 | 14 | PLUGINLIB_EXPORT_CLASS(orcvio::SystemNodelet, nodelet::Nodelet); 15 | 16 | } // end namespace orcvio 17 | -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/gt_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "gt_odom.h" 11 | 12 | namespace orcvio 13 | { 14 | 15 | void OdomToPath::gt_odom_path_cb(const geometry_msgs::PoseStamped::ConstPtr >_odom_ptr){ 16 | 17 | if (first_pose_flag) 18 | { 19 | p0(0,0) = gt_odom_ptr->pose.position.x; 20 | p0(1,0) = gt_odom_ptr->pose.position.y; 21 | 22 | p0(2,0) = gt_odom_ptr->pose.position.z; 23 | // convert robot pose to imu pose 24 | // p0(2,0) = gt_odom_ptr->pose.position.z + 0.5; 25 | 26 | // convert to quaternion 27 | Eigen::Quaterniond q0; 28 | q0.x() = gt_odom_ptr->pose.orientation.x; 29 | q0.y() = gt_odom_ptr->pose.orientation.y; 30 | q0.z() = gt_odom_ptr->pose.orientation.z; 31 | q0.w() = gt_odom_ptr->pose.orientation.w; 32 | 33 | // convert to rotation matrix 34 | R0 = q0.normalized().toRotationMatrix(); 35 | 36 | first_pose_flag = false; 37 | } 38 | 39 | // convert to quaternion 40 | Eigen::Quaterniond q1; 41 | q1.x() = gt_odom_ptr->pose.orientation.x; 42 | q1.y() = gt_odom_ptr->pose.orientation.y; 43 | q1.z() = gt_odom_ptr->pose.orientation.z; 44 | q1.w() = gt_odom_ptr->pose.orientation.w; 45 | 46 | // convert to rotation matrix 47 | Eigen::Matrix3d R1; 48 | R1 = q1.normalized().toRotationMatrix(); 49 | 50 | // normalize rotation 51 | Eigen::Matrix3d R1_normalized; 52 | if (set_first_pose_as_origin_flag) 53 | // set first pose as origin 54 | R1_normalized = R0.transpose() * R1; 55 | else 56 | R1_normalized = R1; 57 | 58 | // convert to quaternion 59 | Eigen::Quaterniond q1_normalized = Eigen::Quaterniond(R1_normalized); 60 | // normalize the quaternion 61 | q1_normalized = q1_normalized.normalized(); 62 | 63 | geometry_msgs::PoseStamped cur_pose; 64 | cur_pose.header = gt_odom_ptr->header; 65 | cur_pose.header.frame_id = "/world"; 66 | 67 | // set first pose as origin 68 | Eigen::Vector3d t1, t1_new; 69 | t1 << gt_odom_ptr->pose.position.x, gt_odom_ptr->pose.position.y, gt_odom_ptr->pose.position.z; 70 | // convert robot pose to imu pose 71 | // t1 << gt_odom_ptr->pose.position.x, gt_odom_ptr->pose.position.y, gt_odom_ptr->pose.position.z + 0.5; 72 | 73 | if (set_first_pose_as_origin_flag) 74 | // set first pose as origin 75 | t1_new = R0.transpose() * (t1 - p0); 76 | else 77 | t1_new = t1; 78 | 79 | cur_pose.pose.position.x = t1_new(0,0); 80 | cur_pose.pose.position.y = t1_new(1,0); 81 | cur_pose.pose.position.z = t1_new(2,0); 82 | 83 | cur_pose.pose.orientation.x = q1_normalized.x(); 84 | cur_pose.pose.orientation.y = q1_normalized.y(); 85 | cur_pose.pose.orientation.z = q1_normalized.z(); 86 | cur_pose.pose.orientation.w = q1_normalized.w(); 87 | 88 | path.header = cur_pose.header; 89 | path.header.frame_id = "/world"; 90 | path.poses.push_back(cur_pose); 91 | 92 | pub_gt_path.publish(path); 93 | 94 | // save the pose to txt for trajectory evaluation 95 | // timestamp tx ty tz qx qy qz qw 96 | fStateToSave << cur_pose.header.stamp.toSec() << " " 97 | << cur_pose.pose.position.x << " " << cur_pose.pose.position.y << " " << cur_pose.pose.position.z << " " 98 | << cur_pose.pose.orientation.x << " " << cur_pose.pose.orientation.y << " " << cur_pose.pose.orientation.z << " " << cur_pose.pose.orientation.w << std::endl; 99 | 100 | 101 | return; 102 | 103 | } 104 | 105 | } -------------------------------------------------------------------------------- /ros_wrapper/src/orcvio/src/publish_gt_path.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "gt_odom.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 topic; 17 | nh.getParam("topic_pose", topic); 18 | 19 | // Debug 20 | ROS_INFO("Done reading config values"); 21 | ROS_INFO(" - topic = %s", topic.c_str()); 22 | 23 | OdomToPath gt_odom(nh, topic); 24 | 25 | // Done! 26 | ros::spin(); 27 | return EXIT_SUCCESS; 28 | 29 | } -------------------------------------------------------------------------------- /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.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Image1 8 | Splitter Ratio: 0.5 9 | Tree Height: 177 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Image 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Class: rviz/Image 35 | Enabled: true 36 | Image Topic: /SortRos/compressed_image 37 | Max Value: 1 38 | Median window: 5 39 | Min Value: 0 40 | Name: Image 41 | Normalize Range: true 42 | Queue Size: 2 43 | Transport Hint: compressed 44 | Unreliable: false 45 | Value: true 46 | Enabled: true 47 | Global Options: 48 | Background Color: 48; 48; 48 49 | Default Light: false 50 | Fixed Frame: map 51 | Frame Rate: 30 52 | Name: root 53 | Tools: 54 | - Class: rviz/Interact 55 | Hide Inactive Objects: true 56 | - Class: rviz/MoveCamera 57 | - Class: rviz/Select 58 | - Class: rviz/FocusCamera 59 | - Class: rviz/Measure 60 | - Class: rviz/SetInitialPose 61 | Topic: /initialpose 62 | - Class: rviz/SetGoal 63 | Topic: /move_base_simple/goal 64 | - Class: rviz/PublishPoint 65 | Single click: true 66 | Topic: /clicked_point 67 | Value: true 68 | Views: 69 | Current: 70 | Class: rviz/Orbit 71 | Distance: 10 72 | Enable Stereo Rendering: 73 | Stereo Eye Separation: 0.0599999987 74 | Stereo Focal Distance: 1 75 | Swap Stereo Eyes: false 76 | Value: false 77 | Focal Point: 78 | X: 0 79 | Y: 0 80 | Z: 0 81 | Focal Shape Fixed Size: true 82 | Focal Shape Size: 0.0500000007 83 | Invert Z Axis: false 84 | Name: Current View 85 | Near Clip Distance: 0.00999999978 86 | Pitch: 0.785398006 87 | Target Frame: 88 | Value: Orbit (rviz) 89 | Yaw: 0.785398006 90 | Saved: ~ 91 | Window Geometry: 92 | Displays: 93 | collapsed: false 94 | Height: 1176 95 | Hide Left Dock: false 96 | Hide Right Dock: false 97 | Image: 98 | collapsed: false 99 | QMainWindow State: 000000ff00000000fd00000004000000000000071900000452fc020000000bfb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d00610067006501000000280000035a0000001600fffffffb000000100044006900730070006c0061007900730100000388000000f2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000105000000db0000000000000000fb0000000a0049006d006100670065010000014e0000032c0000000000000000000000010000010f00000452fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000452000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000000200000045200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 100 | Selection: 101 | collapsed: false 102 | Time: 103 | collapsed: false 104 | Tool Properties: 105 | collapsed: false 106 | Views: 107 | collapsed: false 108 | Width: 1855 109 | X: 65 110 | Y: 24 111 | -------------------------------------------------------------------------------- /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_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 | -------------------------------------------------------------------------------- /run_euroc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | path=$1 4 | output=$2 5 | 6 | mkdir -p "$output" 7 | if [[ -e "$path" ]]; then 8 | if [[ -f "$path/mav0/imu0/data.csv" ]]; then 9 | ./build/orcvio $path/mav0/imu0/data.csv \ 10 | $path/mav0/cam0/data.csv \ 11 | $path/mav0/cam0/data \ 12 | $output/ \ 13 | config/euroc.yaml 14 | python scripts/convert_csv_to_txt.py $path/mav0/state_groundtruth_estimate0/data.csv $output/stamped_groundtruth.txt 15 | else 16 | echo "bad dataset dir. $path." 17 | fi 18 | else 19 | echo "No dataset dir. For example, ./run_euroc /PATH_TO_EUROC/ " 20 | fi 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /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/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 | -------------------------------------------------------------------------------- /src/FlexibleInitializer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 19-8-13. 3 | // A flexible initializer that can automatically initialize in case of static or dynamic scene. 4 | // 5 | 6 | #include "Initializer/FlexibleInitializer.h" 7 | 8 | namespace orcvio { 9 | 10 | bool FlexibleInitializer::tryIncInit(std::vector& imu_msg_buffer, 11 | MonoCameraMeasurementPtr img_msg, 12 | Eigen::Vector3d& m_gyro_old, Eigen::Vector3d& m_acc_old, 13 | IMUState& imu_state) { 14 | 15 | if(staticInitPtr->tryIncInit(imu_msg_buffer, img_msg)) { 16 | staticInitPtr->assignInitialState(imu_msg_buffer, 17 | m_gyro_old, m_acc_old, imu_state); 18 | return true; 19 | } else if (dynamicInitPtr->tryDynInit(imu_msg_buffer, img_msg)) { 20 | dynamicInitPtr->assignInitialState(imu_msg_buffer, 21 | m_gyro_old, m_acc_old, imu_state); 22 | return true; 23 | } 24 | 25 | return false; 26 | } 27 | 28 | 29 | } -------------------------------------------------------------------------------- /src/StaticInitializer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiaochen at 19-8-13. 3 | // A inclinometer-initializer utilizing the static scene. 4 | // 5 | 6 | #include "Initializer/StaticInitializer.h" 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | namespace orcvio { 12 | 13 | bool StaticInitializer::tryIncInit(const std::vector& imu_msg_buffer, 14 | MonoCameraMeasurementPtr img_msg) { 15 | // return false if this is the 1st image for inclinometer-initializer 16 | if (0 == staticImgCounter) { 17 | staticImgCounter++; 18 | init_features.clear(); 19 | // add features to init_features 20 | for (const auto& feature : img_msg->features) 21 | init_features[feature.id] = Vector2d(feature.u, feature.v); 22 | // assign the lower time bound as the timestamp of first img 23 | lower_time_bound = img_msg->timeStampToSec+td; 24 | return false; 25 | } 26 | 27 | // calculate feature distance of matched features between prev and curr images 28 | InitFeatures curr_features; 29 | list feature_dis; 30 | for (const auto& feature : img_msg->features) { 31 | curr_features[feature.id] = Vector2d(feature.u, feature.v); 32 | if (init_features.find(feature.id) != init_features.end()) { 33 | Vector2d vec2d_c(feature.u, feature.v); 34 | Vector2d vec2d_p = init_features[feature.id]; 35 | feature_dis.push_back((vec2d_c-vec2d_p).norm()); 36 | } 37 | } 38 | // return false if number of matched features is small 39 | if (feature_dis.empty() 40 | || feature_dis.size()<20) { 41 | staticImgCounter = 0; 42 | return false; 43 | } 44 | // ignore outliers rudely 45 | feature_dis.sort(); 46 | auto itr = feature_dis.end(); 47 | for (int i = 0; i < 19; i++) 48 | itr--; 49 | double maxDis = *itr; 50 | // classified as static image if maxDis is smaller than threshold, otherwise reset image counter 51 | if (maxDis < max_feature_dis) { 52 | staticImgCounter++; 53 | init_features.swap(curr_features); 54 | if (staticImgCounter < static_Num) // return false if number of consecitive static images does not reach @static_Num 55 | return false; 56 | } else { 57 | // printf("inclinometer-initializer failed at No.%d static image.",staticImgCounter+1); 58 | staticImgCounter = 0; 59 | return false; 60 | } 61 | 62 | /* reach here means staticImgCounter is equal to static_Num */ 63 | 64 | // initialize rotation and gyro bias by imu data between the 1st and the No.static_Num image 65 | // set take_off_stamp as time of the No.static_Num image 66 | // set initial imu_state as the state of No.static_Num image 67 | // earse imu data with timestamp earlier than the No.static_Num image 68 | initializeGravityAndBias(img_msg->timeStampToSec+td, imu_msg_buffer); 69 | 70 | bInit = true; 71 | 72 | return true; 73 | } 74 | 75 | 76 | void StaticInitializer::initializeGravityAndBias(const double& time_bound, 77 | const std::vector& imu_msg_buffer) { 78 | // Initialize gravity and gyro bias. 79 | Vector3d sum_angular_vel = Vector3d::Zero(); 80 | Vector3d sum_linear_acc = Vector3d::Zero(); 81 | 82 | int usefulImuSize = 0; 83 | double last_imu_time; 84 | for (const auto& imu_msg : imu_msg_buffer) { 85 | double imu_time = imu_msg.timeStampToSec; 86 | if (imu_time < lower_time_bound) continue; 87 | if (imu_time > time_bound) break; 88 | 89 | sum_angular_vel += Tg*(imu_msg.angular_velocity-As*Ma*imu_msg.linear_acceleration); 90 | sum_linear_acc += Ma*imu_msg.linear_acceleration; 91 | 92 | usefulImuSize++; 93 | 94 | last_imu_time = imu_time; 95 | } 96 | 97 | // Compute gyro bias. 98 | gyro_bias = sum_angular_vel / usefulImuSize; 99 | 100 | // This is the gravity in the IMU frame. 101 | Vector3d gravity_imu = 102 | sum_linear_acc / usefulImuSize; 103 | 104 | // Initialize the initial orientation, so that the estimation 105 | // is consistent with the inertial frame. 106 | double gravity_norm = gravity_imu.norm(); 107 | Vector3d gravity_world(0.0, 0.0, -gravity_norm); 108 | 109 | // Set rotation 110 | Quaterniond q0_w_i = Quaterniond::FromTwoVectors( 111 | gravity_imu, -gravity_world); 112 | // orientation = q0_w_i.coeffs(); 113 | orientation = quaternionToRotation(q0_w_i.coeffs()); 114 | 115 | // Set other state and timestamp 116 | state_time = last_imu_time; 117 | position = Vector3d(0.0, 0.0, 0.0); 118 | velocity = Vector3d(0.0, 0.0, 0.0); 119 | acc_bias = Vector3d(0.0, 0.0, 0.0); 120 | 121 | printf("Inclinometer-initializer completed by using %d imu data !!!\n\n",usefulImuSize); 122 | 123 | return; 124 | } 125 | 126 | 127 | void StaticInitializer::assignInitialState(std::vector& imu_msg_buffer, 128 | Eigen::Vector3d& m_gyro_old, Eigen::Vector3d& m_acc_old, IMUState& imu_state) { 129 | if (!bInit) { 130 | printf("Cannot assign initial state before initialization !!!\n"); 131 | return; 132 | } 133 | 134 | // Remove used imu data 135 | int usefulImuSize = 0; 136 | for (const auto& imu_msg : imu_msg_buffer) { 137 | double imu_time = imu_msg.timeStampToSec; 138 | if (imu_time > state_time) break; 139 | usefulImuSize++; 140 | } 141 | if (usefulImuSize>=imu_msg_buffer.size()) 142 | usefulImuSize--; 143 | 144 | // Initialize last m_gyro and last m_acc 145 | const auto& imu_msg = imu_msg_buffer[usefulImuSize]; 146 | m_gyro_old = imu_msg.angular_velocity; 147 | m_acc_old = imu_msg.linear_acceleration; 148 | 149 | // Earse used imu data 150 | imu_msg_buffer.erase(imu_msg_buffer.begin(), 151 | imu_msg_buffer.begin()+usefulImuSize); 152 | 153 | // Set initial state 154 | imu_state.time = state_time; 155 | imu_state.gyro_bias = gyro_bias; 156 | imu_state.acc_bias = acc_bias; 157 | imu_state.orientation = orientation; 158 | imu_state.position = position; 159 | imu_state.velocity = velocity; 160 | 161 | return; 162 | } 163 | 164 | 165 | } -------------------------------------------------------------------------------- /src/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 | // Assert that we have all the parts of a measurement 12 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 13 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 14 | 15 | // Our iterators 16 | auto it1 = timestamps[pair.first].begin(); 17 | auto it2 = uvs[pair.first].begin(); 18 | auto it3 = uvs_norm[pair.first].begin(); 19 | 20 | // Loop through measurement times, remove ones that are not in our timestamps 21 | while (it1 != timestamps[pair.first].end()) { 22 | if (std::find(valid_times.begin(),valid_times.end(),*it1) == valid_times.end()) { 23 | it1 = timestamps[pair.first].erase(it1); 24 | it2 = uvs[pair.first].erase(it2); 25 | it3 = uvs_norm[pair.first].erase(it3); 26 | } else { 27 | ++it1; 28 | ++it2; 29 | ++it3; 30 | } 31 | } 32 | } 33 | 34 | } 35 | 36 | void Feature::clean_older_measurements(double timestamp) { 37 | 38 | 39 | // Loop through each of the cameras we have 40 | for(auto const &pair : timestamps) { 41 | 42 | // Assert that we have all the parts of a measurement 43 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 44 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 45 | 46 | // Our iterators 47 | auto it1 = timestamps[pair.first].begin(); 48 | auto it2 = uvs[pair.first].begin(); 49 | auto it3 = uvs_norm[pair.first].begin(); 50 | 51 | // Loop through measurement times, remove ones that are older then the specified one 52 | while (it1 != timestamps[pair.first].end()) { 53 | if (*it1 <= timestamp) { 54 | it1 = timestamps[pair.first].erase(it1); 55 | it2 = uvs[pair.first].erase(it2); 56 | it3 = uvs_norm[pair.first].erase(it3); 57 | } else { 58 | ++it1; 59 | ++it2; 60 | ++it3; 61 | } 62 | } 63 | } 64 | 65 | } 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/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 | -------------------------------------------------------------------------------- /src/obj/ObjectState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "orcvio/obj/ObjectState.h" 10 | 11 | namespace orcvio 12 | { 13 | 14 | Eigen::MatrixX3d transform_mean_keypoints_to_global(const Eigen::MatrixX3d& object_keypoints_mean_shape, const Eigen::Matrix4d& object_pose) 15 | { 16 | 17 | // get the number of keypoints for this object class 18 | const int kps_num = object_keypoints_mean_shape.rows(); 19 | 20 | Eigen::MatrixX3d object_keypoints_shape_global_frame; 21 | object_keypoints_shape_global_frame = Eigen::MatrixXd::Zero(kps_num, 3); 22 | 23 | Eigen::Matrix3d wRq = object_pose.block(0, 0, 3, 3); 24 | Eigen::Vector3d wPq = object_pose.block(0, 3, 3, 1); 25 | 26 | // std::cout << "wRq " << wRq << std::endl; 27 | // std::cout << "wPq " << wPq.transpose() << std::endl; 28 | 29 | for (int i = 0; i < kps_num; ++i) 30 | { 31 | // std::cout << "keypoint position in object frame " << object_keypoints_mean_shape.row(i) << std::endl; 32 | Eigen::Vector3d keypoint_global_frame = wRq * object_keypoints_mean_shape.row(i).transpose() + wPq; 33 | // std::cout << "keypoint position in global frame " << keypoint_global_frame.transpose() << std::endl; 34 | object_keypoints_shape_global_frame.row(i) = keypoint_global_frame.transpose(); 35 | } 36 | 37 | return object_keypoints_shape_global_frame; 38 | 39 | } 40 | 41 | void save_object_state_to_file(const ObjectState & object_state, const std::vector& timestamps, 42 | std::string filepath_format) 43 | { 44 | boost::format boost_filepath_format(filepath_format); 45 | std::ofstream file((boost_filepath_format % object_state.object_id).str()); 46 | 47 | // std::cout << "debug file " << file.is_open() << std::endl; 48 | 49 | if (file.is_open()) 50 | { 51 | file << "object id:\n" << object_state.object_id << '\n'; 52 | file << "object class:\n" << object_state.object_class << '\n'; 53 | file << "wTq:\n" << object_state.object_pose << '\n'; 54 | file << "keypoints in global frame:\n" << object_state.object_keypoints_shape_global_frame << '\n'; 55 | file << "ellipsoid shape:\n" << object_state.ellipsoid_shape << '\n'; 56 | file << "observation timestamps:\n"; 57 | for (const auto& time: timestamps) 58 | { 59 | file << time << " "; 60 | } 61 | } 62 | // else 63 | // std::cout << "cannot open file" << std::endl; 64 | 65 | } 66 | 67 | } 68 | -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_0.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_0.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_1.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_1.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_10.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_10.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_11.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_11.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_12.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_12.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_13.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_13.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_14.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_14.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_15.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_15.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_16.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_16.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_17.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_17.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_18.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_18.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_19.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_19.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_2.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_2.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_20.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_20.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_21.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_21.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_22.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_22.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_23.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_23.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_24.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_24.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_25.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_25.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_26.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_26.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_27.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_27.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_28.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_28.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_29.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_29.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_3.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_3.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_30.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_30.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_31.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_31.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_32.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_32.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_33.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_33.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_34.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_34.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_35.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_35.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_36.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_36.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_37.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_37.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_38.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_38.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_39.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_39.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_4.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_4.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_40.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_40.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_41.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_41.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_42.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_42.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_43.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_43.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_44.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_44.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_45.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_45.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_46.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_46.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_5.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_5.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_6.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_6.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_7.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_7.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_8.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_8.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car/frame_9.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car/frame_9.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_0.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_0.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_1.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_1.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_10.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_10.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_11.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_11.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_12.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_12.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_13.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_13.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_14.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_14.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_15.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_15.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_16.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_16.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_17.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_17.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_18.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_18.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_19.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_19.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_2.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_2.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_20.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_20.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_21.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_21.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_22.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_22.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_23.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_23.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_24.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_24.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_25.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_25.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_26.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_26.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_27.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_27.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_28.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_28.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_29.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_29.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_3.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_3.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_30.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_30.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_31.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_31.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_32.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_32.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_33.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_33.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_34.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_34.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_35.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_35.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_36.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_36.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_37.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_37.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_38.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_38.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_39.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_39.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_4.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_4.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_40.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_40.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_41.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_41.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_42.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_42.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_43.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_43.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_44.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_44.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_45.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_45.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_46.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_46.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_5.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_5.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_6.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_6.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_7.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_7.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_8.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_8.h5 -------------------------------------------------------------------------------- /src/tests/data/one_car_no_zb/frame_9.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/one_car_no_zb/frame_9.h5 -------------------------------------------------------------------------------- /src/tests/data/test_error_bbox_quadric.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/test_error_bbox_quadric.h5 -------------------------------------------------------------------------------- /src/tests/data/test_error_deform_reg.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/test_error_deform_reg.h5 -------------------------------------------------------------------------------- /src/tests/data/test_error_feature_quadric.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/test_error_feature_quadric.h5 -------------------------------------------------------------------------------- /src/tests/data/test_error_mean_shape_reg.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shanmo/OrcVIO-Lite/f0df1fb8b362db02103cef8b74b204ba163ba0f5/src/tests/data/test_error_mean_shape_reg.h5 -------------------------------------------------------------------------------- /src/tests/test_kabsch.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace std; 6 | using namespace orcvio; 7 | using namespace Eigen; 8 | 9 | // TEST(TestSuite, testCase1) 10 | TEST(KabschTest, test_random) 11 | { 12 | // Create datasets with known transform 13 | Eigen::Matrix3Xd in(3, 100), out(3, 100); 14 | Eigen::Quaternion Q(1, 3, 5, 2); 15 | Q.normalize(); 16 | 17 | Eigen::Matrix3d R = Q.toRotationMatrix(); 18 | double scale = 2.0; 19 | 20 | for (int row = 0; row < in.rows(); row++) 21 | { 22 | for (int col = 0; col < in.cols(); col++) 23 | { 24 | in(row, col) = log(2*row + 10.0)/sqrt(1.0*col + 4.0) + sqrt(col*1.0)/(row + 1.0); 25 | } 26 | } 27 | 28 | Eigen::Vector3d S; 29 | S << -5, 6, -27; 30 | 31 | for (int col = 0; col < in.cols(); col++) 32 | out.col(col) = scale * R * in.col(col) + S; 33 | 34 | Eigen::Matrix4d Trans = findTransform(in, out); 35 | 36 | Eigen::Matrix3d Trans_R = Trans.block<3,3>(0,0); 37 | Eigen::Vector3d Trans_t = Trans.block<3,1>(0,3); 38 | 39 | // See if we got the transform we expected 40 | if ( (scale * R - Trans_R).cwiseAbs().maxCoeff() > 1e-13 || (S - Trans_t).cwiseAbs().maxCoeff() > 1e-13) 41 | throw "Could not determine the affine transform accurately enough"; 42 | 43 | } 44 | 45 | // TEST(TestSuite, testCase1) 46 | TEST(KabschTest, test_planar) 47 | { 48 | // Create datasets with known transform 49 | Eigen::Matrix3Xd in(3, 4), out(3, 4); 50 | Eigen::Quaternion Q(1, 3, 5, 2); 51 | Q.normalize(); 52 | 53 | Eigen::Matrix3d R = Q.toRotationMatrix(); 54 | double scale = 2.0; 55 | 56 | in(0, 0) = -1.25; 57 | in(1, 0) = 0; 58 | in(2, 0) = -1.25; 59 | 60 | in(0, 1) = 1.25; 61 | in(1, 1) = 0; 62 | in(2, 1) = -1.25; 63 | 64 | in(0, 2) = 1.25; 65 | in(1, 2) = 0; 66 | in(2, 2) = 1.25; 67 | 68 | in(0, 3) = -1.25; 69 | in(1, 3) = 0; 70 | in(2, 3) = 1.25; 71 | 72 | Eigen::Vector3d S; 73 | S << -5, 6, -27; 74 | 75 | for (int col = 0; col < in.cols(); col++) 76 | out.col(col) = scale * R * in.col(col) + S; 77 | 78 | Eigen::Matrix4d Trans = findTransform(in, out); 79 | 80 | Eigen::Matrix3d Trans_R = Trans.block<3,3>(0,0); 81 | Eigen::Vector3d Trans_t = Trans.block<3,1>(0,3); 82 | 83 | // See if we got the transform we expected 84 | if ( (scale * R - Trans_R).cwiseAbs().maxCoeff() > 1e-13 || (S - Trans_t).cwiseAbs().maxCoeff() > 1e-13) 85 | throw "Could not determine the affine transform accurately enough"; 86 | 87 | } 88 | -------------------------------------------------------------------------------- /src/tests/test_object_init_multiframe.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "orcvio/obj/ObjectFeature.h" 8 | #include "orcvio/obj/ObjectFeatureInitializer.h" 9 | #include "orcvio/utils/se3_ops.hpp" 10 | #include "orcvio/tests/test_utils.h" 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | using orcvio::vector_eigen; 16 | 17 | using namespace orcvio; 18 | using orcvio::load_multi_frame_test_data; 19 | using orcvio::ensure_path_exists; 20 | using boost::format; 21 | 22 | // TEST(TestSuite, testCase1) 23 | TEST(ObjectInitMultiFrame, test_object_init_multiframe) 24 | { 25 | 26 | std::unordered_map> clones_cam; 27 | 28 | // initialize object feature structure 29 | std::shared_ptr obj_obs_ptr; 30 | obj_obs_ptr.reset(new ObjectFeature(1, "car")); 31 | 32 | Eigen::Matrix4d wTq_gt; 33 | Eigen::Vector3d object_mean_shape; 34 | Eigen::MatrixX3d object_keypoints_mean; 35 | 36 | std::unique_ptr object_feat_init; 37 | Matrix3d camera_intrinsics = Matrix3d::Identity(); 38 | FeatureInitializerOptions featinit_options; 39 | 40 | std::vector input_file_names{"src/tests/data/one_car_no_zb/frame_%d.h5", 41 | "src/tests/data/one_car/frame_%d.h5"}; 42 | 43 | for (auto const & input_file_name : input_file_names) { 44 | 45 | ensure_path_exists((format(input_file_name) % 0).str()); 46 | 47 | if (clones_cam.size()) 48 | clones_cam.erase(clones_cam.begin()); 49 | 50 | obj_obs_ptr.reset(new ObjectFeature(1, "car")); 51 | load_multi_frame_test_data(input_file_name, 52 | clones_cam, 53 | obj_obs_ptr, 54 | wTq_gt, 55 | object_mean_shape, 56 | object_keypoints_mean); 57 | 58 | std::cout << "total frame no.: " << obj_obs_ptr->timestamps[0].size() << std::endl; 59 | 60 | object_feat_init.reset( 61 | new ObjectFeatureInitializer(featinit_options, 62 | object_mean_shape, 63 | object_keypoints_mean, 64 | camera_intrinsics)); 65 | 66 | bool init_success_flag; 67 | Eigen::Matrix4d wTq_est; 68 | std::tie(init_success_flag, wTq_est) = 69 | object_feat_init->single_object_initialization(obj_obs_ptr, 70 | clones_cam); 71 | 72 | //ASSERT_NEAR((wTq_gt - wTq_est).norm(), 0, 6e-1) 73 | // << "wTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 74 | double dispR, dispt; 75 | std::tie(dispR, dispt) = displacement(wTq_gt, wTq_est); 76 | EXPECT_NEAR(dispR, 0, 0.5) 77 | << "Rotation does not match. " 78 | << "\nwTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 79 | ASSERT_NEAR(dispt, 0, 3.5e-1) 80 | << "translation does not match. " 81 | << "\nwTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 82 | } 83 | 84 | 85 | /* 86 | load_multi_frame_test_data("src/tests/data/one_car/frame_%d.h5", 87 | clones_cam, 88 | obj_obs_ptr, 89 | wTq_gt, 90 | object_mean_shape, 91 | object_keypoints_mean); 92 | 93 | object_feat_init.reset(new ObjectFeatureInitializer(featinit_options, 94 | object_mean_shape, 95 | object_keypoints_mean, 96 | camera_intrinsics)); 97 | 98 | std::tie(init_success_flag, wTq_est) = object_feat_init->single_object_initialization(obj_obs_ptr, clones_cam); 99 | 100 | // ASSERT_NEAR((wTq_gt - wTq_est).norm(), 0, 6e-1) 101 | // << "wTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 102 | std::tie(dispR, dispt) = displacement(wTq_gt, wTq_est); 103 | EXPECT_NEAR(dispR, 0, 0.5) 104 | << "Rotation does not match. " 105 | << "\nwTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 106 | ASSERT_NEAR(dispt, 0, 3.5e-1) 107 | << "translation does not match. " 108 | << "\nwTq_est: \n" << wTq_est << "\n" << "wTq_gt: \n" << wTq_gt << "\n"; 109 | */ 110 | } 111 | -------------------------------------------------------------------------------- /src/tests/test_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/imgproc.hpp" 2 | #include "opencv2/highgui.hpp" 3 | 4 | #include "boost/format.hpp" 5 | #include "boost/filesystem.hpp" 6 | 7 | #include "orcvio/obj/ObjectFeatureInitializer.h" 8 | #include "orcvio/obj/ObjectFeature.h" 9 | #include "orcvio/feat/FeatureInitializer.h" 10 | #include "orcvio/tests/test_utils.h" 11 | #include "orcvio/utils/se3_ops.hpp" 12 | 13 | using orcvio::dsread; 14 | using orcvio::ObjectFeature; 15 | using orcvio::FeatureInitializer; 16 | using boost::filesystem::exists; 17 | using boost::filesystem::path; 18 | namespace fs = boost::filesystem; 19 | 20 | 21 | namespace orcvio { 22 | 23 | constexpr bool DEBUG = false; 24 | 25 | void visualize_multiframe_test_data(const std::shared_ptr& obj_obs_ptr, 26 | const Eigen::MatrixX3d& kps_gt_3d_world, 27 | const std::unordered_map>& clones_cam, 28 | const Eigen::Matrix4d& wTq) 29 | { 30 | // intrinsic matrix 31 | Eigen::Matrix3d K; 32 | K << 200., 0, 320., 33 | 0, 200., 240., 34 | 0, 0, 1; 35 | cv::Scalar obs_color{0, 0, 255}; 36 | cv::Scalar gt_color{0, 255, 0}; 37 | 38 | for (size_t t = 0; t < obj_obs_ptr->timestamps[0].size(); ++t) { 39 | auto const& zs_per_frame = obj_obs_ptr->zs[t]; 40 | cv::Mat frame(K(1,2)*2, K(0,2)*2, CV_8UC3, cv::Scalar(0, 0, 0)); 41 | 42 | // Plot observed keypoints 43 | for (int r = 0; r < zs_per_frame.rows(); ++r) { 44 | if (zs_per_frame.allFinite()) { 45 | Eigen::Vector2i pt = (K.topLeftCorner<2, 2>() * zs_per_frame.row(r).transpose() + K.topRightCorner<2, 1>()).cast(); 46 | cv::drawMarker(frame, {pt(0), pt(1)}, obs_color); 47 | } 48 | } 49 | 50 | // Plot gt keypoints 51 | FeatureInitializer::ClonePose clonecam = clones_cam.at(0).at(t); 52 | for (int r = 0; r < kps_gt_3d_world.rows(); ++r) { 53 | Eigen::Vector3d kps_gt_3d_cam = clonecam.transformGlobalToCam(kps_gt_3d_world.row(r).transpose()); 54 | Eigen::Vector2d kps_gt_2d = kps_gt_3d_cam.head<2>() / kps_gt_3d_cam(2); 55 | Eigen::Vector2i pt = (K.topLeftCorner<2, 2>() * kps_gt_2d + K.topRightCorner<2, 1>()).cast(); 56 | cv::drawMarker(frame, {pt(0), pt(1)}, gt_color); 57 | } 58 | 59 | 60 | // Plot observed bounding boxes 61 | if (obj_obs_ptr->zb.size() > t) { 62 | auto const& zb_per_frame = obj_obs_ptr->zb[t]; 63 | Eigen::Vector2i topleft = (K.topLeftCorner<2, 2>() * zb_per_frame.head<2>() + K.topRightCorner<2, 1>()).cast(); 64 | Eigen::Vector2i bottomright = (K.topLeftCorner<2, 2>() * zb_per_frame.tail<2>() + K.topRightCorner<2, 1>()).cast(); 65 | cv::rectangle(frame, {topleft(0), topleft(1)}, {bottomright(0), bottomright(1)}, obs_color); 66 | } 67 | 68 | cv::imshow("c", frame); 69 | cv::waitKey(30); 70 | } 71 | } 72 | 73 | int load_multi_frame_test_data(const std::string& filename_fmt, 74 | std::unordered_map>& clones_cam, 75 | std::shared_ptr& obj_obs_ptr, 76 | Eigen::Matrix4d& wTq_gt, 77 | Eigen::Vector3d& object_mean_shape, 78 | Eigen::MatrixX3d& object_keypoints_mean) 79 | { 80 | boost::format filename_fmt_b(filename_fmt); 81 | size_t cam_id = 0; 82 | std::unordered_map clones_cami; 83 | Eigen::MatrixX3d kps_gt_3d; 84 | for (int i = 0; i < 100 && exists(path((filename_fmt_b % i).str())); i++) 85 | { 86 | 87 | // insert timestamps into object observations 88 | obj_obs_ptr->timestamps[cam_id].emplace_back(i); 89 | 90 | // construct filename 91 | std::string filename = (filename_fmt_b % i).str(); 92 | 93 | // load test data 94 | auto name2mat = cv::hdf::open(filename); 95 | 96 | // zs : groundtruth semantic measurements 97 | auto const zs_per_frame = dsread(name2mat, "zs"); 98 | obj_obs_ptr->zs.push_back(zs_per_frame); 99 | 100 | // zb : groundtruth bbox measurements in x,y,width, height format 101 | if ( name2mat->hlexists("zb") ) { 102 | Eigen::Vector4d xywh = dsread(name2mat, "zb").transpose(); 103 | Eigen::Vector4d xyminmax; 104 | xyminmax << xywh(0), xywh(1), xywh(0) + xywh(2), xywh(1) + xywh(3); 105 | obj_obs_ptr->zb.push_back(xyminmax); 106 | } 107 | 108 | // wTo : groundtruth optical frame to World transform (4 x 4) 109 | auto const wTo = dsread(name2mat, "wTo"); 110 | 111 | clones_cami.insert({i, FeatureInitializer::ClonePose(wTo)}); 112 | 113 | // wTq : groundtruth object to World transform (4 x 4) 114 | wTq_gt = dsread(name2mat, "wTq"); 115 | 116 | // 12 x 3 groundtruth keypoints coordinates 117 | kps_gt_3d = dsread(name2mat, "kps_gt_3d"); 118 | 119 | // 12 x 3 mean shape keypoints coordinates 120 | object_keypoints_mean = dsread(name2mat, "mean_shape"); 121 | 122 | // 3 x 1 ellipsoid shape 123 | object_mean_shape = dsread(name2mat, "ellipsoid_shape"); 124 | } 125 | 126 | clones_cam.insert({cam_id, clones_cami}); 127 | 128 | if (DEBUG) 129 | visualize_multiframe_test_data(obj_obs_ptr, kps_gt_3d, clones_cam, wTq_gt); 130 | 131 | return 1; 132 | } 133 | 134 | 135 | const std::string ensure_path_exists(const std::string& filepath) 136 | { 137 | if ( ! fs::exists(fs::path(filepath))) { 138 | throw std::runtime_error("filepath (" + filepath + ") not found in working directory: " + fs::current_path().string() + ". Please run from the top of the orcvio_cpp project directory."); 139 | } 140 | return filepath; 141 | } 142 | 143 | 144 | } // namespace orcvio 145 | --------------------------------------------------------------------------------