├── data └── resources │ └── readme.png ├── .gitignore ├── results ├── .gitignore ├── exp_track_stats │ └── plot.sh ├── exp_euroc_mav │ └── plot.sh ├── exp_rpng_plane │ └── plot.sh ├── sim_general │ └── plot.sh ├── run_ros_tracking.sh ├── run_simulation.sh ├── run_ros_eth.sh └── run_ros_rpng_plane.sh ├── run_format.sh ├── thirdparty ├── render │ ├── render_model.h │ ├── LICENSE │ └── render_model.cpp ├── cdt │ ├── remove_at.hpp │ ├── LocatorKDTree.h │ ├── CDT.hpp │ └── CDTUtils.hpp └── ikd │ └── ikd_tree.h ├── config ├── rpng_plane │ ├── kalibr_imu_chain.yaml │ ├── kalibr_imucam_chain.yaml │ └── estimator_config.yaml ├── sim │ ├── kalibr_imu_chain.yaml │ ├── kalibr_imucam_chain.yaml │ └── estimator_config.yaml └── euroc_mav │ ├── kalibr_imu_chain.yaml │ ├── kalibr_imucam_chain.yaml │ └── estimator_config.yaml ├── run_copyright.sh ├── ov_plane ├── src │ ├── update │ │ ├── UpdaterOptions.h │ │ ├── UpdaterMSCKF.h │ │ ├── UpdaterSLAM.h │ │ ├── UpdaterPlane.h │ │ ├── UpdaterZeroVelocity.h │ │ └── UpdaterHelper.h │ ├── ceres │ │ ├── Factor_PointOnPlane.h │ │ └── Factor_PointOnPlane.cpp │ ├── utils │ │ └── NoiseManager.h │ ├── state │ │ ├── State.cpp │ │ └── State.h │ ├── ros │ │ ├── ROSVisualizerHelper.h │ │ └── ROS1Visualizer.h │ ├── track_plane │ │ ├── PlaneFitting.h │ │ └── TrackPlaneOptions.h │ ├── sim │ │ ├── SimPlane.h │ │ └── Simulator.h │ ├── timing_custom.cpp │ └── run_simulation.cpp ├── package.xml ├── launch │ ├── serial.launch │ └── simulation.launch └── CMakeLists.txt ├── Dockerfile_ros1_18_04 ├── Dockerfile_ros1_20_04 ├── .github └── workflows │ └── build_ros1.yml ├── .clang-format └── ReadMe.md /data/resources/readme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpng/ov_plane/HEAD/data/resources/readme.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-* 2 | build 3 | Build 4 | *.*~ 5 | .idea 6 | doxgen_generated 7 | *.swp 8 | *.swo 9 | doc 10 | .vscode 11 | -------------------------------------------------------------------------------- /results/.gitignore: -------------------------------------------------------------------------------- 1 | exp_euroc_mav/ 2 | !exp_euroc_mav/plot.sh 3 | exp_rpng_plane/ 4 | !exp_rpng_plane/plot.sh 5 | exp_track_stats/ 6 | !exp_track_stats/plot.sh 7 | sim_general/ 8 | !sim_general/plot.sh 9 | -------------------------------------------------------------------------------- /run_format.sh: -------------------------------------------------------------------------------- 1 | 2 | 3 | # sudo apt install clang-format 4 | find ov_plane/ -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' -exec clang-format -style=file -i {} \; 5 | 6 | # sudo apt install libc++-dev clang-tidy 7 | #find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' -exec clang-tidy {} \; 8 | 9 | -------------------------------------------------------------------------------- /thirdparty/render/render_model.h: -------------------------------------------------------------------------------- 1 | #ifndef __MODEL_H__ 2 | #define __MODEL_H__ 3 | 4 | #include "Eigen/Eigen" 5 | #include 6 | 7 | class Model { 8 | private: 9 | std::vector verts_; 10 | std::vector> faces_; 11 | 12 | public: 13 | Model(const char *filename); 14 | ~Model(); 15 | int nverts(); 16 | int nfaces(); 17 | Eigen::Vector3d vert(int i); 18 | std::vector face(int idx); 19 | }; 20 | 21 | #endif //__MODEL_H__ -------------------------------------------------------------------------------- /config/rpng_plane/kalibr_imu_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | imu0: 4 | T_i_b: 5 | - [1.0, 0.0, 0.0, 0.0] 6 | - [0.0, 1.0, 0.0, 0.0] 7 | - [0.0, 0.0, 1.0, 0.0] 8 | - [0.0, 0.0, 0.0, 1.0] 9 | accelerometer_noise_density: 0.00207649074 10 | accelerometer_random_walk: 0.00041327852 11 | gyroscope_noise_density: 0.00020544166 12 | gyroscope_random_walk: 1.110622e-05 13 | model: calibrated 14 | rostopic: /d455/imu 15 | time_offset: 0.0 16 | update_rate: 400 17 | -------------------------------------------------------------------------------- /results/exp_track_stats/plot.sh: -------------------------------------------------------------------------------- 1 | 2 | # Source our workspace directory to load ENV variables 3 | source /home/patrick/workspace/catkin_ws_plane/devel/setup.bash 4 | 5 | 6 | # get directory 7 | BASEDIR="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" 8 | echo "$BASEDIR" 9 | 10 | 11 | for d in $BASEDIR/trackings/* ; do 12 | files=($d/$dataset/*) 13 | echo $d 14 | first_run=${files[0]} 15 | new_run="$export_folder/$d" 16 | cmd_files="$new_run $cmd_files" 17 | done 18 | 19 | rosrun ov_plane timing_custom $cmd_files > "$BASEDIR/output_tracking.txt" 20 | 21 | -------------------------------------------------------------------------------- /config/sim/kalibr_imu_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | imu0: 4 | T_i_b: 5 | - [1.0, 0.0, 0.0, 0.0] 6 | - [0.0, 1.0, 0.0, 0.0] 7 | - [0.0, 0.0, 1.0, 0.0] 8 | - [0.0, 0.0, 0.0, 1.0] 9 | accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) 10 | accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 11 | gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) 12 | gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 13 | model: calibrated 14 | rostopic: /imu0 15 | time_offset: 0.0 16 | update_rate: 200.0 -------------------------------------------------------------------------------- /config/euroc_mav/kalibr_imu_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | imu0: 4 | T_i_b: 5 | - [1.0, 0.0, 0.0, 0.0] 6 | - [0.0, 1.0, 0.0, 0.0] 7 | - [0.0, 0.0, 1.0, 0.0] 8 | - [0.0, 0.0, 0.0, 1.0] 9 | accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) 10 | accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 11 | gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) 12 | gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 13 | model: calibrated 14 | rostopic: /imu0 15 | time_offset: 0.0 16 | update_rate: 200.0 -------------------------------------------------------------------------------- /config/rpng_plane/kalibr_imucam_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | 4 | cam0: 5 | T_cam_imu: 6 | - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095] 7 | - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722] 8 | - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803] 9 | - [0.0, 0.0, 0.0, 1.0] 10 | cam_overlaps: [] 11 | camera_model: pinhole 12 | distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453] 13 | distortion_model: radtan 14 | intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077] 15 | resolution: [848, 480] 16 | rostopic: /d455/color/image_raw 17 | timeshift_cam_imu: 0.002524377913673846 18 | 19 | 20 | -------------------------------------------------------------------------------- /thirdparty/render/LICENSE: -------------------------------------------------------------------------------- 1 | Tiny Renderer, https://github.com/ssloy/tinyrenderer 2 | Copyright Dmitry V. Sokolov 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | 14 | -------------------------------------------------------------------------------- /results/exp_euroc_mav/plot.sh: -------------------------------------------------------------------------------- 1 | 2 | # Source our workspace directory to load ENV variables 3 | source /home/patrick/workspace/catkin_ws_plane/devel/setup.bash 4 | 5 | 6 | # get directory 7 | BASEDIR="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" 8 | echo "$BASEDIR" 9 | 10 | 11 | # do not display figures 12 | export MPLBACKEND=Agg 13 | 14 | rosrun ov_eval error_comparison posyaw "$BASEDIR/truths/" "$BASEDIR/algorithms/" > "$BASEDIR/output_traj.txt" 15 | #rosrun --prefix 'gdb -ex run --args' ov_eval error_comparison posyaw truths/ algorithms/ 16 | 17 | 18 | 19 | echo "\n\n=========================================" 20 | echo "timing_comparison V1_01_easy.txt" 21 | echo "=========================================" 22 | dataset="V1_01_easy" 23 | cmd_files="" 24 | export_folder="$BASEDIR/timings_edited/" 25 | mkdir -p $export_folder 26 | for d in $BASEDIR/timings/* ; do 27 | files=("$d/$dataset/"*) 28 | echo $d 29 | first_run=${files[0]} 30 | filename=$(basename -- "$d") 31 | filename="$(echo "$filename" | sed "s/ //g")" # no space support... 32 | new_run="$export_folder/$filename.txt" 33 | cp -- "$first_run" "$new_run" 34 | cmd_files="$new_run $cmd_files" 35 | done 36 | rosrun ov_eval timing_comparison $cmd_files > "$BASEDIR/output_timing.txt" 37 | rm -rf $export_folder 38 | 39 | 40 | -------------------------------------------------------------------------------- /results/exp_rpng_plane/plot.sh: -------------------------------------------------------------------------------- 1 | 2 | # Source our workspace directory to load ENV variables 3 | source /home/patrick/workspace/catkin_ws_plane/devel/setup.bash 4 | 5 | 6 | # get directory 7 | BASEDIR="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" 8 | echo "$BASEDIR" 9 | 10 | 11 | # do not display figures 12 | export MPLBACKEND=Agg 13 | 14 | rosrun ov_eval error_comparison posyaw "$BASEDIR/truths/" "$BASEDIR/algorithms/" > "$BASEDIR/output_traj.txt" 15 | #rosrun --prefix 'gdb -ex run --args' ov_eval error_comparison posyaw truths/ algorithms/ 16 | 17 | 18 | 19 | echo "\n\n=========================================" 20 | echo "timing_comparison table_01.txt" 21 | echo "=========================================" 22 | dataset="table_01" 23 | cmd_files="" 24 | export_folder="$BASEDIR/timings_edited/" 25 | mkdir -p $export_folder 26 | for d in $BASEDIR/timings/* ; do 27 | files=("$d/$dataset/"*) 28 | echo $d 29 | first_run=${files[0]} 30 | filename=$(basename -- "$d") 31 | filename="$(echo "$filename" | sed "s/ //g")" # no space support... 32 | new_run="$export_folder/$filename.txt" 33 | cp -- "$first_run" "$new_run" 34 | cmd_files="$new_run $cmd_files" 35 | done 36 | rosrun ov_eval timing_comparison $cmd_files > "$BASEDIR/output_timing.txt" 37 | rm -rf $export_folder 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /results/sim_general/plot.sh: -------------------------------------------------------------------------------- 1 | 2 | # Source our workspace directory to load ENV variables 3 | source /home/patrick/workspace/catkin_ws_plane/devel/setup.bash 4 | 5 | 6 | # get directory 7 | BASEDIR="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" 8 | echo "$BASEDIR" 9 | 10 | 11 | # do not display figures 12 | export MPLBACKEND=Agg 13 | 14 | rosrun ov_eval error_dataset none "$BASEDIR/truths/udel_arl_short.txt" "$BASEDIR/algorithms/" > "$BASEDIR/output_traj.txt" 15 | #rosrun --prefix 'gdb -ex run --args' ov_eval error_comparison posyaw truths/ algorithms/ 16 | 17 | 18 | echo "\n\n=========================================" 19 | echo "timing_comparison udel_arl_short.txt" 20 | echo "=========================================" 21 | dataset="udel_arl_short" 22 | cmd_files="" 23 | export_folder="$BASEDIR/timings_edited/" 24 | mkdir -p $export_folder 25 | for d in $BASEDIR/timings/* ; do 26 | files=("$d/$dataset/"*) 27 | echo $d 28 | first_run=${files[0]} 29 | filename=$(basename -- "$d") 30 | filename="$(echo "$filename" | sed "s/ //g")" # no space support... 31 | new_run="$export_folder/$filename.txt" 32 | cp -- "$first_run" "$new_run" 33 | cmd_files="$new_run $cmd_files" 34 | done 35 | rosrun ov_eval timing_comparison $cmd_files > "$BASEDIR/output_timing.txt" 36 | rm -rf $export_folder 37 | 38 | 39 | -------------------------------------------------------------------------------- /config/sim/kalibr_imucam_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | cam0: 4 | T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI 5 | - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] 6 | - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] 7 | - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] 8 | - [0.0, 0.0, 0.0, 1.0] 9 | cam_overlaps: [1] 10 | camera_model: pinhole 11 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 12 | distortion_model: radtan 13 | intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv 14 | resolution: [752, 480] 15 | rostopic: /cam0/image_raw 16 | cam1: 17 | T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI 18 | - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] 19 | - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] 20 | - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | cam_overlaps: [0] 23 | camera_model: pinhole 24 | distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] 25 | distortion_model: radtan 26 | intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv 27 | resolution: [752, 480] 28 | rostopic: /cam1/image_raw -------------------------------------------------------------------------------- /config/euroc_mav/kalibr_imucam_chain.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | cam0: 4 | T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI 5 | - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] 6 | - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] 7 | - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] 8 | - [0.0, 0.0, 0.0, 1.0] 9 | cam_overlaps: [1] 10 | camera_model: pinhole 11 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 12 | distortion_model: radtan 13 | intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv 14 | resolution: [752, 480] 15 | rostopic: /cam0/image_raw 16 | cam1: 17 | T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI 18 | - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] 19 | - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] 20 | - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | cam_overlaps: [0] 23 | camera_model: pinhole 24 | distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] 25 | distortion_model: radtan 26 | intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv 27 | resolution: [752, 480] 28 | rostopic: /cam1/image_raw -------------------------------------------------------------------------------- /thirdparty/render/render_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "render_model.h" 8 | 9 | Model::Model(const char *filename) : verts_(), faces_() { 10 | std::ifstream in; 11 | in.open(filename, std::ifstream::in); 12 | if (in.fail()) 13 | return; 14 | std::string line; 15 | double scale = 0.10; 16 | while (!in.eof()) { 17 | std::getline(in, line); 18 | std::istringstream iss(line.c_str()); 19 | char trash; 20 | if (!line.compare(0, 2, "v ")) { 21 | iss >> trash; 22 | Eigen::Vector3d v; 23 | for (int i = 0; i < 3; i++) { 24 | float ft; 25 | iss >> ft; 26 | v(i) = scale * (double)ft; 27 | } 28 | verts_.push_back(v); 29 | } else if (!line.compare(0, 2, "f ")) { 30 | std::vector f; 31 | // int itrash, idx; 32 | int idx; 33 | iss >> trash; 34 | // while (iss >> idx >> trash >> itrash >> trash >> itrash) { 35 | while (iss >> idx) { 36 | idx--; // in wavefront obj all indices start at 1, not zero 37 | f.push_back(idx); 38 | } 39 | faces_.push_back(f); 40 | } 41 | } 42 | std::cerr << "# v# " << verts_.size() << " f# " << faces_.size() << std::endl; 43 | } 44 | 45 | Model::~Model() {} 46 | 47 | int Model::nverts() { return (int)verts_.size(); } 48 | 49 | int Model::nfaces() { return (int)faces_.size(); } 50 | 51 | std::vector Model::face(int idx) { return faces_[idx]; } 52 | 53 | Eigen::Vector3d Model::vert(int i) { return verts_[i]; } 54 | -------------------------------------------------------------------------------- /run_copyright.sh: -------------------------------------------------------------------------------- 1 | 2 | 3 | # 1. first find all source files 4 | # 2. remove copywrite 5 | find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' \ 6 | -not -path "*/thirdparty/*" -not -path "*/cmake-build-debug/*" -not -path "*/.idea/*" \ 7 | -exec sed -i '1,/\*\//{ /\/\*/,/\*\//d }' {} \; 8 | 9 | # 3. Create copytext file 10 | COPYHEADER="/* 11 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 12 | * Copyright (C) 2022-2023 Chuchu Chen 13 | * Copyright (C) 2022-2023 Patrick Geneva 14 | * Copyright (C) 2022-2023 Guoquan Huang 15 | * 16 | * OpenVINS: An Open Platform for Visual-Inertial Research 17 | * Copyright (C) 2018-2023 Patrick Geneva 18 | * Copyright (C) 2018-2023 Guoquan Huang 19 | * Copyright (C) 2018-2023 OpenVINS Contributors 20 | * Copyright (C) 2018-2019 Kevin Eckenhoff 21 | * 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU General Public License as published by 24 | * the Free Software Foundation, either version 3 of the License, or 25 | * (at your option) any later version. 26 | * 27 | * This program is distributed in the hope that it will be useful, 28 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 29 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 30 | * GNU General Public License for more details. 31 | * 32 | * You should have received a copy of the GNU General Public License 33 | * along with this program. If not, see . 34 | */ 35 | 36 | " 37 | echo "$COPYHEADER" > /tmp/out_copy 38 | 39 | 40 | # 4. now append the new header to the files! 41 | find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' \ 42 | -not -path "*/thirdparty/*" -not -path "*/cmake-build-debug/*" -not -path "*/.idea/*" \ 43 | -print0 | 44 | while IFS= read -r -d '' file; do 45 | echo $file; 46 | cat /tmp/out_copy $file > /tmp/out && mv /tmp/out $file 47 | done 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /thirdparty/cdt/remove_at.hpp: -------------------------------------------------------------------------------- 1 | #ifndef REMOVE_AT_HPP 2 | #define REMOVE_AT_HPP 3 | 4 | // check if c++11 is supported 5 | #if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) 6 | #define REMOVE_AT_CXX11_IS_SUPPORTED 7 | #elif !defined(__cplusplus) && !defined(_MSC_VER) 8 | typedef char couldnt_parse_cxx_standard[-1]; 9 | #endif 10 | 11 | #include 12 | #include 13 | 14 | template 15 | inline ForwardIt remove_at( 16 | ForwardIt first, 17 | ForwardIt last, 18 | SortUniqIndsFwdIt ii_first, 19 | SortUniqIndsFwdIt ii_last) 20 | { 21 | if(ii_first == ii_last) // no indices-to-remove are given 22 | return last; 23 | typedef typename std::iterator_traits::difference_type diff_t; 24 | typedef typename std::iterator_traits::value_type ind_t; 25 | ForwardIt destination = first + static_cast(*ii_first); 26 | while(ii_first != ii_last) 27 | { 28 | // advance to an index after a chunk of elements-to-keep 29 | for(ind_t cur = *ii_first++; ii_first != ii_last; ++ii_first) 30 | { 31 | const ind_t nxt = *ii_first; 32 | if(nxt - cur > 1) 33 | break; 34 | cur = nxt; 35 | } 36 | // move the chunk of elements-to-keep to new destination 37 | const ForwardIt source_first = 38 | first + static_cast(*(ii_first - 1)) + 1; 39 | const ForwardIt source_last = 40 | ii_first != ii_last ? first + static_cast(*ii_first) : last; 41 | #ifdef REMOVE_AT_CXX11_IS_SUPPORTED 42 | std::move(source_first, source_last, destination); 43 | #else 44 | std::copy(source_first, source_last, destination); // c++98 version 45 | #endif 46 | destination += source_last - source_first; 47 | } 48 | return destination; 49 | } 50 | 51 | #endif // REMOVE_AT_HPP 52 | -------------------------------------------------------------------------------- /thirdparty/cdt/LocatorKDTree.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * Adapter between for KDTree and CDT 4 | */ 5 | 6 | #ifndef CDT_POINTKDTREE_H 7 | #define CDT_POINTKDTREE_H 8 | 9 | #include "CDTUtils.h" 10 | #include "KDTree.h" 11 | 12 | namespace CDT 13 | { 14 | 15 | /// KD-tree holding points 16 | template < 17 | typename TCoordType, 18 | size_t NumVerticesInLeaf = 32, 19 | size_t InitialStackDepth = 32, 20 | size_t StackDepthIncrement = 32> 21 | class LocatorKDTree 22 | { 23 | public: 24 | /// Initialize KD-tree with points 25 | void initialize(const std::vector >& points) 26 | { 27 | typedef V2d V2d_t; 28 | V2d_t min = points.front(); 29 | V2d_t max = min; 30 | typedef typename std::vector::const_iterator Cit; 31 | for(Cit it = points.begin(); it != points.end(); ++it) 32 | { 33 | min = V2d_t::make(std::min(min.x, it->x), std::min(min.y, it->y)); 34 | max = V2d_t::make(std::max(max.x, it->x), std::max(max.y, it->y)); 35 | } 36 | m_kdTree = KDTree_t(min, max); 37 | for(VertInd i = 0; i < points.size(); ++i) 38 | { 39 | m_kdTree.insert(i, points); 40 | } 41 | } 42 | /// Add point to KD-tree 43 | void addPoint(const VertInd i, const std::vector >& points) 44 | { 45 | m_kdTree.insert(i, points); 46 | } 47 | /// Find nearest point using R-tree 48 | VertInd nearPoint( 49 | const V2d& pos, 50 | const std::vector >& points) const 51 | { 52 | return m_kdTree.nearest(pos, points).second; 53 | } 54 | 55 | private: 56 | typedef KDTree::KDTree< 57 | TCoordType, 58 | NumVerticesInLeaf, 59 | InitialStackDepth, 60 | StackDepthIncrement> 61 | KDTree_t; 62 | KDTree_t m_kdTree; 63 | }; 64 | 65 | } // namespace CDT 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterOptions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_OPTIONS_H 28 | #define OV_PLANE_UPDATER_OPTIONS_H 29 | 30 | #include "utils/print.h" 31 | 32 | namespace ov_plane { 33 | 34 | /** 35 | * @brief Struct which stores general updater options 36 | */ 37 | struct UpdaterOptions { 38 | 39 | /// What chi-squared multipler we should apply 40 | double chi2_multipler = 5; 41 | 42 | /// Noise sigma for our raw pixel measurements 43 | double sigma_pix = 1; 44 | 45 | /// Covariance for our raw pixel measurements 46 | double sigma_pix_sq = 1; 47 | 48 | /// Nice print function of what parameters we have loaded 49 | void print() { 50 | PRINT_DEBUG(" - chi2_multipler: %.1f\n", chi2_multipler); 51 | PRINT_DEBUG(" - sigma_pix: %.2f\n", sigma_pix); 52 | } 53 | }; 54 | 55 | } // namespace ov_plane 56 | 57 | #endif // OV_PLANE_UPDATER_OPTIONS_H -------------------------------------------------------------------------------- /Dockerfile_ros1_18_04: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:melodic-desktop-full 2 | 3 | # ========================================================= 4 | # ========================================================= 5 | 6 | # Are you are looking for how to use this docker file? 7 | # - https://docs.openvins.com/dev-docker.html 8 | # - https://docs.docker.com/get-started/ 9 | # - http://wiki.ros.org/docker/Tutorials/Docker 10 | 11 | # ========================================================= 12 | # ========================================================= 13 | 14 | # Dependencies we use, catkin tools is very good build system 15 | # Also some helper utilities for fast in terminal edits (nano etc) 16 | RUN apt-get update && apt-get install -y libeigen3-dev nano git 17 | RUN sudo apt-get install -y python-catkin-tools 18 | 19 | # Ceres solver install and setup 20 | RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev 21 | # ENV CERES_VERSION="2.0.0" 22 | # RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \ 23 | # cd ceres-solver && \ 24 | # git checkout tags/${CERES_VERSION} && \ 25 | # mkdir build && cd build && \ 26 | # cmake .. && \ 27 | # make -j$(nproc) install && \ 28 | # rm -rf ../../ceres-solver 29 | 30 | # Seems this has Python 3.6 installed on it... 31 | RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk 32 | 33 | # Install deps needed for clion remote debugging 34 | # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ 35 | # RUN sed -i '6i\source "/catkin_ws/devel/setup.bash"\' /ros_entrypoint.sh 36 | RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ 37 | gdb clang cmake rsync tar python && apt-get clean 38 | RUN ( \ 39 | echo 'LogLevel DEBUG2'; \ 40 | echo 'PermitRootLogin yes'; \ 41 | echo 'PasswordAuthentication yes'; \ 42 | echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ 43 | ) > /etc/ssh/sshd_config_test_clion \ 44 | && mkdir /run/sshd 45 | RUN useradd -m user && yes password | passwd user 46 | RUN usermod -s /bin/bash user 47 | CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] 48 | 49 | -------------------------------------------------------------------------------- /Dockerfile_ros1_20_04: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:noetic-desktop-full 2 | 3 | # ========================================================= 4 | # ========================================================= 5 | 6 | # Are you are looking for how to use this docker file? 7 | # - https://docs.openvins.com/dev-docker.html 8 | # - https://docs.docker.com/get-started/ 9 | # - http://wiki.ros.org/docker/Tutorials/Docker 10 | 11 | # ========================================================= 12 | # ========================================================= 13 | 14 | # Dependencies we use, catkin tools is very good build system 15 | # Also some helper utilities for fast in terminal edits (nano etc) 16 | RUN apt-get update && apt-get install -y libeigen3-dev nano git 17 | RUN sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon 18 | 19 | # Ceres solver install and setup 20 | RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev 21 | # ENV CERES_VERSION="2.0.0" 22 | # RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \ 23 | # cd ceres-solver && \ 24 | # git checkout tags/${CERES_VERSION} && \ 25 | # mkdir build && cd build && \ 26 | # cmake .. && \ 27 | # make -j$(nproc) install && \ 28 | # rm -rf ../../ceres-solver 29 | 30 | # Seems this has Python 3.8 installed on it... 31 | RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk 32 | 33 | # Install deps needed for clion remote debugging 34 | # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ 35 | # RUN sed -i '6i\source "/catkin_ws/devel/setup.bash"\' /ros_entrypoint.sh 36 | RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ 37 | gdb clang cmake rsync tar python && apt-get clean 38 | RUN ( \ 39 | echo 'LogLevel DEBUG2'; \ 40 | echo 'PermitRootLogin yes'; \ 41 | echo 'PasswordAuthentication yes'; \ 42 | echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ 43 | ) > /etc/ssh/sshd_config_test_clion \ 44 | && mkdir /run/sshd 45 | RUN useradd -m user && yes password | passwd user 46 | RUN usermod -s /bin/bash user 47 | CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] 48 | 49 | -------------------------------------------------------------------------------- /ov_plane/src/ceres/Factor_PointOnPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_CERES_POINTONPLANE_H 28 | #define OV_PLANE_CERES_POINTONPLANE_H 29 | 30 | #include 31 | #include 32 | 33 | namespace ov_plane { 34 | 35 | /** 36 | * @brief Point-on-plane ceres factor. 37 | * 38 | * This is a function of the feature and CP plane object. 39 | * The residual is the distance of the point to the plane in the normal direction. 40 | */ 41 | class Factor_PointOnPlane : public ceres::CostFunction { 42 | public: 43 | // Measurement constraint noise 44 | double sigma_c = 1.0; 45 | 46 | /** 47 | * @brief Default constructor 48 | * @param sigma_c_ Constraint uncertainty (should be small non-zero) 49 | */ 50 | Factor_PointOnPlane(double sigma_c_); 51 | 52 | virtual ~Factor_PointOnPlane() {} 53 | 54 | /** 55 | * @brief Error residual and Jacobian calculation 56 | */ 57 | bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override; 58 | }; 59 | 60 | } // namespace ov_plane 61 | 62 | #endif // OV_PLANE_CERES_POINTONPLANE_H -------------------------------------------------------------------------------- /.github/workflows/build_ros1.yml: -------------------------------------------------------------------------------- 1 | name: ROS 1 Workflow 2 | 3 | on: 4 | push: 5 | branches: [ main ] 6 | pull_request: 7 | 8 | jobs: 9 | build_1804: 10 | name: "ROS1 Ubuntu 18.04" 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Code Checkout 14 | uses: actions/checkout@v2 15 | - name: Create Workspace and Docker Image 16 | run: | 17 | export REPO=$(basename $GITHUB_REPOSITORY) && 18 | cd $GITHUB_WORKSPACE/.. && mkdir src/ && 19 | cd src/ && git clone https://github.com/rpng/open_vins && cd .. && 20 | mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && 21 | docker build -t ov_plane -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . 22 | - name: Run Build in Docker 23 | run: | 24 | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws ov_plane /bin/bash -c "cd /catkin_ws && catkin build ov_plane ov_data" 25 | - name: Run Simulation! 26 | run: | 27 | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws ov_plane /bin/bash -c "cd /catkin_ws && source devel/setup.bash && roslaunch ov_plane simulation.launch verbosity:=WARNING" 28 | build_2004: 29 | name: "ROS1 Ubuntu 20.04" 30 | runs-on: ubuntu-latest 31 | steps: 32 | - name: Code Checkout 33 | uses: actions/checkout@v2 34 | - name: Create Workspace and Docker Image 35 | run: | 36 | export REPO=$(basename $GITHUB_REPOSITORY) && 37 | cd $GITHUB_WORKSPACE/.. && mkdir src/ && 38 | cd src/ && git clone https://github.com/rpng/open_vins && cd .. && 39 | mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && 40 | docker build -t ov_plane -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . 41 | - name: Run Build in Docker 42 | run: | 43 | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws ov_plane /bin/bash -c "cd /catkin_ws && catkin build ov_plane ov_data" 44 | - name: Run Simulation! 45 | run: | 46 | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws ov_plane /bin/bash -c "cd /catkin_ws && source devel/setup.bash && roslaunch ov_plane simulation.launch verbosity:=WARNING" 47 | -------------------------------------------------------------------------------- /ov_plane/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | ov_plane 6 | 1.0.0 7 | 8 | Monocular Visual-Inertial Odometry with Planar Regularities 9 | 10 | https://github.com/rpng/ov_plane/issues 11 | https://github.com/rpng/ov_plane 12 | 13 | 14 | Chuchu Chen 15 | Patrick Geneva 16 | Yuxiang Peng 17 | Woosik Lee 18 | Guoquan Huang 19 | Chuchu Chen 20 | Patrick Geneva 21 | 22 | 23 | GNU General Public License v3.0 24 | 25 | 26 | catkin 27 | cmake_modules 28 | roscpp 29 | rosbag 30 | tf 31 | std_msgs 32 | sensor_msgs 33 | geometry_msgs 34 | nav_msgs 35 | visualization_msgs 36 | image_transport 37 | cv_bridge 38 | ov_core 39 | ov_init 40 | ov_eval 41 | 42 | 43 | eigen 44 | libopencv-dev 45 | libopencv-contrib-dev 46 | boost 47 | libceres-dev 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /ov_plane/src/utils/NoiseManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_NOISEMANAGER_H 28 | #define OV_PLANE_NOISEMANAGER_H 29 | 30 | #include 31 | 32 | #include "utils/print.h" 33 | 34 | namespace ov_plane { 35 | 36 | /** 37 | * @brief Struct of our imu noise parameters 38 | */ 39 | struct NoiseManager { 40 | 41 | /// Gyroscope white noise (rad/s/sqrt(hz)) 42 | double sigma_w = 1.6968e-04; 43 | 44 | /// Gyroscope white noise covariance 45 | double sigma_w_2 = pow(1.6968e-04, 2); 46 | 47 | /// Gyroscope random walk (rad/s^2/sqrt(hz)) 48 | double sigma_wb = 1.9393e-05; 49 | 50 | /// Gyroscope random walk covariance 51 | double sigma_wb_2 = pow(1.9393e-05, 2); 52 | 53 | /// Accelerometer white noise (m/s^2/sqrt(hz)) 54 | double sigma_a = 2.0000e-3; 55 | 56 | /// Accelerometer white noise covariance 57 | double sigma_a_2 = pow(2.0000e-3, 2); 58 | 59 | /// Accelerometer random walk (m/s^3/sqrt(hz)) 60 | double sigma_ab = 3.0000e-03; 61 | 62 | /// Accelerometer random walk covariance 63 | double sigma_ab_2 = pow(3.0000e-03, 2); 64 | 65 | /// Nice print function of what parameters we have loaded 66 | void print() { 67 | PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); 68 | PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); 69 | PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); 70 | PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); 71 | } 72 | }; 73 | 74 | } // namespace ov_plane 75 | 76 | #endif // OV_PLANE_NOISEMANAGER_H -------------------------------------------------------------------------------- /ov_plane/src/ceres/Factor_PointOnPlane.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #include "Factor_PointOnPlane.h" 28 | 29 | using namespace ov_plane; 30 | 31 | Factor_PointOnPlane::Factor_PointOnPlane(double sigma_c_) : sigma_c(sigma_c_) { 32 | 33 | // Parameters we are a function of 34 | set_num_residuals(1); 35 | mutable_parameter_block_sizes()->push_back(3); // p_FinG 36 | mutable_parameter_block_sizes()->push_back(3); // cp_inG 37 | } 38 | 39 | bool Factor_PointOnPlane::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 40 | 41 | // Recover the current state from our parameters 42 | Eigen::Vector3d p_FinG = Eigen::Map(parameters[0]); 43 | Eigen::Vector3d cp_inG = Eigen::Map(parameters[1]); 44 | 45 | // Recover cp info 46 | double d_inG = cp_inG.norm(); 47 | Eigen::Vector3d n_inG = cp_inG / d_inG; 48 | 49 | // Compute residual 50 | // NOTE: we make this negative ceres cost function is only min||f(x)||^2 51 | // TODO: is this right? it optimizes if we flip this sign... 52 | double white_c = 1.0 / sigma_c; 53 | residuals[0] = -1.0 * white_c * (0.0 - (n_inG.transpose() * p_FinG - d_inG)); 54 | 55 | // Compute jacobians if requested by ceres 56 | if (jacobians) { 57 | 58 | // Jacobian wrt feature p_FinG 59 | if (jacobians[0]) { 60 | Eigen::Map> jacobian(jacobians[0]); 61 | jacobian.block(0, 0, 1, 3) = white_c * n_inG.transpose(); 62 | } 63 | 64 | // Jacobian wrt cp_inG 65 | if (jacobians[1]) { 66 | Eigen::Map> jacobian(jacobians[1]); 67 | jacobian.block(0, 0, 1, 3) = 68 | white_c * 1.0 / d_inG * (p_FinG.transpose() - n_inG.transpose() * p_FinG * n_inG.transpose() - d_inG * n_inG.transpose()); 69 | } 70 | } 71 | return true; 72 | } -------------------------------------------------------------------------------- /thirdparty/cdt/CDT.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * Public API - implementation 4 | */ 5 | 6 | #include "CDT.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace CDT 14 | { 15 | 16 | CDT_INLINE_IF_HEADER_ONLY VerticesTriangles calculateTrianglesByVertex( 17 | const TriangleVec& triangles, 18 | const VertInd verticesSize) 19 | { 20 | VerticesTriangles vertTris(verticesSize); 21 | for(TriInd iT = 0; iT < triangles.size(); ++iT) 22 | { 23 | const VerticesArr3& vv = triangles[iT].vertices; 24 | for(VerticesArr3::const_iterator v = vv.begin(); v != vv.end(); ++v) 25 | { 26 | vertTris[*v].push_back(iT); 27 | } 28 | } 29 | return vertTris; 30 | } 31 | 32 | template 33 | DuplicatesInfo RemoveDuplicates(std::vector >& vertices) 34 | { 35 | const DuplicatesInfo di = FindDuplicates( 36 | vertices.begin(), vertices.end(), getX_V2d, getY_V2d); 37 | RemoveDuplicates(vertices, di.duplicates); 38 | return di; 39 | } 40 | 41 | CDT_INLINE_IF_HEADER_ONLY void 42 | RemapEdges(std::vector& edges, const std::vector& mapping) 43 | { 44 | RemapEdges( 45 | edges.begin(), 46 | edges.end(), 47 | mapping, 48 | edge_get_v1, 49 | edge_get_v2, 50 | edge_make); 51 | } 52 | 53 | template 54 | DuplicatesInfo RemoveDuplicatesAndRemapEdges( 55 | std::vector >& vertices, 56 | std::vector& edges) 57 | { 58 | return RemoveDuplicatesAndRemapEdges( 59 | vertices, 60 | getX_V2d, 61 | getY_V2d, 62 | edges.begin(), 63 | edges.end(), 64 | edge_get_v1, 65 | edge_get_v2, 66 | edge_make); 67 | } 68 | 69 | CDT_INLINE_IF_HEADER_ONLY EdgeUSet 70 | extractEdgesFromTriangles(const TriangleVec& triangles) 71 | { 72 | EdgeUSet edges; 73 | typedef TriangleVec::const_iterator CIt; 74 | for(CIt t = triangles.begin(); t != triangles.end(); ++t) 75 | { 76 | edges.insert(Edge(VertInd(t->vertices[0]), VertInd(t->vertices[1]))); 77 | edges.insert(Edge(VertInd(t->vertices[1]), VertInd(t->vertices[2]))); 78 | edges.insert(Edge(VertInd(t->vertices[2]), VertInd(t->vertices[0]))); 79 | } 80 | return edges; 81 | } 82 | 83 | CDT_INLINE_IF_HEADER_ONLY unordered_map 84 | EdgeToPiecesMapping(const unordered_map& pieceToOriginals) 85 | { 86 | unordered_map originalToPieces; 87 | typedef unordered_map::const_iterator Cit; 88 | for(Cit ptoIt = pieceToOriginals.begin(); ptoIt != pieceToOriginals.end(); 89 | ++ptoIt) 90 | { 91 | const Edge piece = ptoIt->first; 92 | const EdgeVec& originals = ptoIt->second; 93 | for(EdgeVec::const_iterator origIt = originals.begin(); 94 | origIt != originals.end(); 95 | ++origIt) 96 | { 97 | originalToPieces[*origIt].push_back(piece); 98 | } 99 | } 100 | return originalToPieces; 101 | } 102 | 103 | } // namespace CDT 104 | -------------------------------------------------------------------------------- /results/run_ros_tracking.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Source our workspace directory to load ENV variables 4 | SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 5 | source ${SCRIPT_DIR}/../../../devel/setup.bash 6 | 7 | #============================================================= 8 | #============================================================= 9 | #============================================================= 10 | 11 | config=( 12 | "euroc_mav" 13 | "euroc_mav" 14 | "euroc_mav" 15 | "euroc_mav" 16 | "euroc_mav" 17 | "euroc_mav" 18 | "rpng_plane" 19 | "rpng_plane" 20 | "rpng_plane" 21 | "rpng_plane" 22 | "rpng_plane" 23 | "rpng_plane" 24 | "rpng_plane" 25 | "rpng_plane" 26 | ) 27 | 28 | 29 | # dataset locations 30 | bagnames=( 31 | "V1_01_easy" 32 | "V1_02_medium" 33 | "V1_03_difficult" 34 | "V2_01_easy" 35 | "V2_02_medium" 36 | "V2_03_difficult" 37 | "table_01" 38 | "table_02" 39 | "table_03" 40 | "table_04" 41 | "table_05" 42 | "table_06" 43 | "table_07" 44 | "table_08" 45 | ) 46 | 47 | # how far we should start into the dataset 48 | # this can be used to skip the initial sections 49 | bagstarttimes=( 50 | "0" 51 | "0" 52 | "0" 53 | "0" 54 | "0" 55 | "0" 56 | "0" 57 | "0" 58 | "0" 59 | "0" 60 | "0" 61 | "0" 62 | "0" 63 | "0" 64 | ) 65 | 66 | do_slam_plane="true" 67 | sigma_c="0.010" 68 | max_slam="30" 69 | 70 | 71 | 72 | # location to save log files into 73 | bag_path="/home/patrick/datasets/" 74 | base_path="/home/patrick/workspace/catkin_ws_plane/src/ov_plane/results/" 75 | save_path3="$base_path/exp_track_stats/trackings" 76 | 77 | 78 | #============================================================= 79 | #============================================================= 80 | #============================================================= 81 | 82 | # Loop through all datasets 83 | for i in "${!bagnames[@]}"; do 84 | 85 | 86 | # start timing 87 | start_time="$(date -u +%s)" 88 | filename_track="$save_path3/${config[i]}_${bagnames[i]}.txt" 89 | 90 | 91 | # run our ROS launch file (note we send console output to terminator) 92 | roslaunch ov_plane serial.launch \ 93 | verbosity:="WARNING" \ 94 | max_cameras:="1" \ 95 | use_stereo:="false" \ 96 | config:="${config[i]}" \ 97 | object:="" \ 98 | dataset:="${bagnames[i]}" \ 99 | bag:="$bag_path/${config[i]}/${bagnames[i]}.bag" \ 100 | bag_start:="${bagstarttimes[i]}" \ 101 | use_plane_constraint:="true" \ 102 | use_plane_constraint_msckf:="true" \ 103 | use_plane_constraint_slamu:="true" \ 104 | use_plane_constraint_slamd:="true" \ 105 | use_plane_slam_feats:="$do_slam_plane" \ 106 | use_refine_plane_feat:="true" \ 107 | sigma_constraint:="$sigma_c" \ 108 | const_init_multi:="1.00" \ 109 | const_init_chi2:="1.00" \ 110 | num_pts:="200" \ 111 | num_slam:="${max_slam[c]}" \ 112 | dobag:="true" \ 113 | bag_rate:="2" \ 114 | dosave:="false" \ 115 | dotime:="false" \ 116 | dotrackinfo:="true" \ 117 | path_track:="$filename_track" \ 118 | dolivetraj:="true" &> /dev/null 119 | 120 | 121 | 122 | # print out the time elapsed 123 | end_time="$(date -u +%s)" 124 | elapsed="$(($end_time-$start_time))" 125 | echo "BASH: ${config[i]}_${bagnames[i]} - took $elapsed seconds"; 126 | 127 | done 128 | 129 | 130 | -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterMSCKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_MSCKF_H 28 | #define OV_PLANE_UPDATER_MSCKF_H 29 | 30 | #include 31 | #include 32 | 33 | #include "feat/FeatureInitializerOptions.h" 34 | 35 | #include "UpdaterOptions.h" 36 | 37 | namespace ov_core { 38 | class Feature; 39 | class FeatureInitializer; 40 | } // namespace ov_core 41 | 42 | namespace ov_plane { 43 | 44 | class State; 45 | 46 | /** 47 | * @brief Will compute the system for our sparse features and update the filter. 48 | * 49 | * This class is responsible for computing the entire linear system for all features that are going to be used in an update. 50 | * This follows the original MSCKF, where we first triangulate features, we then nullspace project the feature Jacobian. 51 | * After this we compress all the measurements to have an efficient update and update the state. 52 | */ 53 | class UpdaterMSCKF { 54 | 55 | public: 56 | /** 57 | * @brief Default constructor for our MSCKF updater 58 | * 59 | * Our updater has a feature initializer which we use to initialize features as needed. 60 | * Also the options allow for one to tune the different parameters for update. 61 | * 62 | * @param options Updater options (include measurement noise value) 63 | * @param feat_init_options Feature initializer options 64 | */ 65 | UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options); 66 | 67 | /** 68 | * @brief Given tracked features, this will try to use them to update the state. 69 | * 70 | * @param state State of the filter 71 | * @param feature_vec Features that can be used for update 72 | * @param feature_vec_extra Features that lie on planes that we can also *try* to use 73 | * @param feature_vec_used Features that were used to initialize a new plane! 74 | * @param feat2plane Feature to planeid mapping! 75 | */ 76 | void update(std::shared_ptr state, std::vector> &feature_vec, 77 | std::vector> &feature_vec_extra, 78 | std::vector> &feature_vec_used, const std::map &feat2plane); 79 | 80 | protected: 81 | /// Options used during update 82 | UpdaterOptions _options; 83 | 84 | /// Feature initializer class object 85 | std::shared_ptr initializer_feat; 86 | 87 | /// Chi squared 95th percentile table (lookup would be size of residual) 88 | std::map chi_squared_table; 89 | }; 90 | 91 | } // namespace ov_plane 92 | 93 | #endif // OV_PLANE_UPDATER_MSCKF_H 94 | -------------------------------------------------------------------------------- /results/run_simulation.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Source our workspace directory to load ENV variables 4 | SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 5 | source ${SCRIPT_DIR}/../../../devel/setup.bash 6 | 7 | #============================================================= 8 | #============================================================= 9 | #============================================================= 10 | 11 | 12 | # dataset locations 13 | datasets=( 14 | "udel_arl_short" 15 | # "udel_room_05" 16 | # "udel_room_05_short" 17 | ) 18 | 19 | # how far we should start into the dataset 20 | # this can be used to skip the initial sections 21 | do_msckf_plane=( 22 | "false" 23 | "true" 24 | ) 25 | 26 | do_slam_plane=( 27 | "false" 28 | "true" 29 | ) 30 | 31 | 32 | sigma_c=( 33 | "0.001" 34 | # "0.01" 35 | # "0.05" 36 | # "0.10" 37 | ) 38 | 39 | max_slam=( 40 | "00" 41 | "15" 42 | # "30" 43 | ) 44 | 45 | # location to save log files into 46 | base_path="/home/patrick/workspace/catkin_ws_plane/src/ov_plane/results" 47 | save_path1="$base_path/sim_general/algorithms" 48 | save_path_gt="$base_path/sim_general/truths" 49 | save_path2="$base_path/sim_general/timings" 50 | sufix="" 51 | 52 | 53 | #============================================================= 54 | #============================================================= 55 | #============================================================= 56 | 57 | # Loop through all datasets 58 | for i in "${!datasets[@]}"; do 59 | for a in "${!do_msckf_plane[@]}"; do 60 | for b in "${!sigma_c[@]}"; do 61 | for c in "${!max_slam[@]}"; do 62 | for d in "${!do_slam_plane[@]}"; do 63 | 64 | # groundtruth file save location 65 | # ONLY SAVE FOR NONE SO WE DON'T KEEP OVERWRITTING IT! 66 | if [[ "${do_msckf_plane[a]}" == "false" ]]; then 67 | filename_gt="$save_path_gt/${datasets[i]}.txt" 68 | else 69 | filename_gt="/tmp/groundtruth.txt" 70 | fi 71 | 72 | # Monte Carlo runs for this dataset 73 | # If you want more runs, change the below loop 74 | for j in {00..19}; do 75 | 76 | # for none loop we only need to run the first one 77 | if [[ "${do_msckf_plane[a]}" == "false" && "$b" != "0" ]]; then 78 | break 79 | fi 80 | if [[ "${do_msckf_plane[a]}" == "false" && "${do_slam_plane[d]}" == "true" ]]; then 81 | break 82 | fi 83 | 84 | # start timing 85 | start_time="$(date -u +%s)" 86 | foldername="${max_slam[c]}slam" 87 | if [ "${do_msckf_plane[a]}" == "true" ] 88 | then 89 | foldername="${max_slam[c]}slam_${sigma_c[b]}plane" 90 | fi 91 | if [[ "${do_msckf_plane[a]}" == "true" && "${do_slam_plane[d]}" == "true" ]] 92 | then 93 | foldername="${foldername}_slam" 94 | fi 95 | 96 | foldername="${foldername}${sufix}" 97 | filename_est="$save_path1/$foldername/${datasets[i]}/${j}_estimate.txt" 98 | filename_time="$save_path2/$foldername/${datasets[i]}/${j}_timing.txt" 99 | 100 | roslaunch ov_plane simulation.launch \ 101 | verbosity:="WARNING" \ 102 | seed:="$((10#$j + 5))" \ 103 | dataset:="${datasets[i]}.txt" \ 104 | use_plane_constraint:="${do_msckf_plane[a]}" \ 105 | use_plane_constraint_msckf:="${do_msckf_plane[a]}" \ 106 | use_plane_constraint_slamu:="${do_msckf_plane[a]}" \ 107 | use_plane_constraint_slamd:="${do_msckf_plane[a]}" \ 108 | use_plane_slam_feats:="${do_slam_plane[d]}" \ 109 | use_refine_plane_feat:="true" \ 110 | use_groundtruths:="false" \ 111 | sigma_constraint:="${sigma_c[b]}" \ 112 | const_init_multi:="1.00" \ 113 | num_pts:="0" \ 114 | num_pts_plane:="150" \ 115 | num_slam:="${max_slam[c]}" \ 116 | dosave_pose:="true" \ 117 | path_est:="$filename_est" \ 118 | path_gt:="$filename_gt" \ 119 | dotime:="true" \ 120 | path_time:="$filename_time" &> /dev/null 121 | 122 | # print out the time elapsed 123 | end_time="$(date -u +%s)" 124 | elapsed="$(($end_time-$start_time))" 125 | echo "BASH: $foldername - ${datasets[i]} - run $j took $elapsed seconds"; 126 | 127 | done 128 | done 129 | done 130 | done 131 | done 132 | done 133 | 134 | 135 | -------------------------------------------------------------------------------- /ov_plane/src/state/State.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #include "State.h" 28 | 29 | using namespace ov_core; 30 | using namespace ov_type; 31 | using namespace ov_plane; 32 | 33 | State::State(StateOptions &options) { 34 | 35 | // Save our options 36 | _options = options; 37 | 38 | // Append the imu to the state and covariance 39 | int current_id = 0; 40 | _imu = std::make_shared(); 41 | _imu->set_local_id(current_id); 42 | _variables.push_back(_imu); 43 | current_id += _imu->size(); 44 | 45 | // Camera to IMU time offset 46 | _calib_dt_CAMtoIMU = std::make_shared(1); 47 | if (_options.do_calib_camera_timeoffset) { 48 | _calib_dt_CAMtoIMU->set_local_id(current_id); 49 | _variables.push_back(_calib_dt_CAMtoIMU); 50 | current_id += _calib_dt_CAMtoIMU->size(); 51 | } 52 | 53 | // Loop through each camera and create extrinsic and intrinsics 54 | for (int i = 0; i < _options.num_cameras; i++) { 55 | 56 | // Allocate extrinsic transform 57 | auto pose = std::make_shared(); 58 | 59 | // Allocate intrinsics for this camera 60 | auto intrin = std::make_shared(8); 61 | 62 | // Add these to the corresponding maps 63 | _calib_IMUtoCAM.insert({i, pose}); 64 | _cam_intrinsics.insert({i, intrin}); 65 | 66 | // If calibrating camera-imu pose, add to variables 67 | if (_options.do_calib_camera_pose) { 68 | pose->set_local_id(current_id); 69 | _variables.push_back(pose); 70 | current_id += pose->size(); 71 | } 72 | 73 | // If calibrating camera intrinsics, add to variables 74 | if (_options.do_calib_camera_intrinsics) { 75 | intrin->set_local_id(current_id); 76 | _variables.push_back(intrin); 77 | current_id += intrin->size(); 78 | } 79 | } 80 | 81 | // Finally initialize our covariance to small value 82 | _Cov = std::pow(1e-3, 2) * Eigen::MatrixXd::Identity(current_id, current_id); 83 | 84 | // Finally, set some of our priors for our calibration parameters 85 | if (_options.do_calib_camera_timeoffset) { 86 | _Cov(_calib_dt_CAMtoIMU->id(), _calib_dt_CAMtoIMU->id()) = std::pow(0.01, 2); 87 | } 88 | if (_options.do_calib_camera_pose) { 89 | for (int i = 0; i < _options.num_cameras; i++) { 90 | _Cov.block(_calib_IMUtoCAM.at(i)->id(), _calib_IMUtoCAM.at(i)->id(), 3, 3) = std::pow(0.005, 2) * Eigen::MatrixXd::Identity(3, 3); 91 | _Cov.block(_calib_IMUtoCAM.at(i)->id() + 3, _calib_IMUtoCAM.at(i)->id() + 3, 3, 3) = 92 | std::pow(0.01, 2) * Eigen::MatrixXd::Identity(3, 3); 93 | } 94 | } 95 | if (_options.do_calib_camera_intrinsics) { 96 | for (int i = 0; i < _options.num_cameras; i++) { 97 | _Cov.block(_cam_intrinsics.at(i)->id(), _cam_intrinsics.at(i)->id(), 4, 4) = std::pow(1.0, 2) * Eigen::MatrixXd::Identity(4, 4); 98 | _Cov.block(_cam_intrinsics.at(i)->id() + 4, _cam_intrinsics.at(i)->id() + 4, 4, 4) = 99 | std::pow(0.005, 2) * Eigen::MatrixXd::Identity(4, 4); 100 | } 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Right 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: Never 20 | AllowShortLoopsOnASingleLine: false 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: false 24 | AlwaysBreakTemplateDeclarations: MultiLine 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 140 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Preserve 70 | IncludeCategories: 71 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 75 | Priority: 3 76 | SortPriority: 0 77 | - Regex: '.*' 78 | Priority: 1 79 | SortPriority: 0 80 | IncludeIsMainRegex: '(Test)?$' 81 | IncludeIsMainSourceRegex: '' 82 | IndentCaseLabels: false 83 | IndentGotoLabels: true 84 | IndentPPDirectives: None 85 | IndentWidth: 2 86 | IndentWrappedFunctionNames: false 87 | JavaScriptQuotes: Leave 88 | JavaScriptWrapImports: true 89 | KeepEmptyLinesAtTheStartOfBlocks: true 90 | MacroBlockBegin: '' 91 | MacroBlockEnd: '' 92 | MaxEmptyLinesToKeep: 1 93 | NamespaceIndentation: None 94 | ObjCBinPackProtocolList: Auto 95 | ObjCBlockIndentWidth: 2 96 | ObjCSpaceAfterProperty: false 97 | ObjCSpaceBeforeProtocolList: true 98 | PenaltyBreakAssignment: 2 99 | PenaltyBreakBeforeFirstCallParameter: 19 100 | PenaltyBreakComment: 300 101 | PenaltyBreakFirstLessLess: 120 102 | PenaltyBreakString: 1000 103 | PenaltyBreakTemplateDeclaration: 10 104 | PenaltyExcessCharacter: 1000000 105 | PenaltyReturnTypeOnItsOwnLine: 60 106 | PointerAlignment: Right 107 | ReflowComments: true 108 | SortIncludes: true 109 | SortUsingDeclarations: true 110 | SpaceAfterCStyleCast: false 111 | SpaceAfterLogicalNot: false 112 | SpaceAfterTemplateKeyword: true 113 | SpaceBeforeAssignmentOperators: true 114 | SpaceBeforeCpp11BracedList: false 115 | SpaceBeforeCtorInitializerColon: true 116 | SpaceBeforeInheritanceColon: true 117 | SpaceBeforeParens: ControlStatements 118 | SpaceBeforeRangeBasedForLoopColon: true 119 | SpaceInEmptyBlock: false 120 | SpaceInEmptyParentheses: false 121 | SpacesBeforeTrailingComments: 1 122 | SpacesInAngles: false 123 | SpacesInConditionalStatement: false 124 | SpacesInContainerLiterals: true 125 | SpacesInCStyleCastParentheses: false 126 | SpacesInParentheses: false 127 | SpacesInSquareBrackets: false 128 | SpaceBeforeSquareBrackets: false 129 | Standard: Latest 130 | StatementMacros: 131 | - Q_UNUSED 132 | - QT_REQUIRE_VERSION 133 | TabWidth: 2 134 | UseCRLF: false 135 | UseTab: Never 136 | ... 137 | 138 | -------------------------------------------------------------------------------- /ov_plane/src/ros/ROSVisualizerHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_ROSVISUALIZER_HELPER_H 28 | #define OV_PLANE_ROSVISUALIZER_HELPER_H 29 | 30 | #include 31 | 32 | #if ROS_AVAILABLE == 1 33 | #include 34 | #include 35 | #include 36 | #include 37 | #endif 38 | 39 | namespace ov_type { 40 | class PoseJPL; 41 | } 42 | 43 | namespace ov_plane { 44 | 45 | class State; 46 | class VioManager; 47 | class Simulator; 48 | 49 | /** 50 | * @brief Helper class that handles some common versions into and out of ROS formats 51 | */ 52 | class ROSVisualizerHelper { 53 | 54 | public: 55 | #if ROS_AVAILABLE == 1 56 | /** 57 | * @brief Will visualize the system if we have new things 58 | * @param feats Vector of features we will convert into ros format 59 | * @return ROS pointcloud 60 | */ 61 | static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector &feats) { 62 | std::vector feats_tmp; 63 | for (auto const &feat : feats) 64 | feats_tmp.emplace_back(feat); 65 | return get_ros_pointcloud(feats_tmp); 66 | } 67 | 68 | /** 69 | * @brief Will visualize the system if we have new things 70 | * @param feats Vector of features we will convert into ros format 71 | * @return ROS pointcloud 72 | */ 73 | static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector &feats); 74 | /** 75 | * @brief Will visualize the system if we have new things 76 | * @param feats Vector of features we will convert into ros format 77 | * @param colors Map from size_t to RGB color 78 | * @return ROS pointcloud 79 | */ 80 | static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::map> &feats, 81 | const std::map &colors); 82 | 83 | /** 84 | * @brief Given a ov_type::PoseJPL this will convert into the ros format. 85 | * 86 | * NOTE: frame ids need to be handled externally! 87 | * 88 | * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) 89 | * @param flip_trans If we should flip / inverse the translation 90 | * @return TF of our pose in global (e.g. q_ItoG, p_IinG) 91 | */ 92 | static tf::StampedTransform get_stamped_transform_from_pose(const std::shared_ptr &pose, bool flip_trans); 93 | #endif 94 | 95 | /** 96 | * @brief Save current estimate state and groundtruth including calibration 97 | * @param state Pointer to the state 98 | * @param sim Pointer to the simulator (or null) 99 | * @param of_state_est Output file for state estimate 100 | * @param of_state_std Output file for covariance 101 | * @param of_state_gt Output file for groundtruth (if we have it from sim) 102 | */ 103 | static void sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, std::ofstream &of_state_est, 104 | std::ofstream &of_state_std, std::ofstream &of_state_gt); 105 | 106 | private: 107 | // Cannot create this class 108 | ROSVisualizerHelper() = default; 109 | }; 110 | 111 | } // namespace ov_plane 112 | 113 | #endif // OV_PLANE_ROSVISUALIZER_HELPER_H 114 | -------------------------------------------------------------------------------- /results/run_ros_eth.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Source our workspace directory to load ENV variables 4 | SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 5 | source ${SCRIPT_DIR}/../../../devel/setup.bash 6 | 7 | #============================================================= 8 | #============================================================= 9 | #============================================================= 10 | 11 | # dataset locations 12 | bagnames=( 13 | "V1_01_easy" 14 | "V1_02_medium" 15 | "V1_03_difficult" 16 | "V2_01_easy" 17 | "V2_02_medium" 18 | "V2_03_difficult" 19 | ) 20 | 21 | # how far we should start into the dataset 22 | # this can be used to skip the initial sections 23 | bagstarttimes=( 24 | "0" 25 | "0" 26 | "0" 27 | "0" 28 | "0" 29 | "0" 30 | ) 31 | 32 | do_msckf_plane=( 33 | "false" 34 | "true" 35 | ) 36 | 37 | do_slam_plane=( 38 | "false" 39 | "true" 40 | ) 41 | 42 | sigma_c=( 43 | # "0.001" 44 | "0.010" 45 | # "0.030" 46 | ) 47 | 48 | max_slam=( 49 | "00" 50 | "15" 51 | # "30" 52 | ) 53 | 54 | # location to save log files into 55 | bag_path="/home/patrick/datasets/" 56 | base_path="/home/patrick/workspace/catkin_ws_plane/src/ov_plane/results/" 57 | save_path1="$base_path/exp_euroc_mav/algorithms" 58 | save_path2="$base_path/exp_euroc_mav/timings" 59 | save_path3="$base_path/exp_euroc_mav/trackings" 60 | sufix="" 61 | 62 | 63 | #============================================================= 64 | #============================================================= 65 | #============================================================= 66 | 67 | # Loop through all datasets 68 | for i in "${!bagnames[@]}"; do 69 | 70 | # Loop through all modes 71 | for a in "${!do_msckf_plane[@]}"; do 72 | for b in "${!sigma_c[@]}"; do 73 | for c in "${!max_slam[@]}"; do 74 | for d in "${!do_slam_plane[@]}"; do 75 | 76 | # Monte Carlo runs for this dataset 77 | # If you want more runs, change the below loop 78 | for j in {00..00}; do 79 | 80 | # for none loop we only need to run the first one 81 | if [[ "${do_msckf_plane[a]}" == "false" && "$b" != "0" ]]; then 82 | break 83 | fi 84 | if [[ "${do_msckf_plane[a]}" == "false" && "${do_slam_plane[d]}" == "true" && "$d" != "0" ]]; then 85 | break 86 | fi 87 | 88 | # start timing 89 | start_time="$(date -u +%s)" 90 | if [ "${max_slam[c]}" == "00" ]; then 91 | foldername="M-PT" 92 | else 93 | foldername="MS-PT(${max_slam[c]})" 94 | fi 95 | if [[ "${do_msckf_plane[a]}" == "true" && "${do_slam_plane[d]}" == "false" ]]; then 96 | foldername="${foldername} - M-PL(${sigma_c[b]})" 97 | fi 98 | if [[ "${do_msckf_plane[a]}" == "true" && "${do_slam_plane[d]}" == "true" ]]; then 99 | foldername="${foldername} - MS-PL(${sigma_c[b]})" 100 | fi 101 | foldername="${foldername}${sufix}" 102 | filename_est="$save_path1/$foldername/${bagnames[i]}/${j}_estimate.txt" 103 | filename_time="$save_path2/$foldername/${bagnames[i]}/${j}_timing.txt" 104 | filename_track="$save_path3/$foldername/${bagnames[i]}/${j}_tracking.txt" 105 | 106 | 107 | # run our ROS launch file (note we send console output to terminator) 108 | roslaunch ov_plane serial.launch \ 109 | verbosity:="WARNING" \ 110 | max_cameras:="1" \ 111 | use_stereo:="false" \ 112 | config:="euroc_mav" \ 113 | object:="" \ 114 | dataset:="${bagnames[i]}" \ 115 | bag:="$bag_path/euroc_mav/${bagnames[i]}.bag" \ 116 | bag_start:="${bagstarttimes[i]}" \ 117 | use_plane_constraint:="${do_msckf_plane[a]}" \ 118 | use_plane_constraint_msckf:="${do_msckf_plane[a]}" \ 119 | use_plane_constraint_slamu:="${do_msckf_plane[a]}" \ 120 | use_plane_constraint_slamd:="${do_msckf_plane[a]}" \ 121 | use_plane_slam_feats:="${do_slam_plane[d]}" \ 122 | use_refine_plane_feat:="true" \ 123 | sigma_constraint:="${sigma_c[b]}" \ 124 | const_init_multi:="1.00" \ 125 | const_init_chi2:="1.00" \ 126 | num_pts:="200" \ 127 | num_slam:="${max_slam[c]}" \ 128 | dobag:="true" \ 129 | bag_rate:="2" \ 130 | dosave:="true" \ 131 | path_est:="$filename_est" \ 132 | dotime:="true" \ 133 | path_time:="$filename_time" \ 134 | dotrackinfo:="true" \ 135 | path_track:="$filename_track" \ 136 | dolivetraj:="true" &> /dev/null 137 | 138 | 139 | 140 | # print out the time elapsed 141 | end_time="$(date -u +%s)" 142 | elapsed="$(($end_time-$start_time))" 143 | echo "BASH: $foldername - ${bagnames[i]} - run $j took $elapsed seconds"; 144 | 145 | done 146 | 147 | done 148 | done 149 | done 150 | done 151 | 152 | done 153 | 154 | 155 | -------------------------------------------------------------------------------- /results/run_ros_rpng_plane.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Source our workspace directory to load ENV variables 4 | SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 5 | source ${SCRIPT_DIR}/../../../devel/setup.bash 6 | 7 | #============================================================= 8 | #============================================================= 9 | #============================================================= 10 | 11 | # dataset locations 12 | bagnames=( 13 | "table_01" 14 | "table_02" 15 | "table_03" 16 | "table_04" 17 | "table_05" 18 | "table_06" 19 | "table_07" 20 | "table_08" 21 | ) 22 | 23 | # how far we should start into the dataset 24 | # this can be used to skip the initial sections 25 | bagstarttimes=( 26 | "0" 27 | "0" 28 | "0" 29 | "0" 30 | "0" 31 | "0" 32 | "0" 33 | "0" 34 | ) 35 | 36 | do_msckf_plane=( 37 | "false" 38 | "true" 39 | ) 40 | 41 | do_slam_plane=( 42 | "false" 43 | "true" 44 | ) 45 | 46 | sigma_c=( 47 | # "0.001" 48 | "0.010" 49 | # "0.030" 50 | ) 51 | 52 | max_slam=( 53 | "00" 54 | "15" 55 | # "30" 56 | ) 57 | 58 | # location to save log files into 59 | bag_path="/home/patrick/datasets/" 60 | base_path="/home/patrick/workspace/catkin_ws_plane/src/ov_plane/results/" 61 | save_path1="$base_path/exp_rpng_plane/algorithms" 62 | save_path2="$base_path/exp_rpng_plane/timings" 63 | save_path3="$base_path/exp_rpng_plane/trackings" 64 | sufix="" 65 | 66 | 67 | #============================================================= 68 | #============================================================= 69 | #============================================================= 70 | 71 | # Loop through all datasets 72 | for i in "${!bagnames[@]}"; do 73 | 74 | # Loop through all modes 75 | for a in "${!do_msckf_plane[@]}"; do 76 | for b in "${!sigma_c[@]}"; do 77 | for c in "${!max_slam[@]}"; do 78 | for d in "${!do_slam_plane[@]}"; do 79 | 80 | # Monte Carlo runs for this dataset 81 | # If you want more runs, change the below loop 82 | for j in {00..00}; do 83 | 84 | # for none loop we only need to run the first one 85 | if [[ "${do_msckf_plane[a]}" == "false" && "$b" != "0" ]]; then 86 | break 87 | fi 88 | if [[ "${do_msckf_plane[a]}" == "false" && "${do_slam_plane[d]}" == "true" && "$d" != "0" ]]; then 89 | break 90 | fi 91 | 92 | # start timing 93 | start_time="$(date -u +%s)" 94 | if [ "${max_slam[c]}" == "00" ]; then 95 | foldername="M-PT" 96 | else 97 | foldername="MS-PT(${max_slam[c]})" 98 | fi 99 | if [[ "${do_msckf_plane[a]}" == "true" && "${do_slam_plane[d]}" == "false" ]]; then 100 | foldername="${foldername} - M-PL(${sigma_c[b]})" 101 | fi 102 | if [[ "${do_msckf_plane[a]}" == "true" && "${do_slam_plane[d]}" == "true" ]]; then 103 | foldername="${foldername} - MS-PL(${sigma_c[b]})" 104 | fi 105 | foldername="${foldername}${sufix}" 106 | filename_est="$save_path1/$foldername/${bagnames[i]}/${j}_estimate.txt" 107 | filename_time="$save_path2/$foldername/${bagnames[i]}/${j}_timing.txt" 108 | filename_track="$save_path3/$foldername/${bagnames[i]}/${j}_tracking.txt" 109 | 110 | 111 | # run our ROS launch file (note we send console output to terminator) 112 | roslaunch ov_plane serial.launch \ 113 | verbosity:="WARNING" \ 114 | max_cameras:="1" \ 115 | use_stereo:="false" \ 116 | config:="rpng_plane" \ 117 | object:="" \ 118 | dataset:="${bagnames[i]}" \ 119 | bag:="$bag_path/rpng_plane/${bagnames[i]}.bag" \ 120 | bag_start:="${bagstarttimes[i]}" \ 121 | use_plane_constraint:="${do_msckf_plane[a]}" \ 122 | use_plane_constraint_msckf:="${do_msckf_plane[a]}" \ 123 | use_plane_constraint_slamu:="${do_msckf_plane[a]}" \ 124 | use_plane_constraint_slamd:="${do_msckf_plane[a]}" \ 125 | use_plane_slam_feats:="${do_slam_plane[d]}" \ 126 | use_refine_plane_feat:="true" \ 127 | sigma_constraint:="${sigma_c[b]}" \ 128 | const_init_multi:="1.00" \ 129 | const_init_chi2:="1.00" \ 130 | num_pts:="200" \ 131 | num_slam:="${max_slam[c]}" \ 132 | dobag:="true" \ 133 | bag_rate:="2" \ 134 | dosave:="true" \ 135 | path_est:="$filename_est" \ 136 | dotime:="true" \ 137 | path_time:="$filename_time" \ 138 | dotrackinfo:="true" \ 139 | path_track:="$filename_track" \ 140 | dolivetraj:="true" &> /dev/null 141 | 142 | 143 | # print out the time elapsed 144 | end_time="$(date -u +%s)" 145 | elapsed="$(($end_time-$start_time))" 146 | echo "BASH: $foldername - ${bagnames[i]} - run $j took $elapsed seconds"; 147 | 148 | done 149 | 150 | done 151 | done 152 | done 153 | done 154 | 155 | done 156 | 157 | -------------------------------------------------------------------------------- /ov_plane/src/track_plane/PlaneFitting.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_PLANE_FITTING_H 28 | #define OV_PLANE_PLANE_FITTING_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "feat/FeatureInitializer.h" 35 | 36 | namespace ov_core { 37 | class Feature; 38 | } // namespace ov_core 39 | 40 | namespace ov_plane { 41 | 42 | /** 43 | * @brief Functions to initialize planes from sparse points 44 | * 45 | * Planes have already been classified into sets of pointcloud features. 46 | * The logic here is trying to find the closest-point plane initial value and refining it. 47 | * To get an initial guess, we a planar RANSAC which classifies inliers based on the point-to-plane distance. 48 | * From there, the plane and features can be jointly refined to improve their result. 49 | */ 50 | class PlaneFitting { 51 | public: 52 | /** 53 | * @brief Given a set of point this will fit a plane to it 54 | * 55 | * Creates a linear system of A*norm = b using: 56 | * ax + by + cz + d = 0 57 | * p_FinG.dot(norm) + d = 0 58 | * 59 | * @param feats Set of features (need valid p_FinG) 60 | * @param abcd Plane parameters 61 | * @param cond_thresh Threshold of the linear system condition number 62 | * @return True on success 63 | */ 64 | static bool fit_plane(const std::vector> &feats, Eigen::Vector4d &abcd, double cond_thresh, 65 | bool cond_check = true); 66 | 67 | /** 68 | * @brief Computes the plane to point distance 69 | * @param feats Set of features (need valid p_FinG) 70 | * @param abcd Plane parameters (plane in global) 71 | * @return Scalar distance to plane 72 | */ 73 | static inline double point_to_plane_distance(const Eigen::Vector3d &point, const Eigen::Vector4d &abcd) { 74 | return point.dot(abcd.head(3)) + abcd(3); 75 | } 76 | 77 | /** 78 | * @brief This function will use ransac to find the plane. 79 | * @param feats Set of features (will be returned as the inlier set!) 80 | * @param abcd Plane parameters 81 | * @return True if success 82 | */ 83 | static bool plane_fitting(std::vector> &feats, Eigen::Vector4d &plane_abcd, int min_inlier_num = 5, 84 | double max_plane_solver_condition_number = 200.0); 85 | 86 | /** 87 | * @brief This function will optimize a set of features and plane 88 | * @param feats Set of features (will optimize p_FinG and returned as the inlier set!) 89 | * @param cp_inG Closest point plane in global frame 90 | * @param[in] clonesCAM Map between camera ID to map of timestamp to camera pose estimate (rotation from global to camera, position of 91 | * camera in global frame) 92 | * @param[in] sigma_px_norm Sigma of feature observation (normalized image coordinates sigma_n = sigma_raw / focal_length) 93 | * @param[in] sigma_c Point-on-plane constraint 94 | * @param[in] fix_plane If the plane estimate should not be optimized 95 | * @param[in] stateI Current pose [q_GtoI, p_IinG] 96 | * @param[in] calib0 Calibration from IMU to camera [q_ItoC, p_IinC] 97 | * @return True if success 98 | */ 99 | static bool optimize_plane(std::vector> &feats, Eigen::Vector3d &cp_inG, 100 | std::unordered_map> &clonesCAM, 101 | double sigma_px_norm, double sigma_c, bool fix_plane, const Eigen::VectorXd &stateI, 102 | const Eigen::VectorXd &calib0); 103 | }; 104 | 105 | } // namespace ov_plane 106 | 107 | #endif // OV_PLANE_PLANE_FITTING_H -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterSLAM.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_SLAM_H 28 | #define OV_PLANE_UPDATER_SLAM_H 29 | 30 | #include 31 | #include 32 | 33 | #include "feat/FeatureInitializerOptions.h" 34 | 35 | #include "UpdaterOptions.h" 36 | 37 | namespace ov_core { 38 | class Feature; 39 | class FeatureInitializer; 40 | } // namespace ov_core 41 | namespace ov_type { 42 | class Landmark; 43 | } // namespace ov_type 44 | 45 | namespace ov_plane { 46 | 47 | class State; 48 | 49 | /** 50 | * @brief Will compute the system for our sparse SLAM features and update the filter. 51 | * 52 | * This class is responsible for performing delayed feature initialization, SLAM update, and 53 | * SLAM anchor change for anchored feature representations. 54 | */ 55 | class UpdaterSLAM { 56 | 57 | public: 58 | /** 59 | * @brief Default constructor for our SLAM updater 60 | * 61 | * Our updater has a feature initializer which we use to initialize features as needed. 62 | * Also the options allow for one to tune the different parameters for update. 63 | * 64 | * @param options_slam Updater options (include measurement noise value) for SLAM features 65 | * @param options_aruco Updater options (include measurement noise value) for ARUCO features 66 | * @param feat_init_options Feature initializer options 67 | */ 68 | UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options); 69 | 70 | /** 71 | * @brief Given tracked SLAM features, this will try to use them to update the state. 72 | * @param state State of the filter 73 | * @param feature_vec Features that can be used for update 74 | * @param feat2plane Feature to planeid mapping! 75 | */ 76 | void update(std::shared_ptr state, std::vector> &feature_vec, 77 | const std::map &feat2plane); 78 | 79 | /** 80 | * @brief Given max track features, this will try to use them to initialize them in the state. 81 | * @param state State of the filter 82 | * @param feature_vec Features that can be used for update 83 | * @param feat2plane Feature to planeid mapping! 84 | */ 85 | void delayed_init(std::shared_ptr state, std::vector> &feature_vec, 86 | const std::map &feat2plane); 87 | 88 | /** 89 | * @brief Will change SLAM feature anchors if it will be marginalized 90 | * 91 | * Makes sure that if any clone is about to be marginalized, it changes anchor representation. 92 | * By default, this will shift the anchor into the newest IMU clone and keep the camera calibration anchor the same. 93 | * 94 | * @param state State of the filter 95 | */ 96 | void change_anchors(std::shared_ptr state); 97 | 98 | protected: 99 | /** 100 | * @brief Shifts landmark anchor to new clone 101 | * @param state State of filter 102 | * @param landmark landmark whose anchor is being shifter 103 | * @param new_anchor_timestamp Clone timestamp we want to move to 104 | * @param new_cam_id Which camera frame we want to move to 105 | */ 106 | void perform_anchor_change(std::shared_ptr state, std::shared_ptr landmark, double new_anchor_timestamp, 107 | size_t new_cam_id); 108 | 109 | /// Options used during update for slam features 110 | UpdaterOptions _options_slam; 111 | 112 | /// Options used during update for aruco features 113 | UpdaterOptions _options_aruco; 114 | 115 | /// Feature initializer class object 116 | std::shared_ptr initializer_feat; 117 | 118 | /// Chi squared 95th percentile table (lookup would be size of residual) 119 | std::map chi_squared_table; 120 | }; 121 | 122 | } // namespace ov_plane 123 | 124 | #endif // OV_PLANE_UPDATER_SLAM_H 125 | -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_PLANE_H 28 | #define OV_PLANE_UPDATER_PLANE_H 29 | 30 | #include 31 | #include 32 | 33 | #include "feat/FeatureInitializerOptions.h" 34 | 35 | #include "UpdaterOptions.h" 36 | 37 | namespace ov_core { 38 | class Feature; 39 | class FeatureInitializer; 40 | } // namespace ov_core 41 | namespace ov_type { 42 | class Landmark; 43 | } // namespace ov_type 44 | 45 | namespace ov_plane { 46 | 47 | class State; 48 | 49 | /** 50 | * @brief Will try to do the plane initialization for features. 51 | * 52 | * This really only does plane initialization right now. 53 | * The logic is that we will try to collect points that we can use to initialize a plane. 54 | * If we have enough points, we will initialize the plane. 55 | * Importantly, we ensure to mark the used measurements as "deleted" so that we don't reuse information and become inconsistent. 56 | * 57 | * 58 | */ 59 | class UpdaterPlane { 60 | 61 | public: 62 | /** 63 | * @brief Default constructor for our plane updater 64 | * @param options Updater options (include measurement noise value) for features 65 | * @param feat_init_options Feature initializer options 66 | */ 67 | UpdaterPlane(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options); 68 | 69 | /** 70 | * @brief This will try to initialize SLAM plane features 71 | * 72 | * Given the state and its SLAM features, along with the MSCKF features for the current update, 73 | * we will try to initialize any *new* planes that we have not yet added into our state vector! 74 | * 75 | * @param state State of the filter 76 | * @param feature_vec Features that can be used for update 77 | * @param feature_vec_used Features that were used to initialize a new plane! 78 | * @param feat2plane Feature to planeid mapping! 79 | */ 80 | void init_vio_plane(std::shared_ptr state, std::vector> &feature_vec, 81 | std::vector> &feature_vec_used, const std::map &feat2plane); 82 | /** 83 | * @brief This will project the left nullspace of H_f onto the linear system. 84 | * 85 | * Please see the @ref update-null for details on how this works. 86 | * This is the MSCKF nullspace projection which removes the dependency on the feature state. 87 | * Note that this is done **in place** so all matrices will be different after a function call. 88 | * 89 | * NOTE: This function is slightly different from the UpdaterHelper one as we need to apply it to two seperate Jacobians. 90 | * 91 | * @param H_f Jacobian with nullspace we want to project onto the system [res = Hx*(x-xhat)+Hf(f-fhat)+n] 92 | * @param H_x State jacobian 93 | * @param H_cp CP plane jacobian 94 | * @param res Measurement residual 95 | */ 96 | static void nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::MatrixXd &H_cp, Eigen::VectorXd &res); 97 | 98 | /** 99 | * @brief This will perform measurement compression 100 | * 101 | * Please see the @ref update-compress for details on how this works. 102 | * Note that this is done **in place** so all matrices will be different after a function call. 103 | * 104 | * NOTE: This function is slightly different from the UpdaterHelper one as we need to apply it to two seperate Jacobians. 105 | * 106 | * @param H_x State jacobian 107 | * @param H_cp CP plane jacobian 108 | * @param res Measurement residual 109 | */ 110 | static void measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::MatrixXd &H_cp, Eigen::VectorXd &res); 111 | 112 | protected: 113 | /// Options used during update for features 114 | UpdaterOptions _options; 115 | 116 | /// Feature initializer class object 117 | std::shared_ptr initializer_feat; 118 | 119 | /// Chi squared 95th percentile table (lookup would be size of residual) 120 | std::map chi_squared_table; 121 | }; 122 | 123 | } // namespace ov_plane 124 | 125 | #endif // OV_PLANE_UPDATER_PLANE_H 126 | -------------------------------------------------------------------------------- /ov_plane/src/state/State.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_STATE_H 28 | #define OV_PLANE_STATE_H 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "StateOptions.h" 36 | #include "cam/CamBase.h" 37 | #include "types/IMU.h" 38 | #include "types/Landmark.h" 39 | #include "types/PoseJPL.h" 40 | #include "types/Type.h" 41 | #include "types/Vec.h" 42 | 43 | namespace ov_plane { 44 | 45 | /** 46 | * @brief State of our filter 47 | * 48 | * This state has all the current estimates for the filter. 49 | * This system is modeled after the MSCKF filter, thus we have a sliding window of clones. 50 | * We additionally have more parameters for online estimation of calibration and SLAM features. 51 | * We also have the covariance of the system, which should be managed using the StateHelper class. 52 | */ 53 | class State { 54 | 55 | public: 56 | /** 57 | * @brief Default Constructor (will initialize variables to defaults) 58 | * @param options_ Options structure containing filter options 59 | */ 60 | State(StateOptions &options_); 61 | 62 | ~State() {} 63 | 64 | /** 65 | * @brief Will return the timestep that we will marginalize next. 66 | * As of right now, since we are using a sliding window, this is the oldest clone. 67 | * But if you wanted to do a keyframe system, you could selectively marginalize clones. 68 | * @return timestep of clone we will marginalize 69 | */ 70 | double margtimestep() { 71 | double time = INFINITY; 72 | for (const auto &clone_imu : _clones_IMU) { 73 | if (clone_imu.first < time) { 74 | time = clone_imu.first; 75 | } 76 | } 77 | return time; 78 | } 79 | 80 | /** 81 | * @brief Calculates the current max size of the covariance 82 | * @return Size of the current covariance matrix 83 | */ 84 | int max_covariance_size() { return (int)_Cov.rows(); } 85 | 86 | /// Current timestamp (should be the last update time!) 87 | double _timestamp = -1; 88 | 89 | /// Struct containing filter options 90 | StateOptions _options; 91 | 92 | /// Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba) 93 | std::shared_ptr _imu; 94 | 95 | /// Map between imaging times and clone poses (q_GtoIi, p_IiinG) 96 | std::map> _clones_IMU; 97 | 98 | /// Our current set of SLAM features (3d positions) 99 | std::unordered_map> _features_SLAM; 100 | 101 | /// Time offset base IMU to camera (t_imu = t_cam + t_off) 102 | std::shared_ptr _calib_dt_CAMtoIMU; 103 | 104 | /// Calibration poses for each camera (R_ItoC, p_IinC) 105 | std::unordered_map> _calib_IMUtoCAM; 106 | 107 | /// Camera intrinsics 108 | std::unordered_map> _cam_intrinsics; 109 | 110 | /// Camera intrinsics camera objects 111 | std::unordered_map> _cam_intrinsics_cameras; 112 | 113 | /// Our current set of PLANE closest-point features (3d positions in global frame) 114 | std::unordered_map> _features_PLANE; 115 | 116 | /// This is the mapping between current SLAM features and their planes (enforces feat is always function of same plane) 117 | std::unordered_map _features_SLAM_to_PLANE; 118 | 119 | // Store the true planes and point features 120 | std::unordered_map _true_planes; 121 | std::unordered_map _true_features; 122 | 123 | private: 124 | // Define that the state helper is a friend class of this class 125 | // This will allow it to access the below functions which should normally not be called 126 | // This prevents a developer from thinking that the "insert clone" will actually correctly add it to the covariance 127 | friend class StateHelper; 128 | 129 | /// Covariance of all active variables 130 | Eigen::MatrixXd _Cov; 131 | 132 | /// Vector of variables 133 | std::vector> _variables; 134 | }; 135 | 136 | } // namespace ov_plane 137 | 138 | #endif // OV_PLANE_STATE_H -------------------------------------------------------------------------------- /ov_plane/src/sim/SimPlane.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_PLANE_H 28 | #define OV_PLANE_PLANE_H 29 | 30 | #include 31 | 32 | namespace ov_plane { 33 | 34 | /** 35 | * @brief Plane helper class 36 | * 37 | * This class can be constructed using the 4 boundary points of a plane. 38 | * A 3d ray can be intersected with this plane and its intersection can be found. 39 | */ 40 | class SimPlane { 41 | 42 | public: 43 | /** 44 | * @brief Default constructor 45 | * @param _plane_id ID of this plane 46 | * @param _pt_top_left Top left point 47 | * @param _pt_top_right Top right point 48 | * @param _pt_bottom_left Bottom left point (line start point in floorplan) 49 | * @param _pt_bottom_right Bottom right point (line end point in floorplan) 50 | */ 51 | SimPlane(size_t _plane_id, Eigen::Vector3d &_pt_top_left, Eigen::Vector3d &_pt_top_right, Eigen::Vector3d &_pt_bottom_left, 52 | Eigen::Vector3d &_pt_bottom_right) 53 | : plane_id(_plane_id), pt_top_left(_pt_top_left), pt_top_right(_pt_top_right), pt_bottom_left(_pt_bottom_left), 54 | pt_bottom_right(_pt_bottom_right) { 55 | Eigen::Vector3d V1 = pt_top_right - pt_top_left; 56 | Eigen::Vector3d V2 = pt_bottom_left - pt_top_left; 57 | Eigen::Vector3d N = V1.cross(V2); 58 | A = N(0); 59 | B = N(1); 60 | C = N(2); 61 | D = -(A * pt_top_left(0) + B * pt_top_left(1) + C * pt_top_left(2)); 62 | } 63 | 64 | /** 65 | * @brief This will try to intersect the given ray and this plane. 66 | * 67 | * This assumes that the ray and the plane are in the same frame of reference. 68 | * This will return true if it is a hit, and false otherwise. 69 | * Given a plane in the form Ax+By+Cz+D=0 we can first solve for the intersection. 70 | * R(t) = R0 + Rd*t 71 | * A(x0 + xd*t) + B(y0 + yd*t) + (z0 + zd*t) + D = 0 72 | * We can inverse the above function to get the distance along this ray bearing 73 | * t = -(A*x0 + B*y0 + C*z0 + D) / (A*xd + B*yd + C*zd) 74 | * 75 | * @param ray Bearing ray in the frame this plane is represented in [ray_origin, ray_bearing] 76 | * @param point_intersection Scalar distance along the ray that makes the point lie on this plane 77 | * @return True if we found an intersection 78 | */ 79 | bool calculate_intersection(const Eigen::Matrix &ray, double &point_intersection) const { 80 | 81 | // Intersect the ray with our plane 82 | point_intersection = -(A * ray(0) + B * ray(1) + C * ray(2) + D) / (A * ray(3) + B * ray(4) + C * ray(5)); 83 | 84 | // Calculate the actual intersection 3d point 85 | Eigen::Vector3d pt_inter = ray.head(3) + point_intersection * ray.tail(3); 86 | 87 | // Check the result 88 | Eigen::Vector3d V1 = pt_top_right - pt_top_left; 89 | V1.normalize(); 90 | Eigen::Vector3d V2 = pt_bottom_left - pt_top_left; 91 | V2.normalize(); 92 | Eigen::Vector3d V3 = pt_top_right - pt_bottom_right; 93 | V3.normalize(); 94 | Eigen::Vector3d V4 = pt_bottom_left - pt_bottom_right; 95 | V4.normalize(); 96 | Eigen::Vector3d U1 = pt_inter - pt_top_left; 97 | U1.normalize(); 98 | Eigen::Vector3d U2 = pt_inter - pt_bottom_right; 99 | U2.normalize(); 100 | 101 | return (point_intersection > 0 && U1.dot(V1) > 0 && U1.dot(V2) > 0 && U2.dot(V3) > 0 && U2.dot(V4) > 0); 102 | } 103 | 104 | /** 105 | * @brief Recovers the closest point representation of this plane 106 | * @return Closest-point 3d (same frame as features) 107 | */ 108 | Eigen::Vector3d cp() const { 109 | Eigen::Vector3d V1 = pt_top_right - pt_top_left; 110 | Eigen::Vector3d V2 = pt_bottom_left - pt_top_left; 111 | Eigen::Vector3d N = V1.cross(V2); 112 | Eigen::Vector3d n_PhiinG = N.normalized(); 113 | return -D / N.norm() * n_PhiinG; 114 | } 115 | 116 | // Our id of the plane 117 | size_t plane_id; 118 | 119 | // Our top-left plane point 120 | Eigen::Vector3d pt_top_left; 121 | 122 | // Our top-right plane point 123 | Eigen::Vector3d pt_top_right; 124 | 125 | // Our top-bottom plane point 126 | Eigen::Vector3d pt_bottom_left; 127 | 128 | // Our top-bottom plane point 129 | Eigen::Vector3d pt_bottom_right; 130 | 131 | // Plane parameters for a general plane (Ax + By + Cz + D = 0) 132 | double A, B, C, D; 133 | }; 134 | 135 | } // namespace ov_plane 136 | 137 | #endif // OV_PLANE_PLANE_H -------------------------------------------------------------------------------- /ov_plane/src/track_plane/TrackPlaneOptions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_TRACKPLANE_OPTIONS_H 28 | #define OV_PLANE_TRACKPLANE_OPTIONS_H 29 | 30 | #include "utils/opencv_yaml_parse.h" 31 | #include "utils/print.h" 32 | 33 | namespace ov_plane { 34 | 35 | /** 36 | * @brief Struct which stores all our TrackPlane options 37 | */ 38 | struct TrackPlaneOptions { 39 | 40 | /// If we should extract and track plane features 41 | bool track_planes = false; 42 | 43 | /// MAX triangle line length allowed to be considered (long ones are bad meshed) 44 | int max_tri_side_px = 200; 45 | 46 | /// How many triangular planar normals we should average over 47 | int max_norm_count = 5; 48 | 49 | /// MAX triangle norm difference to mark a vertex as invalid 50 | double max_norm_avg_max = 25.00; 51 | 52 | /// MAX triangle norm variance to mark a vertex as invalid 53 | double max_norm_avg_var = 25.00; 54 | 55 | /// MAX vertex norm difference to be considered the same plane 56 | double max_norm_deg = 25.00; 57 | 58 | /// MAX point-on-plane distance for two points to be considered the same plane 59 | double max_dist_between_z = 0.10; 60 | 61 | /// MAX distance between vertex to be considered the same plane 62 | int max_pairwise_px = 100; 63 | 64 | /// MIN number of norms to do a pairwise match to be considered the same plane 65 | int min_norms = 3; 66 | 67 | /// If we should try to do pairwise matching on vertex that have already been matched 68 | bool check_old_feats = true; 69 | 70 | /// MIN number of points for a plane to be considered large enough for an update (spatial filter) 71 | int filter_num_feat = 4; 72 | 73 | /// Z-test threshold to see if a feature lies on a detected plane (checked for all features in that plane cluster) 74 | double filter_z_thresh = 1.2; 75 | 76 | // feature triangulation 77 | int feat_init_min_obs = 4; 78 | double min_dist = 0.10; 79 | double max_dist = 60; 80 | double max_cond_number = 8000; 81 | 82 | /// Nice print function of what parameters we have loaded 83 | void print(const std::shared_ptr &parser = nullptr) { 84 | if (parser != nullptr) { 85 | parser->parse_config("use_plane_constraint", track_planes); 86 | // matching 87 | parser->parse_config("plane_max_tri_side_px", max_tri_side_px); 88 | parser->parse_config("plane_max_norm_count", max_norm_count); 89 | parser->parse_config("plane_max_norm_avg_max", max_norm_avg_max); 90 | parser->parse_config("plane_max_norm_avg_var", max_norm_avg_var); 91 | parser->parse_config("plane_max_norm_deg", max_norm_deg); 92 | parser->parse_config("plane_max_dist_between_z", max_dist_between_z); 93 | parser->parse_config("plane_max_pairwise_px", max_pairwise_px); 94 | parser->parse_config("plane_min_norms", min_norms); 95 | parser->parse_config("plane_check_old_feats", check_old_feats); 96 | // spatial filter 97 | parser->parse_config("plane_filter_num_feat", filter_num_feat); 98 | parser->parse_config("plane_filter_z_thresh", filter_z_thresh); 99 | // feature triangulation 100 | parser->parse_config("plane_feat_min_obs", feat_init_min_obs, false); 101 | parser->parse_config("plane_min_dist", min_dist, false); 102 | parser->parse_config("plane_max_dist", max_dist, false); 103 | parser->parse_config("plane_max_cond_number", max_cond_number, false); 104 | } 105 | PRINT_DEBUG("TRACK PLANE OPTIONS:\n"); 106 | PRINT_DEBUG(" - track_planes (use_plane_constraint): %d\n", track_planes); 107 | // matching 108 | PRINT_DEBUG(" - plane_max_tri_side_px: %d\n", max_tri_side_px); 109 | PRINT_DEBUG(" - plane_max_norm_count: %d\n", max_norm_count); 110 | PRINT_DEBUG(" - plane_max_norm_avg_max: %.2f\n", max_norm_avg_max); 111 | PRINT_DEBUG(" - plane_max_norm_avg_var: %.2f\n", max_norm_avg_var); 112 | PRINT_DEBUG(" - plane_max_norm_deg: %.2f\n", max_norm_deg); 113 | PRINT_DEBUG(" - plane_max_dist_between_z: %.2f\n", max_dist_between_z); 114 | PRINT_DEBUG(" - plane_max_pairwise_px: %d\n", max_pairwise_px); 115 | PRINT_DEBUG(" - plane_min_norms: %d\n", min_norms); 116 | PRINT_DEBUG(" - plane_check_old_feats: %d\n", check_old_feats); 117 | // spatial filter 118 | PRINT_DEBUG(" - plane_filter_num_feat: %d\n", filter_num_feat); 119 | PRINT_DEBUG(" - plane_filter_z_thresh: %.2f\n", filter_z_thresh); 120 | // feature triangulation 121 | PRINT_DEBUG(" - plane_feat_min_obs: %d\n", feat_init_min_obs); 122 | PRINT_DEBUG(" - min_dist: %.3f\n", min_dist); 123 | PRINT_DEBUG(" - max_dist: %.3f\n", max_dist); 124 | PRINT_DEBUG(" - max_cond_number: %.3f\n", max_cond_number); 125 | } 126 | }; 127 | 128 | } // namespace ov_plane 129 | 130 | #endif // OV_PLANE_TRACKPLANE_OPTIONS_H -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterZeroVelocity.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_ZEROVELOCITY_H 28 | #define OV_PLANE_UPDATER_ZEROVELOCITY_H 29 | 30 | #include 31 | 32 | #include "utils/sensor_data.h" 33 | 34 | #include "UpdaterOptions.h" 35 | #include "utils/NoiseManager.h" 36 | 37 | namespace ov_core { 38 | class Feature; 39 | class FeatureDatabase; 40 | } // namespace ov_core 41 | namespace ov_type { 42 | class Landmark; 43 | } // namespace ov_type 44 | 45 | namespace ov_plane { 46 | 47 | class State; 48 | class Propagator; 49 | 50 | /** 51 | * @brief Will try to *detect* and then update using zero velocity assumption. 52 | * 53 | * Consider the case that a VIO unit remains stationary for a period time. 54 | * Typically this can cause issues in a monocular system without SLAM features since no features can be triangulated. 55 | * Additional, if features could be triangulated (e.g. stereo) the quality can be poor and hurt performance. 56 | * If we can detect the cases where we are stationary then we can leverage this to prevent the need to do feature update during this period. 57 | * The main application would be using this on a **wheeled vehicle** which needs to stop (e.g. stop lights or parking). 58 | */ 59 | class UpdaterZeroVelocity { 60 | 61 | public: 62 | /** 63 | * @brief Default constructor for our zero velocity detector and updater. 64 | * @param options Updater options (chi2 multiplier) 65 | * @param noises imu noise characteristics (continuous time) 66 | * @param db Feature tracker database with all features in it 67 | * @param prop Propagator class object which can predict the state forward in time 68 | * @param gravity_mag Global gravity magnitude of the system (normally 9.81) 69 | * @param zupt_max_velocity Max velocity we should consider to do a update with 70 | * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) 71 | * @param zupt_max_disparity Max disparity we should consider to do a update with 72 | */ 73 | UpdaterZeroVelocity(UpdaterOptions &options, NoiseManager &noises, std::shared_ptr db, 74 | std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, 75 | double zupt_max_disparity); 76 | 77 | /** 78 | * @brief Feed function for inertial data 79 | * @param message Contains our timestamp and inertial information 80 | * @param oldest_time Time that we can discard measurements before 81 | */ 82 | void feed_imu(const ov_core::ImuData &message, double oldest_time = -1) { 83 | 84 | // Append it to our vector 85 | imu_data.emplace_back(message); 86 | 87 | // Sort our imu data (handles any out of order measurements) 88 | // std::sort(imu_data.begin(), imu_data.end(), [](const IMUDATA i, const IMUDATA j) { 89 | // return i.timestamp < j.timestamp; 90 | //}); 91 | 92 | // Loop through and delete imu messages that are older than our requested time 93 | if (oldest_time != -1) { 94 | auto it0 = imu_data.begin(); 95 | while (it0 != imu_data.end()) { 96 | if (it0->timestamp < oldest_time - 0.10) { 97 | it0 = imu_data.erase(it0); 98 | } else { 99 | it0++; 100 | } 101 | } 102 | } 103 | } 104 | 105 | /** 106 | * @brief Will first detect if the system is zero velocity, then will update. 107 | * @param state State of the filter 108 | * @param timestamp Next camera timestamp we want to see if we should propagate to. 109 | * @return True if the system is currently at zero velocity 110 | */ 111 | bool try_update(std::shared_ptr state, double timestamp); 112 | 113 | protected: 114 | /// Options used during update (chi2 multiplier) 115 | UpdaterOptions _options; 116 | 117 | /// Container for the imu noise values 118 | NoiseManager _noises; 119 | 120 | /// Feature tracker database with all features in it 121 | std::shared_ptr _db; 122 | 123 | /// Our propagator! 124 | std::shared_ptr _prop; 125 | 126 | /// Gravity vector 127 | Eigen::Vector3d _gravity; 128 | 129 | /// Max velocity (m/s) that we should consider a zupt with 130 | double _zupt_max_velocity = 1.0; 131 | 132 | /// Multiplier of our IMU noise matrix (default should be 1.0) 133 | double _zupt_noise_multiplier = 1.0; 134 | 135 | /// Max disparity (pixels) that we should consider a zupt with 136 | double _zupt_max_disparity = 1.0; 137 | 138 | /// Chi squared 95th percentile table (lookup would be size of residual) 139 | std::map chi_squared_table; 140 | 141 | /// Our history of IMU messages (time, angular, linear) 142 | std::vector imu_data; 143 | 144 | /// Estimate for time offset at last propagation time 145 | double last_prop_time_offset = 0.0; 146 | bool have_last_prop_time_offset = false; 147 | 148 | /// Last timestamp we did zero velocity update with 149 | double last_zupt_state_timestamp = 0.0; 150 | }; 151 | 152 | } // namespace ov_plane 153 | 154 | #endif // OV_PLANE_UPDATER_ZEROVELOCITY_H 155 | -------------------------------------------------------------------------------- /ov_plane/src/timing_custom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "utils/Loader.h" 33 | #include "utils/Statistics.h" 34 | #include "utils/colors.h" 35 | #include "utils/print.h" 36 | 37 | int main(int argc, char **argv) { 38 | 39 | // Verbosity setting 40 | ov_core::Printer::setPrintLevel("INFO"); 41 | 42 | // Ensure we have a path 43 | if (argc < 2) { 44 | PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET); 45 | PRINT_ERROR(RED "ERROR: ./timing_custom \n" RESET); 46 | PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_custom \n" RESET); 47 | std::exit(EXIT_FAILURE); 48 | } 49 | 50 | // Skip zeros for feat/plane 51 | // feat/plane,num plane,track length(avg),track length(std),track length(max), 52 | // num constraint updates,state planes, 53 | // triangulation,delaunay,matching,total 54 | std::vector skip_zeros = {true, false, false, false, false, false, false, false, false, false, false}; 55 | std::vector print_last = {false, false, true, true, true, false, false, false, false, false, false}; 56 | std::vector scale = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1000.0, 1000.0, 1000.0, 1000.0}; 57 | std::vector skip_print = {false, false, false, false, false, true, false, true, true, true, false}; 58 | 59 | // Read in all our trajectories from file 60 | std::vector names; 61 | std::vector names_cols; 62 | std::vector>> print_vals; 63 | for (int z = 1; z < argc; z++) { 64 | 65 | // Parse the name of this timing 66 | boost::filesystem::path path(argv[z]); 67 | std::string name = path.stem().string(); 68 | PRINT_INFO("[TIME]: loading data for %s\n", name.c_str()); 69 | 70 | // Load it!! 71 | std::vector names_temp; 72 | std::vector times; 73 | std::vector timing_values; 74 | ov_eval::Loader::load_timing_flamegraph(argv[z], names_temp, times, timing_values); 75 | PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size()); 76 | 77 | // Our categories 78 | std::vector stats; 79 | for (size_t i = 0; i < names_temp.size(); i++) 80 | stats.push_back(ov_eval::Statistics()); 81 | 82 | // Loop through each and report the average timing information 83 | for (size_t i = 0; i < times.size(); i++) { 84 | for (size_t c = 0; c < names_temp.size(); c++) { 85 | assert((int)skip_zeros.size() == timing_values.at(i).rows()); 86 | if (skip_zeros.at(c) && timing_values.at(i)(c) == 0.0) { 87 | // std::cout << timing_values.at(i).transpose() << std::endl; 88 | continue; 89 | } 90 | stats.at(c).timestamps.push_back(times.at(i)); 91 | stats.at(c).values.push_back(scale.at(c) * timing_values.at(i)(c)); 92 | } 93 | } 94 | 95 | // Now print the statistic for this run 96 | names_cols.clear(); 97 | std::vector> values; 98 | for (size_t i = 0; i < names_temp.size(); i++) { 99 | if (print_last.at(i)) { 100 | PRINT_INFO("last_value = %.4f (%s)\n", stats.at(i).values.at(stats.at(i).values.size() - 1), names_temp.at(i).c_str()); 101 | if (names_temp.at(i) == "track length(avg)" && !skip_print.at(i)) { 102 | names_cols.push_back(names_temp.at(i)); 103 | values.push_back( 104 | {stats.at(i).values.at(stats.at(i).values.size() - 1), stats.at(i + 1).values.at(stats.at(i + 1).values.size() - 1)}); 105 | } 106 | } else { 107 | stats.at(i).calculate(); 108 | PRINT_INFO("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, 109 | stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str()); 110 | if (!skip_print.at(i)) { 111 | names_cols.push_back(names_temp.at(i)); 112 | values.push_back({stats.at(i).mean, stats.at(i).std}); 113 | } 114 | } 115 | } 116 | 117 | // Append the total stats to the big vector 118 | if (stats.empty()) { 119 | PRINT_ERROR(RED "[TIME]: unable to load any data.....\n" RESET); 120 | } 121 | 122 | // Save data 123 | names.push_back(name); 124 | print_vals.push_back(values); 125 | } 126 | 127 | // feat/plane,num plane,track length(avg),track length(std),track length(max), 128 | // num constraint updates,state planes, 129 | // triangulation,delaunay,matching,total 130 | PRINT_INFO("============================================\n"); 131 | PRINT_INFO("TIMING LATEX TABLE\n"); 132 | PRINT_INFO("============================================\n"); 133 | for (size_t i = 0; i < names_cols.size(); i++) { 134 | std::string name = names_cols.at(i); 135 | boost::replace_all(name, "_", "\\_"); 136 | PRINT_INFO(" & \\textbf{%s}", name.c_str()); 137 | } 138 | PRINT_INFO(" \\\\\\hline\n"); 139 | for (int i = (int)names.size() - 1; i >= 0; i--) { 140 | std::string algoname = names.at(i); 141 | boost::replace_all(algoname, "_", "\\_"); 142 | PRINT_INFO(algoname.c_str()); 143 | for (auto &vals : print_vals.at(i)) { 144 | PRINT_INFO(" & %.1f $\\pm$ %.1f", vals.first, vals.second); 145 | } 146 | PRINT_INFO(" \\\\\n"); 147 | } 148 | 149 | // Done! 150 | return EXIT_SUCCESS; 151 | } -------------------------------------------------------------------------------- /ov_plane/launch/serial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 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 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /ov_plane/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | project(ov_plane) 3 | 4 | # Include libraries (if we don't have opencv 4, then fallback to opencv 3) 5 | # The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! 6 | find_package(Eigen3 REQUIRED) 7 | find_package(OpenCV 3 QUIET) 8 | if (NOT OpenCV_FOUND) 9 | find_package(OpenCV 4 REQUIRED) 10 | endif () 11 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 12 | find_package(Ceres REQUIRED) 13 | message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) 14 | 15 | # We need c++14 for ROS2, thus just require it for everybody 16 | # NOTE: To future self, hope this isn't an issue... 17 | set(CMAKE_CXX_STANDARD 14) 18 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 19 | set(CMAKE_CXX_EXTENSIONS OFF) 20 | 21 | # Enable compile optimizations 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 23 | 24 | # Enable debug flags (use if you want to debug in gdb) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -fno-omit-frame-pointer") 26 | 27 | # Find ROS build system 28 | find_package(catkin REQUIRED COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init ov_eval) 29 | 30 | # Describe ROS project 31 | catkin_package( 32 | CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init ov_eval 33 | INCLUDE_DIRS src/ 34 | LIBRARIES ov_plane_lib 35 | ) 36 | add_definitions(-DROS_AVAILABLE=1) 37 | 38 | # Include our header files 39 | include_directories( 40 | src 41 | ${EIGEN3_INCLUDE_DIR} 42 | ${Boost_INCLUDE_DIRS} 43 | ${CERES_INCLUDE_DIRS} 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | # Set link libraries used by all binaries 48 | list(APPEND thirdparty_libraries 49 | ${Boost_LIBRARIES} 50 | ${OpenCV_LIBRARIES} 51 | ${CERES_LIBRARIES} 52 | ${catkin_LIBRARIES} 53 | ) 54 | 55 | ################################################## 56 | # Make the shared library 57 | ################################################## 58 | 59 | # thirdparty ikd library! 60 | file(GLOB_RECURSE source_ikd ../thirdparty/ikd/ikd_tree.cpp) 61 | add_library(thirdparty_ikd SHARED ${source_ikd}) 62 | target_link_libraries(thirdparty_ikd ${thirdparty_libraries}) 63 | set_target_properties(thirdparty_ikd PROPERTIES LINKER_LANGUAGE CXX) 64 | target_include_directories(thirdparty_ikd PUBLIC ../thirdparty/ikd/) 65 | list(APPEND thirdparty_libraries thirdparty_ikd) 66 | 67 | # thirdparty render library! 68 | file(GLOB_RECURSE source_render ../thirdparty/render/render_model.cpp) 69 | add_library(thirdparty_render SHARED ${source_render}) 70 | target_link_libraries(thirdparty_render ${thirdparty_libraries}) 71 | set_target_properties(thirdparty_render PROPERTIES LINKER_LANGUAGE CXX) 72 | target_include_directories(thirdparty_render PUBLIC ../thirdparty/render/) 73 | list(APPEND thirdparty_libraries thirdparty_render) 74 | 75 | # thirdparty cdt library! 76 | file(GLOB_RECURSE source_cdt ../thirdparty/cdt/CDT.hpp ../thirdparty/cdt/CDTUtils.hpp ../thirdparty/cdt/remove_at.hpp ../thirdparty/cdt/Triangulation.hpp) 77 | add_library(thirdparty_cdt SHARED ${source_cdt}) 78 | target_link_libraries(thirdparty_cdt ${thirdparty_libraries}) 79 | set_target_properties(thirdparty_cdt PROPERTIES LINKER_LANGUAGE CXX) 80 | target_include_directories(thirdparty_cdt PUBLIC ../thirdparty/cdt/) 81 | list(APPEND thirdparty_libraries thirdparty_cdt) 82 | 83 | # main source code now 84 | list(APPEND LIBRARY_SOURCES 85 | src/ceres/Factor_PointOnPlane.cpp 86 | src/core/VioManager.cpp 87 | src/core/VioManagerHelper.cpp 88 | src/sim/Simulator.cpp 89 | src/state/State.cpp 90 | src/state/StateHelper.cpp 91 | src/state/Propagator.cpp 92 | src/track_plane/TrackPlane.cpp 93 | src/track_plane/PlaneFitting.cpp 94 | src/update/UpdaterHelper.cpp 95 | src/update/UpdaterMSCKF.cpp 96 | src/update/UpdaterPlane.cpp 97 | src/update/UpdaterSLAM.cpp 98 | src/update/UpdaterZeroVelocity.cpp 99 | src/ros/ROS1Visualizer.cpp 100 | src/ros/ROSVisualizerHelper.cpp 101 | ) 102 | file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") 103 | add_library(ov_plane_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) 104 | target_link_libraries(ov_plane_lib ${thirdparty_libraries}) 105 | target_include_directories(ov_plane_lib PUBLIC src/) 106 | install(TARGETS ov_plane_lib 107 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 108 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 109 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 110 | ) 111 | install(DIRECTORY src/ 112 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 113 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 114 | ) 115 | 116 | ################################################## 117 | # Make binary files! 118 | ################################################## 119 | 120 | 121 | add_executable(ros1_serial_msckf src/ros1_serial_msckf.cpp) 122 | target_link_libraries(ros1_serial_msckf ov_plane_lib ${thirdparty_libraries}) 123 | install(TARGETS ros1_serial_msckf 124 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 125 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 126 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 127 | ) 128 | 129 | add_executable(run_simulation src/run_simulation.cpp) 130 | target_link_libraries(run_simulation ov_plane_lib ${thirdparty_libraries}) 131 | install(TARGETS run_simulation 132 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 133 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 134 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 135 | ) 136 | 137 | 138 | add_executable(timing_custom src/timing_custom.cpp) 139 | target_link_libraries(timing_custom ov_plane_lib ${thirdparty_libraries}) 140 | install(TARGETS timing_custom 141 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 142 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 143 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | ) 145 | 146 | ################################################## 147 | # Launch files! 148 | ################################################## 149 | 150 | install(DIRECTORY launch/ 151 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 152 | ) 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Monocular Visual-Inertial Odometry with Planar Regularities 2 | 3 | 4 | [![](data/resources/readme.png)](https://www.youtube.com/watch?v=bec7LbYaOS8) 5 | 6 | [![ROS 1 Workflow](https://github.com/rpng/ov_plane/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/ov_plane/actions/workflows/build_ros1.yml) 7 | 8 | This is a real-time monocular visual-inertial odometry (VIO) system leverage environmental **planes** within a multi-state constraint Kalman filter (MSCKF) framework. 9 | At the core of our method is an efficient robust monocular-based plane detection algorithm which does not require additional sensing modalities such as a stereo, depth camera or neural network. 10 | The project is built on top of [OpenVINS](https://github.com/rpng/open_vins) and thus shares similar tooling and architecture. 11 | This plane detection and tracking algorithm enables real-time regularization of point features to environmental planes which are either maintained in the state vector as long-lived planes, or marginalized for efficiency. 12 | Planar regularities are applied to both in-state SLAM and out-of-state MSCKF point features, enabling long-term point-to-plane loop-closures due to the large spacial volume of planes. 13 | 14 | * Github project page - https://github.com/rpng/ov_plane 15 | * Demonstration video - https://www.youtube.com/watch?v=bec7LbYaOS8 16 | * Publication reference: https://chuchuchen.net/downloads/papers/Chen2023ICRA.pdf 17 | 18 | 19 | 20 | ## Installation and Building 21 | 22 | You will need to ensure you have installed OpenCV 3 or 4, Eigen3, Ceres Solver, and a valid ROS 1 installation. 23 | Please additionally refer to the OpenVINS [Installation Guide](https://docs.openvins.com/gs-installing.html) as dependencies are shared with it. 24 | For Ubuntu linux-based system the system dependencies are (in addition to [ROS 1](https://docs.openvins.com/gs-installing.html#gs-install-ros-1)): 25 | 26 | ```bash 27 | sudo apt-get install libeigen3-dev libboost-all-dev libceres-dev 28 | ``` 29 | 30 | We now setup a catkin workspace with both OpenVINS and the ov_plane project. 31 | Specifically, we can clone both projects: 32 | 33 | ```bash 34 | mkdir -p ~/workspace/catkin_ws_plane/src/ 35 | cd ~/workspace/catkin_ws_plane/src/ 36 | git clone https://github.com/rpng/open_vins/ 37 | git clone https://github.com/rpng/ov_plane/ 38 | cd open_vins/ 39 | git checkout 74a63cf758f595cc280f2165e5a6576e7ef8521d # last tested OpenVINS commit 40 | cd ~/workspace/catkin_ws_plane/ 41 | catkin build 42 | source devel/setup.bash 43 | ``` 44 | 45 | 46 | ## Running - Simulation 47 | 48 | We extended the OpenVINS simulator to simulated environmental planes which contain environmental features. 49 | A certain number of points are simulated as being planar and "free" non-planar per frame. 50 | This simulator leverages a C++ implementation of our [lips](https://github.com/rpng/lips) simulator. 51 | 52 | ```bash 53 | roslaunch ov_plane simulation.launch 54 | ``` 55 | 56 | To generate simulation results, one inspect the [results/run_simulation.sh](results/run_simulation.sh) script to reproduce the results of the paper. 57 | To evaluate the results relative to the groundtruth please refer to [ov_eval](https://docs.openvins.com/namespaceov__eval.html) of OpenVINS. 58 | The code has changed since publication to upgrade to the newest OpenVINS, but the results should still be representative. 59 | 60 | 61 | ## Running - Real-World Datasets 62 | 63 | We support both the [The EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) and our released [RPNG AR Table Dataset](https://github.com/rpng/ar_table_dataset) datasets. 64 | To run on either real-world dataset, use one of the following commands: 65 | 66 | ```bash 67 | roslaunch ov_plane serial.launch config:=euroc_mav dataset:=V1_01_easy bag:= 68 | roslaunch ov_plane serial.launch config:=rpng_plane dataset:=table_01 bag:= 69 | ``` 70 | 71 | Users can reproduce the results presented in our publication with the scripts provided in the [results/](results/) folder. 72 | We additionally provide the script used to compute the evaluation, which are simple calls to [ov_eval](https://docs.openvins.com/namespaceov__eval.html) of OpenVINS. 73 | The groundtruth "truths" files can be found in [ov_data](https://github.com/rpng/open_vins/tree/master/ov_data). 74 | Please note the project has been updated since the time of publication and thus will cause slight variations in performance relative to the paper 75 | 76 | ```bash 77 | ./src/ov_plane/results/run_ros_eth.sh # Euroc MAV 78 | ./src/ov_plane/results/run_ros_rpng_plane.sh # RPNG Indoor AR Table 79 | ./src/ov_plane/results/run_ros_tracking.sh # Tracking statistics 80 | ``` 81 | 82 | 83 | 84 | ## Acknowledgements 85 | 86 | This project additionally leveraged the following wonderful libraries. 87 | - [CDT](https://github.com/artem-ogre/CDT) - For generating Delaunay triangulations 88 | - [ikd-tree](https://github.com/hku-mars/ikd-Tree) - For KNN searching without requiring ICP 89 | - [tinyrender](https://github.com/ssloy/tinyrenderer) - For loading models for the AR demo 90 | 91 | 92 | 93 | ## Credit / Licensing 94 | 95 | This code was written by the [Robot Perception and Navigation Group (RPNG)](https://sites.udel.edu/robot/) at the 96 | University of Delaware. If you have any issues with the code please open an issue on our github page with relevant 97 | implementation details and references. For researchers that have leveraged or compared to this work, please cite the 98 | following: 99 | 100 | 101 | ```bibtex 102 | @Conference{Chen2023ICRA, 103 | Title = {Monocular Visual-Inertial Odometry with Planar Regularities}, 104 | Author = {Chuchu Chen and Patrick Geneva and Yuxiang Peng and Woosik Lee and Guoquan Huang}, 105 | Booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, 106 | Year = {2023}, 107 | Address = {London, UK}, 108 | Url = {\url{https://github.com/rpng/ov_plane}} 109 | } 110 | ``` 111 | 112 | ```bibtex 113 | @Conference{Geneva2020ICRA, 114 | Title = {{OpenVINS}: A Research Platform for Visual-Inertial Estimation}, 115 | Author = {Patrick Geneva and Kevin Eckenhoff and Woosik Lee and Yulin Yang and Guoquan Huang}, 116 | Booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, 117 | Year = {2020}, 118 | Address = {Paris, France}, 119 | Url = {\url{https://github.com/rpng/open_vins}} 120 | } 121 | ``` 122 | 123 | The codebase and documentation is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt). 124 | You must preserve the copyright and license notices in your derivative work and make available the complete source code with modifications under the same license ([see this](https://choosealicense.com/licenses/gpl-3.0/); this is not legal advice). 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /ov_plane/src/run_simulation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #include 28 | #include 29 | 30 | #include "core/VioManager.h" 31 | #include "sim/Simulator.h" 32 | #include "state/State.h" 33 | #include "utils/colors.h" 34 | #include "utils/dataset_reader.h" 35 | #include "utils/print.h" 36 | #include "utils/sensor_data.h" 37 | 38 | #if ROS_AVAILABLE == 1 39 | #include "ros/ROS1Visualizer.h" 40 | #include 41 | #endif 42 | 43 | using namespace ov_plane; 44 | 45 | std::shared_ptr sim; 46 | std::shared_ptr sys; 47 | #if ROS_AVAILABLE == 1 48 | std::shared_ptr viz; 49 | #endif 50 | 51 | // Define the function to be called when ctrl-c (SIGINT) is sent to process 52 | void signal_callback_handler(int signum) { std::exit(signum); } 53 | 54 | // Main function 55 | int main(int argc, char **argv) { 56 | 57 | // Ensure we have a path, if the user passes it then we should use it 58 | std::string config_path = "unset_path_to_config.yaml"; 59 | if (argc > 1) { 60 | config_path = argv[1]; 61 | } 62 | 63 | #if ROS_AVAILABLE == 1 64 | // Launch our ros node 65 | ros::init(argc, argv, "run_simulation"); 66 | auto nh = std::make_shared("~"); 67 | nh->param("config_path", config_path, config_path); 68 | #endif 69 | 70 | // Load the config 71 | auto parser = std::make_shared(config_path); 72 | #if ROS_AVAILABLE == 1 73 | parser->set_node_handler(nh); 74 | #endif 75 | 76 | // Verbosity 77 | std::string verbosity = "INFO"; 78 | parser->parse_config("verbosity", verbosity); 79 | ov_core::Printer::setPrintLevel(verbosity); 80 | 81 | // Create our VIO system 82 | VioManagerOptions params; 83 | params.print_and_load(parser); 84 | params.print_and_load_simulation(parser); 85 | params.num_opencv_threads = 0; // for repeatability 86 | params.use_multi_threading_pubs = false; 87 | params.use_multi_threading_subs = false; 88 | sim = std::make_shared(params); 89 | sys = std::make_shared(params); 90 | #if ROS_AVAILABLE == 1 91 | viz = std::make_shared(nh, sys, sim); 92 | #endif 93 | 94 | // Ensure we read in all parameters required 95 | if (!parser->successful()) { 96 | PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); 97 | std::exit(EXIT_FAILURE); 98 | } 99 | 100 | //=================================================================================== 101 | //=================================================================================== 102 | //=================================================================================== 103 | 104 | // Get initial state 105 | // NOTE: we are getting it at the *next* timestep so we get the first IMU message 106 | double next_imu_time = sim->current_timestamp() + 1.0 / params.sim_freq_imu; 107 | Eigen::Matrix imustate; 108 | bool success = sim->get_state(next_imu_time, imustate); 109 | if (!success) { 110 | PRINT_ERROR(RED "[SIM]: Could not initialize the filter to the first state\n" RESET); 111 | PRINT_ERROR(RED "[SIM]: Did the simulator load properly???\n" RESET); 112 | std::exit(EXIT_FAILURE); 113 | } 114 | 115 | // Since the state time is in the camera frame of reference 116 | // Subtract out the imu to camera time offset 117 | imustate(0, 0) -= sim->get_true_parameters().calib_camimu_dt; 118 | 119 | // Initialize our filter with the groundtruth 120 | sys->initialize_with_gt(imustate); 121 | 122 | // Record the groundtruth point features 123 | for (auto const &tmp : sim->get_map()) { 124 | // TrackSIM adds 1 to sim id and for each aruco we have 4 points 125 | size_t id = tmp.first + 4 * params.state_options.max_aruco_features + 1; 126 | sys->get_state()->_true_features.insert({id, tmp.second.block(0, 0, 3, 1)}); 127 | } 128 | 129 | // Record the groundtruth plane features 130 | for (auto const &tmp : sim->get_planes()) { 131 | sys->get_state()->_true_planes.insert({tmp.plane_id, tmp.cp()}); 132 | } 133 | 134 | //=================================================================================== 135 | //=================================================================================== 136 | //=================================================================================== 137 | 138 | // Buffer our camera image 139 | double buffer_timecam = -1; 140 | std::vector buffer_camids; 141 | std::vector>> buffer_feats; 142 | 143 | // Step through the rosbag 144 | #if ROS_AVAILABLE == 1 145 | while (sim->ok() && ros::ok()) { 146 | #else 147 | signal(SIGINT, signal_callback_handler); 148 | while (sim->ok()) { 149 | #endif 150 | 151 | // IMU: get the next simulated IMU measurement if we have it 152 | ov_core::ImuData message_imu; 153 | bool hasimu = sim->get_next_imu(message_imu.timestamp, message_imu.wm, message_imu.am); 154 | if (hasimu) { 155 | sys->feed_measurement_imu(message_imu); 156 | } 157 | 158 | // CAM: get the next simulated camera uv measurements if we have them 159 | double time_cam; 160 | std::vector camids; 161 | std::vector>> feats; 162 | bool hascam = sim->get_next_cam(time_cam, camids, feats); 163 | if (hascam) { 164 | if (buffer_timecam != -1) { 165 | #if ROS_AVAILABLE == 1 166 | viz->visualize_odometry(buffer_timecam); 167 | #endif 168 | sys->feed_measurement_simulation(buffer_timecam, buffer_camids, buffer_feats); 169 | #if ROS_AVAILABLE == 1 170 | viz->visualize(); 171 | #endif 172 | } 173 | buffer_timecam = time_cam; 174 | buffer_camids = camids; 175 | buffer_feats = feats; 176 | } 177 | } 178 | 179 | // Final visualization 180 | #if ROS_AVAILABLE == 1 181 | viz->visualize_final(); 182 | ros::shutdown(); 183 | #endif 184 | 185 | // Done! 186 | return EXIT_SUCCESS; 187 | } 188 | -------------------------------------------------------------------------------- /ov_plane/src/update/UpdaterHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_UPDATER_HELPER_H 28 | #define OV_PLANE_UPDATER_HELPER_H 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "types/LandmarkRepresentation.h" 35 | 36 | namespace ov_type { 37 | class Type; 38 | } // namespace ov_type 39 | 40 | namespace ov_plane { 41 | 42 | class State; 43 | 44 | /** 45 | * @brief Class that has helper functions for our updaters. 46 | * 47 | * Can compute the Jacobian for a single feature representation. 48 | * This will create the Jacobian based on what representation our state is in. 49 | * If we are using the anchor representation then we also have additional Jacobians in respect to the anchor state. 50 | * Also has functions such as nullspace projection and full jacobian construction. 51 | * For derivations look at @ref update-feat page which has detailed equations. 52 | * 53 | * If a feature has been marked as being "on a plane" then we will append constraint measurements to it. 54 | * Thus, the MSCKF, SLAM, and PLANE init updaters will all have bearing and constraint Jacobians stacked. 55 | * A feature will add a plane constraint if it has a non-zero planeid value. 56 | */ 57 | class UpdaterHelper { 58 | public: 59 | /** 60 | * @brief Feature object that our UpdaterHelper leverages, has all measurements and means 61 | */ 62 | struct UpdaterHelperFeature { 63 | 64 | /// Unique ID of this feature 65 | size_t featid; 66 | 67 | /// UV coordinates that this feature has been seen from (mapped by camera ID) 68 | std::unordered_map> uvs; 69 | 70 | /// UV normalized coordinates that this feature has been seen from (mapped by camera ID) 71 | std::unordered_map> uvs_norm; 72 | 73 | /// Timestamps of each UV measurement (mapped by camera ID) 74 | std::unordered_map> timestamps; 75 | 76 | /// What representation our feature is in 77 | ov_type::LandmarkRepresentation::Representation feat_representation; 78 | 79 | /// What camera ID our pose is anchored in!! By default the first measurement is the anchor. 80 | int anchor_cam_id = -1; 81 | 82 | /// Timestamp of anchor clone 83 | double anchor_clone_timestamp = -1; 84 | 85 | /// Triangulated position of this feature, in the anchor frame 86 | Eigen::Vector3d p_FinA; 87 | 88 | /// Triangulated position of this feature, in the anchor frame first estimate 89 | Eigen::Vector3d p_FinA_fej; 90 | 91 | /// Triangulated position of this feature, in the global frame 92 | Eigen::Vector3d p_FinG; 93 | 94 | /// Triangulated position of this feature, in the global frame first estimate 95 | Eigen::Vector3d p_FinG_fej; 96 | 97 | /// Unique ID of the plane this feature is on (0 is not on a plane) 98 | size_t planeid = 0; 99 | 100 | /// Closest-point feature representation of plane this feature is on 101 | Eigen::Vector3d cp_FinG; 102 | 103 | /// Closest-point feature representation of plane this feature is on first estimate 104 | Eigen::Vector3d cp_FinG_fej; 105 | }; 106 | 107 | /** 108 | * @brief This gets the feature and state Jacobian in respect to the feature representation 109 | * 110 | * @param[in] state State of the filter system 111 | * @param[in] feature Feature we want to get Jacobians of (must have feature means) 112 | * @param[out] H_f Jacobians in respect to the feature error state (will be either 3x3 or 3x1 for single depth) 113 | * @param[out] H_x Extra Jacobians in respect to the state (for example anchored pose) 114 | * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose) 115 | */ 116 | static void get_feature_jacobian_representation(std::shared_ptr state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f, 117 | std::vector &H_x, std::vector> &x_order); 118 | 119 | /** 120 | * @brief Will construct the "stacked" Jacobians for a single feature from all its measurements 121 | * 122 | * @param[in] state State of the filter system 123 | * @param[in] feature Feature we want to get Jacobians of (must have feature means) 124 | * @param[in] sigma_px Sigma of feature observation 125 | * @param[in] sigma_c Point-on-plane constraint 126 | * @param[out] H_f Jacobians in respect to the feature error state 127 | * @param[out] H_x Extra Jacobians in respect to the state (for example anchored pose) 128 | * @param[out] res Measurement residual for this feature 129 | * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose) 130 | */ 131 | static void get_feature_jacobian_full(std::shared_ptr state, UpdaterHelperFeature &feature, double sigma_px, double sigma_c, 132 | Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res, 133 | std::vector> &x_order); 134 | 135 | /** 136 | * @brief This will project the left nullspace of H_f onto the linear system. 137 | * 138 | * Please see the @ref update-null for details on how this works. 139 | * This is the MSCKF nullspace projection which removes the dependency on the feature state. 140 | * Note that this is done **in place** so all matrices will be different after a function call. 141 | * 142 | * @param H_f Jacobian with nullspace we want to project onto the system [res = Hx*(x-xhat)+Hf(f-fhat)+n] 143 | * @param H_x State jacobian 144 | * @param res Measurement residual 145 | */ 146 | static void nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 147 | 148 | /** 149 | * @brief This will perform measurement compression 150 | * 151 | * Please see the @ref update-compress for details on how this works. 152 | * Note that this is done **in place** so all matrices will be different after a function call. 153 | * 154 | * @param H_x State jacobian 155 | * @param res Measurement residual 156 | */ 157 | static void measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 158 | }; 159 | 160 | } // namespace ov_plane 161 | 162 | #endif // OV_PLANE_UPDATER_HELPER_H -------------------------------------------------------------------------------- /config/sim/estimator_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 # need to specify the file type at the top! 2 | 3 | verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT 4 | 5 | use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) 6 | use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it 7 | use_rk4int: true # if rk4 integration should be used (overrides imu averaging) 8 | 9 | use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints 10 | max_cameras: 1 11 | 12 | calib_cam_extrinsics: true 13 | calib_cam_intrinsics: true 14 | calib_cam_timeoffset: true 15 | 16 | max_clones: 11 17 | max_slam: 50 18 | max_slam_in_update: 25 19 | max_msckf_in_update: 40 20 | dt_slam_delay: 2 21 | 22 | gravity_mag: 9.81 23 | 24 | feat_rep_msckf: "GLOBAL_3D" 25 | feat_rep_slam: "GLOBAL_3D" 26 | feat_rep_aruco: "GLOBAL_3D" 27 | 28 | # zero velocity update parameters we can use 29 | # we support either IMU-based or disparity detection. 30 | try_zupt: false 31 | zupt_chi2_multipler: 0 # set to 0 for only disp-based 32 | zupt_max_velocity: 0.1 33 | zupt_noise_multiplier: 50 34 | zupt_max_disparity: 0.5 # set to 0 for only imu-based 35 | zupt_only_at_beginning: true 36 | 37 | 38 | # ================================================================== 39 | # Plane-related parameters 40 | # ================================================================== 41 | 42 | use_plane_constraint: true # use point on plane constraints at all 43 | use_plane_constraint_msckf: true # use point on plane constraints for MSCKF points 44 | use_plane_constraint_slamu: true # use point on plane constraints for SLAM point update 45 | use_plane_constraint_slamd: true # use point on plane constraints for SLAM point delayed initialization 46 | 47 | use_plane_slam_feats: true # if we should insert planes into the state as SLAM features 48 | use_refine_plane_feat: true # if 3d feature estimates should be refined to the plane before update 49 | use_groundtruths: false # if groundtruth planes should be used (simulation only) 50 | 51 | sigma_constraint: 0.05 # sigma used for point-on-plane constraints 52 | const_init_multi: 5.00 53 | const_init_chi2: 1.00 54 | max_msckf_plane: 20 55 | 56 | sigma_plane_merge: 0.10 # sigma used when merging two CP planes 57 | plane_merge_chi2: 1.00 # chi2 to reject bad merges 58 | plane_merge_deg_max: 1.00 # max norm degree difference 59 | 60 | plane_max_tri_side_px: 200 # max pixels a triangle edge can be 61 | plane_max_norm_count: 8 # num tri norms to average 62 | plane_max_norm_avg_max: 20.0 # max diff for norm average (degrees) 63 | plane_max_norm_avg_var: 20.0 # max variance for norm average (degrees) 64 | plane_max_norm_deg: 25.0 # pair-wise comparison 65 | plane_max_dist_between_z: 0.30 # distance in vertical direction to plane 66 | plane_max_pairwise_px: 100 # max pixels pairwise comparison can be 67 | plane_min_norms: 5 # min norms to do pair-wise comparison 68 | plane_check_old_feats: true # update plane ids of features matched in previous frame 69 | 70 | plane_filter_num_feat: 4 # we only perform spacial filtering if more than this feat count 71 | plane_filter_z_thresh: 5.0 # z-test threshold (probably should be > 1.0) https://www.simplypsychology.org/z-table.html 72 | 73 | plane_feat_min_obs: 2 74 | plane_min_dist: 0.10 75 | plane_max_dist: 60.0 76 | plane_max_cond_number: 20000 77 | 78 | plane_collect_init_feats: true # should we use extra MSCKF features in our plane init 79 | plane_collect_msckf_feats: true # should we use long-MSCKF features in our plane init 80 | plane_init_min_feat: 10 # min number of on-plane points required to init a plane 81 | plane_init_max_cond: 50.0 # max condition number for plane fitting when init a plane 82 | plane_msckf_min_feat: 5 # min number of features to use plane constraint (MSCKF update) 83 | plane_msckf_max_cond: 50.0 # max condition number for plane fitting (MSCKF update) 84 | 85 | 86 | # ================================================================== 87 | # ================================================================== 88 | 89 | init_window_time: 2.0 90 | init_imu_thresh: 1.0 91 | init_max_disparity: 1.5 92 | init_max_features: 15 93 | 94 | init_dyn_use: true # if dynamic initialization should be used 95 | init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) 96 | init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) 97 | init_dyn_mle_max_time: 0.5 # how many seconds the MLE should be completed in 98 | init_dyn_mle_max_threads: 6 # how many threads the MLE should use 99 | init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) 100 | init_dyn_min_deg: 15.0 # orientation change needed to try to init 101 | 102 | init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by 103 | init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by 104 | init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by 105 | init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by 106 | init_dyn_min_rec_cond: 1e-15 # reciprocal condition number thresh for info inversion 107 | 108 | init_dyn_bias_g: [0.0, 0.0, 0.0] # initial gyroscope bias guess 109 | init_dyn_bias_a: [0.0, 0.0, 0.0] # initial accelerometer bias guess 110 | 111 | # ================================================================== 112 | # ================================================================== 113 | 114 | record_timing_information: false 115 | record_timing_filepath: "/tmp/traj_timing.txt" 116 | 117 | record_plane_tracking_information: false 118 | record_plane_tracking_filepath: "/tmp/traj_tracking.txt" 119 | 120 | save_total_state: false 121 | filepath_est: "/tmp/ov_estimate.txt" 122 | filepath_std: "/tmp/ov_estimate_std.txt" 123 | filepath_gt: "/tmp/ov_groundtruth.txt" 124 | 125 | # ================================================================== 126 | # ================================================================== 127 | 128 | # our front-end feature tracking parameters 129 | # we have a KLT and descriptor based (KLT is better implemented...) 130 | use_klt: true 131 | num_pts: 250 132 | fast_threshold: 15 133 | grid_x: 20 134 | grid_y: 20 135 | min_px_dist: 15 136 | knn_ratio: 0.70 137 | track_frequency: 21.0 138 | downsample_cameras: false 139 | num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads 140 | histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE 141 | 142 | # aruco tag tracker for the system 143 | # DICT_6X6_1000 from https://chev.me/arucogen/ 144 | use_aruco: false 145 | num_aruco: 1024 146 | downsize_aruco: true 147 | 148 | # ================================================================== 149 | # ================================================================== 150 | 151 | # camera noises and chi-squared threshold multipliers 152 | up_msckf_sigma_px: 1 153 | up_msckf_chi2_multipler: 99999 154 | up_slam_sigma_px: 1 155 | up_slam_chi2_multipler: 99999 156 | up_aruco_sigma_px: 1 157 | up_aruco_chi2_multipler: 99999 158 | 159 | # masks for our images 160 | use_mask: false 161 | 162 | # imu and camera spacial-temporal 163 | # imu config should also have the correct noise values 164 | relative_config_imu: "kalibr_imu_chain.yaml" 165 | relative_config_imucam: "kalibr_imucam_chain.yaml" 166 | 167 | 168 | # ================================================================== 169 | # ================================================================== 170 | 171 | 172 | sim_seed_state_init: 0 173 | sim_seed_preturb: 0 174 | sim_seed_measurements: 0 175 | sim_do_perturbation: false 176 | sim_traj_path: "src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" 177 | sim_distance_threshold: 1.2 178 | sim_freq_cam: 10 179 | sim_freq_imu: 400 180 | sim_min_feature_gen_dist: 2.0 181 | sim_max_feature_gen_dist: 5.0 182 | num_pts_plane: 250 183 | 184 | rand_init_ori_seed: 0 185 | -------------------------------------------------------------------------------- /thirdparty/cdt/CDTUtils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * Utilities and helpers - implementation 4 | */ 5 | 6 | #include "CDTUtils.h" 7 | 8 | #include "predicates.h" // robust predicates: orient, in-circle 9 | 10 | #include 11 | 12 | namespace CDT 13 | { 14 | 15 | //***************************************************************************** 16 | // V2d 17 | //***************************************************************************** 18 | template 19 | V2d V2d::make(const T x, const T y) 20 | { 21 | V2d out = {x, y}; 22 | return out; 23 | } 24 | 25 | //***************************************************************************** 26 | // Box2d 27 | //***************************************************************************** 28 | template 29 | Box2d envelopBox(const std::vector >& vertices) 30 | { 31 | return envelopBox( 32 | vertices.begin(), vertices.end(), getX_V2d, getY_V2d); 33 | } 34 | 35 | //***************************************************************************** 36 | // Edge 37 | //***************************************************************************** 38 | CDT_INLINE_IF_HEADER_ONLY Edge::Edge(VertInd iV1, VertInd iV2) 39 | : m_vertices( 40 | iV1 < iV2 ? std::make_pair(iV1, iV2) : std::make_pair(iV2, iV1)) 41 | {} 42 | 43 | CDT_INLINE_IF_HEADER_ONLY bool Edge::operator==(const Edge& other) const 44 | { 45 | return m_vertices == other.m_vertices; 46 | } 47 | 48 | CDT_INLINE_IF_HEADER_ONLY bool Edge::operator!=(const Edge& other) const 49 | { 50 | return !(this->operator==(other)); 51 | } 52 | 53 | CDT_INLINE_IF_HEADER_ONLY VertInd Edge::v1() const 54 | { 55 | return m_vertices.first; 56 | } 57 | 58 | CDT_INLINE_IF_HEADER_ONLY VertInd Edge::v2() const 59 | { 60 | return m_vertices.second; 61 | } 62 | 63 | CDT_INLINE_IF_HEADER_ONLY const std::pair& Edge::verts() const 64 | { 65 | return m_vertices; 66 | } 67 | 68 | //***************************************************************************** 69 | // Utility functions 70 | //***************************************************************************** 71 | CDT_INLINE_IF_HEADER_ONLY Index ccw(Index i) 72 | { 73 | return Index((i + 1) % 3); 74 | } 75 | 76 | CDT_INLINE_IF_HEADER_ONLY Index cw(Index i) 77 | { 78 | return Index((i + 2) % 3); 79 | } 80 | 81 | CDT_INLINE_IF_HEADER_ONLY bool isOnEdge(const PtTriLocation::Enum location) 82 | { 83 | return location > PtTriLocation::Outside; 84 | } 85 | 86 | CDT_INLINE_IF_HEADER_ONLY Index edgeNeighbor(const PtTriLocation::Enum location) 87 | { 88 | assert(location >= PtTriLocation::OnEdge1); 89 | return static_cast(location - PtTriLocation::OnEdge1); 90 | } 91 | 92 | template 93 | T orient2D(const V2d& p, const V2d& v1, const V2d& v2) 94 | { 95 | return predicates::adaptive::orient2d(v1.x, v1.y, v2.x, v2.y, p.x, p.y); 96 | } 97 | 98 | template 99 | PtLineLocation::Enum locatePointLine( 100 | const V2d& p, 101 | const V2d& v1, 102 | const V2d& v2, 103 | const T orientationTolerance) 104 | { 105 | return classifyOrientation(orient2D(p, v1, v2), orientationTolerance); 106 | } 107 | 108 | template 109 | PtLineLocation::Enum 110 | classifyOrientation(const T orientation, const T orientationTolerance) 111 | { 112 | if(orientation < -orientationTolerance) 113 | return PtLineLocation::Right; 114 | if(orientation > orientationTolerance) 115 | return PtLineLocation::Left; 116 | return PtLineLocation::OnLine; 117 | } 118 | 119 | template 120 | PtTriLocation::Enum locatePointTriangle( 121 | const V2d& p, 122 | const V2d& v1, 123 | const V2d& v2, 124 | const V2d& v3) 125 | { 126 | using namespace predicates::adaptive; 127 | PtTriLocation::Enum result = PtTriLocation::Inside; 128 | PtLineLocation::Enum edgeCheck = locatePointLine(p, v1, v2); 129 | if(edgeCheck == PtLineLocation::Right) 130 | return PtTriLocation::Outside; 131 | if(edgeCheck == PtLineLocation::OnLine) 132 | result = PtTriLocation::OnEdge1; 133 | edgeCheck = locatePointLine(p, v2, v3); 134 | if(edgeCheck == PtLineLocation::Right) 135 | return PtTriLocation::Outside; 136 | if(edgeCheck == PtLineLocation::OnLine) 137 | result = PtTriLocation::OnEdge2; 138 | edgeCheck = locatePointLine(p, v3, v1); 139 | if(edgeCheck == PtLineLocation::Right) 140 | return PtTriLocation::Outside; 141 | if(edgeCheck == PtLineLocation::OnLine) 142 | result = PtTriLocation::OnEdge3; 143 | return result; 144 | } 145 | 146 | CDT_INLINE_IF_HEADER_ONLY Index opoNbr(const Index vertIndex) 147 | { 148 | if(vertIndex == Index(0)) 149 | return Index(1); 150 | if(vertIndex == Index(1)) 151 | return Index(2); 152 | if(vertIndex == Index(2)) 153 | return Index(0); 154 | throw std::runtime_error("Invalid vertex index"); 155 | } 156 | 157 | CDT_INLINE_IF_HEADER_ONLY Index opoVrt(const Index neighborIndex) 158 | { 159 | if(neighborIndex == Index(0)) 160 | return Index(2); 161 | if(neighborIndex == Index(1)) 162 | return Index(0); 163 | if(neighborIndex == Index(2)) 164 | return Index(1); 165 | throw std::runtime_error("Invalid neighbor index"); 166 | } 167 | 168 | CDT_INLINE_IF_HEADER_ONLY Index 169 | opposedTriangleInd(const Triangle& tri, const VertInd iVert) 170 | { 171 | for(Index vi = Index(0); vi < Index(3); ++vi) 172 | if(iVert == tri.vertices[vi]) 173 | return opoNbr(vi); 174 | throw std::runtime_error("Could not find opposed triangle index"); 175 | } 176 | 177 | CDT_INLINE_IF_HEADER_ONLY Index opposedTriangleInd( 178 | const Triangle& tri, 179 | const VertInd iVedge1, 180 | const VertInd iVedge2) 181 | { 182 | for(Index vi = Index(0); vi < Index(3); ++vi) 183 | { 184 | const VertInd iVert = tri.vertices[vi]; 185 | if(iVert != iVedge1 && iVert != iVedge2) 186 | return opoNbr(vi); 187 | } 188 | throw std::runtime_error("Could not find opposed-to-edge triangle index"); 189 | } 190 | 191 | CDT_INLINE_IF_HEADER_ONLY Index 192 | opposedVertexInd(const Triangle& tri, const TriInd iTopo) 193 | { 194 | for(Index ni = Index(0); ni < Index(3); ++ni) 195 | if(iTopo == tri.neighbors[ni]) 196 | return opoVrt(ni); 197 | throw std::runtime_error("Could not find opposed vertex index"); 198 | } 199 | 200 | CDT_INLINE_IF_HEADER_ONLY Index 201 | neighborInd(const Triangle& tri, const TriInd iTnbr) 202 | { 203 | for(Index ni = Index(0); ni < Index(3); ++ni) 204 | if(iTnbr == tri.neighbors[ni]) 205 | return ni; 206 | throw std::runtime_error("Could not find neighbor triangle index"); 207 | } 208 | 209 | CDT_INLINE_IF_HEADER_ONLY Index vertexInd(const Triangle& tri, const VertInd iV) 210 | { 211 | for(Index i = Index(0); i < Index(3); ++i) 212 | if(iV == tri.vertices[i]) 213 | return i; 214 | throw std::runtime_error("Could not find vertex index in triangle"); 215 | } 216 | 217 | CDT_INLINE_IF_HEADER_ONLY TriInd 218 | opposedTriangle(const Triangle& tri, const VertInd iVert) 219 | { 220 | return tri.neighbors[opposedTriangleInd(tri, iVert)]; 221 | } 222 | 223 | CDT_INLINE_IF_HEADER_ONLY VertInd 224 | opposedVertex(const Triangle& tri, const TriInd iTopo) 225 | { 226 | return tri.vertices[opposedVertexInd(tri, iTopo)]; 227 | } 228 | 229 | template 230 | bool isInCircumcircle( 231 | const V2d& p, 232 | const V2d& v1, 233 | const V2d& v2, 234 | const V2d& v3) 235 | { 236 | using namespace predicates::adaptive; 237 | return incircle(v1.x, v1.y, v2.x, v2.y, v3.x, v3.y, p.x, p.y) > T(0); 238 | } 239 | 240 | CDT_INLINE_IF_HEADER_ONLY 241 | bool verticesShareEdge(const TriIndVec& aTris, const TriIndVec& bTris) 242 | { 243 | for(TriIndVec::const_iterator it = aTris.begin(); it != aTris.end(); ++it) 244 | if(std::find(bTris.begin(), bTris.end(), *it) != bTris.end()) 245 | return true; 246 | return false; 247 | } 248 | 249 | template 250 | T distanceSquared(const T ax, const T ay, const T bx, const T by) 251 | { 252 | const T dx = bx - ax; 253 | const T dy = by - ay; 254 | return dx * dx + dy * dy; 255 | } 256 | 257 | template 258 | T distance(const T ax, const T ay, const T bx, const T by) 259 | { 260 | return std::sqrt(distanceSquared(ax, ay, bx, by)); 261 | } 262 | 263 | template 264 | T distance(const V2d& a, const V2d& b) 265 | { 266 | return distance(a.x, a.y, b.x, b.y); 267 | } 268 | 269 | template 270 | T distanceSquared(const V2d& a, const V2d& b) 271 | { 272 | return distanceSquared(a.x, a.y, b.x, b.y); 273 | } 274 | 275 | } // namespace CDT 276 | -------------------------------------------------------------------------------- /ov_plane/launch/simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 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 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /config/euroc_mav/estimator_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 # need to specify the file type at the top! 2 | 3 | verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT 4 | 5 | use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) 6 | use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it 7 | use_rk4int: true # if rk4 integration should be used (overrides imu averaging) 8 | 9 | use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs 10 | max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) 11 | 12 | calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI 13 | calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) 14 | calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized 15 | 16 | max_clones: 11 # how many clones in the sliding window 17 | max_slam: 0 # number of features in our state vector 18 | max_slam_in_update: 15 # update can be split into sequential updates of batches, how many in a batch 19 | max_msckf_in_update: 20 # how many MSCKF features to use in the update 20 | dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...) 21 | 22 | gravity_mag: 9.81 # magnitude of gravity in this location 23 | 24 | feat_rep_msckf: "GLOBAL_3D" 25 | feat_rep_slam: "GLOBAL_3D" 26 | feat_rep_aruco: "GLOBAL_3D" 27 | 28 | # zero velocity update parameters we can use 29 | # we support either IMU-based or disparity detection. 30 | try_zupt: false 31 | zupt_chi2_multipler: 0 # set to 0 for only disp-based 32 | zupt_max_velocity: 0.1 33 | zupt_noise_multiplier: 50 34 | zupt_max_disparity: 1.5 # set to 0 for only imu-based 35 | zupt_only_at_beginning: true 36 | 37 | 38 | # ================================================================== 39 | # Plane-related parameters 40 | # ================================================================== 41 | 42 | use_plane_constraint: true # use point on plane constraints at all 43 | use_plane_constraint_msckf: true # use point on plane constraints for MSCKF points 44 | use_plane_constraint_slamu: true # use point on plane constraints for SLAM point update 45 | use_plane_constraint_slamd: true # use point on plane constraints for SLAM point delayed initialization 46 | 47 | use_plane_slam_feats: true # if we should insert planes into the state as SLAM features 48 | use_refine_plane_feat: true # if 3d feature estimates should be refined to the plane before update 49 | use_groundtruths: false # if groundtruth planes should be used (simulation only) 50 | 51 | sigma_constraint: 0.01 52 | const_init_multi: 1.00 53 | const_init_chi2: 1.00 54 | max_msckf_plane: 20 55 | 56 | sigma_plane_merge: 0.01 # sigma used when merging two CP planes 57 | plane_merge_chi2: 0.75 # chi2 to reject bad merges 58 | plane_merge_deg_max: 1.00 # max norm degree difference 59 | 60 | plane_max_tri_side_px: 400 # max pixels a triangle edge can be 61 | plane_max_norm_count: 12 # num tri norms to average 62 | plane_max_norm_avg_max: 20.0 # max diff for norm average (degrees) 63 | plane_max_norm_avg_var: 15.0 # max variance for norm average (degrees) 64 | plane_max_norm_deg: 10.0 # pair-wise comparison 65 | plane_max_dist_between_z: 0.05 # distance in vertical direction to plane: 66 | plane_max_pairwise_px: 300 # max pixels pairwise comparison can be 67 | plane_min_norms: 5 # min norms to do pair-wise comparison 68 | plane_check_old_feats: true # update plane ids of features matched in previous frame 69 | 70 | plane_filter_num_feat: 8 # we only perform spacial filtering if more than this feat count 71 | plane_filter_z_thresh: 1.5 # z-test threshold (probably should be > 1.0) https://www.simplypsychology.org/z-table.html 72 | 73 | plane_feat_min_obs: 2 74 | plane_min_dist: 0.10 75 | plane_max_dist: 20.0 76 | plane_max_cond_number: 20000 77 | 78 | plane_collect_init_feats: true # should we use extra MSCKF features in our plane init 79 | plane_collect_msckf_feats: true # should we use long-MSCKF features in our plane init 80 | plane_init_min_feat: 10 # min number of on-plane points required to init a plane 81 | plane_init_max_cond: 50.0 # max condition number for plane fitting when init a plane 82 | plane_msckf_min_feat: 8 # min number of features to use plane constraint (MSCKF update) 83 | plane_msckf_max_cond: 50.0 # max condition number for plane fitting (MSCKF update) 84 | 85 | 86 | # ================================================================== 87 | # ================================================================== 88 | 89 | init_window_time: 2.0 # how many seconds to collect initialization information 90 | init_imu_thresh: 1.45 # threshold for variance of the accelerometer to detect a "jerk" in motion 91 | init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) 92 | init_max_features: 75 # how many features to track during initialization (saves on computation) 93 | 94 | init_dyn_use: false # if dynamic initialization should be used 95 | init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) 96 | init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) 97 | init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in 98 | init_dyn_mle_max_threads: 6 # how many threads the MLE should use 99 | init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) 100 | init_dyn_min_deg: 10.0 # orientation change needed to try to init 101 | 102 | init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by 103 | init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by 104 | init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by 105 | init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by 106 | init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion 107 | 108 | init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess 109 | init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess 110 | 111 | # ================================================================== 112 | # ================================================================== 113 | 114 | record_timing_information: false # if we want to record timing information of the method 115 | record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame 116 | 117 | record_plane_tracking_information: false # if we want to record plane tracking information 118 | record_plane_tracking_filepath: "/tmp/traj_tracking.txt" 119 | 120 | # if we want to save the simulation state and its diagional covariance 121 | # use this with rosrun ov_eval error_simulation 122 | save_total_state: false 123 | filepath_est: "/tmp/ov_estimate.txt" 124 | filepath_std: "/tmp/ov_estimate_std.txt" 125 | filepath_gt: "/tmp/ov_groundtruth.txt" 126 | 127 | # ================================================================== 128 | # ================================================================== 129 | 130 | # our front-end feature tracking parameters 131 | # we have a KLT and descriptor based (KLT is better implemented...) 132 | use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching 133 | num_pts: 200 # number of points (per camera) we will extract and try to track 134 | fast_threshold: 15 # threshold for fast extraction (warning: lower threshs can be expensive) 135 | grid_x: 12 # extraction sub-grid count for horizontal direction (uniform tracking) 136 | grid_y: 12 # extraction sub-grid count for vertical direction (uniform tracking) 137 | min_px_dist: 15 # distance between features (features near each other provide less information) 138 | knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches 139 | track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz) 140 | downsample_cameras: false # will downsample image in half if true 141 | num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads 142 | histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE 143 | 144 | # aruco tag tracker for the system 145 | # DICT_6X6_1000 from https://chev.me/arucogen/ 146 | use_aruco: false 147 | num_aruco: 1024 148 | downsize_aruco: true 149 | 150 | # ================================================================== 151 | # ================================================================== 152 | 153 | # camera noises and chi-squared threshold multipliers 154 | up_msckf_sigma_px: 1 155 | up_msckf_chi2_multipler: 1 156 | up_slam_sigma_px: 1 157 | up_slam_chi2_multipler: 1 158 | up_aruco_sigma_px: 1 159 | up_aruco_chi2_multipler: 1 160 | 161 | # masks for our images 162 | use_mask: false 163 | 164 | # imu and camera spacial-temporal 165 | # imu config should also have the correct noise values 166 | relative_config_imu: "kalibr_imu_chain.yaml" 167 | relative_config_imucam: "kalibr_imucam_chain.yaml" 168 | 169 | 170 | 171 | -------------------------------------------------------------------------------- /config/rpng_plane/estimator_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 # need to specify the file type at the top! 2 | 3 | verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT 4 | 5 | use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) 6 | use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it 7 | use_rk4int: true # if rk4 integration should be used (overrides imu averaging) 8 | 9 | use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs 10 | max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) 11 | 12 | calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI 13 | calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) 14 | calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized 15 | 16 | max_clones: 11 # how many clones in the sliding window 17 | max_slam: 0 # number of features in our state vector 18 | max_slam_in_update: 15 # update can be split into sequential updates of batches, how many in a batch 19 | max_msckf_in_update: 20 # how many MSCKF features to use in the update 20 | dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...) 21 | 22 | gravity_mag: 9.8065 # magnitude of gravity in this location 23 | 24 | feat_rep_msckf: "GLOBAL_3D" 25 | feat_rep_slam: "GLOBAL_3D" 26 | feat_rep_aruco: "GLOBAL_3D" 27 | 28 | # zero velocity update parameters we can use 29 | # we support either IMU-based or disparity detection. 30 | try_zupt: false 31 | zupt_chi2_multipler: 0 # set to 0 for only disp-based 32 | zupt_max_velocity: 0.1 33 | zupt_noise_multiplier: 50 34 | zupt_max_disparity: 1.5 # set to 0 for only imu-based 35 | zupt_only_at_beginning: true 36 | 37 | 38 | # ================================================================== 39 | # Plane-related parameters 40 | # ================================================================== 41 | 42 | use_plane_constraint: true # use point on plane constraints at all 43 | use_plane_constraint_msckf: true # use point on plane constraints for MSCKF points 44 | use_plane_constraint_slamu: true # use point on plane constraints for SLAM point update 45 | use_plane_constraint_slamd: true # use point on plane constraints for SLAM point delayed initialization 46 | 47 | use_plane_slam_feats: true # if we should insert planes into the state as SLAM features 48 | use_refine_plane_feat: true # if 3d feature estimates should be refined to the plane before update 49 | use_groundtruths: false # if groundtruth planes should be used (simulation only) 50 | 51 | sigma_constraint: 0.01 52 | const_init_multi: 1.00 53 | const_init_chi2: 1.00 54 | max_msckf_plane: 20 55 | 56 | sigma_plane_merge: 0.01 # sigma used when merging two CP planes 57 | plane_merge_chi2: 0.75 # chi2 to reject bad merges 58 | plane_merge_deg_max: 1.00 # max norm degree difference 59 | 60 | plane_max_tri_side_px: 300 # max pixels a triangle edge can be 61 | plane_max_norm_count: 12 # num tri norms to average 62 | plane_max_norm_avg_max: 15.0 # max diff for norm average (degrees) 63 | plane_max_norm_avg_var: 15.0 # max variance for norm average (degrees) 64 | plane_max_norm_deg: 7.0 # pair-wise comparison 65 | plane_max_dist_between_z: 0.10 # distance in vertical direction to plane 66 | plane_max_pairwise_px: 200 # max pixels pairwise comparison can be 67 | plane_min_norms: 5 # min norms to do pair-wise comparison 68 | plane_check_old_feats: true # update plane ids of features matched in previous frame 69 | 70 | plane_filter_num_feat: 8 # we only perform spacial filtering if more than this feat count 71 | plane_filter_z_thresh: 1.5 # z-test threshold (probably should be > 1.0) https://www.simplypsychology.org/z-table.html 72 | 73 | plane_feat_min_obs: 2 74 | plane_min_dist: 0.10 75 | plane_max_dist: 20.0 76 | plane_max_cond_number: 20000 77 | 78 | plane_collect_init_feats: true # should we use extra MSCKF features in our plane init 79 | plane_collect_msckf_feats: true # should we use long-MSCKF features in our plane init 80 | plane_init_min_feat: 10 # min number of on-plane points required to init a plane 81 | plane_init_max_cond: 50.0 # max condition number for plane fitting when init a plane 82 | plane_msckf_min_feat: 8 # min number of features to use plane constraint (MSCKF update) 83 | plane_msckf_max_cond: 50.0 # max condition number for plane fitting (MSCKF update) 84 | 85 | 86 | # ================================================================== 87 | # ================================================================== 88 | 89 | init_window_time: 2.0 # how many seconds to collect initialization information 90 | init_imu_thresh: 0.5 # threshold for variance of the accelerometer to detect a "jerk" in motion 91 | init_max_disparity: 4.0 # max disparity to consider the platform stationary (dependent on resolution) 92 | init_max_features: 75 # how many features to track during initialization (saves on computation) 93 | 94 | init_dyn_use: false # if dynamic initialization should be used 95 | init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) 96 | init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) 97 | init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in 98 | init_dyn_mle_max_threads: 6 # how many threads the MLE should use 99 | init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) 100 | init_dyn_min_deg: 10.0 # orientation change needed to try to init 101 | 102 | init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by 103 | init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by 104 | init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by 105 | init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by 106 | init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion 107 | 108 | init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess 109 | init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess 110 | 111 | # ================================================================== 112 | # ================================================================== 113 | 114 | record_timing_information: false # if we want to record timing information of the method 115 | record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame 116 | 117 | record_plane_tracking_information: false # if we want to record plane tracking information 118 | record_plane_tracking_filepath: "/tmp/traj_tracking.txt" 119 | 120 | # if we want to save the simulation state and its diagional covariance 121 | # use this with rosrun ov_eval error_simulation 122 | save_total_state: false 123 | filepath_est: "/tmp/ov_estimate.txt" 124 | filepath_std: "/tmp/ov_estimate_std.txt" 125 | filepath_gt: "/tmp/ov_groundtruth.txt" 126 | 127 | # ================================================================== 128 | # ================================================================== 129 | 130 | # our front-end feature tracking parameters 131 | # we have a KLT and descriptor based (KLT is better implemented...) 132 | use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching 133 | num_pts: 200 # number of points (per camera) we will extract and try to track 134 | fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive) 135 | grid_x: 12 # extraction sub-grid count for horizontal direction (uniform tracking) 136 | grid_y: 12 # extraction sub-grid count for vertical direction (uniform tracking) 137 | min_px_dist: 20 # distance between features (features near each other provide less information) 138 | knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches 139 | track_frequency: 31.0 # frequency we will perform feature tracking at (in frames per second / hertz) 140 | downsample_cameras: false # will downsample image in half if true 141 | num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads 142 | histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE 143 | 144 | # aruco tag tracker for the system 145 | # DICT_6X6_1000 from https://chev.me/arucogen/ 146 | use_aruco: false 147 | num_aruco: 1024 148 | downsize_aruco: true 149 | 150 | # ================================================================== 151 | # ================================================================== 152 | 153 | # camera noises and chi-squared threshold multipliers 154 | up_msckf_sigma_px: 1 155 | up_msckf_chi2_multipler: 1 156 | up_slam_sigma_px: 1 157 | up_slam_chi2_multipler: 1 158 | up_aruco_sigma_px: 1 159 | up_aruco_chi2_multipler: 1 160 | 161 | # masks for our images 162 | use_mask: false 163 | 164 | # imu and camera spacial-temporal 165 | # imu config should also have the correct noise values 166 | relative_config_imu: "kalibr_imu_chain.yaml" 167 | relative_config_imucam: "kalibr_imucam_chain.yaml" 168 | 169 | 170 | 171 | -------------------------------------------------------------------------------- /ov_plane/src/sim/Simulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_SIMULATOR_H 28 | #define OV_PLANE_SIMULATOR_H 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "SimPlane.h" 39 | #include "core/VioManagerOptions.h" 40 | 41 | namespace ov_core { 42 | class BsplineSE3; 43 | } // namespace ov_core 44 | 45 | namespace ov_plane { 46 | 47 | /** 48 | * @brief Master simulator class that generated visual-inertial measurements 49 | * 50 | * Given a trajectory this will generate a SE(3) @ref ov_core::BsplineSE3 for that trajectory. 51 | * This allows us to get the inertial measurement information at each timestep during this trajectory. 52 | * After creating the bspline we will generate an environmental feature map which will be used as our feature measurements. 53 | * This map will be projected into the frame at each timestep to get our "raw" uv measurements. 54 | * We inject bias and white noises into our inertial readings while adding our white noise to the uv measurements also. 55 | * The user should specify the sensor rates that they desire along with the seeds of the random number generators. 56 | * 57 | */ 58 | class Simulator { 59 | 60 | public: 61 | /** 62 | * @brief Default constructor, will load all configuration variables 63 | * @param params_ VioManager parameters. Should have already been loaded from cmd. 64 | */ 65 | Simulator(VioManagerOptions ¶ms_); 66 | 67 | /** 68 | * @brief Will get a set of perturbed parameters 69 | * @param gen_state Random number gen to use 70 | * @param params_ Parameters we will perturb 71 | */ 72 | static void perturb_parameters(std::mt19937 gen_state, VioManagerOptions ¶ms_); 73 | 74 | /** 75 | * @brief Returns if we are actively simulating 76 | * @return True if we still have simulation data 77 | */ 78 | bool ok() { return is_running; } 79 | 80 | /** 81 | * @brief Gets the timestamp we have simulated up too 82 | * @return Timestamp 83 | */ 84 | double current_timestamp() { return timestamp; } 85 | 86 | /** 87 | * @brief Get the simulation state at a specified timestep 88 | * @param desired_time Timestamp we want to get the state at 89 | * @param imustate State in the MSCKF ordering: [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel] 90 | * @return True if we have a state 91 | */ 92 | bool get_state(double desired_time, Eigen::Matrix &imustate); 93 | 94 | /** 95 | * @brief Gets the next inertial reading if we have one. 96 | * @param time_imu Time that this measurement occured at 97 | * @param wm Angular velocity measurement in the inertial frame 98 | * @param am Linear velocity in the inertial frame 99 | * @return True if we have a measurement 100 | */ 101 | bool get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vector3d &am); 102 | 103 | /** 104 | * @brief Gets the next inertial reading if we have one. 105 | * @param time_cam Time that this measurement occured at 106 | * @param camids Camera ids that the corresponding vectors match 107 | * @param feats Noisy uv measurements and ids for the returned time 108 | * @return True if we have a measurement 109 | */ 110 | bool get_next_cam(double &time_cam, std::vector &camids, std::vector>> &feats); 111 | 112 | /// Returns the true 3d map of features 113 | std::map get_map() { return featmap; } 114 | 115 | /// Returns the true 3d planes 116 | std::vector get_planes() { return planes; } 117 | 118 | /// Returns the true 3d map of features 119 | std::vector get_map_vec() { 120 | std::vector feats; 121 | for (auto const &feat : featmap) 122 | feats.push_back(feat.second); 123 | return feats; 124 | } 125 | 126 | /// Access function to get the true parameters (i.e. calibration and settings) 127 | VioManagerOptions get_true_parameters() { return params; } 128 | 129 | protected: 130 | /** 131 | * @brief Projects the passed map features into the desired camera frame. 132 | * @param R_GtoI Orientation of the IMU pose 133 | * @param p_IinG Position of the IMU pose 134 | * @param camid Camera id of the camera sensor we want to project into 135 | * @param feats Our set of 3d features 136 | * @return True distorted raw image measurements and their ids for the specified camera 137 | */ 138 | std::vector> project_pointcloud(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, 139 | int camid, const std::map &feats); 140 | 141 | /** 142 | * @brief Will generate points in the fov of the specified camera 143 | * @param R_GtoI Orientation of the IMU pose 144 | * @param p_IinG Position of the IMU pose 145 | * @param camid Camera id of the camera sensor we want to project into 146 | * @param[out] feats Map we will append new features to 147 | * @param numpts Number of points we should generate 148 | * @param on_plane If points are on a plane 149 | */ 150 | void generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, int camid, std::map &feats, 151 | int numpts, bool on_plane); 152 | 153 | /** 154 | * Generate planes given the trajectory 155 | */ 156 | void generate_planes(); 157 | 158 | //=================================================================== 159 | // Configuration variables 160 | //=================================================================== 161 | 162 | /// True vio manager params (a copy of the parsed ones) 163 | VioManagerOptions params; 164 | 165 | //=================================================================== 166 | // State related variables 167 | //=================================================================== 168 | 169 | /// Our loaded trajectory data (timestamp(s), q_GtoI, p_IinG) 170 | std::vector traj_data; 171 | 172 | /// Our b-spline trajectory 173 | std::shared_ptr spline; 174 | 175 | /// Our map of 3d features 176 | size_t id_map = 0; 177 | std::map featmap; 178 | 179 | /// Mersenne twister PRNG for measurements (IMU) 180 | std::mt19937 gen_meas_imu; 181 | 182 | /// Mersenne twister PRNG for measurements (CAMERAS) 183 | std::vector gen_meas_cams; 184 | 185 | /// Mersenne twister PRNG for state initialization 186 | std::mt19937 gen_state_init; 187 | 188 | /// Mersenne twister PRNG for state perturbations 189 | std::mt19937 gen_state_perturb; 190 | 191 | /// If our simulation is running 192 | bool is_running; 193 | 194 | //=================================================================== 195 | // Simulation specific variables 196 | //=================================================================== 197 | 198 | /// Current timestamp of the system 199 | double timestamp; 200 | 201 | /// Last time we had an IMU reading 202 | double timestamp_last_imu; 203 | 204 | /// Last time we had an CAMERA reading 205 | double timestamp_last_cam; 206 | 207 | /// Our running acceleration bias 208 | Eigen::Vector3d true_bias_accel = Eigen::Vector3d::Zero(); 209 | 210 | /// Our running gyroscope bias 211 | Eigen::Vector3d true_bias_gyro = Eigen::Vector3d::Zero(); 212 | 213 | // Our history of true biases 214 | bool has_skipped_first_bias = false; 215 | std::vector hist_true_bias_time; 216 | std::vector hist_true_bias_accel; 217 | std::vector hist_true_bias_gyro; 218 | 219 | // Our simulated environmental planes 220 | std::vector planes; 221 | }; 222 | 223 | } // namespace ov_plane 224 | 225 | #endif // OV_PLANE_SIMULATOR_H 226 | -------------------------------------------------------------------------------- /ov_plane/src/ros/ROS1Visualizer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities 3 | * Copyright (C) 2022-2023 Chuchu Chen 4 | * Copyright (C) 2022-2023 Patrick Geneva 5 | * Copyright (C) 2022-2023 Guoquan Huang 6 | * 7 | * OpenVINS: An Open Platform for Visual-Inertial Research 8 | * Copyright (C) 2018-2023 Patrick Geneva 9 | * Copyright (C) 2018-2023 Guoquan Huang 10 | * Copyright (C) 2018-2023 OpenVINS Contributors 11 | * Copyright (C) 2018-2019 Kevin Eckenhoff 12 | * 13 | * This program is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef OV_PLANE_ROS1VISUALIZER_H 28 | #define OV_PLANE_ROS1VISUALIZER_H 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | #include "ikd_tree.h" 61 | 62 | namespace ov_core { 63 | class YamlParser; 64 | struct CameraData; 65 | } // namespace ov_core 66 | 67 | namespace ov_plane { 68 | 69 | class VioManager; 70 | class Simulator; 71 | 72 | /** 73 | * @brief Helper class that will publish results onto the ROS framework. 74 | * 75 | * Also save to file the current total state and covariance along with the groundtruth if we are simulating. 76 | * We visualize the following things: 77 | * - State of the system on TF, pose message, and path 78 | * - Image of our tracker 79 | * - Our different features (SLAM, MSCKF, ARUCO) 80 | * - Groundtruth trajectory if we have it 81 | */ 82 | class ROS1Visualizer { 83 | 84 | public: 85 | /** 86 | * @brief Default constructor 87 | * @param nh ROS node handler 88 | * @param app Core estimator manager 89 | * @param sim Simulator if we are simulating 90 | */ 91 | ROS1Visualizer(std::shared_ptr nh, std::shared_ptr app, std::shared_ptr sim = nullptr); 92 | 93 | /** 94 | * @brief Will setup ROS subscribers and callbacks 95 | * @param parser Configuration file parser 96 | */ 97 | void setup_subscribers(std::shared_ptr parser); 98 | 99 | /** 100 | * @brief Will visualize the system if we have new things 101 | */ 102 | void visualize(); 103 | 104 | /** 105 | * @brief Will publish our odometry message for the current timestep. 106 | * This will take the current state estimate and get the propagated pose to the desired time. 107 | * This can be used to get pose estimates on systems which require high frequency pose estimates. 108 | */ 109 | void visualize_odometry(double timestamp); 110 | 111 | /** 112 | * @brief After the run has ended, print results 113 | */ 114 | void visualize_final(); 115 | 116 | /// Callback for inertial information 117 | void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); 118 | 119 | /// Callback for monocular cameras information 120 | void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); 121 | 122 | /// Callback for synchronized stereo camera information 123 | void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); 124 | 125 | protected: 126 | /// Publish the current state 127 | void publish_state(); 128 | 129 | /// Publish the active tracking image 130 | void publish_images(); 131 | 132 | /// Publish current features 133 | void publish_features(); 134 | 135 | /// Publish groundtruth (if we have it) 136 | void publish_groundtruth(); 137 | 138 | /// Publish loop-closure information of current pose and active track information 139 | void publish_loopclosure_information(); 140 | 141 | // Publish planes (sim and slam) 142 | void publish_planes(); 143 | 144 | // Publish latest plane info 145 | void publish_plane_information(); 146 | 147 | /// Global node handler 148 | std::shared_ptr _nh; 149 | 150 | /// Core application of the filter system 151 | std::shared_ptr _app; 152 | 153 | /// Simulator (is nullptr if we are not sim'ing) 154 | std::shared_ptr _sim; 155 | 156 | // Our publishers 157 | image_transport::Publisher it_pub_tracks, it_pub_tracksplane, it_pub_tracksplanenorm, it_pub_ardisp; 158 | image_transport::Publisher it_pub_loop_img_depth, it_pub_loop_img_depth_color; 159 | ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu; 160 | ros::Publisher pub_plane_points, pub_plane_constraints, pub_plane_sim, pub_plane_slam, pub_plane_slam_map; 161 | ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim, pub_points_tracker; 162 | ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics; 163 | std::shared_ptr mTfBr; 164 | 165 | // Our subscribers and camera synchronizers 166 | ros::Subscriber sub_imu; 167 | std::vector subs_cam; 168 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 169 | std::vector>> sync_cam; 170 | std::vector>> sync_subs_cam; 171 | 172 | // For path viz 173 | unsigned int poses_seq_imu = 0; 174 | std::vector poses_imu; 175 | 176 | // Groundtruth infomation 177 | ros::Publisher pub_pathgt, pub_posegt; 178 | double summed_mse_ori = 0.0; 179 | double summed_mse_pos = 0.0; 180 | double summed_nees_ori = 0.0; 181 | double summed_nees_pos = 0.0; 182 | size_t summed_number = 0; 183 | 184 | // Start and end timestamps 185 | bool start_time_set = false; 186 | boost::posix_time::ptime rT1, rT2; 187 | 188 | // Thread atomics 189 | std::atomic thread_update_running; 190 | 191 | /// Queue up camera measurements sorted by time and trigger once we have 192 | /// exactly one IMU measurement with timestamp newer than the camera measurement 193 | /// This also handles out-of-order camera measurements, which is rare, but 194 | /// a nice feature to have for general robustness to bad camera drivers. 195 | std::deque camera_queue; 196 | std::mutex camera_queue_mtx; 197 | 198 | // Last camera message timestamps we have received (mapped by cam id) 199 | std::map camera_last_timestamp; 200 | 201 | // Last timestamp we visualized at 202 | double last_visualization_timestamp = 0; 203 | double last_visualization_timestamp_image = 0; 204 | 205 | // Our groundtruth states 206 | std::map> gt_states; 207 | 208 | // For path viz 209 | unsigned int poses_seq_gt = 0; 210 | std::vector poses_gt; 211 | bool publish_global2imu_tf = true; 212 | bool publish_calibration_tf = true; 213 | 214 | // Files and if we should save total state 215 | bool save_total_state = false; 216 | std::ofstream of_state_est, of_state_std, of_state_gt; 217 | 218 | // Total history of plane constraints 219 | std::map> good_features_PLANE_SLAM_MAP; 220 | std::map features_PLANE_MAP; 221 | std::map ar_location_inPi; 222 | std::map ar_rotation_inPi; 223 | std::map ar_flip_inPi; 224 | 225 | // ikd tree for vis downsampling 226 | shared_ptr> ikd_tree = make_shared>(); 227 | 228 | // Location of the AR display object we want to read in 229 | std::string path_to_object; 230 | }; 231 | 232 | } // namespace ov_plane 233 | 234 | #endif // OV_PLANE_ROS1VISUALIZER_H 235 | -------------------------------------------------------------------------------- /thirdparty/ikd/ikd_tree.h: -------------------------------------------------------------------------------- 1 | /// hku-mars ikd-Tree 2 | /// GNU General Public License v2.0 3 | /// Repo: https://github.com/hku-mars/ikd-Tree/commits/main 4 | /// Commit version: c0e36a16b6e4d557d3783b16911207f6398dd478 5 | 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define EPSS 1e-6 19 | #define Minimal_Unbalanced_Tree_Size 10 20 | #define Multi_Thread_Rebuild_Point_Num 1500 21 | #define DOWNSAMPLE_SWITCH true 22 | #define ForceRebuildPercentage 0.2 23 | #define Q_LEN 1000000 24 | 25 | using namespace std; 26 | 27 | struct ikdTree_PointType { 28 | float x, y, z; 29 | size_t id; 30 | ikdTree_PointType(float px = 0.0f, float py = 0.0f, float pz = 0.0f, size_t pid = 0) { 31 | x = px; 32 | y = py; 33 | z = pz; 34 | id = pid; 35 | } 36 | }; 37 | 38 | struct BoxPointType { 39 | float vertex_min[3]; 40 | float vertex_max[3]; 41 | }; 42 | 43 | enum operation_set { ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN }; 44 | 45 | enum delete_point_storage_set { NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC }; 46 | 47 | template class MANUAL_Q { 48 | private: 49 | int head = 0, tail = 0, counter = 0; 50 | T q[Q_LEN]; 51 | bool is_empty; 52 | 53 | public: 54 | void pop(); 55 | T front(); 56 | T back(); 57 | void clear(); 58 | void push(T op); 59 | bool empty(); 60 | int size(); 61 | }; 62 | 63 | template class KD_TREE { 64 | public: 65 | using PointVector = vector>; 66 | using Ptr = shared_ptr>; 67 | struct KD_TREE_NODE { 68 | PointType point; 69 | uint8_t division_axis; 70 | int TreeSize = 1; 71 | int invalid_point_num = 0; 72 | int down_del_num = 0; 73 | bool point_deleted = false; 74 | bool tree_deleted = false; 75 | bool point_downsample_deleted = false; 76 | bool tree_downsample_deleted = false; 77 | bool need_push_down_to_left = false; 78 | bool need_push_down_to_right = false; 79 | bool working_flag = false; 80 | float radius_sq; 81 | pthread_mutex_t push_down_mutex_lock; 82 | float node_range_x[2], node_range_y[2], node_range_z[2]; 83 | KD_TREE_NODE *left_son_ptr = nullptr; 84 | KD_TREE_NODE *right_son_ptr = nullptr; 85 | KD_TREE_NODE *father_ptr = nullptr; 86 | // For paper data record 87 | float alpha_del; 88 | float alpha_bal; 89 | }; 90 | 91 | struct Operation_Logger_Type { 92 | PointType point; 93 | BoxPointType boxpoint; 94 | bool tree_deleted, tree_downsample_deleted; 95 | operation_set op; 96 | }; 97 | 98 | struct PointType_CMP { 99 | PointType point; 100 | float dist = 0.0; 101 | PointType_CMP(PointType p = PointType(), float d = INFINITY) { 102 | this->point = p; 103 | this->dist = d; 104 | }; 105 | bool operator<(const PointType_CMP &a) const { 106 | if (fabs(dist - a.dist) < 1e-10) 107 | return point.x < a.point.x; 108 | else 109 | return dist < a.dist; 110 | } 111 | }; 112 | 113 | class MANUAL_HEAP { 114 | public: 115 | MANUAL_HEAP(int max_capacity = 100) { 116 | cap = max_capacity; 117 | heap = new PointType_CMP[max_capacity]; 118 | heap_size = 0; 119 | } 120 | 121 | ~MANUAL_HEAP() { delete[] heap; } 122 | 123 | void pop() { 124 | if (heap_size == 0) 125 | return; 126 | heap[0] = heap[heap_size - 1]; 127 | heap_size--; 128 | MoveDown(0); 129 | return; 130 | } 131 | 132 | PointType_CMP top() { return heap[0]; } 133 | 134 | void push(PointType_CMP point) { 135 | if (heap_size >= cap) 136 | return; 137 | heap[heap_size] = point; 138 | FloatUp(heap_size); 139 | heap_size++; 140 | return; 141 | } 142 | 143 | int size() { return heap_size; } 144 | 145 | void clear() { heap_size = 0; } 146 | 147 | private: 148 | int heap_size = 0; 149 | int cap = 0; 150 | PointType_CMP *heap; 151 | void MoveDown(int heap_index) { 152 | int l = heap_index * 2 + 1; 153 | PointType_CMP tmp = heap[heap_index]; 154 | while (l < heap_size) { 155 | if (l + 1 < heap_size && heap[l] < heap[l + 1]) 156 | l++; 157 | if (tmp < heap[l]) { 158 | heap[heap_index] = heap[l]; 159 | heap_index = l; 160 | l = heap_index * 2 + 1; 161 | } else 162 | break; 163 | } 164 | heap[heap_index] = tmp; 165 | return; 166 | } 167 | 168 | void FloatUp(int heap_index) { 169 | int ancestor = (heap_index - 1) / 2; 170 | PointType_CMP tmp = heap[heap_index]; 171 | while (heap_index > 0) { 172 | if (heap[ancestor] < tmp) { 173 | heap[heap_index] = heap[ancestor]; 174 | heap_index = ancestor; 175 | ancestor = (heap_index - 1) / 2; 176 | } else 177 | break; 178 | } 179 | heap[heap_index] = tmp; 180 | return; 181 | } 182 | }; 183 | 184 | private: 185 | // Multi-thread Tree Rebuild 186 | bool termination_flag = false; 187 | bool rebuild_flag = false; 188 | pthread_t rebuild_thread; 189 | pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; 190 | pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; 191 | // queue Rebuild_Logger; 192 | MANUAL_Q Rebuild_Logger; 193 | PointVector Rebuild_PCL_Storage; 194 | KD_TREE_NODE **Rebuild_Ptr = nullptr; 195 | int search_mutex_counter = 0; 196 | static void *multi_thread_ptr(void *arg); 197 | void multi_thread_rebuild(); 198 | void start_thread(); 199 | void stop_thread(); 200 | void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); 201 | // KD Tree Functions and augmented variables 202 | int Treesize_tmp = 0, Validnum_tmp = 0; 203 | float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; 204 | float delete_criterion_param = 0.5f; 205 | float balance_criterion_param = 0.7f; 206 | float downsample_size = 0.2f; 207 | bool Delete_Storage_Disabled = false; 208 | KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; 209 | PointVector Points_deleted; 210 | PointVector Downsample_Storage; 211 | PointVector Multithread_Points_deleted; 212 | void InitTreeNode(KD_TREE_NODE *root); 213 | void Test_Lock_States(KD_TREE_NODE *root); 214 | void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); 215 | void Rebuild(KD_TREE_NODE **root); 216 | int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); 217 | void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); 218 | void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); 219 | void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); 220 | void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist); // priority_queue 221 | void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); 222 | void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); 223 | bool Criterion_Check(KD_TREE_NODE *root); 224 | void Push_Down(KD_TREE_NODE *root); 225 | void Update(KD_TREE_NODE *root); 226 | void downsample(KD_TREE_NODE **root); 227 | bool same_point(PointType a, PointType b); 228 | float calc_dist(PointType a, PointType b); 229 | float calc_box_dist(KD_TREE_NODE *node, PointType point); 230 | static bool point_cmp_x(PointType a, PointType b); 231 | static bool point_cmp_y(PointType a, PointType b); 232 | static bool point_cmp_z(PointType a, PointType b); 233 | 234 | public: 235 | KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); 236 | ~KD_TREE(); 237 | void delete_tree_nodes(KD_TREE_NODE **root); 238 | void Set_delete_criterion_param(float delete_param); 239 | void Set_balance_criterion_param(float balance_param); 240 | void set_downsample_param(float box_length); 241 | void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); 242 | int size(); 243 | int validnum(); 244 | void root_alpha(float &alpha_bal, float &alpha_del); 245 | void Build(PointVector point_cloud); 246 | void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, 247 | double max_dist = INFINITY); 248 | void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); 249 | void Radius_Search(PointType point, const float radius, PointVector &Storage); 250 | int Add_Points(PointVector &PointToAdd, bool downsample_on); 251 | void Add_Point_Boxes(vector &BoxPoints); 252 | void Delete_Points(PointVector &PointToDel); 253 | int Delete_Point_Boxes(vector &BoxPoints); 254 | void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); 255 | void acquire_removed_points(PointVector &removed_points); 256 | BoxPointType tree_range(); 257 | PointVector PCL_Storage; 258 | KD_TREE_NODE *Root_Node = nullptr; 259 | int max_queue_size = 0; 260 | // custom function. reset everything 261 | void reset(); 262 | bool initialized() { return Root_Node != nullptr; } 263 | }; --------------------------------------------------------------------------------