├── 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 ├── eyecatcher_with_youtube_logo.jpg ├── frontend │ ├── frontend_fla.md │ └── visual_frontend.md ├── global_map.md ├── images │ ├── git_img.png │ ├── mh03_gm.gif │ ├── v102_gm.gif │ └── v202_mono_vio.gif ├── known_issues_and_improvements.md └── vio.md ├── gps_simulator ├── CMakeLists.txt ├── README.md ├── config │ ├── constellation.yaml │ └── reciever_config.yaml ├── include │ ├── constellation.hpp │ ├── emitter.hpp │ ├── frame_handler.hpp │ ├── gnss.hpp │ ├── receiver.hpp │ └── satellite_param.hpp ├── launch │ └── gnss_simulator.launch ├── msg │ ├── DoubleArrayMsg.msg │ ├── DoubleMsg.msg │ ├── EmitterMsg.msg │ └── IntMsg.msg ├── package.xml ├── scripts │ └── gps_error_publisher.py └── src │ ├── constellation.cpp │ ├── emitter.cpp │ ├── frame_handler.cpp │ ├── gnss.cpp │ ├── main.cpp │ └── satellite_param.cpp ├── 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 ├── scripts ├── bash │ └── record_vio.sh └── python │ ├── extract_from_rosbag.py │ └── include_simulated_gp_in_rosbag.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 │ │ ├── global_pose_handler.h │ │ ├── global_positions_handler.h │ │ ├── imu_handler.h │ │ ├── initialization.h │ │ ├── io.h │ │ ├── map.h │ │ ├── pose_optimizer.h │ │ ├── raw_gnss_handler.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 │ ├── global_pose_handler.cpp │ ├── global_positions_handler.cpp │ ├── imu_handler.cpp │ ├── initialization.cpp │ ├── io.cpp │ ├── map.cpp │ ├── pose_optimizer.cpp │ ├── raw_gnss_handler.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 │ ├── MH_04_difficult │ │ └── data │ │ │ └── imu.txt │ ├── 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 │ │ ├── global_position_error.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 │ ├── global_position_error.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 │ │ ├── global_pose_settings.h │ │ ├── global_positions_settings.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 │ └── PoseWithCloud.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_registration ├── CMakeLists.txt ├── Dataset │ ├── 0_0_Bag_conversion.py │ ├── 0_1_fusing_colmaptraj.py │ ├── 0_colmaptrajextraction copy.py │ ├── 0_colmaptrajextraction.py │ ├── 1_GMap2ply.py │ ├── 2_Lidar2ENU.py │ ├── 3_colmap_transform.py │ ├── 4_colmap_add_GT.py │ ├── 5_GPS_reading.py │ ├── 6_align_stamps.py │ ├── 7_VINS_pinhole.py │ ├── Colmap │ │ ├── Readme.md │ │ └── elios2bag.py │ ├── Kalibr │ │ ├── README.md │ │ ├── elios2bag.py │ │ └── yaml │ │ │ ├── april_6x6.yaml │ │ │ └── imu.yaml │ └── README.md ├── Evaluation │ ├── FM │ │ ├── PlotAllTraj.py │ │ ├── SVO2RPG.py │ │ └── VINS2RPG.py │ └── Oerlikon │ │ ├── PlotAllTraj.py │ │ ├── SVO2RPGFormat.py │ │ ├── Traj2CommunFrame.py │ │ └── Vins2RPGFormat.py ├── config │ ├── FM.yaml │ └── oerlikon.yaml ├── package.xml └── scripts │ ├── Map_Creation │ ├── 1_obj2ply.py │ └── README.md │ ├── RealWorldData │ └── README.md │ └── online_registration.py ├── svo_ros ├── CMakeLists.txt ├── RVIZ │ └── rviz_config_digital_twins.rviz ├── build │ ├── .cmake │ │ └── api │ │ │ └── v1 │ │ │ └── query │ │ │ └── client-integration-vscode │ │ │ ├── cache-v2 │ │ │ ├── cmakeFiles-v1 │ │ │ └── codemodel-v2 │ ├── CATKIN_IGNORE │ ├── CMakeCache.txt │ ├── CMakeFiles │ │ ├── 3.16.3 │ │ │ ├── CMakeCCompiler.cmake │ │ │ ├── CMakeCXXCompiler.cmake │ │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ │ ├── CMakeSystem.cmake │ │ │ ├── CompilerIdC │ │ │ │ ├── CMakeCCompilerId.c │ │ │ │ └── a.out │ │ │ └── CompilerIdCXX │ │ │ │ ├── CMakeCXXCompilerId.cpp │ │ │ │ └── a.out │ │ ├── CMakeError.log │ │ ├── CMakeOutput.log │ │ └── cmake.check_cache │ ├── CTestConfiguration.ini │ ├── CTestCustom.cmake │ ├── atomic_configure │ │ ├── .rosinstall │ │ ├── _setup_util.py │ │ ├── env.sh │ │ ├── local_setup.bash │ │ ├── local_setup.sh │ │ ├── local_setup.zsh │ │ ├── setup.bash │ │ ├── setup.sh │ │ └── setup.zsh │ ├── catkin │ │ └── catkin_generated │ │ │ └── version │ │ │ └── package.cmake │ ├── catkin_generated │ │ ├── env_cached.sh │ │ ├── generate_cached_setup.py │ │ ├── installspace │ │ │ ├── .rosinstall │ │ │ ├── _setup_util.py │ │ │ ├── env.sh │ │ │ ├── local_setup.bash │ │ │ ├── local_setup.sh │ │ │ ├── local_setup.zsh │ │ │ ├── setup.bash │ │ │ ├── setup.sh │ │ │ ├── setup.zsh │ │ │ ├── svo_ros.pc │ │ │ ├── svo_rosConfig-version.cmake │ │ │ └── svo_rosConfig.cmake │ │ ├── ordered_paths.cmake │ │ ├── package.cmake │ │ ├── pkg.develspace.context.pc.py │ │ ├── pkg.installspace.context.pc.py │ │ ├── setup_cached.sh │ │ └── stamps │ │ │ └── svo_ros │ │ │ ├── _setup_util.py.stamp │ │ │ ├── interrogate_setup_dot_py.py.stamp │ │ │ ├── package.xml.stamp │ │ │ └── pkg.pc.em.stamp │ └── devel │ │ ├── .catkin │ │ ├── .rosinstall │ │ ├── _setup_util.py │ │ ├── cmake.lock │ │ ├── env.sh │ │ ├── lib │ │ └── pkgconfig │ │ │ └── svo_ros.pc │ │ ├── local_setup.bash │ │ ├── local_setup.sh │ │ ├── local_setup.zsh │ │ ├── setup.bash │ │ ├── setup.sh │ │ ├── setup.zsh │ │ └── share │ │ └── svo_ros │ │ └── cmake │ │ ├── svo_rosConfig-version.cmake │ │ └── svo_rosConfig.cmake ├── 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 │ ├── Default │ │ ├── euroc_vio_gp_mono.launch │ │ ├── euroc_vio_gp_stereo.launch │ │ ├── euroc_vio_mono.launch │ │ ├── euroc_vio_mono_test.launch │ │ └── euroc_vio_stereo.launch │ ├── Flightmare │ │ ├── flightmare_imu_frontend_mono.launch │ │ ├── flightmare_imu_frontend_stereo.launch │ │ ├── flightmare_vio_mono.launch │ │ └── flightmare_vio_stereo.launch │ └── Oerlikon.launch ├── package.xml ├── param │ ├── Default │ │ ├── backend.yaml │ │ ├── euroc_mono.yaml │ │ ├── euroc_mono_test.yaml │ │ ├── euroc_stereo.yaml │ │ ├── fisheye.yaml │ │ ├── flightmare.yaml │ │ ├── param_frontend_mono.yaml │ │ ├── param_frontend_stereo.yaml │ │ ├── param_vio_mono.yaml │ │ ├── vio_mono.yaml │ │ ├── vio_mono_flightmare.yaml │ │ ├── vio_mono_test.yaml │ │ └── vio_stereo.yaml │ ├── Flightmare │ │ ├── VIO_mono.yaml │ │ ├── calibration.yaml │ │ ├── gp_settings.yaml │ │ └── gpose_settings.yaml │ └── Oerlikon │ │ ├── calibration.yaml │ │ ├── gp_settings.yaml │ │ ├── gpose_settings.yaml │ │ └── mono_vio.yaml ├── rqt.perspective ├── 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 /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: master 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/eyecatcher_with_youtube_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/doc/eyecatcher_with_youtube_logo.jpg -------------------------------------------------------------------------------- /doc/images/git_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/doc/images/git_img.png -------------------------------------------------------------------------------- /doc/images/mh03_gm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/doc/images/mh03_gm.gif -------------------------------------------------------------------------------- /doc/images/v102_gm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/doc/images/v102_gm.gif -------------------------------------------------------------------------------- /doc/images/v202_mono_vio.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/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 | -------------------------------------------------------------------------------- /gps_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(satellite_simulator) 3 | 4 | find_package(Ceres REQUIRED) 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | roscpp 8 | geometry_msgs 9 | std_msgs 10 | ) 11 | 12 | find_package(GeographicLib REQUIRED) 13 | find_package(Eigen3 REQUIRED yaml-cpp REQUIRED) 14 | 15 | 16 | add_message_files( 17 | FILES 18 | DoubleMsg.msg 19 | EmitterMsg.msg 20 | DoubleArrayMsg.msg 21 | IntMsg.msg 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | geometry_msgs 28 | ) 29 | catkin_package( 30 | CATKIN_DEPENDS message_runtime std_msgs 31 | ) 32 | 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | LIBRARIES satellite_simulator 37 | CATKIN_DEPENDS roscpp geometry_msgs std_msgs 38 | DEPENDS EIGEN3 39 | ) 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | ${YAML_CPP_INCLUDE_DIR} 46 | ${GeographicLib_INCLUDE_DIRS} 47 | ) 48 | 49 | add_executable(gnss_simulator src/main.cpp src/gnss.cpp src/satellite_param.cpp src/emitter.cpp src/frame_handler.cpp) 50 | target_link_libraries(gnss_simulator ${catkin_LIBRARIES} pthread yaml-cpp ${GeographicLib_LIBRARIES} ${CERES_LIBRARIES}) 51 | 52 | 53 | install(PROGRAMS 54 | script/gps_error_publisher.py 55 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | ) 57 | -------------------------------------------------------------------------------- /gps_simulator/config/reciever_config.yaml: -------------------------------------------------------------------------------- 1 | gps_noise_model_folder: '' 2 | gps_noise_model_multipath: 'gmm_multipath_error_in_function_of_height.pkl' 3 | gps_noise_model_number_of_sat: 'gp_satellites_los_in_function_of_height.joblib' 4 | gps_noise_model_number_of_sat_scaler: 'gp_scaler.joblib' 5 | 6 | pseudorange_error_topic: '/gnss_simulator/multipath_error' 7 | number_of_satellite_topic: '/gnss_simulator/number_of_visible_satellites' 8 | ground_truth_topic: '/kingfisher/agiros_pilot/groundtruth/pose' 9 | 10 | n_sat_class_0 : 2 11 | n_class: 8 12 | 13 | max_time_before_resetting_error: 15.0 14 | max_distance_before_resetting_error: 25.0 -------------------------------------------------------------------------------- /gps_simulator/include/constellation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONSTELLATION_H 2 | #define CONSTELLATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "ros/ros.h" 10 | #include "geometry_msgs/PointStamped.h" 11 | #include 12 | #include "satellite_param.hpp" 13 | #include 14 | #include 15 | #include 16 | 17 | class Constellation { 18 | public: 19 | Constellation(const std::string& filename, ros::NodeHandle& nh); 20 | ~Constellation(); 21 | 22 | void startAllSatellites(); 23 | void stopAllSatellites(); 24 | 25 | private: 26 | std::thread processing_thread; 27 | 28 | std::mutex mtx; 29 | 30 | bool running; 31 | 32 | void processingLoop(); 33 | }; 34 | 35 | #endif // CONSTELLATION_H 36 | -------------------------------------------------------------------------------- /gps_simulator/include/emitter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EMITTER_HPP 2 | #define EMITTER_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "constellation.hpp" 11 | #include "ros/ros.h" 12 | #include "geometry_msgs/PointStamped.h" 13 | #include 14 | #include "satellite_param.hpp" 15 | #include 16 | #include 17 | #include 18 | 19 | class Emitter { 20 | public: 21 | 22 | Emitter(ros::NodeHandle& nh, double t0, const SatelliteParam& param); 23 | 24 | ~Emitter(); 25 | 26 | void emit_pseudorange(Eigen::Vector3d position_receiver_ECEF, Eigen::Quaterniond orientation_receiver_enu); 27 | 28 | void publish() ; 29 | 30 | void process() ; 31 | ros::Publisher position_publisher; 32 | 33 | SatelliteParam param; 34 | 35 | bool above_horizon = false; 36 | double pseudorange; 37 | double timestamp; 38 | Eigen::Vector3d Position_ECEF; 39 | 40 | private: 41 | 42 | ros::Publisher pseudorange_publisher; 43 | 44 | double calculate_elevation_angle(); 45 | 46 | double calculate_pseudorange(Eigen::Vector3d position_receiver_ECEF); 47 | 48 | 49 | double t0_; 50 | 51 | double n; 52 | 53 | double tol; 54 | 55 | Eigen::Vector3d Position_geo; 56 | Eigen::Vector3d Linearization_pt_; 57 | 58 | Eigen::Matrix3d R_ecef_enu; 59 | 60 | 61 | Eigen::Vector3d Initial_ECEF; 62 | 63 | double elevation_angle; 64 | double azimuth_angle; 65 | 66 | void updateTimestamp() ; 67 | 68 | void calculate_position() ; 69 | 70 | }; 71 | 72 | #endif // EMITTER_HPP 73 | -------------------------------------------------------------------------------- /gps_simulator/include/frame_handler.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FRAME_HANDLER_HPP 2 | #define FRAME_HANDLER_HPP 3 | 4 | #include 5 | 6 | 7 | #include "constellation.hpp" 8 | #include 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 | 23 | // LLA = geocentric coordinate : latitude, longitude, altitude 24 | void LLAtoENU(double lat, double lon, double alt, double refLat, double refLon, double refAlt, double &east, double &north, double &up); 25 | 26 | void ENUtoECEF(double east, double north, double up, double refLat, double refLon, double refAlt, double &ecefX, double &ecefY, double &ecefZ); 27 | 28 | void ENUToLLA(double refLon, double refLat, double refAlt, 29 | double enuE, double enuN, double enuU, 30 | double& lon, double& lat, double& alt) ; 31 | 32 | void LLAToECEF(double lat, double lon, double alt, double& x, double& y, double& z); 33 | 34 | void ECEFToLLA(double x, double y, double z, double& lat, double& lon, double& alt); 35 | 36 | #endif // FRAME_HANDLER_HPP -------------------------------------------------------------------------------- /gps_simulator/include/gnss.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GPS_SIM_HPP 2 | #define GPS_SIM_HPP 3 | 4 | #include 5 | #include "constellation.hpp" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "frame_handler.hpp" 20 | 21 | class Emitter; 22 | 23 | class GNSS { 24 | public: 25 | GNSS(const std::string& constallation_config_fn, const std::string& reciever_config_fn, ros::NodeHandle& nh) ; 26 | 27 | ~GNSS(); 28 | 29 | void startAllEmitters(std::string ground_truth_topic); 30 | 31 | Eigen::Vector3d receiver_enu_gt; 32 | Eigen::Vector3d receiver_ecef_gt; 33 | 34 | int number_visible_satellites; 35 | std::vector emitters; // TODO : map the PRN to the emitters/satellite for more clarity 36 | 37 | Eigen::Vector3d linearization_Point; 38 | 39 | private: 40 | ros::NodeHandle nh_; 41 | 42 | Eigen::Vector3d Initial_ECEF; 43 | ros::Subscriber receiver_enu_gt_sub; 44 | ros::Publisher ecef_receiver_gt_pub; 45 | 46 | std::thread processing_thread; 47 | std::mutex emitters_mtx; 48 | 49 | bool running; 50 | 51 | void EphemerisLoop(); 52 | 53 | void groundtruthCallback(const geometry_msgs::PoseStamped::ConstPtr& msg); 54 | 55 | }; 56 | 57 | 58 | #endif // GPS_SIM_HPP -------------------------------------------------------------------------------- /gps_simulator/include/satellite_param.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SATELLITEPARAM_H 2 | #define SATELLITEPARAM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class SatelliteParam { 10 | public: 11 | double muE; // Earth's mass 12 | double RE; // Earth's radius 13 | 14 | double sa; // semi-major axis 15 | double mu_anomalie; // mean anomaly 16 | double arg; // argument of periapsis 17 | double orbinc; // orbital inclination 18 | double RAANangle; // right ascension of the ascending node 19 | double eccen; // eccentricity 20 | 21 | int satellite_number_; 22 | 23 | 24 | SatelliteParam(const std::string& filename, int satellite_number) ; 25 | 26 | SatelliteParam() ; 27 | 28 | ~SatelliteParam() {} 29 | }; 30 | 31 | #endif // SATELLITEPARAM_H 32 | -------------------------------------------------------------------------------- /gps_simulator/launch/gnss_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /gps_simulator/msg/DoubleArrayMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64[32] data 3 | -------------------------------------------------------------------------------- /gps_simulator/msg/DoubleMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | std_msgs/Float64 data -------------------------------------------------------------------------------- /gps_simulator/msg/EmitterMsg.msg: -------------------------------------------------------------------------------- 1 | # EmitterMsg.msg 2 | std_msgs/Header header # Standard ROS message header 3 | geometry_msgs/Point position_ecef # Position in ECEF frame 4 | geometry_msgs/Point position_geo # Position in lat long alt frame 5 | std_msgs/Float64 range # Additional range (float) data 6 | std_msgs/Float64 elevation_angle # Additional range (float) data 7 | std_msgs/Float64 azimuth_angle # Additional range (float) data 8 | std_msgs/Int32 idx_satellite # Index of the satellite -------------------------------------------------------------------------------- /gps_simulator/msg/IntMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | std_msgs/Int32 data 3 | -------------------------------------------------------------------------------- /gps_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | satellite_simulator 4 | 0.0.0 5 | The Satelite simulator package 6 | 7 | Your Name 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | geometry_msgs 13 | std_msgs 14 | 15 | message_generation 16 | 17 | roscpp 18 | geometry_msgs 19 | 20 | message_runtime 21 | std_msgs 22 | 23 | 24 | -------------------------------------------------------------------------------- /gps_simulator/src/constellation.cpp: -------------------------------------------------------------------------------- 1 | #include "constellation.hpp" 2 | 3 | Constellation::Constellation(const std::string& filename, ros::NodeHandle& nh) { 4 | YAML::Node config = YAML::LoadFile(filename); 5 | int numSatellites = config["sqrt_sa"].size(); // Assuming this represents the number of satellites 6 | 7 | for (int i = 0; i < numSatellites; i++) { 8 | SatelliteParam sat_param(filename, i); 9 | satellites.emplace_back(nh, ros::Time::now().toSec(), sat_param); 10 | } 11 | } 12 | 13 | Constellation::~Constellation() { 14 | stopAllSatellites(); 15 | if (processing_thread.joinable()) { 16 | processing_thread.join(); 17 | } 18 | } 19 | 20 | void Constellation::startAllSatellites() { 21 | running = true; 22 | processing_thread = std::thread(&Constellation::processingLoop, this); 23 | } 24 | 25 | void Constellation::stopAllSatellites() { 26 | running = false; 27 | } 28 | -------------------------------------------------------------------------------- /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/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_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/rqt_svo/src/rqt_svo/__init__.py -------------------------------------------------------------------------------- /scripts/bash/record_vio.sh: -------------------------------------------------------------------------------- 1 | # EuRoC 2 | # run: ./record_vio.sh $dir 3 | rosbag record -O $1/vio_estimate.bag /svo/backend_pose_imu -------------------------------------------------------------------------------- /svo/.gitignore: -------------------------------------------------------------------------------- 1 | include/svo/config.h -------------------------------------------------------------------------------- /svo/doc/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore -------------------------------------------------------------------------------- /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 | #include 23 | #include 24 | -------------------------------------------------------------------------------- /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 | python3 ../../../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 | python3 ../../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('# timestamp tx ty tz qx qy qz qw\n') 10 | 11 | with open(gt,'rb') as fin: 12 | data = np.genfromtxt(fin, delimiter=",") 13 | for l in data: 14 | fout.write('%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' % 15 | (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 gt 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_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/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_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/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/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 | svo_msgs 18 | 19 | svo_vio_common 20 | svo_common 21 | svo 22 | pcl_ros 23 | 24 | -------------------------------------------------------------------------------- /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::kGpError, std::string("GpError") }, 16 | {ErrorType::kRelativePoseError, std::string("RelativePoseError") }, 17 | }; 18 | 19 | } 20 | 21 | } 22 | -------------------------------------------------------------------------------- /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/global_positions_settings.h 16 | include/svo/common/logging.h 17 | include/svo/common/occupancy_grid_2d.h 18 | include/svo/common/point.h 19 | include/svo/common/seed.h 20 | include/svo/common/transformation.h 21 | include/svo/common/types.h 22 | ) 23 | 24 | set(SOURCES 25 | src/frame.cpp 26 | src/point.cpp 27 | ) 28 | 29 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 30 | 31 | ################################################################################ 32 | # TESTS 33 | 34 | ################################################################################ 35 | ### 36 | ### GTEST 37 | ### 38 | 39 | catkin_add_gtest(${PROJECT_NAME}-test 40 | test/test_main.cpp 41 | test/test_feature_wrapper.cpp 42 | ) 43 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread) 44 | 45 | catkin_add_gtest(container-helpers-test 46 | test/test_container_helpers.cpp 47 | ) 48 | target_link_libraries(container-helpers-test ${PROJECT_NAME}) 49 | 50 | ################################################################################ 51 | cs_install() 52 | cs_export() 53 | -------------------------------------------------------------------------------- /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/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_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/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_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/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/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/msg/PoseWithCloud.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/PoseStamped pose 3 | sensor_msgs/PointCloud2 point_cloud 4 | bool initial_refinement -------------------------------------------------------------------------------- /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 | message(STATUS "CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") 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_registration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(svo_registration) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | rospy 7 | sensor_msgs 8 | std_msgs 9 | svo_msgs 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS rospy std_msgs sensor_msgs geometry_msgs message_runtime svo_msgs 14 | ) 15 | 16 | include_directories( 17 | # include 18 | ${catkin_INCLUDE_DIRS} 19 | ) 20 | 21 | install(PROGRAMS 22 | svo_registration/scripts/online_registration.py 23 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | ) 25 | 26 | -------------------------------------------------------------------------------- /svo_registration/Dataset/Colmap/Readme.md: -------------------------------------------------------------------------------- 1 | # 2nd step : GT generation via colmap 2 | 3 | 1) Again transform it to a readable bag -------------------------------------------------------------------------------- /svo_registration/Dataset/Kalibr/README.md: -------------------------------------------------------------------------------- 1 | # First step : calibration : 2 | 3 | 1)convert to kalibr readable bag 4 | 2) run kalibr with : 5 | rosrun kalibr kalibr_calibrate_cameras --target /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Kalibr/april_6x6.yaml --models pinhole-equi --topics /camera_2/image_raw --bag /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Calib.bag --show-extraction 6 | 7 | rosrun kalibr kalibr_calibrate_imu_camera --bag /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Calib.bag --cam /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Kalibr/Calib-camchain.yaml --imu /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Kalibr/imu.yaml --target /home/roxane/elios3_2706/EL300825315585_01644_213_2hand/Kalibr/april_6x6.yaml 8 | -------------------------------------------------------------------------------- /svo_registration/Dataset/Kalibr/yaml/april_6x6.yaml: -------------------------------------------------------------------------------- 1 | 2 | 3 | #example for aprilgrid 4 | target_type: 'aprilgrid' #gridtype 5 | tagCols: 6 #number of apriltags 6 | tagRows: 6 #number of apriltags 7 | tagSize: 0.083 #size of apriltag, edge to edge [m] 8 | tagSpacing: 0.30120482 #ratio of space between tags to tagSize 9 | #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 10 | 11 | #example for checkerboard 12 | # targetType: 'checkerboard' #gridtype 13 | # targetCols: 6 #number of internal chessboard corners 14 | # targetRows: 7 #number of internal chessboard corners 15 | # rowsMetricSize: 0.06 #size of one chessboard square [m] 16 | # colsMetricSize: 0.06 #size of one chessboard square [m] 17 | 18 | -------------------------------------------------------------------------------- /svo_registration/Dataset/Kalibr/yaml/imu.yaml: -------------------------------------------------------------------------------- 1 | # Info derived from https://octopart.com/datasheet/icm-42688-p-invensense-102280765 2 | #Accelerometers 3 | accelerometer_noise_density: 0.012753 #Noise density (continuous-time) : 70×10^-6*9.81 = 0.00063765 rounded up *10 4 | accelerometer_random_walk: 0.0004508866 #Bias random walk -> just did noise density/sqrt(200 = the freq), not sure this is correct, cf https://www.vectornav.com/resources/inertial-navigation-primer/specifications--and--error-budgets/specs-imuspecs # 0.00004508866 rounded *5 5 | 6 | #Gyroscopes 7 | gyroscope_noise_density: 0.00073303815 # Noise density (continuous-time) # 2.8×(10^−3)*pi/180 = 0.00004886921 rounded up *15 8 | gyroscope_random_walk: 0.0002 #12094495 #Bias random walk same : abov/sqrt(freq) # 0.00000345557 rounded *30 9 | 10 | rostopic: /vio_module/imu_filtered #the IMU ROS topic 11 | update_rate: 200.0 #Hz (for discretization of the values above) 12 | -------------------------------------------------------------------------------- /svo_registration/Dataset/README.md: -------------------------------------------------------------------------------- 1 | The different file to convert data from elios3 to SVO (VINS) usable data. 2 | 3 | Step 1 : 4 | 1) Create a pointcloud fro google map (sample uniformely obj). Be carefull to export with Axe y forward, z up from blender. 5 | 2) Optionnal : convert the lidar data to ENU 6 | 3) Make ground truth out of COLMAP : need to find scale + transform. 7 | 4) Add colmap to bag 8 | 5) Read gpx file 9 | 6) Align GPS stamp 10 | 7) Optionnal : convert to pinhole camera for VINS. 11 | -------------------------------------------------------------------------------- /svo_registration/Evaluation/Oerlikon/SVO2RPGFormat.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_registration/Evaluation/Oerlikon/SVO2RPGFormat.py -------------------------------------------------------------------------------- /svo_registration/config/FM.yaml: -------------------------------------------------------------------------------- 1 | scale_factor: 10 2 | max_correspondence_distance: 10 3 | inliers_rmse_thr: 2 #1 #0.8 #1.0 #.5 #3 # 1.5 4 | max_translation_distance: 25 #10 5 | information_matrix_power: 1 6 | max_rotation_distance: 100 #5 7 | reference_map: "/home/roxane/svo_gps_ws/src/svo_volocopter/svo_registration/maps/Flightmare_ENU.ply" 8 | # reference_map: "/home/roxane/Desktop/CITY/CityAndPark/CityWithPark_Winter.ply" 9 | -------------------------------------------------------------------------------- /svo_registration/config/oerlikon.yaml: -------------------------------------------------------------------------------- 1 | registration_settings: 2 | scale_factor_position: 9 #6 3 | scale_factor_rotation: 2 4 | max_correspondence_distance: 8 5 | inliers_rmse_thr: 1.6 6 | max_translation_distance: 20 7 | information_matrix_power: 1 8 | max_rotation_distance: 15 9 | size_bounding_box: 350 10 | -------------------------------------------------------------------------------- /svo_registration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | svo_registration 4 | 0.0.0 5 | The svo_registration package 6 | 7 | y 8 | roxane 9 | TODO 10 | 11 | catkin 12 | geometry_msgs 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | svo_msgs 17 | message_generation 18 | message_runtime 19 | 20 | geometry_msgs 21 | rospy 22 | sensor_msgs 23 | std_msgs 24 | svo_msgs 25 | 26 | geometry_msgs 27 | rospy 28 | sensor_msgs 29 | std_msgs 30 | message_runtime 31 | svo_msgs 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /svo_registration/scripts/Map_Creation/README.md: -------------------------------------------------------------------------------- 1 | # How to create a map since a blender file : 2 | 1) In blender, export your file as an .obj. Select forward axis = y, up axis = z. Keep the normals 3 | 2) Run the script ```1_obj2ply.py``` to get an equily sampeld point cloud 4 | 5 | If your file is already in the correct frame this is it. -------------------------------------------------------------------------------- /svo_registration/scripts/RealWorldData/README.md: -------------------------------------------------------------------------------- 1 | To use real world data, we need to make sure to have consistent frames. 2 | 3 | -------------------------------------------------------------------------------- /svo_ros/build/.cmake/api/v1/query/client-integration-vscode/cache-v2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/.cmake/api/v1/query/client-integration-vscode/cache-v2 -------------------------------------------------------------------------------- /svo_ros/build/.cmake/api/v1/query/client-integration-vscode/cmakeFiles-v1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/.cmake/api/v1/query/client-integration-vscode/cmakeFiles-v1 -------------------------------------------------------------------------------- /svo_ros/build/.cmake/api/v1/query/client-integration-vscode/codemodel-v2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/.cmake/api/v1/query/client-integration-vscode/codemodel-v2 -------------------------------------------------------------------------------- /svo_ros/build/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/CATKIN_IGNORE -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/3.16.3/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/CMakeFiles/3.16.3/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/3.16.3/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/CMakeFiles/3.16.3/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/3.16.3/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_HOST_SYSTEM "Linux-5.15.0-117-generic") 2 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 3 | set(CMAKE_HOST_SYSTEM_VERSION "5.15.0-117-generic") 4 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 5 | 6 | 7 | 8 | set(CMAKE_SYSTEM "Linux-5.15.0-117-generic") 9 | set(CMAKE_SYSTEM_NAME "Linux") 10 | set(CMAKE_SYSTEM_VERSION "5.15.0-117-generic") 11 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/3.16.3/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/CMakeFiles/3.16.3/CompilerIdC/a.out -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/3.16.3/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/CMakeFiles/3.16.3/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /svo_ros/build/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /svo_ros/build/CTestCustom.cmake: -------------------------------------------------------------------------------- 1 | set(CTEST_CUSTOM_MAXIMUM_PASSED_TEST_OUTPUT_SIZE 0) 2 | set(CTEST_CUSTOM_MAXIMUM_FAILED_TEST_OUTPUT_SIZE 0) 3 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: 2 | local-name: /home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel/setup.sh 3 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/local_setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/local_setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" --extend --local 9 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/local_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/template/local_setup.sh.in 3 | 4 | # since this file is sourced either use the provided _CATKIN_SETUP_DIR 5 | # or fall back to the destination set at configure time 6 | : ${_CATKIN_SETUP_DIR:=/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel} 7 | CATKIN_SETUP_UTIL_ARGS="--extend --local" 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | unset CATKIN_SETUP_UTIL_ARGS 10 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/local_setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/local_setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local' 9 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | -------------------------------------------------------------------------------- /svo_ros/build/atomic_configure/setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/env_cached.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup_cached.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup_cached.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/generate_cached_setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from __future__ import print_function 3 | 4 | import os 5 | import stat 6 | import sys 7 | 8 | # find the import for catkin's python package - either from source space or from an installed underlay 9 | if os.path.exists(os.path.join('/opt/ros/noetic/share/catkin/cmake', 'catkinConfig.cmake.in')): 10 | sys.path.insert(0, os.path.join('/opt/ros/noetic/share/catkin/cmake', '..', 'python')) 11 | try: 12 | from catkin.environment_cache import generate_environment_script 13 | except ImportError: 14 | # search for catkin package in all workspaces and prepend to path 15 | for workspace in ''.split(';'): 16 | python_path = os.path.join(workspace, 'lib/python3/dist-packages') 17 | if os.path.isdir(os.path.join(python_path, 'catkin')): 18 | sys.path.insert(0, python_path) 19 | break 20 | from catkin.environment_cache import generate_environment_script 21 | 22 | code = generate_environment_script('/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel/env.sh') 23 | 24 | output_filename = '/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/catkin_generated/setup_cached.sh' 25 | with open(output_filename, 'w') as f: 26 | # print('Generate script for cached setup "%s"' % output_filename) 27 | f.write('\n'.join(code)) 28 | 29 | mode = os.stat(output_filename).st_mode 30 | os.chmod(output_filename, mode | stat.S_IXUSR) 31 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: 2 | local-name: /usr/local/setup.sh 3 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/local_setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/local_setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" --extend --local 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/local_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/template/local_setup.sh.in 3 | 4 | # since this file is sourced either use the provided _CATKIN_SETUP_DIR 5 | # or fall back to the destination set at configure time 6 | : ${_CATKIN_SETUP_DIR:=/usr/local} 7 | CATKIN_SETUP_UTIL_ARGS="--extend --local" 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | unset CATKIN_SETUP_UTIL_ARGS 10 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/local_setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/local_setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local' 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/svo_ros.pc: -------------------------------------------------------------------------------- 1 | prefix=/usr/local 2 | 3 | Name: svo_ros 4 | Description: Description of svo_ros 5 | Version: 0.1.0 6 | Cflags: -I${prefix}/include 7 | Libs: -L${prefix}/lib -lsvo_ros -lsvo_nodelet 8 | Requires: cmake_modules cv_bridge image_transport nodelet roscpp nav_msgs pcl_ros sensor_msgs std_msgs tf visualization_msgs 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/installspace/svo_rosConfig-version.cmake: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkgConfig-version.cmake.in 2 | set(PACKAGE_VERSION "0.1.0") 3 | 4 | set(PACKAGE_VERSION_EXACT False) 5 | set(PACKAGE_VERSION_COMPATIBLE False) 6 | 7 | if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") 8 | set(PACKAGE_VERSION_EXACT True) 9 | set(PACKAGE_VERSION_COMPATIBLE True) 10 | endif() 11 | 12 | if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") 13 | set(PACKAGE_VERSION_COMPATIBLE True) 14 | endif() 15 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/ordered_paths.cmake: -------------------------------------------------------------------------------- 1 | set(ORDERED_PATHS "/opt/ros/noetic/lib") -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/pkg.develspace.context.pc.py: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkg.context.pc.in 2 | CATKIN_PACKAGE_PREFIX = "" 3 | PROJECT_PKG_CONFIG_INCLUDE_DIRS = "/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/include".split(';') if "/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/include" != "" else [] 4 | PROJECT_CATKIN_DEPENDS = "cmake_modules;cv_bridge;image_transport;nodelet;roscpp;nav_msgs;pcl_ros;sensor_msgs;std_msgs;tf;visualization_msgs".replace(';', ' ') 5 | PKG_CONFIG_LIBRARIES_WITH_PREFIX = "-lsvo_ros;-lsvo_nodelet".split(';') if "-lsvo_ros;-lsvo_nodelet" != "" else [] 6 | PROJECT_NAME = "svo_ros" 7 | PROJECT_SPACE_DIR = "/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel" 8 | PROJECT_VERSION = "0.1.0" 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/pkg.installspace.context.pc.py: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkg.context.pc.in 2 | CATKIN_PACKAGE_PREFIX = "" 3 | PROJECT_PKG_CONFIG_INCLUDE_DIRS = "${prefix}/include".split(';') if "${prefix}/include" != "" else [] 4 | PROJECT_CATKIN_DEPENDS = "cmake_modules;cv_bridge;image_transport;nodelet;roscpp;nav_msgs;pcl_ros;sensor_msgs;std_msgs;tf;visualization_msgs".replace(';', ' ') 5 | PKG_CONFIG_LIBRARIES_WITH_PREFIX = "-lsvo_ros;-lsvo_nodelet".split(';') if "-lsvo_ros;-lsvo_nodelet" != "" else [] 6 | PROJECT_NAME = "svo_ros" 7 | PROJECT_SPACE_DIR = "/usr/local" 8 | PROJECT_VERSION = "0.1.0" 9 | -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/setup_cached.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/python/catkin/environment_cache.py 3 | 4 | # based on a snapshot of the environment before and after calling the setup script 5 | # it emulates the modifications of the setup script without recurring computations 6 | 7 | # new environment variables 8 | 9 | # modified environment variables 10 | export CMAKE_PREFIX_PATH='/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel:/home/rox/svo_gps_ws/build/catkin_simple/catkin_generated/installspace' 11 | export LD_LIBRARY_PATH='/usr/local/cuda-10.1/lib64:/usr/local/cuda-12.1/lib64' 12 | export PATH='/home/rox/.local/bin:/home/rox/bin:/usr/local/cuda-10.1/bin:/usr/local/cuda-12.1/bin:/home/rox/miniconda3/bin:/home/rox/miniconda3/condabin:/home/rox/.local/bin:/home/rox/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/usr/local/MATLAB/R2023b/bin/' 13 | export PKG_CONFIG_PATH='' 14 | export PWD='/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build' 15 | export PYTHONPATH='' -------------------------------------------------------------------------------- /svo_ros/build/catkin_generated/stamps/svo_ros/package.xml.stamp: -------------------------------------------------------------------------------- 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/build/catkin_generated/stamps/svo_ros/pkg.pc.em.stamp: -------------------------------------------------------------------------------- 1 | prefix=@PROJECT_SPACE_DIR 2 | 3 | Name: @(CATKIN_PACKAGE_PREFIX + PROJECT_NAME) 4 | Description: Description of @PROJECT_NAME 5 | Version: @PROJECT_VERSION 6 | Cflags: @(' '.join(['-I%s' % include for include in PROJECT_PKG_CONFIG_INCLUDE_DIRS])) 7 | Libs: -L${prefix}/lib @(' '.join(PKG_CONFIG_LIBRARIES_WITH_PREFIX)) 8 | Requires: @(PROJECT_CATKIN_DEPENDS) 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/.catkin: -------------------------------------------------------------------------------- 1 | /home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros -------------------------------------------------------------------------------- /svo_ros/build/devel/.rosinstall: -------------------------------------------------------------------------------- 1 | - setup-file: 2 | local-name: /home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel/setup.sh 3 | -------------------------------------------------------------------------------- /svo_ros/build/devel/cmake.lock: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_svo_pro_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/svo_ros/build/devel/cmake.lock -------------------------------------------------------------------------------- /svo_ros/build/devel/env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /svo_ros/build/devel/lib/pkgconfig/svo_ros.pc: -------------------------------------------------------------------------------- 1 | prefix=/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel 2 | 3 | Name: svo_ros 4 | Description: Description of svo_ros 5 | Version: 0.1.0 6 | Cflags: -I/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/include 7 | Libs: -L${prefix}/lib -lsvo_ros -lsvo_nodelet 8 | Requires: cmake_modules cv_bridge image_transport nodelet roscpp nav_msgs pcl_ros sensor_msgs std_msgs tf visualization_msgs 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/local_setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/local_setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" --extend --local 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/local_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/template/local_setup.sh.in 3 | 4 | # since this file is sourced either use the provided _CATKIN_SETUP_DIR 5 | # or fall back to the destination set at configure time 6 | : ${_CATKIN_SETUP_DIR:=/home/rox/svo_gps_ws/src/rpg_svo_pro_gps/svo_ros/build/devel} 7 | CATKIN_SETUP_UTIL_ARGS="--extend --local" 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | unset CATKIN_SETUP_UTIL_ARGS 10 | -------------------------------------------------------------------------------- /svo_ros/build/devel/local_setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/local_setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local' 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' 9 | -------------------------------------------------------------------------------- /svo_ros/build/devel/share/svo_ros/cmake/svo_rosConfig-version.cmake: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkgConfig-version.cmake.in 2 | set(PACKAGE_VERSION "0.1.0") 3 | 4 | set(PACKAGE_VERSION_EXACT False) 5 | set(PACKAGE_VERSION_COMPATIBLE False) 6 | 7 | if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") 8 | set(PACKAGE_VERSION_EXACT True) 9 | set(PACKAGE_VERSION_COMPATIBLE True) 10 | endif() 11 | 12 | if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") 13 | set(PACKAGE_VERSION_COMPATIBLE True) 14 | endif() 15 | -------------------------------------------------------------------------------- /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_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/Default/euroc_vio_gp_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 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /svo_ros/launch/Default/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 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /svo_ros/launch/Default/euroc_vio_mono_test.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/Default/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 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /svo_ros/launch/Flightmare/flightmare_imu_frontend_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 | -------------------------------------------------------------------------------- /svo_ros/launch/Flightmare/flightmare_imu_frontend_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 | -------------------------------------------------------------------------------- /svo_ros/launch/Flightmare/flightmare_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 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /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 | message_generation 17 | 18 | message_runtime 19 | cmake_modules 20 | cv_bridge 21 | eigen_catkin 22 | eigen_checks 23 | image_transport 24 | nodelet 25 | roscpp 26 | minkindr 27 | nav_msgs 28 | pcl_ros 29 | rpg_common 30 | sensor_msgs 31 | std_msgs 32 | svo 33 | svo_cmake 34 | svo_common 35 | svo_msgs 36 | svo_tracker 37 | svo_backend 38 | svo_ceres_backend 39 | svo_online_loopclosing 40 | svo_global_map 41 | tf 42 | vikit_common 43 | vikit_ros 44 | visualization_msgs 45 | geometry_msgs 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /svo_ros/param/Default/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/Default/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/Default/param_frontend_mono.yaml: -------------------------------------------------------------------------------- 1 | label: "Euroc" 2 | id: 412eab8e4058621f7036b5e765dfe812 3 | cameras: 4 | - camera: 5 | label: cam0 # Right camera, same position as imu 6 | id: 54812562fa109c40fe90b29a59dd7798 7 | line-delay-nanoseconds: 0 8 | image_height: 768 9 | image_width: 768 10 | type: pinhole 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [611.9588338070289,458.8516417444384, 385.97336677336034,392.494078919] #375.5059210806708] 15 | distortion: 16 | type: radial-tangential 17 | parameters: 18 | cols: 1 19 | rows: 4 20 | data: [-0.02216820165861635, 0.003754640020819623, -0.0031896617660559096, 0.00015846337168769477] 21 | T_B_C: 22 | cols: 4 23 | rows: 4 24 | data: [0,0.5,0.86602540378, 0, 25 | -1, 0, 0, 0, 26 | 0,-0.86602540378,0.5, 0, 27 | 0, 0, 0, 1] 28 | 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-5 36 | sigma_acc_c: 8.0e-4 37 | sigma_omega_bias_c: 0.003 38 | sigma_acc_bias_c: 0.01 39 | sigma_integration: 0.0 40 | g: 9.8066 41 | imu_rate: 200 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: 1.0 48 | omega_bias_sigma: 0.001 #0.001 49 | acc_bias_sigma: 0.001 #0.001 50 | 51 | -------------------------------------------------------------------------------- /svo_ros/param/Flightmare/gp_settings.yaml: -------------------------------------------------------------------------------- 1 | gp_settings: 2 | # Covariance. 3 | variance_x: 400 #5044 4 | variance_y: 400 #5044 5 | variance_z: 400 #5044 # 18 of std. 6 | 7 | # position of point P (e.g. gps receiver) in body frame. 8 | B_t_PB: [0.0, 0.0, 0.0] 9 | # Max number of global residuals for each keyframe. 10 | max_num_residuals: 1 11 | estimate_world_vio_transform : true 12 | 13 | # alignement_threshold: 0.00025 #5 #0.1 #001 #0.005 #005 #2 #0.001 for GPS enu or real worldc 14 | # alignement_threshold: 0.037 #0.0001 #00025 #5 #0.1 #001 #0.005 #005 #2 #0.001 for GPS enu or real worldc 15 | alignement_threshold: 0.05 #08 #16 #00025 #5 #0.1 #001 #0.005 #005 #2 #0.001 for GPS enu or real worldc 16 | alignement_speed_min: 0.0 #0.25 # minimum speed for a measurement to be used in the alignement process 17 | alignement_d_min: 6 #2 #0 #6 #2 #5 18 | 19 | # Initialize with a known orientaion. 20 | # Set to true in the case the initial orientation is known (e.g. through an external motion capture system) 21 | initial_orientation_known: false 22 | 23 | coordinates: 'local' #'local' = x y z, 'geographic' = lat long alt, will activate transformation from LLA to ECEF 24 | -------------------------------------------------------------------------------- /svo_ros/param/Flightmare/gpose_settings.yaml: -------------------------------------------------------------------------------- 1 | gpose_settings: 2 | # Covariance. 3 | variance_x: 10000 4 | variance_y: 10000 5 | variance_z: 10000 6 | 7 | alignement_threshold: 0.001 #0.016 #001 #000000001 #0.001 # threshold - when variace of the yaw angle is below this value, the alignement is considered as successful 8 | #TODO : also consider variance how the translation. 9 | 10 | # position of point P (e.g. gps receiver) in body frame. 11 | B_t_PB: [0.0, 0.0, 0.0] 12 | # Max number of global residuals for each keyframe. 13 | max_num_residuals: 1 14 | # Initialize with a known orientaion. 15 | # Set to true in the case the initial orientation is known (e.g. through an external motion capture system) 16 | initial_orientation_known: false 17 | refine_T_WL: true -------------------------------------------------------------------------------- /svo_ros/param/Oerlikon/calibration.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - T_B_C: 3 | cols: 4 4 | data: [0.003282939472533192, -0.19294942909971347, 0.9812032104098082, 0.030087415920571525, 5 | -0.9999469827980318, 0.008943046094240282, 0.005104264850980736, 0.014700960773767908, 6 | -0.009759810527481877, -0.9811679467535876, -0.19290983998183384, -0.010556222345895877, 7 | 0.0, 0.0, 0.0, 1.0] 8 | rows: 4 9 | calib_date: 0 10 | camera: 11 | distortion: 12 | parameters: 13 | cols: 1 14 | data: [-0.07321701837897686, -0.02964399348764655, 0.028207592603742136, -0.013234383835932766] 15 | rows: 4 16 | type: equidistant 17 | image_height: 400 18 | image_width: 640 19 | intrinsics: 20 | cols: 1 21 | data: [387.7113338951933, 387.8098232626537, 313.42251525400184, 211.99430613976855] 22 | rows: 4 23 | label: cam0 24 | line-delay-nanoseconds: 0 25 | type: pinhole 26 | description: /camera_2/image_raw 27 | serial_no: 0 28 | label: Calibration-camchain-imucam.yaml 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: 3.0e-5 38 | sigma_acc_bias_c: 1.0e-4 39 | sigma_integration: 0.0 40 | g: 9.81 41 | imu_rate: 200 42 | 43 | 44 | imu_initialization: 45 | acc_bias: [0.0, 0.0, 0.0] 46 | acc_bias_sigma: 0.01 47 | omega_bias: [0.0, 0.0, 0.0] 48 | omega_bias_sigma: 0.01 49 | velocity: [0.0, 0.0, 0.0] 50 | velocity_sigma: 1.0 51 | -------------------------------------------------------------------------------- /svo_ros/param/Oerlikon/gp_settings.yaml: -------------------------------------------------------------------------------- 1 | gp_settings: 2 | # Covariance. 3 | variance_x: 900 4 | variance_y: 900 5 | variance_z: 900 6 | # position of point P (e.g. gps receiver) in body frame. 7 | B_t_PB: [0.0, 0.0, 0.0] 8 | # Max number of global residuals for each keyframe. 9 | max_num_residuals: 1 10 | estimate_world_vio_transform : true 11 | 12 | alignement_threshold: 5.0e-4 # covariance pose threshold for initial GPS alignement, assume weights are 1 13 | alignement_speed_min: 0.0 # minimum speed for a measurement to be used in the alignement process 14 | alignement_d_min: 2 # yaw not observable if parcoured distance is too small. 15 | 16 | # Initialize with a known orientaion. 17 | # Set to true in the case the initial orientation is known (e.g. through an external motion capture system) 18 | initial_orientation_known: false 19 | 20 | coordinates: 'local' #'local' = x y z, 'geographic' = lat long alt, will activate transformation from LLA to ECEF 21 | -------------------------------------------------------------------------------- /svo_ros/param/Oerlikon/gpose_settings.yaml: -------------------------------------------------------------------------------- 1 | gpose_settings: 2 | # threshold on the variance of the yaw angle for the alignement to be considered successful 3 | alignement_threshold: 3.0e-4 4 | max_num_residuals: 1 5 | initial_orientation_known: false 6 | refine_T_WL: true -------------------------------------------------------------------------------- /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 | svo_ros::SvoNodeBase node; 7 | node.run(); 8 | } 9 | -------------------------------------------------------------------------------- /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", true)) 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/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/README.md: -------------------------------------------------------------------------------- 1 | ### VIKIT 2 | 3 | Vikit (Vision-Kit) provides some tools for your vision/robotics project. 4 | Far from stable. -------------------------------------------------------------------------------- /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/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/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/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/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_with_digital_twins/d64132270dd448d471a964b63035fdb995eed927/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/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 | --------------------------------------------------------------------------------