├── .flake8 ├── yac_ros ├── msgs │ ├── KeyPoint.msg │ ├── Extrinsics.msg │ ├── CalibTargetMeasurementSet.msg │ ├── CalibTarget.msg │ ├── CameraParams.msg │ ├── CalibTargetMeasurement.msg │ ├── CalibVI.msg │ └── ImuParams.msg ├── models │ └── aprilgrid │ │ ├── aprilgrid.png │ │ ├── aprilgrid.blend │ │ └── aprilgrid.blend1 ├── launch │ ├── calib_camera.launch │ ├── calib_mocap.launch │ ├── record_camera.launch │ ├── calib_imucam.launch │ ├── record_mocap.launch │ ├── intel_d435i.launch │ ├── calib_mocap_inspect-intel_realsense.launch │ ├── intel_d435i-calib_camera.launch │ └── intel_d435i-calib_imucam.launch ├── src │ ├── nodes │ │ ├── calib_nbt_eval_node.cpp │ │ ├── calib_nbv_eval_node.cpp │ │ ├── calib_nbv_node.cpp │ │ ├── calib_nbt_rt_node.cpp │ │ ├── calib_mocap_inspect_node.cpp │ │ └── calib_camera_node.cpp │ └── ros_calib.hpp ├── config │ ├── euroc-calib_camera.yaml │ ├── calib_mocap.yaml │ ├── calib_mocap-intel_d435i.yaml │ ├── intel_d435i.yaml │ ├── euroc-calib_imucam.yaml │ └── calib_mocap-sotiris.yaml └── package.xml ├── .gitignore ├── docs ├── aprilgrid.png ├── aprilgrid_A0.pdf └── kalibr_notes.txt ├── yac ├── tests │ ├── test_data │ │ ├── calib │ │ │ ├── chessboard │ │ │ │ ├── chessboard.yaml │ │ │ │ └── img0.jpg │ │ │ ├── aprilgrid │ │ │ │ ├── aprilgrid.png │ │ │ │ ├── target.yaml │ │ │ │ └── target2.yaml │ │ │ ├── imu_april │ │ │ │ ├── cam0 │ │ │ │ │ ├── 1404733461532800000.csv │ │ │ │ │ ├── 1404733461582799872.csv │ │ │ │ │ ├── 1404733461632800000.csv │ │ │ │ │ ├── 1404733461682800128.csv │ │ │ │ │ ├── 1404733461732800000.csv │ │ │ │ │ ├── 1404733461782799872.csv │ │ │ │ │ ├── 1404733461832800000.csv │ │ │ │ │ ├── 1404733461882800128.csv │ │ │ │ │ ├── 1404733461932800000.csv │ │ │ │ │ ├── 1404733461982799872.csv │ │ │ │ │ ├── 1404733462032800000.csv │ │ │ │ │ ├── 1404733462082800128.csv │ │ │ │ │ ├── 1404733462132800000.csv │ │ │ │ │ ├── 1404733462182799872.csv │ │ │ │ │ ├── 1404733462232800000.csv │ │ │ │ │ ├── 1404733462282800128.csv │ │ │ │ │ ├── 1404733462332800000.csv │ │ │ │ │ ├── 1404733462382799872.csv │ │ │ │ │ ├── 1404733462432800000.csv │ │ │ │ │ ├── 1404733462482800128.csv │ │ │ │ │ ├── 1404733462532800000.csv │ │ │ │ │ ├── 1404733462582799872.csv │ │ │ │ │ ├── 1404733462632800000.csv │ │ │ │ │ ├── 1404733463182799872.csv │ │ │ │ │ ├── 1404733462682800128.csv │ │ │ │ │ ├── 1404733463132800000.csv │ │ │ │ │ ├── 1404733463232800000.csv │ │ │ │ │ ├── 1404733463282800128.csv │ │ │ │ │ ├── 1404733463332800000.csv │ │ │ │ │ ├── 1404733463382799872.csv │ │ │ │ │ ├── 1404733462732800000.csv │ │ │ │ │ ├── 1404733462882800128.csv │ │ │ │ │ ├── 1404733462932800000.csv │ │ │ │ │ ├── 1404733462982799872.csv │ │ │ │ │ ├── 1404733463032800000.csv │ │ │ │ │ ├── 1404733463082800128.csv │ │ │ │ │ ├── 1404733462782799872.csv │ │ │ │ │ ├── 1404733462832800000.csv │ │ │ │ │ ├── 1404733463432800000.csv │ │ │ │ │ ├── 1404733461482800128.csv │ │ │ │ │ ├── 1404733463482800128.csv │ │ │ │ │ └── 1404733461432800000.csv │ │ │ │ ├── cam1 │ │ │ │ │ ├── 1404733461582799872.csv │ │ │ │ │ ├── 1404733461632800000.csv │ │ │ │ │ ├── 1404733461682800128.csv │ │ │ │ │ ├── 1404733461732800000.csv │ │ │ │ │ ├── 1404733461782799872.csv │ │ │ │ │ ├── 1404733461832800000.csv │ │ │ │ │ ├── 1404733461882800128.csv │ │ │ │ │ ├── 1404733461932800000.csv │ │ │ │ │ ├── 1404733461982799872.csv │ │ │ │ │ ├── 1404733462032800000.csv │ │ │ │ │ ├── 1404733462082800128.csv │ │ │ │ │ ├── 1404733462132800000.csv │ │ │ │ │ ├── 1404733462182799872.csv │ │ │ │ │ ├── 1404733462232800000.csv │ │ │ │ │ ├── 1404733462282800128.csv │ │ │ │ │ ├── 1404733462332800000.csv │ │ │ │ │ ├── 1404733462382799872.csv │ │ │ │ │ ├── 1404733462432800000.csv │ │ │ │ │ ├── 1404733462482800128.csv │ │ │ │ │ ├── 1404733462532800000.csv │ │ │ │ │ ├── 1404733462582799872.csv │ │ │ │ │ ├── 1404733462632800000.csv │ │ │ │ │ ├── 1404733462682800128.csv │ │ │ │ │ ├── 1404733463132800000.csv │ │ │ │ │ ├── 1404733463182799872.csv │ │ │ │ │ ├── 1404733463232800000.csv │ │ │ │ │ ├── 1404733463282800128.csv │ │ │ │ │ ├── 1404733463382799872.csv │ │ │ │ │ ├── 1404733461532800000.csv │ │ │ │ │ ├── 1404733462732800000.csv │ │ │ │ │ ├── 1404733462932800000.csv │ │ │ │ │ ├── 1404733462982799872.csv │ │ │ │ │ ├── 1404733463032800000.csv │ │ │ │ │ ├── 1404733463082800128.csv │ │ │ │ │ ├── 1404733463332800000.csv │ │ │ │ │ ├── 1404733462782799872.csv │ │ │ │ │ ├── 1404733462882800128.csv │ │ │ │ │ ├── 1404733463432800000.csv │ │ │ │ │ ├── 1404733462832800000.csv │ │ │ │ │ ├── 1404733433732800000.csv │ │ │ │ │ ├── 1404733463482800128.csv │ │ │ │ │ └── 1404733461482800128.csv │ │ │ │ └── config.yaml │ │ │ ├── stereo │ │ │ │ ├── cam0_1403709395937837056.png │ │ │ │ └── cam1_1403709395937837056.png │ │ │ └── cam_april │ │ │ │ ├── stereo_config.yaml │ │ │ │ └── mono_config.yaml │ │ ├── core │ │ │ ├── test_image.jpg │ │ │ └── config │ │ │ │ └── config.yaml │ │ └── mocap_data │ │ │ ├── cam0 │ │ │ ├── data │ │ │ │ ├── 1606153907495166540.jpg │ │ │ │ ├── 1606153915653051138.jpg │ │ │ │ ├── 1606153921853466272.jpg │ │ │ │ ├── 1606153950890122890.jpg │ │ │ │ ├── 1606153963139782667.jpg │ │ │ │ ├── 1606153971086688995.jpg │ │ │ │ ├── 1606153977721161127.jpg │ │ │ │ ├── 1606153983972179413.jpg │ │ │ │ ├── 1606153997421799183.jpg │ │ │ │ ├── 1606154004360154629.jpg │ │ │ │ ├── 1606154011402862549.jpg │ │ │ │ ├── 1606154017373862028.jpg │ │ │ │ ├── 1606154022959313869.jpg │ │ │ │ ├── 1606154029409966230.jpg │ │ │ │ ├── 1606154036334099054.jpg │ │ │ │ ├── 1606154045208428621.jpg │ │ │ │ ├── 1606154057587632418.jpg │ │ │ │ ├── 1606154065240525723.jpg │ │ │ │ ├── 1606154073179670334.jpg │ │ │ │ ├── 1606154080918648243.jpg │ │ │ │ ├── 1606154091296098471.jpg │ │ │ │ ├── 1606154103557152987.jpg │ │ │ │ ├── 1606154110540388823.jpg │ │ │ │ ├── 1606154117645546675.jpg │ │ │ │ ├── 1606154125251100540.jpg │ │ │ │ ├── 1606154131806932688.jpg │ │ │ │ ├── 1606154140441553831.jpg │ │ │ │ ├── 1606154148298093319.jpg │ │ │ │ ├── 1606154155277716398.jpg │ │ │ │ └── 1606154165058666229.jpg │ │ │ └── data.csv │ │ │ └── calib_mocap-intel_d435i.yaml │ ├── test_calib_mocap.cpp │ ├── test_calib_data.cpp │ └── util │ │ ├── test_timeline.cpp │ │ ├── test_data.cpp │ │ └── test_fs.cpp ├── lib │ ├── util │ │ ├── util.hpp │ │ ├── ceres_utils.hpp │ │ ├── net.hpp │ │ └── timeline.cpp │ ├── yac.hpp │ ├── calib_loss.cpp │ ├── calib_data.hpp │ └── calib_loss.hpp └── demo │ ├── calib_vi.cpp │ ├── calib_camera.cpp │ ├── calib_inspect.cpp │ └── calib_mocap.cpp ├── .style.yapf ├── scripts ├── .style.yapf ├── plot_xyz.py ├── plot_poses.py ├── download_test_data.bash ├── plot_nbt_find.py ├── blake_zisserman.py ├── lissajous.m ├── marg_sandbox.py ├── plot_camera.py ├── plot_stats.py ├── plot_aprilgrid_detection.py ├── plot_frames.py ├── plot_imu.py └── plot_info.py ├── deps ├── Makefile └── scripts │ ├── install_opencv.bash │ ├── install_yamlcpp.bash │ ├── install_apriltag3.bash │ ├── install_apriltag.bash │ ├── install_eigen.bash │ ├── install_ros.bash │ ├── install_ceres.bash │ ├── install_suitesparse.bash │ ├── install.bash │ └── config.bash ├── .github └── workflows │ └── ccpp.yml ├── .clang-format ├── pylintrc ├── LICENSE ├── Dockerfile └── Makefile /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | ignore=E111 3 | -------------------------------------------------------------------------------- /yac_ros/msgs/KeyPoint.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | -------------------------------------------------------------------------------- /yac_ros/msgs/Extrinsics.msg: -------------------------------------------------------------------------------- 1 | string name 2 | float64[7] pose 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | tags 3 | 4 | deps/*.log 5 | *.mp4 6 | .catkin_tools/ 7 | -------------------------------------------------------------------------------- /docs/aprilgrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/docs/aprilgrid.png -------------------------------------------------------------------------------- /docs/aprilgrid_A0.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/docs/aprilgrid_A0.pdf -------------------------------------------------------------------------------- /yac/tests/test_data/calib/chessboard/chessboard.yaml: -------------------------------------------------------------------------------- 1 | nb_rows: 6 2 | nb_cols: 7 3 | square_size: 0.028 4 | -------------------------------------------------------------------------------- /yac_ros/models/aprilgrid/aprilgrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac_ros/models/aprilgrid/aprilgrid.png -------------------------------------------------------------------------------- /yac/tests/test_data/core/test_image.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/core/test_image.jpg -------------------------------------------------------------------------------- /yac_ros/models/aprilgrid/aprilgrid.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac_ros/models/aprilgrid/aprilgrid.blend -------------------------------------------------------------------------------- /yac_ros/models/aprilgrid/aprilgrid.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac_ros/models/aprilgrid/aprilgrid.blend1 -------------------------------------------------------------------------------- /yac/tests/test_data/calib/chessboard/img0.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/calib/chessboard/img0.jpg -------------------------------------------------------------------------------- /yac_ros/msgs/CalibTargetMeasurementSet.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | yac_ros/CalibTargetMeasurement[] measurements 3 | -------------------------------------------------------------------------------- /.style.yapf: -------------------------------------------------------------------------------- 1 | [style] 2 | based_on_style = google 3 | column_limit = 80 4 | indent_width = 2 5 | blank_line_before_nested_class_or_def = 0 6 | -------------------------------------------------------------------------------- /scripts/.style.yapf: -------------------------------------------------------------------------------- 1 | [style] 2 | based_on_style = google 3 | column_limit = 80 4 | indent_width = 2 5 | blank_line_before_nested_class_or_def = 0 6 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/aprilgrid/aprilgrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/calib/aprilgrid/aprilgrid.png -------------------------------------------------------------------------------- /yac_ros/msgs/CalibTarget.msg: -------------------------------------------------------------------------------- 1 | string target_type 2 | uint32 tag_rows 3 | uint32 tag_cols 4 | float64 tag_size 5 | float64 tag_spacing 6 | -------------------------------------------------------------------------------- /deps/Makefile: -------------------------------------------------------------------------------- 1 | default: install 2 | .PHONY: install 3 | 4 | install: 5 | @echo "Installing dependencies ..." 6 | @sudo bash ./scripts/install.bash 7 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461532800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461582799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461632800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461682800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461732800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461782799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461832800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461882800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461932800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461982799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462032800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462082800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462132800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462182799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462232800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462282800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462332800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462382799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462432800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462532800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462582799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462632800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461582799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461632800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461682800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461732800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461782799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461832800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461882800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461932800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461982799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462032800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462082800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462132800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462182799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462232800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462282800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462332800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462382799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462432800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462532800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462582799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462632800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462682800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/stereo/cam0_1403709395937837056.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/calib/stereo/cam0_1403709395937837056.png -------------------------------------------------------------------------------- /yac/tests/test_data/calib/stereo/cam1_1403709395937837056.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/calib/stereo/cam1_1403709395937837056.png -------------------------------------------------------------------------------- /deps/scripts/install_opencv.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e # exit on first error 3 | BASEDIR=$(dirname "$0") 4 | source $BASEDIR/config.bash 5 | 6 | apt_install libopencv-* 7 | -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153907495166540.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153907495166540.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153915653051138.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153915653051138.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153921853466272.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153921853466272.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153950890122890.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153950890122890.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153963139782667.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153963139782667.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153971086688995.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153971086688995.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153977721161127.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153977721161127.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153983972179413.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153983972179413.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606153997421799183.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606153997421799183.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154004360154629.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154004360154629.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154011402862549.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154011402862549.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154017373862028.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154017373862028.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154022959313869.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154022959313869.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154029409966230.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154029409966230.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154036334099054.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154036334099054.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154045208428621.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154045208428621.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154057587632418.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154057587632418.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154065240525723.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154065240525723.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154073179670334.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154073179670334.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154080918648243.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154080918648243.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154091296098471.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154091296098471.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154103557152987.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154103557152987.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154110540388823.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154110540388823.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154117645546675.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154117645546675.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154125251100540.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154125251100540.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154131806932688.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154131806932688.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154140441553831.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154140441553831.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154148298093319.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154148298093319.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154155277716398.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154155277716398.jpg -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data/1606154165058666229.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/yac/HEAD/yac/tests/test_data/mocap_data/cam0/data/1606154165058666229.jpg -------------------------------------------------------------------------------- /deps/scripts/install_yamlcpp.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e # exit on first error 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | 6 | apt_install libyaml-cpp-dev 7 | -------------------------------------------------------------------------------- /yac_ros/msgs/CameraParams.msg: -------------------------------------------------------------------------------- 1 | uint32 cam_idx 2 | string proj_model 3 | string dist_model 4 | float64[2] resolution 5 | float64[] proj_params 6 | float64[] dist_params 7 | -------------------------------------------------------------------------------- /yac_ros/launch/calib_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /yac_ros/launch/calib_mocap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /yac/lib/util/util.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_UTIL_HPP 2 | #define YAC_UTIL_HPP 3 | 4 | #include "aprilgrid.hpp" 5 | #include "ceres_utils.hpp" 6 | #include "config.hpp" 7 | #include "core.hpp" 8 | #include "cv.hpp" 9 | #include "data.hpp" 10 | #include "euroc.hpp" 11 | #include "fs.hpp" 12 | #include "net.hpp" 13 | #include "timeline.hpp" 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /yac_ros/msgs/CalibTargetMeasurement.msg: -------------------------------------------------------------------------------- 1 | # Header 2 | std_msgs/Header header 3 | 4 | # CalibTarget 5 | string target_type 6 | uint32 tag_rows 7 | uint32 tag_cols 8 | float64 tag_size 9 | float64 tag_spacing 10 | 11 | # Data 12 | uint32 camera_index 13 | yac_ros/KeyPoint[] keypoints 14 | -------------------------------------------------------------------------------- /yac/lib/yac.hpp: -------------------------------------------------------------------------------- 1 | #include "util/util.hpp" 2 | 3 | #include "calib_camera.hpp" 4 | #include "calib_data.hpp" 5 | #include "calib_loss.hpp" 6 | #include "calib_mocap.hpp" 7 | #include "calib_nbt.hpp" 8 | #include "calib_nbv.hpp" 9 | #include "calib_params.hpp" 10 | #include "calib_residuals.hpp" 11 | #include "calib_vi.hpp" 12 | #include "solver.hpp" 13 | #include "suitesparse.hpp" 14 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/aprilgrid/target.yaml: -------------------------------------------------------------------------------- 1 | target_type: 'aprilgrid' # Target type 2 | tag_rows: 6 # Number of rows 3 | tag_cols: 6 # Number of cols 4 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 5 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 6 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 7 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/aprilgrid/target2.yaml: -------------------------------------------------------------------------------- 1 | target_type: 'aprilgrid' # Target type 2 | tag_rows: 6 # Number of rows 3 | tag_cols: 6 # Number of cols 4 | tag_size: 0.021 # Size of apriltag, edge to edge [m] 5 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 6 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 7 | -------------------------------------------------------------------------------- /.github/workflows/ccpp.yml: -------------------------------------------------------------------------------- 1 | name: ci 2 | on: [push] 3 | jobs: 4 | ci: 5 | runs-on: ubuntu-latest 6 | 7 | steps: 8 | - uses: actions/checkout@master 9 | 10 | - name: install dependencies 11 | run: sudo make deps 12 | 13 | - name: build yac 14 | run: sudo make lib 15 | 16 | - name: build yac_ros 17 | run: make release 18 | 19 | - name: run tests 20 | run: make tests 21 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_nbt_eval_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_nbt_eval_node.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Setup ROS Node 5 | const std::string node_name = yac::ros_node_name(argc, argv); 6 | if (ros::isInitialized() == false) { 7 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 8 | } 9 | 10 | yac::calib_nbt_eval_t node{node_name}; 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_nbv_eval_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_nbv_eval_node.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Setup ROS Node 5 | const std::string node_name = yac::ros_node_name(argc, argv); 6 | if (ros::isInitialized() == false) { 7 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 8 | } 9 | 10 | yac::calib_nbv_eval_t node{node_name}; 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_nbv_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_nbv_node.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Setup ROS Node 5 | const std::string node_name = yac::ros_node_name(argc, argv); 6 | if (ros::isInitialized() == false) { 7 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 8 | } 9 | 10 | // Start calibrating 11 | yac::calib_nbv_t calib{node_name}; 12 | 13 | return 0; 14 | } 15 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | AllowShortCaseLabelsOnASingleLine: false 2 | AllowShortFunctionsOnASingleLine: true 3 | AllowShortBlocksOnASingleLine: false 4 | AlwaysBreakTemplateDeclarations: true 5 | BinPackArguments: false 6 | BinPackParameters: false 7 | FixNamespaceComments: true 8 | IndentCaseLabels: true 9 | PenaltyReturnTypeOnItsOwnLine: 100 10 | PenaltyBreakBeforeFirstCallParameter: 10000 11 | SortIncludes: false 12 | SpaceAfterCStyleCast: false 13 | -------------------------------------------------------------------------------- /scripts/plot_xyz.py: -------------------------------------------------------------------------------- 1 | """ Plot XYZ """ 2 | #!/usr/bin/env python3 3 | import sys 4 | import pandas 5 | import proto 6 | import matplotlib.pylab as plt 7 | 8 | csv_data = pandas.read_csv(sys.argv[1]) 9 | key_time = sys.argv[2] 10 | key_x = sys.argv[3] 11 | key_y = sys.argv[4] 12 | key_z = sys.argv[5] 13 | label_y = "Displacement [m]" 14 | data = {"Estimates": csv_data} 15 | line_styles = ["."] 16 | proto.plot_xyz("", data, key_time, key_x, key_y, key_z, label_y, line_styles=line_styles) 17 | plt.show() 18 | -------------------------------------------------------------------------------- /yac_ros/msgs/CalibVI.msg: -------------------------------------------------------------------------------- 1 | # Header 2 | std_msgs/Header header 3 | 4 | # Calibration Target 5 | yac_ros/CalibTarget calib_target 6 | 7 | # IMU 8 | float64 imu_rate 9 | yac_ros/ImuParams imu_params 10 | yac_ros/Extrinsics imu_exts 11 | 12 | # Cameras 13 | float64 cam_rate 14 | yac_ros/CameraParams[] cam_params 15 | yac_ros/Extrinsics[] cam_exts 16 | 17 | # Fiducial pose 18 | float64[7] fiducial_pose 19 | 20 | # Calibration info 21 | float64[36] calib_info 22 | -------------------------------------------------------------------------------- /yac/demo/calib_vi.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_vi.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Check arguments 5 | if (argc != 3) { 6 | printf("usage: %s \n", argv[0]); 7 | return -1; 8 | } 9 | 10 | // Calibrate 11 | const std::string config_file = argv[1]; 12 | const std::string dataset_path = argv[2]; 13 | yac::calib_vi_t calib{config_file}; 14 | calib.load_data(dataset_path); 15 | calib.solve(); 16 | calib.save_results("/tmp/calib-results.yaml"); 17 | 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /yac/demo/calib_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_camera.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Check arguments 5 | if (argc != 3) { 6 | printf("usage: %s \n", argv[0]); 7 | return -1; 8 | } 9 | 10 | // Calibrate 11 | const std::string config_file = argv[1]; 12 | const std::string data_path = argv[2]; 13 | yac::calib_camera_t calib{config_file}; 14 | calib.load_data(data_path); 15 | calib.solve(); 16 | calib.save_results("/tmp/calib_camera-results.yaml"); 17 | 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /yac_ros/msgs/ImuParams.msg: -------------------------------------------------------------------------------- 1 | float64 rate # IMU rate [Hz] 2 | float64 a_max # Max accelerometer measurement [m/s^2] 3 | float64 g_max # Max gyroscope measurement [rad/s] 4 | float64 sigma_g_c # Gyro noise density [rad/s/sqrt(Hz)] 5 | float64 sigma_a_c # Accel noise density [m/s^s/sqrt(Hz)] 6 | float64 sigma_gw_c # Gyro drift noise density [rad/s^s/sqrt(Hz)] 7 | float64 sigma_aw_c # Accel drift noise density [m/s^2/sqrt(Hz)] 8 | float64 sigma_bg # Gyro bias prior [rad/s] 9 | float64 sigma_ba # Accel bias prior [m/s^2] 10 | float64 g # Gravity vector [ms^-2] 11 | -------------------------------------------------------------------------------- /yac/tests/test_calib_mocap.cpp: -------------------------------------------------------------------------------- 1 | #include "munit.hpp" 2 | #include "calib_mocap.hpp" 3 | 4 | namespace yac { 5 | 6 | #ifndef TEST_PATH 7 | #define TEST_PATH "." 8 | #endif 9 | 10 | #define TEST_CONF TEST_PATH "/test_data/mocap_data/calib_mocap-intel_d435i.yaml" 11 | #define TEST_DATA TEST_PATH "/test_data/mocap_data" 12 | 13 | int test_calib_mocap() { 14 | calib_mocap_t calib(TEST_CONF, TEST_DATA); 15 | calib.solve(); 16 | calib.show_results(); 17 | return 0; 18 | } 19 | 20 | void test_suite() { MU_ADD_TEST(test_calib_mocap); } 21 | 22 | } // namespace yac 23 | 24 | MU_RUN_TESTS(yac::test_suite); 25 | -------------------------------------------------------------------------------- /deps/scripts/install_apriltag3.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | echo "building apriltag3 ..." 6 | 7 | # Clone 8 | if [ ! -d $INSTALL_PREFIX/src/apriltag3 ]; then 9 | cd $INSTALL_PREFIX/src 10 | git clone --quiet https://github.com/AprilRobotics/apriltag apriltag3 11 | cd .. 12 | fi 13 | 14 | # Build 15 | cd $INSTALL_PREFIX/src/apriltag3 16 | 17 | mkdir -p build 18 | cd build 19 | cmake .. \ 20 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 21 | -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 22 | -DCMAKE_PREFIX_PATH=$INSTALL_PREFIX 23 | make 24 | make install 25 | -------------------------------------------------------------------------------- /scripts/plot_poses.py: -------------------------------------------------------------------------------- 1 | """ Plot Poses """ 2 | import sys 3 | 4 | import matplotlib.pylab as plt 5 | from mpl_toolkits import mplot3d 6 | 7 | from proto import load_poses 8 | from proto import plot_tf 9 | from proto import plot_set_axes_equal 10 | 11 | if __name__ == "__main__": 12 | calib_poses = load_poses(sys.argv[1]) 13 | 14 | plt.figure() 15 | ax = plt.axes(projection='3d') 16 | 17 | for calib_pose in calib_poses: 18 | _, T_OC = calib_pose 19 | plot_tf(ax, T_OC, size=0.1) 20 | 21 | ax.set_xlabel("x [m]") 22 | ax.set_ylabel("y [m]") 23 | ax.set_zlabel("z [m]") 24 | plot_set_axes_equal(ax) 25 | plt.show() 26 | -------------------------------------------------------------------------------- /yac_ros/launch/record_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_nbt_rt_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_nbv_node.hpp" 2 | #include "calib_nbt_rt_node.hpp" 3 | 4 | int main(int argc, char *argv[]) { 5 | // Setup ROS Node 6 | const std::string node_name = yac::ros_node_name(argc, argv); 7 | if (ros::isInitialized() == false) { 8 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 9 | } 10 | 11 | // Calibrate cameras 12 | { yac::calib_nbv_t nbv{node_name}; } 13 | // IMPORTANT NOTE: The brackets around calib_nbt_t() are necessary to trigger 14 | // the destructor on finish. 15 | 16 | // Calibrate imu-camera 17 | yac::calib_nbt_t nbt{node_name}; 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/calib_mocap-intel_d435i.yaml: -------------------------------------------------------------------------------- 1 | settings: 2 | fix_intrinsics: true 3 | fix_mocap_poses: true 4 | fix_fiducial_pose: false 5 | outlier_threshold: 4.0 6 | 7 | calib_target: 8 | target_type: 'aprilgrid' # Target type 9 | tag_rows: 6 # Number of rows 10 | tag_cols: 6 # Number of cols 11 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 12 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 13 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 14 | 15 | cam0: 16 | resolution: [640, 480] 17 | proj_model: "pinhole" 18 | dist_model: "radtan4" 19 | -------------------------------------------------------------------------------- /deps/scripts/install_apriltag.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | echo "building apriltag ..." 6 | 7 | # Clone 8 | if [ ! -d $INSTALL_PREFIX/src/apriltag ]; then 9 | cd $INSTALL_PREFIX/src 10 | git clone --quiet https://github.com/chutsu/apriltag 11 | cd .. 12 | fi 13 | 14 | # Build 15 | cd $INSTALL_PREFIX/src/apriltag 16 | 17 | git submodule --quiet init 18 | git submodule --quiet update 19 | 20 | mkdir -p build 21 | cd build 22 | cmake .. \ 23 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 24 | -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 25 | -DCMAKE_PREFIX_PATH=$INSTALL_PREFIX 26 | make 27 | make install 28 | mv $INSTALL_PREFIX/lib/libapriltag.a $INSTALL_PREFIX/lib/libapriltags.a 29 | -------------------------------------------------------------------------------- /deps/scripts/install_eigen.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | 6 | # Install Eigen3 7 | apt_install libeigen3-dev 8 | apt_install libeigen3-doc 9 | 10 | 11 | # VERSION=3.4 12 | # echo "building eigen [$VERSION] ..." 13 | # 14 | # # Clone 15 | # if [ ! -d $INSTALL_PREFIX/src/eigen ]; then 16 | # cd $INSTALL_PREFIX/src 17 | # git clone --quiet https://gitlab.com/libeigen/eigen.git 18 | # cd .. 19 | # fi 20 | # 21 | # # Build 22 | # cd $INSTALL_PREFIX/src/eigen 23 | # git checkout ${VERSION} --quiet 24 | # 25 | # mkdir -p build 26 | # cd build 27 | # cmake .. \ 28 | # -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 29 | # -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 30 | # -DCMAKE_PREFIX_PATH=$INSTALL_PREFIX 31 | # make install 32 | -------------------------------------------------------------------------------- /deps/scripts/install_ros.bash: -------------------------------------------------------------------------------- 1 | set -e 2 | BASEDIR=$(dirname "$0") 3 | source "$BASEDIR/config.bash" 4 | 5 | # Check if ROS is installed 6 | [ -d "/opt/ros/${ROS_VERSION}" ] && exit; 7 | 8 | # Install ROS 9 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 10 | sudo apt-key adv \ 11 | --keyserver 'hkp://keyserver.ubuntu.com:80' \ 12 | --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 13 | apt_update 14 | apt_install dpkg 15 | sudo dpkg --configure -a 16 | apt_install ros-${ROS_VERSION}-desktop 17 | apt_install ros-${ROS_VERSION}-gazebo-dev* 18 | apt_install ros-${ROS_VERSION}-tf2* 19 | apt_install python3-catkin-tools 20 | apt_install python3-rosdep 21 | sudo rosdep init 22 | rosdep update 23 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_mocap_inspect_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_mocap_inspect_node.hpp" 2 | 3 | int main(int argc, char *argv[]) { 4 | // Setup ROS Node 5 | const std::string node_name = yac::ros_node_name(argc, argv); 6 | if (ros::isInitialized() == false) { 7 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 8 | } 9 | 10 | // Run ROS node 11 | ros::NodeHandle nh; 12 | std::string calib_file; 13 | std::string camera_topic; 14 | std::string mocap_topic; 15 | ROS_PARAM(nh, node_name + "/calib_file", calib_file); 16 | ROS_PARAM(nh, node_name + "/camera_topic", camera_topic); 17 | ROS_PARAM(nh, node_name + "/mocap_topic", mocap_topic); 18 | calib_mocap_inspector_t node{calib_file, camera_topic, mocap_topic}; 19 | node.loop(); 20 | 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /yac_ros/launch/calib_imucam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /yac_ros/config/euroc-calib_camera.yaml: -------------------------------------------------------------------------------- 1 | ros: 2 | bag: "/data/euroc/rosbags/cam_april.bag" 3 | cam0_topic: "/cam0/image_raw" 4 | cam1_topic: "/cam1/image_raw" 5 | 6 | settings: 7 | results_fpath: "/tmp/cam_april/calib_results.yaml" 8 | imshow: true 9 | 10 | calib_target: 11 | target_type: 'aprilgrid' # Target type 12 | tag_rows: 6 # Number of rows 13 | tag_cols: 6 # Number of cols 14 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 15 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 16 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 17 | 18 | cam0: 19 | resolution: [752, 480] 20 | proj_model: "pinhole" 21 | dist_model: "radtan4" 22 | 23 | cam1: 24 | resolution: [752, 480] 25 | proj_model: "pinhole" 26 | dist_model: "radtan4" 27 | -------------------------------------------------------------------------------- /pylintrc: -------------------------------------------------------------------------------- 1 | [MASTER] 2 | 3 | jobs=8 4 | 5 | # A comma-separated list of package or module names from where C extensions may 6 | # be loaded. Extensions are loading into the active Python interpreter and may 7 | # run arbitrary code. 8 | extension-pkg-whitelist=numpy,cv2 9 | 10 | 11 | [MESSAGES CONTROL] 12 | disable= 13 | invalid-name, 14 | too-many-arguments, 15 | too-many-function-args, 16 | too-many-locals, 17 | too-many-lines, 18 | too-many-statements, 19 | too-many-branches, 20 | too-many-instance-attributes, 21 | too-many-public-methods, 22 | too-few-public-methods, 23 | line-too-long, 24 | len-as-condition, 25 | assignment-from-no-return, 26 | ungrouped-imports, 27 | wrong-import-order, 28 | wrong-import-position, 29 | protected-access, 30 | 31 | 32 | [TYPECHECK] 33 | 34 | ignored-modules = numpy,cv2,websockets 35 | 36 | 37 | [FORMAT] 38 | # Use 2 spaces 39 | indent-string=' ' 40 | -------------------------------------------------------------------------------- /yac_ros/config/calib_mocap.yaml: -------------------------------------------------------------------------------- 1 | ros: 2 | # rosbag: "/data/aabm/calib/200115-vicon-train.bag" 3 | # rosbag: "/data/aabm/calib/201123-vicon.bag" 4 | rosbag: "/home/chutsu/Downloads/mocap_data.bag" 5 | cam0_topic: "/cam0/image" 6 | body0_topic: "/body0/pose" 7 | target0_topic: "/target0/pose" 8 | 9 | settings: 10 | fix_intrinsics: true 11 | fix_mocap_poses: true 12 | fix_fiducial_pose: false 13 | outlier_threshold: 4.0 14 | 15 | calib_target: 16 | target_type: 'aprilgrid' # Target type 17 | tag_rows: 6 # Number of rows 18 | tag_cols: 6 # Number of cols 19 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 20 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 21 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 22 | 23 | cam0: 24 | resolution: [640, 480] 25 | proj_model: "pinhole" 26 | dist_model: "radtan4" 27 | rate: 30.0 28 | -------------------------------------------------------------------------------- /deps/scripts/install_ceres.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | 6 | VERSION=2.0.0 7 | echo "installing ceres-solver [$VERSION] ..." 8 | 9 | # Install GFLAGS, GLOG 10 | # apt_install libgflags-dev 11 | # apt_install libgflags-doc 12 | # apt_install libgoogle-glog-dev 13 | apt_install libceres-dev 14 | # apt_install libceres2 15 | 16 | # Dependencies 17 | # apt_install libtbb2-dev 18 | 19 | # # Clone 20 | # if [ ! -d $INSTALL_PREFIX/src/ceres-solver ]; then 21 | # cd $INSTALL_PREFIX/src 22 | # git clone --quiet https://github.com/ceres-solver/ceres-solver 23 | # cd .. 24 | # fi 25 | 26 | # # Build 27 | # cd $INSTALL_PREFIX/src/ceres-solver 28 | # git checkout ${VERSION} --quiet 29 | 30 | # mkdir -p build 31 | # cd build 32 | # cmake .. \ 33 | # -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 34 | # -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 35 | # -DCMAKE_PREFIX_PATH=$INSTALL_PREFIX 36 | # make -j4 37 | # make install 38 | -------------------------------------------------------------------------------- /scripts/download_test_data.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e # Exit on first error 3 | BASE_URL="http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset" 4 | CALIB_URL=${BASE_URL}"/calibration_datasets" 5 | 6 | 7 | # DOWNLOAD DATASET FLAT-FILES 8 | mkdir -p /data/euroc 9 | cd /data/euroc 10 | 11 | if [ ! -f cam_april.zip ]; then 12 | wget "${CALIB_URL}/cam_april/cam_april.zip" 13 | fi 14 | if [ ! -f imu_april.zip ]; then 15 | wget "${CALIB_URL}/imu_april/imu_april.zip" 16 | fi 17 | 18 | echo "unzipping cam_april.zip" 19 | mkdir -p cam_april 20 | unzip -oq cam_april.zip -d cam_april 21 | 22 | echo "unzipping imu_april.zip" 23 | mkdir -p imu_april 24 | unzip -oq imu_april.zip -d imu_april 25 | 26 | 27 | # DOWNLOAD ROSBAGS 28 | cd /data/euroc 29 | mkdir -p rosbags 30 | 31 | if [ ! -f cam_april.bag ]; then 32 | wget "${CALIB_URL}/cam_april/cam_april.bag" 33 | fi 34 | 35 | if [ ! -f imu_april.bag ]; then 36 | wget "${CALIB_URL}/imu_april/imu_april.bag" 37 | fi 38 | -------------------------------------------------------------------------------- /scripts/plot_nbt_find.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Plot nbt_find() results 4 | """ 5 | # import numpy as np 6 | import matplotlib.pylab as plt 7 | 8 | # import proto 9 | from proto import load_poses 10 | from proto import plot_tf 11 | from proto import plot_set_axes_equal 12 | 13 | 14 | def plot_poses(): 15 | """ Plot poses """ 16 | before = load_poses("/tmp/nbt_before/poses_est.csv") 17 | after = load_poses("/tmp/nbt_after/poses_est.csv") 18 | 19 | # Plot NBT 20 | plt.figure() 21 | ax = plt.axes(projection='3d') 22 | 23 | for pose_data in before: 24 | _, pose = pose_data 25 | plot_tf(ax, pose, size=0.1, colors=['r', 'r', 'r']) 26 | 27 | for pose_data in after: 28 | _, pose = pose_data 29 | plot_tf(ax, pose, size=0.1, colors=['b', 'b', 'b']) 30 | 31 | ax.set_xlabel("x [m]") 32 | ax.set_ylabel("y [m]") 33 | ax.set_zlabel("z [m]") 34 | plot_set_axes_equal(ax) 35 | plt.show() 36 | 37 | 38 | plot_poses() 39 | plt.show() 40 | -------------------------------------------------------------------------------- /yac_ros/config/calib_mocap-intel_d435i.yaml: -------------------------------------------------------------------------------- 1 | ros: 2 | rosbag: "/data/mocap/mocap1.bag" 3 | cam0_topic: "/rs/ir0/image" 4 | body0_topic: "/vicon/chris_d435i/chris_d435i" 5 | target0_topic: "/vicon/AprilGrid/AprilGrid" 6 | 7 | settings: 8 | fix_intrinsics: true 9 | fix_mocap_poses: true 10 | fix_fiducial_pose: false 11 | outlier_threshold: 4.0 12 | info_gain_threshold: 0.2 13 | enable_loss_fn: false 14 | loss_fn_type: "CAUCHY" 15 | loss_fn_param: 1.0 16 | 17 | calib_target: 18 | target_type: 'aprilgrid' # Target type 19 | tag_rows: 6 # Number of rows 20 | tag_cols: 6 # Number of cols 21 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 22 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 23 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 24 | 25 | cam0: 26 | resolution: [640, 480] 27 | proj_model: "pinhole" 28 | dist_model: "radtan4" 29 | -------------------------------------------------------------------------------- /yac/tests/test_calib_data.cpp: -------------------------------------------------------------------------------- 1 | #include "munit.hpp" 2 | #include "calib_data.hpp" 3 | 4 | namespace yac { 5 | 6 | #ifndef TEST_PATH 7 | #define TEST_PATH "." 8 | #endif 9 | 10 | #define APRILGRID_CONF TEST_PATH "/test_data/calib/aprilgrid/target.yaml" 11 | #define CAM0_IMAGE_DIR "/data/euroc/calib/cam_april/mav0/cam0/data" 12 | #define CAM1_IMAGE_DIR "/data/euroc/calib/cam_april/mav0/cam1/data" 13 | #define MONO_OUTPUT_DIR "/tmp/aprilgrid_test/mono" 14 | 15 | int test_calib_target() { 16 | calib_target_t calib_target; 17 | if (calib_target.load(APRILGRID_CONF) != 0) { 18 | LOG_ERROR("Failed to load calib target [%s]!", APRILGRID_CONF); 19 | return -1; 20 | } 21 | 22 | MU_CHECK(calib_target.tag_rows == 6); 23 | MU_CHECK(calib_target.tag_cols == 6); 24 | MU_CHECK(fltcmp(calib_target.tag_size, 0.088) == 0); 25 | MU_CHECK(fltcmp(calib_target.tag_spacing, 0.3) == 0); 26 | 27 | return 0; 28 | } 29 | 30 | void test_suite() { MU_ADD_TEST(test_calib_target); } 31 | 32 | } // namespace yac 33 | 34 | MU_RUN_TESTS(yac::test_suite); 35 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/cam_april/stereo_config.yaml: -------------------------------------------------------------------------------- 1 | settings: 2 | verbose: true 3 | max_num_threads: 8 4 | enable_nbv: false 5 | enable_shuffle_views: false 6 | enable_nbv_filter: false 7 | enable_outlier_filter: false 8 | enable_marginalization: false 9 | enable_early_stopping: false 10 | min_nbv_views: 40 11 | outlier_threshold: 4.0 12 | info_gain_threshold: 0.2 13 | sliding_window_size: 10 14 | early_stop_threshold: 30 15 | 16 | calib_target: 17 | target_type: 'aprilgrid' # Target type 18 | tag_rows: 6 # Number of rows 19 | tag_cols: 6 # Number of cols 20 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 21 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 22 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 23 | 24 | cam0: 25 | proj_model: "pinhole" 26 | dist_model: "radtan4" 27 | resolution: [752, 480] 28 | 29 | cam1: 30 | proj_model: "pinhole" 31 | dist_model: "radtan4" 32 | resolution: [752, 480] 33 | -------------------------------------------------------------------------------- /deps/scripts/install_suitesparse.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR/config.bash" 5 | 6 | # Install Deps 7 | apt_install libgmp3-dev 8 | apt_install libmpfr-dev 9 | apt_install libmpfr-doc 10 | apt_install libsuitesparse-dev 11 | apt_install libsuitesparse-doc 12 | 13 | # VERSION="v5.10.1" 14 | # echo "building suitesparse [$VERSION] ..." 15 | # 16 | # # Clone 17 | # if [ ! -d $INSTALL_PREFIX/src/SuiteSparse ]; then 18 | # cd $INSTALL_PREFIX/src 19 | # git clone --quiet https://github.com/DrTimothyAldenDavis/SuiteSparse 20 | # cd .. 21 | # fi 22 | # 23 | # # Install Deps 24 | # # sudo apt-get install libgmp3-dev -y -qq 25 | # # sudo apt-get install libmpfr-dev libmpfr-doc -y -qq 26 | # 27 | # # Build 28 | # cd $INSTALL_PREFIX/src/SuiteSparse 29 | # git checkout ${VERSION} --quiet 30 | # make library -j4 31 | # make install INSTALL="$INSTALL_PREFIX" 32 | 33 | sudo apt-get install -y -qq libgmp3-dev libmpfr-dev libmpfr-doc 34 | sudo apt-get install -y -qq libsuitesparse-dev 35 | sudo apt-get install -y -qq libsuitesparse-doc 36 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/cam_april/mono_config.yaml: -------------------------------------------------------------------------------- 1 | settings: 2 | # General 3 | verbose: true 4 | max_num_threads: 8 5 | 6 | # Extrinsics initialization 7 | enable_extrinsics_outlier_filter: true 8 | 9 | # Final stage settings 10 | enable_nbv: true 11 | enable_shuffle_views: true 12 | enable_nbv_filter: true 13 | enable_outlier_filter: true 14 | enable_marginalization: false 15 | enable_early_stopping: false 16 | min_nbv_views: 40 17 | outlier_threshold: 4.0 18 | info_gain_threshold: 0.2 19 | sliding_window_size: 10 20 | early_stop_threshold: 30 21 | 22 | 23 | calib_target: 24 | target_type: 'aprilgrid' # Target type 25 | tag_rows: 6 # Number of rows 26 | tag_cols: 6 # Number of cols 27 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 28 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 29 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 30 | 31 | cam0: 32 | proj_model: "pinhole" 33 | dist_model: "radtan4" 34 | resolution: [752, 480] 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) <2020> 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /yac_ros/launch/record_mocap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /scripts/blake_zisserman.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | import matplotlib.pylab as plt 4 | import sympy 5 | 6 | 7 | def diff_blake_zisserman(): 8 | 9 | s = sympy.Symbol('s') 10 | eps = sympy.Symbol('eps') 11 | 12 | expr = -sympy.log(sympy.exp(-s) + eps) 13 | expr_diff = sympy.diff(expr, s) 14 | expr_ddiff = sympy.diff(expr_diff, s) 15 | 16 | sympy.pprint(expr) 17 | sympy.pprint(sympy.simplify(expr_diff)) 18 | sympy.pprint(sympy.simplify(expr_ddiff)) 19 | 20 | 21 | diff_blake_zisserman() 22 | 23 | 24 | def blake_zisserman(s): 25 | eps = 0.000009 26 | x = eps * np.exp(s) + 1.0 27 | rho0 = -np.log(eps + np.exp(-s)) 28 | rho1 = 1.0 / x 29 | rho2 = (x - x**2) / x**3.0 30 | 31 | return (rho0, rho1, rho2) 32 | 33 | 34 | r = np.array([0.1, 0.2]) 35 | s = np.linalg.norm(r @ r) 36 | h = 1e-8 37 | 38 | (rho0, rho1, rho2) = blake_zisserman(s) 39 | (rho0_fwd, rho1_fwd, rho2_fwd) = blake_zisserman(s + h) 40 | (rho0_bwd, rho1_bwd, rho2_bwd) = blake_zisserman(s - h) 41 | 42 | J_diff = rho1 - ((rho0_fwd - rho0_bwd) / (2.0 * h)) 43 | JJ_diff = rho2 - ((rho1_fwd - rho1_bwd) / (2.0 * h)) 44 | print(J_diff) 45 | print(JJ_diff) 46 | -------------------------------------------------------------------------------- /yac/tests/test_data/core/config/config.yaml: -------------------------------------------------------------------------------- 1 | bool: true 2 | int: 1 3 | float: 2.2 4 | double: 3.3 5 | string: "hello world!" 6 | 7 | bool_array: [true, false, true, false] 8 | int_array: [1, 2, 3, 4, 5] 9 | float_array: [1.1, 2.2, 3.3, 4.4, 5.5] 10 | double_array: [1.1, 2.2, 3.3, 4.4, 5.5] 11 | string_array: ["1.1", "2.2", "3.3", "4.4", "5.5"] 12 | 13 | vector2: [1.1, 2.2] 14 | vector3: [1.1, 2.2, 3.3] 15 | vector4: [1.1, 2.2, 3.3, 4.4] 16 | vector: [1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9] 17 | 18 | matrix2: 19 | rows: 2 20 | cols: 2 21 | data: [1.1, 2.2, 22 | 3.3, 4.4] 23 | 24 | matrix3: 25 | rows: 3 26 | cols: 3 27 | data: [1.1, 2.2, 3.3, 28 | 4.4, 5.5, 6.6, 29 | 7.7, 8.8, 9.9] 30 | 31 | matrix4: 32 | rows: 4 33 | cols: 4 34 | data: [1.1, 2.2, 3.3, 4.4, 35 | 5.5, 6.6, 7.7, 8.8, 36 | 9.9, 11.0, 12.1, 13.2, 37 | 14.3, 15.4, 16.5, 17.6] 38 | 39 | matrix: 40 | rows: 3 41 | cols: 4 42 | data: [1.1, 2.2, 3.3, 4.4, 43 | 5.5, 6.6, 7.7, 8.8, 44 | 9.9, 11.0, 12.1, 13.2] 45 | 46 | level3: 47 | a: 48 | b: 49 | c: 3 50 | d: [1e-6, 1e-6, 1e-6] 51 | -------------------------------------------------------------------------------- /scripts/lissajous.m: -------------------------------------------------------------------------------- 1 | %% lissajous 2 | % written by Stefan Leutenegger 03/2022 3 | 4 | %% parameters 5 | fps = 30; % animation framerate (approx) 6 | 7 | % distance from target -- point will move on sphere 8 | R = 1.5; 9 | 10 | % lissajous curve parameters 11 | a = 1; 12 | b = 0.5; 13 | m = 1; 14 | n = 2; 15 | d = 0.5; 16 | 17 | % timing 18 | T = 3.0; % [sec] 19 | 20 | %% static plot 21 | phi = 0:0.01:2*pi; 22 | x = [a*cos(m*phi); 23 | b*cos(n*phi+pi*d)]; 24 | z = sqrt(R^2-x(1,:).^2-x(2,:).^2); 25 | plot3(x(1,:),x(2,:), z(:)) 26 | axis equal 27 | hold on 28 | 29 | % initial point 30 | phi0 = 0; 31 | x = [a*cos(m*phi0); 32 | b*cos(n*phi0+pi*d)]; 33 | z = sqrt(R^2-x(1)^2-x(2)^2); 34 | p = plot3(x(1),x(2),z,'rx'); 35 | 36 | %% Loop on the line angle (from 0 radians to 2pi radians 37 | for t=0:1.0/fps:T 38 | phi=2*pi*sin(pi/2*t/T)^2; 39 | % Update the line coordinates 40 | x = [a*cos(m*phi); 41 | b*cos(n*phi+pi*d)]; 42 | z = sqrt(R^2-x(1)^2-x(2)^2); 43 | set (p, 'XData', x(1), 'YData',x(2), 'ZData',z ); 44 | 45 | % Refresh the figure 46 | drawnow; 47 | 48 | % run at desired framerate 49 | pause(1.0/fps) 50 | end 51 | ginput(); 52 | 53 | -------------------------------------------------------------------------------- /scripts/marg_sandbox.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | import pandas 4 | import matplotlib.pylab as plt 5 | 6 | m = 6 7 | H = pandas.read_csv("/tmp/H.csv", header=None).to_numpy() 8 | b = pandas.read_csv("/tmp/b.csv", header=None).to_numpy() 9 | 10 | 11 | def shur_complement(H, b): 12 | # Schur complement 13 | H_mm = H[0:m, 0:m] 14 | H_mr = H[0:m, m:] 15 | H_rm = H[m:, 0:m] 16 | H_rr = H[m:, m:] 17 | 18 | b_mm = b[:m] 19 | b_rr = b[m:] 20 | 21 | H_mm = 0.5 * (H_mm + H_mm.T) # Enforce symmetry 22 | w, V = np.linalg.eig(H_mm) 23 | for idx, w_i in enumerate(w): 24 | if w_i < 1e-12: 25 | w[idx] = 0.0 26 | 27 | Lambda_inv = np.diag(1.0 / w) 28 | H_mm_inv = V @ Lambda_inv @ V.T 29 | H_marg = H_rr - (H_rm @ H_mm_inv @ H_mr) 30 | b_marg = b_rr - (H_rm @ H_mm_inv @ b_mm) 31 | # print(((H_mm @ H_mm_inv) - np.eye(m, m)).sum()) 32 | 33 | return (H_marg, b_marg) 34 | 35 | 36 | def eig_decomp(H): 37 | # Decompose H into JtJ 38 | w, V = np.linalg.eig(H) 39 | for idx, w_i in enumerate(w): 40 | if w_i < 1e-12: 41 | w[idx] = 0.0 42 | 43 | S_sqrt = np.diag(w**0.5) 44 | J = S_sqrt @ V.T 45 | 46 | return J 47 | 48 | 49 | # print(np.max(((J.T @ J) - H_marg))) 50 | # plt.imshow(J.T @ J - H_marg) 51 | # plt.colorbar() 52 | # plt.show() 53 | 54 | # plt.imshow(H) 55 | # plt.colorbar() 56 | # plt.show() 57 | -------------------------------------------------------------------------------- /scripts/plot_camera.py: -------------------------------------------------------------------------------- 1 | """ Plot camera view """ 2 | #!/usr/bin/env python3 3 | from os import listdir 4 | from os.path import join 5 | import numpy as np 6 | import matplotlib.pylab as plt 7 | from matplotlib import animation 8 | 9 | from proto import AprilGrid 10 | 11 | 12 | def parse_timestamp(fname): 13 | """ Parse timestamp """ 14 | ts, _ = fname.split(".") 15 | return int(ts) 16 | 17 | 18 | if __name__ == "__main__": 19 | # Load data 20 | save_path = "/tmp/sim_cam/animation.mp4" 21 | grid_path = "/tmp/sim_cam/cam0" 22 | grid_fnames = listdir(grid_path) 23 | 24 | # Figure setup 25 | fig = plt.figure() 26 | ax = plt.axes() 27 | ax.set_xlim([0, 640]) 28 | ax.set_ylim([0, 480]) 29 | ax.set_xlabel("pixel") 30 | ax.set_ylabel("pixel") 31 | ax.invert_yaxis() 32 | ax.xaxis.tick_top() 33 | ax.xaxis.set_label_position('top') 34 | 35 | # Setup FFMpeg writer 36 | writer = animation.FFMpegWriter(fps=30) 37 | writer.setup(fig, save_path, 100) 38 | 39 | # Animate 40 | for grid_fname in sorted(grid_fnames, key=parse_timestamp): 41 | grid = AprilGrid.load(join(grid_path, grid_fname)) 42 | grid_data = grid.get_measurements() 43 | 44 | kps = [] 45 | for tag_id, corner_idx, obj_pt, kp in grid_data: 46 | kps.append(kp) 47 | kps = np.array(kps) 48 | 49 | points = ax.plot(kps[:, 0], kps[:, 1], 'r.')[0] 50 | writer.grab_frame() 51 | ax.lines.remove(points) 52 | -------------------------------------------------------------------------------- /docs/kalibr_notes.txt: -------------------------------------------------------------------------------- 1 | Kalibr AprilGrid front-end 2 | -------------------------- 3 | 4 | - AprilTags by Michael Kaess 5 | - Remove tag if distance to image border is less than 4px 6 | - Remove tag if tag is not 'good' (calculated by AprilTags)! 7 | - Perform corner subpixel refinement 8 | - Remove corner if refined corner is ~1.2pixels away from prev 9 | - Mark Grid detection failed if less than 4 tags detected 10 | 11 | 12 | Kalibr Camera Calibration 13 | ------------------------- 14 | 15 | Optimizes for: 16 | - Camera intrinsics 17 | - Camera-camera extrinsics 18 | - Relative camera-target pose 19 | - Target pose 20 | 21 | Optimization settings: 22 | - Sigma reprojectio: 1.0 23 | - No losses 24 | 25 | Calibration process: 26 | - Initializes the camera intrinsics for each camera 27 | - By solving BA 28 | - Initializes the camera to camera transforms 29 | - By solving BA 30 | - Solves the optimization problem in an incremental manner 31 | - Removes outliers from current views 32 | - Removes view if information gain isn't high enough 33 | - svLog2Sum = S.head(rank).array().log().sum() / log(2) 34 | - 0.5 - (svLog2Sum_now - svLog2Sum_prev) 35 | 36 | 37 | Kalibr IMU-Camera Calibration 38 | ------------------------- 39 | 40 | Optimizes for: 41 | - imu-cam0, imu-cam1, ... extrinsics 42 | - imu sensor poses 43 | - Target orientation 44 | 45 | Optimization settings: 46 | - Sigma reprojectio: 1.0 47 | - No losses 48 | - Max iter: 30 49 | -------------------------------------------------------------------------------- /yac/tests/util/test_timeline.cpp: -------------------------------------------------------------------------------- 1 | #include "../munit.hpp" 2 | #include "util/timeline.hpp" 3 | 4 | namespace yac { 5 | 6 | int test_timeline_event() { 7 | timeline_event_t event("event_t", 0); 8 | return 0; 9 | } 10 | 11 | int test_camera_event() { 12 | const timestamp_t ts = 0; 13 | const int camera_index = 0; 14 | const std::string image_path = ""; 15 | camera_event_t event(ts, camera_index, image_path); 16 | return 0; 17 | } 18 | 19 | int test_imu_event() { 20 | const timestamp_t ts = 0; 21 | const vec3_t acc{0.0, 0.0, 0.0}; 22 | const vec3_t gyr{0.0, 0.0, 0.0}; 23 | imu_event_t event(ts, acc, gyr); 24 | return 0; 25 | } 26 | 27 | int test_aprilgrid_event() { 28 | const timestamp_t ts = 0; 29 | const int camera_index = 0; 30 | const aprilgrid_t grid; 31 | aprilgrid_event_t event(ts, camera_index, grid); 32 | return 0; 33 | } 34 | 35 | int test_mocap_event() { 36 | const timestamp_t ts = 0; 37 | const std::string object_name = "drone"; 38 | const quat_t q_WM{1.0, 0.0, 0.0, 0.0}; 39 | const vec3_t r_WM{0.0, 0.0, 0.0}; 40 | mocap_event_t event(ts, object_name, q_WM, r_WM); 41 | return 0; 42 | } 43 | 44 | void test_suite() { 45 | MU_ADD_TEST(test_timeline_event); 46 | MU_ADD_TEST(test_camera_event); 47 | MU_ADD_TEST(test_imu_event); 48 | MU_ADD_TEST(test_aprilgrid_event); 49 | MU_ADD_TEST(test_mocap_event); 50 | } 51 | 52 | } // namespace yac 53 | 54 | MU_RUN_TESTS(yac::test_suite); 55 | -------------------------------------------------------------------------------- /deps/scripts/install.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e # Exit on first error 3 | BASEDIR=$(dirname "$0") 4 | source "$BASEDIR"/config.bash 5 | 6 | export DEBIAN_FRONTEND="noninteractive" 7 | export DOWNLOAD_PATH=$DOWNLOAD_PATH 8 | mkdir -p "$INSTALL_PREFIX" 9 | mkdir -p "$DOWNLOAD_PATH" 10 | 11 | install() { 12 | echo "Installing $1 ... "; 13 | bash $BASEDIR/install_$1.bash 14 | # if [ "$BASEDIR"/install_"$1".bash > "install_${1}.log" 2>&1 ]; then 15 | # echo "DONE!" 16 | # else 17 | # echo "FAILED!" 18 | # cat "install_${1}.log" 19 | # fi 20 | } 21 | 22 | install_base() { 23 | apt_install \ 24 | dialog \ 25 | apt-utils \ 26 | git \ 27 | mercurial \ 28 | cmake \ 29 | g++ \ 30 | clang \ 31 | wget 32 | # libasan4 \ 33 | # libasan5 34 | } 35 | 36 | check_ubuntu_version() { 37 | if [ -x "$(command -v lsb_release)" ]; then 38 | VERSION=$(lsb_release -r | grep -oP '(?<=Release:\s)[0-9.]+') 39 | if [ $? -eq 0 ] && [ $VERSION != $UBUNTU_VERSION ]; then 40 | echo "This script is only for Ubuntu $UBUNTU_VERSION" 41 | exit -1; 42 | fi 43 | else 44 | # lsb_release command is not installed - Not ubuntu 45 | exit -1; 46 | fi 47 | } 48 | 49 | check_ubuntu_version 50 | apt_update 51 | install_base 52 | # install ros 53 | install eigen 54 | install suitesparse 55 | install ceres 56 | install opencv 57 | install yamlcpp 58 | install apriltag 59 | install apriltag3 60 | -------------------------------------------------------------------------------- /yac/tests/test_data/mocap_data/cam0/data.csv: -------------------------------------------------------------------------------- 1 | timestamp [ns],filename 2 | 1606153907495166540,1606153907495166540.png 3 | 1606153915653051138,1606153915653051138.png 4 | 1606153921853466272,1606153921853466272.png 5 | 1606153950890122890,1606153950890122890.png 6 | 1606153963139782667,1606153963139782667.png 7 | 1606153971086688995,1606153971086688995.png 8 | 1606153977721161127,1606153977721161127.png 9 | 1606153983972179413,1606153983972179413.png 10 | 1606153997421799183,1606153997421799183.png 11 | 1606154004360154629,1606154004360154629.png 12 | 1606154011402862549,1606154011402862549.png 13 | 1606154017373862028,1606154017373862028.png 14 | 1606154022959313869,1606154022959313869.png 15 | 1606154029409966230,1606154029409966230.png 16 | 1606154036334099054,1606154036334099054.png 17 | 1606154045208428621,1606154045208428621.png 18 | 1606154057587632418,1606154057587632418.png 19 | 1606154065240525723,1606154065240525723.png 20 | 1606154073179670334,1606154073179670334.png 21 | 1606154080918648243,1606154080918648243.png 22 | 1606154091296098471,1606154091296098471.png 23 | 1606154103557152987,1606154103557152987.png 24 | 1606154110540388823,1606154110540388823.png 25 | 1606154117645546675,1606154117645546675.png 26 | 1606154125251100540,1606154125251100540.png 27 | 1606154131806932688,1606154131806932688.png 28 | 1606154140441553831,1606154140441553831.png 29 | 1606154148298093319,1606154148298093319.png 30 | 1606154155277716398,1606154155277716398.png 31 | 1606154165058666229,1606154165058666229.png 32 | -------------------------------------------------------------------------------- /yac_ros/config/intel_d435i.yaml: -------------------------------------------------------------------------------- 1 | ros: 2 | data_path: "/tmp/calib_data" 3 | cam0_topic: "/rs/ir0/image" 4 | cam1_topic: "/rs/ir1/image" 5 | imu0_topic: "/rs/imu0/data" 6 | 7 | settings: 8 | info_gain_threshold: 0.2 9 | 10 | calib_target: 11 | target_type: 'aprilgrid' # Target type 12 | tag_rows: 6 # Number of rows 13 | tag_cols: 6 # Number of cols 14 | tag_size: 0.0375 # Size of apriltag, edge to edge [m] 15 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 16 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 17 | 18 | cam0: 19 | resolution: [640, 480] 20 | proj_model: "pinhole" 21 | dist_model: "radtan4" 22 | 23 | cam1: 24 | resolution: [640, 480] 25 | proj_model: "pinhole" 26 | dist_model: "radtan4" 27 | 28 | imu0: 29 | rate: 250 30 | a_max: 160.0 # acceleration saturation [m/s^2] 31 | g_max: 10.0 # gyro saturation [rad/s] 32 | sigma_g_c: 0.00278 # gyro noise density [rad/s/sqrt(Hz)] 33 | sigma_a_c: 0.0252 # accelerometer noise density [m/s^2/sqrt(Hz)] 34 | sigma_gw_c: 1.65e-05 # gyro drift noise density [rad/s^s/sqrt(Hz)] 35 | sigma_aw_c: 0.000441 # accelerometer drift noise density [m/s^2/sqrt(Hz)] 36 | sigma_bg: 0.03 # gyro bias prior [rad/s] 37 | sigma_ba: 0.1 # accelerometer bias prior [m/s^2] 38 | tau: 3600.0 # reversion time constant, currently not in use [s] 39 | g: 9.81007 # Earth's acceleration due to gravity [m/s^2] 40 | a0: [ 0.0, 0.0, 0.0 ] # Accelerometer bias [m/s^2] 41 | -------------------------------------------------------------------------------- /yac/lib/util/ceres_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_CERES_UTILS_HPP 2 | #define YAC_CERES_UTILS_HPP 3 | 4 | #include 5 | 6 | #include "core.hpp" 7 | 8 | namespace yac { 9 | 10 | // Plus matrix of a quaternion. 11 | // i.e. q_AB * q_BC = plus(q_AB) * q_BC.coeffs(). 12 | Eigen::Matrix4d plus(const quat_t &q_AB); 13 | 14 | // Oplus matrix of a quaternion, 15 | // Right quaternion product matrix 16 | // i.e. q_AB * q_BC = oplus(q_BC) * q_AB.coeffs(). 17 | mat4_t oplus(const quat_t &q_BC); 18 | 19 | // Right Jacobian, see Forster et al. RSS 2015 eqn. (8) 20 | mat3_t rightJacobian(const vec3_t &PhiVec); 21 | 22 | // Delta quaternion. 23 | quat_t deltaQ(const vec3_t &dAlpha); 24 | 25 | // Calculate lift jacobian 26 | mat_t<6, 7, row_major_t> lift_pose_jacobian(const mat4_t pose); 27 | 28 | // Evaluate residual block 29 | bool evaluate_residual_block(const ceres::Problem &problem, 30 | const ceres::ResidualBlockId res_id, 31 | vec2_t &r); 32 | 33 | // Pose local parameterization 34 | class PoseLocalParameterization : public ceres::LocalParameterization { 35 | public: 36 | PoseLocalParameterization(); 37 | virtual ~PoseLocalParameterization(); 38 | virtual bool Plus(const double *x, 39 | const double *delta, 40 | double *x_plus_delta) const; 41 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 42 | virtual int GlobalSize() const { return 7; } 43 | virtual int LocalSize() const { return 6; } 44 | }; 45 | 46 | } // namespace yac 47 | #endif // YAC_CERES_UTILS_HPP 48 | -------------------------------------------------------------------------------- /yac/tests/util/test_data.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "../munit.hpp" 4 | #include "util/data.hpp" 5 | 6 | namespace yac { 7 | 8 | #ifndef TEST_PATH 9 | #define TEST_PATH "." 10 | #endif 11 | 12 | #define TEST_DATA TEST_PATH "/test_data/core/data/matrix.dat" 13 | #define TEST_OUTPUT "/tmp/matrix.dat" 14 | 15 | int test_csv_rows() { 16 | int rows; 17 | rows = csv_rows(TEST_DATA); 18 | MU_CHECK(rows == 281); 19 | return 0; 20 | } 21 | 22 | int test_csv_cols() { 23 | int cols; 24 | cols = csv_cols(TEST_DATA); 25 | MU_CHECK(cols == 2); 26 | return 0; 27 | } 28 | 29 | int test_csv2mat() { 30 | matx_t data; 31 | 32 | csv2mat(TEST_DATA, true, data); 33 | MU_CHECK(data.rows() == 280); 34 | MU_CHECK(data.cols() == 2); 35 | MU_CHECK_NEAR(-2.22482078596, data(0, 0), 1e-4); 36 | MU_CHECK_NEAR(9.9625789766, data(0, 1), 1e-4); 37 | MU_CHECK_NEAR(47.0485650525, data(279, 0), 1e-4); 38 | MU_CHECK_NEAR(613.503760567, data(279, 1), 1e-4); 39 | 40 | return 0; 41 | } 42 | 43 | int test_mat2csv() { 44 | matx_t x; 45 | matx_t y; 46 | 47 | csv2mat(TEST_DATA, true, x); 48 | mat2csv(TEST_OUTPUT, x); 49 | csv2mat(TEST_OUTPUT, false, y); 50 | 51 | for (int i = 0; i < x.rows(); i++) { 52 | for (int j = 0; j < x.cols(); j++) { 53 | MU_CHECK_NEAR(x(i, j), y(i, j), 0.1); 54 | } 55 | } 56 | 57 | return 0; 58 | } 59 | 60 | void test_suite() { 61 | MU_ADD_TEST(test_csv_rows); 62 | MU_ADD_TEST(test_csv_cols); 63 | MU_ADD_TEST(test_csv2mat); 64 | MU_ADD_TEST(test_mat2csv); 65 | } 66 | 67 | } // namespace yac 68 | 69 | MU_RUN_TESTS(yac::test_suite); 70 | -------------------------------------------------------------------------------- /yac/tests/util/test_fs.cpp: -------------------------------------------------------------------------------- 1 | #include "../munit.hpp" 2 | #include "util/fs.hpp" 3 | 4 | namespace yac { 5 | 6 | #ifndef TEST_PATH 7 | #define TEST_PATH "." 8 | #endif 9 | 10 | #define TEST_CONFIG TEST_PATH "/test_data/core/config/config.yaml" 11 | #define TEST_BAD_CONFIG TEST_PATH "/test_data/core/config/bogus.yaml" 12 | 13 | int test_file_exists() { 14 | bool retval; 15 | 16 | retval = file_exists(TEST_CONFIG); 17 | MU_CHECK(retval == true); 18 | 19 | retval = file_exists(TEST_BAD_CONFIG); 20 | MU_CHECK(retval == false); 21 | 22 | return 0; 23 | } 24 | 25 | int test_path_split() { 26 | std::vector splits; 27 | 28 | splits = path_split("/a/b/c.yaml"); 29 | MU_CHECK(3 == (int) splits.size()); 30 | MU_CHECK("a" == splits[0]); 31 | MU_CHECK("b" == splits[1]); 32 | MU_CHECK("c.yaml" == splits[2]); 33 | 34 | return 0; 35 | } 36 | 37 | int test_paths_join() { 38 | std::string out; 39 | 40 | out = paths_join("/a/b/c", "../"); 41 | std::cout << out << std::endl; 42 | MU_CHECK("/a/b" == out); 43 | 44 | out = paths_join("/a/b/c", "../.."); 45 | std::cout << out << std::endl; 46 | MU_CHECK("/a" == out); 47 | 48 | out = paths_join("/a/b/c", "d/e"); 49 | std::cout << out << std::endl; 50 | MU_CHECK("/a/b/c/d/e" == out); 51 | 52 | out = paths_join("./a/b/c", "../d/e"); 53 | std::cout << out << std::endl; 54 | MU_CHECK("./a/b/d/e" == out); 55 | 56 | return 0; 57 | } 58 | 59 | void test_suite() { 60 | MU_ADD_TEST(test_file_exists); 61 | MU_ADD_TEST(test_path_split); 62 | MU_ADD_TEST(test_paths_join); 63 | } 64 | 65 | } // namespace yac 66 | 67 | MU_RUN_TESTS(yac::test_suite); 68 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic-perception-focal 2 | SHELL ["/bin/bash", "-c"] 3 | ENV HOME /root 4 | 5 | # Permissions 6 | RUN apt-get update && apt-get install -y sudo 7 | ARG USER=docker 8 | ARG PASS=docker 9 | ARG UID=1000 10 | ARG GID=1000 11 | RUN useradd -m ${USER} --uid=${UID} && echo "${USER}:${PASS}" | chpasswd 12 | RUN adduser ${USER} sudo 13 | ENV HOME /home/$USER 14 | 15 | # Install basic things 16 | RUN apt-get update -yq 17 | RUN apt-get update && apt-get install -qq -y \ 18 | sudo \ 19 | lsb-release \ 20 | build-essential \ 21 | git \ 22 | cmake \ 23 | vim \ 24 | wget 25 | 26 | # Install catkin-tools 27 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" \ 28 | > /etc/apt/sources.list.d/ros-latest.list' 29 | RUN wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 30 | RUN apt-get update -yq 31 | RUN apt-get install -qq -y python3-catkin-tools 32 | 33 | # Build yac 34 | WORKDIR /usr/local/src 35 | RUN git clone https://github.com/chutsu/yac 36 | WORKDIR /usr/local/src/yac 37 | RUN git checkout iros2023 38 | RUN make deps 39 | RUN make lib 40 | 41 | # Switch to $USER 42 | USER $USER 43 | RUN echo 'export PS1="[\u@docker] \W # "' >> $HOME/.bashrc 44 | 45 | # Update environment variables 46 | ENV LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:/opt/yac/lib" 47 | ENV PATH="${PATH}:/opt/yac/bin" 48 | 49 | # Create catkin_ws 50 | WORKDIR $HOME 51 | RUN mkdir -p $HOME/catkin_ws/src/ 52 | RUN git clone https://github.com/chutsu/rs4se 53 | RUN ln -s /usr/local/src/yac/yac_ros $HOME/catkin_ws/src/yac_ros 54 | RUN . /opt/ros/noetic/setup.sh \ 55 | && cd $HOME/catkin_ws \ 56 | && catkin init \ 57 | && catkin build 58 | -------------------------------------------------------------------------------- /yac_ros/config/euroc-calib_imucam.yaml: -------------------------------------------------------------------------------- 1 | ros: 2 | bag: "/data/euroc/rosbags/imu_april.bag" 3 | cam0_topic: "/cam0/image_raw" 4 | cam1_topic: "/cam1/image_raw" 5 | imu0_topic: "/imu0" 6 | 7 | settings: 8 | data_path: "/tmp/imu_april" 9 | results_path: "/tmp/imu_april/calib_results.yaml" 10 | 11 | calib_target: 12 | target_type: 'aprilgrid' # Target type 13 | tag_rows: 6 # Number of rows 14 | tag_cols: 6 # Number of cols 15 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 16 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 17 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 18 | 19 | imu0: 20 | rate: 200.0 21 | sigma_a_c: 0.002 22 | sigma_g_c: 1.6968e-04 23 | sigma_aw_c: 0.003 24 | sigma_gw_c: 1.9393e-05 25 | g: 9.81007 26 | 27 | cam0: 28 | resolution: [752, 480] 29 | proj_model: "pinhole" 30 | dist_model: "radtan4" 31 | proj_params: [458.654, 457.296, 367.215, 248.375] 32 | dist_params: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 33 | 34 | cam1: 35 | resolution: [752, 480] 36 | proj_model: "pinhole" 37 | dist_model: "radtan4" 38 | proj_params: [457.587, 456.134, 379.999, 255.238] 39 | dist_params: [-0.28368365, 0.07451284, -0.00010473, -3.555e-05] 40 | 41 | T_cam0_cam1: 42 | rows: 4 43 | cols: 4 44 | data: [0.999997256478, -0.002317135723, -0.000343393121, 0.110074137800, 45 | 0.002312067192, 0.999898048507, -0.014090668453, -0.000156612054, 46 | 0.000376008102, 0.014089835847, 0.999900662638, 0.000889382785, 47 | 0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000] 48 | -------------------------------------------------------------------------------- /yac/demo/calib_inspect.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_camera.hpp" 2 | #include "calib_vi.hpp" 3 | 4 | void print_usage(char *argv[]) { 5 | printf("usage: %s \n", argv[0]); 6 | printf("examples:\n"); 7 | printf(" %s camera config.yaml /data/euroc\n", argv[0]); 8 | printf(" %s camera-imu config.yaml /data/euroc\n", argv[0]); 9 | } 10 | 11 | std::map load_dataset(const std::string data_path) { 12 | const yac::calib_target_t calib_target; 13 | std::map cam_paths; 14 | cam_paths[0] = data_path + "/mav0/cam0/data"; 15 | cam_paths[1] = data_path + "/mav0/cam1/data"; 16 | const std::string grids_path = data_path + "/mav0/grid0"; 17 | return yac::calib_data_preprocess(calib_target, cam_paths, grids_path); 18 | } 19 | 20 | int main(int argc, char *argv[]) { 21 | // Check command-line args 22 | if (argc != 4) { 23 | print_usage(argv); 24 | return -1; 25 | } 26 | 27 | // Parse commandline args 28 | const std::string mode = argv[1]; 29 | const std::string config_file = argv[2]; 30 | const std::string dataset_path = argv[3]; 31 | 32 | // Inspect 33 | if (mode == "camera") { 34 | const auto inspect_data = load_dataset(dataset_path); 35 | yac::calib_camera_t calib{config_file}; 36 | calib.inspect(inspect_data); 37 | 38 | } else if (mode == "camera-imu") { 39 | yac::calib_vi_t calib{config_file}; 40 | calib.max_iter = 0; 41 | calib.enable_outlier_rejection = false; 42 | calib.load_data(dataset_path); 43 | calib.solve(); 44 | calib.print_stats(stdout); 45 | 46 | } else { 47 | FATAL("Unrecognized inspection mode [%s]!", mode.c_str()); 48 | } 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /scripts/plot_stats.py: -------------------------------------------------------------------------------- 1 | """ Plot Stats """ 2 | #!/usr/bin/env python3 3 | import numpy as np 4 | import pandas 5 | import matplotlib.pylab as plt 6 | import matplotlib as mpl 7 | 8 | # Load statistics 9 | stats_data = pandas.read_csv("/tmp/calib-stats.csv") 10 | 11 | # Residuals 12 | rx = stats_data["rx"].to_numpy() 13 | ry = stats_data["ry"].to_numpy() 14 | 15 | # Reprojection Errors 16 | reproj_errors = [] 17 | for i in range(len(rx)): 18 | r = np.array([rx[i], ry[i]]) 19 | reproj_errors.append(np.linalg.norm(r)) 20 | reproj_errors = np.array(reproj_errors) 21 | reproj_errors_max = np.max(reproj_errors) 22 | reproj_errors_median = np.median(reproj_errors) 23 | reproj_errors_mean = np.mean(reproj_errors) 24 | reproj_errors_stddev = np.std(reproj_errors) 25 | 26 | # Histogram - Residuals x-axis 27 | plt.figure() 28 | plt.hist(rx, bins=200) 29 | plt.xlim([-1.0, 1.0]) 30 | plt.title("Residuals in x-axis") 31 | plt.xlabel("Residual [px]") 32 | plt.ylabel("Frequency") 33 | 34 | # Histogram - Residuals y-axis 35 | plt.figure() 36 | plt.hist(ry, bins=200) 37 | plt.xlim([-1.0, 1.0]) 38 | plt.title("Residuals in y-axis") 39 | plt.xlabel("Residual [px]") 40 | plt.ylabel("Frequency") 41 | 42 | # Scatter XY Plot 43 | plt.figure() 44 | 45 | cmap = plt.cm.get_cmap("jet") 46 | ax = plt.subplot(111) 47 | ax.scatter(rx, ry, c=reproj_errors, cmap=cmap, vmax=2.0) 48 | 49 | ax.grid(True) 50 | ax.spines['left'].set_position('zero') 51 | ax.spines['right'].set_color('none') 52 | ax.spines['bottom'].set_position('zero') 53 | ax.spines['top'].set_color('none') 54 | 55 | # max_val = reproj_errors_stddev * 10.0 56 | # max_val = reproj_errors_max 57 | # max_val = 5.0 58 | max_val = 3.0 59 | ax.set_xlim([-max_val, max_val]) 60 | ax.set_ylim([-max_val, max_val]) 61 | 62 | plt.show() 63 | -------------------------------------------------------------------------------- /yac_ros/launch/intel_d435i.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /yac_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | yac_ros 4 | 0.0.1 5 | Prototype ros interface 6 | 7 | 8 | Chris Choi 9 | 10 | 11 | catkin 12 | 13 | 14 | roscpp 15 | rospy 16 | rosbag 17 | message_runtime 18 | message_generation 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | external_gps_msgs 23 | tf 24 | tf2 25 | tf2_ros 26 | tf2_geometry_msgs 27 | cv_bridge 28 | image_transport 29 | 30 | roscpp 31 | rospy 32 | rosbag 33 | message_runtime 34 | message_generation 35 | std_msgs 36 | geometry_msgs 37 | sensor_msgs 38 | external_gps_msgs 39 | tf 40 | tf2 41 | tf2_ros 42 | tf2_geometry_msgs 43 | cv_bridge 44 | image_transport 45 | 46 | 47 | GPLv3 48 | 49 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463182799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463182799872,6,6,0.088000,0.300000,25.5337944031,45.3681526184,0.457600,0.457600,0.000000,28,0 3 | 1404733463182799872,6,6,0.088000,0.300000,12.3127279282,80.9425888062,0.545600,0.457600,0.000000,28,1 4 | 1404733463182799872,6,6,0.088000,0.300000,41.5288352966,98.9333572388,0.545600,0.545600,0.000000,28,2 5 | 1404733463182799872,6,6,0.088000,0.300000,55.6869659424,63.2812995911,0.457600,0.545600,0.000000,28,3 6 | 1404733463182799872,6,6,0.088000,0.300000,87.6996154785,20.2681560516,0.343200,0.572000,0.000000,33,0 7 | 1404733463182799872,6,6,0.088000,0.300000,70.1503448486,57.7128372192,0.431200,0.572000,0.000000,33,1 8 | 1404733463182799872,6,6,0.088000,0.300000,102.6929092407,77.3447036743,0.431200,0.660000,0.000000,33,2 9 | 1404733463182799872,6,6,0.088000,0.300000,121.5324783325,39.8137474060,0.343200,0.660000,0.000000,33,3 10 | 1404733463182799872,6,6,0.088000,0.300000,65.3508758545,69.0614471436,0.457600,0.572000,0.000000,34,0 11 | 1404733463182799872,6,6,0.088000,0.300000,50.6941299438,104.4102172852,0.545600,0.572000,0.000000,34,1 12 | 1404733463182799872,6,6,0.088000,0.300000,82.0325088501,123.3835449219,0.545600,0.660000,0.000000,34,2 13 | 1404733463182799872,6,6,0.088000,0.300000,97.6077880859,88.2454605103,0.457600,0.660000,0.000000,34,3 14 | 1404733463182799872,6,6,0.088000,0.300000,46.7736968994,114.8091659546,0.572000,0.572000,0.000000,35,0 15 | 1404733463182799872,6,6,0.088000,0.300000,35.1048240662,147.4707031250,0.660000,0.572000,0.000000,35,1 16 | 1404733463182799872,6,6,0.088000,0.300000,65.2010345459,165.7202911377,0.660000,0.660000,0.000000,35,2 17 | 1404733463182799872,6,6,0.088000,0.300000,77.7933044434,133.4159851074,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463132800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463132800000,6,6,0.088000,0.300000,27.9481334686,19.3736686707,0.343200,0.457600,0.000000,27,0 3 | 1404733463132800000,6,6,0.088000,0.300000,14.1645078659,55.2622871399,0.431200,0.457600,0.000000,27,1 4 | 1404733463132800000,6,6,0.088000,0.300000,41.6627082825,72.3419570923,0.431200,0.545600,0.000000,27,2 5 | 1404733463132800000,6,6,0.088000,0.300000,56.5199203491,36.2010421753,0.343200,0.545600,0.000000,27,3 6 | 1404733463132800000,6,6,0.088000,0.300000,65.5063705444,41.6157722473,0.343200,0.572000,0.000000,33,0 7 | 1404733463132800000,6,6,0.088000,0.300000,50.5557327271,77.5622406006,0.431200,0.572000,0.000000,33,1 8 | 1404733463132800000,6,6,0.088000,0.300000,80.5144348145,96.0005111694,0.431200,0.660000,0.000000,33,2 9 | 1404733463132800000,6,6,0.088000,0.300000,96.4523544312,60.0869216919,0.343200,0.660000,0.000000,33,3 10 | 1404733463132800000,6,6,0.088000,0.300000,46.4929008484,88.3534088135,0.457600,0.572000,0.000000,34,0 11 | 1404733463132800000,6,6,0.088000,0.300000,34.4072113037,121.9346466064,0.545600,0.572000,0.000000,34,1 12 | 1404733463132800000,6,6,0.088000,0.300000,63.1482429504,139.9988555908,0.545600,0.660000,0.000000,34,2 13 | 1404733463132800000,6,6,0.088000,0.300000,76.1769943237,106.4554519653,0.457600,0.660000,0.000000,34,3 14 | 1404733463132800000,6,6,0.088000,0.300000,31.1564083099,131.8328857422,0.572000,0.572000,0.000000,35,0 15 | 1404733463132800000,6,6,0.088000,0.300000,21.5688838959,162.8116455078,0.660000,0.572000,0.000000,35,1 16 | 1404733463132800000,6,6,0.088000,0.300000,49.1881256104,180.3125915527,0.660000,0.660000,0.000000,35,2 17 | 1404733463132800000,6,6,0.088000,0.300000,59.6295928955,149.5513153076,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463182799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463182799872,6,6,0.088000,0.300000,22.5238437653,21.2177200317,0.343200,0.457600,0.000000,27,0 3 | 1404733463182799872,6,6,0.088000,0.300000,8.7381668091,57.1533050537,0.431200,0.457600,0.000000,27,1 4 | 1404733463182799872,6,6,0.088000,0.300000,36.4104347229,74.1517028809,0.431200,0.545600,0.000000,27,2 5 | 1404733463182799872,6,6,0.088000,0.300000,50.9128532410,37.8402900696,0.343200,0.545600,0.000000,27,3 6 | 1404733463182799872,6,6,0.088000,0.300000,59.8654518127,43.3170394897,0.343200,0.572000,0.000000,33,0 7 | 1404733463182799872,6,6,0.088000,0.300000,45.1845397949,79.3660049438,0.431200,0.572000,0.000000,33,1 8 | 1404733463182799872,6,6,0.088000,0.300000,74.9729156494,97.6497955322,0.431200,0.660000,0.000000,33,2 9 | 1404733463182799872,6,6,0.088000,0.300000,90.7871246338,61.5278549194,0.343200,0.660000,0.000000,33,3 10 | 1404733463182799872,6,6,0.088000,0.300000,41.1436424255,90.1615066528,0.457600,0.572000,0.000000,34,0 11 | 1404733463182799872,6,6,0.088000,0.300000,29.2299747467,123.7925033569,0.545600,0.572000,0.000000,34,1 12 | 1404733463182799872,6,6,0.088000,0.300000,57.8147277832,141.6756591797,0.545600,0.660000,0.000000,34,2 13 | 1404733463182799872,6,6,0.088000,0.300000,70.7474822998,108.1356048584,0.457600,0.660000,0.000000,34,3 14 | 1404733463182799872,6,6,0.088000,0.300000,25.9148216248,133.6909027100,0.572000,0.572000,0.000000,35,0 15 | 1404733463182799872,6,6,0.088000,0.300000,16.6074829102,164.7892150879,0.660000,0.572000,0.000000,35,1 16 | 1404733463182799872,6,6,0.088000,0.300000,44.2094306946,182.1504211426,0.660000,0.660000,0.000000,35,2 17 | 1404733463182799872,6,6,0.088000,0.300000,54.5316810608,151.3518524170,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463232800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463232800000,6,6,0.088000,0.300000,20.8553524017,24.3057308197,0.343200,0.457600,0.000000,27,0 3 | 1404733463232800000,6,6,0.088000,0.300000,7.4664516449,60.3014755249,0.431200,0.457600,0.000000,27,1 4 | 1404733463232800000,6,6,0.088000,0.300000,34.8505020142,77.0869674683,0.431200,0.545600,0.000000,27,2 5 | 1404733463232800000,6,6,0.088000,0.300000,49.3709678650,40.7618064880,0.343200,0.545600,0.000000,27,3 6 | 1404733463232800000,6,6,0.088000,0.300000,58.4136276245,46.2577285767,0.343200,0.572000,0.000000,33,0 7 | 1404733463232800000,6,6,0.088000,0.300000,43.7195625305,82.3010559082,0.431200,0.572000,0.000000,33,1 8 | 1404733463232800000,6,6,0.088000,0.300000,73.6178359985,100.3651504517,0.431200,0.660000,0.000000,33,2 9 | 1404733463232800000,6,6,0.088000,0.300000,89.1793365479,64.2991027832,0.343200,0.660000,0.000000,33,3 10 | 1404733463232800000,6,6,0.088000,0.300000,39.7745933533,93.0840530396,0.457600,0.572000,0.000000,34,0 11 | 1404733463232800000,6,6,0.088000,0.300000,28.0672645569,126.7317123413,0.545600,0.572000,0.000000,34,1 12 | 1404733463232800000,6,6,0.088000,0.300000,56.6950492859,144.4591064453,0.545600,0.660000,0.000000,34,2 13 | 1404733463232800000,6,6,0.088000,0.300000,69.5168075562,110.7530670166,0.457600,0.660000,0.000000,34,3 14 | 1404733463232800000,6,6,0.088000,0.300000,24.8363285065,136.6608734131,0.572000,0.572000,0.000000,35,0 15 | 1404733463232800000,6,6,0.088000,0.300000,15.6880369186,167.7518157959,0.660000,0.572000,0.000000,35,1 16 | 1404733463232800000,6,6,0.088000,0.300000,43.3827934265,184.9874877930,0.660000,0.660000,0.000000,35,2 17 | 1404733463232800000,6,6,0.088000,0.300000,53.4629364014,154.1411590576,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463282800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463282800128,6,6,0.088000,0.300000,20.1771392822,29.0348091125,0.343200,0.457600,0.000000,27,0 3 | 1404733463282800128,6,6,0.088000,0.300000,7.0121774673,64.9335021973,0.431200,0.457600,0.000000,27,1 4 | 1404733463282800128,6,6,0.088000,0.300000,34.5800170898,81.5280532837,0.431200,0.545600,0.000000,27,2 5 | 1404733463282800128,6,6,0.088000,0.300000,48.5865173340,45.2671775818,0.343200,0.545600,0.000000,27,3 6 | 1404733463282800128,6,6,0.088000,0.300000,57.6108207703,50.5097770691,0.343200,0.572000,0.000000,33,0 7 | 1404733463282800128,6,6,0.088000,0.300000,43.4061508179,86.5678100586,0.431200,0.572000,0.000000,33,1 8 | 1404733463282800128,6,6,0.088000,0.300000,73.3905868530,104.4652099609,0.431200,0.660000,0.000000,33,2 9 | 1404733463282800128,6,6,0.088000,0.300000,88.5476608276,68.2981567383,0.343200,0.660000,0.000000,33,3 10 | 1404733463282800128,6,6,0.088000,0.300000,39.5436553955,97.3726043701,0.457600,0.572000,0.000000,34,0 11 | 1404733463282800128,6,6,0.088000,0.300000,28.0944099426,131.1558837891,0.545600,0.572000,0.000000,34,1 12 | 1404733463282800128,6,6,0.088000,0.300000,56.7770500183,148.5915527344,0.545600,0.660000,0.000000,34,2 13 | 1404733463282800128,6,6,0.088000,0.300000,69.3221435547,114.8638839722,0.457600,0.660000,0.000000,34,3 14 | 1404733463282800128,6,6,0.088000,0.300000,24.9784278870,141.0978393555,0.572000,0.572000,0.000000,35,0 15 | 1404733463282800128,6,6,0.088000,0.300000,16.0834388733,172.2259521484,0.660000,0.572000,0.000000,35,1 16 | 1404733463282800128,6,6,0.088000,0.300000,43.6993942261,189.1534881592,0.660000,0.660000,0.000000,35,2 17 | 1404733463282800128,6,6,0.088000,0.300000,53.5976562500,158.2453155518,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac_ros/config/calib_mocap-sotiris.yaml: -------------------------------------------------------------------------------- 1 | # ros: 2 | # rosbag: "/data/calibration_d455_ros_20220713/calibration_d455_ros_20220713_183241.bag" 3 | # cam0_topic: "/D455/rgb0/image" 4 | # body0_topic: "/vicon/d455_ros/d455_ros" 5 | # target0_topic: "/vicon/aprilgrid_tv/aprilgrid_tv" 6 | 7 | # ros: 8 | # rosbag: "/data/calibration_s550_jetson_20220714/calibration_s550_jetson_20220714_192445.bag" 9 | # cam0_topic: "/D455/rgb0/image" 10 | # body0_topic: "/vicon/s550_jetson/s550_jetson" 11 | # target0_topic: "/vicon/AprilGridTV/AprilGridTV" 12 | 13 | # ros: 14 | # rosbag: "/data/calibration_s550_jetson_20220714/calibration_s550_jetson_20220714_192600.bag" 15 | # cam0_topic: "/D455/rgb0/image" 16 | # body0_topic: "/vicon/s550_jetson/s550_jetson" 17 | # target0_topic: "/vicon/AprilGridTV/AprilGridTV" 18 | 19 | ros: 20 | rosbag: "/data/calibration_s550_jetson_20220718/s550_jetson_pc_eth_target_20220718_164258.bag" 21 | cam0_topic: "/D455/rgb0/image" 22 | body0_topic: "/vicon/s550_jetson/s550_jetson" 23 | target0_topic: "/vicon/AprilGrid/AprilGrid" 24 | 25 | settings: 26 | fix_intrinsics: true 27 | fix_mocap_poses: true 28 | fix_fiducial_pose: false 29 | outlier_threshold: 4.0 30 | info_gain_threshold: 0.2 31 | enable_loss_fn: false 32 | loss_fn_type: "CAUCHY" 33 | loss_fn_param: 1.0 34 | 35 | calib_target: 36 | target_type: 'aprilgrid' # Target type 37 | tag_rows: 6 # Number of rows 38 | tag_cols: 6 # Number of cols 39 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 40 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 41 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 42 | 43 | cam0: 44 | resolution: [640, 480] 45 | proj_model: "pinhole" 46 | dist_model: "radtan4" 47 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462682800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462682800128,6,6,0.088000,0.300000,72.7036819458,22.1328544617,0.228800,0.572000,0.000000,32,0 3 | 1404733462682800128,6,6,0.088000,0.300000,56.7763519287,62.5658645630,0.316800,0.572000,0.000000,32,1 4 | 1404733462682800128,6,6,0.088000,0.300000,91.7802810669,79.5801086426,0.316800,0.660000,0.000000,32,2 5 | 1404733462682800128,6,6,0.088000,0.300000,108.7487945557,38.6992416382,0.228800,0.660000,0.000000,32,3 6 | 1404733462682800128,6,6,0.088000,0.300000,52.5122261047,74.8935852051,0.343200,0.572000,0.000000,33,0 7 | 1404733462682800128,6,6,0.088000,0.300000,39.6516685486,113.3584671021,0.431200,0.572000,0.000000,33,1 8 | 1404733462682800128,6,6,0.088000,0.300000,73.4620361328,129.9380493164,0.431200,0.660000,0.000000,33,2 9 | 1404733462682800128,6,6,0.088000,0.300000,87.2910232544,91.4450454712,0.343200,0.660000,0.000000,33,3 10 | 1404733462682800128,6,6,0.088000,0.300000,36.3401870728,124.8379745483,0.457600,0.572000,0.000000,34,0 11 | 1404733462682800128,6,6,0.088000,0.300000,26.3162803650,160.4534912109,0.545600,0.572000,0.000000,34,1 12 | 1404733462682800128,6,6,0.088000,0.300000,58.6320686340,176.5837097168,0.545600,0.660000,0.000000,34,2 13 | 1404733462682800128,6,6,0.088000,0.300000,69.7335433960,141.0302734375,0.457600,0.660000,0.000000,34,3 14 | 1404733462682800128,6,6,0.088000,0.300000,23.6981163025,170.9587860107,0.572000,0.572000,0.000000,35,0 15 | 1404733462682800128,6,6,0.088000,0.300000,16.2902336121,203.5393981934,0.660000,0.572000,0.000000,35,1 16 | 1404733462682800128,6,6,0.088000,0.300000,47.2691383362,219.0116424561,0.660000,0.660000,0.000000,35,2 17 | 1404733462682800128,6,6,0.088000,0.300000,55.7617874146,186.6984558105,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463132800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463132800000,6,6,0.088000,0.300000,31.3701686859,43.4644584656,0.457600,0.457600,0.000000,28,0 3 | 1404733463132800000,6,6,0.088000,0.300000,17.8488903046,78.9297637939,0.545600,0.457600,0.000000,28,1 4 | 1404733463132800000,6,6,0.088000,0.300000,47.1703224182,97.0055084229,0.545600,0.545600,0.000000,28,2 5 | 1404733463132800000,6,6,0.088000,0.300000,61.5402870178,61.4434127808,0.457600,0.545600,0.000000,28,3 6 | 1404733463132800000,6,6,0.088000,0.300000,93.7737731934,18.5864086151,0.343200,0.572000,0.000000,33,0 7 | 1404733463132800000,6,6,0.088000,0.300000,75.9179611206,56.0601310730,0.431200,0.572000,0.000000,33,1 8 | 1404733463132800000,6,6,0.088000,0.300000,108.6371383667,75.7431106567,0.431200,0.660000,0.000000,33,2 9 | 1404733463132800000,6,6,0.088000,0.300000,127.5249023438,38.4258384705,0.343200,0.660000,0.000000,33,3 10 | 1404733463132800000,6,6,0.088000,0.300000,71.0476913452,67.2938003540,0.457600,0.572000,0.000000,34,0 11 | 1404733463132800000,6,6,0.088000,0.300000,56.3595581055,102.4992218018,0.545600,0.572000,0.000000,34,1 12 | 1404733463132800000,6,6,0.088000,0.300000,87.6371307373,121.5647125244,0.545600,0.660000,0.000000,34,2 13 | 1404733463132800000,6,6,0.088000,0.300000,103.4858856201,86.5515975952,0.457600,0.660000,0.000000,34,3 14 | 1404733463132800000,6,6,0.088000,0.300000,52.4102363586,112.8720245361,0.572000,0.572000,0.000000,35,0 15 | 1404733463132800000,6,6,0.088000,0.300000,40.4932975769,145.4185485840,0.660000,0.572000,0.000000,35,1 16 | 1404733463132800000,6,6,0.088000,0.300000,70.4959564209,163.7360534668,0.660000,0.660000,0.000000,35,2 17 | 1404733463132800000,6,6,0.088000,0.300000,83.4660873413,131.5627899170,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463232800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463232800000,6,6,0.088000,0.300000,23.9793949127,48.6345443726,0.457600,0.457600,0.000000,28,0 3 | 1404733463232800000,6,6,0.088000,0.300000,10.9648475647,84.3316802979,0.545600,0.457600,0.000000,28,1 4 | 1404733463232800000,6,6,0.088000,0.300000,40.2794609070,102.1045074463,0.545600,0.545600,0.000000,28,2 5 | 1404733463232800000,6,6,0.088000,0.300000,54.1928825378,66.3965988159,0.457600,0.545600,0.000000,28,3 6 | 1404733463232800000,6,6,0.088000,0.300000,85.8291168213,23.2657680511,0.343200,0.572000,0.000000,33,0 7 | 1404733463232800000,6,6,0.088000,0.300000,68.4818267822,60.7693443298,0.431200,0.572000,0.000000,33,1 8 | 1404733463232800000,6,6,0.088000,0.300000,101.2372512817,80.2517242432,0.431200,0.660000,0.000000,33,2 9 | 1404733463232800000,6,6,0.088000,0.300000,119.6322250366,42.5908546448,0.343200,0.660000,0.000000,33,3 10 | 1404733463232800000,6,6,0.088000,0.300000,63.6821517944,72.1120758057,0.457600,0.572000,0.000000,34,0 11 | 1404733463232800000,6,6,0.088000,0.300000,49.4548301697,107.4516906738,0.545600,0.572000,0.000000,34,1 12 | 1404733463232800000,6,6,0.088000,0.300000,80.7010269165,126.2098617554,0.545600,0.660000,0.000000,34,2 13 | 1404733463232800000,6,6,0.088000,0.300000,96.1191024780,91.0396270752,0.457600,0.660000,0.000000,34,3 14 | 1404733463232800000,6,6,0.088000,0.300000,45.5709915161,117.8846359253,0.572000,0.572000,0.000000,35,0 15 | 1404733463232800000,6,6,0.088000,0.300000,34.1015205383,150.5804290771,0.660000,0.572000,0.000000,35,1 16 | 1404733463232800000,6,6,0.088000,0.300000,64.1235580444,168.6464080811,0.660000,0.660000,0.000000,35,2 17 | 1404733463232800000,6,6,0.088000,0.300000,76.6120758057,136.3186950684,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463382799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463382799872,6,6,0.088000,0.300000,72.9815139771,20.4733867645,0.228800,0.572000,0.000000,32,0 3 | 1404733463382799872,6,6,0.088000,0.300000,57.0930023193,58.4328727722,0.316800,0.572000,0.000000,32,1 4 | 1404733463382799872,6,6,0.088000,0.300000,88.5106811523,75.5402679443,0.316800,0.660000,0.000000,32,2 5 | 1404733463382799872,6,6,0.088000,0.300000,105.3001632690,37.3436088562,0.228800,0.660000,0.000000,32,3 6 | 1404733463382799872,6,6,0.088000,0.300000,52.6411323547,69.8697433472,0.343200,0.572000,0.000000,33,0 7 | 1404733463382799872,6,6,0.088000,0.300000,39.5948295593,106.0374908447,0.431200,0.572000,0.000000,33,1 8 | 1404733463382799872,6,6,0.088000,0.300000,69.8144531250,122.9433593750,0.431200,0.660000,0.000000,33,2 9 | 1404733463382799872,6,6,0.088000,0.300000,83.8046417236,86.6082305908,0.343200,0.660000,0.000000,33,3 10 | 1404733463382799872,6,6,0.088000,0.300000,36.1767425537,116.7723464966,0.457600,0.572000,0.000000,34,0 11 | 1404733463382799872,6,6,0.088000,0.300000,25.7665367126,150.5778503418,0.545600,0.572000,0.000000,34,1 12 | 1404733463382799872,6,6,0.088000,0.300000,54.8335037231,167.2289428711,0.545600,0.660000,0.000000,34,2 13 | 1404733463382799872,6,6,0.088000,0.300000,66.1338882446,133.4577178955,0.457600,0.660000,0.000000,34,3 14 | 1404733463382799872,6,6,0.088000,0.300000,22.9849720001,160.4722747803,0.572000,0.572000,0.000000,35,0 15 | 1404733463382799872,6,6,0.088000,0.300000,15.1023092270,191.6330871582,0.660000,0.572000,0.000000,35,1 16 | 1404733463382799872,6,6,0.088000,0.300000,43.1027297974,207.7458190918,0.660000,0.660000,0.000000,35,2 17 | 1404733463382799872,6,6,0.088000,0.300000,51.9483757019,176.8811187744,0.572000,0.660000,0.000000,35,3 18 | -------------------------------------------------------------------------------- /yac/lib/calib_loss.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_loss.hpp" 2 | #include 3 | 4 | namespace yac { 5 | 6 | // HUBER LOSS ///////////////////////////////////////////////////////////////// 7 | 8 | HuberLoss::HuberLoss(const double s) : calib_loss_t{"HUBER"} { 9 | loss = new ceres::HuberLoss(s); 10 | } 11 | 12 | HuberLoss::~HuberLoss() { 13 | if (loss) { 14 | delete loss; 15 | } 16 | } 17 | 18 | void HuberLoss::Evaluate(double sq_norm, double rho[3]) const { 19 | loss->Evaluate(sq_norm, rho); 20 | } 21 | 22 | // CAUCHY LOSS //////////////////////////////////////////////////////////////// 23 | 24 | CauchyLoss::CauchyLoss(const double s) : calib_loss_t{"CAUCHY"} { 25 | loss = new ceres::CauchyLoss(s); 26 | } 27 | 28 | CauchyLoss::~CauchyLoss() { 29 | if (loss) { 30 | delete loss; 31 | } 32 | } 33 | 34 | void CauchyLoss::Evaluate(double sq_norm, double rho[3]) const { 35 | loss->Evaluate(sq_norm, rho); 36 | } 37 | 38 | // BLAKE-ZISSERMAN LOSS /////////////////////////////////////////////////////// 39 | 40 | BlakeZissermanLoss::BlakeZissermanLoss(size_t df, double p, double w_q) 41 | : calib_loss_t{"BLAKE-ZISSERMAN"} { 42 | const auto chisq_dist = boost::math::chi_squared_distribution<>(df); 43 | const auto q = boost::math::quantile(chisq_dist, p); 44 | eps = ((1.0 - w_q) / w_q) * exp(-q); 45 | } 46 | 47 | void BlakeZissermanLoss::Evaluate(double sq_norm, double rho[3]) const { 48 | const double x = eps * exp(sq_norm) + 1.0; 49 | const double x2 = x * x; 50 | const double x3 = x2 * x; 51 | 52 | rho[0] = -log(eps + exp(-sq_norm)); 53 | rho[1] = 1.0 / x; 54 | rho[2] = (x - x2) / x3; 55 | 56 | rho[1] = std::max(std::numeric_limits::min(), rho[1]); 57 | rho[2] = std::max(std::numeric_limits::min(), rho[2]); 58 | } 59 | 60 | } // namespace yac 61 | -------------------------------------------------------------------------------- /scripts/plot_aprilgrid_detection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import pandas 3 | import seaborn as sns 4 | import matplotlib.pylab as plt 5 | 6 | # Load data 7 | data_full = pandas.read_csv("/tmp/aprilgrid_detect-pthreads_fullsize.csv") 8 | data_50 = pandas.read_csv("/tmp/aprilgrid_detect-pthreads_resized_50.csv") 9 | data_75 = pandas.read_csv("/tmp/aprilgrid_detect-pthreads_resized_75.csv") 10 | 11 | # Time series plots 12 | plt.figure() 13 | timestamps = data_full['timestamps'].to_numpy() 14 | time = (timestamps - timestamps[0]) * 1e-9 15 | # -- Plot time vs num_detections 16 | plt.subplot(211) 17 | plt.plot(time, data_full["timings"], label="Full-size") 18 | plt.plot(time, data_75["timings"], label="564x360") 19 | plt.plot(time, data_50["timings"], label="376x240") 20 | plt.xlabel("Time [s]") 21 | plt.ylabel("Timings [s]") 22 | plt.title("Detection Times") 23 | plt.legend(loc=0) 24 | # -- Plot time vs timings 25 | plt.subplot(212) 26 | plt.plot(time, data_full["detections"], label="Full-size") 27 | plt.plot(time, data_75["detections"], label="564x360") 28 | plt.plot(time, data_50["detections"], label="376x240") 29 | plt.xlabel("Time [s]") 30 | plt.ylabel("Num of AprilTags Corners Detected") 31 | plt.title("Num of AprilTag Corners Detected") 32 | plt.legend(loc=0) 33 | 34 | # Box plots 35 | plt.figure() 36 | # -- Timings 37 | plt.subplot(211) 38 | timings = [data_full["timings"], data_75["timings"], data_50["timings"]] 39 | sns.boxplot(data=timings) 40 | plt.xticks([0, 1, 2], ["Full-Size", "564x360", "376x240"]) 41 | plt.ylabel("Detection Time [s]") 42 | plt.title("Detection Times") 43 | # -- Corners detected 44 | plt.subplot(212) 45 | detections = [data_full["detections"], data_75["detections"], data_50["detections"]] 46 | sns.boxplot(data=detections) 47 | plt.xticks([0, 1, 2], ["Full-Size", "564x360", "376x240"]) 48 | plt.ylabel("Num of AprilTag Corners") 49 | plt.title("Num of AprilTag Corners Detected") 50 | 51 | plt.show() 52 | -------------------------------------------------------------------------------- /scripts/plot_frames.py: -------------------------------------------------------------------------------- 1 | """ Plot Poses """ 2 | import numpy as np 3 | from numpy.linalg import inv 4 | 5 | import matplotlib.pylab as plt 6 | from mpl_toolkits import mplot3d 7 | 8 | from proto import plot_tf 9 | from proto import plot_set_axes_equal 10 | 11 | if __name__ == "__main__": 12 | plt.figure() 13 | ax = plt.axes(projection='3d') 14 | 15 | # yapf:disable 16 | T_WS = np.array([ 17 | [0.320679, 0.062018, 0.945155, -0.827194], 18 | [0.000000, -0.997854, 0.065476, -0.526340], 19 | [0.947188, -0.020997, -0.319991, 0.513301], 20 | [0.000000, 0.000000, 0.000000, 1.000000] 21 | ]) 22 | 23 | T_WF = np.array([ 24 | [0.199203, 0.220606, -0.954804, 0.000000], 25 | [-0.979300, 0.080513, -0.185712, 0.000000], 26 | [0.035905, 0.972034, 0.232078, 0.000000], 27 | [0.000000, 0.000000, 0.000000, 1.000000] 28 | ]) 29 | 30 | T_BS = np.array([ 31 | [1.000000, 0.000000, 0.000000, 0.000000], 32 | [0.000000, 1.000000, 0.000000, 0.000000], 33 | [0.000000, 0.000000, 1.000000, 0.000000], 34 | [0.000000, 0.000000, 0.000000, 1.000000] 35 | ]) 36 | 37 | T_BC0 = np.array([ 38 | [0.014866, -0.999881, 0.004140, -0.021640], 39 | [0.999557, 0.014967, 0.025716, -0.064677], 40 | [-0.025774, 0.003756, 0.999661, 0.009811], 41 | [0.000000, 0.000000, 0.000000, 1.000000] 42 | ]) 43 | 44 | T_BC1 = np.array([ 45 | [0.012555, -0.999755, 0.018224, -0.019844], 46 | [0.999599, 0.013012, 0.025159, 0.045369], 47 | [-0.025390, 0.017901, 0.999517, 0.007862], 48 | [0.000000, 0.000000, 0.000000, 1.000000] 49 | ]) 50 | # yapf:enable 51 | 52 | plot_tf(ax, T_WF, name="T_WF", size=0.3) 53 | plot_tf(ax, T_WS, name="T_WS", size=0.3) 54 | plot_tf(ax, T_WS @ inv(T_BS) @ T_BC0, name="T_WC0", size=0.3) 55 | plot_tf(ax, T_WS @ inv(T_BS) @ T_BC1, name="T_WC1", size=0.3) 56 | 57 | ax.set_xlabel("x [m]") 58 | ax.set_ylabel("y [m]") 59 | ax.set_zlabel("z [m]") 60 | plot_set_axes_equal(ax) 61 | plt.show() 62 | -------------------------------------------------------------------------------- /yac/lib/util/net.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_NET_HPP 2 | #define YAC_NET_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "core.hpp" 14 | 15 | namespace yac { 16 | 17 | /***************************************************************************** 18 | * NETWORKING 19 | ****************************************************************************/ 20 | 21 | /** 22 | * Return IP and Port info from socket file descriptor `sockfd` to `ip` and 23 | * `port`. Returns `0` for success and `-1` for failure. 24 | */ 25 | int ip_port_info(const int sockfd, char *ip, int *port); 26 | 27 | /** 28 | * Return IP and Port info from socket file descriptor `sockfd` to `ip` and 29 | * `port`. Returns `0` for success and `-1` for failure. 30 | */ 31 | int ip_port_info(const int sockfd, std::string &ip, int &port); 32 | 33 | /** 34 | * TCP server 35 | */ 36 | struct tcp_server_t { 37 | int port = 8080; 38 | int sockfd = -1; 39 | std::vector conns; 40 | void *(*conn_thread)(void *) = nullptr; 41 | 42 | tcp_server_t(int port_ = 8080); 43 | }; 44 | 45 | /** 46 | * TCP client 47 | */ 48 | struct tcp_client_t { 49 | std::string server_ip; 50 | int server_port = 8080; 51 | int sockfd = -1; 52 | int (*loop_cb)(tcp_client_t &) = nullptr; 53 | 54 | tcp_client_t(const std::string &server_ip_ = "127.0.0.1", 55 | int server_port_ = 8080); 56 | }; 57 | 58 | /** 59 | * Configure TCP server 60 | */ 61 | int tcp_server_config(tcp_server_t &server); 62 | 63 | /** 64 | * Loop TCP server 65 | */ 66 | int tcp_server_loop(tcp_server_t &server); 67 | 68 | /** 69 | * Configure TCP client 70 | */ 71 | int tcp_client_config(tcp_client_t &client); 72 | 73 | /** 74 | * Loop TCP client 75 | */ 76 | int tcp_client_loop(tcp_client_t &client); 77 | 78 | } // namespace yac 79 | #endif // YAC_NET_HPP 80 | -------------------------------------------------------------------------------- /yac/lib/util/timeline.cpp: -------------------------------------------------------------------------------- 1 | #include "timeline.hpp" 2 | 3 | namespace yac { 4 | 5 | timeline_t::timeline_t() {} 6 | 7 | timeline_t::~timeline_t() { 8 | for (auto &kv : data) { 9 | delete kv.second; 10 | } 11 | } 12 | 13 | void timeline_t::add(const timestamp_t &ts, 14 | const int cam_idx, 15 | const std::string &img_path) { 16 | timestamps.insert(ts); 17 | data.insert({ts, new camera_event_t{ts, cam_idx, img_path}}); 18 | } 19 | 20 | void timeline_t::add(const timestamp_t &ts, 21 | const int cam_idx, 22 | const cv::Mat &image) { 23 | timestamps.insert(ts); 24 | data.insert({ts, new camera_event_t{ts, cam_idx, image}}); 25 | } 26 | 27 | void timeline_t::add(const timestamp_t &ts, 28 | const vec3_t &acc, 29 | const vec3_t &gyr) { 30 | timestamps.insert(ts); 31 | data.insert({ts, new imu_event_t{ts, acc, gyr}}); 32 | } 33 | 34 | void timeline_t::add(const timestamp_t &ts, 35 | const int cam_idx, 36 | const aprilgrid_t &grid) { 37 | timestamps.insert(ts); 38 | data.insert({ts, new aprilgrid_event_t{ts, cam_idx, grid}}); 39 | } 40 | 41 | void timeline_t::add(const timestamp_t &ts, 42 | const std::string &object_name, 43 | const quat_t &q_WM, 44 | const vec3_t &r_WM) { 45 | timestamps.insert(ts); 46 | data.insert({ts, new mocap_event_t{ts, object_name, q_WM, r_WM}}); 47 | } 48 | 49 | int timeline_t::nb_events(const timestamp_t &ts) { 50 | const auto range = data.equal_range(ts); 51 | return std::distance(range.first, range.second); 52 | } 53 | 54 | std::vector timeline_t::get_events(const timestamp_t &ts) { 55 | std::vectorevents; 56 | 57 | const auto range = data.equal_range(ts); 58 | for (auto it = range.first; it != range.second; it++) { 59 | events.push_back(it->second); 60 | } 61 | 62 | return events; 63 | } 64 | 65 | } // namespace yac 66 | -------------------------------------------------------------------------------- /yac_ros/launch/calib_mocap_inspect-intel_realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 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 | -------------------------------------------------------------------------------- /deps/scripts/config.bash: -------------------------------------------------------------------------------- 1 | # Configurations 2 | INSTALL_PREFIX="/opt/yac" 3 | DOWNLOAD_PATH="$INSTALL_PREFIX/src" 4 | BUILD_TYPE="Release" 5 | ROS_VERSION="noetic" 6 | UBUNTU_VERSION="20.04" 7 | 8 | apt_update() { 9 | echo "[Updating APT package list]"; 10 | apt-get update -qqq; 11 | } 12 | 13 | apt_install() { 14 | apt-get install -qqq -y "$@"; 15 | } 16 | 17 | # $1 - Git Repo URL 18 | # $2 - Repo folder name 19 | install_git_repo() { 20 | # Clone repo 21 | mkdir -p $DOWNLOAD_PATH 22 | cd $DOWNLOAD_PATH || return 23 | if [ ! -d "$2" ]; then 24 | git clone "$1" "$2" 25 | fi 26 | 27 | # Go into repo 28 | cd "$2" || return 29 | # git pull 30 | 31 | # Prepare for build 32 | mkdir -p build 33 | cd build || return 34 | cmake .. \ 35 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 36 | -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 37 | $CMAKE_EXTRA_ARGS 38 | 39 | # Compile and install 40 | make -j2 && make install 41 | } 42 | 43 | # $1 - Mercurial Repo URL 44 | # $2 - Repo folder name 45 | install_hg_repo() { 46 | # Clone repo 47 | mkdir -p $DOWNLOAD_PATH 48 | cd "$DOWNLOAD_PATH" || return 49 | if [ ! -d "$2" ]; then 50 | hg clone "$1" "$2" 51 | fi 52 | 53 | # Go into repo 54 | cd "$2" || return 55 | hg pull 56 | 57 | # Prepare for build 58 | mkdir -p build 59 | cd build || return 60 | cmake .. \ 61 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 62 | -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 63 | $CMAKE_EXTRA_ARGS 64 | 65 | # Compile and install 66 | make -j2 && make install 67 | } 68 | 69 | # "$1" - Repo URL 70 | # "$2" - Repo folder name 71 | install_zip_repo() { 72 | # Download repo 73 | mkdir -p $DOWNLOAD_PATH 74 | cd $DOWNLOAD_PATH || return 75 | if [ ! -f "$2".zip ]; then 76 | wget --no-check-certificate "$1" -O "$2".zip 77 | fi 78 | unzip -oqq "$2".zip 79 | cd "$2" || return 80 | 81 | # Compile and install opencv 82 | mkdir -p build 83 | cd build || return 84 | cmake .. \ 85 | -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ 86 | -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX \ 87 | $CMAKE_EXTRA_ARGS 88 | 89 | make -j"$(nproc)" && make install 90 | } 91 | -------------------------------------------------------------------------------- /yac/demo/calib_mocap.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_mocap.hpp" 2 | /** 3 | * Calibrate the extrinsics T_MC between object mocap frame (M) and camera 4 | * frame (C). The configuration file `config_file` is expected to be of the 5 | * form: 6 | * 7 | * settings: 8 | * fix_intrinsics: true 9 | * fix_mocap_poses: true 10 | * fix_fiducial_pose: false 11 | * outlier_threshold: 4.0 12 | * 13 | * calib_target: 14 | * target_type: 'aprilgrid' # Target type 15 | * tag_rows: 6 # Number of rows 16 | * tag_cols: 6 # Number of cols 17 | * tag_size: 0.088 # Size of apriltag, edge to edge [m] 18 | * tag_spacing: 0.3 # Ratio of space between tags to tag_size 19 | * # Example: tag_size=2m, spacing=0.5m 20 | * # tag_spacing=0.25 21 | * 22 | * cam0: 23 | * resolution: [640, 480] 24 | * proj_model: "pinhole" 25 | * dist_model: "radtan4" 26 | * 27 | * Data directory `data_path` is expected to follow a similiar pattern to EuRoC 28 | * format: 29 | * 30 | * data_path/ 31 | * body0/ # Object-Mocap Frame T_WM 32 | * data.csv 33 | * 34 | * cam0/ # Camera images 35 | * data/ 36 | * 1658156653981475879.png 37 | * 1658156654189449268.png 38 | * 1658156654397423877.png 39 | * ... 40 | * # Note image files are Unix-timestamps in nano-seconds 41 | * 42 | * target0/ # Calibration Target frame T_WM 43 | * data.csv 44 | * 45 | * where each `data.csv` file has contains pose data: 46 | * 47 | * #timestamp [ns], x, y, z, qx, qy, qz, qw 48 | * 49 | */ 50 | int main(int argc, char *argv[]) { 51 | // Check arguments 52 | if (argc != 3) { 53 | printf("usage: %s \n", argv[0]); 54 | return -1; 55 | } 56 | 57 | // Calibrate 58 | const std::string config_file = argv[1]; 59 | const std::string data_path = argv[2]; 60 | yac::calib_mocap_t calib{config_file, data_path}; 61 | calib.solve(); 62 | calib.save_results(data_path + "/calib_mocap-results.yaml"); 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463282800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463282800128,6,6,0.088000,0.300000,43.5565338135,4.6431479454,0.343200,0.457600,0.000000,27,0 3 | 1404733463282800128,6,6,0.088000,0.300000,27.6936416626,42.3457946777,0.431200,0.457600,0.000000,27,1 4 | 1404733463282800128,6,6,0.088000,0.300000,58.3757324219,60.0408058167,0.431200,0.545600,0.000000,27,2 5 | 1404733463282800128,6,6,0.088000,0.300000,74.9235610962,22.1602573395,0.343200,0.545600,0.000000,27,3 6 | 1404733463282800128,6,6,0.088000,0.300000,23.4968147278,53.5572471619,0.457600,0.457600,0.000000,28,0 7 | 1404733463282800128,6,6,0.088000,0.300000,10.6662397385,89.2761917114,0.545600,0.457600,0.000000,28,1 8 | 1404733463282800128,6,6,0.088000,0.300000,40.0539550781,106.7417144775,0.545600,0.545600,0.000000,28,2 9 | 1404733463282800128,6,6,0.088000,0.300000,53.6463890076,70.9978637695,0.457600,0.545600,0.000000,28,3 10 | 1404733463282800128,6,6,0.088000,0.300000,84.8924636841,27.6791534424,0.343200,0.572000,0.000000,33,0 11 | 1404733463282800128,6,6,0.088000,0.300000,67.8658218384,65.4341888428,0.431200,0.572000,0.000000,33,1 12 | 1404733463282800128,6,6,0.088000,0.300000,100.6135101318,84.3862915039,0.431200,0.660000,0.000000,33,2 13 | 1404733463282800128,6,6,0.088000,0.300000,118.6940460205,46.7432289124,0.343200,0.660000,0.000000,33,3 14 | 1404733463282800128,6,6,0.088000,0.300000,63.3138618469,76.6165771484,0.457600,0.572000,0.000000,34,0 15 | 1404733463282800128,6,6,0.088000,0.300000,49.3166847229,111.9956130981,0.545600,0.572000,0.000000,34,1 16 | 1404733463282800128,6,6,0.088000,0.300000,80.7201461792,130.5493927002,0.545600,0.660000,0.000000,34,2 17 | 1404733463282800128,6,6,0.088000,0.300000,95.6636123657,95.3130569458,0.457600,0.660000,0.000000,34,3 18 | 1404733463282800128,6,6,0.088000,0.300000,45.5877876282,122.4905014038,0.572000,0.572000,0.000000,35,0 19 | 1404733463282800128,6,6,0.088000,0.300000,34.3955726624,155.2119293213,0.660000,0.572000,0.000000,35,1 20 | 1404733463282800128,6,6,0.088000,0.300000,64.5210189819,173.0284576416,0.660000,0.660000,0.000000,35,2 21 | 1404733463282800128,6,6,0.088000,0.300000,76.6793060303,140.5973815918,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463332800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463332800000,6,6,0.088000,0.300000,41.3640747070,13.0203390121,0.343200,0.457600,0.000000,27,0 3 | 1404733463332800000,6,6,0.088000,0.300000,26.2273826599,50.6651458740,0.431200,0.457600,0.000000,27,1 4 | 1404733463332800000,6,6,0.088000,0.300000,56.7356643677,67.9028396606,0.431200,0.545600,0.000000,27,2 5 | 1404733463332800000,6,6,0.088000,0.300000,72.7946929932,29.9091396332,0.343200,0.545600,0.000000,27,3 6 | 1404733463332800000,6,6,0.088000,0.300000,22.0401420593,62.0470962524,0.457600,0.457600,0.000000,28,0 7 | 1404733463332800000,6,6,0.088000,0.300000,9.8800811768,97.6996383667,0.545600,0.457600,0.000000,28,1 8 | 1404733463332800000,6,6,0.088000,0.300000,39.4565277100,114.7845382690,0.545600,0.545600,0.000000,28,2 9 | 1404733463332800000,6,6,0.088000,0.300000,52.5108337402,78.9954376221,0.457600,0.545600,0.000000,28,3 10 | 1404733463332800000,6,6,0.088000,0.300000,82.8228073120,35.4528274536,0.343200,0.572000,0.000000,33,0 11 | 1404733463332800000,6,6,0.088000,0.300000,66.4901962280,73.2434158325,0.431200,0.572000,0.000000,33,1 12 | 1404733463332800000,6,6,0.088000,0.300000,99.5089416504,91.7406921387,0.431200,0.660000,0.000000,33,2 13 | 1404733463332800000,6,6,0.088000,0.300000,116.8272018433,54.0160179138,0.343200,0.660000,0.000000,33,3 14 | 1404733463332800000,6,6,0.088000,0.300000,62.0266723633,84.4716415405,0.457600,0.572000,0.000000,34,0 15 | 1404733463332800000,6,6,0.088000,0.300000,48.6325111389,119.9232711792,0.545600,0.572000,0.000000,34,1 16 | 1404733463332800000,6,6,0.088000,0.300000,80.3681869507,137.9565887451,0.545600,0.660000,0.000000,34,2 17 | 1404733463332800000,6,6,0.088000,0.300000,94.6638793945,102.5965423584,0.457600,0.660000,0.000000,34,3 18 | 1404733463332800000,6,6,0.088000,0.300000,45.0801849365,130.4399871826,0.572000,0.572000,0.000000,35,0 19 | 1404733463332800000,6,6,0.088000,0.300000,34.4020690918,163.0981903076,0.660000,0.572000,0.000000,35,1 20 | 1404733463332800000,6,6,0.088000,0.300000,64.6675262451,180.5099182129,0.660000,0.660000,0.000000,35,2 21 | 1404733463332800000,6,6,0.088000,0.300000,76.4806213379,148.0927124023,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463382799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463382799872,6,6,0.088000,0.300000,37.3001823425,26.0086116791,0.343200,0.457600,0.000000,27,0 3 | 1404733463382799872,6,6,0.088000,0.300000,22.7825393677,63.6973419189,0.431200,0.457600,0.000000,27,1 4 | 1404733463382799872,6,6,0.088000,0.300000,53.6016693115,80.5582962036,0.431200,0.545600,0.000000,27,2 5 | 1404733463382799872,6,6,0.088000,0.300000,68.8293609619,42.5356369019,0.343200,0.545600,0.000000,27,3 6 | 1404733463382799872,6,6,0.088000,0.300000,18.9018821716,75.0850677490,0.457600,0.457600,0.000000,28,0 7 | 1404733463382799872,6,6,0.088000,0.300000,7.4678568840,110.6589584351,0.545600,0.457600,0.000000,28,1 8 | 1404733463382799872,6,6,0.088000,0.300000,37.1248855591,127.4773406982,0.545600,0.545600,0.000000,28,2 9 | 1404733463382799872,6,6,0.088000,0.300000,49.5018615723,91.5636215210,0.457600,0.545600,0.000000,28,3 10 | 1404733463382799872,6,6,0.088000,0.300000,79.0332412720,47.8672714233,0.343200,0.572000,0.000000,33,0 11 | 1404733463382799872,6,6,0.088000,0.300000,63.4474945068,85.6266784668,0.431200,0.572000,0.000000,33,1 12 | 1404733463382799872,6,6,0.088000,0.300000,96.5458908081,103.7443466187,0.431200,0.660000,0.000000,33,2 13 | 1404733463382799872,6,6,0.088000,0.300000,113.1572952271,65.8923110962,0.343200,0.660000,0.000000,33,3 14 | 1404733463382799872,6,6,0.088000,0.300000,59.1785469055,96.9847869873,0.457600,0.572000,0.000000,34,0 15 | 1404733463382799872,6,6,0.088000,0.300000,46.4793128967,132.4572448730,0.545600,0.572000,0.000000,34,1 16 | 1404733463382799872,6,6,0.088000,0.300000,78.3329010010,149.9650878906,0.545600,0.660000,0.000000,34,2 17 | 1404733463382799872,6,6,0.088000,0.300000,91.9309692383,114.6098327637,0.457600,0.660000,0.000000,34,3 18 | 1404733463382799872,6,6,0.088000,0.300000,43.0520439148,142.9019165039,0.572000,0.572000,0.000000,35,0 19 | 1404733463382799872,6,6,0.088000,0.300000,32.9899253845,175.6006317139,0.660000,0.572000,0.000000,35,1 20 | 1404733463382799872,6,6,0.088000,0.300000,63.5027885437,192.4815521240,0.660000,0.660000,0.000000,35,2 21 | 1404733463382799872,6,6,0.088000,0.300000,74.5826187134,160.1269378662,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461532800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733461532800000,6,6,0.088000,0.300000,30.4759559631,9.2912845612,0.572000,0.343200,0.000000,23,0 3 | 1404733461532800000,6,6,0.088000,0.300000,9.4995021820,38.3191528320,0.660000,0.343200,0.000000,23,1 4 | 1404733461532800000,6,6,0.088000,0.300000,27.7026748657,64.5877685547,0.660000,0.431200,0.000000,23,2 5 | 1404733461532800000,6,6,0.088000,0.300000,49.5745887756,35.7223320007,0.572000,0.431200,0.000000,23,3 6 | 1404733461532800000,6,6,0.088000,0.300000,88.4083099365,5.2954640388,0.457600,0.457600,0.000000,28,0 7 | 1404733461532800000,6,6,0.088000,0.300000,62.8923797607,35.3249320984,0.545600,0.457600,0.000000,28,1 8 | 1404733461532800000,6,6,0.088000,0.300000,84.2497329712,63.8332748413,0.545600,0.545600,0.000000,28,2 9 | 1404733461532800000,6,6,0.088000,0.300000,110.6110610962,34.0869674683,0.457600,0.545600,0.000000,28,3 10 | 1404733461532800000,6,6,0.088000,0.300000,55.5426826477,44.4769172668,0.572000,0.457600,0.000000,29,0 11 | 1404733461532800000,6,6,0.088000,0.300000,33.6874694824,73.0448684692,0.660000,0.457600,0.000000,29,1 12 | 1404733461532800000,6,6,0.088000,0.300000,54.2749938965,101.1218109131,0.660000,0.545600,0.000000,29,2 13 | 1404733461532800000,6,6,0.088000,0.300000,76.8249893188,72.7691955566,0.572000,0.545600,0.000000,29,3 14 | 1404733461532800000,6,6,0.088000,0.300000,117.5802536011,43.5092163086,0.457600,0.572000,0.000000,34,0 15 | 1404733461532800000,6,6,0.088000,0.300000,91.1442260742,73.0585098267,0.545600,0.572000,0.000000,34,1 16 | 1404733461532800000,6,6,0.088000,0.300000,114.5203399658,103.4411544800,0.545600,0.660000,0.000000,34,2 17 | 1404733461532800000,6,6,0.088000,0.300000,141.8786926270,74.4212799072,0.457600,0.660000,0.000000,34,3 18 | 1404733461532800000,6,6,0.088000,0.300000,83.4999771118,82.0092391968,0.572000,0.572000,0.000000,35,0 19 | 1404733461532800000,6,6,0.088000,0.300000,60.7533607483,109.9387512207,0.660000,0.572000,0.000000,35,1 20 | 1404733461532800000,6,6,0.088000,0.300000,83.3416519165,139.2530975342,0.660000,0.660000,0.000000,35,2 21 | 1404733461532800000,6,6,0.088000,0.300000,106.8599472046,111.9741210938,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462732800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462732800000,6,6,0.088000,0.300000,23.8655891418,9.2231006622,0.228800,0.457600,0.000000,26,0 3 | 1404733462732800000,6,6,0.088000,0.300000,10.7548675537,47.3278617859,0.316800,0.457600,0.000000,26,1 4 | 1404733462732800000,6,6,0.088000,0.300000,40.2041740417,62.2555961609,0.316800,0.545600,0.000000,26,2 5 | 1404733462732800000,6,6,0.088000,0.300000,54.0321388245,23.4911594391,0.228800,0.545600,0.000000,26,3 6 | 1404733462732800000,6,6,0.088000,0.300000,63.7713317871,28.1836814880,0.228800,0.572000,0.000000,32,0 7 | 1404733462732800000,6,6,0.088000,0.300000,49.5275573730,66.6212539673,0.316800,0.572000,0.000000,32,1 8 | 1404733462732800000,6,6,0.088000,0.300000,81.4926986694,82.8976135254,0.316800,0.660000,0.000000,32,2 9 | 1404733462732800000,6,6,0.088000,0.300000,96.7361907959,44.0529670715,0.228800,0.660000,0.000000,32,3 10 | 1404733462732800000,6,6,0.088000,0.300000,45.6266860962,78.3376464844,0.343200,0.572000,0.000000,33,0 11 | 1404733462732800000,6,6,0.088000,0.300000,34.3252182007,114.6718215942,0.431200,0.572000,0.000000,33,1 12 | 1404733462732800000,6,6,0.088000,0.300000,64.9128570557,130.8335723877,0.431200,0.660000,0.000000,33,2 13 | 1404733462732800000,6,6,0.088000,0.300000,77.3799972534,94.3140640259,0.343200,0.660000,0.000000,33,3 14 | 1404733462732800000,6,6,0.088000,0.300000,31.3054561615,125.5595474243,0.457600,0.572000,0.000000,34,0 15 | 1404733462732800000,6,6,0.088000,0.300000,22.4850559235,159.3325805664,0.545600,0.572000,0.000000,34,1 16 | 1404733462732800000,6,6,0.088000,0.300000,51.9418678284,175.1983337402,0.545600,0.660000,0.000000,34,2 17 | 1404733462732800000,6,6,0.088000,0.300000,61.6755676270,141.4056091309,0.457600,0.660000,0.000000,34,3 18 | 1404733462732800000,6,6,0.088000,0.300000,20.2424945831,169.2868804932,0.572000,0.572000,0.000000,35,0 19 | 1404733462732800000,6,6,0.088000,0.300000,13.6615991592,200.2211761475,0.660000,0.572000,0.000000,35,1 20 | 1404733462732800000,6,6,0.088000,0.300000,41.9398155212,215.5297393799,0.660000,0.660000,0.000000,35,2 21 | 1404733462732800000,6,6,0.088000,0.300000,49.5094909668,184.7388916016,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462932800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462932800000,6,6,0.088000,0.300000,43.8202857971,21.9139537811,0.343200,0.457600,0.000000,27,0 3 | 1404733462932800000,6,6,0.088000,0.300000,30.0388507843,57.5080490112,0.431200,0.457600,0.000000,27,1 4 | 1404733462932800000,6,6,0.088000,0.300000,57.6423149109,74.6786727905,0.431200,0.545600,0.000000,27,2 5 | 1404733462932800000,6,6,0.088000,0.300000,72.4906463623,38.7406044006,0.343200,0.545600,0.000000,27,3 6 | 1404733462932800000,6,6,0.088000,0.300000,26.3517055511,68.1825027466,0.457600,0.457600,0.000000,28,0 7 | 1404733462932800000,6,6,0.088000,0.300000,15.3292665482,101.5494842529,0.545600,0.457600,0.000000,28,1 8 | 1404733462932800000,6,6,0.088000,0.300000,41.7220115662,118.6502532959,0.545600,0.545600,0.000000,28,2 9 | 1404733462932800000,6,6,0.088000,0.300000,53.6953353882,85.1808853149,0.457600,0.545600,0.000000,28,3 10 | 1404733462932800000,6,6,0.088000,0.300000,81.5241622925,44.3394966125,0.343200,0.572000,0.000000,33,0 11 | 1404733462932800000,6,6,0.088000,0.300000,66.5189208984,80.0183868408,0.431200,0.572000,0.000000,33,1 12 | 1404733462932800000,6,6,0.088000,0.300000,96.5376052856,98.6044921875,0.431200,0.660000,0.000000,33,2 13 | 1404733462932800000,6,6,0.088000,0.300000,112.5678100586,62.8149452209,0.343200,0.660000,0.000000,33,3 14 | 1404733462932800000,6,6,0.088000,0.300000,62.4689102173,90.6269531250,0.457600,0.572000,0.000000,34,0 15 | 1404733462932800000,6,6,0.088000,0.300000,50.2986717224,123.9564361572,0.545600,0.572000,0.000000,34,1 16 | 1404733462932800000,6,6,0.088000,0.300000,78.9436950684,142.1618041992,0.545600,0.660000,0.000000,34,2 17 | 1404733462932800000,6,6,0.088000,0.300000,92.1801452637,108.9013290405,0.457600,0.660000,0.000000,34,3 18 | 1404733462932800000,6,6,0.088000,0.300000,46.9173927307,133.7245635986,0.572000,0.572000,0.000000,35,0 19 | 1404733462932800000,6,6,0.088000,0.300000,37.3557243347,164.4290466309,0.660000,0.572000,0.000000,35,1 20 | 1404733462932800000,6,6,0.088000,0.300000,64.7682189941,182.0643768311,0.660000,0.660000,0.000000,35,2 21 | 1404733462932800000,6,6,0.088000,0.300000,75.5323867798,151.5846252441,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462982799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462982799872,6,6,0.088000,0.300000,42.2711715698,18.7778968811,0.343200,0.457600,0.000000,27,0 3 | 1404733462982799872,6,6,0.088000,0.300000,28.2195110321,54.4714736938,0.431200,0.457600,0.000000,27,1 4 | 1404733462982799872,6,6,0.088000,0.300000,55.7068824768,71.7610321045,0.431200,0.545600,0.000000,27,2 5 | 1404733462982799872,6,6,0.088000,0.300000,70.7429733276,35.7692146301,0.343200,0.545600,0.000000,27,3 6 | 1404733462982799872,6,6,0.088000,0.300000,24.3997154236,65.1303863525,0.457600,0.457600,0.000000,28,0 7 | 1404733462982799872,6,6,0.088000,0.300000,13.1546401978,98.6243057251,0.545600,0.457600,0.000000,28,1 8 | 1404733462982799872,6,6,0.088000,0.300000,39.6005516052,115.7824249268,0.545600,0.545600,0.000000,28,2 9 | 1404733462982799872,6,6,0.088000,0.300000,51.7141418457,82.2507171631,0.457600,0.545600,0.000000,28,3 10 | 1404733462982799872,6,6,0.088000,0.300000,79.7642745972,41.3983726501,0.343200,0.572000,0.000000,33,0 11 | 1404733462982799872,6,6,0.088000,0.300000,64.5605392456,77.1798095703,0.431200,0.572000,0.000000,33,1 12 | 1404733462982799872,6,6,0.088000,0.300000,94.5429916382,95.7262725830,0.431200,0.660000,0.000000,33,2 13 | 1404733462982799872,6,6,0.088000,0.300000,110.7343902588,60.0708503723,0.343200,0.660000,0.000000,33,3 14 | 1404733462982799872,6,6,0.088000,0.300000,60.4784889221,87.7236633301,0.457600,0.572000,0.000000,34,0 15 | 1404733462982799872,6,6,0.088000,0.300000,48.1018905640,121.1399612427,0.545600,0.572000,0.000000,34,1 16 | 1404733462982799872,6,6,0.088000,0.300000,76.7237854004,139.3821411133,0.545600,0.660000,0.000000,34,2 17 | 1404733462982799872,6,6,0.088000,0.300000,90.2093353271,106.1908950806,0.457600,0.660000,0.000000,34,3 18 | 1404733462982799872,6,6,0.088000,0.300000,44.7285156250,131.0039825439,0.572000,0.572000,0.000000,35,0 19 | 1404733462982799872,6,6,0.088000,0.300000,34.9021873474,161.6827545166,0.660000,0.572000,0.000000,35,1 20 | 1404733462982799872,6,6,0.088000,0.300000,62.4574127197,179.4013671875,0.660000,0.660000,0.000000,35,2 21 | 1404733462982799872,6,6,0.088000,0.300000,73.3259201050,148.9592590332,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463032800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463032800000,6,6,0.088000,0.300000,39.2688751221,17.5619220734,0.343200,0.457600,0.000000,27,0 3 | 1404733463032800000,6,6,0.088000,0.300000,25.1768302917,53.2752838135,0.431200,0.457600,0.000000,27,1 4 | 1404733463032800000,6,6,0.088000,0.300000,52.7027854919,70.5437088013,0.431200,0.545600,0.000000,27,2 5 | 1404733463032800000,6,6,0.088000,0.300000,67.7376327515,34.5272598267,0.343200,0.545600,0.000000,27,3 6 | 1404733463032800000,6,6,0.088000,0.300000,21.4059925079,63.8839263916,0.457600,0.457600,0.000000,28,0 7 | 1404733463032800000,6,6,0.088000,0.300000,10.1156616211,97.4524383545,0.545600,0.457600,0.000000,28,1 8 | 1404733463032800000,6,6,0.088000,0.300000,36.5730438232,114.6679763794,0.545600,0.545600,0.000000,28,2 9 | 1404733463032800000,6,6,0.088000,0.300000,48.7014198303,81.0217132568,0.457600,0.545600,0.000000,28,3 10 | 1404733463032800000,6,6,0.088000,0.300000,76.7687911987,40.1871299744,0.343200,0.572000,0.000000,33,0 11 | 1404733463032800000,6,6,0.088000,0.300000,61.5647315979,75.8657760620,0.431200,0.572000,0.000000,33,1 12 | 1404733463032800000,6,6,0.088000,0.300000,91.5310745239,94.5817260742,0.431200,0.660000,0.000000,33,2 13 | 1404733463032800000,6,6,0.088000,0.300000,107.7682952881,58.6895103455,0.343200,0.660000,0.000000,33,3 14 | 1404733463032800000,6,6,0.088000,0.300000,57.4426345825,86.5503387451,0.457600,0.572000,0.000000,34,0 15 | 1404733463032800000,6,6,0.088000,0.300000,45.0514831543,119.9808425903,0.545600,0.572000,0.000000,34,1 16 | 1404733463032800000,6,6,0.088000,0.300000,73.6529693604,138.2217254639,0.545600,0.660000,0.000000,34,2 17 | 1404733463032800000,6,6,0.088000,0.300000,87.1522369385,104.9227371216,0.457600,0.660000,0.000000,34,3 18 | 1404733463032800000,6,6,0.088000,0.300000,41.6873588562,129.8158264160,0.572000,0.572000,0.000000,35,0 19 | 1404733463032800000,6,6,0.088000,0.300000,31.8196849823,160.6408386230,0.660000,0.572000,0.000000,35,1 20 | 1404733463032800000,6,6,0.088000,0.300000,59.4123039246,178.3459472656,0.660000,0.660000,0.000000,35,2 21 | 1404733463032800000,6,6,0.088000,0.300000,70.2419586182,147.7874908447,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463082800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463082800128,6,6,0.088000,0.300000,34.4754981995,17.5248851776,0.343200,0.457600,0.000000,27,0 3 | 1404733463082800128,6,6,0.088000,0.300000,20.4076175690,53.3643074036,0.431200,0.457600,0.000000,27,1 4 | 1404733463082800128,6,6,0.088000,0.300000,47.9511260986,70.6261520386,0.431200,0.545600,0.000000,27,2 5 | 1404733463082800128,6,6,0.088000,0.300000,62.8857421875,34.5240898132,0.343200,0.545600,0.000000,27,3 6 | 1404733463082800128,6,6,0.088000,0.300000,16.6059169769,64.0348968506,0.457600,0.457600,0.000000,28,0 7 | 1404733463082800128,6,6,0.088000,0.300000,5.4494318962,97.6399688721,0.545600,0.457600,0.000000,28,1 8 | 1404733463082800128,6,6,0.088000,0.300000,31.8270969391,114.7778244019,0.545600,0.545600,0.000000,28,2 9 | 1404733463082800128,6,6,0.088000,0.300000,44.0172843933,81.1251678467,0.457600,0.545600,0.000000,28,3 10 | 1404733463082800128,6,6,0.088000,0.300000,71.9582595825,40.1331596375,0.343200,0.572000,0.000000,33,0 11 | 1404733463082800128,6,6,0.088000,0.300000,56.7534599304,75.8852386475,0.431200,0.572000,0.000000,33,1 12 | 1404733463082800128,6,6,0.088000,0.300000,86.6691894531,94.5026626587,0.431200,0.660000,0.000000,33,2 13 | 1404733463082800128,6,6,0.088000,0.300000,102.8618698120,58.6067390442,0.343200,0.660000,0.000000,33,3 14 | 1404733463082800128,6,6,0.088000,0.300000,52.6435317993,86.5494537354,0.457600,0.572000,0.000000,34,0 15 | 1404733463082800128,6,6,0.088000,0.300000,40.4005928040,120.1404418945,0.545600,0.572000,0.000000,34,1 16 | 1404733463082800128,6,6,0.088000,0.300000,69.0487747192,138.3492736816,0.545600,0.660000,0.000000,34,2 17 | 1404733463082800128,6,6,0.088000,0.300000,82.4243164062,104.8079071045,0.457600,0.660000,0.000000,34,3 18 | 1404733463082800128,6,6,0.088000,0.300000,37.0341567993,129.9774932861,0.572000,0.572000,0.000000,35,0 19 | 1404733463082800128,6,6,0.088000,0.300000,27.3646430969,160.8453979492,0.660000,0.572000,0.000000,35,1 20 | 1404733463082800128,6,6,0.088000,0.300000,54.7759704590,178.4970397949,0.660000,0.660000,0.000000,35,2 21 | 1404733463082800128,6,6,0.088000,0.300000,65.5608367920,147.8686828613,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463332800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463332800000,6,6,0.088000,0.300000,18.3668670654,37.0014762878,0.343200,0.457600,0.000000,27,0 3 | 1404733463332800000,6,6,0.088000,0.300000,5.6192584038,72.9198074341,0.431200,0.457600,0.000000,27,1 4 | 1404733463332800000,6,6,0.088000,0.300000,33.4679489136,89.1580505371,0.431200,0.545600,0.000000,27,2 5 | 1404733463332800000,6,6,0.088000,0.300000,46.8302268982,52.7018508911,0.343200,0.545600,0.000000,27,3 6 | 1404733463332800000,6,6,0.088000,0.300000,77.2425155640,8.5958089828,0.228800,0.572000,0.000000,32,0 7 | 1404733463332800000,6,6,0.088000,0.300000,60.5501480103,46.5085144043,0.316800,0.572000,0.000000,32,1 8 | 1404733463332800000,6,6,0.088000,0.300000,91.7108459473,63.9702301025,0.316800,0.660000,0.000000,32,2 9 | 1404733463332800000,6,6,0.088000,0.300000,109.5121383667,25.8635978699,0.228800,0.660000,0.000000,32,3 10 | 1404733463332800000,6,6,0.088000,0.300000,55.9009056091,57.9699592590,0.343200,0.572000,0.000000,33,0 11 | 1404733463332800000,6,6,0.088000,0.300000,42.3028030396,94.0364074707,0.431200,0.572000,0.000000,33,1 12 | 1404733463332800000,6,6,0.088000,0.300000,72.4272308350,111.4932937622,0.431200,0.660000,0.000000,33,2 13 | 1404733463332800000,6,6,0.088000,0.300000,86.9320602417,75.2617568970,0.343200,0.660000,0.000000,33,3 14 | 1404733463332800000,6,6,0.088000,0.300000,38.5905456543,104.8183517456,0.457600,0.572000,0.000000,34,0 15 | 1404733463332800000,6,6,0.088000,0.300000,27.6596126556,138.6193237305,0.545600,0.572000,0.000000,34,1 16 | 1404733463332800000,6,6,0.088000,0.300000,56.6068687439,155.6484069824,0.545600,0.660000,0.000000,34,2 17 | 1404733463332800000,6,6,0.088000,0.300000,68.4999542236,121.8792953491,0.457600,0.660000,0.000000,34,3 18 | 1404733463332800000,6,6,0.088000,0.300000,24.6896686554,148.5462799072,0.572000,0.572000,0.000000,35,0 19 | 1404733463332800000,6,6,0.088000,0.300000,16.2984542847,179.7015380859,0.660000,0.572000,0.000000,35,1 20 | 1404733463332800000,6,6,0.088000,0.300000,44.0863761902,196.3313140869,0.660000,0.660000,0.000000,35,2 21 | 1404733463332800000,6,6,0.088000,0.300000,53.5162849426,165.3687591553,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462732800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462732800000,6,6,0.088000,0.300000,29.1144638062,35.6482391357,0.343200,0.457600,0.000000,27,0 3 | 1404733462732800000,6,6,0.088000,0.300000,16.4828815460,73.7542800903,0.431200,0.457600,0.000000,27,1 4 | 1404733462732800000,6,6,0.088000,0.300000,47.7469177246,89.8644027710,0.431200,0.545600,0.000000,27,2 5 | 1404733462732800000,6,6,0.088000,0.300000,61.4463386536,51.3627166748,0.343200,0.545600,0.000000,27,3 6 | 1404733462732800000,6,6,0.088000,0.300000,93.4792098999,4.2897715569,0.228800,0.572000,0.000000,32,0 7 | 1404733462732800000,6,6,0.088000,0.300000,76.3395843506,44.4136505127,0.316800,0.572000,0.000000,32,1 8 | 1404733462732800000,6,6,0.088000,0.300000,111.4500198364,61.8319969177,0.316800,0.660000,0.000000,32,2 9 | 1404733462732800000,6,6,0.088000,0.300000,129.5204315186,21.5297527313,0.228800,0.660000,0.000000,32,3 10 | 1404733462732800000,6,6,0.088000,0.300000,71.6431427002,56.4913825989,0.343200,0.572000,0.000000,33,0 11 | 1404733462732800000,6,6,0.088000,0.300000,57.6355628967,94.6227493286,0.431200,0.572000,0.000000,33,1 12 | 1404733462732800000,6,6,0.088000,0.300000,91.4251937866,111.8759384155,0.431200,0.660000,0.000000,33,2 13 | 1404733462732800000,6,6,0.088000,0.300000,106.4565048218,73.5864410400,0.343200,0.660000,0.000000,33,3 14 | 1404733462732800000,6,6,0.088000,0.300000,53.9151306152,106.0705795288,0.457600,0.572000,0.000000,34,0 15 | 1404733462732800000,6,6,0.088000,0.300000,42.8868217468,141.5910644531,0.545600,0.572000,0.000000,34,1 16 | 1404733462732800000,6,6,0.088000,0.300000,75.2223281860,158.3711547852,0.545600,0.660000,0.000000,34,2 17 | 1404733462732800000,6,6,0.088000,0.300000,87.3634719849,122.8011322021,0.457600,0.660000,0.000000,34,3 18 | 1404733462732800000,6,6,0.088000,0.300000,39.9004974365,152.0793914795,0.572000,0.572000,0.000000,35,0 19 | 1404733462732800000,6,6,0.088000,0.300000,31.4402580261,184.5173339844,0.660000,0.572000,0.000000,35,1 20 | 1404733462732800000,6,6,0.088000,0.300000,62.3189468384,200.6083068848,0.660000,0.660000,0.000000,35,2 21 | 1404733462732800000,6,6,0.088000,0.300000,71.8580932617,168.3867034912,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462882800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462882800128,6,6,0.088000,0.300000,47.2722244263,50.7750930786,0.457600,0.457600,0.000000,28,0 3 | 1404733462882800128,6,6,0.088000,0.300000,34.1964187622,85.7664718628,0.545600,0.457600,0.000000,28,1 4 | 1404733462882800128,6,6,0.088000,0.300000,63.4866676331,103.7724914551,0.545600,0.545600,0.000000,28,2 5 | 1404733462882800128,6,6,0.088000,0.300000,77.4999313354,68.5871353149,0.457600,0.545600,0.000000,28,3 6 | 1404733462882800128,6,6,0.088000,0.300000,30.6306667328,96.2944793701,0.572000,0.457600,0.000000,29,0 7 | 1404733462882800128,6,6,0.088000,0.300000,20.3308677673,128.7158660889,0.660000,0.457600,0.000000,29,1 8 | 1404733462882800128,6,6,0.088000,0.300000,48.4067535400,146.3067321777,0.660000,0.545600,0.000000,29,2 9 | 1404733462882800128,6,6,0.088000,0.300000,59.6233711243,113.7934875488,0.572000,0.545600,0.000000,29,3 10 | 1404733462882800128,6,6,0.088000,0.300000,109.2336425781,26.2219734192,0.343200,0.572000,0.000000,33,0 11 | 1404733462882800128,6,6,0.088000,0.300000,91.7499694824,63.3483924866,0.431200,0.572000,0.000000,33,1 12 | 1404733462882800128,6,6,0.088000,0.300000,124.6191101074,82.8509902954,0.431200,0.660000,0.000000,33,2 13 | 1404733462882800128,6,6,0.088000,0.300000,143.0566406250,45.7857589722,0.343200,0.660000,0.000000,33,3 14 | 1404733462882800128,6,6,0.088000,0.300000,86.9790802002,74.3918762207,0.457600,0.572000,0.000000,34,0 15 | 1404733462882800128,6,6,0.088000,0.300000,72.6295928955,109.2406845093,0.545600,0.572000,0.000000,34,1 16 | 1404733462882800128,6,6,0.088000,0.300000,104.0258026123,128.2059326172,0.545600,0.660000,0.000000,34,2 17 | 1404733462882800128,6,6,0.088000,0.300000,119.5663375854,93.5502853394,0.457600,0.660000,0.000000,34,3 18 | 1404733462882800128,6,6,0.088000,0.300000,68.7058334351,119.4654846191,0.572000,0.572000,0.000000,35,0 19 | 1404733462882800128,6,6,0.088000,0.300000,57.1933555603,151.5527954102,0.660000,0.572000,0.000000,35,1 20 | 1404733462882800128,6,6,0.088000,0.300000,87.1344146729,169.7948150635,0.660000,0.660000,0.000000,35,2 21 | 1404733462882800128,6,6,0.088000,0.300000,99.8031311035,138.0825347900,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462932800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462932800000,6,6,0.088000,0.300000,48.4270935059,46.0912208557,0.457600,0.457600,0.000000,28,0 3 | 1404733462932800000,6,6,0.088000,0.300000,34.8127059937,81.1859893799,0.545600,0.457600,0.000000,28,1 4 | 1404733462932800000,6,6,0.088000,0.300000,64.1031265259,99.3329315186,0.545600,0.545600,0.000000,28,2 5 | 1404733462932800000,6,6,0.088000,0.300000,78.6043319702,64.2036895752,0.457600,0.545600,0.000000,28,3 6 | 1404733462932800000,6,6,0.088000,0.300000,31.2953948975,91.5198516846,0.572000,0.457600,0.000000,29,0 7 | 1404733462932800000,6,6,0.088000,0.300000,20.4810295105,124.0503845215,0.660000,0.457600,0.000000,29,1 8 | 1404733462932800000,6,6,0.088000,0.300000,48.4886131287,141.7626647949,0.660000,0.545600,0.000000,29,2 9 | 1404733462932800000,6,6,0.088000,0.300000,60.2214317322,109.4074020386,0.572000,0.545600,0.000000,29,3 10 | 1404733462932800000,6,6,0.088000,0.300000,110.8772811890,21.8728790283,0.343200,0.572000,0.000000,33,0 11 | 1404733462932800000,6,6,0.088000,0.300000,93.1085205078,58.8186264038,0.431200,0.572000,0.000000,33,1 12 | 1404733462932800000,6,6,0.088000,0.300000,125.6934051514,78.7131652832,0.431200,0.660000,0.000000,33,2 13 | 1404733462932800000,6,6,0.088000,0.300000,144.6633453369,41.7069206238,0.343200,0.660000,0.000000,33,3 14 | 1404733462932800000,6,6,0.088000,0.300000,88.2082977295,70.0263214111,0.457600,0.572000,0.000000,34,0 15 | 1404733462932800000,6,6,0.088000,0.300000,73.3370513916,104.7216033936,0.545600,0.572000,0.000000,34,1 16 | 1404733462932800000,6,6,0.088000,0.300000,104.5904922485,123.9781341553,0.545600,0.660000,0.000000,34,2 17 | 1404733462932800000,6,6,0.088000,0.300000,120.5481948853,89.4495620728,0.457600,0.660000,0.000000,34,3 18 | 1404733462932800000,6,6,0.088000,0.300000,69.3337020874,115.0690231323,0.572000,0.572000,0.000000,35,0 19 | 1404733462932800000,6,6,0.088000,0.300000,57.2784652710,147.2017974854,0.660000,0.572000,0.000000,35,1 20 | 1404733462932800000,6,6,0.088000,0.300000,87.2023544312,165.6983947754,0.660000,0.660000,0.000000,35,2 21 | 1404733462932800000,6,6,0.088000,0.300000,100.3614349365,133.8875122070,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462982799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462982799872,6,6,0.088000,0.300000,46.4264526367,42.8371620178,0.457600,0.457600,0.000000,28,0 3 | 1404733462982799872,6,6,0.088000,0.300000,32.6147041321,77.9494171143,0.545600,0.457600,0.000000,28,1 4 | 1404733462982799872,6,6,0.088000,0.300000,61.7742347717,96.2965316772,0.545600,0.545600,0.000000,28,2 5 | 1404733462982799872,6,6,0.088000,0.300000,76.5717697144,61.0731582642,0.457600,0.545600,0.000000,28,3 6 | 1404733462982799872,6,6,0.088000,0.300000,28.8945083618,88.3916397095,0.572000,0.457600,0.000000,29,0 7 | 1404733462982799872,6,6,0.088000,0.300000,18.0284595490,121.0308151245,0.660000,0.457600,0.000000,29,1 8 | 1404733462982799872,6,6,0.088000,0.300000,45.9354667664,138.8406524658,0.660000,0.545600,0.000000,29,2 9 | 1404733462982799872,6,6,0.088000,0.300000,57.7965774536,106.4147033691,0.572000,0.545600,0.000000,29,3 10 | 1404733462982799872,6,6,0.088000,0.300000,109.2746047974,18.7201805115,0.343200,0.572000,0.000000,33,0 11 | 1404733462982799872,6,6,0.088000,0.300000,91.1624984741,55.7644767761,0.431200,0.572000,0.000000,33,1 12 | 1404733462982799872,6,6,0.088000,0.300000,123.6989517212,75.7681121826,0.431200,0.660000,0.000000,33,2 13 | 1404733462982799872,6,6,0.088000,0.300000,142.8641052246,38.7455368042,0.343200,0.660000,0.000000,33,3 14 | 1404733462982799872,6,6,0.088000,0.300000,86.1398468018,66.8728408813,0.457600,0.572000,0.000000,34,0 15 | 1404733462982799872,6,6,0.088000,0.300000,71.1268539429,101.7367019653,0.545600,0.572000,0.000000,34,1 16 | 1404733462982799872,6,6,0.088000,0.300000,102.3958129883,121.1774139404,0.545600,0.660000,0.000000,34,2 17 | 1404733462982799872,6,6,0.088000,0.300000,118.4850616455,86.4876403809,0.457600,0.660000,0.000000,34,3 18 | 1404733462982799872,6,6,0.088000,0.300000,66.9233398438,112.0918350220,0.572000,0.572000,0.000000,35,0 19 | 1404733462982799872,6,6,0.088000,0.300000,54.7416114807,144.3522796631,0.660000,0.572000,0.000000,35,1 20 | 1404733462982799872,6,6,0.088000,0.300000,84.6550903320,162.8687438965,0.660000,0.660000,0.000000,35,2 21 | 1404733462982799872,6,6,0.088000,0.300000,97.9157104492,131.1409301758,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463032800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463032800000,6,6,0.088000,0.300000,43.1871109009,41.5135841370,0.457600,0.457600,0.000000,28,0 3 | 1404733463032800000,6,6,0.088000,0.300000,29.4108047485,76.7190475464,0.545600,0.457600,0.000000,28,1 4 | 1404733463032800000,6,6,0.088000,0.300000,58.5989036560,94.9660263062,0.545600,0.545600,0.000000,28,2 5 | 1404733463032800000,6,6,0.088000,0.300000,73.3592681885,59.6906204224,0.457600,0.545600,0.000000,28,3 6 | 1404733463032800000,6,6,0.088000,0.300000,25.6662311554,87.2170104980,0.572000,0.457600,0.000000,29,0 7 | 1404733463032800000,6,6,0.088000,0.300000,14.7192230225,119.8971481323,0.660000,0.457600,0.000000,29,1 8 | 1404733463032800000,6,6,0.088000,0.300000,42.6591835022,137.7127532959,0.660000,0.545600,0.000000,29,2 9 | 1404733463032800000,6,6,0.088000,0.300000,54.6471748352,105.1934280396,0.572000,0.545600,0.000000,29,3 10 | 1404733463032800000,6,6,0.088000,0.300000,106.0172424316,17.3474102020,0.343200,0.572000,0.000000,33,0 11 | 1404733463032800000,6,6,0.088000,0.300000,87.8304901123,54.5033493042,0.431200,0.572000,0.000000,33,1 12 | 1404733463032800000,6,6,0.088000,0.300000,120.5530319214,74.4902725220,0.431200,0.660000,0.000000,33,2 13 | 1404733463032800000,6,6,0.088000,0.300000,139.6949005127,37.4138336182,0.343200,0.660000,0.000000,33,3 14 | 1404733463032800000,6,6,0.088000,0.300000,82.8156204224,65.5931091309,0.457600,0.572000,0.000000,34,0 15 | 1404733463032800000,6,6,0.088000,0.300000,67.7712173462,100.5481033325,0.545600,0.572000,0.000000,34,1 16 | 1404733463032800000,6,6,0.088000,0.300000,99.1567459106,119.8968734741,0.545600,0.660000,0.000000,34,2 17 | 1404733463032800000,6,6,0.088000,0.300000,115.2532653809,85.2403335571,0.457600,0.660000,0.000000,34,3 18 | 1404733463032800000,6,6,0.088000,0.300000,63.6980667114,110.8668365479,0.572000,0.572000,0.000000,35,0 19 | 1404733463032800000,6,6,0.088000,0.300000,51.5185699463,143.2505187988,0.660000,0.572000,0.000000,35,1 20 | 1404733463032800000,6,6,0.088000,0.300000,81.4834136963,161.7339935303,0.660000,0.660000,0.000000,35,2 21 | 1404733463032800000,6,6,0.088000,0.300000,94.7000808716,129.8307189941,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463082800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463082800128,6,6,0.088000,0.300000,38.0703163147,41.5601577759,0.457600,0.457600,0.000000,28,0 3 | 1404733463082800128,6,6,0.088000,0.300000,24.4279861450,76.8808517456,0.545600,0.457600,0.000000,28,1 4 | 1404733463082800128,6,6,0.088000,0.300000,53.5957183838,95.0722808838,0.545600,0.545600,0.000000,28,2 5 | 1404733463082800128,6,6,0.088000,0.300000,68.3142013550,59.6520652771,0.457600,0.545600,0.000000,28,3 6 | 1404733463082800128,6,6,0.088000,0.300000,20.6833000183,87.3896102905,0.572000,0.457600,0.000000,29,0 7 | 1404733463082800128,6,6,0.088000,0.300000,9.7978677750,120.2540664673,0.660000,0.457600,0.000000,29,1 8 | 1404733463082800128,6,6,0.088000,0.300000,37.7773132324,137.9469757080,0.660000,0.545600,0.000000,29,2 9 | 1404733463082800128,6,6,0.088000,0.300000,49.6126251221,105.2763748169,0.572000,0.545600,0.000000,29,3 10 | 1404733463082800128,6,6,0.088000,0.300000,100.7576751709,17.2054042816,0.343200,0.572000,0.000000,33,0 11 | 1404733463082800128,6,6,0.088000,0.300000,82.7204360962,54.4411468506,0.431200,0.572000,0.000000,33,1 12 | 1404733463082800128,6,6,0.088000,0.300000,115.4352798462,74.3085021973,0.431200,0.660000,0.000000,33,2 13 | 1404733463082800128,6,6,0.088000,0.300000,134.5246276855,37.2113037109,0.343200,0.660000,0.000000,33,3 14 | 1404733463082800128,6,6,0.088000,0.300000,77.7420043945,65.5458450317,0.457600,0.572000,0.000000,34,0 15 | 1404733463082800128,6,6,0.088000,0.300000,62.7884826660,100.5953826904,0.545600,0.572000,0.000000,34,1 16 | 1404733463082800128,6,6,0.088000,0.300000,94.1115188599,119.8572692871,0.545600,0.660000,0.000000,34,2 17 | 1404733463082800128,6,6,0.088000,0.300000,110.0987548828,85.1136093140,0.457600,0.660000,0.000000,34,3 18 | 1404733463082800128,6,6,0.088000,0.300000,58.7161598206,110.9717941284,0.572000,0.572000,0.000000,35,0 19 | 1404733463082800128,6,6,0.088000,0.300000,46.6347084045,143.3677062988,0.660000,0.572000,0.000000,35,1 20 | 1404733463082800128,6,6,0.088000,0.300000,76.5717468262,161.8406219482,0.660000,0.660000,0.000000,35,2 21 | 1404733463082800128,6,6,0.088000,0.300000,89.7085876465,129.8666992188,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462782799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462782799872,6,6,0.088000,0.300000,21.4405059814,46.4491539001,0.343200,0.457600,0.000000,27,0 3 | 1404733462782799872,6,6,0.088000,0.300000,10.1653375626,82.5506439209,0.431200,0.457600,0.000000,27,1 4 | 1404733462782799872,6,6,0.088000,0.300000,38.3202400208,98.1243972778,0.431200,0.545600,0.000000,27,2 5 | 1404733462782799872,6,6,0.088000,0.300000,50.5011329651,61.6103019714,0.343200,0.545600,0.000000,27,3 6 | 1404733462782799872,6,6,0.088000,0.300000,79.3413238525,16.8801631927,0.228800,0.572000,0.000000,32,0 7 | 1404733462782799872,6,6,0.088000,0.300000,63.8487014771,55.1890945435,0.316800,0.572000,0.000000,32,1 8 | 1404733462782799872,6,6,0.088000,0.300000,95.7293243408,72.1322021484,0.316800,0.660000,0.000000,32,2 9 | 1404733462782799872,6,6,0.088000,0.300000,112.1584014893,33.5058708191,0.228800,0.660000,0.000000,32,3 10 | 1404733462782799872,6,6,0.088000,0.300000,59.6579322815,66.6066207886,0.343200,0.572000,0.000000,33,0 11 | 1404733462782799872,6,6,0.088000,0.300000,47.3262138367,102.8555374146,0.431200,0.572000,0.000000,33,1 12 | 1404733462782799872,6,6,0.088000,0.300000,77.8108749390,119.6787948608,0.431200,0.660000,0.000000,33,2 13 | 1404733462782799872,6,6,0.088000,0.300000,91.3089447021,83.3702163696,0.343200,0.660000,0.000000,33,3 14 | 1404733462782799872,6,6,0.088000,0.300000,43.9476928711,113.6403656006,0.457600,0.572000,0.000000,34,0 15 | 1404733462782799872,6,6,0.088000,0.300000,34.2433509827,147.3811645508,0.545600,0.572000,0.000000,34,1 16 | 1404733462782799872,6,6,0.088000,0.300000,63.5594215393,163.7713470459,0.545600,0.660000,0.000000,34,2 17 | 1404733462782799872,6,6,0.088000,0.300000,74.3724746704,130.2476959229,0.457600,0.660000,0.000000,34,3 18 | 1404733462782799872,6,6,0.088000,0.300000,31.6116962433,157.2962951660,0.572000,0.572000,0.000000,35,0 19 | 1404733462782799872,6,6,0.088000,0.300000,24.2411537170,188.1897125244,0.660000,0.572000,0.000000,35,1 20 | 1404733462782799872,6,6,0.088000,0.300000,52.3776283264,204.0946044922,0.660000,0.660000,0.000000,35,2 21 | 1404733462782799872,6,6,0.088000,0.300000,60.7226219177,173.4451141357,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462882800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462882800128,6,6,0.088000,0.300000,42.3536338806,26.4668445587,0.343200,0.457600,0.000000,27,0 3 | 1404733462882800128,6,6,0.088000,0.300000,28.8974571228,62.0573539734,0.431200,0.457600,0.000000,27,1 4 | 1404733462882800128,6,6,0.088000,0.300000,56.6034202576,78.9419326782,0.431200,0.545600,0.000000,27,2 5 | 1404733462882800128,6,6,0.088000,0.300000,70.8867111206,43.1405067444,0.343200,0.545600,0.000000,27,3 6 | 1404733462882800128,6,6,0.088000,0.300000,25.3613014221,72.6481475830,0.457600,0.457600,0.000000,28,0 7 | 1404733462882800128,6,6,0.088000,0.300000,14.6458578110,106.1400451660,0.545600,0.457600,0.000000,28,1 8 | 1404733462882800128,6,6,0.088000,0.300000,41.3592681885,122.9950637817,0.545600,0.545600,0.000000,28,2 9 | 1404733462882800128,6,6,0.088000,0.300000,52.7343254089,89.4369964600,0.457600,0.545600,0.000000,28,3 10 | 1404733462882800128,6,6,0.088000,0.300000,80.0337982178,48.5067214966,0.343200,0.572000,0.000000,33,0 11 | 1404733462882800128,6,6,0.088000,0.300000,65.5005416870,84.3013305664,0.431200,0.572000,0.000000,33,1 12 | 1404733462882800128,6,6,0.088000,0.300000,95.5710296631,102.5509643555,0.431200,0.660000,0.000000,33,2 13 | 1404733462882800128,6,6,0.088000,0.300000,111.1915969849,66.7034683228,0.343200,0.660000,0.000000,33,3 14 | 1404733462882800128,6,6,0.088000,0.300000,61.5344429016,94.8658065796,0.457600,0.572000,0.000000,34,0 15 | 1404733462882800128,6,6,0.088000,0.300000,49.7310485840,128.2451934814,0.545600,0.572000,0.000000,34,1 16 | 1404733462882800128,6,6,0.088000,0.300000,78.5740966797,146.0305938721,0.545600,0.660000,0.000000,34,2 17 | 1404733462882800128,6,6,0.088000,0.300000,91.4111022949,112.8395309448,0.457600,0.660000,0.000000,34,3 18 | 1404733462882800128,6,6,0.088000,0.300000,46.5944900513,137.9835968018,0.572000,0.572000,0.000000,35,0 19 | 1404733462882800128,6,6,0.088000,0.300000,37.3380203247,168.6003417969,0.660000,0.572000,0.000000,35,1 20 | 1404733462882800128,6,6,0.088000,0.300000,64.7997741699,185.9525756836,0.660000,0.660000,0.000000,35,2 21 | 1404733462882800128,6,6,0.088000,0.300000,75.2129898071,155.5531463623,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463432800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463432800000,6,6,0.088000,0.300000,33.3591995239,16.8845024109,0.228800,0.457600,0.000000,26,0 3 | 1404733463432800000,6,6,0.088000,0.300000,19.1077098846,54.2842559814,0.316800,0.457600,0.000000,26,1 4 | 1404733463432800000,6,6,0.088000,0.300000,47.9120521545,69.6862106323,0.316800,0.545600,0.000000,26,2 5 | 1404733463432800000,6,6,0.088000,0.300000,62.8790245056,31.6445274353,0.228800,0.545600,0.000000,26,3 6 | 1404733463432800000,6,6,0.088000,0.300000,72.5360946655,36.5736236572,0.228800,0.572000,0.000000,32,0 7 | 1404733463432800000,6,6,0.088000,0.300000,57.2990303040,74.4118423462,0.316800,0.572000,0.000000,32,1 8 | 1404733463432800000,6,6,0.088000,0.300000,88.6441955566,91.0464782715,0.316800,0.660000,0.000000,32,2 9 | 1404733463432800000,6,6,0.088000,0.300000,104.8138198853,53.0024147034,0.228800,0.660000,0.000000,32,3 10 | 1404733463432800000,6,6,0.088000,0.300000,53.1269721985,85.7271652222,0.343200,0.572000,0.000000,33,0 11 | 1404733463432800000,6,6,0.088000,0.300000,40.6795730591,121.6930465698,0.431200,0.572000,0.000000,33,1 12 | 1404733463432800000,6,6,0.088000,0.300000,70.8853378296,138.4004974365,0.431200,0.660000,0.000000,33,2 13 | 1404733463432800000,6,6,0.088000,0.300000,84.3936462402,102.2853851318,0.343200,0.660000,0.000000,33,3 14 | 1404733463432800000,6,6,0.088000,0.300000,37.3841590881,132.5198364258,0.457600,0.572000,0.000000,34,0 15 | 1404733463432800000,6,6,0.088000,0.300000,27.5221004486,166.0602874756,0.545600,0.572000,0.000000,34,1 16 | 1404733463432800000,6,6,0.088000,0.300000,56.6785545349,182.3899383545,0.545600,0.660000,0.000000,34,2 17 | 1404733463432800000,6,6,0.088000,0.300000,67.5056915283,148.7191925049,0.457600,0.660000,0.000000,34,3 18 | 1404733463432800000,6,6,0.088000,0.300000,24.8757286072,175.9936523438,0.572000,0.572000,0.000000,35,0 19 | 1404733463432800000,6,6,0.088000,0.300000,17.4412765503,206.8665618896,0.660000,0.572000,0.000000,35,1 20 | 1404733463432800000,6,6,0.088000,0.300000,45.5166397095,222.6889343262,0.660000,0.660000,0.000000,35,2 21 | 1404733463432800000,6,6,0.088000,0.300000,53.8661270142,192.0082702637,0.572000,0.660000,0.000000,35,3 22 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/config.yaml: -------------------------------------------------------------------------------- 1 | settings: 2 | # General 3 | verbose: true 4 | max_num_threads: 8 5 | 6 | # Extrinsics initialization 7 | enable_extrinsics_outlier_filter: true 8 | 9 | # Final stage settings 10 | enable_nbv: true 11 | enable_shuffle_views: true 12 | enable_nbv_filter: true 13 | enable_outlier_filter: false 14 | enable_marginalization: true 15 | enable_cross_validation: false 16 | enable_early_stopping: false 17 | min_nbv_views: 40 18 | outlier_threshold: 4.0 19 | info_gain_threshold: 0.2 20 | sliding_window_size: 10 21 | early_stop_threshold: 30 22 | 23 | enable_vision_loss_fn: false 24 | vision_loss_fn_type: "CAUCHY" 25 | vision_loss_fn_param: 1.5 26 | enable_imu_loss_fn: false 27 | imu_loss_fn_type: "HUBER" 28 | imu_loss_fn_param: 2.0 29 | 30 | estimate_time_delay: true 31 | time_delay_jac_step: 1e-8 32 | 33 | calib_target: 34 | target_type: 'aprilgrid' # Target type 35 | tag_rows: 6 # Number of rows 36 | tag_cols: 6 # Number of cols 37 | tag_size: 0.088 # Size of apriltag, edge to edge [m] 38 | tag_spacing: 0.3 # Ratio of space between tags to tagSize 39 | # Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 40 | 41 | imu0: 42 | rate: 200.0 43 | sigma_a_c: 0.002 44 | sigma_g_c: 1.6968e-04 45 | sigma_aw_c: 0.003 46 | sigma_gw_c: 1.9393e-05 47 | g: 9.81007 48 | 49 | cam0: 50 | resolution: [752, 480] 51 | proj_model: "pinhole" 52 | dist_model: "radtan4" 53 | proj_params: [458.654, 457.296, 367.215, 248.375] 54 | dist_params: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 55 | 56 | cam1: 57 | resolution: [752, 480] 58 | proj_model: "pinhole" 59 | dist_model: "radtan4" 60 | proj_params: [457.587, 456.134, 379.999, 255.238] 61 | dist_params: [-0.28368365, 0.07451284, -0.00010473, -3.555e-05] 62 | 63 | T_cam0_cam1: 64 | rows: 4 65 | cols: 4 66 | data: [0.999997256478, -0.002317135723, -0.000343393121, 0.110074137800, 67 | 0.002312067192, 0.999898048507, -0.014090668453, -0.000156612054, 68 | 0.000376008102, 0.014089835847, 0.999900662638, 0.000889382785, 69 | 0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000] 70 | -------------------------------------------------------------------------------- /yac_ros/src/nodes/calib_camera_node.cpp: -------------------------------------------------------------------------------- 1 | #include "yac/yac.hpp" 2 | #include "../ros_utils.hpp" 3 | 4 | using namespace yac; 5 | 6 | void process_rosbag(const std::string &config_file, calib_camera_t &calib) { 7 | // Parse config file 8 | config_t config{config_file}; 9 | 10 | std::string rosbag_path; 11 | std::map cam_topics; 12 | std::string data_path; 13 | parse(config, "ros.bag", rosbag_path); 14 | parse_camera_topics(config, cam_topics); 15 | 16 | // Open ROS bag 17 | LOG_INFO("Processing ROS bag [%s]", rosbag_path.c_str()); 18 | rosbag::Bag bag; 19 | bag.open(rosbag_path, rosbag::bagmode::Read); 20 | 21 | // Process ROS bag 22 | rosbag::View bag_view(bag); 23 | size_t msg_idx = 0; 24 | std::map> cam_data; 25 | 26 | for (const auto &msg : bag_view) { 27 | // Handle camera mesages 28 | for (const auto [cam_idx, cam_topic] : cam_topics) { 29 | if (msg.getTopic() == cam_topic) { 30 | const auto image_msg = msg.instantiate(); 31 | const auto image_ptr = cv_bridge::toCvCopy(image_msg, "mono8"); 32 | const timestamp_t ts = ros::Time(image_msg->header.stamp).toNSec(); 33 | const cv::Mat cam_img = image_ptr->image; 34 | cam_data[cam_idx][ts] = calib.detector->detect(ts, cam_img); 35 | } 36 | } 37 | 38 | // Print progress 39 | msg_idx++; 40 | if ((msg_idx % 10) == 0) { 41 | printf("."); 42 | fflush(stdout); 43 | } 44 | } 45 | printf("\n"); 46 | 47 | // Add camera data to calibrator 48 | calib.add_camera_data(cam_data); 49 | 50 | // Clean up rosbag 51 | bag.close(); 52 | } 53 | 54 | int main(int argc, char *argv[]) { 55 | // Setup ROS Node 56 | std::string node_name = ros_node_name(argc, argv); 57 | if (ros::isInitialized() == false) { 58 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 59 | } 60 | 61 | // Get ROS params 62 | const ros::NodeHandle ros_nh; 63 | std::string config_file; 64 | ROS_PARAM(ros_nh, node_name + "/config_file", config_file); 65 | 66 | // Calibrate camera intrinsics 67 | calib_camera_t calib{config_file}; 68 | process_rosbag(config_file, calib); 69 | calib.solve(); 70 | calib.save_results("/tmp/calib-results.yaml"); 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /yac_ros/launch/intel_d435i-calib_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 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 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /yac/lib/calib_data.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_CALIB_DATA_HPP 2 | #define YAC_CALIB_DATA_HPP 3 | 4 | #include "util/util.hpp" 5 | 6 | namespace yac { 7 | 8 | // CALIB UTILS ///////////////////////////////////////////////////////////////// 9 | 10 | /** Camera Data **/ 11 | using camera_data_t = std::map>; 12 | 13 | void lerp_body_poses(const aprilgrids_t &grids, 14 | const timestamps_t &body_timestamps, 15 | const mat4s_t &body_poses, 16 | aprilgrids_t &lerped_grids, 17 | mat4s_t &lerped_poses, 18 | timestamp_t ts_offset = 0); 19 | 20 | void load_imu_data(const std::string &csv_path, 21 | timestamps_t ×tamps, 22 | vec3s_t &a_B, 23 | vec3s_t &w_B); 24 | 25 | // CALIB TARGET //////////////////////////////////////////////////////////////// 26 | 27 | /** Calibration target */ 28 | struct calib_target_t { 29 | std::string target_type = "aprilgrid"; 30 | int tag_rows = 6; 31 | int tag_cols = 6; 32 | real_t tag_size = 0.088; 33 | real_t tag_spacing = 0.3; 34 | 35 | calib_target_t() = default; 36 | calib_target_t(const std::string target_type_, 37 | const int tag_rows_, 38 | const int tag_cols_, 39 | const real_t tag_size_, 40 | const real_t tag_spacing_); 41 | ~calib_target_t() = default; 42 | 43 | /** 44 | * Load calibration target. 45 | * @returns 0 or -1 for success or failure 46 | */ 47 | int load(const std::string &target_file, const std::string &prefix = ""); 48 | 49 | /** Print calibration target */ 50 | void print() const; 51 | }; 52 | 53 | // CALIBRATION DETECTION ////////////////////////////////////////////////////// 54 | 55 | /*** 56 | * Preprocess / Load AprilGrid data 57 | * 58 | * @param[in] calib_target Calibration Target Configuration 59 | * @param[in] cam_paths Paths to camera images 60 | * @param[in] grids_path Path to save detected AprilGrids 61 | * @param[in] imshow Show detections 62 | * 63 | * @returns AprilGrid data observed by each camera 64 | */ 65 | std::map 66 | calib_data_preprocess(const calib_target_t &calib_target, 67 | const std::map cam_paths, 68 | const std::string &grids_path, 69 | const bool imshow = false); 70 | 71 | } // namespace yac 72 | #endif // YAC_CALIB_DATA_HPP 73 | -------------------------------------------------------------------------------- /scripts/plot_imu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import pandas 3 | 4 | import proto 5 | from proto import tf 6 | from proto import tf_trans 7 | from proto import tf_rot 8 | from proto import plot_tf 9 | from proto import plot_set_axes_equal 10 | 11 | import matplotlib.pylab as plt 12 | from mpl_toolkits import mplot3d 13 | 14 | 15 | def plot_imu_measurements(imu_data): 16 | """ Plot IMU Measurements """ 17 | ts = imu_data['ts'].to_numpy() 18 | time = (ts - ts[0]) * 1e-9 19 | acc = imu_data[['ax', 'ay', 'az']].to_numpy() 20 | gyr = imu_data[['wx', 'wy', 'wz']].to_numpy() 21 | 22 | plt.figure() 23 | plt.subplot(211) 24 | plt.plot(time, acc[:, 0], 'r-', label="ax") 25 | plt.plot(time, acc[:, 1], 'g-', label="ay") 26 | plt.plot(time, acc[:, 2], 'b-', label="az") 27 | plt.title("Accelerometer Measurements") 28 | plt.xlabel("Time [s]") 29 | plt.ylabel("Acceleration [ms^-2]") 30 | plt.legend(loc=0) 31 | 32 | plt.subplot(212) 33 | plt.plot(time, gyr[:, 0], 'r-', label="wx") 34 | plt.plot(time, gyr[:, 1], 'g-', label="wy") 35 | plt.plot(time, gyr[:, 2], 'b-', label="wz") 36 | plt.title("Gyroscope Measurements") 37 | plt.xlabel("Time [s]") 38 | plt.ylabel("Angular Velocity [rad s^-1]") 39 | plt.legend(loc=0) 40 | plt.subplots_adjust(hspace=0.5) 41 | 42 | 43 | def plot_imu_velocities(imu_data): 44 | """ Plot IMU Velocities """ 45 | ts = imu_data['ts'].to_numpy() 46 | time = (ts - ts[0]) * 1e-9 47 | vel = imu_data[['vx', 'vy', 'vz']].to_numpy() 48 | 49 | plt.figure() 50 | plt.plot(time, vel[:, 0], 'r-', label="vx") 51 | plt.plot(time, vel[:, 1], 'g-', label="vy") 52 | plt.plot(time, vel[:, 2], 'b-', label="vz") 53 | plt.title("IMU Velocities") 54 | plt.xlabel("Time [s]") 55 | plt.ylabel("Velocity [ms^-1]") 56 | plt.legend(loc=0) 57 | 58 | 59 | def plot_scene(imu_data): 60 | """ Plot 3D Scene """ 61 | plt.figure() 62 | ax = plt.axes(projection='3d') 63 | 64 | for idx, data in imu_data.iterrows(): 65 | if (idx % 50) == 0: 66 | q_WS = data[['qw', 'qx', 'qy', 'qz']].to_numpy() 67 | r_WS = data[['rx', 'ry', 'rz']].to_numpy() 68 | T_WS = tf(q_WS, r_WS) 69 | plot_tf(ax, T_WS, size=0.3) 70 | 71 | ax.set_xlabel("x [m]") 72 | ax.set_ylabel("y [m]") 73 | ax.set_zlabel("z [m]") 74 | plot_set_axes_equal(ax) 75 | 76 | 77 | if __name__ == "__main__": 78 | imu_data = pandas.read_csv("/tmp/imu_data.csv") 79 | plot_imu_measurements(imu_data) 80 | # plot_imu_velocities(imu_data) 81 | # plot_scene(imu_data) 82 | plt.show() 83 | -------------------------------------------------------------------------------- /yac/lib/calib_loss.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_CALIB_LOSS_HPP 2 | #define YAC_CALIB_LOSS_HPP 3 | 4 | #include 5 | 6 | namespace yac { 7 | 8 | // CALIB LOSS ///////////////////////////////////////////////////////////////// 9 | 10 | struct calib_loss_t : public ceres::LossFunction { 11 | // Data 12 | std::string type; 13 | 14 | /* Constructor */ 15 | calib_loss_t(const std::string &type_) : type{type_} {} 16 | 17 | /* Destructor */ 18 | virtual ~calib_loss_t() = default; 19 | 20 | /* Evaluate */ 21 | virtual void Evaluate(double sq_norm, double rho[3]) const = 0; 22 | }; 23 | 24 | // HUBER LOSS ///////////////////////////////////////////////////////////////// 25 | 26 | class HuberLoss : public calib_loss_t { 27 | public: 28 | ceres::HuberLoss *loss; 29 | 30 | /* Constructor */ 31 | HuberLoss(const double s); 32 | 33 | /* Destructor */ 34 | virtual ~HuberLoss(); 35 | 36 | /* Evaluate */ 37 | virtual void Evaluate(double sq_norm, double rho[3]) const override; 38 | }; 39 | 40 | // CAUCHY LOSS ///////////////////////////////////////////////////////////////// 41 | 42 | class CauchyLoss : public calib_loss_t { 43 | public: 44 | ceres::CauchyLoss *loss; 45 | 46 | /* Constructor */ 47 | CauchyLoss(const double s); 48 | 49 | /* Destructor */ 50 | virtual ~CauchyLoss(); 51 | 52 | /* Evaluate */ 53 | virtual void Evaluate(double sq_norm, double rho[3]) const override; 54 | }; 55 | 56 | // BLAKE-ZISSERMAN LOSS /////////////////////////////////////////////////////// 57 | 58 | /** 59 | * Modified Blake-Zisserman Loss function 60 | * 61 | * Reference: 62 | * 63 | * "Online Self-Calibration for Robotic Systems", by Jerome Maye. 64 | * https://doi.org/10.3929/ethz-a-010213941 65 | * 66 | * and page 618 in: 67 | * 68 | * "MULTIPLE VIEW GEOMETRY IN COMPUTER VISION, by Richard Hartley and Andrew 69 | * Zisserman, Cambridge University Press, Cambridge, 2000. 70 | * 71 | */ 72 | class BlakeZissermanLoss : public calib_loss_t { 73 | public: 74 | double eps; 75 | 76 | /** 77 | * Constructor 78 | * 79 | * @param df Dimension of the error term 80 | * @param p Probability of the error you want to down-weight. Setting 81 | * p to 0.99 implies 99% of squared errors will not be 82 | * down-weighted. 83 | * @param w_q Weight (between 0 and 1 not inclusive) 84 | */ 85 | BlakeZissermanLoss(size_t df, double p = 0.999, double w_q = 0.1); 86 | 87 | /* Evaluate */ 88 | virtual void Evaluate(double sq_norm, double rho[3]) const override; 89 | }; 90 | 91 | } // namespace yac 92 | #endif // YAC_CALIB_LOSS_HPP 93 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | SHELL:=/bin/bash 2 | PREFIX=/opt/yac 3 | BUILD_DIR=build 4 | CATKIN_WS=${HOME}/yac_ws 5 | YAC_PATH=${CATKIN_WS}/src/yac 6 | ROS_VERSION="noetic" 7 | NUM_CPU=2 8 | 9 | .PHONY: help deps lib_debug lib debug release download_test_data tests 10 | 11 | define compile_yac 12 | @cd ${BUILD_DIR} \ 13 | && cmake ../yac \ 14 | -DCMAKE_BUILD_TYPE=$(1) \ 15 | -DCMAKE_INSTALL_PREFIX=${PREFIX} \ 16 | && time sudo make install -s -j${NUM_CPU} 17 | endef 18 | 19 | define compile_yac_ros 20 | @cd ${CATKIN_WS} \ 21 | && . /opt/ros/${ROS_VERSION}/setup.sh \ 22 | && catkin build yac_ros -DCMAKE_BUILD_TYPE=Release -j${NUM_CPU} 23 | endef 24 | 25 | help: 26 | @echo -e "\033[1;34m[make targets]:\033[0m" 27 | @grep -E '^[a-zA-Z_-]+:.*?## .*$$' $(MAKEFILE_LIST) \ 28 | | awk 'BEGIN {FS = ":.*?## "}; \ 29 | {printf "\033[1;36m%-20s\033[0m%s\n", $$1, $$2}' 30 | 31 | ${BUILD_DIR}: 32 | @sudo mkdir -p ${BUILD_DIR} 33 | 34 | ${PREFIX}: 35 | @sudo mkdir -p ${PREFIX} 36 | 37 | ${CATKIN_WS}: 38 | @mkdir -p ${CATKIN_WS}/src; catkin init 39 | 40 | ${YAC_PATH}: 41 | ln -sf ${PWD} ${CATKIN_WS}/src/yac 42 | 43 | deps: ## Install dependencies 44 | @echo "[Installing Dependencies]" 45 | @make -s -C deps 46 | 47 | lib_debug: ${BUILD_DIR} ${PREFIX} ## Build libyac in debug mode 48 | $(call compile_yac,Debug) 49 | 50 | lib_relwithdeb: ${BUILD_DIR} ${PREFIX} ## Build libyac in release with debug info 51 | $(call compile_yac,RelWithDebInfo) 52 | 53 | lib: ${BUILD_DIR} ${PREFIX} ## Build libyac in release mode 54 | $(call compile_yac,Release) 55 | 56 | download_test_data: ## Download test data 57 | @bash ./scripts/download_test_data.bash 58 | 59 | tests: ## Build and run tests 60 | @cd build \ 61 | && ./test_util_config \ 62 | && ./test_util_cv \ 63 | && ./test_util_data \ 64 | && ./test_util_fs \ 65 | && ./test_util_net \ 66 | && ./test_util_timeline \ 67 | && ./test_calib_camera \ 68 | && ./test_calib_data \ 69 | && ./test_calib_mocap \ 70 | && ./test_calib_vi 71 | 72 | debug: ${CATKIN_WS} ${YAC_PATH} ## Build libyac and yac_ros in debug mode 73 | $(call compile_yac_ros,Debug) 74 | 75 | relwithdeb: ${CATKIN_WS} ${YAC_PATH} ## Build libyac and yac_ros in release with debug info 76 | $(call compile_yac_ros,RelWithDebInfo) 77 | 78 | release: ${CATKIN_WS} ${YAC_PATH} ## Build libyac and yac_ros in release mode 79 | $(call compile_yac_ros,Release) 80 | 81 | build_docker: 82 | docker build -f Dockerfile --no-cache -t chutsu/yac . 83 | 84 | run_docker: 85 | @xhost +local:docker 86 | @docker run -e DISPLAY \ 87 | -v /tmp/.X11-unix:/tmp/.X11-unix \ 88 | -v /data:/data \ 89 | --network="host" \ 90 | -it \ 91 | --rm chutsu/yac 92 | 93 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733462832800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462832800000,6,6,0.088000,0.300000,34.5675430298,34.1713676453,0.343200,0.457600,0.000000,27,0 3 | 1404733462832800000,6,6,0.088000,0.300000,22.0813922882,69.8459625244,0.431200,0.457600,0.000000,27,1 4 | 1404733462832800000,6,6,0.088000,0.300000,49.8486213684,86.4206542969,0.431200,0.545600,0.000000,27,2 5 | 1404733462832800000,6,6,0.088000,0.300000,63.4528999329,50.2362213135,0.343200,0.545600,0.000000,27,3 6 | 1404733462832800000,6,6,0.088000,0.300000,18.6554450989,80.5718383789,0.457600,0.457600,0.000000,28,0 7 | 1404733462832800000,6,6,0.088000,0.300000,8.7807636261,114.1496047974,0.545600,0.457600,0.000000,28,1 8 | 1404733462832800000,6,6,0.088000,0.300000,35.6165657043,130.5650939941,0.545600,0.545600,0.000000,28,2 9 | 1404733462832800000,6,6,0.088000,0.300000,46.3841171265,96.7498779297,0.457600,0.545600,0.000000,28,3 10 | 1404733462832800000,6,6,0.088000,0.300000,93.7831344604,6.3610658646,0.228800,0.572000,0.000000,32,0 11 | 1404733462832800000,6,6,0.088000,0.300000,77.1143646240,44.1303062439,0.316800,0.572000,0.000000,32,1 12 | 1404733462832800000,6,6,0.088000,0.300000,108.6280670166,61.9739074707,0.316800,0.660000,0.000000,32,2 13 | 1404733462832800000,6,6,0.088000,0.300000,126.4404449463,23.8537158966,0.228800,0.660000,0.000000,32,3 14 | 1404733462832800000,6,6,0.088000,0.300000,72.5236434937,55.4738197327,0.343200,0.572000,0.000000,33,0 15 | 1404733462832800000,6,6,0.088000,0.300000,58.7722129822,91.4465789795,0.431200,0.572000,0.000000,33,1 16 | 1404733462832800000,6,6,0.088000,0.300000,89.0241470337,109.1451339722,0.431200,0.660000,0.000000,33,2 17 | 1404733462832800000,6,6,0.088000,0.300000,103.7854461670,73.1256332397,0.343200,0.660000,0.000000,33,3 18 | 1404733462832800000,6,6,0.088000,0.300000,55.1071205139,102.1324005127,0.457600,0.572000,0.000000,34,0 19 | 1404733462832800000,6,6,0.088000,0.300000,44.1524696350,135.5213165283,0.545600,0.572000,0.000000,34,1 20 | 1404733462832800000,6,6,0.088000,0.300000,73.1802444458,152.8572998047,0.545600,0.660000,0.000000,34,2 21 | 1404733462832800000,6,6,0.088000,0.300000,85.0546112061,119.5220413208,0.457600,0.660000,0.000000,34,3 22 | 1404733462832800000,6,6,0.088000,0.300000,41.1747283936,145.3833923340,0.572000,0.572000,0.000000,35,0 23 | 1404733462832800000,6,6,0.088000,0.300000,32.6098480225,176.0758209229,0.660000,0.572000,0.000000,35,1 24 | 1404733462832800000,6,6,0.088000,0.300000,60.4744834900,192.9114990234,0.660000,0.660000,0.000000,35,2 25 | 1404733462832800000,6,6,0.088000,0.300000,69.9027938843,162.4725189209,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462782799872.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462782799872,6,6,0.088000,0.300000,44.6597900391,22.9224319458,0.343200,0.457600,0.000000,27,0 3 | 1404733462782799872,6,6,0.088000,0.300000,30.8790683746,60.7455215454,0.431200,0.457600,0.000000,27,1 4 | 1404733462782799872,6,6,0.088000,0.300000,62.0456809998,77.4824447632,0.431200,0.545600,0.000000,27,2 5 | 1404733462782799872,6,6,0.088000,0.300000,76.7371063232,39.3303909302,0.343200,0.545600,0.000000,27,3 6 | 1404733462782799872,6,6,0.088000,0.300000,27.2495937347,72.1575164795,0.457600,0.457600,0.000000,28,0 7 | 1404733462782799872,6,6,0.088000,0.300000,16.3856277466,107.6052780151,0.545600,0.457600,0.000000,28,1 8 | 1404733462782799872,6,6,0.088000,0.300000,46.3283195496,124.2185821533,0.545600,0.545600,0.000000,28,2 9 | 1404733462782799872,6,6,0.088000,0.300000,58.0388679504,88.4803237915,0.457600,0.545600,0.000000,28,3 10 | 1404733462782799872,6,6,0.088000,0.300000,13.5288209915,118.2572784424,0.572000,0.457600,0.000000,29,0 11 | 1404733462782799872,6,6,0.088000,0.300000,5.2055077553,150.9247436523,0.660000,0.457600,0.000000,29,1 12 | 1404733462782799872,6,6,0.088000,0.300000,33.7376556396,167.0241546631,0.660000,0.545600,0.000000,29,2 13 | 1404733462782799872,6,6,0.088000,0.300000,43.1426277161,134.3749542236,0.572000,0.545600,0.000000,29,3 14 | 1404733462782799872,6,6,0.088000,0.300000,86.9886169434,44.5924644470,0.343200,0.572000,0.000000,33,0 15 | 1404733462782799872,6,6,0.088000,0.300000,71.9128494263,82.4779891968,0.431200,0.572000,0.000000,33,1 16 | 1404733462782799872,6,6,0.088000,0.300000,105.4470596313,100.4322738647,0.431200,0.660000,0.000000,33,2 17 | 1404733462782799872,6,6,0.088000,0.300000,121.5722732544,62.5311012268,0.343200,0.660000,0.000000,33,3 18 | 1404733462782799872,6,6,0.088000,0.300000,67.8111343384,93.7933731079,0.457600,0.572000,0.000000,34,0 19 | 1404733462782799872,6,6,0.088000,0.300000,55.5920677185,129.1758422852,0.545600,0.572000,0.000000,34,1 20 | 1404733462782799872,6,6,0.088000,0.300000,87.6641387939,146.5435791016,0.545600,0.660000,0.000000,34,2 21 | 1404733462782799872,6,6,0.088000,0.300000,100.8685913086,111.3801345825,0.457600,0.660000,0.000000,34,3 22 | 1404733462782799872,6,6,0.088000,0.300000,52.4252510071,139.5223693848,0.572000,0.572000,0.000000,35,0 23 | 1404733462782799872,6,6,0.088000,0.300000,42.7654571533,171.9027404785,0.660000,0.572000,0.000000,35,1 24 | 1404733462782799872,6,6,0.088000,0.300000,73.4998550415,188.6552734375,0.660000,0.660000,0.000000,35,2 25 | 1404733462782799872,6,6,0.088000,0.300000,84.1751556396,156.5471801758,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733462832800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733462832800000,6,6,0.088000,0.300000,59.1155014038,10.3793334961,0.343200,0.457600,0.000000,27,0 3 | 1404733462832800000,6,6,0.088000,0.300000,43.9614753723,47.6984405518,0.431200,0.457600,0.000000,27,1 4 | 1404733462832800000,6,6,0.088000,0.300000,74.6863327026,65.3053283691,0.431200,0.545600,0.000000,27,2 5 | 1404733462832800000,6,6,0.088000,0.300000,90.8566284180,27.5798149109,0.343200,0.545600,0.000000,27,3 6 | 1404733462832800000,6,6,0.088000,0.300000,39.8082695007,59.0211868286,0.457600,0.457600,0.000000,28,0 7 | 1404733462832800000,6,6,0.088000,0.300000,27.6412601471,94.2534027100,0.545600,0.457600,0.000000,28,1 8 | 1404733462832800000,6,6,0.088000,0.300000,57.2785873413,111.6284713745,0.545600,0.545600,0.000000,28,2 9 | 1404733462832800000,6,6,0.088000,0.300000,70.4136734009,76.2581710815,0.457600,0.545600,0.000000,28,3 10 | 1404733462832800000,6,6,0.088000,0.300000,24.4676170349,104.6269226074,0.572000,0.457600,0.000000,29,0 11 | 1404733462832800000,6,6,0.088000,0.300000,14.8478116989,137.3092956543,0.660000,0.457600,0.000000,29,1 12 | 1404733462832800000,6,6,0.088000,0.300000,43.2171936035,154.2773742676,0.660000,0.545600,0.000000,29,2 13 | 1404733462832800000,6,6,0.088000,0.300000,53.6463317871,121.6567153931,0.572000,0.545600,0.000000,29,3 14 | 1404733462832800000,6,6,0.088000,0.300000,100.9032974243,33.2792282104,0.343200,0.572000,0.000000,33,0 15 | 1404733462832800000,6,6,0.088000,0.300000,84.5199203491,70.5923233032,0.431200,0.572000,0.000000,33,1 16 | 1404733462832800000,6,6,0.088000,0.300000,117.5553512573,89.5710296631,0.431200,0.660000,0.000000,33,2 17 | 1404733462832800000,6,6,0.088000,0.300000,135.0592498779,52.2880516052,0.343200,0.660000,0.000000,33,3 18 | 1404733462832800000,6,6,0.088000,0.300000,80.0429229736,81.7312698364,0.457600,0.572000,0.000000,34,0 19 | 1404733462832800000,6,6,0.088000,0.300000,66.5002288818,116.7512512207,0.545600,0.572000,0.000000,34,1 20 | 1404733462832800000,6,6,0.088000,0.300000,98.1383972168,135.1859436035,0.545600,0.660000,0.000000,34,2 21 | 1404733462832800000,6,6,0.088000,0.300000,112.6888656616,100.3909988403,0.457600,0.660000,0.000000,34,3 22 | 1404733462832800000,6,6,0.088000,0.300000,62.7850914001,127.1788482666,0.572000,0.572000,0.000000,35,0 23 | 1404733462832800000,6,6,0.088000,0.300000,52.0792160034,159.4043426514,0.660000,0.572000,0.000000,35,1 24 | 1404733462832800000,6,6,0.088000,0.300000,82.3629531860,177.0279846191,0.660000,0.660000,0.000000,35,2 25 | 1404733462832800000,6,6,0.088000,0.300000,94.2478179932,145.1890869141,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463432800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463432800000,6,6,0.088000,0.300000,37.2176628113,43.0423545837,0.343200,0.457600,0.000000,27,0 3 | 1404733463432800000,6,6,0.088000,0.300000,23.4606876373,80.5841522217,0.431200,0.457600,0.000000,27,1 4 | 1404733463432800000,6,6,0.088000,0.300000,54.4279747009,97.2479629517,0.431200,0.545600,0.000000,27,2 5 | 1404733463432800000,6,6,0.088000,0.300000,68.8705139160,59.3131675720,0.343200,0.545600,0.000000,27,3 6 | 1404733463432800000,6,6,0.088000,0.300000,19.7315330505,91.9454803467,0.457600,0.457600,0.000000,28,0 7 | 1404733463432800000,6,6,0.088000,0.300000,8.8942346573,127.3949127197,0.545600,0.457600,0.000000,28,1 8 | 1404733463432800000,6,6,0.088000,0.300000,38.6018714905,143.6969146729,0.545600,0.545600,0.000000,28,2 9 | 1404733463432800000,6,6,0.088000,0.300000,50.4513053894,108.2434158325,0.457600,0.545600,0.000000,28,3 10 | 1404733463432800000,6,6,0.088000,0.300000,101.9167938232,13.4112634659,0.228800,0.572000,0.000000,32,0 11 | 1404733463432800000,6,6,0.088000,0.300000,84.1096954346,52.5969886780,0.316800,0.572000,0.000000,32,1 12 | 1404733463432800000,6,6,0.088000,0.300000,118.4783248901,70.6405334473,0.316800,0.660000,0.000000,32,2 13 | 1404733463432800000,6,6,0.088000,0.300000,137.2680053711,31.2830963135,0.228800,0.660000,0.000000,32,3 14 | 1404733463432800000,6,6,0.088000,0.300000,79.0144805908,64.5136108398,0.343200,0.572000,0.000000,33,0 15 | 1404733463432800000,6,6,0.088000,0.300000,64.1325759888,102.2048263550,0.431200,0.572000,0.000000,33,1 16 | 1404733463432800000,6,6,0.088000,0.300000,97.3962402344,119.8188705444,0.431200,0.660000,0.000000,33,2 17 | 1404733463432800000,6,6,0.088000,0.300000,113.1386413574,82.2971191406,0.343200,0.660000,0.000000,33,3 18 | 1404733463432800000,6,6,0.088000,0.300000,60.0917015076,113.4469909668,0.457600,0.572000,0.000000,34,0 19 | 1404733463432800000,6,6,0.088000,0.300000,48.0526199341,148.6421356201,0.545600,0.572000,0.000000,34,1 20 | 1404733463432800000,6,6,0.088000,0.300000,79.8141555786,165.8097381592,0.545600,0.660000,0.000000,34,2 21 | 1404733463432800000,6,6,0.088000,0.300000,92.9480972290,130.6738739014,0.457600,0.660000,0.000000,34,3 22 | 1404733463432800000,6,6,0.088000,0.300000,44.7550621033,159.1519012451,0.572000,0.572000,0.000000,35,0 23 | 1404733463432800000,6,6,0.088000,0.300000,35.2780838013,191.5456542969,0.660000,0.572000,0.000000,35,1 24 | 1404733463432800000,6,6,0.088000,0.300000,65.7587661743,208.1374511719,0.660000,0.660000,0.000000,35,2 25 | 1404733463432800000,6,6,0.088000,0.300000,76.4194869995,175.8880310059,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733461482800128,6,6,0.088000,0.300000,71.5568313599,15.1611423492,0.572000,0.343200,0.000000,23,0 3 | 1404733461482800128,6,6,0.088000,0.300000,47.7005462646,44.9624443054,0.660000,0.343200,0.000000,23,1 4 | 1404733461482800128,6,6,0.088000,0.300000,68.6223678589,72.6458511353,0.660000,0.431200,0.000000,23,2 5 | 1404733461482800128,6,6,0.088000,0.300000,93.5049362183,42.6874275208,0.572000,0.431200,0.000000,23,3 6 | 1404733461482800128,6,6,0.088000,0.300000,137.2686920166,11.6972970963,0.457600,0.457600,0.000000,28,0 7 | 1404733461482800128,6,6,0.088000,0.300000,108.5662078857,42.4292144775,0.545600,0.457600,0.000000,28,1 8 | 1404733461482800128,6,6,0.088000,0.300000,132.7123870850,72.5815811157,0.545600,0.545600,0.000000,28,2 9 | 1404733461482800128,6,6,0.088000,0.300000,162.4624328613,41.8915786743,0.457600,0.545600,0.000000,28,3 10 | 1404733461482800128,6,6,0.088000,0.300000,100.3921661377,51.9429588318,0.572000,0.457600,0.000000,29,0 11 | 1404733461482800128,6,6,0.088000,0.300000,75.5217437744,81.4867401123,0.660000,0.457600,0.000000,29,1 12 | 1404733461482800128,6,6,0.088000,0.300000,98.6048583984,110.9246673584,0.660000,0.545600,0.000000,29,2 13 | 1404733461482800128,6,6,0.088000,0.300000,124.5499114990,81.6654739380,0.572000,0.545600,0.000000,29,3 14 | 1404733461482800128,6,6,0.088000,0.300000,213.4054260254,11.5486278534,0.343200,0.572000,0.000000,33,0 15 | 1404733461482800128,6,6,0.088000,0.300000,180.2138519287,42.3558769226,0.431200,0.572000,0.000000,33,1 16 | 1404733461482800128,6,6,0.088000,0.300000,207.2748565674,75.1608657837,0.431200,0.660000,0.000000,33,2 17 | 1404733461482800128,6,6,0.088000,0.300000,241.4274291992,44.5247573853,0.343200,0.660000,0.000000,33,3 18 | 1404733461482800128,6,6,0.088000,0.300000,170.3753814697,51.9967117310,0.457600,0.572000,0.000000,34,0 19 | 1404733461482800128,6,6,0.088000,0.300000,140.5519104004,82.1904983521,0.545600,0.572000,0.000000,34,1 20 | 1404733461482800128,6,6,0.088000,0.300000,166.6956024170,114.1896209717,0.545600,0.660000,0.000000,34,2 21 | 1404733461482800128,6,6,0.088000,0.300000,197.4805603027,84.3605575562,0.457600,0.660000,0.000000,34,3 22 | 1404733461482800128,6,6,0.088000,0.300000,132.0500488281,91.3709030151,0.572000,0.572000,0.000000,35,0 23 | 1404733461482800128,6,6,0.088000,0.300000,106.1468124390,120.2070541382,0.660000,0.572000,0.000000,35,1 24 | 1404733461482800128,6,6,0.088000,0.300000,131.2930755615,150.9781188965,0.660000,0.660000,0.000000,35,2 25 | 1404733461482800128,6,6,0.088000,0.300000,158.2149658203,122.7549743652,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733433732800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733433732800000,6,6,0.088000,0.300000,398.4637756348,300.4274902344,0.000000,0.228800,0.000000,12,0 3 | 1404733433732800000,6,6,0.088000,0.300000,406.5133666992,280.6159973145,0.088000,0.228800,0.000000,12,1 4 | 1404733433732800000,6,6,0.088000,0.300000,385.4033508301,273.7596130371,0.088000,0.316800,0.000000,12,2 5 | 1404733433732800000,6,6,0.088000,0.300000,377.8522644043,293.1820983887,0.000000,0.316800,0.000000,12,3 6 | 1404733433732800000,6,6,0.088000,0.300000,452.4183959961,160.5554656982,0.572000,0.228800,0.000000,17,0 7 | 1404733433732800000,6,6,0.088000,0.300000,460.8389892578,138.9723052979,0.660000,0.228800,0.000000,17,1 8 | 1404733433732800000,6,6,0.088000,0.300000,438.8493347168,130.2415466309,0.660000,0.316800,0.000000,17,2 9 | 1404733433732800000,6,6,0.088000,0.300000,430.6072692871,152.3499145508,0.572000,0.316800,0.000000,17,3 10 | 1404733433732800000,6,6,0.088000,0.300000,371.8628845215,197.8966827393,0.343200,0.457600,0.000000,27,0 11 | 1404733433732800000,6,6,0.088000,0.300000,379.8009338379,176.2885894775,0.431200,0.457600,0.000000,27,1 12 | 1404733433732800000,6,6,0.088000,0.300000,357.6223754883,168.4713134766,0.431200,0.545600,0.000000,27,2 13 | 1404733433732800000,6,6,0.088000,0.300000,349.9998168945,190.1964263916,0.343200,0.545600,0.000000,27,3 14 | 1404733433732800000,6,6,0.088000,0.300000,392.9413757324,138.8486022949,0.572000,0.457600,0.000000,29,0 15 | 1404733433732800000,6,6,0.088000,0.300000,401.2134399414,116.7006454468,0.660000,0.457600,0.000000,29,1 16 | 1404733433732800000,6,6,0.088000,0.300000,378.5355834961,108.5740890503,0.660000,0.545600,0.000000,29,2 17 | 1404733433732800000,6,6,0.088000,0.300000,370.2167968750,131.3691558838,0.572000,0.545600,0.000000,29,3 18 | 1404733433732800000,6,6,0.088000,0.300000,352.3925170898,158.4467773438,0.457600,0.572000,0.000000,34,0 19 | 1404733433732800000,6,6,0.088000,0.300000,360.6612854004,135.7068939209,0.545600,0.572000,0.000000,34,1 20 | 1404733433732800000,6,6,0.088000,0.300000,337.6237792969,128.2321624756,0.545600,0.660000,0.000000,34,2 21 | 1404733433732800000,6,6,0.088000,0.300000,330.2372131348,150.7954864502,0.457600,0.660000,0.000000,34,3 22 | 1404733433732800000,6,6,0.088000,0.300000,363.1156921387,128.7519836426,0.572000,0.572000,0.000000,35,0 23 | 1404733433732800000,6,6,0.088000,0.300000,370.8316040039,106.1561737061,0.660000,0.572000,0.000000,35,1 24 | 1404733433732800000,6,6,0.088000,0.300000,348.0792541504,98.3038787842,0.660000,0.660000,0.000000,35,2 25 | 1404733433732800000,6,6,0.088000,0.300000,339.9699096680,121.1954956055,0.572000,0.660000,0.000000,35,3 26 | -------------------------------------------------------------------------------- /yac_ros/launch/intel_d435i-calib_imucam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 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 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /yac_ros/src/ros_calib.hpp: -------------------------------------------------------------------------------- 1 | #ifndef YAC_ROS_CALIB_HPP 2 | #define YAC_ROS_CALIB_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "yac/yac.hpp" 13 | #include "ros_utils.hpp" 14 | 15 | namespace yac { 16 | 17 | void draw_hcentered_text(const std::string &text, 18 | const float text_scale, 19 | const int text_thickness, 20 | const int text_ypos, 21 | cv::Mat &image); 22 | void draw_camera_index(const int cam_idx, cv::Mat &image); 23 | void draw_nbv_reproj_error(const real_t nbv_reproj_error, cv::Mat &image); 24 | void draw_status_text(const std::string &text, cv::Mat &image); 25 | void draw_detected(const aprilgrid_t &grid, cv::Mat &image); 26 | void draw_nbv(const mat4_t &T_FC0, cv::Mat &image); 27 | bool tf_ok(const mat4_t &pose); 28 | void update_aprilgrid_model(const ros::Time &ts, 29 | const calib_target_t &target, 30 | ros::Publisher &rviz_pub, 31 | bool remove = false); 32 | 33 | void publish_fiducial_tf(const ros::Time &ts, 34 | const calib_target_t &target, 35 | const mat4_t &T_WF, 36 | tf2_ros::TransformBroadcaster &tf_br, 37 | ros::Publisher rviz_pub, 38 | bool remove = false); 39 | void publish_tf(const ros::Time &ts, 40 | const std::string &pose_name, 41 | const mat4_t &pose, 42 | tf2_ros::TransformBroadcaster &tf_br); 43 | 44 | /* ROS Calibration Configuration */ 45 | struct ros_config_t { 46 | ros::NodeHandle *ros_nh = nullptr; 47 | std::string node_name; 48 | 49 | bool publish_tfs = true; 50 | std::string image_format = "bgr8"; 51 | std::string calib_file; 52 | std::string cam0_topic; 53 | std::string cam1_topic; 54 | std::string imu0_topic; 55 | 56 | ros_config_t(int argc, char **argv) { 57 | node_name = yac::ros_node_name(argc, argv); 58 | if (ros::isInitialized() == false) { 59 | ros::init(argc, argv, node_name, ros::init_options::NoSigintHandler); 60 | } 61 | ros_nh = new ros::NodeHandle(); 62 | 63 | ROS_PTR_PARAM(ros_nh, node_name + "/publish_tfs", publish_tfs); 64 | ROS_PTR_PARAM(ros_nh, node_name + "/image_format", image_format); 65 | ROS_PTR_PARAM(ros_nh, node_name + "/calib_file", calib_file); 66 | ROS_PTR_PARAM(ros_nh, node_name + "/cam0_topic", cam0_topic); 67 | ROS_PTR_PARAM(ros_nh, node_name + "/cam1_topic", cam1_topic); 68 | ROS_PTR_OPTIONAL_PARAM(ros_nh, node_name + "/imu0_topic", imu0_topic, ""); 69 | } 70 | 71 | ~ros_config_t() { 72 | if (ros_nh) { 73 | delete ros_nh; 74 | } 75 | } 76 | }; 77 | 78 | } // namespace yac 79 | #endif // YAC_ROS_CALIB_HPP 80 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733463482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463482800128,6,6,0.088000,0.300000,36.2278099060,39.9757461548,0.228800,0.457600,0.000000,26,0 3 | 1404733463482800128,6,6,0.088000,0.300000,22.7158527374,77.1671218872,0.316800,0.457600,0.000000,26,1 4 | 1404733463482800128,6,6,0.088000,0.300000,51.7296028137,92.3378601074,0.316800,0.545600,0.000000,26,2 5 | 1404733463482800128,6,6,0.088000,0.300000,65.8984527588,54.5048141479,0.228800,0.545600,0.000000,26,3 6 | 1404733463482800128,6,6,0.088000,0.300000,19.0907802582,88.4703903198,0.343200,0.457600,0.000000,27,0 7 | 1404733463482800128,6,6,0.088000,0.300000,8.4447164536,123.9717636108,0.431200,0.457600,0.000000,27,1 8 | 1404733463482800128,6,6,0.088000,0.300000,36.5204124451,139.3751373291,0.431200,0.545600,0.000000,27,2 9 | 1404733463482800128,6,6,0.088000,0.300000,47.8946266174,103.4605865479,0.343200,0.545600,0.000000,27,3 10 | 1404733463482800128,6,6,0.088000,0.300000,97.5345153809,8.8482885361,0.114400,0.572000,0.000000,31,0 11 | 1404733463482800128,6,6,0.088000,0.300000,80.4228439331,47.4185523987,0.202400,0.572000,0.000000,31,1 12 | 1404733463482800128,6,6,0.088000,0.300000,112.8742141724,63.7920989990,0.202400,0.660000,0.000000,31,2 13 | 1404733463482800128,6,6,0.088000,0.300000,130.8593597412,24.6915321350,0.114400,0.660000,0.000000,31,3 14 | 1404733463482800128,6,6,0.088000,0.300000,75.5402069092,59.4117088318,0.228800,0.572000,0.000000,32,0 15 | 1404733463482800128,6,6,0.088000,0.300000,61.2038230896,96.8798751831,0.316800,0.572000,0.000000,32,1 16 | 1404733463482800128,6,6,0.088000,0.300000,92.6289215088,113.3492050171,0.316800,0.660000,0.000000,32,2 17 | 1404733463482800128,6,6,0.088000,0.300000,107.8571548462,75.4619750977,0.228800,0.660000,0.000000,32,3 18 | 1404733463482800128,6,6,0.088000,0.300000,57.2137336731,108.3558349609,0.343200,0.572000,0.000000,33,0 19 | 1404733463482800128,6,6,0.088000,0.300000,45.5364036560,143.9964599609,0.431200,0.572000,0.000000,33,1 20 | 1404733463482800128,6,6,0.088000,0.300000,75.8528060913,160.3845520020,0.431200,0.660000,0.000000,33,2 21 | 1404733463482800128,6,6,0.088000,0.300000,88.5451889038,124.4508361816,0.343200,0.660000,0.000000,33,3 22 | 1404733463482800128,6,6,0.088000,0.300000,42.4102325439,154.7115478516,0.457600,0.572000,0.000000,34,0 23 | 1404733463482800128,6,6,0.088000,0.300000,33.2163047791,187.9986572266,0.545600,0.572000,0.000000,34,1 24 | 1404733463482800128,6,6,0.088000,0.300000,62.5244598389,203.9662170410,0.545600,0.660000,0.000000,34,2 25 | 1404733463482800128,6,6,0.088000,0.300000,72.6078338623,170.6326141357,0.457600,0.660000,0.000000,34,3 26 | 1404733463482800128,6,6,0.088000,0.300000,30.6804809570,197.7317199707,0.572000,0.572000,0.000000,35,0 27 | 1404733463482800128,6,6,0.088000,0.300000,23.7393341064,228.4247283936,0.660000,0.572000,0.000000,35,1 28 | 1404733463482800128,6,6,0.088000,0.300000,51.9382553101,243.9826965332,0.660000,0.660000,0.000000,35,2 29 | 1404733463482800128,6,6,0.088000,0.300000,59.8508949280,213.5200347900,0.572000,0.660000,0.000000,35,3 30 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733463482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733463482800128,6,6,0.088000,0.300000,61.1064186096,16.3782329559,0.228800,0.457600,0.000000,26,0 3 | 1404733463482800128,6,6,0.088000,0.300000,45.3126373291,55.2350578308,0.316800,0.457600,0.000000,26,1 4 | 1404733463482800128,6,6,0.088000,0.300000,77.4731979370,71.5330352783,0.316800,0.545600,0.000000,26,2 5 | 1404733463482800128,6,6,0.088000,0.300000,93.7756652832,32.1960525513,0.228800,0.545600,0.000000,26,3 6 | 1404733463482800128,6,6,0.088000,0.300000,40.8619766235,67.1135253906,0.343200,0.457600,0.000000,27,0 7 | 1404733463482800128,6,6,0.088000,0.300000,28.0708427429,104.4266052246,0.431200,0.457600,0.000000,27,1 8 | 1404733463482800128,6,6,0.088000,0.300000,59.0239219666,120.6677703857,0.431200,0.545600,0.000000,27,2 9 | 1404733463482800128,6,6,0.088000,0.300000,72.6946182251,83.0035324097,0.343200,0.545600,0.000000,27,3 10 | 1404733463482800128,6,6,0.088000,0.300000,24.6098995209,115.6812973022,0.457600,0.457600,0.000000,28,0 11 | 1404733463482800128,6,6,0.088000,0.300000,14.4397134781,150.6473541260,0.545600,0.457600,0.000000,28,1 12 | 1404733463482800128,6,6,0.088000,0.300000,44.3587379456,166.8159942627,0.545600,0.545600,0.000000,28,2 13 | 1404733463482800128,6,6,0.088000,0.300000,55.4122467041,131.5384826660,0.457600,0.545600,0.000000,28,3 14 | 1404733463482800128,6,6,0.088000,0.300000,104.5712127686,37.3701896667,0.228800,0.572000,0.000000,32,0 15 | 1404733463482800128,6,6,0.088000,0.300000,87.6496276855,76.3647384644,0.316800,0.572000,0.000000,32,1 16 | 1404733463482800128,6,6,0.088000,0.300000,122.2149505615,93.9371643066,0.316800,0.660000,0.000000,32,2 17 | 1404733463482800128,6,6,0.088000,0.300000,139.7944183350,54.6513214111,0.228800,0.660000,0.000000,32,3 18 | 1404733463482800128,6,6,0.088000,0.300000,82.9224929810,88.2819747925,0.343200,0.572000,0.000000,33,0 19 | 1404733463482800128,6,6,0.088000,0.300000,68.9283523560,125.5306930542,0.431200,0.572000,0.000000,33,1 20 | 1404733463482800128,6,6,0.088000,0.300000,102.2503128052,142.8522033691,0.431200,0.660000,0.000000,33,2 21 | 1404733463482800128,6,6,0.088000,0.300000,117.2786560059,105.4521179199,0.343200,0.660000,0.000000,33,3 22 | 1404733463482800128,6,6,0.088000,0.300000,65.1423263550,136.7394409180,0.457600,0.572000,0.000000,34,0 23 | 1404733463482800128,6,6,0.088000,0.300000,53.7429428101,171.6356506348,0.545600,0.572000,0.000000,34,1 24 | 1404733463482800128,6,6,0.088000,0.300000,85.7295303345,188.5379638672,0.545600,0.660000,0.000000,34,2 25 | 1404733463482800128,6,6,0.088000,0.300000,98.1839447021,153.6206054688,0.457600,0.660000,0.000000,34,3 26 | 1404733463482800128,6,6,0.088000,0.300000,50.6764030457,182.0544586182,0.572000,0.572000,0.000000,35,0 27 | 1404733463482800128,6,6,0.088000,0.300000,41.7650947571,214.0838623047,0.660000,0.572000,0.000000,35,1 28 | 1404733463482800128,6,6,0.088000,0.300000,72.5448989868,230.3401336670,0.660000,0.660000,0.000000,35,2 29 | 1404733463482800128,6,6,0.088000,0.300000,82.5115890503,198.4523773193,0.572000,0.660000,0.000000,35,3 30 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam1/1404733461482800128.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733461482800128,6,6,0.088000,0.300000,23.6739730835,5.7051100731,0.572000,0.228800,0.000000,17,0 3 | 1404733461482800128,6,6,0.088000,0.300000,4.0391469002,34.5009269714,0.660000,0.228800,0.000000,17,1 4 | 1404733461482800128,6,6,0.088000,0.300000,20.6206245422,58.8486518860,0.660000,0.316800,0.000000,17,2 5 | 1404733461482800128,6,6,0.088000,0.300000,41.3436203003,30.0574321747,0.572000,0.316800,0.000000,17,3 6 | 1404733461482800128,6,6,0.088000,0.300000,46.7201347351,38.3477973938,0.572000,0.343200,0.000000,23,0 7 | 1404733461482800128,6,6,0.088000,0.300000,26.2384300232,67.0289535522,0.660000,0.343200,0.000000,23,1 8 | 1404733461482800128,6,6,0.088000,0.300000,45.0777320862,93.3942413330,0.660000,0.431200,0.000000,23,2 9 | 1404733461482800128,6,6,0.088000,0.300000,66.5160064697,64.5686492920,0.572000,0.431200,0.000000,23,3 10 | 1404733461482800128,6,6,0.088000,0.300000,104.8983383179,34.2155456543,0.457600,0.457600,0.000000,28,0 11 | 1404733461482800128,6,6,0.088000,0.300000,79.8291244507,64.1544036865,0.545600,0.457600,0.000000,28,1 12 | 1404733461482800128,6,6,0.088000,0.300000,101.8160781860,92.6020431519,0.545600,0.545600,0.000000,28,2 13 | 1404733461482800128,6,6,0.088000,0.300000,127.8825378418,62.7801513672,0.457600,0.545600,0.000000,28,3 14 | 1404733461482800128,6,6,0.088000,0.300000,72.6311569214,73.4111022949,0.572000,0.457600,0.000000,29,0 15 | 1404733461482800128,6,6,0.088000,0.300000,51.2897453308,101.7912750244,0.660000,0.457600,0.000000,29,1 16 | 1404733461482800128,6,6,0.088000,0.300000,72.3410491943,129.7459259033,0.660000,0.545600,0.000000,29,2 17 | 1404733461482800128,6,6,0.088000,0.300000,94.6527252197,101.6043930054,0.572000,0.545600,0.000000,29,3 18 | 1404733461482800128,6,6,0.088000,0.300000,173.6129455566,32.3303756714,0.343200,0.572000,0.000000,33,0 19 | 1404733461482800128,6,6,0.088000,0.300000,143.7871704102,62.7611083984,0.431200,0.572000,0.000000,33,1 20 | 1404733461482800128,6,6,0.088000,0.300000,168.9411621094,93.8589859009,0.431200,0.660000,0.000000,33,2 21 | 1404733461482800128,6,6,0.088000,0.300000,199.6735229492,63.4858474731,0.343200,0.660000,0.000000,33,3 22 | 1404733461482800128,6,6,0.088000,0.300000,135.2534942627,72.3147430420,0.457600,0.572000,0.000000,34,0 23 | 1404733461482800128,6,6,0.088000,0.300000,109.1248931885,101.8580017090,0.545600,0.572000,0.000000,34,1 24 | 1404733461482800128,6,6,0.088000,0.300000,133.2854919434,132.1456756592,0.545600,0.660000,0.000000,34,2 25 | 1404733461482800128,6,6,0.088000,0.300000,160.4259643555,103.1101455688,0.457600,0.660000,0.000000,34,3 26 | 1404733461482800128,6,6,0.088000,0.300000,101.5335083008,110.8257827759,0.572000,0.572000,0.000000,35,0 27 | 1404733461482800128,6,6,0.088000,0.300000,79.1499862671,138.6365203857,0.660000,0.572000,0.000000,35,1 28 | 1404733461482800128,6,6,0.088000,0.300000,102.2906494141,167.8277435303,0.660000,0.660000,0.000000,35,2 29 | 1404733461482800128,6,6,0.088000,0.300000,125.6738967896,140.7117004395,0.572000,0.660000,0.000000,35,3 30 | -------------------------------------------------------------------------------- /yac/tests/test_data/calib/imu_april/cam0/1404733461432800000.csv: -------------------------------------------------------------------------------- 1 | #ts,tag_rows,tag_cols,tag_size,tag_spacing,kp_x,kp_y,p_x,p_y,p_z,tag_id,corner_idx 2 | 1404733461432800000,6,6,0.088000,0.300000,68.1369934082,4.6094269753,0.572000,0.228800,0.000000,17,0 3 | 1404733461432800000,6,6,0.088000,0.300000,45.6418380737,34.1912460327,0.660000,0.228800,0.000000,17,1 4 | 1404733461432800000,6,6,0.088000,0.300000,64.9295806885,59.5854187012,0.660000,0.316800,0.000000,17,2 5 | 1404733461432800000,6,6,0.088000,0.300000,88.4672241211,29.7539825439,0.572000,0.316800,0.000000,17,3 6 | 1404733461432800000,6,6,0.088000,0.300000,94.7377319336,38.5490722656,0.572000,0.343200,0.000000,23,0 7 | 1404733461432800000,6,6,0.088000,0.300000,71.3235626221,68.0496597290,0.660000,0.343200,0.000000,23,1 8 | 1404733461432800000,6,6,0.088000,0.300000,92.8860244751,95.6307907104,0.660000,0.431200,0.000000,23,2 9 | 1404733461432800000,6,6,0.088000,0.300000,117.4197082520,65.9666442871,0.572000,0.431200,0.000000,23,3 10 | 1404733461432800000,6,6,0.088000,0.300000,160.7344512939,35.2516250610,0.457600,0.457600,0.000000,28,0 11 | 1404733461432800000,6,6,0.088000,0.300000,132.6100158691,65.6187744141,0.545600,0.457600,0.000000,28,1 12 | 1404733461432800000,6,6,0.088000,0.300000,157.5055999756,95.6906585693,0.545600,0.545600,0.000000,28,2 13 | 1404733461432800000,6,6,0.088000,0.300000,186.6511535645,65.2892532349,0.457600,0.545600,0.000000,28,3 14 | 1404733461432800000,6,6,0.088000,0.300000,124.5158309937,75.2836837769,0.572000,0.457600,0.000000,29,0 15 | 1404733461432800000,6,6,0.088000,0.300000,100.0295410156,104.4801406860,0.660000,0.457600,0.000000,29,1 16 | 1404733461432800000,6,6,0.088000,0.300000,123.6679077148,133.7588043213,0.660000,0.545600,0.000000,29,2 17 | 1404733461432800000,6,6,0.088000,0.300000,149.3914031982,104.7475891113,0.572000,0.545600,0.000000,29,3 18 | 1404733461432800000,6,6,0.088000,0.300000,237.1846618652,34.7895889282,0.343200,0.572000,0.000000,33,0 19 | 1404733461432800000,6,6,0.088000,0.300000,204.5024108887,65.5293197632,0.431200,0.572000,0.000000,33,1 20 | 1404733461432800000,6,6,0.088000,0.300000,232.4152069092,98.2590103149,0.431200,0.660000,0.000000,33,2 21 | 1404733461432800000,6,6,0.088000,0.300000,265.7654724121,67.5151596069,0.343200,0.660000,0.000000,33,3 22 | 1404733461432800000,6,6,0.088000,0.300000,194.9212188721,75.1744461060,0.457600,0.572000,0.000000,34,0 23 | 1404733461432800000,6,6,0.088000,0.300000,165.5382537842,105.3185806274,0.545600,0.572000,0.000000,34,1 24 | 1404733461432800000,6,6,0.088000,0.300000,192.4157562256,136.7721405029,0.545600,0.660000,0.000000,34,2 25 | 1404733461432800000,6,6,0.088000,0.300000,222.6617279053,107.3571701050,0.457600,0.660000,0.000000,34,3 26 | 1404733461432800000,6,6,0.088000,0.300000,157.1547851562,114.3917160034,0.572000,0.572000,0.000000,35,0 27 | 1404733461432800000,6,6,0.088000,0.300000,131.3263397217,142.9844970703,0.660000,0.572000,0.000000,35,1 28 | 1404733461432800000,6,6,0.088000,0.300000,157.1488494873,173.5774230957,0.660000,0.660000,0.000000,35,2 29 | 1404733461432800000,6,6,0.088000,0.300000,183.7749786377,145.6189575195,0.572000,0.660000,0.000000,35,3 30 | -------------------------------------------------------------------------------- /scripts/plot_info.py: -------------------------------------------------------------------------------- 1 | """ Plot INFO """ 2 | import pandas 3 | import numpy as np 4 | from numpy.linalg import inv 5 | import matplotlib.pylab as plt 6 | from mpl_toolkits import mplot3d 7 | 8 | all_views_csv = "/data/euroc_results/experiments/kalibr_data/data/timestamps.csv" 9 | kalibr_views_csv = "/data/euroc_results/experiments/kalibr_data/data/views.csv" 10 | kalibr_info_csv = "/data/euroc_results/experiments/kalibr_data/data/info.csv" 11 | yac_views_csv = "/tmp/yac_views.csv" 12 | yac_info_csv = "/tmp/yac_info.csv" 13 | 14 | # Load all views 15 | all_views = {} 16 | for idx, row in pandas.read_csv(all_views_csv, header=None).iterrows(): 17 | ts = int(row[0] * 1e6) 18 | all_views[ts] = idx 19 | 20 | # Load kalibr views 21 | kalibr_info = [] 22 | kalibr_info_gain = [] 23 | for idx, row in pandas.read_csv(kalibr_info_csv, header=None).iterrows(): 24 | ts = int(row[0] * 1e6) 25 | info = row[2] 26 | info_gain = row[3] 27 | kalibr_info.append(-1.0 * info) 28 | kalibr_info_gain.append(info_gain) 29 | 30 | # Load Kalibr info 31 | kalibr_views = [] 32 | for idx, row in pandas.read_csv(kalibr_views_csv, header=None).iterrows(): 33 | ts = int(row[0] * 1e6) 34 | kalibr_views.append(all_views[ts]) 35 | 36 | # Load YAC views 37 | yac_views = [] 38 | for idx, row in pandas.read_csv(yac_views_csv, header=None).iterrows(): 39 | ts = row[0] 40 | yac_views.append(all_views[ts]) 41 | 42 | # Load YAC info 43 | yac_info = [] 44 | yac_info_gain = [] 45 | for idx, row in pandas.read_csv(yac_info_csv, header=None).iterrows(): 46 | ts = row[0] 47 | info = row[1] 48 | info_gain = row[3] 49 | yac_info.append(info) 50 | yac_info_gain.append(info_gain) 51 | 52 | # Plot NBVs 53 | lo = [0, 1.0] 54 | c = ["red", "blue"] 55 | plt.figure() 56 | ax = plt.subplot(111) 57 | ax.eventplot([kalibr_views, yac_views], lineoffsets=lo, color=c) 58 | ax.set_xlabel("View Index") 59 | ax.set_yticks(lo) 60 | ax.set_yticklabels(["Kalibr", "YAC"]) 61 | 62 | # Plot information gain over time 63 | plt.figure() 64 | ax = plt.subplot(111) 65 | ax.plot(yac_info, label="YAC") 66 | ax.plot(kalibr_info, label="Kalibr") 67 | ax.set_xlabel("View Index") 68 | ax.legend(loc=0) 69 | ax.set_title("sum(log(S)) / log(2)") 70 | plt.show() 71 | 72 | # from numpy import genfromtxt 73 | # H_marg = genfromtxt('/tmp/H_marg.csv', delimiter=',') 74 | # 75 | # # Decompose H_marg into JtJ 76 | # w, V = np.linalg.eig(H_marg) 77 | # for idx, w_i in enumerate(w): 78 | # if w_i < 1e-12: 79 | # w[idx] = 0.0 80 | # S_sqrt = np.diag(w**0.5) 81 | # J = S_sqrt @ V.T 82 | # 83 | # # SVD H 84 | # u, s, vh = np.linalg.svd(H_marg) 85 | # 86 | # plt.figure() 87 | # plt.imshow(u - V) 88 | # plt.colorbar() 89 | # 90 | # print(np.log(w).sum()) 91 | # print(np.log(s).sum()) 92 | # 93 | # plt.figure() 94 | # plt.imshow(vh.T - V) 95 | # plt.colorbar() 96 | # 97 | # # plt.figure() 98 | # # plt.imshow((u - vh.T)) 99 | # # plt.colorbar() 100 | # 101 | # plt.show() 102 | 103 | # covar = np.linalg.inv(H_marg) 104 | # w, V = np.linalg.eig(H_marg) 105 | 106 | # print(f"S.log().sum(): {-1.0 * np.sum(np.log(w))}") 107 | 108 | # print(f"det(covar): {np.linalg.det(covar)}") 109 | # print(f"S.log().sum(): {-1.0 * np.sum(np.log(s))}") 110 | 111 | # plt.imshow((J.T @ J) - H_marg) 112 | # plt.colorbar() 113 | # plt.show() 114 | --------------------------------------------------------------------------------