├── .gitignore ├── LICENSE ├── README.md ├── dependencies.yaml ├── doc ├── benchmarking.md ├── bib │ ├── Cioffi20iros.bib │ ├── Forster14icra.bib │ ├── Forster17tro.bib │ ├── Galvez12tro.bib │ ├── Kaess12ijrr.bib │ ├── Kuo20icra.bib │ ├── Leutenegger15ijrr.bib │ ├── Muglikar20icra.bib │ ├── Zhang16icra.bib │ └── Zhang17icra.bib ├── calibration.md ├── frontend │ ├── frontend_fla.md │ └── visual_frontend.md ├── global_map.md ├── images │ ├── mh03_gm.gif │ ├── v102_gm.gif │ └── v202_mono_vio.gif ├── known_issues_and_improvements.md └── vio.md ├── rpg_common ├── CMakeLists.txt ├── include │ └── rpg_common │ │ ├── aligned.h │ │ ├── batch_worker.h │ │ ├── callback_host.h │ │ ├── eigen_hash.h │ │ ├── eigen_type.h │ │ ├── fs.h │ │ ├── pose.h │ │ ├── realtime_worker.h │ │ ├── threadsafe_queue.h │ │ ├── timer.h │ │ └── worker_base.h ├── package.xml └── src │ └── fs.cpp ├── rqt_svo ├── CMakeLists.txt ├── README.md ├── package.xml ├── plugin.xml ├── resource │ └── widget.ui ├── scripts │ └── rqt_svo ├── setup.py └── src │ └── rqt_svo │ ├── .gitignore │ ├── __init__.py │ ├── svo.py │ └── svo_widget.py ├── svo ├── .gitignore ├── CMakeLists.txt ├── doc │ └── .gitignore ├── include │ └── svo │ │ ├── abstract_bundle_adjustment.h │ │ ├── frame_handler_array.h │ │ ├── frame_handler_base.h │ │ ├── frame_handler_mono.h │ │ ├── frame_handler_stereo.h │ │ ├── global.h │ │ ├── imu_handler.h │ │ ├── initialization.h │ │ ├── io.h │ │ ├── map.h │ │ ├── pose_optimizer.h │ │ ├── reprojector.h │ │ ├── stereo_triangulation.h │ │ └── svo.h ├── package.xml ├── src │ ├── frame_handler_array.cpp │ ├── frame_handler_base.cpp │ ├── frame_handler_mono.cpp │ ├── frame_handler_stereo.cpp │ ├── imu_handler.cpp │ ├── initialization.cpp │ ├── io.cpp │ ├── map.cpp │ ├── pose_optimizer.cpp │ ├── reprojector.cpp │ └── stereo_triangulation.cpp ├── test │ └── test_frame.cpp └── trace │ └── .gitignore ├── svo_benchmarking ├── CMakeLists.txt ├── README.md ├── analzye_traj_cfg │ ├── exp_euroc_vio.yaml │ ├── exp_euroc_vio_lc.yaml │ └── exp_gm_euroc_all.yaml ├── data │ └── .gitignore ├── dataset_tools │ ├── .gitignore │ ├── README.md │ ├── generate_ground_truth.sh │ ├── handeye_tfs │ │ ├── handeye_davis.yaml │ │ ├── handeye_visensor.yaml │ │ └── handeye_visensor_1903.yaml │ ├── prepare_dataset_monocular.sh │ ├── prepare_dataset_stereo.sh │ └── python │ │ ├── asl_groundtruth_to_pose.py │ │ ├── associate_timestamps.py │ │ ├── bag_to_image.py │ │ ├── bag_to_imu.py │ │ ├── bag_to_pose.py │ │ ├── change_image_path.py │ │ ├── combine_images.py │ │ ├── prepend_id.py │ │ ├── restamp_bag.py │ │ ├── strip_gt_id.py │ │ ├── swap_stamped_imu_meas.py │ │ └── transform_trajectory.py ├── evaluation_tools.md ├── experiments │ ├── exp_euroc_lc.yaml │ ├── exp_euroc_lc_stereo.yaml │ ├── exp_euroc_nolc.yaml │ ├── exp_euroc_nolc_stereo.yaml │ └── exp_global_map.yaml ├── package.xml ├── results │ └── .gitignore ├── scripts │ ├── analyze_all_euroc.sh │ ├── analyze_imu_integration_dynamic.py │ ├── analyze_imu_integration_static.py │ ├── analyze_imu_properties_static.py │ ├── analyze_lc_timing.py │ ├── analyze_vio_timing.py │ ├── bag_eval_scripts │ │ ├── analyze_stationary.py │ │ └── cal_traj_len.py │ ├── benchmark.py │ ├── compute_hand_eye_transformation.py │ ├── convert_old_fmt.py │ ├── evaluate.py │ ├── evaluate_all.py │ ├── organize_results.py │ ├── organize_results_mul.py │ └── ros_node.py ├── setup.py └── src │ └── svo_ceres_benchmarking │ ├── __init__.py │ ├── dataset_loading.py │ ├── hand_eye_calib.py │ ├── results_loading.py │ ├── tfs_utils.py │ ├── tracefile_reader.py │ ├── trajectory_loading.py │ ├── trajectory_plotting.py │ ├── trajectory_utils.py │ └── transformations.py ├── svo_ceres_backend ├── CMakeLists.txt ├── include │ └── svo │ │ ├── ceres_backend │ │ ├── ceres_iteration_callback.hpp │ │ ├── epipolar_error.hpp │ │ ├── error_interface.hpp │ │ ├── estimator.hpp │ │ ├── estimator_impl.hpp │ │ ├── estimator_types.hpp │ │ ├── homogeneous_point_error.hpp │ │ ├── homogeneous_point_local_parameterization.hpp │ │ ├── homogeneous_point_parameter_block.hpp │ │ ├── imu_error.hpp │ │ ├── local_parameterization_additional_interfaces.hpp │ │ ├── map.hpp │ │ ├── map_alignment.hpp │ │ ├── marginalization_error.hpp │ │ ├── marginalization_error_impl.hpp │ │ ├── parameter_block.hpp │ │ ├── pose_error.hpp │ │ ├── pose_local_parameterization.hpp │ │ ├── pose_parameter_block.hpp │ │ ├── relative_pose_error.hpp │ │ ├── reprojection_error.hpp │ │ ├── reprojection_error_base.hpp │ │ ├── reprojection_error_impl.hpp │ │ ├── reprojection_error_simple.hpp │ │ ├── speed_and_bias_error.hpp │ │ └── speed_and_bias_parameter_block.hpp │ │ ├── ceres_backend_interface.hpp │ │ ├── ceres_backend_publisher.hpp │ │ ├── motion_detector.hpp │ │ └── outlier_rejection.hpp ├── package.xml ├── src │ ├── ceres_backend_interface.cpp │ ├── ceres_backend_publisher.cpp │ ├── error_interface.cpp │ ├── estimator.cpp │ ├── homogeneous_point_error.cpp │ ├── homogeneous_point_local_parameterization.cpp │ ├── homogeneous_point_parameter_block.cpp │ ├── imu_error.cpp │ ├── local_parameterization_additional_interfaces.cpp │ ├── map.cpp │ ├── map_alignment.cpp │ ├── marginalization_error.cpp │ ├── motion_detector.cpp │ ├── outlier_rejection.cpp │ ├── pose_error.cpp │ ├── pose_local_parameterization.cpp │ ├── pose_parameter_block.cpp │ ├── relative_pose_error.cpp │ ├── speed_and_bias_error.cpp │ └── speed_and_bias_parameter_block.cpp └── test │ ├── test_backend_id.cpp │ ├── test_estimator.cpp │ ├── test_homogeneous_point_error.cpp │ ├── test_imu_error.cpp │ ├── test_map.cpp │ ├── test_marginalization.cpp │ ├── test_pose_error.cpp │ ├── test_pose_local_parameterization.cpp │ ├── test_relative_pose_error.cpp │ ├── test_reprojection_error.cpp │ └── test_speed_and_bias_error.cpp ├── svo_cmake ├── CMakeLists.txt ├── cmake │ ├── Modules │ │ ├── Findyaml-cpp.cmake │ │ └── SvoSetup.cmake │ ├── svo_cmake-extras.cmake.develspace.in │ └── svo_cmake-extras.cmake.installspace.in └── package.xml ├── svo_common ├── CMakeLists.txt ├── include │ └── svo │ │ └── common │ │ ├── camera.h │ │ ├── camera_fwd.h │ │ ├── container_helpers.h │ │ ├── conversions.h │ │ ├── feature_wrapper.h │ │ ├── frame.h │ │ ├── imu_calibration.h │ │ ├── logging.h │ │ ├── occupancy_grid_2d.h │ │ ├── point.h │ │ ├── seed.h │ │ ├── transformation.h │ │ └── types.h ├── package.xml ├── src │ ├── empty.cpp │ ├── frame.cpp │ ├── point.cpp │ └── test │ │ └── synthetic_dataset.cpp └── test │ ├── test_container_helpers.cpp │ ├── test_feature_wrapper.cpp │ └── test_main.cpp ├── svo_direct ├── CMakeLists.txt ├── include │ └── svo │ │ └── direct │ │ ├── depth_estimation.h │ │ ├── depth_filter.h │ │ ├── elder_zucker.h │ │ ├── feature_alignment.h │ │ ├── feature_detection.h │ │ ├── feature_detection_types.h │ │ ├── feature_detection_utils.h │ │ ├── matcher.h │ │ ├── patch_score.h │ │ ├── patch_utils.h │ │ └── patch_warp.h ├── package.xml ├── src │ ├── depth_estimation.cpp │ ├── depth_filter.cpp │ ├── elder_zucker.cpp │ ├── feature_alignment.cpp │ ├── feature_detection.cpp │ ├── feature_detection_utils.cpp │ ├── matcher.cpp │ └── patch_warp.cpp └── test │ ├── test_depth_estimation.cpp │ ├── test_edgelet_direction.cpp │ ├── test_edgelet_extraction.cpp │ ├── test_feature_alignment.cpp │ ├── test_feature_detection.cpp │ ├── test_patch_score.cpp │ └── test_utils.h ├── svo_global_map ├── CATKIN_IGNORE ├── CMakeLists.txt ├── include │ └── svo │ │ ├── global_map.h │ │ ├── gtsam │ │ ├── camera_bearing_extrinsics_factor.h │ │ ├── camera_bearing_factor.h │ │ ├── graph_manager.h │ │ ├── gtsam_optimizer.h │ │ └── smart_factors_fwd.h │ │ └── null_stream.h ├── package.xml └── src │ ├── global_map.cpp │ ├── gtsam │ ├── graph_manager.cpp │ └── gtsam_optimizer.cpp │ └── null_stream.cpp ├── svo_img_align ├── CMakeLists.txt ├── include │ └── svo │ │ └── img_align │ │ ├── frame_gpu.h │ │ ├── gpu_types_cuda.h │ │ ├── sparse_img_align.h │ │ ├── sparse_img_align_base.h │ │ ├── sparse_img_align_device_utils.cuh │ │ └── sparse_img_align_gpu.h ├── package.xml └── src │ ├── frame_gpu.cpp │ ├── sparse_img_align.cpp │ ├── sparse_img_align_base.cpp │ ├── sparse_img_align_device_utils.cu │ └── sparse_img_align_gpu.cpp ├── svo_msgs ├── CMakeLists.txt ├── msg │ ├── DenseInput.msg │ ├── DenseInputWithFeatures.msg │ ├── Feature.msg │ ├── Info.msg │ └── NbvTrajectory.msg └── package.xml ├── svo_online_loopclosing ├── CMakeLists.txt ├── include │ └── svo │ │ └── online_loopclosing │ │ ├── bow.h │ │ ├── ceres │ │ ├── epipolar_error.h │ │ └── reprojection_error_simple.h │ │ ├── geometric_verification.h │ │ ├── keyframe.h │ │ ├── loop_closing.h │ │ ├── loop_closing_types.h │ │ ├── map_alignment.h │ │ └── read_file.h ├── package.xml ├── src │ ├── bow.cpp │ ├── geometric_verification.cpp │ ├── loop_closing.cpp │ ├── map_alignment.cpp │ └── read_file.cpp ├── test │ └── createvoc.cpp └── vocabularies │ ├── .gitignore │ └── download_voc.sh ├── svo_pgo ├── CMakeLists.txt ├── include │ └── svo │ │ └── pgo │ │ ├── ceres │ │ ├── pose_graph_3d_error_term.h │ │ └── types.h │ │ └── pgo.h ├── package.xml ├── src │ └── pgo.cpp └── test │ ├── plot_results.py │ └── test.cpp ├── svo_ros ├── CMakeLists.txt ├── include │ └── svo_ros │ │ ├── backend_factory.h │ │ ├── backend_visualizer.h │ │ ├── ceres_backend_factory.h │ │ ├── csv_dataset_reader.h │ │ ├── svo_factory.h │ │ ├── svo_interface.h │ │ ├── svo_node_base.h │ │ ├── svo_nodelet.h │ │ └── visualizer.h ├── launch │ ├── euroc_global_map_mono.launch │ ├── euroc_vio_mono.launch │ ├── euroc_vio_stereo.launch │ ├── frontend │ │ ├── euroc_mono_frontend_imu.launch │ │ ├── euroc_stereo_frontend_imu.launch │ │ ├── fla_stereo_imu.launch │ │ └── run_from_bag.launch │ └── live_nodelet.launch ├── package.xml ├── param │ ├── backend.yaml │ ├── calib │ │ ├── 25000826_fisheye_mask.png │ │ ├── bluefox_25000826_fisheye.yaml │ │ ├── davis_flyingroom.yaml │ │ ├── euroc_mono.yaml │ │ ├── euroc_stereo.yaml │ │ ├── fla_stereo_imu.yaml │ │ ├── minidavis_bias.yaml │ │ ├── svo_test_pinhole.yaml │ │ ├── visensor_flyingroom_mono.yaml │ │ └── visensor_flyingroom_stereo.yaml │ ├── davis.yaml │ ├── fisheye.yaml │ ├── frontend_imu │ │ ├── euroc_mono_imu.yaml │ │ ├── euroc_stereo_imu.yaml │ │ └── fla_stereo_imu.yaml │ ├── global_map.yaml │ ├── pinhole.yaml │ ├── vio_mono.yaml │ └── vio_stereo.yaml ├── rqt.perspective ├── rviz_config.rviz ├── rviz_config_gm.rviz ├── rviz_config_vio.rviz ├── scripts │ ├── kalibr_to_svo.py │ └── omni_matlab_to_rpg.py ├── src │ ├── backend_factory.cpp │ ├── backend_visualizer.cpp │ ├── benchmark_node.cpp │ ├── ceres_backend_factory.cpp │ ├── csv_dataset_reader.cpp │ ├── svo_factory.cpp │ ├── svo_interface.cpp │ ├── svo_node.cpp │ ├── svo_node_base.cpp │ ├── svo_nodelet.cpp │ └── visualizer.cpp └── svo_nodelet.xml ├── svo_test_utils ├── CMakeLists.txt ├── include │ └── svo │ │ └── test_utils │ │ ├── imu_simulation.h │ │ ├── point_cloud_generator.h │ │ ├── simple_raytracer.h │ │ ├── synthetic_dataset.h │ │ ├── test_utils.h │ │ └── trajectory_generator.h ├── package.xml └── src │ ├── imu_simulation.cpp │ ├── point_cloud_generator.cpp │ ├── simple_raytracer.cpp │ ├── synthetic_dataset.cpp │ ├── test_utils.cpp │ └── trajectory_generator.cpp ├── svo_tracker ├── CMakeLists.txt ├── include │ └── svo │ │ └── tracker │ │ ├── feature_tracker.h │ │ ├── feature_tracking_types.h │ │ ├── feature_tracking_utils.h │ │ └── feature_tracking_viz.h ├── package.xml └── src │ ├── feature_tracker.cpp │ ├── feature_tracking_types.cpp │ ├── feature_tracking_utils.cpp │ └── feature_tracking_viz.cpp ├── svo_vio_common ├── CMakeLists.txt ├── include │ └── svo │ │ └── vio_common │ │ ├── backend_types.hpp │ │ ├── logging.hpp │ │ ├── matrix.hpp │ │ ├── matrix_operations.hpp │ │ └── test_utils.hpp ├── package.xml └── src │ ├── test_utils.cpp │ └── vio_common.cpp └── vikit ├── LICENSE ├── README.md ├── vikit_cameras ├── CMakeLists.txt ├── include │ └── vikit │ │ ├── cameras.h │ │ └── cameras │ │ ├── atan_distortion.h │ │ ├── camera_factory.h │ │ ├── camera_geometry.h │ │ ├── camera_geometry_base.h │ │ ├── equidistant_distortion.h │ │ ├── equidistant_fisheye_geometry.h │ │ ├── equidistant_fisheye_projection.h │ │ ├── implementation │ │ ├── camera_geometry.hpp │ │ ├── camera_geometry_base.hpp │ │ └── pinhole_projection.hpp │ │ ├── ncamera.h │ │ ├── no_distortion.h │ │ ├── omni_geometry.h │ │ ├── omni_projection.h │ │ ├── pinhole_projection.h │ │ ├── radial_tangential_distortion.h │ │ └── yaml │ │ ├── camera-yaml-serialization.h │ │ └── ncamera-yaml-serialization.h ├── package.xml ├── src │ ├── camera_factory.cpp │ ├── camera_geometry_base.cpp │ ├── camera_yaml_serialization.cpp │ ├── equidistant_fisheye_geometry.cpp │ ├── equidistant_fisheye_projection.cpp │ ├── ncamera.cpp │ ├── ncamera_yaml_serialization.cpp │ ├── omni_geometry.cpp │ └── omni_projection.cpp └── test │ ├── data │ ├── calib_cam.yaml │ ├── calib_kitti.yaml │ ├── calib_omni.yaml │ ├── calib_pinhole_atan.yaml │ ├── calib_pinhole_equidistant.yaml │ ├── calib_pinhole_nodistortion.yaml │ └── calib_pinhole_radtan.yaml │ └── test_cameras.cpp ├── vikit_common ├── CMakeLists.txt ├── include │ ├── aslam │ │ └── common │ │ │ ├── entrypoint.h │ │ │ ├── macros.h │ │ │ ├── memory.h │ │ │ ├── numdiff-jacobian-tester.h │ │ │ ├── pose-types.h │ │ │ ├── yaml-serialization-eigen.h │ │ │ └── yaml-serialization.h │ └── vikit │ │ ├── backtrace.h │ │ ├── blender_utils.h │ │ ├── csv_utils.h │ │ ├── homography.h │ │ ├── homography_decomp.h │ │ ├── math_utils.h │ │ ├── path_utils.h │ │ ├── performance_monitor.h │ │ ├── ringbuffer.h │ │ ├── sample.h │ │ ├── timer.h │ │ ├── user_input_thread.h │ │ └── vision.h ├── package.xml ├── src │ ├── homography.cpp │ ├── math_utils.cpp │ ├── performance_monitor.cpp │ ├── sample.cpp │ ├── user_input_thread.cpp │ └── vision.cpp ├── test │ ├── test_camera.cpp │ ├── test_common.cpp │ ├── test_homography.cpp │ ├── test_math_utils.cpp │ ├── test_timer.cpp │ └── test_triangulation.cpp └── vikit_commonConfig.cmake.in ├── vikit_py ├── CMakeLists.txt ├── package.xml ├── setup.py └── src │ └── vikit_py │ ├── .gitignore │ ├── __init__.py │ ├── align_trajectory.py │ ├── associate_timestamps.py │ ├── cpu_info.py │ ├── depth_estimation.py │ ├── depthmap_utils.py │ ├── math_utils.py │ ├── pinhole_camera.py │ ├── plot_utils.py │ ├── ros_node.py │ ├── rotation_utils.py │ ├── test_transformations.py │ └── transformations.py ├── vikit_ros ├── CMakeLists.txt ├── include │ └── vikit │ │ ├── img_type_conversion.h │ │ ├── output_helper.h │ │ └── params_helper.h ├── package.xml └── src │ ├── img_type_conversion.cpp │ └── output_helper.cpp └── vikit_solver ├── CMakeLists.txt ├── include └── vikit │ └── solver │ ├── implementation │ └── mini_least_squares_solver.hpp │ ├── mini_least_squares_solver.h │ └── robust_cost.h ├── package.xml └── src └── robust_cost.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | -------------------------------------------------------------------------------- /dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | cmake_external_project_catkin: 7 | type: git 8 | url: git@github.com:zurich-eye/cmake_external_project_catkin.git 9 | version: master 10 | eigen_catkin: 11 | type: git 12 | url: git@github.com:ethz-asl/eigen_catkin.git 13 | version: master 14 | eigen_checks: 15 | type: git 16 | url: git@github.com:ethz-asl/eigen_checks.git 17 | version: master 18 | fast_neon: 19 | type: git 20 | url: git@github.com:uzh-rpg/fast_neon.git 21 | version: master 22 | gflags_catkin: 23 | type: git 24 | url: git@github.com:ethz-asl/gflags_catkin.git 25 | version: master 26 | glog_catkin: 27 | type: git 28 | url: git@github.com:ethz-asl/glog_catkin.git 29 | version: master 30 | minkindr: 31 | type: git 32 | url: git@github.com:ethz-asl/minkindr.git 33 | version: master 34 | opengv: 35 | type: git 36 | url: git@github.com:ethz-asl/opengv.git 37 | version: master 38 | minkindr_ros: 39 | type: git 40 | url: git@github.com:ethz-asl/minkindr_ros.git 41 | version: master 42 | ceres_catkin: 43 | type: git 44 | url: git@github.com:ethz-asl/ceres_catkin.git 45 | version: master 46 | dbow2_catkin: 47 | type: git 48 | url: git@github.com:uzh-rpg/dbow2_catkin.git 49 | version: master 50 | rpg_trajectory_evaluation: 51 | type: git 52 | url: git@github.com:uzh-rpg/rpg_trajectory_evaluation.git 53 | version: dev 54 | -------------------------------------------------------------------------------- /doc/benchmarking.md: -------------------------------------------------------------------------------- 1 | # Benchmarking 2 | 3 | We provide some infrastructure code in `svo_benchmarking` package to evaluate the pose estimation accuracy. 4 | 5 | The `svo_benchmarking` package allows to execute SVO (with/without VIO backend/global map) on multiple datasets at the same time. It also makes use of the [rpg_trajectory_evaluation]() package to evaluate the trajectory estimation accuracy conveniently. Please see the [README](../svo_benchmarking/README.md) in `svo_benchmarking` for details. 6 | 7 | -------------------------------------------------------------------------------- /doc/bib/Cioffi20iros.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Cioffi20iros, 2 | author = {Giovanni Cioffi, Davide Scaramuzza}, 3 | title = {Tightly-coupled Fusion of Global Positional Measurements in Optimization-based Visual-Inertial Odometry}, 4 | booktitle = {{IEEE} Int. Conf. Intel. Robots and Syst. (IROS)}, 5 | year = 2020 6 | } 7 | -------------------------------------------------------------------------------- /doc/bib/Forster14icra.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Forster14icra, 2 | author = {Christian Forster and Matia Pizzoli and Davide Scaramuzza}, 3 | title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry}, 4 | booktitle = {{IEEE} Int. Conf. Robot. Autom. (ICRA)}, 5 | year = 2014, 6 | pages = {15--22}, 7 | doi = {10.1109/ICRA.2014.6906584} 8 | } 9 | -------------------------------------------------------------------------------- /doc/bib/Forster17tro.bib: -------------------------------------------------------------------------------- 1 | @Article{Forster17troSVO, 2 | author = {Christian Forster and Zichao Zhang and Michael Gassner and 3 | Manuel Werlberger and Davide Scaramuzza}, 4 | title = {{SVO}: Semidirect Visual Odometry for Monocular and 5 | Multicamera Systems}, 6 | journal = {{IEEE} Trans. Robot.}, 7 | year = 2017, 8 | volume = 33, 9 | number = 2, 10 | pages = {249--265}, 11 | doi = {10.1109/TRO.2016.2623335} 12 | } 13 | -------------------------------------------------------------------------------- /doc/bib/Galvez12tro.bib: -------------------------------------------------------------------------------- 1 | @Article{Galvez12tro, 2 | author = {Dorian G\'alvez-L\'opez and J. D. Tard\'os}, 3 | title = {Bags of Binary Words for Fast Place Recognition in Image 4 | Sequences}, 5 | journal = {{IEEE} Trans. Robot.}, 6 | year = 2012, 7 | volume = 28, 8 | number = 5, 9 | pages = {1188--1197}, 10 | month = oct, 11 | doi = {10.1109/TRO.2012.2197158} 12 | } 13 | -------------------------------------------------------------------------------- /doc/bib/Kaess12ijrr.bib: -------------------------------------------------------------------------------- 1 | @Article{Kaess12ijrr, 2 | author = {M. Kaess and H. Johannsson and R. Roberts and V. Ila and J.J. 3 | Leonard and F. Dellaert}, 4 | title = {{iSAM2}: Incremental Smoothing and Mapping Using the {B}ayes 5 | Tree}, 6 | journal = {"Int. J. Robot. Research"}, 7 | year = 2012, 8 | volume = 31, 9 | number = 2, 10 | pages = {217--236}, 11 | month = feb 12 | } 13 | -------------------------------------------------------------------------------- /doc/bib/Kuo20icra.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Kuo20icra, 2 | author = {Juichung Kuo, Manasi Muglikar, Zichao Zhang, Davide Scaramuzza}, 3 | title = {Redesigning SLAM for Arbitrary Multi-Camera Systems}, 4 | booktitle = {{IEEE} Int. Conf. Robot. Autom. (ICRA)}, 5 | year = 2020 6 | } 7 | -------------------------------------------------------------------------------- /doc/bib/Leutenegger15ijrr.bib: -------------------------------------------------------------------------------- 1 | @Article{Leutenegger15ijrr, 2 | author = {S. Leutenegger and S. Lynen and M. Bosse and R. Siegwart and 3 | P. Furgale}, 4 | title = {Keyframe-Based Visual-Inertial {SLAM} using Nonlinear 5 | Optimization}, 6 | journal = {Int. J. Robot. Research}, 7 | year = 2015 8 | } 9 | -------------------------------------------------------------------------------- /doc/bib/Muglikar20icra.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Muglikar20icra, 2 | author = {Manasi Muglikar, Zichao Zhang, Davide Scaramuzza}, 3 | title = {Voxel Map for Visual SLAM}, 4 | booktitle = {{IEEE} Int. Conf. Robot. Autom. (ICRA)}, 5 | year = 2020 6 | } 7 | -------------------------------------------------------------------------------- /doc/bib/Zhang16icra.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Zhang16icra, 2 | author = {Zichao Zhang and Henri Rebecq and Christian Forster and 3 | Davide Scaramuzza}, 4 | title = {Benefit of Large Field-of-View Cameras for Visual Odometry}, 5 | booktitle = {{IEEE} Int. Conf. Robot. Autom. (ICRA)}, 6 | year = 2016 7 | } 8 | -------------------------------------------------------------------------------- /doc/bib/Zhang17icra.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Zhang17icra, 2 | author = {Zichao Zhang and Christian Forster and 3 | Davide Scaramuzza}, 4 | title = {Active Exposure Control for Robust Visual Odometry in HDR Environments}, 5 | booktitle = {{IEEE} Int. Conf. Robot. Autom. (ICRA)}, 6 | year = 2017 7 | } 8 | -------------------------------------------------------------------------------- /doc/images/mh03_gm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/doc/images/mh03_gm.gif -------------------------------------------------------------------------------- /doc/images/v102_gm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/doc/images/v102_gm.gif -------------------------------------------------------------------------------- /doc/images/v202_mono_vio.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/doc/images/v202_mono_vio.gif -------------------------------------------------------------------------------- /doc/known_issues_and_improvements.md: -------------------------------------------------------------------------------- 1 | # Known issues and possible improvements 2 | 3 | 1. The initialization of the visual-inertial odometry is currently done in a relatively naive manner: we simple wait for the backend to converge and then scale the map from the front-end accordingly. This mostly affects the stability of the monocular setup. More sophisticated methods, such as the ones used in VINS-Mono and ORB-SLAM3 could be adopted. 4 | 2. The global map at this moment takes relatively much memory, since no sparsification of the keyframes and landmarks are done. Some work could be put in this direction to make it more efficient. 5 | 3. The matching of the features in current frame to the global map is relatively fragile. The features are matched using direct matching individually. Selecting and matching multiple features simultaneously may improve the robustness. 6 | 4. In case of loop closing, we shift the sliding window and drop the marginalization term, as the marginalizaiton term depends on previous linearization points, which are not valid any more. This avoids inconsistent linearization points but makes the tracking after loop closing a bit unstable. Recalculating it from the global map, using relative formulation or adopting some approximation could be used to improve this. 7 | 5. Occasionally the pipeline with the global map may crash, some investigation needs to be done to improve the stability. 8 | 9 | -------------------------------------------------------------------------------- /doc/vio.md: -------------------------------------------------------------------------------- 1 | # Visual-inertial Odometry with SVO frontend 2 | 3 | The visual-inertial odometry consists of the SVO 2.0 frontend and OKVIS style sliding window backend. Both monocular and stereo setups are supprted. 4 | 5 | Loop closure and correction using DBoW2 is also supported, but we are relative conservative on regarding loop corrections (i.e., only very good match results after 2D-2D/2D-3D check + large pose correction will be accepted). In brief, we additionally extract ORB features for the purpose of loop closure, and the depths of these ORB features are estimated using depth filters. 6 | 7 | ## Examples on EuRoC 8 | Source the workspace first: 9 | ```sh 10 | source ~/svo_ws/devel/setup.bash 11 | ``` 12 | 13 | ## Monocular VIO 14 | ```sh 15 | # launch svo 16 | roslaunch svo_ros euroc_vio_mono.launch 17 | # run the EuRoC bag 18 | rosbag play V2_02_medium.bag -s 10 19 | ``` 20 | You might need to adjust a starting time to get the monocular pipeline started properly, as in the pure monocular case. You should be able to see a visualization like this: 21 | 22 | 23 | 24 | ![](./images/v202_mono_vio.gif) 25 | 26 | where the green points are the landmarks optimized by the backend, which are fed back to the frontend for tracking. The brighter the color, the newer the landmarks are. 27 | 28 | You will also see line segments that connecting the current frame and the past keyframes. Thoes indicates loop closures, where a red line indicates successful recognition and a green one indicates actual pose correction. 29 | 30 | ## Stereo VIO 31 | ```sh 32 | # launch svo 33 | roslaunch svo_ros euroc_vio_stereo.launch 34 | # run the EuRoC bag 35 | rosbag play V2_03_difficult.bag 36 | ``` 37 | You should be able to see similar visualization as in the monocular case. 38 | 39 | ## Parameters 40 | The above launch files use the parameters in `svo_ros/param/vio_mono` and `svo_ros/param/vio_stere`. They are essentially identical, except for the flat `pipeline_is_stereo` to indicate whether stereo or monocular setup should be used. 41 | -------------------------------------------------------------------------------- /rpg_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(rpg_common) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | find_package(OpenCV REQUIRED) 7 | 8 | add_definitions(-std=c++11) 9 | 10 | FILE(GLOB_RECURSE HEADERS "include/*") 11 | 12 | list(APPEND SOURCES src/fs.cpp) 13 | 14 | cs_add_library(${PROJECT_NAME} ${HEADERS} ${SOURCES}) 15 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) 16 | 17 | cs_install() 18 | cs_export() 19 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/batch_worker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "rpg_common/threadsafe_queue.h" 9 | #include "rpg_common/worker_base.h" 10 | 11 | namespace rpg_common { 12 | 13 | template 14 | class BatchWorker : public WorkerBase 15 | { 16 | public: 17 | BatchWorker() : paused_(true) 18 | { 19 | resume(); 20 | } 21 | 22 | virtual ~BatchWorker() 23 | { 24 | shutdown(); 25 | } 26 | 27 | void addTask(const DataType& item) 28 | { 29 | queue_.push(item); 30 | } 31 | 32 | virtual void shutdown() override 33 | { 34 | if (!thread_.joinable()) 35 | { 36 | return; 37 | } 38 | queue_.shutdown(); 39 | thread_.join(); 40 | } 41 | 42 | virtual void softShutdown() override 43 | { 44 | if (!thread_.joinable()) 45 | { 46 | return; 47 | } 48 | queue_.waitUntilEmpty(); 49 | queue_.shutdown(); 50 | thread_.join(); 51 | } 52 | 53 | virtual void pause() override 54 | { 55 | CHECK(!paused_); 56 | CHECK(thread_.joinable()); 57 | paused_ = true; 58 | thread_.join(); 59 | } 60 | 61 | virtual bool isPaused() const override 62 | { 63 | return paused_; 64 | } 65 | 66 | virtual void resume() override 67 | { 68 | CHECK(paused_); 69 | CHECK(!thread_.joinable()); 70 | paused_ = false; 71 | thread_ = std::thread(&BatchWorker::workLoop, this); 72 | } 73 | 74 | private: 75 | virtual void process(const std::vector& item) = 0; 76 | 77 | void workLoop() 78 | { 79 | std::vector items; 80 | while (!paused_ && queue_.waitAndPopAvailable(&items)) 81 | { 82 | process(items); 83 | } 84 | } 85 | 86 | ThreadSafeQueue queue_; 87 | std::atomic paused_; 88 | std::thread thread_; 89 | }; 90 | 91 | } // namespace rpg_common 92 | namespace rpg = rpg_common; 93 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/eigen_hash.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace std { 8 | 9 | template 10 | struct hash> 11 | { 12 | // https://wjngkoh.wordpress.com/2015/03/04/c-hash-function-for-eigen-matrix-and-vector/ 13 | size_t operator()(const Eigen::Matrix& matrix) const 14 | { 15 | size_t seed = 0; 16 | for (size_t i = 0; i < matrix.size(); ++i) 17 | { 18 | Scalar elem = *(matrix.data() + i); 19 | seed ^= 20 | std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); 21 | } 22 | return seed; 23 | } 24 | }; 25 | 26 | } // namespace std 27 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/fs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rpg_common 7 | { 8 | namespace fs 9 | { 10 | 11 | void concatenateFolderAndFileName(const std::string& folder, 12 | const std::string& file_name, 13 | std::string* path); 14 | 15 | std::string concatenateFolderAndFileName(const std::string& folder, 16 | const std::string& file_name); 17 | 18 | bool fileExists(const std::string& path); 19 | bool pathExists(const std::string& path); 20 | 21 | void splitPathAndFilename( 22 | const std::string& str, std::string* path, std::string* filename); 23 | 24 | // Returns full paths. 25 | void getFilesAndSubfolders(const std::string& path, 26 | std::vector* files, 27 | std::vector* folders); 28 | 29 | } // namespace fs 30 | } // namespace rpg_common 31 | 32 | namespace rpg = rpg_common; 33 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/pose.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "rpg_common/aligned.h" 8 | 9 | namespace rpg_common { 10 | 11 | typedef kindr::minimal::QuatTransformation Pose; 12 | typedef kindr::minimal::RotationQuaternion Rotation; 13 | typedef Eigen::Vector3d Position; 14 | 15 | using PoseVec = Aligned; 16 | using PositionVec = Aligned; 17 | using RotationVec = Aligned; 18 | 19 | namespace pose { 20 | 21 | inline Pose xRotationDeg(const double angle_deg) 22 | { 23 | const double c_d2 = cos(angle_deg * M_PI / 360.); 24 | const double s_d2 = sin(angle_deg * M_PI / 360.); 25 | const double w = c_d2; 26 | return Pose(Pose::Rotation(w, s_d2, 0., 0.), Eigen::Vector3d::Zero()); 27 | } 28 | inline Pose yRotationDeg(const double angle_deg) 29 | { 30 | const double c_d2 = cos(angle_deg * M_PI / 360.); 31 | const double s_d2 = sin(angle_deg * M_PI / 360.); 32 | const double w = c_d2; 33 | return Pose(Pose::Rotation(w, 0., s_d2, 0.), Eigen::Vector3d::Zero()); 34 | } 35 | inline Pose zRotationDeg(const double angle_deg) 36 | { 37 | const double c_d2 = cos(angle_deg * M_PI / 360.); 38 | const double s_d2 = sin(angle_deg * M_PI / 360.); 39 | const double w = c_d2; 40 | return Pose(Pose::Rotation(w, 0., 0., s_d2), Eigen::Vector3d::Zero()); 41 | } 42 | 43 | inline Pose yawPitchRollDeg( 44 | const double yaw, const double pitch, const double roll) 45 | { 46 | return zRotationDeg(yaw) * yRotationDeg(pitch) * xRotationDeg(roll); 47 | } 48 | 49 | } // namespace pose 50 | 51 | } // namespace rpg_common 52 | namespace rpg = rpg_common; 53 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/realtime_worker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "rpg_common/threadsafe_queue.h" 6 | 7 | namespace rpg_common { 8 | 9 | template 10 | class RealtimeWorker 11 | { 12 | public: 13 | RealtimeWorker() : thread_(&RealtimeWorker::workLoop, this) {} 14 | 15 | virtual ~RealtimeWorker() 16 | { 17 | shutdown(); 18 | } 19 | 20 | void addTask(const DataType& item) 21 | { 22 | queue_.push(item); 23 | } 24 | 25 | void shutdown() 26 | { 27 | if (!thread_.joinable()) 28 | { 29 | LOG(WARNING) << "Redundant shutdown call of real-time worker!"; 30 | return; 31 | } 32 | queue_.shutdown(); 33 | thread_.join(); 34 | } 35 | 36 | void printBacklogWarningsWithTag(const std::string& tag) 37 | { 38 | queue_.printBacklogWarningsWithTag( 39 | "Queue of worker with tag \"" + tag + "\""); 40 | } 41 | 42 | private: 43 | virtual void process(const DataType& item) = 0; 44 | 45 | void workLoop() 46 | { 47 | DataType item; 48 | while (queue_.skipToLatest(&item)) 49 | { 50 | process(item); 51 | } 52 | } 53 | 54 | ThreadSafeQueue queue_; 55 | std::thread thread_; 56 | }; 57 | 58 | } // namespace rpg_common 59 | namespace rpg = rpg_common; 60 | -------------------------------------------------------------------------------- /rpg_common/include/rpg_common/worker_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace rpg_common { 4 | 5 | // Template-free interface to worker features that do not depend on the queue 6 | // data type. 7 | class WorkerBase 8 | { 9 | public: 10 | virtual ~WorkerBase() = default; 11 | 12 | // Shuts down once the current loop is finished. 13 | virtual void shutdown() = 0; 14 | // Shuts down once the queue is empty. 15 | virtual void softShutdown() = 0; 16 | 17 | virtual void pause() = 0; 18 | virtual bool isPaused() const = 0; 19 | virtual void resume() = 0; 20 | }; 21 | 22 | } // namespace rpg_common 23 | namespace rpg = rpg_common; 24 | -------------------------------------------------------------------------------- /rpg_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpg_common 4 | 0.0.0 5 | 6 | Common C++11 code for RPG. 7 | 8 | 9 | Titus Cieslewski 10 | Titus Cieslewski 11 | Private 12 | 13 | catkin 14 | catkin_simple 15 | 16 | cv_bridge 17 | eigen_catkin 18 | eigen_checks 19 | gflags_catkin 20 | glog_catkin 21 | minkindr 22 | 23 | -------------------------------------------------------------------------------- /rqt_svo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rqt_svo) 3 | 4 | IF(NOT DEFINED ENV{ARM_ARCHITECTURE}) 5 | find_package(catkin REQUIRED COMPONENTS 6 | rospy 7 | rqt_gui 8 | rqt_gui_py 9 | ) 10 | catkin_package() 11 | catkin_python_setup() 12 | 13 | install(FILES plugin.xml 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | ) 16 | 17 | install(DIRECTORY resource 18 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 19 | ) 20 | 21 | install(PROGRAMS scripts/rqt_svo 22 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 23 | ) 24 | ENDIF() 25 | -------------------------------------------------------------------------------- /rqt_svo/README.md: -------------------------------------------------------------------------------- 1 | run with: 2 | 3 | rosrun rqt_svo rqt_svo 4 | 5 | or start `rqt` and display the widget. 6 | 7 | If it does not work, try this: 8 | 9 | rm ~/.config/ros.org/rqt_gui.ini 10 | rqt -------------------------------------------------------------------------------- /rqt_svo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_svo 4 | 0.0.0 5 | The rqt_svo package 6 | 7 | cforster 8 | 9 | 10 | TODO 11 | 12 | catkin 13 | rospy 14 | rqt_gui 15 | rqt_gui_py 16 | rospy 17 | rqt_gui 18 | rqt_gui_py 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rqt_svo/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plugin for SVO - Semi-Direct Visual Odometry 5 | 6 | 7 | 8 | system-help 9 | Great user interface to provide real value. 10 | 11 | 12 | -------------------------------------------------------------------------------- /rqt_svo/scripts/rqt_svo: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_svo.svo import Svo 6 | from rqt_gui.main import Main 7 | 8 | plugin = 'rqt_svo' 9 | main = Main(filename=plugin) 10 | sys.exit(main.main(sys.argv, standalone=plugin)) # , plugin_argument_provider=Svo.add_arguments -------------------------------------------------------------------------------- /rqt_svo/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_svo'], 8 | package_dir={'': 'src'}, 9 | scripts=['scripts/rqt_svo'] 10 | ) 11 | 12 | setup(**d) -------------------------------------------------------------------------------- /rqt_svo/src/rqt_svo/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /rqt_svo/src/rqt_svo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/rqt_svo/src/rqt_svo/__init__.py -------------------------------------------------------------------------------- /svo/.gitignore: -------------------------------------------------------------------------------- 1 | include/svo/config.h -------------------------------------------------------------------------------- /svo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple) 5 | catkin_simple() 6 | 7 | include(SvoSetup) 8 | 9 | list(APPEND SVO_SOURCEFILES 10 | src/frame_handler_mono.cpp 11 | src/frame_handler_stereo.cpp 12 | src/frame_handler_array.cpp 13 | src/frame_handler_base.cpp 14 | src/map.cpp 15 | src/pose_optimizer.cpp 16 | src/initialization.cpp 17 | src/reprojector.cpp 18 | src/imu_handler.cpp 19 | src/stereo_triangulation.cpp 20 | src/io.cpp 21 | ) 22 | 23 | list(APPEND SVO_HEADERFILES 24 | include/svo/abstract_bundle_adjustment.h 25 | include/svo/frame_handler_base.h 26 | include/svo/frame_handler_mono.h 27 | include/svo/frame_handler_stereo.h 28 | include/svo/frame_handler_array.h 29 | include/svo/global.h 30 | include/svo/imu_handler.h 31 | include/svo/initialization.h 32 | include/svo/map.h 33 | include/svo/pose_optimizer.h 34 | include/svo/reprojector.h 35 | include/svo/stereo_triangulation.h 36 | include/svo/io.h 37 | include/svo/svo.h 38 | ) 39 | 40 | ################################################################################ 41 | # Create svo library 42 | include_directories(include ${INCLUDE_DIRS}) 43 | cs_add_library(svo SHARED ${SVO_SOURCEFILES} ${SVO_HEADERFILES}) 44 | target_link_libraries(svo ${LINK_LIBS}) 45 | 46 | ################################################################################ 47 | ### 48 | ### GTEST 49 | ### 50 | 51 | catkin_add_gtest(${PROJECT_NAME}-test-frame test/test_frame.cpp) 52 | target_link_libraries(${PROJECT_NAME}-test-frame ${PROJECT_NAME}) 53 | 54 | ################################################################################ 55 | cs_install() 56 | cs_export() 57 | -------------------------------------------------------------------------------- /svo/doc/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore -------------------------------------------------------------------------------- /svo/include/svo/frame_handler_array.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // This file is subject to the terms and conditions defined in the file 7 | // 'LICENSE', which is part of this source code package. 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace svo { 14 | 15 | class FrameHandlerArray : public FrameHandlerBase 16 | { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | typedef std::shared_ptr Ptr; 21 | 22 | /// Default constructor 23 | FrameHandlerArray( 24 | const BaseOptions& base_options, 25 | const DepthFilterOptions& depth_filter_options, 26 | const DetectorOptions& feature_detector_options, 27 | const InitializationOptions& init_options, 28 | const ReprojectorOptions& reprojector_options, 29 | const FeatureTrackerOptions& tracker_options, 30 | const CameraBundle::Ptr& cameras); 31 | 32 | virtual ~FrameHandlerArray() = default; 33 | 34 | // deprecated. use addImageBundle(). 35 | void addImages( 36 | const std::vector& images, 37 | const uint64_t timestamp); 38 | 39 | const FrameBundlePtr& lastFrames() const 40 | { 41 | return last_frames_; 42 | } 43 | 44 | protected: 45 | 46 | /// Pipeline implementation. Called by base class. 47 | virtual UpdateResult processFrameBundle() override; 48 | 49 | UpdateResult processFirstFrame(); 50 | 51 | UpdateResult processSecondFrame(); 52 | 53 | UpdateResult processFrame(); 54 | 55 | UpdateResult makeKeyframe(const size_t camera_id); 56 | 57 | /// Reset the frame handler. Implement in derived class. 58 | virtual void resetAll() override; 59 | }; 60 | 61 | } // namespace svo 62 | -------------------------------------------------------------------------------- /svo/include/svo/io.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | namespace io { 8 | 9 | bool saveMap( 10 | const Map::Ptr& map, 11 | const std::string& save_dir, 12 | const std::string& map_name); 13 | 14 | bool loadMap( 15 | const std::string& save_dir, 16 | Map::Ptr& map); 17 | 18 | } // namespace io 19 | } // namespace svo 20 | -------------------------------------------------------------------------------- /svo/include/svo/stereo_triangulation.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | namespace svo { 11 | 12 | struct StereoTriangulationOptions 13 | { 14 | size_t triangulate_n_features = 120; 15 | double mean_depth_inv = 1.0/3.0; 16 | double min_depth_inv = 1.0/1.0; 17 | double max_depth_inv = 1.0/50.0; 18 | }; 19 | 20 | class StereoTriangulation 21 | { 22 | public: 23 | typedef std::shared_ptr Ptr; 24 | 25 | StereoTriangulationOptions options_; 26 | DetectorPtr feature_detector_; 27 | 28 | StereoTriangulation( 29 | const StereoTriangulationOptions& options, 30 | const DetectorPtr& feature_detector); 31 | ~StereoTriangulation() = default; 32 | 33 | void compute(const FramePtr& frame0, const FramePtr& frame1); 34 | }; 35 | 36 | } // namespace svo 37 | -------------------------------------------------------------------------------- /svo/include/svo/svo.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // This file is subject to the terms and conditions defined in the file 7 | // 'LICENSE', which is part of this source code package. 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | -------------------------------------------------------------------------------- /svo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo 4 | 0.1.0 5 | 6 | Monocular Visual Odometry 7 | 8 | Christian Forster 9 | Christian Forster 10 | GPLv3 11 | 12 | catkin 13 | 14 | cmake_modules 15 | svo_cmake 16 | svo_common 17 | svo_direct 18 | svo_tracker 19 | svo_test_utils 20 | svo_img_align 21 | svo_online_loopclosing 22 | svo_global_map 23 | svo_vio_common 24 | roscpp 25 | roslib 26 | rpg_common 27 | vikit_common 28 | vikit_cameras 29 | vikit_solver 30 | vikit_ros 31 | eigen_catkin 32 | minkindr 33 | fast 34 | opengv 35 | cv_bridge 36 | glog_catkin 37 | 38 | 39 | -------------------------------------------------------------------------------- /svo/trace/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | -------------------------------------------------------------------------------- /svo_benchmarking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(svo_benchmarking) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple() 9 | set(CMAKE_BUILD_TYPE "Release") 10 | set(CMAKE_CXX_FLAGS "-std=c++11") 11 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 12 | # find_package(catkin REQUIRED COMPONENTS 13 | # rospy 14 | # roscpp 15 | # rosbag 16 | # ) 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | catkin_python_setup() 22 | 23 | # catkin_package( 24 | # # INCLUDE_DIRS include 25 | # # LIBRARIES bag_to_pose 26 | # # CATKIN_DEPENDS other_catkin_pkg 27 | # # DEPENDS system_lib 28 | # ) 29 | # 30 | # include_directories( 31 | # ${catkin_INCLUDE_DIRS} 32 | # ) 33 | -------------------------------------------------------------------------------- /svo_benchmarking/analzye_traj_cfg/exp_euroc_vio.yaml: -------------------------------------------------------------------------------- 1 | Datasets: 2 | MH_01: 3 | label: MH01 4 | MH_02: 5 | label: MH02 6 | MH_03: 7 | label: MH03 8 | MH_04: 9 | label: MH04 10 | MH_05: 11 | label: MH05 12 | V1_01: 13 | label: V101 14 | V1_02: 15 | label: V102 16 | V1_03: 17 | label: V103 18 | V2_01: 19 | label: V201 20 | V2_02: 21 | label: V202 22 | V2_03: 23 | label: V203 24 | Algorithms: 25 | svo_ceres_rp: 26 | fn: traj_est 27 | label: svoceres-rp 28 | -------------------------------------------------------------------------------- /svo_benchmarking/analzye_traj_cfg/exp_euroc_vio_lc.yaml: -------------------------------------------------------------------------------- 1 | Datasets: 2 | MH_01: 3 | label: MH01 4 | MH_02: 5 | label: MH02 6 | MH_03: 7 | label: MH03 8 | MH_04: 9 | label: MH04 10 | MH_05: 11 | label: MH05 12 | V1_01: 13 | label: V101 14 | V1_02: 15 | label: V102 16 | V1_03: 17 | label: V103 18 | V2_01: 19 | label: V201 20 | V2_02: 21 | label: V202 22 | V2_03: 23 | label: V203 24 | Algorithms: 25 | svo_ceres_rp: 26 | fn: traj_est 27 | label: svoceres-rp 28 | svo_ceres_pg: 29 | fn: pose_graph 30 | label: svoceres-pg 31 | -------------------------------------------------------------------------------- /svo_benchmarking/analzye_traj_cfg/exp_gm_euroc_all.yaml: -------------------------------------------------------------------------------- 1 | Datasets: 2 | MH_01: 3 | label: MH01 4 | MH_02: 5 | label: MH02 6 | MH_03: 7 | label: MH03 8 | MH_04: 9 | label: MH04 10 | MH_05: 11 | label: MH05 12 | V1_01: 13 | label: V101 14 | V1_02: 15 | label: V102 16 | V1_03: 17 | label: V103 18 | V2_01: 19 | label: V201 20 | V2_02: 21 | label: V202 22 | V2_03: 23 | label: V203 24 | Algorithms: 25 | svo_ceres_rp: 26 | fn: traj_est 27 | label: svoceres-rp 28 | svo_ceres_ba: 29 | fn: ba_estimate 30 | label: svoceres-ba 31 | -------------------------------------------------------------------------------- /svo_benchmarking/data/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | */ 3 | !.gitignore 4 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/.gitignore: -------------------------------------------------------------------------------- 1 | *.bag 2 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/generate_ground_truth.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$#" -ne 3 ]; then 4 | echo "Required arguments: " 5 | exit 1 6 | else 7 | printf "Going to process %s with gt topic %s and hand-eye %s.\n" "$1" "$2" "$3" 8 | fi 9 | 10 | # make sure to set the datatype of the ground truth to correct value in bag_to_pose.py 11 | BAG_NAME=$1 12 | GROUNDTRUTH_TOPIC=$2 13 | HAND_EYE=$3 14 | 15 | DATA_DIR_NAME=${BAG_NAME%.bag} 16 | mkdir -p $DATA_DIR_NAME/data/ 17 | cd $DATA_DIR_NAME/data 18 | 19 | echo "> Extracting groundtruth..." 20 | python ../../python/bag_to_pose.py ../../$BAG_NAME $GROUNDTRUTH_TOPIC 21 | 22 | echo "> Associating timestampes..." 23 | python ../../python/associate_timestamps.py images.txt groundtruth.txt 24 | mv matches.txt groundtruth_matches.txt 25 | 26 | echo "> handeye transformation..." 27 | if [ "$HAND_EYE" = "none" ]; then 28 | echo "Not performing hand-eye transformation." 29 | else 30 | python ../../python/transform_trajectory.py groundtruth.txt ../../handeye_tfs/"handeye_$HAND_EYE.yaml" 31 | rm groundtruth.txt 32 | mv groundtruth_transformed.txt groundtruth.txt 33 | fi 34 | 35 | echo "> create stamped groundtruth..." 36 | python ../../python/strip_gt_id.py groundtruth.txt 37 | 38 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/handeye_tfs/handeye_davis.yaml: -------------------------------------------------------------------------------- 1 | # Transformation T_HE 2 | 5.798803966029131e-1, -5.243452865009353e-1, -6.235388890517578e-1, -3.537084841910371e-2 3 | 5.601360558458526e-1, -2.991685865692151e-1, 7.724932075763710e-1, 6.588057192258504e-3 4 | -5.915964203552116e-1, -7.972202815623987e-1, 1.202227020511080e-1, -2.464106641941784e-2 5 | 0, 0, 0, 1.0 6 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/handeye_tfs/handeye_visensor.yaml: -------------------------------------------------------------------------------- 1 | # Transformation T_HE 2 | -0.44118918, -0.6380195, 0.63109694, 0.01109726 3 | -0.87464701, 0.46310671, -0.14326431, -0.04249026 4 | -0.20085987, -0.61519374, -0.76235951, -0.02652775 5 | 0., 0., 0., 1. 6 | 7 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/handeye_tfs/handeye_visensor_1903.yaml: -------------------------------------------------------------------------------- 1 | # Transformation T_HB 2 | -0.87813349, -0.36807792, -0.30561448, -0.01968708 3 | 0.47740762, -0.63273731, -0.60969293, 0.01529268 4 | 0.03104082, -0.68129446, 0.73135102, -0.01370188 5 | 0., 0., 0., 1. 6 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/prepare_dataset_monocular.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$#" -ne 3 ]; then 4 | echo "Required arguments: " 5 | exit 1 6 | else 7 | printf "Going to process %s with image topic %s and imu topic %s.\n" "$1" "$2" "$3" 8 | fi 9 | 10 | BAG_NAME=$1 11 | CAM0_TOPIC=$2 12 | IMU_TOPIC=$3 13 | DATA_DIR_NAME=${BAG_NAME%.bag} 14 | 15 | mkdir -p $DATA_DIR_NAME/data/img 16 | cd $DATA_DIR_NAME/data/img 17 | 18 | # generate images 19 | python2 ../../../python/bag_to_image.py ../../../$BAG_NAME $CAM0_TOPIC 0 20 | mv images.txt ../images.txt 21 | 22 | cd .. #navigate back to /data 23 | 24 | # read imu data and ground truth 25 | python2 ../../python/bag_to_imu.py ../../$BAG_NAME $IMU_TOPIC 26 | 27 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/prepare_dataset_stereo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$#" -ne 4 ]; then 4 | echo "Required arguments: " 5 | exit 1 6 | else 7 | printf "Going to process %s with image topics %s and %s and imu topic %s.\n" "$1" "$2" "$3" "$4" 8 | fi 9 | 10 | BAG_NAME=$1 11 | CAM0_TOPIC=$2 12 | CAM1_TOPIC=$3 13 | IMU_TOPIC=$4 14 | DATA_DIR_NAME=${BAG_NAME%.bag} 15 | mkdir -p $DATA_DIR_NAME/data/img 16 | cd $DATA_DIR_NAME/data/img 17 | 18 | # generate images 19 | python2 ../../../python/bag_to_image.py ../../../$BAG_NAME $CAM0_TOPIC 0 20 | mv images.txt ../images_0.txt 21 | python2 ../../../python/bag_to_image.py ../../../$BAG_NAME $CAM1_TOPIC 1 22 | mv images.txt ../images_1.txt 23 | cd .. #navigate back to /data 24 | python2 ../../python/combine_images.py 25 | mv images_combined.txt images.txt 26 | rm images_0.txt 27 | rm images_1.txt 28 | 29 | # read imu data and ground truth 30 | python2 ../../python/bag_to_imu.py ../../$BAG_NAME $IMU_TOPIC 31 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/asl_groundtruth_to_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import argparse 4 | import numpy as np 5 | 6 | def extract(gt, out_filename): 7 | n = 0 8 | fout = open(out_filename, 'w') 9 | fout.write('# id timestamp tx ty tz qx qy qz qz\n') 10 | 11 | with open(gt,'rb') as fin: 12 | data = np.genfromtxt(fin, delimiter=",") 13 | for l in data: 14 | fout.write('%d %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' % 15 | (n, l[0]/1e9, l[1], l[2], l[3], l[5], l[6], l[7], l[4])) 16 | n += 1 17 | print('wrote ' + str(n) + ' poses to the file: ' + out_filename) 18 | fout.close() 19 | 20 | 21 | if __name__ == '__main__': 22 | parser = argparse.ArgumentParser(description=''' 23 | Extracts IMU messages from a csv file in ASL format. 24 | ''') 25 | parser.add_argument('gt', help='Ground truth csv') 26 | parser.add_argument('--output', default='groundtruth.txt') 27 | args = parser.parse_args() 28 | out_filename = args.output 29 | print('Extract ground truth pose from file '+args.gt) 30 | print('Saving to file '+out_filename) 31 | extract(args.gt, out_filename) 32 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/bag_to_image.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | import rosbag 4 | import argparse 5 | import cv2 6 | import numpy as np 7 | from cv_bridge import CvBridge, CvBridgeError 8 | 9 | def extract(bagfile, pose_topic, out_filename, cam_id): 10 | n = 0 11 | f = open(out_filename, 'w') 12 | f.write('# id timestamp image_name\n') 13 | cv_bridge = CvBridge() 14 | extract_every_nth_image = 1 15 | #max_imgs = 400 16 | 17 | with rosbag.Bag(bagfile, 'r') as bag: 18 | for (topic, msg, ts) in bag.read_messages(topics=str(pose_topic)): 19 | if np.mod(n, extract_every_nth_image) == 0: 20 | try: 21 | img = cv_bridge.imgmsg_to_cv2(msg, 'bgr8') 22 | except CvBridgeError, e: 23 | print e 24 | 25 | ts = msg.header.stamp.to_sec() 26 | image_name = 'image_'+str(cam_id)+'_'+str(n)+'.png' 27 | f.write('%d %.12f img/%s \n' % (n, ts, image_name)) 28 | cv2.imwrite(image_name, img) 29 | n += 1 30 | #if n > max_imgs: 31 | # break 32 | 33 | print('wrote ' + str(n) + ' images messages to the file: ' + out_filename) 34 | 35 | if __name__ == '__main__': 36 | parser = argparse.ArgumentParser(description=''' 37 | Extracts Images messages from bagfile. 38 | ''') 39 | parser.add_argument('bag', help='Bagfile') 40 | parser.add_argument('topic', help='Topic') 41 | parser.add_argument('cam_id', help='Camera-Id') 42 | args = parser.parse_args() 43 | print('Extract images from bag '+args.bag+' in topic ' + args.topic) 44 | extract(args.bag, args.topic, 'images.txt', args.cam_id) 45 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/bag_to_imu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rosbag 4 | import argparse 5 | 6 | def extract(bagfile, imu_topic, out_filename): 7 | n = 0 8 | f = open(out_filename, 'w') 9 | f.write('# timestamp ang_vel_x ang_vel_y ang_vel_z lin_acc_x lin_acc_y lin_acc_z\n') 10 | with rosbag.Bag(bagfile, 'r') as bag: 11 | for (topic, msg, ts) in bag.read_messages(topics=str(imu_topic)): 12 | f.write('%d %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' % 13 | (n, 14 | msg.header.stamp.to_sec(), 15 | msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, 16 | msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z)) 17 | n += 1 18 | print('wrote ' + str(n) + ' imu messages to the file: ' + out_filename) 19 | 20 | if __name__ == '__main__': 21 | parser = argparse.ArgumentParser(description=''' 22 | Extracts IMU messages from bagfile. 23 | ''') 24 | parser.add_argument('bag', help='Bagfile') 25 | parser.add_argument('topic', help='Topic') 26 | args = parser.parse_args() 27 | 28 | extract(args.bag, args.topic, 'imu.txt') 29 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/change_image_path.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import argparse 5 | 6 | import numpy as np 7 | 8 | if __name__ == '__main__': 9 | parser = argparse.ArgumentParser( 10 | description='change the image path in images.txt') 11 | parser.add_argument('org_img_list', 12 | help="file contains original image file values") 13 | parser.add_argument('new_path', 14 | help="new path that is to be prepended " 15 | "to the image name") 16 | args = parser.parse_args() 17 | 18 | assert os.path.exists(args.org_img_list) 19 | outdir = os.path.dirname(args.org_img_list) 20 | outfn = os.path.join(outdir, 21 | 'changed_' + os.path.basename(args.org_img_list)) 22 | 23 | print("Going to prepend {0} to {1} and write to {2}".format( 24 | args.new_path, args.org_img_list, outfn)) 25 | 26 | org_content = [] 27 | with open(args.org_img_list) as f: 28 | org_content = f.readlines() 29 | org_content = [x.strip().split(' ') for x in org_content] 30 | print("Found {0} data".format(len(org_content))) 31 | 32 | changed_content = [] 33 | cnt = 0 34 | for l in org_content: 35 | if l[0].startswith('#'): 36 | continue 37 | old_fn = os.path.basename(l[-1]) 38 | l[-1] = os.path.join(args.new_path, old_fn) 39 | changed_content.append(l) 40 | 41 | changed_content = [' '.join(x) + '\n' for x in changed_content] 42 | with open(outfn, 'w') as f: 43 | f.writelines(changed_content) 44 | 45 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/combine_images.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | """ 3 | @author: Jonathan Huber 4 | 5 | @brief: combine two image lists of stereo pair to format, which can be used 6 | by benchmark_node_with_ceres. Will append each image in images_1.txt to 7 | corresponding line in images_0.txt 8 | """ 9 | 10 | f = open('images_1.txt', "r") 11 | lines = f.readlines() 12 | images_1 = [] 13 | timestamps_1 = [] 14 | for x in lines: 15 | images_1.append(x.split(' ')[2]) 16 | timestamps_1.append(x.split(' ')[1]) 17 | f.close() 18 | 19 | file_lines = [] 20 | with open('images_0.txt', 'r') as f: 21 | i = 0 22 | for x in f.readlines(): 23 | timestamp_0 = x.split(' ')[1] 24 | while(timestamps_1[i] < timestamp_0): 25 | i = i+1 26 | if(i == len(timestamps_1)): 27 | break 28 | if(i == len(timestamps_1)): 29 | break 30 | if timestamps_1[i] == timestamp_0: 31 | file_lines.append(''.join([x.strip(), ' ', images_1[i], '\n'])) 32 | i = i+1 33 | if(i == len(timestamps_1)): 34 | break 35 | elif timestamps_1[i] > timestamp_0: 36 | continue 37 | 38 | 39 | with open('images_combined.txt', 'w') as f: 40 | f.writelines(file_lines) 41 | 42 | print('Created file images_combined.txt') 43 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/prepend_id.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import argparse 5 | 6 | import numpy as np 7 | 8 | if __name__ == '__main__': 9 | parser = argparse.ArgumentParser( 10 | description='prepend id to values in a text file') 11 | parser.add_argument('org_file', 12 | help="file contains original values" 13 | "the first two columns") 14 | args = parser.parse_args() 15 | 16 | assert os.path.exists(args.org_file) 17 | outdir = os.path.dirname(args.org_file) 18 | outfn = os.path.join(outdir, 19 | 'id_' + os.path.basename(args.org_file)) 20 | 21 | print("Going to prepend id to {0} and write to {1}".format( 22 | args.org_file, outfn)) 23 | 24 | org_content = [] 25 | with open(args.org_file) as f: 26 | org_content = f.readlines() 27 | print("Found {0} data".format(len(org_content))) 28 | 29 | id_content = [] 30 | cnt = 0 31 | for l in org_content: 32 | if not l.startswith('#'): 33 | l = str(cnt) + ' ' + l 34 | cnt += 1 35 | else: 36 | if cnt == 0: 37 | l = l.replace('#', '# id') 38 | id_content.append(l) 39 | 40 | with open(outfn, 'w') as f: 41 | f.writelines(id_content) 42 | 43 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/strip_gt_id.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import argparse 5 | 6 | import numpy as np 7 | 8 | 9 | def strip_gt_id_and_save(id_stamp_gt_fn): 10 | outdir = os.path.dirname(id_stamp_gt_fn) 11 | outfn = os.path.join(outdir, 12 | 'stamped_' + os.path.basename(id_stamp_gt_fn)) 13 | 14 | print("Going to strip id from {0} and write to {1}".format( 15 | id_stamp_gt_fn, outfn)) 16 | 17 | id_stamp_gt = np.loadtxt(id_stamp_gt_fn) 18 | print("Found {0} groundtruth data".format(id_stamp_gt.shape[0])) 19 | 20 | stamp_gt = [] 21 | for v in id_stamp_gt.tolist(): 22 | stamped_v = v[1:] 23 | stamp_gt.append(stamped_v) 24 | 25 | np.savetxt(outfn, stamp_gt, header='time x y z qx qy qz qw') 26 | print("Written {0} stamped groundtruth.".format(len(stamp_gt))) 27 | 28 | 29 | if __name__ == '__main__': 30 | parser = argparse.ArgumentParser( 31 | description='strip the id from the groundtruth') 32 | parser.add_argument('id_stamp_gt', 33 | help="groundtruth file constains id and stamps as " 34 | "the first two columns") 35 | args = parser.parse_args() 36 | 37 | assert os.path.exists(args.id_stamp_gt) 38 | strip_gt_id_and_save(args.id_stamp_gt) 39 | -------------------------------------------------------------------------------- /svo_benchmarking/dataset_tools/python/swap_stamped_imu_meas.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import argparse 5 | 6 | import numpy as np 7 | 8 | if __name__ == '__main__': 9 | parser = argparse.ArgumentParser( 10 | description='swap stamped imu values (Acc and Gyr)') 11 | parser.add_argument('org_imu', 12 | help="orignal imu values as N x 7 matrix") 13 | args = parser.parse_args() 14 | 15 | assert os.path.exists(args.org_imu) 16 | outdir = os.path.dirname(args.org_imu) 17 | outfn = os.path.join(outdir, 18 | 'swapped_' + os.path.basename(args.org_imu)) 19 | 20 | print("Going to swap Acc and Gyr from {0} to {1}".format( 21 | args.org_imu, outfn)) 22 | 23 | org_values = np.loadtxt(args.org_imu) 24 | assert org_values.shape[1] == 7 25 | print("Read {0} IMU values.".format(org_values.shape[0])) 26 | 27 | swapped_values = np.zeros(org_values.shape) 28 | 29 | swapped_values[:, 0] = org_values[:, 0] 30 | swapped_values[:, 1:4] = org_values[:, 4:7] 31 | swapped_values[:, 4:7] = org_values[:, 1:4] 32 | 33 | np.savetxt(outfn, swapped_values) 34 | -------------------------------------------------------------------------------- /svo_benchmarking/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_benchmarking 4 | 0.0.0 5 | svo_benchmarking utils 6 | 7 | Zichao Zhang 8 | Zichao Zhang 9 | Jonathan Huber 10 | Christian Forster 11 | BSD 12 | 13 | catkin 14 | catkin_simple 15 | rospy 16 | roscpp 17 | rosbag 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | python-rospkg 22 | python-yaml 23 | svo_ros 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /svo_benchmarking/results/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | */ 3 | !.gitignore 4 | -------------------------------------------------------------------------------- /svo_benchmarking/scripts/analyze_all_euroc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./MH_01 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 4 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./MH_02 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 5 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./MH_03 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 6 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./MH_04 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 7 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./MH_05 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 8 | 9 | 10 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V1_01 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 11 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V1_02 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 12 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V1_03 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 13 | 14 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V2_01 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 15 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V2_02 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 16 | rosrun rpg_trajectory_evaluation analyze_trajectory_single.py ./V2_03 --recalculate_errors --mul_trials 5 --est_types traj_est pose_graph 17 | -------------------------------------------------------------------------------- /svo_benchmarking/scripts/bag_eval_scripts/cal_traj_len.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rosbag 5 | import argparse 6 | import os 7 | import numpy as np 8 | 9 | def calGTLengthFromBag(bag_fn, gt_topic): 10 | bag = rosbag.Bag(bag_fn) 11 | gt_msgs = [] 12 | for topic, msg, t in bag.read_messages(topics=[gt_topic]): 13 | gt_msgs.append(msg) 14 | 15 | prev_seq = -1 16 | gt_positions = [] 17 | for msg_i in gt_msgs: 18 | seq_i = msg_i.header.seq 19 | assert seq_i > prev_seq 20 | prev_seq = seq_i 21 | pos_i = msg_i.pose.position 22 | gt_positions.append([pos_i.x, pos_i.y, pos_i.z]) 23 | assert len(gt_positions) > 0, "Found no groundtruth messages." 24 | 25 | gt_positions = np.array(gt_positions) 26 | print("Collecting {0} groundtruth positions.".format(gt_positions.shape[0])) 27 | 28 | gt_dist = np.diff(gt_positions, axis=0) 29 | 30 | gt_len = np.sum([np.linalg.norm(v) for v in gt_dist]) 31 | 32 | return gt_len 33 | 34 | 35 | if __name__ == '__main__': 36 | parser = argparse.ArgumentParser(description="Calculate the trajectory length for bag files under a directory.") 37 | parser.add_argument('dir', help='directory that contains the bags to calculate the groundtruth') 38 | parser.add_argument('gt_topic', help='groundtruth topic') 39 | 40 | args = parser.parse_args() 41 | 42 | print("Going to analyze bags under {0} for groundtruth topic {1}.".format(args.dir, args.gt_topic)) 43 | 44 | bag_fns = [x for x in os.listdir(args.dir) if x.endswith('.bag')] 45 | bag_fns.sort() 46 | bag_fullfns = [os.path.join(args.dir, v) for v in bag_fns] 47 | print("Found bag files {0}.".format(bag_fullfns)) 48 | 49 | gt_len = [] 50 | for bag_i in bag_fullfns: 51 | print("Analyzing {0}".format(bag_i)) 52 | gt_len_i = calGTLengthFromBag(bag_i, args.gt_topic) 53 | gt_len.append(gt_len_i) 54 | print(">>> GT {0} meters.".format(gt_len_i)) 55 | -------------------------------------------------------------------------------- /svo_benchmarking/scripts/convert_old_fmt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import argparse 5 | import subprocess 6 | 7 | 8 | kNsToEstFnMapping = {'traj_est': 'stamped_traj_estimate', 9 | 'pose_graph': 'stamped_pose_graph_estimate'} 10 | kNsToAlgoMapping = {'traj_est': 'svo_ceres_lc', 11 | 'pose_graph': 'svo_ceres_lc_pg'} 12 | # kNsToAlgoMapping = {'traj_est': 'svo_ceres_nolc'} 13 | kFnExt = '.txt' 14 | 15 | 16 | if __name__ == '__main__': 17 | parser = argparse.ArgumentParser( 18 | description='''Organize reuslts for analysis.''') 19 | parser.add_argument( 20 | 'result_dir', type=str, 21 | help="Folder containing the groundtruth and the estimate.") 22 | args = parser.parse_args() 23 | 24 | abs_res_dir = os.path.abspath(args.result_dir) 25 | assert os.path.exists(abs_res_dir), "Result folder does not exist." 26 | 27 | res_dirs = [v for v in os.listdir(abs_res_dir) 28 | if os.path.isdir(os.path.join(abs_res_dir, v))] 29 | res_dirs.sort() 30 | print("Found directories {0}".format(res_dirs)) 31 | 32 | for d in res_dirs: 33 | print("Processing {0}...".format(d)) 34 | abs_dir = os.path.join(abs_res_dir, d) 35 | subprocess.call( 36 | ['rosrun', 'rpg_trajectory_evaluation', 37 | 'stamp_state_est_using_matches.py', 'traj_estimate.txt'], 38 | cwd=abs_dir) 39 | subprocess.call( 40 | ['rosrun', 'rpg_trajectory_evaluation', 'strip_gt_id.py', 41 | 'groundtruth.txt'], cwd=abs_dir) 42 | subprocess.call( 43 | ['mv', 'stamped_traj_estimate.txt', 'stamped_traj_estimate0.txt'], 44 | cwd=abs_dir) 45 | 46 | -------------------------------------------------------------------------------- /svo_benchmarking/scripts/evaluate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | """ 3 | @author: Christian Forster 4 | """ 5 | 6 | import os 7 | import argparse 8 | import yaml 9 | 10 | from colorama import init, Fore 11 | 12 | init(autoreset=True) 13 | 14 | 15 | def run_evaluation_script(trace_dir, script): 16 | command = 'rosrun ' + script['ros_node'] + ' ' + script['ros_executable'] 17 | # position parameters 18 | if 'params' in script: 19 | for param in script['params']: 20 | if param == 'trace_dir': 21 | command += " " + trace_dir 22 | else: 23 | command += " " + param 24 | # flags 25 | if 'flags' in script: 26 | for key, option in script['flags'].items(): 27 | command += ' --'+str(key)+'='+str(option) 28 | print(Fore.RED+'==> Staring: '+command) 29 | os.system(command) 30 | print(Fore.GREEN+'<== Finished') 31 | 32 | 33 | def evaluate_dataset(trace_dir): 34 | if os.path.exists(os.path.join(trace_dir, 'evaluation_scripts.yaml')): 35 | eval_scripts = yaml.load( 36 | open(os.path.join(trace_dir, 'evaluation_scripts.yaml'), 'r')) 37 | for script in eval_scripts: 38 | run_evaluation_script(trace_dir, script) 39 | else: 40 | print('Folder "'+trace_dir + 41 | '" does not contain a file "evaluation_scripts.yaml".') 42 | 43 | 44 | if __name__ == "__main__": 45 | parser = argparse.ArgumentParser(description=''' 46 | Evaluates tracefiles of SVO and generates the plots. 47 | The experiment folder should contain an "evaluation_scripts.yaml" that 48 | specifies scripts to run. 49 | ''') 50 | parser.add_argument( 51 | 'experiment_dir', help='directory name of the tracefiles') 52 | args = parser.parse_args() 53 | 54 | evaluate_dataset(args.experiment_dir) 55 | -------------------------------------------------------------------------------- /svo_benchmarking/scripts/evaluate_all.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import argparse 4 | import os 5 | 6 | from evaluate import evaluate_dataset 7 | 8 | if __name__ == "__main__": 9 | parser = argparse.ArgumentParser(description=''' 10 | run the evaluation for all subfolders 11 | ''') 12 | parser.add_argument( 13 | 'top_dir', help='directory name of the tracefiles') 14 | args = parser.parse_args() 15 | 16 | subfolders = [v for v in os.listdir(args.top_dir) 17 | if os.path.join(args.top_dir, v)] 18 | print("Going to evaluate the following folders:{0}".format(subfolders)) 19 | subfolders = [os.path.join(args.top_dir, v) for v in subfolders] 20 | 21 | for s in subfolders: 22 | if not os.path.exists(os.path.join(s, 'evaluation_scripts.yaml')): 23 | print("skip {0}".format(s)) 24 | continue 25 | evaluate_dataset(s) 26 | -------------------------------------------------------------------------------- /svo_benchmarking/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['svo_benchmarking'], 8 | package_dir={'': 'src'}, 9 | install_requires=['rospkg', 'yaml'] 10 | ) 11 | 12 | setup(**d) 13 | -------------------------------------------------------------------------------- /svo_benchmarking/src/svo_ceres_benchmarking/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/svo_benchmarking/src/svo_ceres_benchmarking/__init__.py -------------------------------------------------------------------------------- /svo_benchmarking/src/svo_ceres_benchmarking/dataset_loading.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import os 4 | import numpy as np 5 | 6 | 7 | def load_imu_meas(trace_dir, fn='imu.txt'): 8 | abs_fn = os.path.join(trace_dir, fn) 9 | assert os.path.exists(abs_fn), "{0} does not exist.".format(abs_fn) 10 | 11 | imu_id = [] 12 | imu_time_sec = [] 13 | gyr_meas = [] 14 | acc_meas = [] 15 | 16 | imu_data = np.loadtxt(abs_fn) 17 | assert imu_data.shape[1] == 8, "Wrong data format." 18 | for row in imu_data: 19 | imu_id.append(int(row[0])) 20 | imu_time_sec.append(row[1]) 21 | gyr_meas.append(row[2:5]) 22 | acc_meas.append(row[5:8]) 23 | 24 | return np.array(imu_id), np.array(imu_time_sec),\ 25 | np.array(gyr_meas), np.array(acc_meas) 26 | -------------------------------------------------------------------------------- /svo_benchmarking/src/svo_ceres_benchmarking/hand_eye_calib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import numpy as np 4 | import transformations as tf 5 | 6 | import tfs_utils as tu 7 | 8 | 9 | def hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True): 10 | """Implementation of the least squares solution described in the paper: 11 | Robot Sensor Calibration: Solving AX=XB on the Euclidean Group 12 | by Frank C. Park and Bryan J. Martin 13 | """ 14 | n = np.shape(I)[0] 15 | M = np.zeros([3, 3]) 16 | C = np.zeros([3*n, 3]) 17 | b_A = np.zeros([3*n, 1]) 18 | b_B = np.zeros([3*n, 1]) 19 | for ix, i in enumerate(I): 20 | A1 = tu.quat2dcm(q_es[i, :]) 21 | A2 = tu.quat2dcm(q_es[i+delta, :]) 22 | A = np.dot(A1.transpose(), A2) 23 | B1 = tu.quat2dcm(q_gt[i, :]) 24 | B2 = tu.quat2dcm(q_gt[i+delta, :]) 25 | B = np.dot(B1.transpose(), B2) 26 | alpha = tf.logmap_so3(A) 27 | beta = tf.logmap_so3(B) 28 | M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha)) 29 | C[3*ix:3*ix+3, :] = np.eye(3) - A 30 | b_A[3*ix:3*ix+3, 0] = np.dot(np.transpose(A1), 31 | p_es[i+delta, :]-p_es[i, :]) 32 | b_B[3*ix:3*ix+3, 0] = np.dot(np.transpose(B1), 33 | p_gt[i+delta, :]-p_gt[i, :]) 34 | # compute rotation 35 | D, V = np.linalg.linalg.eig(np.dot(M.transpose(), M)) 36 | Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])]) 37 | Vinv = np.linalg.linalg.inv(V) 38 | X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose()))) 39 | # compute translation 40 | d = np.zeros([3*n, 1]) 41 | for i in range(n): 42 | d[3*i:3*i+3, :] = b_A[3*i:3*i+3, :] - np.dot(X, b_B[3*i:3*i+3, :]) 43 | b = np.dot(np.linalg.inv(np.dot(np.transpose(C), C)), 44 | np.dot(np.transpose(C), d)) 45 | return np.array(X), b 46 | -------------------------------------------------------------------------------- /svo_benchmarking/src/svo_ceres_benchmarking/tracefile_reader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | @author: Christian Forster 4 | """ 5 | 6 | import os 7 | import csv 8 | import numpy as np 9 | 10 | 11 | def read(trace_dir, trace_name): 12 | # read tracefile 13 | data = csv.reader(open(os.path.join(trace_dir, trace_name))) 14 | fields = data.next() 15 | D = dict() 16 | for field in fields: 17 | D[field] = list() 18 | 19 | # fill dictionary with column values 20 | for row in data: 21 | for (field, value) in zip(fields, row): 22 | D[field].append(float(value)) 23 | 24 | # change dictionary values from list to numpy array for easier manipulation 25 | for field, value in D.items(): 26 | D[field] = np.array(D[field]) 27 | 28 | return D 29 | -------------------------------------------------------------------------------- /svo_benchmarking/src/svo_ceres_benchmarking/trajectory_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | @author: Christian Forster 4 | """ 5 | 6 | import os 7 | import numpy as np 8 | import transformations as tf 9 | 10 | 11 | def compute_comparison_indices_time(t_gt, dist, max_error): 12 | max_idx = len(t_gt) 13 | comparisons = [] 14 | for idx, d in enumerate(t_gt): 15 | j = -1 16 | error = max_error 17 | for i in range(idx, max_idx): 18 | if np.abs(t_gt[i]-(d+dist)) < error: 19 | j = i 20 | error = np.abs(t_gt[i]-(d+dist)) 21 | comparisons.append(j) 22 | return comparisons 23 | 24 | 25 | def get_distance_from_start(gt_translation): 26 | distances = np.diff(gt_translation[:, 0:3], axis=0) 27 | distances = np.sqrt(np.sum(np.multiply(distances, distances), 1)) 28 | distances = np.cumsum(distances) 29 | distances = np.concatenate(([0], distances)) 30 | return distances 31 | 32 | 33 | def distances_along_trajectory(traj): 34 | """ 35 | Compute the translational distances along a trajectory. 36 | """ 37 | 38 | motion = [np.linalg.norm(traj[i, :]-traj[i+1, :]) 39 | for i in range(len(traj)-1)] 40 | distances = [0] 41 | sum = 0 42 | for t in motion: 43 | sum += t 44 | distances.append(sum) 45 | return distances 46 | -------------------------------------------------------------------------------- /svo_ceres_backend/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_ceres_backend 4 | 0.1.4 5 | 6 | svo_ceres_backend 7 | 8 | Jonathan Huber 9 | BSD 10 | 11 | catkin 12 | catkin_simple 13 | 14 | ceres_catkin 15 | eigen_catkin 16 | glog_catkin 17 | 18 | svo_vio_common 19 | svo_common 20 | svo 21 | pcl_ros 22 | 23 | -------------------------------------------------------------------------------- /svo_ceres_backend/src/error_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "svo/ceres_backend/error_interface.hpp" 2 | 3 | namespace svo 4 | { 5 | namespace ceres_backend 6 | { 7 | const std::map kErrorToStr 8 | { 9 | {ErrorType::kHomogeneousPointError, std::string("HomogeneousPointError") }, 10 | {ErrorType::kReprojectionError, std::string("ReprojectionError") }, 11 | {ErrorType::kSpeedAndBiasError, std::string("SpeedAndBiasError") }, 12 | {ErrorType::kMarginalizationError, std::string("MarginalizationError") }, 13 | {ErrorType::kPoseError, std::string("PoseError") }, 14 | {ErrorType::kIMUError, std::string("IMUError") }, 15 | {ErrorType::kRelativePoseError, std::string("RelativePoseError") }, 16 | }; 17 | 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /svo_cmake/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(svo_cmake) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | # install all included cmake files 8 | install(DIRECTORY cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake) 9 | 10 | cs_install() 11 | cs_export(CFG_EXTRAS svo_cmake-extras.cmake) 12 | -------------------------------------------------------------------------------- /svo_cmake/cmake/Modules/SvoSetup.cmake: -------------------------------------------------------------------------------- 1 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 2 | #SET(CMAKE_BUILD_TYPE Debug) # Release, RelWithDebInfo 3 | SET(CMAKE_VERBOSE_MAKEFILE OFF) 4 | 5 | # user build settings 6 | SET(USE_LOOP_CLOSING TRUE) 7 | SET(USE_GLOBAL_MAP FALSE) 8 | 9 | # Set definitions 10 | IF(USE_LOOP_CLOSING) 11 | ADD_DEFINITIONS(-DSVO_LOOP_CLOSING) 12 | ENDIF() 13 | 14 | IF(USE_GLOBAL_MAP) 15 | ADD_DEFINITIONS(-DSVO_GLOBAL_MAP) 16 | ENDIF() 17 | 18 | ADD_DEFINITIONS(-DSVO_USE_ROS) 19 | ADD_DEFINITIONS(-DSVO_USE_OPENGV) 20 | ADD_DEFINITIONS(-DSVO_DEPTHFILTER_IN_REPROJECTOR) 21 | 22 | ############################################################################# 23 | # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid 24 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -Wall -Werror -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas -Wno-unused-but-set-parameter -Wno-int-in-bool-context -Wno-maybe-uninitialized -Wno-unused-function") 25 | 26 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 28 | ADD_DEFINITIONS(-DHAVE_FAST_NEON) 29 | ELSE() 30 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse2 -msse3 -mssse3 -mno-avx") 31 | ENDIF() 32 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 33 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops -ffast-math -fno-finite-math-only") 34 | -------------------------------------------------------------------------------- /svo_cmake/cmake/svo_cmake-extras.cmake.develspace.in: -------------------------------------------------------------------------------- 1 | list(INSERT CMAKE_MODULE_PATH 0 "@CMAKE_CURRENT_SOURCE_DIR@/cmake/Modules") 2 | -------------------------------------------------------------------------------- /svo_cmake/cmake/svo_cmake-extras.cmake.installspace.in: -------------------------------------------------------------------------------- 1 | list(INSERT CMAKE_MODULE_PATH 0 2 | "${cmake_modules_DIR}/../../../@CATKIN_PACKAGE_SHARE_DESTINATION@/cmake/Modules") 3 | 4 | -------------------------------------------------------------------------------- /svo_cmake/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_cmake 4 | 5 | CMake configuration, scripts and macros common for common SVO usage 6 | 7 | 0.1.0 8 | tbd 9 | 10 | Christian Forster 11 | Manuel Werlberger 12 | 13 | catkin 14 | catkin_simple 15 | 16 | -------------------------------------------------------------------------------- /svo_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_common) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS 10 | include/svo/common/camera.h 11 | include/svo/common/camera_fwd.h 12 | include/svo/common/feature_wrapper.h 13 | include/svo/common/frame.h 14 | include/svo/common/imu_calibration.h 15 | include/svo/common/logging.h 16 | include/svo/common/occupancy_grid_2d.h 17 | include/svo/common/point.h 18 | include/svo/common/seed.h 19 | include/svo/common/transformation.h 20 | include/svo/common/types.h 21 | ) 22 | 23 | set(SOURCES 24 | src/frame.cpp 25 | src/point.cpp 26 | ) 27 | 28 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 29 | 30 | ################################################################################ 31 | # TESTS 32 | 33 | ################################################################################ 34 | ### 35 | ### GTEST 36 | ### 37 | 38 | catkin_add_gtest(${PROJECT_NAME}-test 39 | test/test_main.cpp 40 | test/test_feature_wrapper.cpp 41 | ) 42 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread) 43 | 44 | catkin_add_gtest(container-helpers-test 45 | test/test_container_helpers.cpp 46 | ) 47 | target_link_libraries(container-helpers-test ${PROJECT_NAME}) 48 | 49 | ################################################################################ 50 | cs_install() 51 | cs_export() 52 | -------------------------------------------------------------------------------- /svo_common/include/svo/common/camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | # include 6 | # include 7 | -------------------------------------------------------------------------------- /svo_common/include/svo/common/camera_fwd.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace vk { 6 | namespace cameras { 7 | class CameraGeometryBase; 8 | class NCamera; 9 | } 10 | } 11 | 12 | namespace svo 13 | { 14 | using Camera = vk::cameras::CameraGeometryBase; 15 | using CameraPtr = std::shared_ptr; 16 | using CameraConstPtr = std::shared_ptr; 17 | using CameraBundle = vk::cameras::NCamera; 18 | using CameraBundlePtr = std::shared_ptr; 19 | } // namespace svo 20 | -------------------------------------------------------------------------------- /svo_common/include/svo/common/conversions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace svo { 6 | namespace common { 7 | namespace conversions { 8 | 9 | constexpr double kDegToRad = M_PI / 180.; 10 | 11 | constexpr size_t kMilliSecondsToNanoSeconds = 1e6; 12 | constexpr double kNanoSecondsToSeconds = 1e-9; 13 | 14 | } // namespace conversions 15 | } // namespace common 16 | } // namespace svo 17 | -------------------------------------------------------------------------------- /svo_common/include/svo/common/feature_wrapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | 8 | // forward declaration 9 | class Point; 10 | using PointPtr = std::shared_ptr; 11 | 12 | class Frame; 13 | using FramePtr = std::shared_ptr; 14 | 15 | struct SeedRef 16 | { 17 | FramePtr keyframe; 18 | int seed_id = -1; 19 | SeedRef(const FramePtr& _keyframe, const int _seed_id) 20 | : keyframe(_keyframe) 21 | , seed_id(_seed_id) 22 | { ; } 23 | SeedRef() = default; 24 | ~SeedRef() = default; 25 | }; 26 | 27 | /** @todo (MWE) 28 | */ 29 | struct FeatureWrapper 30 | { 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | FeatureType& type; //!< Type can be corner or edgelet. 34 | Eigen::Ref px; //!< Coordinates in pixels on pyramid level 0. 35 | Eigen::Ref f; //!< Unit-bearing vector of the feature. 36 | Eigen::Ref grad; //!< Dominant gradient direction for edglets, normalized. 37 | Score& score; 38 | Level& level; //!< Image pyramid level where feature was extracted. 39 | PointPtr& landmark; 40 | SeedRef& seed_ref; 41 | int& track_id; 42 | 43 | FeatureWrapper( 44 | FeatureType& _type, 45 | Eigen::Ref _px, 46 | Eigen::Ref _f, 47 | Eigen::Ref _grad, 48 | Score& _score, 49 | Level& _pyramid_level, 50 | PointPtr& _landmark, 51 | SeedRef& _seed_ref, 52 | int& _track_id) 53 | : type(_type) 54 | , px(_px) 55 | , f(_f) 56 | , grad(_grad) 57 | , score(_score) 58 | , level(_pyramid_level) 59 | , landmark(_landmark) 60 | , seed_ref(_seed_ref) 61 | , track_id(_track_id) 62 | { ; } 63 | 64 | FeatureWrapper() = delete; 65 | ~FeatureWrapper() = default; 66 | 67 | //! @todo (MWE) do copy and copy-asignment operators make sense? 68 | FeatureWrapper(const FeatureWrapper& other) = default; 69 | FeatureWrapper& operator=(const FeatureWrapper& other) = default; 70 | }; 71 | 72 | } // namespace svo 73 | -------------------------------------------------------------------------------- /svo_common/include/svo/common/transformation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace svo { 6 | 7 | using Transformation = kindr::minimal::QuatTransformation; 8 | using Quaternion = kindr::minimal::RotationQuaternion; 9 | 10 | } // namespace svo 11 | -------------------------------------------------------------------------------- /svo_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_common 4 | 5 | SVO common module for types and common base functionality used by other modules 6 | 7 | 0.0.1 8 | tbd 9 | 10 | Christian Forster 11 | Manuel Werlberger 12 | Christian Forster 13 | 14 | catkin 15 | catkin_simple 16 | 17 | svo_cmake 18 | vikit_common 19 | vikit_cameras 20 | fast 21 | 22 | glog_catkin 23 | eigen_catkin 24 | 25 | gtest 26 | eigen_checks 27 | 28 | -------------------------------------------------------------------------------- /svo_common/src/empty.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/svo_common/src/empty.cpp -------------------------------------------------------------------------------- /svo_common/test/test_container_helpers.cpp: -------------------------------------------------------------------------------- 1 | #include "svo/common/container_helpers.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace svo { 9 | namespace common { 10 | namespace container_helpers { 11 | 12 | class ContainerHelpersTest : public ::testing::Test { 13 | protected: 14 | const std::vector original_ = std::vector({0, 1, 2, 3, 4}); 15 | 16 | bool checkEraseIndicesFromVector(const std::vector& to_remove, 17 | const std::vector& expected) { 18 | std::vector copy(original_); 19 | eraseIndicesFromVector(to_remove, ©); 20 | return std::equal(copy.begin(), copy.end(), expected.begin()); 21 | } 22 | }; 23 | 24 | TEST_F(ContainerHelpersTest, EraseIndicesFromVector) { 25 | EXPECT_TRUE(checkEraseIndicesFromVector({0u}, {1, 2, 3, 4})); 26 | EXPECT_TRUE(checkEraseIndicesFromVector({2u}, {0, 1, 3, 4})); 27 | EXPECT_TRUE(checkEraseIndicesFromVector({4u}, {0, 1, 2, 3})); 28 | EXPECT_TRUE(checkEraseIndicesFromVector({0u, 1u}, {2, 3, 4})); 29 | EXPECT_TRUE(checkEraseIndicesFromVector({1u, 2u}, {0, 3, 4})); 30 | EXPECT_TRUE(checkEraseIndicesFromVector({3u, 4u}, {0, 1, 2})); 31 | EXPECT_TRUE(checkEraseIndicesFromVector({0u, 1u, 2u, 3u, 4u}, {})); 32 | } 33 | 34 | } // namespace container_helpers 35 | } // namespace common 36 | } // namespace svo 37 | 38 | VIKIT_UNITTEST_ENTRYPOINT 39 | -------------------------------------------------------------------------------- /svo_common/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv) 5 | { 6 | testing::InitGoogleTest(&argc, argv); 7 | return RUN_ALL_TESTS(); 8 | } 9 | -------------------------------------------------------------------------------- /svo_direct/include/svo/direct/elder_zucker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | namespace elder_zucker { 8 | 9 | void detectEdges( 10 | const std::vector& img_pyr, 11 | const double sigma, 12 | cv::Mat& edge_map, 13 | cv::Mat& level_map); 14 | 15 | void getCovarEntries( 16 | const cv::Mat& src, 17 | cv::Mat& dxdx, 18 | cv::Mat& dydy, 19 | cv::Mat& dxdy); 20 | 21 | void filterGauss3by316S( 22 | const cv::Mat& src, 23 | cv::Mat& dst); 24 | 25 | } // namespace elder_zucker 26 | } // namespace svo 27 | -------------------------------------------------------------------------------- /svo_direct/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_direct 4 | 5 | SVO direct matching and feature alignment module. 6 | 7 | 0.0.1 8 | tbd 9 | 10 | Christian Forster 11 | Manuel Werlberger 12 | Christian Forster 13 | 14 | catkin 15 | catkin_simple 16 | 17 | cv_bridge 18 | eigen_catkin 19 | gflags_catkin 20 | glog_catkin 21 | roslib 22 | svo_common 23 | svo_test_utils 24 | vikit_ros 25 | vikit_solver 26 | 27 | gtest 28 | 29 | -------------------------------------------------------------------------------- /svo_global_map/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/svo_global_map/CATKIN_IGNORE -------------------------------------------------------------------------------- /svo_global_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_global_map) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | find_package(GTSAMCMakeTools REQUIRED) 8 | find_package(GTSAM REQUIRED) 9 | message(STATUS ${GTSAM_INCLUDE_DIR}) 10 | message(STATUS ${GTSAM_LIBRARIES}) 11 | 12 | include(SvoSetup) 13 | include_directories(${GTSAM_INCLUDE_DIR}) 14 | 15 | set(HEADERS 16 | # gtsam interface 17 | include/svo/gtsam/gtsam_optimizer.h 18 | include/svo/gtsam/graph_manager.h 19 | include/svo/gtsam/smart_factors_fwd.h 20 | include/svo/gtsam/camera_bearing_factor.h 21 | include/svo/gtsam/camera_bearing_extrinsics_factor.h 22 | include/svo/global_map.h 23 | include/svo/null_stream.h 24 | ) 25 | 26 | set(SOURCES 27 | # gtsam interface 28 | src/gtsam/gtsam_optimizer.cpp 29 | src/gtsam/graph_manager.cpp 30 | src/global_map.cpp 31 | src/null_stream.cpp 32 | ) 33 | 34 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 35 | target_link_libraries(${PROJECT_NAME} gtsam) 36 | 37 | cs_install() 38 | cs_export() 39 | -------------------------------------------------------------------------------- /svo_global_map/include/svo/gtsam/smart_factors_fwd.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // std::pair 4 | #include 5 | 6 | // forward declarations 7 | namespace gtsam { 8 | class Values; 9 | class Pose3; 10 | class Point3; 11 | class Cal3_S2; 12 | template class FastMap; 13 | template class SmartProjectionPoseFactor; 14 | template 15 | class GenericProjectionFactor; 16 | class SmartProjectionParams; 17 | template class BetweenFactor; 18 | template class PoseBetweenFactor; 19 | template class RangeFactorWithTransform; 20 | template class CameraBearingFactor; 21 | template class CameraBearingExtrinsicsFactor; 22 | } // namespace gtsam 23 | 24 | namespace svo { 25 | 26 | // Typedef of desired classes to reduce compile-time 27 | using ProjectionFactor = gtsam::GenericProjectionFactor; 28 | using SmartFactor = gtsam::SmartProjectionPoseFactor; 29 | using SmartFactorPtr = boost::shared_ptr; 30 | using RelativePoseFactor = gtsam::PoseBetweenFactor; 31 | using CamPointDistFactor = gtsam::RangeFactorWithTransform; 32 | using CameraBearingFactor3D = gtsam::CameraBearingFactor; 33 | using CameraBearingTbcFactor = gtsam::CameraBearingExtrinsicsFactor; 34 | using PointMatchFactor = gtsam::BetweenFactor; 35 | 36 | struct SmartFactorInfo 37 | { 38 | boost::shared_ptr factor_ = nullptr; 39 | int slot_in_graph_ = -1; 40 | SmartFactorInfo() = delete; 41 | SmartFactorInfo(const boost::shared_ptr& factor, 42 | const int slot_idx) 43 | :factor_(factor), slot_in_graph_(slot_idx) 44 | { 45 | 46 | } 47 | }; 48 | 49 | using SmartFactorInfoMap = gtsam::FastMap ; 50 | 51 | } // namespace svo 52 | -------------------------------------------------------------------------------- /svo_global_map/include/svo/null_stream.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo 7 | { 8 | class NullBuffer : public std::streambuf 9 | { 10 | public: 11 | int overflow(int c) 12 | { 13 | return c; 14 | } 15 | }; 16 | 17 | class NullStream : public std::ostream 18 | { 19 | public: 20 | NullStream() : std::ostream(&buffer_) 21 | { 22 | } 23 | private: 24 | NullBuffer buffer_; 25 | }; 26 | 27 | extern NullStream kNullOutput; 28 | } 29 | -------------------------------------------------------------------------------- /svo_global_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_global_map 4 | 5 | Maintain an optimized global map. 6 | 7 | 0.0.1 8 | tbd 9 | Christian Forster 10 | Zichao Zhang 11 | Zichao Zhang 12 | 13 | catkin 14 | catkin_simple 15 | gtsam 16 | svo_vio_common 17 | svo_common 18 | rpg_common 19 | 20 | -------------------------------------------------------------------------------- /svo_global_map/src/null_stream.cpp: -------------------------------------------------------------------------------- 1 | #include "svo/null_stream.h" 2 | 3 | namespace svo 4 | { 5 | NullStream kNullOutput; 6 | } 7 | -------------------------------------------------------------------------------- /svo_img_align/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_img_align) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple) 5 | catkin_simple() 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS_CPU 10 | include/svo/img_align/sparse_img_align_base.h 11 | include/svo/img_align/sparse_img_align.h 12 | ) 13 | 14 | set(SOURCES_CPU 15 | src/sparse_img_align_base.cpp 16 | src/sparse_img_align.cpp 17 | ) 18 | 19 | cs_add_library(${PROJECT_NAME} ${SOURCES_CPU} ${HEADERS_CPU}) 20 | target_link_libraries(${PROJECT_NAME} ${LINK_LIBS}) 21 | 22 | cs_install() 23 | cs_export() 24 | -------------------------------------------------------------------------------- /svo_img_align/include/svo/img_align/gpu_types_cuda.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace svo { 6 | 7 | #define USE_SINGLE_PRECISION 8 | 9 | #ifdef USE_SINGLE_PRECISION 10 | typedef float FloatTypeGpu; 11 | #else 12 | typedef double FloatTypeGpu; 13 | #endif 14 | 15 | #ifdef USE_SINGLE_PRECISION 16 | typedef imp::Pixel32fC1 FloatPixelGpu; 17 | typedef float2 Float2TypeGpu; 18 | typedef imp::Pixel32fC2 Float2PixelGpu; 19 | typedef float3 Float3TypeGpu; 20 | typedef imp::Pixel32fC3 Float3PixelGpu; 21 | typedef std::uint32_t UIntTypeGpu; 22 | typedef imp::Pixel32uC1 UIntPixelGpu; 23 | typedef unsigned char BoolTypeGpu; 24 | typedef imp::Pixel8uC1 BoolPixelGpu; 25 | #else 26 | // use double precision 27 | // TODO: Define Pixel64 in imp 28 | typedef imp::Pixel64fC1 FloatPixelGpu; 29 | typedef double2 Float2TypeGpu; 30 | typedef imp::Pixel64fC2 Float2PixelGpu; 31 | typedef double3 Float3TypeGpu; 32 | typedef imp::Pixel64fC3 Float3PixelGpu; 33 | typedef std::uint32_t UIntTypeGpu; 34 | typedef imp::Pixel32uC1 UIntPixelGpu; 35 | typedef unsigned char BoolTypeGpu; 36 | typedef imp::Pixel8uC1 BoolPixelGpu; 37 | #endif 38 | 39 | } // namespace svo 40 | -------------------------------------------------------------------------------- /svo_img_align/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_img_align 4 | 5 | SVO image align module used for direct image alignment 6 | 7 | 0.0.1 8 | tbd 9 | 10 | Christian Forster 11 | Manuel Werlberger 12 | Michael Gassner 13 | Christian Forster 14 | 15 | catkin 16 | catkin_simple 17 | 18 | svo_cmake 19 | svo_common 20 | svo_direct 21 | glog_catkin 22 | eigen_catkin 23 | vikit_solver 24 | vikit_ros 25 | 26 | gtest 27 | eigen_checks 28 | 29 | -------------------------------------------------------------------------------- /svo_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(svo_msgs) 3 | 4 | # search for everything we need to build the messages, dont forget the message_generation 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | # search for all msg files 12 | FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg" 13 | "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 14 | 15 | # notify catkin to look at the previously found msg files 16 | add_message_files( 17 | FILES 18 | ${messages_to_build} 19 | ) 20 | 21 | # build the header files from the msg files, and notify catkin about the dependencies 22 | generate_messages( 23 | DEPENDENCIES 24 | geometry_msgs 25 | sensor_msgs 26 | ) 27 | 28 | # export the dependencis of this package for who ever depends on us 29 | catkin_package( 30 | CATKIN_DEPENDS message_runtime geometry_msgs 31 | ) 32 | 33 | include_directories( 34 | ${catkin_INCLUDE_DIRS} 35 | ) -------------------------------------------------------------------------------- /svo_msgs/msg/DenseInput.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 frame_id 3 | geometry_msgs/Pose pose 4 | sensor_msgs/Image image 5 | float32 min_depth 6 | float32 max_depth -------------------------------------------------------------------------------- /svo_msgs/msg/DenseInputWithFeatures.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 frame_id 3 | geometry_msgs/Pose pose 4 | sensor_msgs/Image image 5 | float32 min_depth 6 | float32 max_depth 7 | svo_msgs/Feature[] features 8 | -------------------------------------------------------------------------------- /svo_msgs/msg/Feature.msg: -------------------------------------------------------------------------------- 1 | float32 x # x component of 3d point in camera frame 2 | float32 y # y component of 3d point in camera frame 3 | float32 z # z component of 3d point in camera frame -------------------------------------------------------------------------------- /svo_msgs/msg/Info.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 processing_time 3 | int32[] keyframes 4 | int32 num_matches 5 | int32 tracking_quality 6 | int32 stage -------------------------------------------------------------------------------- /svo_msgs/msg/NbvTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose[] trajectory -------------------------------------------------------------------------------- /svo_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_msgs 4 | 0.0.0 5 | Custom messages for svo 6 | Christian Forster 7 | BSD 8 | catkin 9 | 10 | message_generation 11 | geometry_msgs 12 | sensor_msgs 13 | 14 | message_runtime 15 | geometry_msgs 16 | sensor_msgs 17 | 18 | 19 | -------------------------------------------------------------------------------- /svo_online_loopclosing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_online_loopclosing) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | find_package(OpenCV REQUIRED) 7 | 8 | include(SvoSetup) 9 | 10 | set(HEADERS 11 | include/svo/online_loopclosing/bow.h 12 | include/svo/online_loopclosing/geometric_verification.h 13 | include/svo/online_loopclosing/loop_closing.h 14 | include/svo/online_loopclosing/read_file.h 15 | include/svo/online_loopclosing/keyframe.h 16 | include/svo/online_loopclosing/loop_closing_types.h 17 | include/svo/online_loopclosing/map_alignment.h 18 | ) 19 | 20 | set(SOURCES 21 | src/bow.cpp 22 | src/geometric_verification.cpp 23 | src/loop_closing.cpp 24 | src/read_file.cpp 25 | src/map_alignment.cpp 26 | ) 27 | 28 | #add_executable(svo_online_loopclosing ${SOURCES} ${HEADERS}) 29 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 30 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) 31 | 32 | cs_add_library(bow src/bow.cpp include/svo/online_loopclosing/bow.h) 33 | target_link_libraries(bow ${OpenCV_LIBRARIES}) 34 | 35 | cs_add_executable(createvoc test/createvoc.cpp) 36 | target_link_libraries(createvoc bow) 37 | 38 | cs_install() 39 | cs_export() 40 | -------------------------------------------------------------------------------- /svo_online_loopclosing/include/svo/online_loopclosing/loop_closing_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace svo 11 | { 12 | // for container indices 13 | using IdCorrespondence = std::pair; 14 | using CorrespondIds = std::vector; 15 | 16 | // point ids 17 | using CorrespondPointIds = std::map; 18 | 19 | // visualization 20 | using LoopVizInfo = Eigen::Matrix; 21 | using LoopVizInfoVec = rpg::Aligned; 22 | 23 | // pose map 24 | using BundleIdToTwb = std::map; 25 | 26 | struct ClosedLoop 27 | { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | int lc_id_ = 0; 30 | int cf_id_ = 0; 31 | double lc_t_sec_ = 0; 32 | double cf_t_sec_ = 0; 33 | Transformation T_lc_cf_corrected_; 34 | ClosedLoop(const int lc_id, const int cf_id, 35 | const double lc_t_sec, const double cf_t_sec, 36 | const Transformation& T_lc_cf_corrected) 37 | : lc_id_(lc_id), cf_id_(cf_id), 38 | lc_t_sec_(lc_t_sec), cf_t_sec_(cf_t_sec), 39 | T_lc_cf_corrected_(T_lc_cf_corrected) {} 40 | }; 41 | 42 | struct LoopCorrectionInfo 43 | { 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | int lc_kf_bundle_id_ = -1; 46 | int cur_kf_bundle_id_ = -1; 47 | Transformation w_T_new_old_; 48 | LoopCorrectionInfo(const int lc_bundle_id, 49 | const int cur_bundle_id, 50 | const Transformation& w_T_correction) 51 | :lc_kf_bundle_id_(lc_bundle_id), cur_kf_bundle_id_(cur_bundle_id), 52 | w_T_new_old_(w_T_correction) 53 | { } 54 | }; 55 | } 56 | -------------------------------------------------------------------------------- /svo_online_loopclosing/include/svo/online_loopclosing/read_file.h: -------------------------------------------------------------------------------- 1 | /* 2 | * read_file.h 3 | * 4 | * Created on: Nov 9, 2017 5 | * Author: kunal71091 6 | */ 7 | 8 | /* 9 | * For functions to read data from text files and store into variables. 10 | */ 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "svo/online_loopclosing/bow.h" 19 | #include "svo/online_loopclosing/geometric_verification.h" 20 | 21 | namespace svo 22 | { 23 | /* Function readFeatureAndLandmarkData 24 | * This function takes pathToFolder as an input along with two frameIDs and 25 | * returns vectors 26 | * of 2D features and 3D Landmarks*/ 27 | 28 | void readFeatureAndLandmarkData( 29 | const std::string& pathToFolder, const std::string& frameID1, 30 | const std::string& frameID2, std::vector* keypoints1, 31 | std::vector* keypoints2, std::vector* landmarks1, 32 | std::vector* landmarks2, std::vector* trackIDs1, 33 | std::vector* trackIDs2); 34 | 35 | /* Function readCampose 36 | * Reads Camera pose (R | T) from a text file*/ 37 | 38 | cv::Mat readCamPose(const std::string& pathToFolder, const std::string& frameID, 39 | int rows, int cols); 40 | 41 | } // namespace svo 42 | -------------------------------------------------------------------------------- /svo_online_loopclosing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_online_loopclosing 4 | 0.0.1 5 | The svo_online_loopclosing package 6 | Kunal Shrivastava 7 | kunal71091 8 | 9 | tbd 10 | 11 | catkin 12 | catkin_simple 13 | 14 | roscpp 15 | roslib 16 | svo_cmake 17 | svo_common 18 | rpg_common 19 | eigen_catkin 20 | dbow2_catkin 21 | cv_bridge 22 | svo_pgo 23 | opengv 24 | 25 | 26 | -------------------------------------------------------------------------------- /svo_online_loopclosing/test/createvoc.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * createvoc.cpp 3 | * 4 | * Use this file to create a new vocabulary. Specify the path where the images 5 | * have been stored in extractFeaturesFromFolder 6 | * function. The Vocabulary Name and other associated parameters, have to be 7 | * specified. 8 | * 9 | * Created on: Nov 8, 2017 10 | * Author: kunal71091 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | #include "svo/online_loopclosing/bow.h" 17 | 18 | using namespace std; 19 | using namespace svo; 20 | using namespace DBoW2; 21 | 22 | int main(int argc, char* argv[]) 23 | { 24 | if (argc < 2) 25 | { 26 | std::cerr << "Provide Image Directory Path and Vocabulary name" 27 | << std::endl; 28 | return 1; 29 | } 30 | string folder_location = std::string(argv[1]); 31 | // branching factor and depth levels 32 | const int k = 8; 33 | const int L = 4; 34 | const WeightingType weight = TF_IDF; 35 | const ScoringType score = BHATTACHARYYA; 36 | stringstream voc_save_path_; 37 | voc_save_path_ << ros::package::getPath("svo_online_loopclosing") << "/vocabu" 38 | "laries"; 39 | string voc_save_path = voc_save_path_.str(); 40 | 41 | string voc_name = std::string(argv[2]); 42 | vector> features; 43 | extractFeaturesFromFolder(folder_location, &features); 44 | OrbVocabulary voc = 45 | createVoc(features, voc_save_path, voc_name, k, L, weight, score); 46 | 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /svo_online_loopclosing/vocabularies/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | !download_voc.sh 4 | *.yml.gz 5 | 6 | -------------------------------------------------------------------------------- /svo_online_loopclosing/vocabularies/download_voc.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | wget http://rpg.ifi.uzh.ch/svo2/vocabularies.tar.gz -O - | tar -xz 3 | -------------------------------------------------------------------------------- /svo_pgo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_pgo) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS 10 | include/svo/pgo/pgo.h 11 | ) 12 | 13 | set(SOURCES 14 | src/pgo.cpp 15 | ) 16 | 17 | #add_executable(svo_online_loopclosing ${SOURCES} ${HEADERS}) 18 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 19 | 20 | cs_add_executable(test_pgo test/test.cpp) 21 | target_link_libraries(test_pgo ${PROJECT_NAME}) 22 | 23 | cs_install() 24 | cs_export() 25 | -------------------------------------------------------------------------------- /svo_pgo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_pgo 4 | 0.0.1 5 | The pose graph optimisation package 6 | Kunal Shrivastava 7 | kunal71091 8 | 9 | tbd 10 | 11 | catkin 12 | catkin_simple 13 | 14 | roscpp 15 | roslib 16 | svo_cmake 17 | svo_common 18 | eigen_catkin 19 | ceres_catkin 20 | cv_bridge 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /svo_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_ros) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | find_package(rostest REQUIRED) 6 | catkin_simple() 7 | find_package(OpenCV REQUIRED) 8 | 9 | set(USE_GTSAM false) 10 | include(SvoSetup) 11 | if(USE_GLOBAL_MAP OR USE_GTSAM) 12 | find_package(GTSAMCMakeTools REQUIRED) 13 | find_package(GTSAM REQUIRED) 14 | set(GTSAM_LIBRARIES "gtsam") 15 | endif() 16 | message(STATUS "GTSAM include dir: ${GTSAM_INCLUDE_DIR}") 17 | message(STATUS "GTSAM library: ${GTSAM_LIBRARIES}") 18 | include_directories(${GTSAM_INCLUDE_DIR}) 19 | 20 | 21 | set(HEADERS 22 | include/svo_ros/svo_interface.h 23 | include/svo_ros/svo_factory.h 24 | include/svo_ros/visualizer.h 25 | include/svo_ros/svo_nodelet.h 26 | include/svo_ros/csv_dataset_reader.h 27 | include/svo_ros/ceres_backend_factory.h 28 | ) 29 | 30 | set(SOURCES 31 | src/ceres_backend_factory.cpp 32 | src/svo_interface.cpp 33 | src/svo_factory.cpp 34 | src/svo_node_base.cpp 35 | src/visualizer.cpp 36 | src/csv_dataset_reader.cpp 37 | ) 38 | 39 | # not used anymore 40 | if(USE_GTSAM) 41 | ADD_DEFINITIONS(-DSVO_USE_GTSAM_BACKEND) 42 | list(APPEND HEADERS 43 | include/svo_ros/backend_visualizer.h 44 | include/svo_ros/backend_factory.h) 45 | list(APPEND SOURCES 46 | src/backend_visualizer.cpp 47 | src/backend_factory.cpp) 48 | endif() 49 | 50 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 51 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ${svo_backend_LIBRARIES} ${GTSAM_LIBRARIES}) 52 | 53 | cs_add_executable(svo_benchmark src/benchmark_node.cpp) 54 | target_link_libraries(svo_benchmark ${PROJECT_NAME}) 55 | 56 | cs_add_executable(svo_node src/svo_node.cpp) 57 | target_link_libraries(svo_node ${PROJECT_NAME}) 58 | 59 | cs_add_library(svo_nodelet src/svo_nodelet.cpp) 60 | target_link_libraries(svo_nodelet ${PROJECT_NAME}) 61 | 62 | cs_install() 63 | install(FILES svo_nodelet.xml 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 65 | ) 66 | cs_export() 67 | -------------------------------------------------------------------------------- /svo_ros/include/svo_ros/backend_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | 8 | // forward declarations 9 | class BackendInterface; 10 | 11 | namespace backend_factory { 12 | 13 | std::shared_ptr makeBackend(const ros::NodeHandle& pnh); 14 | 15 | } // namespace vin_factory 16 | } // namespace svo 17 | -------------------------------------------------------------------------------- /svo_ros/include/svo_ros/ceres_backend_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace svo { 8 | 9 | namespace ceres_backend_factory { 10 | 11 | std::shared_ptr makeBackend( 12 | const ros::NodeHandle& pnh, const CameraBundlePtr& camera_bundle); 13 | 14 | } // namespace ceres_backend_factory 15 | } // namespace svo 16 | -------------------------------------------------------------------------------- /svo_ros/include/svo_ros/svo_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace svo { 8 | 9 | // forward declarations 10 | class ImuHandler; 11 | class LoopClosing; 12 | class GlobalMap; 13 | class FrameHandlerMono; 14 | class FrameHandlerStereo; 15 | class FrameHandlerArray; 16 | class FrameHandlerDenseMono; 17 | 18 | namespace factory { 19 | 20 | /// Get IMU Handler. 21 | std::shared_ptr getImuHandler( 22 | const ros::NodeHandle& pnh); 23 | 24 | #ifdef SVO_LOOP_CLOSING 25 | /// Create loop closing module 26 | std::shared_ptr getLoopClosingModule( 27 | const ros::NodeHandle& pnh, 28 | const CameraBundlePtr& cam=nullptr); 29 | #endif 30 | 31 | #ifdef SVO_GLOBAL_MAP 32 | std::shared_ptr getGlobalMap( 33 | const ros::NodeHandle& pnh, 34 | const CameraBundlePtr& ncams = nullptr); 35 | #endif 36 | 37 | /// Factory for Mono-SVO. 38 | std::shared_ptr makeMono( 39 | const ros::NodeHandle& pnh, 40 | const CameraBundlePtr& cam = nullptr); 41 | 42 | /// Factory for Stereo-SVO. 43 | std::shared_ptr makeStereo( 44 | const ros::NodeHandle& pnh, 45 | const CameraBundlePtr& cam = nullptr); 46 | 47 | /// Factory for Camera-Array-SVO. 48 | std::shared_ptr makeArray( 49 | const ros::NodeHandle& pnh, 50 | const CameraBundlePtr& cam = nullptr); 51 | 52 | /// Factory for Camera-Array-SVO 53 | std::shared_ptr makeDenseMono( 54 | const ros::NodeHandle& pnh); 55 | 56 | } // namespace factory 57 | } // namespace mono 58 | -------------------------------------------------------------------------------- /svo_ros/include/svo_ros/svo_node_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "svo_ros/svo_interface.h" 4 | 5 | namespace svo_ros { 6 | 7 | class SvoNodeBase { 8 | public: 9 | // Initializes glog, gflags and ROS. 10 | static void initThirdParty(int argc, char **argv); 11 | 12 | SvoNodeBase(); 13 | 14 | void run(); 15 | 16 | private: 17 | ros::NodeHandle node_handle_; 18 | ros::NodeHandle private_node_handle_; 19 | svo::PipelineType type_; 20 | 21 | public: 22 | svo::SvoInterface svo_interface_; 23 | }; 24 | 25 | } // namespace svo_ros 26 | -------------------------------------------------------------------------------- /svo_ros/include/svo_ros/svo_nodelet.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | 8 | // forward declarations 9 | class SvoInterface; 10 | 11 | class SvoNodelet : public nodelet::Nodelet 12 | { 13 | public: 14 | SvoNodelet() = default; 15 | virtual ~SvoNodelet(); 16 | 17 | private: 18 | virtual void onInit(); 19 | 20 | std::unique_ptr svo_interface_; 21 | }; 22 | 23 | } // namespace svo 24 | -------------------------------------------------------------------------------- /svo_ros/launch/euroc_global_map_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /svo_ros/launch/euroc_vio_mono.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /svo_ros/launch/euroc_vio_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /svo_ros/launch/frontend/euroc_mono_frontend_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /svo_ros/launch/frontend/euroc_stereo_frontend_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /svo_ros/launch/frontend/fla_stereo_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /svo_ros/launch/frontend/run_from_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /svo_ros/launch/live_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /svo_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_ros 4 | 0.1.0 5 | 6 | Visual Odometry - ROS Nodes 7 | 8 | 9 | Christian Forster 10 | Christian Forster 11 | GPLv3 12 | 13 | catkin 14 | catkin_simple 15 | rostest 16 | 17 | cmake_modules 18 | cv_bridge 19 | eigen_catkin 20 | eigen_checks 21 | image_transport 22 | nodelet 23 | roscpp 24 | minkindr 25 | nav_msgs 26 | pcl_ros 27 | rpg_common 28 | sensor_msgs 29 | std_msgs 30 | svo 31 | svo_cmake 32 | svo_common 33 | svo_msgs 34 | svo_tracker 35 | svo_backend 36 | svo_ceres_backend 37 | svo_online_loopclosing 38 | svo_global_map 39 | tf 40 | vikit_common 41 | vikit_ros 42 | visualization_msgs 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /svo_ros/param/backend.yaml: -------------------------------------------------------------------------------- 1 | # Backend 2 | use_ceres_backend: true 3 | ceres_verbose: false 4 | ceres_marginalize: true 5 | ceres_num_iterations: 3 6 | ceres_num_imu_frames: 3 7 | ceres_max_iteration_time: -1.0 # soft limit, set to -1 to deactivate 8 | publish_marker_scale_backend: 0.5 # set to -1 to deactivate visualization of landmarks 9 | backend_only_use_corners: false 10 | 11 | # outlier rejection 12 | use_outlier_rejection: true 13 | outlier_rejection_px_threshold: 2.0 14 | 15 | # Zero Motion Detection 16 | backend_use_zero_motion_detection: true 17 | backend_zero_motion_check_n_frames: 5 18 | zero_motion_px_diff_threshold: 0.2 19 | zero_motion_ratio_moving_pixels_threshold: 0.1 20 | zero_motion_min_number_correspondences: 5 21 | zero_motion_max_features_to_check: 15 22 | zero_motion_sigma: 0.1 23 | 24 | # Loop closing 25 | ceres_remove_marginalization_term_after_loop: true 26 | ceres_recalculate_imu_terms_after_loop: true 27 | -------------------------------------------------------------------------------- /svo_ros/param/calib/25000826_fisheye_mask.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/svo_ros/param/calib/25000826_fisheye_mask.png -------------------------------------------------------------------------------- /svo_ros/param/calib/bluefox_25000826_fisheye.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 0 7 | data: [] 8 | type: none 9 | image_height: 480 10 | image_width: 752 11 | intrinsics: 12 | cols: 1 13 | data: [-2.065170e+02, 0.000000e+00, 2.207161e-03, -4.879622e-06, 1.865656e-08, 394.552874, 241.800066, 1.000330, 0.000219, 0.000257, 294.182260, 152.289570, -12.191217, 27.959546, 8.241525, -1.970397, 10.689256, 1.871609, -6.437684, 0.430037, 3.215544, 0.921484, 0.0, 1.0] 14 | rows: 24 15 | label: cam0 16 | line-delay-nanoseconds: 0 17 | type: omni 18 | mask: 25000826_fisheye_mask.png 19 | T_B_C: 20 | cols: 4 21 | rows: 4 22 | data: [ 1., 0., 0., 0.0, 23 | 0., 1., 0., 0.0, 24 | 0., 0., 1., 0.0, 25 | 0., 0., 0., 1.] 26 | label: /fisheye_ocam_model 27 | -------------------------------------------------------------------------------- /svo_ros/param/calib/davis_flyingroom.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 4 7 | data: [0.2250580451135414, 0.217092209159718, -0.05534881782056823, 0.16384768043077721] 8 | type: equidistant 9 | image_height: 260 10 | image_width: 346 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [198.71975957912073, 198.68261014084223, 163.13401862233954, 143.59070352169638] 15 | label: cam 16 | line-delay-nanoseconds: 0 17 | type: pinhole 18 | T_B_C: 19 | cols: 4 20 | rows: 4 21 | data: [0.9998587625507148, -0.004714386087811226, 0.016131630865084918, 0.0023346688143100354, 22 | 0.004662404240121532, 0.9999838221198609, 0.003258448289491574, 0.00451539719168432, 23 | -0.016146731472778172, -0.0031827758904210924, 0.9998645673291842, 0.017022713557484578, 24 | 0.0, 0.0, 0.0, 1.0] 25 | serial_no: 0 26 | calib_date: 0 27 | description: DAVIS-cam 28 | label: DAVIS-03460003 29 | 30 | imu_params: 31 | delay_imu_cam: -0.002608748303665296 32 | max_imu_delta_t: 0.01 33 | acc_max: 155.0 34 | omega_max: 34 35 | sigma_omega_c: 5.0e-4 36 | sigma_acc_c: 4.0e-2 37 | sigma_omega_bias_c: 1.2e-07 38 | sigma_acc_bias_c: 1.0e-2 39 | sigma_integration: 0.0 40 | g: 9.8082 41 | imu_rate: 1000.0 42 | 43 | imu_initialization: 44 | velocity: [0.0, 0.0, 0.0] 45 | omega_bias: [0.0, 0.0, 0.0] 46 | acc_bias: [0.0, 0.0, 0.0] 47 | velocity_sigma: 1.0 48 | omega_bias_sigma: 0.05 49 | acc_bias_sigma: 0.4 50 | 51 | # IMU initialization 52 | imu_bias/ax: 0.24937 53 | imu_bias/ay: 0.30818 54 | imu_bias/az: 2.30426 55 | imu_bias/wx: 0.01517 56 | imu_bias/wy: 0.02372 57 | imu_bias/wz: -0.00498 58 | -------------------------------------------------------------------------------- /svo_ros/param/calib/euroc_mono.yaml: -------------------------------------------------------------------------------- 1 | label: "Euroc" 2 | id: 412eab8e4058621f7036b5e765dfe812 3 | cameras: 4 | - camera: 5 | label: cam0 6 | id: 54812562fa109c40fe90b29a59dd7798 7 | line-delay-nanoseconds: 0 8 | image_height: 480 9 | image_width: 752 10 | type: pinhole 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [458.6548807207614, 457.2966964634893, 367.2158039615726, 248.37534060980727] 15 | distortion: 16 | type: radial-tangential 17 | parameters: 18 | cols: 1 19 | rows: 4 20 | data: [-0.28340811217029355, 0.07395907389290132, 0.00019359502856909603, 21 | 1.7618711454538528e-05] 22 | T_B_C: 23 | cols: 4 24 | rows: 4 25 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 26 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 27 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 28 | 0.0, 0.0, 0.0, 1.0] 29 | 30 | imu_params: 31 | delay_imu_cam: 0.0 32 | max_imu_delta_t: 0.01 33 | acc_max: 176.0 34 | omega_max: 17 35 | sigma_omega_c: 12.0e-4 36 | sigma_acc_c: 8.0e-3 37 | sigma_omega_bias_c: 0.03 38 | sigma_acc_bias_c: 0.1 39 | sigma_integration: 0.0 40 | g: 9.8082 41 | imu_rate: 800 42 | 43 | imu_initialization: 44 | velocity: [0.0, 0, 0.0] 45 | omega_bias: [0.0, 0, 0.0] 46 | acc_bias: [0.0, 0.0, 0.0] 47 | velocity_sigma: 2.0 48 | omega_bias_sigma: 0.01 49 | acc_bias_sigma: 0.1 50 | 51 | -------------------------------------------------------------------------------- /svo_ros/param/calib/minidavis_bias.yaml: -------------------------------------------------------------------------------- 1 | imu_bias/ax: 0.24937 2 | imu_bias/ay: 0.30818 3 | imu_bias/az: 2.30426 4 | 5 | imu_bias/wx: 0.01517 6 | imu_bias/wy: 0.02372 7 | imu_bias/wz: -0.00498 8 | -------------------------------------------------------------------------------- /svo_ros/param/calib/svo_test_pinhole.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 1 7 | data: [0.9320] 8 | type: fisheye 9 | image_height: 480 10 | image_width: 752 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [383.013152, 382.39248, 344.7056, 244.32688] 15 | label: cam 16 | line-delay-nanoseconds: 0 17 | type: pinhole 18 | T_B_C: 19 | cols: 4 20 | rows: 4 21 | data: [1, 0, 0, 0, 22 | 0, 1, 0, 0, 23 | 0, 0, 1, 0, 24 | 0, 0, 0, 0] 25 | label: test_camera -------------------------------------------------------------------------------- /svo_ros/param/calib/visensor_flyingroom_mono.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 4 7 | data: [0.004114762928023252, -0.028021075326115103, 0.12075408202580709, -0.10752125648784364] 8 | type: equidistant 9 | image_height: 480 10 | image_width: 752 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [470.2057531638139, 470.6258233077641, 375.0680852766383, 224.00402741395007] 15 | label: cam0 16 | line-delay-nanoseconds: 0 17 | type: pinhole 18 | T_B_C: 19 | cols: 4 20 | rows: 4 21 | data: [0.9999629595323676, 0.006975415260222964, 0.005042136969218305, 0.03751774481760668, 22 | -0.00696559962669021, 0.9999738157981529, -0.0019616676380493454, -0.00417036720012152, 23 | -0.0050556883912640975, 0.0019264734695721847, 0.9999853642503281, 0.0039631907049543, 24 | 0.0, 0.0, 0.0, 1.0] 25 | serial_no: 0 26 | calib_date: 0 27 | description: /cam0/image_raw 28 | label: VI-sensor 29 | 30 | imu_params: 31 | delay_imu_cam: 0.0 32 | max_imu_delta_t: 0.01 33 | acc_max: 176.0 34 | omega_max: 17 35 | sigma_omega_c: 0.005 36 | sigma_acc_c: 0.01 37 | sigma_omega_bias_c: 4.0e-06 38 | sigma_acc_bias_c: 0.0002 39 | sigma_integration: 0.0 40 | g: 9.8082 41 | imu_rate: 200.0 42 | 43 | imu_initialization: 44 | velocity: [0.0, 0.0, 0.0] 45 | omega_bias: [0.0, 0.0, 0.0] 46 | acc_bias: [0.0, 0.0, 0.0] 47 | velocity_sigma: 1.0 48 | omega_bias_sigma: 0.005 49 | acc_bias_sigma: 0.01 50 | -------------------------------------------------------------------------------- /svo_ros/param/fisheye.yaml: -------------------------------------------------------------------------------- 1 | max_fts: 180 2 | grid_size: 25 3 | map_scale: 5 4 | 5 | kfselect_criterion: FORWARD 6 | kfselect_numkfs_upper_thresh: 120 7 | kfselect_numkfs_lower_thresh: 40 8 | kfselect_min_dist_metric: 0.2 9 | kfselect_min_angle: 10 10 | kfselect_min_disparity: 25 11 | update_seeds_with_old_keyframes: True 12 | 13 | T_world_imuinit/qx: 1 14 | T_world_imuinit/qy: 0 15 | T_world_imuinit/qz: 0 16 | T_world_imuinit/qw: 0 17 | 18 | publish_every_nth_dense_input: 5 19 | publish_marker_scale: 0.8 20 | 21 | # fisheye specific 22 | # the distortion of fisheye is not negligible, 23 | # therefore the jacobian need to be considered 24 | img_align_use_distortion_jacobian: True 25 | # pose optimization and epipolar search should be done on unit sphere 26 | poseoptim_using_unit_sphere: True 27 | scan_epi_unit_sphere: True 28 | 29 | use_ceres_backend: False 30 | -------------------------------------------------------------------------------- /svo_ros/param/frontend_imu/euroc_mono_imu.yaml: -------------------------------------------------------------------------------- 1 | grid_size: 35 2 | use_imu: True 3 | poseoptim_prior_lambda: 0.0 4 | img_align_prior_lambda_rot: 0.5 5 | img_align_prior_lambda_trans: 0.0 6 | 7 | # If set to false, we process the next frame(s) only when the depth update is finished 8 | use_threaded_depthfilter: False 9 | # If the number of features are below this number, consider as failure 10 | quality_min_fts: 40 11 | # If the number of features reduce by this number for consecutive frames, consider as failure 12 | quality_max_drop_fts: 80 13 | 14 | map_scale: 5.0 15 | kfselect_criterion: FORWARD 16 | kfselect_numkfs_upper_thresh: 180 17 | kfselect_numkfs_lower_thresh: 90 18 | kfselect_min_dist_metric: 0.001 19 | kfselect_min_angle: 6 20 | kfselect_min_disparity: 40 21 | kfselect_min_num_frames_between_kfs: 1 22 | 23 | img_align_est_illumination_gain: true 24 | img_align_est_illumination_offset: true 25 | depth_filter_affine_est_offset: true 26 | depth_filter_affine_est_gain: true 27 | reprojector_affine_est_offset: true 28 | reprojector_affine_est_gain: true 29 | 30 | 31 | # Disable ceres backend 32 | use_ceres_backend: false 33 | -------------------------------------------------------------------------------- /svo_ros/param/frontend_imu/euroc_stereo_imu.yaml: -------------------------------------------------------------------------------- 1 | pipeline_is_stereo: True 2 | 3 | grid_size: 35 4 | use_async_reprojectors: True 5 | use_imu: False 6 | poseoptim_prior_lambda: 0.0 7 | img_align_prior_lambda_rot: 0.0 8 | img_align_prior_lambda_trans: 0.0 9 | 10 | # If set to false, we process the next frame(s) only when the depth update is finished 11 | use_threaded_depthfilter: False 12 | # if the number of features are below this number, consider as failure 13 | quality_min_fts: 40 14 | # if the number of features reduce by this number for consecutive frames, consider as failure 15 | quality_max_drop_fts: 80 16 | 17 | max_depth_inv: 0.1 18 | min_depth_inv: 10.0 19 | mean_depth_inv: 0.5 20 | 21 | map_scale: 5.0 22 | kfselect_criterion: FORWARD 23 | kfselect_numkfs_upper_thresh: 180 24 | kfselect_numkfs_lower_thresh: 90 25 | kfselect_min_dist_metric: 0.001 26 | kfselect_min_angle: 6 27 | kfselect_min_disparity: 40 28 | kfselect_min_num_frames_between_kfs: 0 29 | 30 | img_align_est_illumination_gain: true 31 | img_align_est_illumination_offset: true 32 | depth_filter_affine_est_offset: true 33 | depth_filter_affine_est_gain: true 34 | reprojector_affine_est_offset: true 35 | reprojector_affine_est_gain: true 36 | 37 | 38 | # Disable ceres backend 39 | use_ceres_backend: false 40 | -------------------------------------------------------------------------------- /svo_ros/param/frontend_imu/fla_stereo_imu.yaml: -------------------------------------------------------------------------------- 1 | # Basic 2 | pipeline_is_stereo: True 3 | automatic_reinitialization: True # When lost, stereo can recover immediately 4 | 5 | # Stereo triangulation depth range 6 | max_depth_inv: 0.05 7 | min_depth_inv: 2.0 8 | mean_depth_inv: 0.3 9 | 10 | # IMU 11 | use_imu: True 12 | img_align_prior_lambda_rot: 5.0 # Gyroscope prior in sparse image alignment 13 | poseoptim_prior_lambda: 2.0 # Gyroscope prior in pose optimization 14 | 15 | 16 | # Keyframe selection 17 | max_n_kfs: 10 18 | kfselect_criterion: FORWARD 19 | kfselect_numkfs_upper_thresh: 160 # If it has more features, it never creates a keyframe 20 | kfselect_numkfs_lower_thresh: 70 # If it has less features, it always creates a keyframe 21 | kfselect_min_num_frames_between_kfs: 2 # ..except this. 22 | kfselect_min_disparity: 30 23 | kfselect_min_angle: 20 24 | kfselect_min_dist_metric: 0.01 25 | update_seeds_with_old_keyframes: True 26 | 27 | # Image Alignment 28 | img_align_max_level: 5 # increase this level if image is double the size of VGA 29 | img_align_min_level: 2 30 | poseoptim_thresh: 3.0 # reprojection threshold, maybe make a bit larger for larger images. 31 | 32 | # Reprojection 33 | use_async_reprojectors: True # For stereo, use multithreading in reprojector 34 | reprojector_max_n_kfs: 10 # Local map size. Larger is computationally more intensive. More reduces drift. 35 | 36 | # Feature detection 37 | max_fts: 180 38 | grid_size: 50 # Larger for larger images, for every cell you have max one feature 39 | n_pyr_levels: 4 # Increase for larger images (image align max minus one) 40 | detector_threshold_primary: 7 # Fast detector threshold 41 | detector_threshold_secondary: 250 # Edgelet detector threshold 42 | 43 | # Disable ceres backend 44 | use_ceres_backend: false 45 | -------------------------------------------------------------------------------- /svo_ros/src/svo_node.cpp: -------------------------------------------------------------------------------- 1 | #include "svo_ros/svo_node_base.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | svo_ros::SvoNodeBase::initThirdParty(argc, argv); 6 | 7 | svo_ros::SvoNodeBase node; 8 | node.run(); 9 | } 10 | -------------------------------------------------------------------------------- /svo_ros/src/svo_node_base.cpp: -------------------------------------------------------------------------------- 1 | #include "svo_ros/svo_node_base.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace svo_ros { 10 | 11 | void SvoNodeBase::initThirdParty(int argc, char **argv) 12 | { 13 | google::InitGoogleLogging(argv[0]); 14 | google::ParseCommandLineFlags(&argc, &argv, true); 15 | google::InstallFailureSignalHandler(); 16 | 17 | ros::init(argc, argv, "svo"); 18 | } 19 | 20 | SvoNodeBase::SvoNodeBase() 21 | : node_handle_(), private_node_handle_("~"), type_( 22 | vk::param(private_node_handle_, "pipeline_is_stereo", false) ? 23 | svo::PipelineType::kStereo : svo::PipelineType::kMono), 24 | svo_interface_(type_, node_handle_, private_node_handle_) 25 | { 26 | if (svo_interface_.imu_handler_) 27 | { 28 | svo_interface_.subscribeImu(); 29 | } 30 | svo_interface_.subscribeImage(); 31 | svo_interface_.subscribeRemoteKey(); 32 | } 33 | 34 | void SvoNodeBase::run() 35 | { 36 | ros::spin(); 37 | SVO_INFO_STREAM("SVO quit"); 38 | svo_interface_.quit_ = true; 39 | SVO_INFO_STREAM("SVO terminated.\n"); 40 | } 41 | 42 | } // namespace svo_ros 43 | -------------------------------------------------------------------------------- /svo_ros/src/svo_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | PLUGINLIB_EXPORT_CLASS(svo::SvoNodelet, nodelet::Nodelet) 8 | 9 | namespace svo { 10 | 11 | SvoNodelet::~SvoNodelet() 12 | { 13 | NODELET_INFO_STREAM("SVO quit"); 14 | svo_interface_->quit_ = true; 15 | } 16 | 17 | void SvoNodelet::onInit() 18 | { 19 | ros::NodeHandle nh(getNodeHandle()); 20 | ros::NodeHandle pnh(getPrivateNodeHandle()); 21 | 22 | NODELET_INFO_STREAM("Initialized " << getName() << " nodelet."); 23 | svo::PipelineType type = svo::PipelineType::kMono; 24 | if(vk::param(pnh, "pipeline_is_stereo", false)) 25 | type = svo::PipelineType::kStereo; 26 | 27 | svo_interface_.reset(new SvoInterface(type, nh, pnh)); 28 | if(svo_interface_->imu_handler_) 29 | svo_interface_->subscribeImu(); 30 | svo_interface_->subscribeImage(); 31 | svo_interface_->subscribeRemoteKey(); 32 | } 33 | 34 | } // namespace svo 35 | -------------------------------------------------------------------------------- /svo_ros/svo_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A nodelet wrapper for SVO. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /svo_test_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_test_utils) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | include(SvoSetup) 8 | 9 | ################################################################################ 10 | set(HEADERS 11 | include/svo/test_utils/test_utils.h 12 | include/svo/test_utils/imu_simulation.h 13 | include/svo/test_utils/synthetic_dataset.h 14 | include/svo/test_utils/simple_raytracer.h 15 | include/svo/test_utils/trajectory_generator.h 16 | include/svo/test_utils/point_cloud_generator.h 17 | ) 18 | 19 | set(SOURCES 20 | src/test_utils.cpp 21 | src/imu_simulation.cpp 22 | src/synthetic_dataset.cpp 23 | src/simple_raytracer.cpp 24 | src/trajectory_generator.cpp 25 | src/point_cloud_generator.cpp 26 | ) 27 | 28 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 29 | 30 | ################################################################################ 31 | cs_install() 32 | cs_export() 33 | -------------------------------------------------------------------------------- /svo_test_utils/include/svo/test_utils/imu_simulation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | namespace simulation { 8 | 9 | void simulateBiases( 10 | const size_t n_measurements, 11 | const double dt, 12 | const ImuCalibration& imu_calib, 13 | const Eigen::Vector3d& bias_gyr_init, 14 | const Eigen::Vector3d& bias_acc_init, 15 | Eigen::Matrix3Xd* bias_gyr, 16 | Eigen::Matrix3Xd* bias_acc); 17 | 18 | 19 | } // namespace simulation 20 | } // namespace svo 21 | -------------------------------------------------------------------------------- /svo_test_utils/include/svo/test_utils/point_cloud_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace svo { 6 | namespace test_utils { 7 | 8 | /// Point Cloud Generators 9 | class PCG 10 | { 11 | public: 12 | typedef std::shared_ptr Ptr; 13 | PCG() {} 14 | virtual std::vector generatePointCloud(size_t num_points) = 0; 15 | }; 16 | 17 | class UniformRandomPCG : public PCG 18 | { 19 | public: 20 | UniformRandomPCG(double min_depth, double max_depth) 21 | : PCG() 22 | , min_depth_(min_depth) 23 | , max_depth_(max_depth) {} 24 | 25 | virtual std::vector generatePointCloud(size_t num_points); 26 | 27 | private: 28 | double min_depth_; 29 | double max_depth_; 30 | }; 31 | 32 | class GaussianRandomPCG : public PCG 33 | { 34 | public: 35 | GaussianRandomPCG(double mean_depth, double std_depth) 36 | : PCG() 37 | , mean_depth_(mean_depth) 38 | , std_depth_(std_depth) {} 39 | 40 | virtual std::vector generatePointCloud(size_t num_points); 41 | 42 | private: 43 | double mean_depth_; 44 | double std_depth_; 45 | }; 46 | 47 | class FromFilePCG : public PCG 48 | { 49 | public: 50 | FromFilePCG(const std::string& filename); 51 | virtual std::vector generatePointCloud(size_t /*num_points*/); 52 | 53 | private: 54 | std::vector points_; 55 | }; 56 | 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /svo_test_utils/include/svo/test_utils/synthetic_dataset.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace svo { 13 | namespace test_utils { 14 | 15 | /// Utility class to load synthetic datasets in tests. 16 | class SyntheticDataset 17 | { 18 | public: 19 | 20 | SyntheticDataset( 21 | const std::string& dataset_dir, 22 | size_t cam_index, 23 | size_t first_frame_id, 24 | double sigma_img_noise = 0.0); 25 | 26 | ~SyntheticDataset() = default; 27 | 28 | const CameraPtr& cam() const { return cam_; } 29 | const CameraBundlePtr& ncam() const { return ncam_; } 30 | 31 | bool getNextFrame( 32 | size_t n_pyramid_levels, 33 | FramePtr& frame, 34 | cv::Mat* depthmap); 35 | 36 | bool skipNImages(size_t n); 37 | 38 | private: 39 | 40 | void init(); 41 | void skipFrames(size_t first_frame_id); 42 | 43 | // dataset dir 44 | std::string dataset_dir_; 45 | 46 | // which camera and frames to test 47 | size_t cam_index_; 48 | size_t first_frame_id_; 49 | 50 | // cameras 51 | CameraBundlePtr ncam_; 52 | CameraPtr cam_; 53 | 54 | // read image sequence 55 | std::ifstream img_fs_; 56 | std::ifstream gt_fs_; 57 | std::ifstream depth_fs_; 58 | 59 | // params 60 | double sigma_img_noise_; 61 | }; 62 | 63 | } // namespace test_utils 64 | } // namespace svo 65 | -------------------------------------------------------------------------------- /svo_test_utils/include/svo/test_utils/test_utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | 6 | #pragma once 7 | 8 | #include // for getenv rand 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | # include 16 | 17 | #include 18 | 19 | 20 | #define SVO_TEST_STREAM(x) {std::cerr<<"\033[0;0m[ ] * "<& v); 39 | 40 | FrameBundle::Ptr createFrameBundle( 41 | CameraPtr cam, 42 | const Transformation& T_w_f, 43 | const Transformation& T_f_b); 44 | 45 | Eigen::Vector3d generateRandomPoint(double max_depth, double min_depth); 46 | 47 | void calcHist(const std::vector& values, size_t bins, std::vector* hist); 48 | 49 | // TODO(zzc): auto zoom for the text 50 | cv::Mat drawHist( 51 | const std::vector& hist, 52 | const std::vector& bounds, 53 | int width, int height); 54 | 55 | } // namespace test_utils 56 | } // namespace svo 57 | -------------------------------------------------------------------------------- /svo_test_utils/include/svo/test_utils/trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace svo { 8 | namespace trajectory { 9 | 10 | class Trajectory 11 | { 12 | public: 13 | typedef std::pair StampedPose; 14 | 15 | static Trajectory loadfromFile(const std::string& filename); 16 | static Trajectory createCircularTrajectory(size_t length, double fps, const Eigen::Vector3d& center, double radius); 17 | static Trajectory createStraightTrajectory(size_t length, double fps, const Eigen::Vector3d& start, const Eigen::Vector3d& end); 18 | 19 | Trajectory() {} 20 | Trajectory(const std::vector& poses) 21 | : current_idx_(0) 22 | , trajectory_(poses) {} 23 | 24 | StampedPose getStampedPoseAtIdx(size_t requested_idx) const 25 | { 26 | return trajectory_[requested_idx]; 27 | } 28 | 29 | StampedPose getNextStampedPose() const 30 | { 31 | return trajectory_[current_idx_++]; 32 | } 33 | 34 | size_t length() const { return trajectory_.size(); } 35 | void seekPose(size_t idx) const { current_idx_ = idx; } 36 | void reset() const { current_idx_ = 0; } 37 | 38 | private: 39 | mutable size_t current_idx_; 40 | std::vector trajectory_; 41 | }; 42 | 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /svo_test_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_test_utils 4 | 5 | Utility functions useful for tests and experiments. 6 | 7 | 0.0.1 8 | tbd 9 | 10 | Christian Forster 11 | Manuel Werlberger 12 | Christian Forster 13 | 14 | catkin 15 | catkin_simple 16 | 17 | cv_bridge 18 | eigen_catkin 19 | glog_catkin 20 | roslib 21 | svo_common 22 | vikit_ros 23 | vikit_common 24 | vikit_cameras 25 | 26 | gtest 27 | 28 | -------------------------------------------------------------------------------- /svo_test_utils/src/imu_simulation.cpp: -------------------------------------------------------------------------------- 1 | // to cope with the warning from the distribution generator 2 | // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59800 3 | #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace svo { 15 | namespace simulation { 16 | 17 | namespace internal { 18 | 19 | void simulateBias( 20 | const size_t n_measurements, 21 | const double dt, 22 | const double sigma_continuous, 23 | const Eigen::Vector3d& bias_init, 24 | Eigen::Matrix3Xd* biases) 25 | { 26 | CHECK_NOTNULL(biases); 27 | const uint8_t seed = std::chrono::system_clock::now().time_since_epoch().count(); 28 | std::mt19937 gen(seed); 29 | std::normal_distribution normal_dist(0.0, 1.0); 30 | const double sqrt_dt = std::sqrt(dt); 31 | biases->resize(Eigen::NoChange, n_measurements); 32 | biases->col(0) = bias_init; 33 | for(size_t i = 1; i < n_measurements; ++i) 34 | { 35 | const Eigen::Vector3d N(normal_dist(gen), normal_dist(gen), normal_dist(gen)); 36 | biases->col(i) = biases->col(i-1) + N * sigma_continuous * sqrt_dt; 37 | } 38 | } 39 | 40 | } // namespace internal 41 | 42 | void simulateBiases( 43 | const size_t n_measurements, 44 | const double dt, 45 | const ImuCalibration& imu_calib, 46 | const Eigen::Vector3d& bias_gyr_init, 47 | const Eigen::Vector3d& bias_acc_init, 48 | Eigen::Matrix3Xd* bias_gyr, 49 | Eigen::Matrix3Xd* bias_acc) 50 | { 51 | CHECK_NOTNULL(bias_gyr); 52 | CHECK_NOTNULL(bias_acc); 53 | internal::simulateBias(n_measurements, dt, imu_calib.gyro_bias_random_walk_sigma, 54 | bias_gyr_init, bias_gyr); 55 | internal::simulateBias(n_measurements, dt, imu_calib.acc_bias_random_walk_sigma, 56 | bias_acc_init, bias_acc); 57 | } 58 | 59 | } // namespace simulation 60 | } // namespace svo 61 | -------------------------------------------------------------------------------- /svo_test_utils/src/point_cloud_generator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace svo { 7 | namespace test_utils { 8 | 9 | using namespace Eigen; 10 | 11 | std::vector UniformRandomPCG::generatePointCloud(size_t num_points) 12 | { 13 | std::vector point_cloud; 14 | for(size_t i=0;i GaussianRandomPCG::generatePointCloud(size_t num_points) 23 | { 24 | std::vector point_cloud; 25 | for(size_t i=0;i= 0.0 ? rnd_depth : 0.0; 32 | point_cloud.push_back(depth * point); 33 | } 34 | return point_cloud; 35 | } 36 | 37 | FromFilePCG::FromFilePCG(const std::string& filename) 38 | : PCG() 39 | { 40 | std::ifstream ss; 41 | ss.open(filename); 42 | if(!ss.is_open()) 43 | LOG(FATAL) << "Could not open point cloud file: " << filename; 44 | 45 | double x, y, z; 46 | while(ss >> x >> y >> z) { 47 | points_.push_back(Vector3d(x, y, z)); 48 | } 49 | } 50 | 51 | std::vector FromFilePCG::generatePointCloud(size_t /*num_points*/) 52 | { 53 | std::random_shuffle(points_.begin(), points_.end()); 54 | return points_; 55 | } 56 | 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /svo_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_tracker) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS 10 | include/svo/tracker/feature_tracking_types.h 11 | include/svo/tracker/feature_tracking_utils.h 12 | include/svo/tracker/feature_tracking_viz.h 13 | include/svo/tracker/feature_tracker.h 14 | ) 15 | 16 | set(SOURCES 17 | src/feature_tracking_types.cpp 18 | src/feature_tracking_utils.cpp 19 | src/feature_tracking_viz.cpp 20 | src/feature_tracker.cpp 21 | ) 22 | 23 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 24 | 25 | ################################################################################ 26 | cs_install() 27 | cs_export() 28 | 29 | -------------------------------------------------------------------------------- /svo_tracker/include/svo/tracker/feature_tracking_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace svo { 8 | 9 | namespace feature_tracking_utils { 10 | 11 | /// pivot_ration needs to be in range(0,1) and if 0.5 it returns the median. 12 | double getTracksDisparityPercentile( 13 | const FeatureTracks& tracks, 14 | double pivot_ratio); 15 | 16 | /// Loops through TrackIds and checks if two frames have tracks in common. 17 | void getFeatureMatches( 18 | const Frame& frame1, const Frame& frame2, 19 | std::vector>* matches_12); 20 | 21 | } // namespace feature_tracking_utils 22 | } // namespace svo 23 | -------------------------------------------------------------------------------- /svo_tracker/include/svo/tracker/feature_tracking_viz.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace svo { 6 | 7 | // Forward declarations 8 | class FeatureTracker; 9 | 10 | void visualizeTracks( 11 | const FeatureTracker& tracker, size_t frame_index, int sleep); 12 | 13 | } // namespace svo 14 | -------------------------------------------------------------------------------- /svo_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_tracker 4 | 5 | SVO tracking module. 6 | 7 | 0.0.1 8 | tbd 9 | 10 | Christian Forster 11 | Christian Forster 12 | 13 | catkin 14 | catkin_simple 15 | 16 | roslib 17 | glog_catkin 18 | cv_bridge 19 | eigen_catkin 20 | svo_common 21 | svo_direct 22 | svo_test_utils 23 | 24 | gtest 25 | 26 | -------------------------------------------------------------------------------- /svo_tracker/src/feature_tracking_types.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace svo { 5 | 6 | // ----------------------------------------------------------------------------- 7 | FeatureRef::FeatureRef( 8 | const FrameBundlePtr& frame_bundle, size_t frame_index, size_t feature_index) 9 | : frame_bundle_(frame_bundle) 10 | , frame_index_(frame_index) 11 | , feature_index_(feature_index) 12 | { 13 | CHECK_LT(frame_index_, frame_bundle_->size()); 14 | } 15 | 16 | const Eigen::Block FeatureRef::getPx() const 17 | { 18 | return frame_bundle_->at(frame_index_)->px_vec_.block<2,1>(0, feature_index_); 19 | } 20 | 21 | const Eigen::Block FeatureRef::getBearing() const 22 | { 23 | return frame_bundle_->at(frame_index_)->f_vec_.block<3,1>(0, feature_index_); 24 | } 25 | 26 | const FramePtr FeatureRef::getFrame() const 27 | { 28 | return frame_bundle_->at(frame_index_); 29 | } 30 | 31 | // ----------------------------------------------------------------------------- 32 | FeatureTrack::FeatureTrack(int track_id) 33 | : track_id_(track_id) 34 | { 35 | feature_track_.reserve(10); 36 | } 37 | 38 | double FeatureTrack::getDisparity() const 39 | { 40 | return (front().getPx() - back().getPx()).norm(); 41 | } 42 | 43 | } // namespace svo 44 | -------------------------------------------------------------------------------- /svo_tracker/src/feature_tracking_viz.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace svo { 10 | 11 | void visualizeTracks( 12 | const FeatureTracker& tracker, size_t frame_index, int sleep) 13 | { 14 | const FeatureTracks tracks = tracker.getActiveTracks(frame_index); 15 | if(tracks.empty()) 16 | { 17 | VLOG(1) << "No features to visualize."; 18 | return; 19 | } 20 | VLOG(5) << "Tracker: Visualize " << tracks.size() << " tracks."; 21 | 22 | cv::Mat img_8u = tracks.at(0).back().getFrame()->img(); 23 | cv::Mat img_rgb(img_8u.size(), CV_8UC3); 24 | cv::cvtColor(img_8u, img_rgb, cv::COLOR_GRAY2RGB); 25 | int frame_id = tracks.at(0).back().getFrame()->id(); 26 | for(size_t i = 0; i < tracks.size(); ++i) 27 | { 28 | const FeatureTrack& track = tracks.at(i); 29 | CHECK_EQ(frame_id, track.back().getFrame()->id()); 30 | cv::line( 31 | img_rgb, 32 | cv::Point2f(track.front().getPx()(0), track.front().getPx()(1)), 33 | cv::Point2f(track.back().getPx()(0), track.back().getPx()(1)), 34 | cv::Scalar(0,255,0), 2); 35 | } 36 | cv::imshow("tracking result", img_rgb); 37 | cv::waitKey(sleep); 38 | } 39 | 40 | } // namespace svo 41 | -------------------------------------------------------------------------------- /svo_vio_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(svo_vio_common) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | list(APPEND catkin_LIBRARIES) 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | set(HEADERS 13 | include/svo/vio_common/logging.hpp 14 | include/svo/vio_common/matrix_operations.hpp 15 | include/svo/vio_common/matrix.hpp 16 | include/svo/vio_common/test_utils.hpp 17 | include/svo/vio_common/backend_types.hpp 18 | ) 19 | 20 | set(SOURCES 21 | src/vio_common.cpp 22 | src/test_utils.cpp 23 | ) 24 | 25 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 26 | 27 | 28 | ########## 29 | # GTESTS # 30 | ########## 31 | 32 | 33 | ########## 34 | # EXPORT # 35 | ########## 36 | cs_install() 37 | cs_export() 38 | -------------------------------------------------------------------------------- /svo_vio_common/include/svo/vio_common/logging.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved 2 | // Unauthorized copying of this file, via any medium is strictly prohibited 3 | // Proprietary and confidential 4 | 5 | #pragma once 6 | 7 | #pragma diagnostic push 8 | #pragma GCC diagnostic ignored "-Wunused-local-typedefs" 9 | // glog has an unused typedef. 10 | // https://github.com/google/glog/pull/33 11 | #include 12 | #pragma diagnostic pop 13 | 14 | //! @file logging.hpp 15 | //! Includes Glog framework and defines macros for DEBUG_CHECK_* which 16 | //! can be compiled away. 17 | 18 | #define DEBUG_CHECK(val) CHECK(val) 19 | #define DEBUG_CHECK_NOTNULL(val) CHECK_NOTNULL(val) 20 | #define DEBUG_CHECK_EQ(val1, val2) CHECK_EQ(val1, val2) 21 | #define DEBUG_CHECK_NE(val1, val2) CHECK_NE(val1, val2) 22 | #define DEBUG_CHECK_LE(val1, val2) CHECK_LE(val1, val2) 23 | #define DEBUG_CHECK_LT(val1, val2) CHECK_LT(val1, val2) 24 | #define DEBUG_CHECK_GE(val1, val2) CHECK_GE(val1, val2) 25 | #define DEBUG_CHECK_GT(val1, val2) CHECK_GT(val1, val2) 26 | #define DEBUG_CHECK_DOUBLE_EQ(val1, val2) CHECK_DOUBLE_EQ(val1, val2) 27 | #define DEBUG_CHECK_NEAR(val1, val2, margin) CHECK_NEAR(val1, val2, margin) 28 | -------------------------------------------------------------------------------- /svo_vio_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_vio_common 4 | 0.1.4 5 | 6 | Common types and utilities for ceres backend. 7 | 8 | Jonathan Huber 9 | ZE 10 | 11 | catkin 12 | catkin_simple 13 | 14 | eigen_catkin 15 | glog_catkin 16 | gflags_catkin 17 | minkindr 18 | geometry_msgs 19 | std_msgs 20 | svo_common 21 | 22 | 23 | gtest 24 | 25 | -------------------------------------------------------------------------------- /svo_vio_common/src/vio_common.cpp: -------------------------------------------------------------------------------- 1 | #include "svo/vio_common/logging.hpp" 2 | #include "svo/vio_common/matrix.hpp" 3 | #include "svo/vio_common/matrix_operations.hpp" 4 | #include "svo/vio_common/backend_types.hpp" 5 | -------------------------------------------------------------------------------- /vikit/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2011-2013, Christian Forster and others. 2 | All rights reserved. 3 | 4 | Unlike otherwise stated in source files, the code in this repository is 5 | published under the Revised BSD (New BSD) license. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Robotics and Perception Group, UZH Zurich, nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /vikit/README.md: -------------------------------------------------------------------------------- 1 | ### VIKIT 2 | 3 | Vikit (Vision-Kit) provides some tools for your vision/robotics project. 4 | Far from stable. -------------------------------------------------------------------------------- /vikit/vikit_cameras/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(vikit_cameras) 2 | cmake_minimum_required (VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS 10 | include/vikit/cameras.h 11 | include/vikit/cameras/camera_geometry_base.h 12 | include/vikit/cameras/implementation/camera_geometry_base.hpp 13 | include/vikit/cameras/no_distortion.h 14 | include/vikit/cameras/atan_distortion.h 15 | include/vikit/cameras/equidistant_distortion.h 16 | include/vikit/cameras/equidistant_fisheye_geometry.h 17 | include/vikit/cameras/equidistant_fisheye_projection.h 18 | include/vikit/cameras/omni_geometry.h 19 | include/vikit/cameras/omni_projection.h 20 | include/vikit/cameras/radial_tangential_distortion.h 21 | include/vikit/cameras/pinhole_projection.h 22 | include/vikit/cameras/implementation/pinhole_projection.hpp 23 | include/vikit/cameras/camera_geometry.h 24 | include/vikit/cameras/implementation/camera_geometry.hpp 25 | include/vikit/cameras/camera_factory.h 26 | include/vikit/cameras/ncamera.h 27 | include/vikit/cameras/yaml/camera-yaml-serialization.h 28 | include/vikit/cameras/yaml/ncamera-yaml-serialization.h 29 | ) 30 | 31 | set(SOURCES 32 | src/camera_factory.cpp 33 | src/ncamera.cpp 34 | src/camera_geometry_base.cpp 35 | src/camera_yaml_serialization.cpp 36 | src/equidistant_fisheye_geometry.cpp 37 | src/equidistant_fisheye_projection.cpp 38 | src/omni_geometry.cpp 39 | src/omni_projection.cpp 40 | src/ncamera_yaml_serialization.cpp 41 | ) 42 | 43 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 44 | target_link_libraries(${PROJECT_NAME} yaml-cpp) 45 | 46 | ################################################################################ 47 | # Tests 48 | 49 | catkin_add_gtest(${PROJECT_NAME}_tests 50 | test/test_cameras 51 | ) 52 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 53 | 54 | ################################################################################ 55 | # Install 56 | 57 | cs_install() 58 | cs_export() 59 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // geometry 4 | /* 5 | #include 6 | #include 7 | */ 8 | 9 | // distortion models 10 | /* 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // projections 17 | #include 18 | */ 19 | 20 | namespace vk { 21 | namespace cameras { 22 | 23 | template 24 | class CameraGeometry; 25 | 26 | template 27 | class PinholeProjection; 28 | 29 | class AtanDistortion; 30 | class EquidistantDistortion; 31 | class NoDistortion; 32 | class RadialTangentialDistortion; 33 | 34 | typedef CameraGeometry> PinholeGeometry; 35 | typedef CameraGeometry> PinholeAtanGeometry; 36 | typedef CameraGeometry> 37 | PinholeEquidistantGeometry; 38 | typedef CameraGeometry> 39 | PinholeRadTanGeometry; 40 | class OmniGeometry; 41 | } // namespace cameras 42 | } // namespace vk 43 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/camera_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace vk { 7 | namespace cameras { 8 | namespace factory { 9 | 10 | CameraGeometryBase::Ptr makePinholeCamera( 11 | const Eigen::VectorXd& intrinsics, uint32_t width, uint32_t height); 12 | 13 | CameraGeometryBase::Ptr loadFromYAML( 14 | const std::string& filename, 15 | const std::string& cam_name); 16 | 17 | } // namespace factory 18 | } // namespace cameras 19 | } // namespace vk 20 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/camera_geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace vk { 9 | namespace cameras { 10 | 11 | template 12 | class CameraGeometry : public CameraGeometryBase 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | typedef Projection projection_t; 17 | 18 | CameraGeometry( 19 | const int width, 20 | const int height, 21 | const projection_t& projection); 22 | 23 | virtual ~CameraGeometry() = default; 24 | 25 | virtual bool backProject3( 26 | const Eigen::Ref& keypoint, 27 | Eigen::Vector3d* out_point_3d) const override; 28 | 29 | virtual const ProjectionResult project3( 30 | const Eigen::Ref& point_3d, 31 | Eigen::Vector2d* out_keypoint, 32 | Eigen::Matrix* out_jacobian_point) const override; 33 | 34 | virtual void printParameters(std::ostream& out, const std::string& s = "Camera: ") const override; 35 | 36 | virtual Eigen::VectorXd getIntrinsicParameters() const override; 37 | 38 | virtual Eigen::VectorXd getDistortionParameters() const override; 39 | 40 | virtual double errorMultiplier() const override; 41 | 42 | virtual double getAngleError(double img_err) const override; 43 | 44 | static std::shared_ptr> createTestCamera(); 45 | 46 | template 47 | const T* projection() const; 48 | 49 | private: 50 | projection_t projection_; 51 | }; 52 | 53 | } // namespace cameras 54 | } // namespace vk 55 | 56 | #include 57 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/equidistant_fisheye_geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "vikit/cameras/camera_geometry.h" 4 | #include "vikit/cameras/equidistant_fisheye_projection.h" 5 | 6 | namespace vk { 7 | namespace cameras { 8 | 9 | // Equidistant fisheye geometry is strongly typed to enforce mask. 10 | class EquidistantFisheyeGeometry : 11 | public CameraGeometry 12 | { 13 | public: 14 | EquidistantFisheyeGeometry( 15 | const int width, const int height, const double focal_length, 16 | const Eigen::Vector2d& principal_point, const double radius); 17 | 18 | EquidistantFisheyeGeometry( 19 | const int width, const int height, 20 | const EquidistantFisheyeProjection& projection, const double radius); 21 | 22 | virtual ~EquidistantFisheyeGeometry() = default; 23 | }; 24 | 25 | } // namespace cameras 26 | } // namespace vk 27 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/equidistant_fisheye_projection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace vk { 8 | namespace cameras { 9 | 10 | // May later be generalized to any distortion once a use case exists. 11 | class EquidistantFisheyeProjection 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | const CameraGeometryBase::Type cam_type_ = CameraGeometryBase::Type::kEqFisheye; 16 | EquidistantFisheyeProjection( 17 | double focal_length, Eigen::Vector2d principal_point); 18 | 19 | bool backProject3( 20 | const Eigen::Ref& keypoint, 21 | Eigen::Vector3d* out_bearing_vector) const; 22 | 23 | void project3( 24 | const Eigen::Ref& point_3d, 25 | Eigen::Vector2d* out_keypoint, 26 | Eigen::Matrix* out_jacobian_point) const; 27 | 28 | double errorMultiplier() const; 29 | 30 | double getAngleError(double img_err) const; 31 | 32 | void print(std::ostream& out) const; 33 | 34 | enum IntrinsicParameters 35 | { 36 | kFocalLength, 37 | kPrincipalPointX, 38 | kPrincipalPointY 39 | }; 40 | // returns the intrinsic values as vector 41 | // [focal_length, cx, cy] 42 | Eigen::VectorXd getIntrinsicParameters() const; 43 | 44 | // Returns the distortion parameters 45 | Eigen::VectorXd getDistortionParameters() const; 46 | 47 | inline const Eigen::Vector2d& principal_point() const { 48 | return principal_point_; 49 | } 50 | 51 | private: 52 | const double focal_length_; 53 | const Eigen::Vector2d principal_point_; 54 | }; 55 | 56 | } // namespace cameras 57 | } // namespace vk 58 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/implementation/camera_geometry_base.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace vk { 4 | namespace cameras { 5 | 6 | template 7 | bool CameraGeometryBase::isKeypointVisible( 8 | const Eigen::MatrixBase& keypoint) const { 9 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedKeyPoint, 2, 1); 10 | typedef typename DerivedKeyPoint::Scalar Scalar; 11 | return keypoint[0] >= static_cast(0.0) 12 | && keypoint[1] >= static_cast(0.0) 13 | && keypoint[0] < static_cast(imageWidth()) 14 | && keypoint[1] < static_cast(imageHeight()); 15 | } 16 | 17 | template 18 | bool CameraGeometryBase::isKeypointVisibleWithMargin( 19 | const Eigen::MatrixBase& keypoint, 20 | typename DerivedKeyPoint::Scalar margin) const { 21 | typedef typename DerivedKeyPoint::Scalar Scalar; 22 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedKeyPoint, 2, 1); 23 | CHECK_LT(2 * margin, static_cast(imageWidth())); 24 | CHECK_LT(2 * margin, static_cast(imageHeight())); 25 | return keypoint[0] >= margin 26 | && keypoint[1] >= margin 27 | && keypoint[0] < (static_cast(imageWidth()) - margin) 28 | && keypoint[1] < (static_cast(imageHeight()) - margin); 29 | } 30 | 31 | } // namespace cameras 32 | } // namespace vk 33 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/no_distortion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace vk { 8 | namespace cameras { 9 | 10 | class NoDistortion 11 | { 12 | public: 13 | NoDistortion() = default; 14 | ~NoDistortion() = default; 15 | 16 | inline void distort( 17 | double& /*u*/, double& /*v*/) const 18 | {} 19 | 20 | inline Eigen::Vector2d distort(const Eigen::Vector2d& vector) const { 21 | return vector; 22 | } 23 | 24 | inline Eigen::Matrix2d jacobian(const Eigen::Vector2d& /*vector*/) const { 25 | return Eigen::Matrix2d::Identity(); 26 | } 27 | 28 | inline void undistort( 29 | double& /*u*/, double& /*v*/) const 30 | {} 31 | 32 | inline void print(std::ostream& out) const 33 | { 34 | out << " Distortion: No" << std::endl; 35 | } 36 | 37 | // returns distortion parameters as vector (here empty vector) 38 | inline Eigen::VectorXd getDistortionParameters() const 39 | { 40 | Eigen::VectorXd distortion; 41 | return distortion; 42 | } 43 | 44 | static inline NoDistortion createTestDistortion() { 45 | return NoDistortion(); 46 | } 47 | 48 | 49 | }; 50 | 51 | 52 | } // namespace cameras 53 | } // namespace vk 54 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/omni_geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "vikit/cameras/camera_geometry.h" 4 | #include "vikit/cameras/omni_projection.h" 5 | 6 | namespace vk { 7 | namespace cameras { 8 | 9 | // Omni geometry is strongly typed to enforce mask. 10 | class OmniGeometry : public CameraGeometry 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | constexpr static size_t kParamNum = 24; 15 | OmniGeometry(const int width, const int height, 16 | const Eigen::Matrix& polynomial, 17 | const Eigen::Vector2d& principal_point, 18 | const Eigen::Vector3d& distortion, 19 | const Eigen::Matrix& inverse_polynomial, 20 | const Eigen::Vector2d& mask_relative_radii); 21 | // Version which does not apply a mask. 22 | OmniGeometry(const int width, const int height, 23 | const Eigen::Matrix& polynomial, 24 | const Eigen::Vector2d& principal_point, 25 | const Eigen::Vector3d& distortion, 26 | const Eigen::Matrix& inverse_polynomial); 27 | OmniGeometry(const int width, const int height, 28 | const Eigen::VectorXd& intrinsics); 29 | 30 | virtual ~OmniGeometry() = default; 31 | 32 | private: 33 | static const Eigen::Vector2d kDontMask; 34 | }; 35 | 36 | } // namespace cameras 37 | } // namespace vk 38 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/omni_projection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace vk { 8 | namespace cameras { 9 | 10 | class OmniProjection 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | static constexpr int kInversePolynomialOrder = 12; 15 | const CameraGeometryBase::Type cam_type_ = CameraGeometryBase::Type::kOmni; 16 | 17 | // TODO(tcies) Outsource distortion to distortion class? 18 | OmniProjection(const Eigen::Matrix& polynomial, 19 | const Eigen::Vector2d& principal_point, 20 | const Eigen::Vector3d& distortion, 21 | const Eigen::Matrix& 22 | inverse_polynomial); 23 | 24 | bool backProject3( 25 | const Eigen::Ref& keypoint, 26 | Eigen::Vector3d* out_bearing_vector) const; 27 | 28 | void project3( 29 | const Eigen::Ref& point_3d, 30 | Eigen::Vector2d* out_keypoint, 31 | Eigen::Matrix* out_jacobian_point) const; 32 | 33 | double errorMultiplier() const; 34 | 35 | double getAngleError(double img_err) const; 36 | 37 | void print(std::ostream& out) const; 38 | 39 | enum IntrinsicParameters 40 | { 41 | kPolinomial0, 42 | kPolinomial1, 43 | kPolinomial2, 44 | kPolinomial3, 45 | kPolinomial4, 46 | kPrincipalPointX, 47 | kPrincipalPointY 48 | }; 49 | // Returns the parameters in Vector form: 50 | // [polynomial(0) ... polynomial(4), cx_, cy_] 51 | Eigen::VectorXd getIntrinsicParameters() const; 52 | 53 | // Returns the distortion parameters 54 | Eigen::VectorXd getDistortionParameters() const; 55 | 56 | private: 57 | const Eigen::Matrix polynomial_; 58 | const Eigen::Vector2d principal_point_; 59 | const Eigen::Matrix inverse_polynomial_; 60 | 61 | const Eigen::Matrix2d affine_correction_; 62 | const Eigen::Matrix2d affine_correction_inverse_; 63 | }; 64 | 65 | } // namespace cameras 66 | } // namespace vk 67 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/yaml/camera-yaml-serialization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 5 | #include 6 | #pragma diagnostic pop 7 | 8 | namespace vk { 9 | namespace cameras { 10 | class CameraGeometryBase; 11 | } // namespace cameras 12 | } // namespace vk 13 | 14 | namespace YAML { 15 | 16 | template<> 17 | struct convert> { 18 | /// This function will attempt to parse a camera from the yaml node. 19 | /// By default, yaml-cpp will throw and exception if the parsing fails. 20 | /// This function was written to *not* throw exceptions. Hence, decode always 21 | /// returns true, but when it fails, the shared pointer will be null. 22 | static bool decode(const Node& node, std::shared_ptr& camera); 23 | static Node encode(const std::shared_ptr& camera); 24 | }; 25 | 26 | template<> 27 | struct convert { 28 | static bool decode(const Node& node, vk::cameras::CameraGeometryBase& camera); 29 | static Node encode(const vk::cameras::CameraGeometryBase& camera); 30 | }; 31 | 32 | } // namespace YAML 33 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/include/vikit/cameras/yaml/ncamera-yaml-serialization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 5 | #include 6 | #pragma diagnostic pop 7 | 8 | namespace vk { 9 | namespace cameras { 10 | class NCamera; 11 | } // namespace cameras 12 | } // namespace vk 13 | 14 | namespace YAML { 15 | 16 | template<> 17 | struct convert> { 18 | /// This function will attempt to parse an ncamera from the yaml node. 19 | /// By default, yaml-cpp will throw an exception if the parsing fails. 20 | /// This function was written to *not* throw exceptions. Hence, decode always 21 | /// returns true, but when it fails, the shared pointer will be null. 22 | static bool decode(const Node& node, std::shared_ptr& ncamera); 23 | static Node encode(const std::shared_ptr& ncamera); 24 | }; 25 | 26 | template<> 27 | struct convert { 28 | static bool decode(const Node& node, vk::cameras::NCamera& ncamera); 29 | static Node encode(const vk::cameras::NCamera& ncamera); 30 | }; 31 | 32 | } // namespace YAML 33 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vikit_cameras 4 | 0.0.0 5 | The vikit_cameras package 6 | cforster 7 | GPLv3 8 | 9 | catkin 10 | catkin_simple 11 | 12 | eigen_catkin 13 | eigen_checks 14 | cv_bridge 15 | glog_catkin 16 | minkindr 17 | svo_cmake 18 | vikit_common 19 | yaml-cpp 20 | 21 | gtest 22 | 23 | 24 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/src/equidistant_fisheye_geometry.cpp: -------------------------------------------------------------------------------- 1 | #include "vikit/cameras/equidistant_fisheye_geometry.h" 2 | 3 | namespace vk { 4 | namespace cameras { 5 | 6 | EquidistantFisheyeGeometry::EquidistantFisheyeGeometry( 7 | const int width, const int height, const double focal_length, 8 | const Eigen::Vector2d& principal_point, const double radius) 9 | : EquidistantFisheyeGeometry(width, height, EquidistantFisheyeProjection( 10 | focal_length, principal_point), radius) 11 | {} 12 | 13 | EquidistantFisheyeGeometry::EquidistantFisheyeGeometry( 14 | const int width, const int height, 15 | const EquidistantFisheyeProjection& projection, const double radius) 16 | : CameraGeometry(width, height, projection) 17 | { 18 | cv::Mat mask = cv::Mat(imageHeight(), imageWidth(), CV_8UC1); 19 | for(uint32_t i=0; i(i) - 22 | static_cast(projection.principal_point()[1]); 23 | int64_t w_dist = static_cast(j) - 24 | static_cast(projection.principal_point()[0]); 25 | double dist_squared = std::pow(h_dist, 2) + std::pow(w_dist, 2); 26 | if (dist_squared > radius * radius) 27 | mask.at(i, j) = 0; 28 | else 29 | mask.at(i, j) = 1; 30 | } 31 | } 32 | setMask(mask); 33 | } 34 | 35 | } // namespace cameras 36 | } // namespace vk 37 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_cam.yaml: -------------------------------------------------------------------------------- 1 | distortion: 2 | parameters: 3 | cols: 1 4 | rows: 0 5 | data: [] 6 | type: none 7 | image_height: 376 8 | image_width: 1241 9 | intrinsics: 10 | cols: 1 11 | data: [7.188560000000e+02, 7.188560000000e+02, 6.071928000000e+02, 1.852157000000e+02] 12 | rows: 4 13 | label: cam0 14 | line-delay-nanoseconds: 0 15 | type: pinhole -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_kitti.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 0 7 | data: [] 8 | type: none 9 | image_height: 376 10 | image_width: 1241 11 | intrinsics: 12 | cols: 1 13 | data: [7.188560000000e+02, 7.188560000000e+02, 6.071928000000e+02, 1.852157000000e+02] 14 | rows: 4 15 | label: cam0 16 | line-delay-nanoseconds: 0 17 | type: pinhole 18 | T_B_C: 19 | cols: 4 20 | rows: 4 21 | data: [1, 0, 0, 0, 22 | 0, 1, 0, 0, 23 | 0, 0, 1, 0, 24 | 0, 0, 0, 0] 25 | - camera: 26 | distortion: 27 | parameters: 28 | cols: 1 29 | rows: 0 30 | data: [] 31 | type: none 32 | image_height: 376 33 | image_width: 1241 34 | intrinsics: 35 | cols: 1 36 | data: [7.188560000000e+02, 7.188560000000e+02, 6.071928000000e+02, 1.852157000000e+02] 37 | rows: 4 38 | label: cam1 39 | line-delay-nanoseconds: 0 40 | type: pinhole 41 | T_B_C: 42 | cols: 4 43 | rows: 4 44 | data: [1, 0, 0, 0.53716571886, 45 | 0, 1, 0, 0, 46 | 0, 0, 1, 0, 47 | 0, 0, 0, 0] 48 | 49 | label: /kitti_camera -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_omni.yaml: -------------------------------------------------------------------------------- 1 | distortion: 2 | parameters: 3 | cols: 1 4 | rows: 0 5 | data: [] 6 | type: none 7 | image_height: 480 8 | image_width: 752 9 | intrinsics: 10 | cols: 1 11 | data: [-69.6915, 0.000000e+00, 5.4772e-4, 2.1371e-5, -8.7523e-9, 320.0, 240.0, 1.0, 0.0, 0.0, 142.7468, 104.8486, 7.3973, 17.4581, 12.6308, -4.3751, 6.9093, 10.9703, -0.6053, -3.9119, -1.0675, 0.0, 0.0, 1.0] 12 | rows: 24 13 | label: cam0 14 | line-delay-nanoseconds: 0 15 | type: omni 16 | mask: test_mask.png 17 | T_B_C: 18 | cols: 4 19 | rows: 4 20 | data: [1, 0, 0, 0, 21 | 0, 1, 0, 0, 22 | 0, 0, 1, 0, 23 | 0, 0, 0, 0] 24 | 25 | -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_pinhole_atan.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_model: PinholeAtan 3 | cam_width: 752 4 | cam_height: 480 5 | cam_fx: 0.514243 6 | cam_fy: 0.802408 7 | cam_cx: 0.472366 8 | cam_cy: 0.553086 9 | cam_d0: 0.934479 10 | T_cam_body: 11 | tx: 0.0 12 | ty: 0.0 13 | tz: -0.02 14 | qx: -1.0 15 | qy: 0.0 16 | qz: 0.0 17 | qw: 0.0 -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_pinhole_equidistant.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_model: PinholeEquidistant 3 | cam_width: 752 4 | cam_height: 480 5 | cam_fx: 463.34128261574796 6 | cam_fy: 461.90887721986877 7 | cam_cx: 361.5557340721321 8 | cam_cy: 231.13558880965206 9 | cam_d0: -0.0027973061697674074 10 | cam_d1: 0.024145501123661265 11 | cam_d2: -0.04304211254137983 12 | cam_d3: 0.031185314072573474 13 | T_cam_base: 14 | tx: 0.0696284 15 | ty: -0.0143884 16 | tz: -0.00211433 17 | qx: 0.00236363 18 | qy: -0.00449622 19 | qz: -0.701244 20 | qw: 0.712903 -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_pinhole_nodistortion.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_model: PinholeNoDistortion 3 | cam_width: 752 4 | cam_height: 480 5 | cam_fx: 315.5 # approximately 100deg field of view 6 | cam_fy: 315.5 7 | cam_cx: 376.0 8 | cam_cy: 240.0 9 | T_cam_body: 10 | tx: 0.0 11 | ty: 0.0 12 | tz: 0.0 13 | qx: 0.0 14 | qy: 0.0 15 | qz: 0.0 16 | qw: 1.0 -------------------------------------------------------------------------------- /vikit/vikit_cameras/test/data/calib_pinhole_radtan.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_model: PinholeRadialTangential 3 | cam_width: 752 4 | cam_height: 480 5 | cam_fx: 465.2005090331355 6 | cam_fy: 465.4821196969644 7 | cam_cx: 407.7552059925612 8 | cam_cy: 244.36152062408814 9 | cam_d0: -0.3091674142711387 10 | cam_d1: 0.10905899434862235 11 | cam_d2: 9.527033720237582e-05 12 | cam_d3: -0.0005776582308113238 13 | T_cam_body: 14 | tx: 0.0725808 15 | ty: -0.0107534 16 | tz: -0.0101489 17 | qx: -0.00324194 18 | qy: -0.00269663 19 | qz: 0.999969 20 | qw: 0.00659526 -------------------------------------------------------------------------------- /vikit/vikit_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(vikit_common) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | find_package(OpenCV REQUIRED) 7 | 8 | include(SvoSetup) 9 | 10 | set(HEADERS 11 | include/vikit/blender_utils.h 12 | include/vikit/math_utils.h 13 | include/vikit/performance_monitor.h 14 | include/vikit/ringbuffer.h 15 | include/vikit/sample.h 16 | include/vikit/timer.h 17 | include/vikit/user_input_thread.h 18 | include/vikit/vision.h 19 | include/vikit/homography_decomp.h 20 | include/vikit/homography.h 21 | include/vikit/path_utils.h 22 | include/vikit/csv_utils.h 23 | include/aslam/common/entrypoint.h 24 | include/aslam/common/macros.h 25 | include/aslam/common/memory.h 26 | include/aslam/common/pose-types.h 27 | include/aslam/common/yaml-serialization.h 28 | include/aslam/common/yaml-serialization-eigen.h 29 | ) 30 | 31 | set(SOURCES 32 | src/homography.cpp 33 | src/math_utils.cpp 34 | src/performance_monitor.cpp 35 | src/sample.cpp 36 | src/user_input_thread.cpp 37 | src/vision.cpp 38 | ) 39 | 40 | # Create vikit shared library 41 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 42 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) 43 | ################################################################################ 44 | # Unit tests 45 | catkin_add_gtest(homography test/test_homography.cpp) 46 | target_link_libraries(homography gtest ${PROJECT_NAME}) 47 | 48 | catkin_add_gtest(math_utils test/test_math_utils.cpp) 49 | target_link_libraries(math_utils gtest ${PROJECT_NAME}) 50 | 51 | ################################################################################ 52 | # Other tests 53 | cs_add_executable(vikit_test_timer test/test_timer.cpp) 54 | target_link_libraries(vikit_test_timer ${PROJECT_NAME}) 55 | 56 | cs_add_executable(vikit_test_common test/test_common.cpp) 57 | target_link_libraries(vikit_test_common ${PROJECT_NAME}) 58 | 59 | ################################################################################ 60 | cs_install() 61 | cs_export() 62 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/aslam/common/entrypoint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define VIKIT_UNITTEST_ENTRYPOINT\ 8 | int main(int argc, char** argv) {\ 9 | ::testing::InitGoogleTest(&argc, argv);\ 10 | google::InitGoogleLogging(argv[0]);\ 11 | google::InstallFailureSignalHandler();\ 12 | ::testing::FLAGS_gtest_death_test_style = "threadsafe";\ 13 | FLAGS_alsologtostderr = true; \ 14 | FLAGS_colorlogtostderr = true; \ 15 | return RUN_ALL_TESTS();\ 16 | } 17 | 18 | // Let the Eclipse parser see the macro. 19 | #ifndef TEST 20 | #define TEST(a, b) int Test_##a##_##b() 21 | #endif 22 | 23 | #ifndef TEST_F 24 | #define TEST_F(a, b) int Test_##a##_##b() 25 | #endif 26 | 27 | #ifndef TEST_P 28 | #define TEST_P(a, b) int Test_##a##_##b() 29 | #endif 30 | 31 | #ifndef TYPED_TEST 32 | #define TYPED_TEST(a, b) int Test_##a##_##b() 33 | #endif 34 | 35 | #ifndef TYPED_TEST_P 36 | #define TYPED_TEST_P(a, b) int Test_##a##_##b() 37 | #endif 38 | 39 | #ifndef TYPED_TEST_CASE 40 | #define TYPED_TEST_CASE(a, b) int Test_##a##_##b() 41 | #endif 42 | 43 | #ifndef REGISTER_TYPED_TEST_CASE_P 44 | #define REGISTER_TYPED_TEST_CASE_P(a, ...) int Test_##a() 45 | #endif 46 | 47 | #ifndef INSTANTIATE_TYPED_TEST_CASE_P 48 | #define INSTANTIATE_TYPED_TEST_CASE_P(a, ...) int Test_##a() 49 | #endif 50 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/aslam/common/macros.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_MACROS_H_ 2 | #define ASLAM_COMMON_MACROS_H_ 3 | 4 | #include 5 | 6 | #define ASLAM_DISALLOW_EVIL_CONSTRUCTORS(TypeName) \ 7 | TypeName(const TypeName&) = delete; \ 8 | void operator=(const TypeName&) = delete 9 | 10 | #define ASLAM_POINTER_TYPEDEFS(TypeName) \ 11 | typedef std::unique_ptr UniquePtr; \ 12 | typedef std::shared_ptr Ptr; \ 13 | typedef std::shared_ptr ConstPtr 14 | 15 | /// Extract the type from an expression which wraps a type inside braces. This 16 | /// is done to protect the commas in some types. 17 | template struct ArgumentType; 18 | template struct ArgumentType { typedef U Type; }; 19 | #define GET_TYPE(TYPE) ArgumentType::Type 20 | 21 | #endif // ASLAM_COMMON_MACROS_H_ 22 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/aslam/common/memory.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_MEMORY_HELPERS_H_ 2 | #define ASLAM_MEMORY_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace aslam { 15 | 16 | template class Container, typename Type> 17 | struct Aligned { 18 | typedef Container > type; 19 | }; 20 | 21 | template 22 | struct AlignedUnorderedMap { 23 | typedef std::unordered_map, std::equal_to, 25 | Eigen::aligned_allocator > > type; 26 | }; 27 | 28 | template 29 | struct AlignedMap { 30 | typedef std::map, 32 | Eigen::aligned_allocator > > type; 33 | }; 34 | 35 | /// \brief Aligned allocator to be used like std::make_shared 36 | /// 37 | /// To be used like: 38 | /// std::shared_ptr my_ptr = aligned_shared(params); 39 | template 40 | inline std::shared_ptr aligned_shared(Arguments&&... arguments) { 41 | typedef typename std::remove_const::type TypeNonConst; 42 | return std::allocate_shared(Eigen::aligned_allocator(), 43 | std::forward(arguments)...); 44 | } 45 | 46 | } // namespace aslam 47 | 48 | #endif // ASLAM_MEMORY_HELPERS_H_ 49 | 50 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/aslam/common/pose-types.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_POSE_TYPES_H_ 2 | #define ASLAM_COMMON_POSE_TYPES_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace aslam { 13 | 14 | typedef kindr::minimal::QuatTransformation Transformation; 15 | typedef Aligned::type TransformationVector; 16 | typedef kindr::minimal::RotationQuaternion Quaternion; 17 | typedef kindr::minimal::AngleAxis AngleAxis; 18 | typedef kindr::minimal::Position Position3D; 19 | 20 | // Types used in ceres error terms, where templating for Jet types 21 | // is necessary. 22 | template 23 | using QuaternionTemplate = kindr::minimal::RotationQuaternionTemplate; 24 | 25 | template 26 | using PositionTemplate = kindr::minimal::PositionTemplate; 27 | 28 | } // namespace aslam 29 | 30 | #endif // ASLAM_COMMON_POSE_TYPES_H_ 31 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/vikit/blender_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * blender_utils.h 3 | * 4 | * Created on: Feb 13, 2014 5 | * Author: cforster 6 | */ 7 | 8 | #ifndef VIKIT_BLENDER_UTILS_H_ 9 | #define VIKIT_BLENDER_UTILS_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace vk { 20 | namespace blender_utils { 21 | 22 | void loadBlenderDepthmap( 23 | const std::string file_name, 24 | const int img_width, 25 | const int img_height, 26 | cv::Mat& z_map) 27 | { 28 | std::ifstream file_stream(file_name.c_str()); 29 | CHECK(file_stream.is_open()) << "file '" << file_name << "' could not be opened."; 30 | z_map = cv::Mat(img_height, img_width, CV_32FC1); 31 | float * img_ptr = z_map.ptr(); 32 | float depth; 33 | for(int y=0; y> depth; 38 | 39 | // blender: 40 | *img_ptr = depth; 41 | 42 | // povray 43 | // *img_ptr = depth/100.0; // depth is in [cm], we want [m] 44 | 45 | if(file_stream.peek() == '\n' && x != img_width-1 && y != img_height-1) 46 | printf("WARNING: did not read the full depthmap!\n"); 47 | } 48 | } 49 | } 50 | 51 | } // namespace blender_utils 52 | } // namespace vk 53 | 54 | #endif // VIKIT_BLENDER_UTILS_H_ 55 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/vikit/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * homography.cpp 3 | * Adaptation of PTAM-GPL HomographyInit class. 4 | * https://github.com/Oxford-PTAM/PTAM-GPL 5 | * Licence: GPLv3 6 | * Copyright 2008 Isis Innovation Limited 7 | * 8 | * Created on: Sep 2, 2012 9 | * by: cforster 10 | * 11 | * This class implements the homography decomposition of Faugeras and Lustman's 12 | * 1988 tech report. Code converted to Eigen from PTAM. 13 | * 14 | */ 15 | 16 | #ifndef VIKIT_HOMOGRAPHY_H_ 17 | #define VIKIT_HOMOGRAPHY_H_ 18 | 19 | #include 20 | #include 21 | 22 | namespace vk { 23 | 24 | using BearingVector = Eigen::Vector3d; 25 | using Bearings = Eigen::Matrix; 26 | 27 | struct Homography 28 | { 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | Eigen::Vector3d t_cur_ref; 32 | Eigen::Matrix3d R_cur_ref; 33 | Eigen::Vector3d n_cur; 34 | double score; 35 | Homography() 36 | : t_cur_ref() 37 | , R_cur_ref() 38 | , n_cur() 39 | , score(0.0) 40 | {} 41 | }; 42 | 43 | /// Estimates Homography from corresponding feature bearing vectors. 44 | /// Score of returned homography is set to the number of inliers. 45 | Homography estimateHomography( 46 | const Bearings& f_cur, 47 | const Bearings& f_ref, 48 | const double focal_length, 49 | const double reproj_error_thresh, 50 | const size_t min_num_inliers); 51 | 52 | } // namespace vk 53 | 54 | #endif // VIKIT_HOMOGRAPHY_H_ 55 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/vikit/path_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace vk { 7 | namespace path_utils { 8 | 9 | // usage: getFileDir(__FILE__) 10 | inline std::string getBaseName(const std::string& filename) 11 | { 12 | const std::string separator = "/"; 13 | std::size_t last_separator = filename.find_last_of(separator); 14 | if(last_separator == std::string::npos) 15 | { 16 | return std::string(); 17 | } 18 | return filename.substr(0, last_separator); 19 | } 20 | 21 | } // namespace path_utils 22 | } // namespace vk 23 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/vikit/performance_monitor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * performance_monitor.h 3 | * 4 | * Created on: Aug 26, 2011 5 | * Author: Christian Forster 6 | */ 7 | 8 | #ifndef VIKIT_PERFORMANCE_MONITOR_H 9 | #define VIKIT_PERFORMANCE_MONITOR_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace vk 18 | { 19 | 20 | struct LogItem 21 | { 22 | double data; 23 | bool set; 24 | }; 25 | 26 | class PerformanceMonitor 27 | { 28 | public: 29 | PerformanceMonitor(); 30 | ~PerformanceMonitor(); 31 | void init(const std::string& trace_name, const std::string& trace_dir); 32 | void addTimer(const std::string& name); 33 | void addLog(const std::string& name); 34 | void writeToFile(); 35 | void startTimer(const std::string& name); 36 | void stopTimer(const std::string& name); 37 | double getTime(const std::string& name) const; 38 | void log(const std::string& name, double data); 39 | 40 | private: 41 | std::map timers_; 42 | std::map logs_; 43 | std::string trace_name_; // 5 | #include 6 | #include 7 | 8 | namespace vk { 9 | 10 | class Sample 11 | { 12 | public: 13 | static void setTimeBasedSeed(); 14 | static int uniform(int from, int to); 15 | static double uniform(); 16 | static double gaussian(double sigma); 17 | static std::ranlux24 gen_real; 18 | static std::mt19937 gen_int; 19 | static Eigen::Vector3d randomDirection3D(); 20 | static Eigen::Vector2d randomDirection2D(); 21 | }; 22 | 23 | } // namespace vk 24 | 25 | #endif // VIKIT_SAMPLE_H_ 26 | -------------------------------------------------------------------------------- /vikit/vikit_common/include/vikit/user_input_thread.h: -------------------------------------------------------------------------------- 1 | #ifndef USER_INPUT_THREAD_H 2 | #define USER_INPUT_THREAD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace vk { 8 | 9 | /// A class that starts its own thread and listens to the console input. The 10 | /// console input can then be inquired using the getInput() function. 11 | class UserInputThread 12 | { 13 | public: 14 | UserInputThread(); 15 | ~UserInputThread(); 16 | 17 | /// Returns the latest acquired user input. Default is set to 0. 18 | /// Once this function is called, the input state is reset to the default. 19 | char getInput(); 20 | 21 | /// Stop the thread 22 | void stop(); 23 | 24 | private: 25 | 26 | /// Main loop that waits for new user input 27 | void acquireUserInput(); 28 | 29 | /// Initialize new terminal i/o settings 30 | void initTermios(int echo); 31 | 32 | /// Restore old terminal i/o settings 33 | void resetTermios(); 34 | 35 | /// Read 1 character - echo defines echo mode 36 | int getch_(int echo); 37 | 38 | /// Read 1 character without echo 39 | int getch(); 40 | 41 | /// Read 1 character with echo 42 | int getche(); 43 | 44 | bool stop_; 45 | std::thread * user_input_thread_; 46 | char input_; 47 | 48 | struct termios original_terminal_settings_; 49 | struct termios old_terminal_settings_, new_terminal_settings_; 50 | }; 51 | 52 | } // end namespace vk 53 | 54 | #endif /* USER_INPUT_THREAD_H */ 55 | -------------------------------------------------------------------------------- /vikit/vikit_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vikit_common 4 | 0.0.0 5 | The vikit_common package 6 | cforster 7 | RPG 8 | 9 | catkin 10 | catkin_simple 11 | 12 | cv_bridge 13 | eigen_catkin 14 | glog_catkin 15 | minkindr 16 | roscpp 17 | svo_cmake 18 | yaml-cpp 19 | 20 | gtest 21 | 22 | -------------------------------------------------------------------------------- /vikit/vikit_common/src/sample.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sample.cpp 3 | * 4 | * Created on: May 14, 2013 5 | * Author: cforster 6 | */ 7 | 8 | #include 9 | 10 | namespace vk { 11 | 12 | std::ranlux24 Sample::gen_real; 13 | std::mt19937 Sample::gen_int; 14 | 15 | void Sample::setTimeBasedSeed() 16 | { 17 | unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); 18 | gen_real = std::ranlux24(seed); 19 | gen_int = std::mt19937(seed); 20 | } 21 | 22 | int Sample::uniform(int from, int to) 23 | { 24 | std::uniform_int_distribution distribution(from, to); 25 | return distribution(gen_int); 26 | } 27 | 28 | double Sample::uniform() 29 | { 30 | std::uniform_real_distribution distribution(0.0, 1.0); 31 | return distribution(gen_real); 32 | } 33 | 34 | double Sample::gaussian(double stddev) 35 | { 36 | std::normal_distribution distribution(0.0, stddev); 37 | return distribution(gen_real); 38 | } 39 | 40 | Eigen::Vector3d Sample::randomDirection3D() 41 | { 42 | // equal-area projection according to: 43 | // https://math.stackexchange.com/questions/44689/how-to-find-a-random-axis-or-unit-vector-in-3d 44 | const double z = Sample::uniform()*2.0-1.0; 45 | const double t = Sample::uniform()*2.0*M_PI; 46 | const double r = std::sqrt(1.0 - z*z); 47 | const double x = r*std::cos(t); 48 | const double y = r*std::sin(t); 49 | return Eigen::Vector3d(x,y,z); 50 | } 51 | 52 | Eigen::Vector2d Sample::randomDirection2D() 53 | { 54 | const double theta = Sample::uniform()*2.0*M_PI; 55 | return Eigen::Vector2d(std::cos(theta), std::sin(theta)); 56 | } 57 | 58 | } // namespace vk 59 | -------------------------------------------------------------------------------- /vikit/vikit_common/src/user_input_thread.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * user_input_thread.cpp 3 | * 4 | * Created on: Jun 12, 2013 5 | * Author: pizzoli, cforster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace vk { 13 | 14 | UserInputThread::UserInputThread() : 15 | stop_(false), 16 | input_( (char) 0) 17 | { 18 | tcgetattr(0, &original_terminal_settings_); // save old terminal i/o settings 19 | new_terminal_settings_ = original_terminal_settings_; // make new settings same as old settings 20 | new_terminal_settings_.c_lflag &= ~ICANON; // disable buffered i/o 21 | new_terminal_settings_.c_lflag &= ~ECHO; // set echo mode 22 | new_terminal_settings_.c_cc[VMIN] = 1; //minimum of number input read. 23 | tcsetattr(0, TCSANOW, &new_terminal_settings_); // use these new terminal i/o settings now 24 | 25 | user_input_thread_ = new std::thread(&UserInputThread::acquireUserInput, this); 26 | } 27 | 28 | UserInputThread::~UserInputThread() 29 | { 30 | tcsetattr(0, TCSANOW, &original_terminal_settings_); 31 | user_input_thread_->join(); 32 | printf("UserInputThread destructed.\n"); 33 | } 34 | 35 | char UserInputThread::getInput() 36 | { 37 | char tmp = input_; 38 | input_ = (char) 0; 39 | return tmp; 40 | } 41 | 42 | void UserInputThread::stop() 43 | { 44 | stop_ = true; 45 | } 46 | 47 | void UserInputThread::acquireUserInput() 48 | { 49 | int c = 0; 50 | while(!stop_) 51 | { 52 | c = getchar(); // TODO: this is blocking, so the interruption point is not reached... 53 | if ((char)c == ' ') 54 | printf("USER INPUT: SPACE\n"); 55 | else 56 | printf("USER INPUT: %c\n", (char) c); 57 | input_ = (char) c; 58 | c = 0; 59 | 60 | // interruption point: 61 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 62 | } 63 | } 64 | 65 | } // end namespace vk 66 | 67 | 68 | -------------------------------------------------------------------------------- /vikit/vikit_common/test/test_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void testEuclideanNorm() 5 | { 6 | Eigen::Matrix A; 7 | A << 1, 2, 3, 4, 5, 8 | 2, 3, 4, 5, 6, 9 | 7, 9, 10, 11, 12; 10 | 11 | std::cout << A.colwise().norm() << std::endl << std::endl; 12 | 13 | Eigen::Matrix B; 14 | B = A.array().rowwise() / A.colwise().norm().array(); 15 | std::cout << B << std::endl; 16 | 17 | for(int i=0; i 2 | #include 3 | #include 4 | 5 | namespace { 6 | 7 | TEST(MathUtils, testProject2_double) 8 | { 9 | Eigen::Vector3d x3(2.0, 2.0, 2.0); 10 | Eigen::Vector2d x2 = vk::project2(x3); 11 | EXPECT_EQ(x2(0), 1.0); 12 | EXPECT_EQ(x2(1), 1.0); 13 | } 14 | 15 | TEST(MathUtils, testProject2_float) 16 | { 17 | Eigen::Vector3f x3(2.0, 2.0, 2.0); 18 | Eigen::Vector2f x2 = vk::project2(x3); 19 | EXPECT_EQ(x2(0), 1.0); 20 | EXPECT_EQ(x2(1), 1.0); 21 | } 22 | 23 | } // namespace 24 | 25 | VIKIT_UNITTEST_ENTRYPOINT 26 | -------------------------------------------------------------------------------- /vikit/vikit_common/test/test_timer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include // std::setprecision 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | vk::Timer t; 10 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 11 | std::cout << "1. sleep took " << t.stop() << " seconds" << std::endl; 12 | std::cout << "1. sleep took " << t.getTime() << " seconds" << std::endl; 13 | 14 | t.resume(); 15 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 16 | std::cout << "2. sleep took " << t.stop() << " seconds" << std::endl; 17 | std::cout << "2. sleep took " << t.getTime() << " seconds" << std::endl; 18 | 19 | std::cout << "total sleep took " << t.getAccumulated() << " seconds" << std::endl; 20 | 21 | std::cout << "seconds since 1.1.1970 is " 22 | << std::setprecision(15) 23 | << vk::Timer::getCurrentTime() << std::endl; 24 | 25 | std::cout << "current date is " << vk::Timer::getCurrentTimeStr() << std::endl; 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /vikit/vikit_common/test/test_triangulation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * camera_pinhole_test.cpp 3 | * 4 | * Created on: Oct 26, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace Eigen; 21 | 22 | int main(int argc, char **argv) 23 | { 24 | vk::PinholeCamera cam(752, 480, 414.5, 414.2, 348.8, 240.0); 25 | Matrix4d T_ref_cur(Matrix4d::Identity()); 26 | T_ref_cur(0,3) = 0.5; 27 | Vector2d u_cur(200, 300); 28 | Vector3d f_cur(cam.cam2world(u_cur)); 29 | double depth_cur = 2.0; 30 | Vector3d f_ref(vk::project3d(T_ref_cur*vk::unproject3d(f_cur*depth_cur))); 31 | double depth_ref = f_ref.norm(); 32 | Vector2d u_ref(cam.world2cam(f_ref)); 33 | double z_ref, z_cur; 34 | vk::depthFromTriangulationExact(T_ref_cur.topLeftCorner<3,3>(), T_ref_cur.topRightCorner<3,1>(), f_ref, f_cur, z_ref, z_cur); 35 | printf("depth = %f, triangulated depth = %f\n", depth_cur, z_cur); 36 | printf("depth = %f, triangulated depth = %f\n", depth_ref, z_ref); 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /vikit/vikit_common/vikit_commonConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ####################################################### 2 | # vikit_common source dir 3 | set( vikit_common_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ####################################################### 6 | # vikit_common build dir 7 | set( vikit_common_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ####################################################### 10 | set( vikit_common_INCLUDE_DIR "@vikit_common_INCLUDE_DIR@" ) 11 | set( vikit_common_INCLUDE_DIRS "@vikit_common_INCLUDE_DIR@" ) 12 | 13 | set( vikit_common_LIBRARIES "@vikit_common_LIBRARIES@" ) 14 | set( vikit_common_LIBRARY "@vikit_common_LIBRARIES@" ) 15 | 16 | set( vikit_common_LIBRARY_DIR "@vikit_common_LIBRARY_DIR@" ) 17 | set( vikit_common_LIBRARY_DIRS "@vikit_common_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /vikit/vikit_py/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vikit_py) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy 9 | ) 10 | 11 | ## Uncomment this if the package has a setup.py. This macro ensures 12 | ## modules and global scripts declared therein get installed 13 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 14 | catkin_python_setup() 15 | 16 | catkin_package( 17 | # INCLUDE_DIRS include 18 | # LIBRARIES vikit_py 19 | # CATKIN_DEPENDS rospy 20 | # DEPENDS system_lib 21 | ) 22 | 23 | include_directories( 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | -------------------------------------------------------------------------------- /vikit/vikit_py/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['vikit_py'], 8 | package_dir={'': 'src'}, 9 | install_requires=['rospkg', 'yaml'], 10 | ) 11 | 12 | setup(**d) -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_open/ca371f304637e7fb355cf4624d0a02da4e3da220/vikit/vikit_py/src/vikit_py/__init__.py -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/cpu_info.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import subprocess, re 4 | 5 | def get_cpu_info(): 6 | command = "cat /proc/cpuinfo" 7 | all_info = subprocess.check_output(command, shell=True).strip() 8 | for line in all_info.split("\n"): 9 | if "model name" in line: 10 | model_name = re.sub(".*model name.*:", "", line,1).strip() 11 | return model_name.replace("(R)","").replace("(TM)", "") -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/math_utils.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Aug 7 22:13:06 2013 4 | 5 | @author: cforster 6 | """ 7 | 8 | import numpy as np 9 | 10 | def unproject(a): 11 | """Makes a vector homogeneous""" 12 | return np.append(a, 1) 13 | 14 | def project(a): 15 | """De-homogenises a vector""" 16 | return a[:-1]/float(a[-1]) 17 | 18 | def skew(v): 19 | """Returns the skew-symmetric matrix of a vector""" 20 | return np.array([[0, -v[2], v[1]], 21 | [v[2], 0, -v[0]], 22 | [-v[1], v[0], 0]], dtype=np.float32) 23 | 24 | def unskew(R): 25 | """Returns the coordinates of a skew-symmetric matrix""" 26 | return np.array([R[2,1], R[0,2], R[1,0]], dtype=np.float32) -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/pinhole_camera.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy as np 4 | import vikit_py.math_utils as math_utils 5 | 6 | class PinholeCamera: 7 | """ Pinhole Camera Model """ 8 | 9 | def __init__(self, width, height, fx, fy, cx, cy): 10 | self.width = width 11 | self.height = height 12 | self.focal_length = np.array([fx, fy]) 13 | self.principle_point = np.array([cx, cy]) 14 | 15 | def back_project(self, px): 16 | """Computes the bearing-vector from the pixel measurement. Z-Component is 1""" 17 | uv = 1.0/self.focal_length * (px - self.principle_point) 18 | f = math_utils.unproject(uv) 19 | return f 20 | 21 | def project(self, xyz): 22 | """Projects a euclidean point to a 2d image measurement""" 23 | if(xyz[2] < 0): 24 | return np.zeros(2), False 25 | uv = math_utils.project(xyz[:3]) 26 | px = uv*self.focal_length + self.principle_point 27 | visible = (px[0] >= 0 and px[0] < self.width and px[1] >= 0 and px[1] < self.height) 28 | return px, visible -------------------------------------------------------------------------------- /vikit/vikit_py/src/vikit_py/plot_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | from matplotlib import rc 7 | 8 | # tell matplotlib to use latex font 9 | rc('font',**{'family':'serif','serif':['Cardo']}) 10 | rc('text', usetex=True) 11 | 12 | def axis_equal_3d(ax): 13 | extents = np.array([getattr(ax, 'get_{}lim'.format(dim))() for dim in 'xyz']) 14 | sz = extents[:,1] - extents[:,0] 15 | centers = np.mean(extents, axis=1) 16 | maxsize = max(abs(sz)) 17 | r = maxsize/2 18 | for ctr, dim in zip(centers, 'xyz'): 19 | getattr(ax, 'set_{}lim'.format(dim))(ctr - r, ctr + r) 20 | 21 | def draw_coordinate_frame(ax, t_world_body, R_world_body, alpha=1.0): 22 | """draws the coordinate frame in the provided figure-axis (world coordinates)""" 23 | T_wf = np.zeros((3,4)) 24 | T_wf[0:3,0:3] = R_world_body 25 | T_wf[0:3,3] = t_world_body 26 | line_length = 0.1 27 | ax.plot([T_wf[0,3], T_wf[0,3]+T_wf[0,0]*line_length], 28 | [T_wf[1,3], T_wf[1,3]+T_wf[1,0]*line_length], 29 | [T_wf[2,3], T_wf[2,3]+T_wf[2,0]*line_length], color='r', alpha=alpha) 30 | ax.plot([T_wf[0,3], T_wf[0,3]+T_wf[0,1]*line_length], 31 | [T_wf[1,3], T_wf[1,3]+T_wf[1,1]*line_length], 32 | [T_wf[2,3], T_wf[2,3]+T_wf[2,1]*line_length], color='g', alpha=alpha) 33 | ax.plot([T_wf[0,3], T_wf[0,3]+T_wf[0,2]*line_length], 34 | [T_wf[1,3], T_wf[1,3]+T_wf[1,2]*line_length], 35 | [T_wf[2,3], T_wf[2,3]+T_wf[2,2]*line_length], color='b', alpha=alpha) -------------------------------------------------------------------------------- /vikit/vikit_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(vikit_ros) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | include(SvoSetup) 8 | 9 | set(HEADERS 10 | include/vikit/img_type_conversion.h 11 | include/vikit/output_helper.h 12 | include/vikit/params_helper.h 13 | ) 14 | 15 | set(SOURCES 16 | src/output_helper.cpp 17 | src/img_type_conversion.cpp 18 | ) 19 | 20 | # Create vikit library 21 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 22 | 23 | ################################################################################ 24 | cs_install() 25 | cs_export() 26 | -------------------------------------------------------------------------------- /vikit/vikit_ros/include/vikit/img_type_conversion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace vk { 8 | namespace img_type_conversion 9 | { 10 | int sensorMsgsEncodingToOpenCVType(const std::string& encoding); 11 | 12 | std::string openCVTypeToSensorMsgsEncoding(const int opencv_type); 13 | } // namespace img_type_conversion 14 | } // namespace vk 15 | -------------------------------------------------------------------------------- /vikit/vikit_ros/include/vikit/params_helper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ros_params_helper.h 3 | * 4 | * Created on: Feb 22, 2013 5 | * Author: cforster 6 | * 7 | * from libpointmatcher_ros 8 | */ 9 | 10 | #ifndef ROS_PARAMS_HELPER_H_ 11 | #define ROS_PARAMS_HELPER_H_ 12 | 13 | #include 14 | #include 15 | 16 | namespace vk { 17 | 18 | inline 19 | bool hasParam(const std::string& name) 20 | { 21 | return ros::param::has(name); 22 | } 23 | 24 | template 25 | T getParam(const std::string& name, const T& defaultValue) 26 | { 27 | T v; 28 | if(ros::param::get(name, v)) 29 | { 30 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 31 | return v; 32 | } 33 | else 34 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 35 | return defaultValue; 36 | } 37 | 38 | template 39 | T getParam(const std::string& name) 40 | { 41 | T v; 42 | if(ros::param::get(name, v)) 43 | { 44 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 45 | return v; 46 | } 47 | else 48 | ROS_ERROR_STREAM("Cannot find value for parameter: " << name); 49 | return T(); 50 | } 51 | 52 | template 53 | T param(const ros::NodeHandle& nh, const std::string& name, const T& defaultValue, 54 | const bool silent=false) 55 | { 56 | if(nh.hasParam(name)) 57 | { 58 | T v; 59 | nh.param(name, v, defaultValue); 60 | if (!silent) 61 | { 62 | ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); 63 | } 64 | return v; 65 | } 66 | if (!silent) 67 | { 68 | ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); 69 | } 70 | return defaultValue; 71 | } 72 | 73 | } // namespace vk 74 | 75 | #endif // ROS_PARAMS_HELPER_H_ 76 | -------------------------------------------------------------------------------- /vikit/vikit_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vikit_ros 4 | 0.0.0 5 | The vikit_ros package 6 | cforster 7 | RPG 8 | 9 | catkin 10 | catkin_simple 11 | 12 | svo_cmake 13 | vikit_common 14 | glog_catkin 15 | eigen_catkin 16 | minkindr 17 | 18 | roscpp 19 | tf 20 | visualization_msgs 21 | cv_bridge 22 | sensor_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /vikit/vikit_solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vikit_solver) 3 | find_package(catkin_simple REQUIRED) 4 | catkin_simple(ALL_DEPS_REQUIRED) 5 | 6 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 7 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++11") 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | cs_add_library(${PROJECT_NAME} 13 | include/vikit/solver/mini_least_squares_solver.h 14 | include/vikit/solver/implementation/mini_least_squares_solver.hpp 15 | src/robust_cost.cpp 16 | include/vikit/solver/robust_cost.h 17 | ) 18 | 19 | ## Declare a cpp executable 20 | #cs_add_executable(elevation_map_node src/elevation_map_node.cpp) 21 | #target_link_libraries(elevation_map_node ${PROJECT_NAME}) 22 | 23 | ########## 24 | # GTESTS # 25 | ########## 26 | #catkin_add_gtest(test_cameras 27 | # test/test-cameras.cc 28 | #) 29 | #target_link_libraries(test_cameras ${PROJECT_NAME}) 30 | 31 | ########## 32 | # EXPORT # 33 | ########## 34 | cs_install() 35 | cs_export() 36 | -------------------------------------------------------------------------------- /vikit/vikit_solver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vikit_solver 4 | 0.0.0 5 | The vikit_solver package 6 | 7 | cfo 8 | BSD 9 | 10 | catkin 11 | catkin_simple 12 | 13 | eigen_catkin 14 | glog_catkin 15 | 16 | gtest 17 | --------------------------------------------------------------------------------